Nguyên lý hoạt động của LiDAR LD14

Nguyên lý hoạt động của LiDAR LD14

LD14 là LiDAR quét 360°. Nó không tự biết đâu là “phía trước robot”; ESP32 phải dựa vào góc đo + khoảng cách để xác định vùng vật cản.

Mỗi vòng quay, LD14 liên tục trả về các điểm dạng:

Góc θ + Khoảng cách D + cường độ tín hiệu

Ví dụ nếu khi lắp cảm biến ta quy ước 0° là phía trước robot:

                 0° / 360°
                 PHÍA TRƯỚC
                     ↑
             330°    |    30°
                 \   |   /
                  \  |  /
270°  TRÁI  ←------ ROBOT ------→  PHẢI  90°
                  /  |  \
                 /   |   \
             210°    |    150°
                     ↓
                   180°
                  PHÍA SAU

Kiểm tra vật cản phía trước

Ví dụ anh muốn robot chỉ quan tâm vùng ±30° phía trước và cảnh báo khi vật cách dưới 50 cm. ESP32 kiểm tra:

bool inFront = (angle >= 330 || angle <= 30);

if (inFront && distance < 500) {   // mm
    obstacleFront = true;
}

Nhưng với robot mài nền của anh, tôi khuyên không chỉ kiểm tra một điểm. Nên chia vùng trước thành 3 khu vực:

                 PHÍA TRƯỚC
                     ↑
            khoảng cách vật cản

       TRÁI          GIỮA          PHẢI
    330°→350°      350°→10°      10°→30°
       \              |              /
        \             |             /
         \            |            /
          ┌───────────────────────┐
          │        ROBOT          │
          │                       │
          └───────────────────────┘

Khi đó ESP32 có thể lấy khoảng cách nhỏ nhất trong từng vùng:

frontLeft  = khoảng cách nhỏ nhất vùng 330°–350°
frontMid   = khoảng cách nhỏ nhất vùng 350°–10°
frontRight = khoảng cách nhỏ nhất vùng 10°–30°

Ví dụ kết quả:

Trái  = 120 cm
Giữa  = 38 cm   ← có vật cản
Phải  = 95 cm

Robot biết vật đang nằm gần chính giữa phía trước.

Với LDROBOT LD14 → ESP32 38 chân, anh đấu qua UART. Sơ đồ cơ bản:

        LD14                         ESP32
     ┌─────────┐                  ┌──────────┐
     │ VCC đỏ  │ ───────────────► │ 5V / VIN │
     │ GND đen │ ───────────────► │ GND      │
     │ TX  vàng│ ───────────────► │ GPIO 16  │ RX2
     │ RX xanh │ ◄─────────────── │ GPIO 17  │ TX2 (bỏ qua)
     └─────────┘                  └──────────┘

Tức là:

Trong code ESP32:

#define LIDAR_RX 16
#define LIDAR_TX 17
HardwareSerial LidarSerial(2);
void setup() {
    Serial.begin(115200);
    LidarSerial.begin(
        230400,          // Baudrate LD14
        SERIAL_8N1,
        LIDAR_RX,
        LIDAR_TX
    );
}

Lưu ý quan trọng: LD14 có nhiều phiên bản/module đi kèm bo nguồn hoặc cáp khác nhau. Không nên cấp 5V trực tiếp chỉ dựa vào màu dây

Code mẫu:

Viết chương trình mẫu cho lidar với esp32 để trả về 3 vùng TRÁI – GIỮA – PHẢI từ 330-0-30 độ hiển thị lên serial monitor

vùng phía trước là 330° → 0° → 30°, tổng góc quét là 60°. Chia đều thành 3 vùng, mỗi vùng 20°:

TRÁI             GIỮA              PHẢI
330°–350°      350°–10°          10°–30°

Chương trình dưới đây lấy khoảng cách gần nhất trong từng vùng và hiển thị lên Serial Monitor.

#include <Arduino.h>

// ======================================================
// ESP32 + LDROBOT LD14
//
// LD14 TX  -> ESP32 GPIO16 (RX2)
// LD14 RX  -> ESP32 GPIO17 (TX2) - nếu cần
// GND      -> GND
//
// Serial Monitor: 115200
// LD14 UART:       230400
// ======================================================

