《自动驾驶与机器人中的SLAM技术》ch10：自动驾驶车辆的实时定位系统

🕗 发布于 2025-01-15 07:01 自动驾驶机器人 人工智能

本章，我们来关注实时的激光雷达定位系统。在点云地图基础之上，我们可以把当前激光扫描数据与地图进行匹配，从而获得车辆自身的位置，再与 IMU 等传感器进行滤波器融合。然而，点云定位并不像 RTK 那样可以直接给出物理世界坐标，而必须先给出一个大致的位置点，再引导点云配准算法收敛。因此，点云定位在实际使用时，会遇到一些特有逻辑问题。本章将使用第 9 章构建的点云地图，展示点云定位的使用方法，并演示一个基于 ESKF 的实时定位方案。

1 点云融合定位的设计方案

从融合手段上来看，点云融合定位可以与传统组合导航一样，使用 ESKF 进行融合；也可以像现有的 SLAM 系统一样，使用位姿图优化进行融合。

①卡尔曼方案从设计到实现都比较简单，其结果往往比较光滑，但可能收敛到错误的解，导致定位跑偏，而且不容易被修正回来。
②图优化方法容易检查各种因子与定位状态的偏差，从而在逻辑层面处理各种异常情况，但其定位平滑性不容易保障（除非我们和第 8 章一样进行手动的边缘化处理，或者使用 GTSAM 这种自带边缘化的优化库）。

点云融合定位算法的框图如下所示。本章将演示基于 ESKF 的定位方案。由于卡尔曼滤波器原理已经在第 3 章中展开介绍过了，本章重点关注点云定位与卡尔曼滤波器的融合部分。点云定位需要一个预测的车辆位置作为搜索的起点，因此设计了一个初始化流程。当滤波器尚未计算出自身位置时，利用第一个有效 RTK 信号来控制点云定位的搜索范围。又由于 NCLT 数据集中的 RTK 并不含有姿态信息，我们需要通过网格搜索来确定车辆的朝向。当卡尔曼滤波器收敛以后，我们再通过滤波器的预测值来作为点云定位的初值进行配准。

2 点云融合定位的流程

1. 将 ROS 包中的 RTK 数据提取出来，用第一个有效的 RTK 数据减去地图原点（建图时第一个有效的 RTK 位姿，定位时从配置文件中加载即可）后加载点云地图，并使用该 RTK 数据进行初始化网格搜索确定车辆在点云地图中的初始位姿。确定位姿后使用该位姿初始化 ESKF 状态。
2. 使用 IMU 数据开始 ESKF 的预测过程，和《自动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航中一样。
3.先使用 ESKF 预测得到的位姿加载点云地图，将当前 LIDAR 数据作为 scan 和点云地图进行 scan to map 的 NDT 配准（使用 PCL 库中的 NDT 配准函数），迭代后得到优化位姿。使用该优化位姿进行 ESKF 的更新过程。

void Fusion::ProcessMeasurements(const MeasureGroup& meas) {
    measures_ = meas;

    if (imu_need_init_) {
        TryInitIMU();
        return;
    }

    /// 以下三步与LIO一致，只是align完成地图匹配工作
    if (status_ == Status::WORKING) {
        Predict();
        Undistort();
    } else {
        scan_undistort_ = measures_.lidar_;
    }

    Align();
}

void Fusion::Align() {
    FullCloudPtr scan_undistort_trans(new FullPointCloudType);
    pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix());
    scan_undistort_ = scan_undistort_trans;
    current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
    current_scan_ = VoxelCloud(current_scan_, 0.5);

    if (status_ == Status::WAITING_FOR_RTK) {
        // 若存在最近的RTK信号，则尝试初始化
        if (last_gnss_ != nullptr) {
            if (SearchRTK()) {
                status_ == Status::WORKING;
                ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
                ui_->UpdateNavState(eskf_.GetNominalState());
            }
        }
    } else {
        LidarLocalization();
        ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
        ui_->UpdateNavState(eskf_.GetNominalState());
    }
}

3 算法实现

3.1 RTK 初始搜索

