Mô phỏng SLAM cơ bản với cảm biến LIDAR trong Qt/C++

Một ứng dụng đồ họa tương tác được xây dựng bằng Qt và C++ nhằm minh họa nguyên lý lập bản đồ môi trường thông qua dữ liệu mô phỏng từ cảm biến LIDAR. Chương trình khởi tạo một không gian 2D ngẫu nhiên, cho phép người dùng điều khiển vị trí của robot bằng các phím mũi tên. Trong quá trình di chuyển, hệ thống liên tục cập nhật bản đồ điểm (point cloud) ở góc trên bên phải — mỗi điểm đại diện cho vị trí vật cản mà tia laser chạm tới. Góc dưới bên phải hiển thị hướng hiện tại của robot dưới dạng kim la bàn động. Người dùng cũng có thể nhấp chuột trái vào vùng bản đồ để chèn chướng ngại vật thủ công, đồng thời toàn bộ hành động đều được ghi log chi tiết.

Ứng dụng đã được kiểm thử thành công trên hai nền tảng: Windows với trình biên dịch MinGW và Ubuntu 18.04, sử dụng các phiên bản Qt 5.13.1 và 5.15.1. Các phiên bản Qt khác có thể yêu cầu điều chỉnh nhỏ trong cấu hình dự án.

Ngay khi nhấn phím mũi tên, robot màu đỏ di chuyển mượt mà từ vị trí trung tâm, để lại dấu vết chuyển động. Đồng thời, bản đồ điểm bắt đầu mở rộng dần như mực lan trên giấy — một cách trực quan hóa hiệu quả quá trình thu thập dữ liệu cảm biến. Kim la bàn ở góc dưới phải xoay theo góc quay thực tế của robot, phản ánh chính xác trạng thái định hướng.

Lớp EnvironmentGenerator chịu trách nhiệm tạo bản đồ khởi tạo:

void EnvironmentGenerator::buildRandomLayout(int width, int height) {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<double> prob(0.0, 1.0);

    for (int col = 0; col < width; col += 12) {
        for (int row = 0; row < height; row += 12) {
            if (prob(gen) < 0.18) {
                obstacles.append(QPointF(col + 6.0, row + 6.0));
            }
        }
    }
}

Hàm này chia không gian thành lưới ô vuông kích thước 12×12 pixel và đặt chướng ngại vật ngẫu nhiên tại tâm mỗi ô với xác suất 18%. Việc cộng thêm 6.0 vào tọa độ đảm bảo vị trí chướng ngại vật luôn nằm đúng trung tâm ô, giúp tối ưu hóa độ chính xác trong các phép kiểm tra va chạm sau đó.

Xử lý sự kiện bàn phím được triển khai trong lớp điều khiển chính:

void SimulationController::handleKeyPress(QKeyEvent* e) {
    const double displacement = 2.8;
    QPointF offset(0, 0);

    switch (e->key()) {
        case Qt::Key_A: offset.setX(-displacement); break;
        case Qt::Key_D: offset.setX(+displacement); break;
        case Qt::Key_W: offset.setY(-displacement); break;
        case Qt::Key_S: offset.setY(+displacement); break;
        default: return;
    }

    QPointF candidatePos = robotPosition + offset;
    if (!environment.isColliding(candidatePos)) {
        robotPosition = candidatePos;
        triggerLaserScan();
    }
}

Cơ chế điều khiển sử dụng các phím WASD thay vì mũi tên, giúp dễ phân biệt hơn trong môi trường phát triển đa nền tảng. Trước khi cập nhật vị trí, hệ thống luôn thực hiện kiểm tra tiên đoán va chạm — nếu vị trí đề xuất nằm trong vùng chiếm bởi chướng ngại vật, thao tác sẽ bị bỏ qua nhằm tránh hiện tượng "lọt tường".

Mô phỏng chùm tia LIDAR được thực hiện bằng vòng lặp góc kết hợp hàm lượng giác:

void LaserScanner::performSweep(const QPointF& origin, qreal heading) {
    scanPoints.clear();
    const int angularResolution = 4; // 4 độ/góc quét
    const qreal maxDistance = 120.0;

    for (int deg = 0; deg < 360; deg += angularResolution) {
        qreal radian = qDegreesToRadians(deg + heading);
        QPointF direction(qCos(radian), qSin(radian));

        for (qreal dist = 0.0; dist <= maxDistance; dist += 0.6) {
            QPointF point = origin + direction * dist;
            if (environment.containsObstacle(point)) {
                scanPoints.append(point);
                break;
            }
        }
    }
}

Vòng lặp ngoài quét toàn bộ dải góc 0–359° với bước 4°, tạo ra tổng cộng 90 tia. Với mỗi tia, thuật toán "bắn" từng đoạn ngắn 0.6 đơn vị chiều dài cho đến khi chạm vật cản hoặc vượt ngưỡng tầm xa tối đa (120 đơn vị). Giá trị 0.6 là kết quả cân bằng giữa độ phân giải không gian và hiệu năng xử lý — giảm xuống sẽ làm chậm tốc độ khung hình, tăng lên sẽ gây mất chi tiết.

Bản đồ điểm được vẽ lại trên QGraphicsScene mỗi lần quét hoàn tất:

void PointCloudRenderer::refresh(const QList<QPointF>& points) {
    scene->clear();
    
    // Vẽ các điểm quét
    for (const auto& pt : points) {
        scene->addEllipse(pt.x() - 1.0, pt.y() - 1.0, 2.0, 2.0,
                           QPen(Qt::blue, 0.5), QBrush(Qt::blue));
    }

    // Vẽ biểu tượng robot
    scene->addEllipse(robotCenter.x() - 4.0, robotCenter.y() - 4.0,
                        8.0, 8.0, QPen(Qt::red, 1.2), QBrush(Qt::darkRed));
}

Mỗi điểm được biểu diễn bằng ellipse bán kính 1 pixel, trong khi robot được thể hiện bằng ellipse lớn hơn có viền rõ ràng. Mặc dù phương pháp xóa toàn bộ scene rồi vẽ lại là đơn giản, nó vẫn đủ hiệu quả với số lượng điểm thường dưới 100 trong mỗi lần quét.

Tính năng thêm chướng ngại vật bằng chuột được kết nối trực tiếp với sự kiện nhấn chuột trái của QGraphicsScene:

QObject::connect(scene, &QGraphicsScene::mousePressEvent,
                 [this](QGraphicsSceneMouseEvent* evt) {
    if (evt->button() == Qt::LeftButton) {
        QPointF worldCoord = evt->scenePos();
        environment.insertBarrier(worldCoord);
        logger.record(QString("Chướng ngại vật mới tại (%1, %2)")
                      .arg(worldCoord.x(), 0, 'f', 1)
                      .arg(worldCoord.y(), 0, 'f', 1));
    }
});

Sử dụng lambda để bao bọc logic xử lý giúp giữ code gọn và dễ bảo trì. Lưu ý rằng scenePos() trả về tọa độ trong hệ quy chiếu của scene — không cần chuyển đổi từ tọa độ màn hình, nhờ đó loại bỏ rủi ro sai lệch do scale hoặc transform.

Nếu gặp lỗi thiếu module khi biên dịch, hãy bổ sung dòng sau vào tập tin .pro:

QT += charts

Đây là yêu cầu bắt buộc nếu ứng dụng sử dụng biểu đồ hoặc widget liên quan đến Qt Charts — một phần phụ trợ không được bật mặc định trong hầu hết cấu hình Qt.

Thẻ: Qt C++ lidar SLAM Simulation

Đăng vào ngày 16 tháng 7 lúc 19:35