#define LIDAR_RX 16
#define LIDAR_TX 17
#define LIDAR_BAUD 230400

HardwareSerial LidarSerial(2);

// ======================================================
// THÔNG SỐ
// ======================================================

// Cường độ tín hiệu tối thiểu
#define LIDAR_MIN_INTENSITY 10

// Chỉ quan tâm vật trong phạm vi tối đa này
#define LIDAR_MAX_CM 500.0f

// Chu kỳ hiển thị
#define PRINT_INTERVAL_MS 200

// ======================================================
// KHOẢNG CÁCH GẦN NHẤT 3 VÙNG
// ======================================================

float lidarLeftCm   = 9999.0f;
float lidarCenterCm = 9999.0f;
float lidarRightCm  = 9999.0f;

unsigned long lastPrintMs = 0;

// ======================================================
// CRC TABLE LD14
// ======================================================

static const uint8_t CrcTable[256] = {
  0x00,0x4d,0x9a,0xd7,0x79,0x34,0xe3,0xae,
  0xf2,0xbf,0x68,0x25,0x8b,0xc6,0x11,0x5c,
  0xa9,0xe4,0x33,0x7e,0xd0,0x9d,0x4a,0x07,
  0x5b,0x16,0xc1,0x8c,0x22,0x6f,0xb8,0xf5,
  0x1f,0x52,0x85,0xc8,0x66,0x2b,0xfc,0xb1,
  0xed,0xa0,0x77,0x3a,0x94,0xd9,0x0e,0x43,
  0xb6,0xfb,0x2c,0x61,0xcf,0x82,0x55,0x18,
  0x44,0x09,0xde,0x93,0x3d,0x70,0xa7,0xea,
  0x3e,0x73,0xa4,0xe9,0x47,0x0a,0xdd,0x90,
  0xcc,0x81,0x56,0x1b,0xb5,0xf8,0x2f,0x62,
  0x97,0xda,0x0d,0x40,0xee,0xa3,0x74,0x39,
  0x65,0x28,0xff,0xb2,0x1c,0x51,0x86,0xcb,
  0x21,0x6c,0xbb,0xf6,0x58,0x15,0xc2,0x8f,
  0xd3,0x9e,0x49,0x04,0xaa,0xe7,0x30,0x7d,
  0x88,0xc5,0x12,0x5f,0xf1,0xbc,0x6b,0x26,
  0x7a,0x37,0xe0,0xad,0x03,0x4e,0x99,0xd4,
  0x7c,0x31,0xe6,0xab,0x05,0x48,0x9f,0xd2,
  0x8e,0xc3,0x14,0x59,0xf7,0xba,0x6d,0x20,
  0xd5,0x98,0x4f,0x02,0xac,0xe1,0x36,0x7b,
  0x27,0x6a,0xbd,0xf0,0x5e,0x13,0xc4,0x89,
  0x63,0x2e,0xf9,0xb4,0x1a,0x57,0x80,0xcd,
  0x91,0xdc,0x0b,0x46,0xe8,0xa5,0x72,0x3f,
  0xca,0x87,0x50,0x1d,0xb3,0xfe,0x29,0x64,
  0x38,0x75,0xa2,0xef,0x41,0x0c,0xdb,0x96,
  0x42,0x0f,0xd8,0x95,0x3b,0x76,0xa1,0xec,
  0xb0,0xfd,0x2a,0x67,0xc9,0x84,0x53,0x1e,
  0xeb,0xa6,0x71,0x3c,0x92,0xdf,0x08,0x45,
  0x19,0x54,0x83,0xce,0x60,0x2d,0xfa,0xb7,
  0x5d,0x10,0xc7,0x8a,0x24,0x69,0xbe,0xf3,
  0xaf,0xe2,0x35,0x78,0xd6,0x9b,0x4c,0x01,
  0xf4,0xb9,0x6e,0x23,0x8d,0xc0,0x17,0x5a,
  0x06,0x4b,0x9c,0xd1,0x7f,0x32,0xe5,0xa8
};

// ======================================================
// TÍNH CRC
// ======================================================