1. 将 ROS 包中的 RTK 数据提取出来，用第一个有效的 RTK 数据减去地图原点（建图时第一个有效的 RTK 位姿，定位时从配置文件中加载即可）后加载点云地图，并使用该 RTK 数据进行初始化网格搜索确定车辆在点云地图中的初始位姿。确定位姿后使用该位姿初始化 ESKF 状态。

网格搜索的结构：

    /// 网格搜索时的结构
    struct GridSearchResult {
        SE3 pose_;           // 按 10 度递增的网格搜索的初始位姿
        SE3 result_pose_;    // NDT 配准优化后的位姿
        double score_ = 0.0; // NDT 配准得分
    };

由于 RTK 不带角度，按 10 度的固定步长进行 360 度搜索，需要搜索 36 次。将按 10 度递增的网格搜索的初始位姿送入 AlignForGrid() 函数中进行多分辨率 NDT 配准（10.0, 5.0, 4.0, 3.0。map 按照 NDT 尺寸乘以 0.1 降采样，scan 不额外进行降采样），求解配准后的位姿及其对应的配准得分，如果最大配准得分大于阈值（4.5）则认为获得了正确的初始位姿。确定位姿后使用该位姿初始化 ESKF 状态。

bool Fusion::SearchRTK() {
    if (init_has_failed_) {
        if ((last_gnss_->utm_pose_.translation() - last_searched_pos_.translation()).norm() < 20.0) {
            LOG(INFO) << "skip this position";
            return false;
        }
    }

    // 由于RTK不带姿态，我们必须先搜索一定的角度范围
    std::vector<GridSearchResult> search_poses;
    LoadMap(last_gnss_->utm_pose_);

    /// 由于RTK不带角度，这里按固定步长扫描RTK角度
    double grid_ang_range = 360.0, grid_ang_step = 10;  // 角度搜索范围与步长
    for (double ang = 0; ang < grid_ang_range; ang += grid_ang_step) {
        SE3 pose(SO3::rotZ(ang * math::kDEG2RAD), Vec3d(0, 0, 0) + last_gnss_->utm_pose_.translation());
        GridSearchResult gr;
        gr.pose_ = pose;
        search_poses.emplace_back(gr);
    }

    LOG(INFO) << "grid search poses: " << search_poses.size();
    std::for_each(std::execution::par_unseq, search_poses.begin(), search_poses.end(),
                  [this](GridSearchResult& gr) { AlignForGrid(gr); });

    // 选择最优的匹配结果
    auto max_ele = std::max_element(search_poses.begin(), search_poses.end(),
                                    [](const auto& g1, const auto& g2) { return g1.score_ < g2.score_; });
    LOG(INFO) << "max score: " << max_ele->score_ << ", pose: \n" << max_ele->result_pose_.matrix();
    if (max_ele->score_ > rtk_search_min_score_) {
        LOG(INFO) << "初始化成功, score: " << max_ele->score_ << ">" << rtk_search_min_score_;
        status_ = Status::WORKING;

        /// 重置滤波器状态
        auto state = eskf_.GetNominalState();
        state.R_ = max_ele->result_pose_.so3();
        state.p_ = max_ele->result_pose_.translation();
        state.v_.setZero();
        eskf_.SetX(state, eskf_.GetGravity());

        ESKFD::Mat18T cov;
        cov = ESKFD::Mat18T::Identity() * 1e-4;
        cov.block<12, 12>(6, 6) = Eigen::Matrix<double, 12, 12>::Identity() * 1e-6;
        eskf_.SetCov(cov);

        return true;
    }

    init_has_failed_ = true;
    last_searched_pos_ = last_gnss_->utm_pose_;
    return false;
}

void Fusion::AlignForGrid(sad::Fusion::GridSearchResult& gr) {
    /// 多分辨率
    pcl::NormalDistributionsTransform<PointType, PointType> ndt;
    ndt.setTransformationEpsilon(0.05);
    ndt.setStepSize(0.7);
    ndt.setMaximumIterations(40);

    ndt.setInputSource(current_scan_);
    auto map = ref_cloud_;

    CloudPtr output(new PointCloudType);
    std::vector<double> res{10.0, 5.0, 4.0, 3.0};
    Mat4f T = gr.pose_.matrix().cast<float>();
    for (auto& r : res) {
        auto rough_map = VoxelCloud(map, r * 0.1);
        ndt.setInputTarget(rough_map);
        ndt.setResolution(r);
        ndt.align(*output, T);
        T = ndt.getFinalTransformation();
    }

    gr.score_ = ndt.getTransformationProbability();
    gr.result_pose_ = Mat4ToSE3(ndt.getFinalTransformation());
}

