Giới thiệu về kiến trúc lưới 2D

Sau khi hoàn tất các bước tiền xử lý điểm mây như đồng bộ thời gian đa cảm biến, bù biến dạng vận động, hiệu chỉnh trọng trường và lọc voxel, dữ liệu lidar đã sẵn sàng cho quy trình khớp quét. Trong không gian 2D, Cartographer không dựa vào các thư viện bản đồ tiêu chuẩn như 3D, mà tự triển khai cơ chế xây dựng lưới xác suất (Probability Grid) thông qua một bộ phân lớp được tối ưu cho tốc độ và bộ nhớ.

Quản lý Submap động (ActiveSubmaps2D)

Lớp ActiveSubmaps2D đóng vai trò trung tâm trong việc điều phối các bản đồ con đang hoạt động. Nó duy trì một cửa sổ trượt gồm tối đa hai submap chồng chéo nhau, đảm bảo tính liên tục khi thực hiện khớp quét và cập nhật xác suất.

Khởi tạo và cấu hình

Đối tượng quản lý submap được khởi tạo bên trong LocalTrajectoryBuilder2D dựa trên file cấu hình Lua. Các tham số quan trọng bao gồm số lượng khung dữ liệu tối đa cho mỗi submap, độ phân giải lưới và loại bộ chèn dữ liệu.

LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& config,
    const std::vector& sensor_ids)
    : config_(config),
      bộQuảnLySubmap_(config.tuChonSubmap()),
      bộLọcChuyenDong_(config.tuChonBoLocChuyenDong()),
      bộKhietQuetThoiGianThuc_(config.tuChonKhietQuetThoiGianThuc()),
      bộKhietQuetCeres_(config.tuChonCeres()),
      bộSắpXepDữLieu_(sensor_ids) {}

Quy trình chèn dữ liệu

Hàm chèn dữ liệu khoảng cách thực hiện ba bước chính: kiểm tra ngưỡng tạo submap mới, đẩy dữ liệu radar vào tất cả submap đang hoạt động, và đánh dấu hoàn thành submap cũ nhất khi đạt giới hạn.

std::vector ActiveSubmaps2D::ChenDuLieuKhoangCach(
    const sensor::RangeData& duLieuRadar) {
  // Tạo submap mới nếu danh sách rỗng hoặc submap cuối đã đủ ngưỡng
  if (danhSachSubmap_.empty() || 
      danhSachSubmap_.back()->laySoLuongDuLieu() == config_.soLuongDuLieuMax()) {
    ThemSubmapMoi(duLieuRadar goc.head<2>());
  }
  
  // Đẩy dữ liệu đồng thời vào các submap đang hoạt động
  for (auto& sp : danhSachSubmap_) {
    sp->ChenDuLieu(duLieuRadar, boChenDữLieu_.get());
  }
  
  // Hoàn thành submap đầu tiên khi đạt ngưỡng 2x để giải phóng bộ nhớ
  if (danhSachSubmap_.front()->laySoLuongDuLieu() >= 2 * config_.soLuongDuLieuMax()) {
    danhSachSubmap_.front()->DanhDauHoanThanh();
  }
  
  return danhSachSubmap_;
}

Cơ chế chồng chéo giúp tránh hiện tượng断层 (đứt gãy) giữa các submap liền kề, đồng thời hỗ trợ quá trình khớp quét dựa trên đặc điểm hình học chung.

Lớp Submap cơ bản và triển khai 2D

Submap định nghĩa giao diện trừu tượng cho bản đồ con, bao gồm các phương thức quản lý trạng thái, chuyển đổi sang định dạng protobuf và lưu trữ vị trí gốc trong hệ tọa độ cục bộ. Submap2D kế thừa lớp này và quản lý một đối tượng Grid2D cùng bảng tra cứu xác suất.

class Submap2D : public Submap {
 private:
  std::unique_ptr<Grid2D> manHinhLưới_;
  const std::vector<float>* bangTraCuu_;
 public:
  void ChenDuLieu(const sensor::RangeData& data, 
                  const RangeDataInserterInterface* boChen) {
    CHECK(manHinhLưới_ && !daHoanThanh_);
    boChen->CapNhat(data, manHinhLưới_.get());
    soLuongDuLieu_++;
  }
  
  void DanhDauHoanThanh() {
    CHECK(manHinhLưới_ && !daHoanThanh_);
    manHinhLưới_ = manHinhLưới_->TinHinhLướiCắtCẽ();
    daHoanThanh_ = true;
  }
};