uint8_t calcCRC8(const uint8_t *data, uint8_t len)
{
  uint8_t crc = 0;

  for (uint8_t i = 0; i < len; i++)
  {
    crc = CrcTable[(crc ^ data[i]) & 0xFF];
  }

  return crc;
}

// ======================================================
// XỬ LÝ 1 ĐIỂM LIDAR
// ======================================================

void processPoint(float angle, float cm, uint8_t intensity)
{
  // Loại dữ liệu lỗi
  if (cm <= 0.0f)
    return;

  // Loại vật quá xa
  if (cm > LIDAR_MAX_CM)
    return;

  // Loại tín hiệu phản xạ quá yếu
  if (intensity < LIDAR_MIN_INTENSITY)
    return;


  // ====================================================
  // TRÁI
  // 330° -> <350°
  // ====================================================

  if (angle >= 330.0f && angle < 350.0f)
  {
    if (cm < lidarLeftCm)
    {
      lidarLeftCm = cm;
    }
  }


  // ====================================================
  // GIỮA
  // 350° -> 360°
  // hoặc
  // 0° -> <10°
  // ====================================================

  else if (angle >= 350.0f || angle < 10.0f)
  {
    if (cm < lidarCenterCm)
    {
      lidarCenterCm = cm;
    }
  }


  // ====================================================
  // PHẢI
  // 10° -> 30°
  // ====================================================

  else if (angle >= 10.0f && angle <= 30.0f)
  {
    if (cm < lidarRightCm)
    {
      lidarRightCm = cm;
    }
  }
}

// ======================================================
// XỬ LÝ FRAME LD14
// Frame LD14 = 47 byte
// ======================================================

void processLD14Frame(const uint8_t *f)
{
  // Kiểm tra Header
  if (f[0] != 0x54 || f[1] != 0x2C)
    return;

  // Kiểm tra CRC
  uint8_t crc = calcCRC8(f, 46);

  if (crc != f[46])
    return;


  // ====================================================
  // GÓC BẮT ĐẦU
  // ====================================================

  uint16_t startRaw =
      (uint16_t)f[4] |
      ((uint16_t)f[5] << 8);

  float startDeg = startRaw / 100.0f;


  // ====================================================
  // GÓC KẾT THÚC
  // ====================================================

  uint16_t endRaw =
      (uint16_t)f[42] |
      ((uint16_t)f[43] << 8);

  float endDeg = endRaw / 100.0f;


  // ====================================================
  // TÍNH KHOẢNG GÓC
  // ====================================================

  float span = endDeg - startDeg;

  // Nếu vượt qua 360 -> 0
  if (span < 0.0f)
  {
    span += 360.0f;
  }

  // Có 12 điểm = 11 khoảng
  float step = span / 11.0f;


  // ====================================================
  // ĐỌC 12 ĐIỂM
  // ====================================================

  for (int i = 0; i < 12; i++)
  {
    int p = 6 + i * 3;


    // --------------------------------------------------
    // Khoảng cách mm
    // --------------------------------------------------

    uint16_t mm =
        (uint16_t)f[p] |
        ((uint16_t)f[p + 1] << 8);


    // --------------------------------------------------
    // Cường độ phản xạ
    // --------------------------------------------------

    uint8_t intensity = f[p + 2];


    // --------------------------------------------------
    // Tính góc của điểm
    // --------------------------------------------------

    float angle = startDeg + step * i;

    if (angle >= 360.0f)
    {
      angle -= 360.0f;
    }


    // --------------------------------------------------
    // mm -> cm
    // --------------------------------------------------

    float cm = mm / 10.0f;


    // --------------------------------------------------
    // Phân loại TRÁI / GIỮA / PHẢI
    // --------------------------------------------------

    processPoint(angle, cm, intensity);
  }
}

// ======================================================
// ĐỌC UART LD14
// ======================================================

