自动驾驶与机器人中的SLAM技术--第十章--自动驾驶车辆的离线地图构建

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

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

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

        在点云定位层面，使用第9章切分好的点云进行定位，每个网格边长范围为100米。本章控制点云地图的载入范围为车辆周边九格。 同时，为了防止车辆在某个区块边缘处运动导致频繁加载与卸载，笔者也设定了一个卸载范围。当某个网格离车距离超过卸载范围，就将它卸载。卸载范围稍大于加载范围(实现中取3格)，超出该范围的点云才会卸载。

10.2 算法实现

10.2.1 RTK初始搜索

        当系统未收到第一个有效的RTK信号时，无法知道自身在地图中的位置，也就无法进行点云定位，只有收到了首个有效的RTK信号，才能开始在周边进行网格搜索。
        通过给LoadMap()传入RTK位姿，让其加载位姿附近的点云图，并卸载掉离得较远的点云图，然后重置ndt目标图。

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_);

    ...
}

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_);
}

        由于RTK不带角度，按10度的固定步长进行360度搜索，需要搜索36次。
        将每次旋转10度后的位姿送入AlignForGrid()进行多分辨率NDT配准，求解配准后的位姿。

bool Fusion::SearchRTK() {
    
    ...

    /// 由于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); });

    ...
}

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());
}

        根据ndt分数选择最优的匹配结果， 若最优结果满足条件，则初始化成功。
        重置滤波器状态，将最优匹配的位姿传给滤波器，并设置协方差等信息。

bool Fusion::SearchRTK() {
    
    ...

    // 选择最优的匹配结果
    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;
}

        如果RTK初始化成功，则系统进入正常工作模式。

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();
}

        在Align()函数中，如果status_状态是WAITING_FOR_RTK且存在最近的RTK信号，则尝试初始化。如果RTK初始化成功，则上传包含最优匹配位姿的名义状态变量。
        如果status_状态不是WAITING_FOR_RTK，则进入LidarLocalization()函数。在函数中，首先加载滤波器预测位姿附近的地图，然后把预测位姿和地图进行NDT配准，使用配准后的位姿作为观测进行滤波器更新。

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());
    }
}

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;
}

        本节向读者演示了点云融合定位的效果。可以看到，点云定位通常比RTK定位具有更好的鲁棒性。只要点云地图本身构建得足够准确，在大部分地图区域内它都可以正常工作，定位系统不必依赖室外RTK信号的好坏。

10.3 本章小结

        本章以第9章的点云建图结果为基础，演示了在已有激光点云地图的基础上，进行融合定位的案例。点云定位的几个优势和劣势可以概括如下。

        1. 点云定位结果与场景结构相关，而与天气、遮挡无关，通常比RTK具有更好的环境适应性。点云定位既可以用于室外，也可以用于室内，没有太多限制因素。

        2. 点云定位的结果可以和RTK一样，融入ESKF滤波器中，也可以像第4章那样组成一个图优化系统。

        3. 如果在较大的场景中使用，应该先对地图点云切分，每次加载周围一小块区域用于定位。

        4. 与RTK 不同的是，点云定位需要事先指定一个大致的位置，而不是像 RTK那样直接出定位结果。如果初始位置精度不够，则需要对附近的位置和角度进行网格搜索，才能确定自身位置。

        5. 点云定位对动态场景有一定的适应性。只要主体结构(通常是建筑物)变化不大，一些小型的动态物体(人群、车辆等)通常不会对点云定位产生很大的干扰。

        6. 如果定位场景属于开阔场地，还可以对点云地图进行压缩，形成2.5D地图。这种压缩之后的地图可以大幅减少所需的存储空间，对自动驾驶应用更加友好。