3.2 使用 IMU 数据进行 ESKF 的预测过程

2.使用 IMU 数据开始 ESKF 的预测过程，和《自动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航中一样。

void Fusion::Predict() {
    imu_states_.clear();
    imu_states_.emplace_back(eskf_.GetNominalState());

    /// 对IMU状态进行预测
    for (auto& imu : measures_.imu_) {
        eskf_.Predict(*imu);
        imu_states_.emplace_back(eskf_.GetNominalState());
    }
}

3.3 使用 LIDAR 数据 NDT 配准位姿进行 ESKF 的更新过程

3.先使用 ESKF 预测得到的位姿加载点云地图，将当前 LIDAR 数据作为 scan 和点云地图进行 scan to map 的 NDT 配准（使用 PCL 库中的 NDT 配准函数），迭代后得到优化位姿。使用该优化位姿进行 ESKF 的更新过程。

bool Fusion::LidarLocalization() {
    SE3 pred = eskf_.GetNominalSE3();
    LoadMap(pred);

    ndt_.setInputCloud(current_scan_);
    CloudPtr output(new PointCloudType);
    ndt_.align(*output, pred.matrix().cast<float>());

    SE3 pose = Mat4ToSE3(ndt_.getFinalTransformation());
    eskf_.ObserveSE3(pose, 1e-1, 1e-2);

    LOG(INFO) << "lidar loc score: " << ndt_.getTransformationProbability();

    return true;
}

3.4 点云地图的加载和卸载

在定位过程中，初始化阶段在 Fusion::Init() 函数中调用 LoadMapIndex() 函数加载了点云地图的索引文件。后续在 2 个地方调用了 Fusion::LoadMap() 函数加载和卸载点云地图。分别在 RTK 初始搜索函数 Fusion::SearchRTK() 和 ESKF 更新函数 Fusion::LidarLocalization() 中。

点云定位层面，我们使用第 9 章切分好的点云进行定位。第 9 章中，我们把点云按边长 100米的范围进行了网格划分，所以本章控制点云地图的载入范围为车辆周边 9 格（如下图所示）。同时，为了防止车辆在某个区块边缘处运动导致频繁加载与卸载，我们也设定一个卸载范围，卸载范围稍大于加载范围，卸载范围取和当前测量距离大于，超出该范围的点云才会被卸载。

代码实现如下所示。这里使用了 std::set<Vec2i, less_vec<2>> 存储地图索引、使用std::map<Vec2i, CloudPtr, less_vec<2>> 存储地图数据。即使用 C++ STL 容器中的 std::set 和 std::map 来管理和加载地图数据。

void Fusion::LoadMap(const SE3& pose) {
    int gx = floor((pose.translation().x() - 50.0) / 100);
    int gy = floor((pose.translation().y() - 50.0) / 100);
    Vec2i key(gx, gy);

    // 一个区域的周边地图，我们认为9个就够了
    std::set<Vec2i, less_vec<2>> surrounding_index{
        key + Vec2i(0, 0), key + Vec2i(-1, 0), key + Vec2i(-1, -1), key + Vec2i(-1, 1), key + Vec2i(0, -1),
        key + Vec2i(0, 1), key + Vec2i(1, 0),  key + Vec2i(1, -1),  key + Vec2i(1, 1),
    };

    // 加载必要区域
    bool map_data_changed = false;
    int cnt_new_loaded = 0, cnt_unload = 0;
    for (auto& k : surrounding_index) {
        if (map_data_index_.find(k) == map_data_index_.end()) {
            // 该地图数据不存在
            continue;
        }

        if (map_data_.find(k) == map_data_.end()) {
            // 加载这个区块
            CloudPtr cloud(new PointCloudType);
            pcl::io::loadPCDFile(data_path_ + std::to_string(k[0]) + "_" + std::to_string(k[1]) + ".pcd", *cloud);
            map_data_.emplace(k, cloud);
            map_data_changed = true;
            cnt_new_loaded++;
        }
    }

    // 卸载不需要的区域，这个稍微加大一点，不需要频繁卸载
    for (auto iter = map_data_.begin(); iter != map_data_.end();) {
        if ((iter->first - key).cast<float>().norm() > 3.0) {
            // 卸载本区块
            iter = map_data_.erase(iter);
            cnt_unload++;
            map_data_changed = true;
        } else {
            iter++;
        }
    }

    LOG(INFO) << "new loaded: " << cnt_new_loaded << ", unload: " << cnt_unload;
    if (map_data_changed) {
        // rebuild ndt target map
        ref_cloud_.reset(new PointCloudType);
        for (auto& mp : map_data_) {
            *ref_cloud_ += *mp.second;
        }

        LOG(INFO) << "rebuild global cloud, grids: " << map_data_.size();
        ndt_.setInputTarget(ref_cloud_);
    }

    ui_->UpdatePointCloudGlobal(map_data_);
}