void readLD14()
{
  static uint8_t frame[47];

  static int index = 0;


  while (LidarSerial.available())
  {
    uint8_t b = LidarSerial.read();


    // ==================================================
    // TÌM BYTE HEADER 0x54
    // ==================================================

    if (index == 0)
    {
      if (b == 0x54)
      {
        frame[0] = b;
        index = 1;
      }

      continue;
    }


    // ==================================================
    // BYTE THỨ 2 PHẢI = 0x2C
    // ==================================================

    if (index == 1)
    {
      if (b == 0x2C)
      {
        frame[1] = b;
        index = 2;
      }
      else
      {
        index = 0;

        // Có thể byte này lại chính là đầu frame mới
        if (b == 0x54)
        {
          frame[0] = b;
          index = 1;
        }
      }

      continue;
    }


    // ==================================================
    // NHẬN CÁC BYTE CÒN LẠI
    // ==================================================

    frame[index] = b;

    index++;


    // ==================================================
    // ĐỦ 47 BYTE
    // ==================================================

    if (index >= 47)
    {
      processLD14Frame(frame);

      index = 0;
    }
  }
}

// ======================================================
// RESET 3 VÙNG
// ======================================================

void resetZones()
{
  lidarLeftCm   = 9999.0f;
  lidarCenterCm = 9999.0f;
  lidarRightCm  = 9999.0f;
}

// ======================================================
// IN KHOẢNG CÁCH
// ======================================================

void printDistance(float cm)
{
  if (cm >= 9999.0f)
  {
    Serial.print("---");
  }
  else
  {
    Serial.print(cm, 1);
    Serial.print(" cm");
  }
}

// ======================================================
// HIỂN THỊ 3 VÙNG
// ======================================================

void printZones()
{
  Serial.print("TRAI: ");
  printDistance(lidarLeftCm);

  Serial.print("   |   GIUA: ");
  printDistance(lidarCenterCm);

  Serial.print("   |   PHAI: ");
  printDistance(lidarRightCm);

  Serial.println();
}

// ======================================================
// SETUP
// ======================================================

void setup()
{
  Serial.begin(115200);

  delay(1000);

  Serial.println();
  Serial.println("==========================================");
  Serial.println("       ESP32 + LD14 - 3 VUNG");
  Serial.println("==========================================");

  Serial.println("TRAI : 330 -> 350 do");
  Serial.println("GIUA : 350 -> 10 do");
  Serial.println("PHAI : 10  -> 30 do");
  Serial.println();


  // Khởi động UART2 cho LD14
  LidarSerial.begin(
      LIDAR_BAUD,
      SERIAL_8N1,
      LIDAR_RX,
      LIDAR_TX
  );

  resetZones();

  lastPrintMs = millis();
}

// ======================================================
// LOOP
// ======================================================

void loop()
{
  // Đọc LiDAR liên tục
  readLD14();


  // ====================================================
  // CỨ 200ms HIỂN THỊ 1 LẦN
  // ====================================================

  if (millis() - lastPrintMs >= PRINT_INTERVAL_MS)
  {
    lastPrintMs = millis();

    printZones();

    // Reset để tìm khoảng cách nhỏ nhất
    // cho chu kỳ 200ms tiếp theo
    resetZones();
  }
}

Serial Monitor sẽ có dạng:

TRAI: 125.3 cm   |   GIUA: 46.8 cm   |   PHAI: 97.5 cm
TRAI: 124.9 cm   |   GIUA: 46.5 cm   |   PHAI: 96.9 cm
TRAI: ---        |   GIUA: 45.9 cm   |   PHAI: 97.2 cm

Ở đây mỗi giá trị là vật gần nhất trong vùng đó, không phải giá trị trung bình.

Sơ đồ vùng:

                       0°
                       ↑
                  350° | 10°
                    \  |  /
                     \ | /
              GIỮA   \|/   GIỮA
                      |
            TRÁI      |      PHẢI
         330° \       |       / 30°
               \      |      /
                \     |     /
                 ┌─────────┐
                 │  ROBOT  │
                 └─────────┘

       TRÁI : 330° → 350°   = 20°
       GIỮA : 350° → 10°    = 20°
       PHẢI :  10° → 30°    = 20°

Lưu ý: GPIO16/17 phù hợp để test riêng LD14. Khi ghép vào chương trình robot hiện tại của anh, cần kiểm tra lại để không trùng các chân Hall/driver đang sử dụng.