Hệ tọa độ và giới hạn lưới (MapLimits)

Cartographer sử dụng hệ tọa độ lưới khác với ROS chuẩn. Gốc tọa độ nằm ở góc trên bên trái, trục X hướng xuống, trục Y hướng sang phải. Lớp MapLimits chịu trách nhiệm ánh xạ giữa tọa độ vật lý (mét) và chỉ số ô lưới (pixel).

class MapLimits {
 public:
  MapLimits(double phanGiaiQuyet, const Eigen::Vector2d& toaDoMax, 
            const CellLimits& gioiHanO)
      : phanGiaiQuyet_(phanGiaiQuyet), toaDoMax_(toaDoMax), gioiHanO_(gioiHanO) {}
      
  Eigen::Array2i GetIndexFromPoint(const Eigen::Vector2f& physicalPt) const {
    // Công thức nghịch đảo hệ trục: trừ từ tọa độ max rồi chia độ phân giải
    return Eigen::Array2i(
        RoundToInt((toaDoMax_.y() - physicalPt.y()) / phanGiaiQuyet_ - 0.5),
        RoundToInt((toaDoMax_.x() - physicalPt.x()) / phanGiaiQuyet_ - 0.5));
  }
  
  Eigen::Vector2f GetPointFromIndex(const Eigen::Array2i& gridIdx) const {
    return Eigen::Vector2f(
        toaDoMax_.x() - phanGiaiQuyet_ * (gridIdx[1] + 0.5),
        toaDoMax_.y() - phanGiaiQuyet_ * (gridIdx[0] + 0.5));
  }
};

Bảng tra cứu xác suất (ValueConversionTables)

Để tối ưu hiệu năng, Cartographer không tính toán xác suất thời gian thực mà sử dụng bảng tra cứu được tính sẵn. Dữ liệu lưới được lưu dưới dạng uint16 trong khoảng [0, 32767], ánh xạ tuyến tính sang khoảng xác suất trống [0.1, 0.9]. Giá trị 0 đại diện cho vùng chưa khám phá.

std::unique_ptr<std::vector<float>> PrecomputeLookupTable(
    uint16 unknownVal, float unknownProb, float lowerBound, float upperBound) {
  auto table = std::make_unique<std::vector<float>>();
  table->reserve(std::numeric_limits<uint16>::max() + 1);
  
  for (uint16 val = 0; val <= 32767; ++val) {
    if (val == unknownVal) {
      table->push_back(unknownProb);
    } else {
      float scale = (upperBound - lowerBound) / 32766.0f;
      table->push_back(val * scale + (lowerBound - scale));
    }
  }
  return table;
}

Cấu trúc Grid2D và ProbabilityGrid

Grid2D lưu trữ mảng tuyến tính các ô lưới (correspondence_cost_cells_), hộp giới hạn các ô đã biết (known_cells_box_) và danh sách chỉ số đang chờ cập nhật. ProbabilityGrid kế thừa lớp này để áp dụng công thức cập nhật xác suất dựa trên odds.

Khi một ô được cập nhật lần đầu, hệ thống cộng thêm hằng số kUpdateMarker (32768) vào giá trị lưu trữ để đánh dấu trạng thái "đang xử lý". Điều này ngăn chặn các tia radar trùng lặp ghi đè lên cùng một ô trong cùng một khung thời gian. Sau khi hoàn tất khung dữ liệu, hàm FinishUpdate sẽ trừ bỏ cờ đánh dấu này.

void ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& idx, 
                                        const std::vector<uint16>& table) {
  int flatIdx = ToFlatIndex(idx);
  uint16* cellPtr = &mutableCells()[flatIdx];
  
  if (*cellPtr >= kUpdateMarker) return; // Đã được cập nhật trong khung này
  
  updateIndices_.push_back(flatIdx);
  *cellPtr = table[*cellPtr]; // Tra cứu giá trị mới + cờ đánh dấu
  knownCellsBox_.extend(idx.matrix());
}

void Grid2D::FinishUpdate() {
  while (!updateIndices_.empty()) {
    int idx = updateIndices_.back();
    cells_[idx] -= kUpdateMarker; // Loại bỏ cờ tạm thời
    updateIndices_.pop_back();
  }
}

Chèn dữ liệu radar và truy vết tia

Lớp ProbabilityGridRangeDataInserter2D chịu trách nhiệm biến đổi điểm mây thành các tia laser. Nó sử dụng hai bảng tra cứu riêng biệt:

• hit_table: Cập nhật xác suất chướng ngại vật (thường > 0.5)
• miss_table: Cập nhật xác suất không gian trống (thường < 0.5)

Quy trình thực hiện qua ba bước:

1. Mở rộng bản đồ động nếu điểm mây vượt ra ngoài giới hạn hiện tại.
2. Tính toán các tia từ gốc cảm biến đến điểm va chạm (hit) và điểm mất tín hiệu (miss).
3. Áp dụng bảng tra cứu tương ứng lên các ô lưới mà tia đi qua.

Thuật toán Bresenham cải tiến (RayToPixelMask)

Để xác định chính xác các ô lưới mà tia laser đi qua, Cartographer sử dụng phiên bản cải tiến của thuật toán Bresenham. Thay vì làm việc trên độ phân giải pixel gốc, thuật toán hoạt động trên hệ tọa độ subpixel (mặc định tỷ lệ 1:1000) để giảm sai số làm tròn và tăng độ mượt khi vẽ đường thẳng trên lưới rời rạc.

std::vector<Eigen::Array2i> RayToPixelMask(
    const Eigen::Array2i& start, const Eigen::Array2i& end, int subScale) {
  auto result = std::vector<Eigen::Array2i>();
  if (start.x() > end.x()) return RayToPixelMask(end, start, subScale);
  
  int64 dx = end.x() - start.x();
  int64 dy = end.y() - start.y();
  int64 denom = 2 * subScale * dx;
  
  Eigen::Array2i current = start / subScale;
  result.push_back(current);
  
  int64 subY = (2 * (start.y() % subScale) + 1) * dx;
  int firstPixel = 2 * subScale - 2 * (start.x() % subScale) - 1;
  
  subY += dy * firstPixel;
  
  while (current.x() < end.x() / subScale) {
    if (!isEqual(result.back(), current)) result.push_back(current);
    
    while (subY > denom) {
      subY -= denom;
      current.y()++;
      if (!isEqual(result.back(), current)) result.push_back(current);
    }
    current.x()++;
    
    if (subY == denom) {
      subY -= denom;
      current.y()++;
    }
    if (current.x() == end.x() / subScale) break;
    
    subY += dy * 2 * subScale;
  }
  return result;
}

Hàm CastRays và GrowLimits

Trước khi vẽ tia, hệ thống kiểm tra bounding box của dữ liệu radar. Nếu điểm cuối vượt khỏi giới hạn lưới hiện tại, GrowLimits sẽ tự động mở rộng bản đồ bằng cách nhân đôi số ô theo trục X và Y, dịch chuyển gốc tọa độ và sao chép dữ liệu cũ sang vùng bộ nhớ mới.

void CastRays(const sensor::RangeData& radarData,
              const std::vector<uint16>& hitTable,
              const std::vector<uint16>& missTable,
              bool insertFreeSpace, ProbabilityGrid* grid) {
  GrowAsNeeded(radarData, grid);
  
  const auto& limits = grid->limits();
  Eigen::Array2i originIdx = limits.GetCellIndex(radarData.origin.head<2>());
  
  // Xử lý điểm va chạm (Hit)
  for (const auto& hit : radarData.returns) {
    Eigen::Array2i endIdx = limits.GetCellIndex(hit.position.head<2>());
    grid->ApplyLookupTable(endIdx / kSubpixelScale, hitTable);
  }
  
  if (!insertFreeSpace) return;
  
  // Xử lý không gian trống (Miss) dọc theo tia
  for (const auto& hit : radarData.returns) {
    Eigen::Array2i endIdx = limits.GetCellIndex(hit.position.head<2>());
    auto rayPath = RayToPixelMask(originIdx, endIdx, kSubpixelScale);
    for (const auto& cell : rayPath) {
      grid->ApplyLookupTable(cell, missTable);
    }
  }
}

Luồng tích hợp trong LocalTrajectoryBuilder2D

Khi dữ liệu cảm biến được nhận, LocalTrajectoryBuilder2D thực hiện chuỗi xử lý: lọc nhiễu, đối trọng, khớp quét và cuối cùng gọi InsertIntoSubmap. Kết quả trả về là đối tượng InsertionResult chứa các con trỏ đến các submap vừa được cập nhật, đồng thời kích hoạt callback để đẩy bản đồ cục bộ lên các node ROS phía trên.

Cơ chế này đảm bảo quá trình xây dựng bản đồ diễn ra song song với ước lượng tư thế, duy trì cân bằng giữa độ chính xác xác suất và yêu cầu xử lý thời gian thực.