4 算法结果

点云定位通常比 RTK 定位具有更好的鲁棒性。只要点云地图本身建的足够准确，在大部分地图区域内它都可以正常工作，定位系统不必依赖室外 RTK 信号的好坏。

在测试时，可以使用 4 月份的数据进行建图，然后用 5 月、6 月的数据测试定位，这样可以包含一部分动态物体的情况。

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_fusion_offline

I0114 22:58:41.102108 436026 cloud_convert.cc:193] Using Velodyne 32 Lidar
I0114 22:59:04.802245 436026 io_utils.cc:77] running in /home/wu/slam_in_autonomous_driving/data/nclt/20130110.bag, reg process func: 3
Failed to find match for field 'intensity'.
Failed to find match for field 'intensity'.
Failed to find match for field 'intensity'.
Failed to find match for field 'intensity'.
I0114 22:59:05.155918 436026 static_imu_init.cc:86] mean acce: -0.133286 0.0166332 009.80364
I0114 22:59:05.155944 436026 static_imu_init.cc:109] IMU 初始化成功，初始化时间= 9.9985, bg = 00.00159973 0000.003007 -0.00239681, ba = 07.40016e-05 -9.23492e-06 0-0.00544309, gyro sq = 00.00067222 3.29092e-06 3.63085e-05, acce sq = 7.68385e-06 00.00197847 1.57072e-06, grav = 0000.13336 -0.0166424 00-9.80908, norm: 9.81
I0114 22:59:05.155958 436026 static_imu_init.cc:113] mean gyro: 00.00159973 0000.003007 -0.00239681 acce: 07.40016e-05 -9.23492e-06 0-0.00544309
imu try init true time:1357847247.28515291
I0114 22:59:05.155977 436026 fusion.cc:88] IMU初始化成功
Failed to find match for field 'intensity'.
I0114 22:59:06.284561 436026 fusion.cc:287] new loaded: 7, unload: 0
I0114 22:59:06.293992 436026 fusion.cc:295] rebuild global cloud, grids: 7
I0114 22:59:06.323706 436026 fusion.cc:172] grid search poses: 36
I0114 22:59:07.356271 436026 fusion.cc:179] max score: 9.63038, pose: 
00-0.989445 0000.141687 0-0.0303793 00-0.941667
00-0.141768 0000-0.9899 0.000527289 0000.593236
0-0.0299978 00.00482854 0000.999538 0000.682519
00000000000 00000000000 00000000000 00000000001
I0114 22:59:07.356317 436026 fusion.cc:181] 初始化成功, score: 9.63038>4.5
Failed to find match for field 'intensity'.
---IMU 预测
I0114 22:59:07.377723 436026 eskf.hpp:233] skip this imu because dt_ = 1.35785e+09
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
I0114 22:59:07.378505 436026 fusion.cc:287] new loaded: 0, unload: 0
---SE3 观测
I0114 22:59:07.442410 436026 fusion.cc:240] lidar loc score: 2.03312
Failed to find match for field 'intensity'.
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
---IMU 预测
I0114 22:59:07.443528 436026 fusion.cc:287] new loaded: 0, unload: 0
---SE3 观测
I0114 22:59:07.449277 436026 fusion.cc:240] lidar loc score: 2.18825

5 本章小结

本章以第 9 章的点云建图结果为基础，演示了如何在已有激光点云地图的基础上，进行融合定位的案例。点云定位的几个优势和劣势可以概括如下：
1. 点云定位结果与场景结构相关，而与天气、遮挡关系无关，通常比 RTK 具有更好的环境适应性。点云定位既可以用于室外，也可以用于室内，没有太多限制因素。
2. 点云定位结果可以和 RTK 一样，融入 ESKF 滤波器中，也可以像第 4 章那样组成一个图优化系统。
3. 如果在较大的场景中使用，应该先对地图点云切分，每次加载周围一小块区域用于定位。
4. 与 RTK 不同的时，点云定位需要事先指定一个大致的位置，而不是像 RTK 那直接给出定位结果。如果初始位置精度不够，我们还需要对附近的位置和角度进行网格搜索，才能确定自身位置。
5. 点云定位对动态场景有一定的适应性。只要主体结构（通常是建筑物）变化不大，一些小型的动态物体（人群、车辆等等）通常不会对点云定位产生很大的干扰。
6. 如果定位场景属于开阔场地，我们还可以对点云地图进行压缩，形成 2.5D 地图 。这种压缩之后的地图可以大幅减少所需的存储空间，对自动驾驶应用更加友好。

原文地址：https://blog.csdn.net/m0_49384824/article/details/145144611

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：【论文笔记】SmileSplat：稀疏视角+pose-free+泛化
下一篇：ros2笔记-6.5 使用ros2_control驱动机器人

服务端开发模式-thinkphp-重新整理workman
服务端开发模式-thinkphp-重新整理workman
阅读更多2025-01-15
C语言导航 7.2函数的调用
2.方式：对于有返回值的函数，可在调用时对函数进行运算操作，对于没有返回值的则不可以；①在调用函数时，虽然形参与实参均表示原始数据，但对于机器来说，形参是一个另外开辟空间的相同值，即对形参的操作是相同
阅读更多2025-01-15
[IGP]ospf ip frr 快速重路由技术
可以看到，AR1访问AR3的下一跳为10.1.1.2，访问路径AR1-AR2-AR3，此时并没有计算备份路径的路由，因此，这条路径出现错误后，AR1需要再次SPF计算。这里我们看得到，多了一个下一跳备
阅读更多2025-01-15
HarmonyOS鸿蒙-@State@Prop装饰器限制条件
4.@State不支持装饰Function类型的变量，框架会抛出运行时错误。1.@State装饰的变量必须初始化，否则编译期会报错。3.数组项中属性的赋值观察不到。2.嵌套属性的赋值观察不到。
阅读更多2025-01-15
JavaScript系列（26）--安全编程实践详解
JavaScript之旅第二十六站
阅读更多2025-01-15
基于spingboot+html技术的博客网站
基于spingboot+html技术的博客网站，实现宠物爱友的学习、分享
阅读更多2025-01-15
【人工智能语音识别】——深入详解人工智能语音信号处理：理解语音信号的特征提取与表示
深入详解人工智能语音识别之语音信号处理：理解语音信号的特征提取与表示
阅读更多2025-01-15
数据结构—《二叉树的定义与特性》
二叉树的定义、性质、存储结构。特殊的二叉树，满二叉树、完全二叉树、二叉排序树、平衡二叉树、正则二叉树；二叉树的存储结构，顺序存储、链式存储。二叉树是一种特殊的树形结构，其特点是每个结点至多只有两棵子树
阅读更多2025-01-15
【蓝桥杯】Python算法——快速幂
如何快速求a^b=p？如果直接循环aaa…毫无疑问时间复杂度是很大的，那么怎么降低计算量呢？快速幂就是从幂运算的性质出发，提出的优化。
阅读更多2025-01-15
《鸿蒙Next ArkTS：开启人工智能应用开发高效新旅程》
它针对鸿蒙系统的特性进行了优化，可更好地利用鸿蒙的底层能力。利用鸿蒙Next的ArkTS语言开发高效的人工智能驱动应用程序，需要开发者深入理解ArkTS语言特性，充分利用鸿蒙系统的优势和相关工具，不断
阅读更多2025-01-15