[人工智能]多传感器融合定位 (单天线gps+ndt匹配)初始化重定位

多传感器融合定位 (单天线gps+ndt匹配)初始化重定位

参考博客：多传感器融合定位 第四章 点云地图构建及基于点云地图定位

代码下载：https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/%E9%99%84:%E5%8D%95%E5%A4%A9%E7%BA%BFgps-%E5%9F%BA%E4%BA%8E%E5%85%88%E9%AA%8C%E5%9C%B0%E5%9B%BE%E9%87%8D%E5%AE%9A%E4%BD%8D

最近工作比较繁忙，所以课程第九章预积分开始的内容，并没有跟上，在之后的日子里会慢慢跟上~

最近项目使用自己的数据集跑定位，目前使用的设备为 双天线RTK + 九轴IMU + 16线Lidar

存在问题：

1.九轴的IMU磁场计受到较大的影响，导致姿态不准确，并且客户每次使用都需要校准，不太方便；

2.为了节约成本有时会使用单天线gps和六轴IMU，只能得到初始大概位置和基于上电时刻的相对姿态，不能得到导航系下的姿态；

解决：

鉴于上述的实际问题，考虑采用 （gps 初始化position、PointCloud_Registration初始化的方法）进行重定位初始化

1.代码框架

整体代码框架为基于先验地图的定位，在 多传感器融合定位 第七章 基于滤波的融合方法的代码框架基础上进行开发。

初始化位姿(POSE)包括两部分 ： 初始化位置(POSI) 初始化姿态(ORI)

1.1初始化位置(POSI):

使用GPS获得当前车体所在的经纬高，并转换为导航系下的导航系(XYZ)坐标，并获取当前所在位置的局部地图

1.2初始化姿态(ORI):

当前点云地图 和 上一步获取的局部地图 进行点云配准，获取初始姿态角。

1.3点云配准算法：

NDT、ICP、Loam 、 ScanContext 均可， 本次使用的配准算法为pcl_ndt

2.算法实现流程

当前的点云 m_current、局部地图的点云 m_local、ndt匹配的先验位姿 predict_pose、ndt匹配后的得分 fitness_score

#step 1     获取当前的点云数据和gnss数据
#step 2    通过gnss返回的载体当前所在地图上的坐标,分割出局部地图的点云 m_local
#step 3    初始化ndt匹配的先验位姿 predict_pose 的 旋转部分 为单位阵、位置部分为step2得到的 posi
#step 3    将360度分成72份，每旋转5度为1份，predict_pose 每次绕Z轴旋转5度，输入到pcl_ndt中，进行 m_current  m_local 点云匹配,  并输出匹配后的得分 score
#step 4     选出得分最低的配准结果对应的旋转矩阵temp_init_rotation，作为定位初始姿态

trick： 进行72次(每5度)一次的旋转匹配中，发现正确的姿态和错误的姿态，在score上有较大的差异，如下表所示，为输入 predict_pose 旋转 5-360 ndt 匹配分数，正确的匹配角度为 60-65 之间，score分数对比其他错误姿态明显很小，为了加速初始化，可通过设置score阈值的方法进行加速初始化。

3.核心代码

3.1 根据gps分割出的局部地图进行ndt配准，根据配准score 获取初始化姿态

FILE : kitti_filtering.cpp

bool KITTIFiltering::InitOri(Eigen::Matrix4f &init_pose,   CloudData &cloud_data ,  Eigen::Matrix3f &init_ori ){        //  use gnss and though ndt get  init_ori
      init_pose.block<3,3>(0,0)  =   Eigen::Matrix3f::Identity();       //   初始化旋转部分为单位阵
      Eigen::Matrix4f  predict_pose =  init_pose;      

      ResetLocalMap(predict_pose(0, 3), predict_pose(1, 3), predict_pose(2, 3));         //   根据gnss 的posi加载局部地图
       // remove invalid measurements:
      std::vector<int> indices;
      pcl::removeNaNFromPointCloud(*cloud_data.cloud_ptr, *cloud_data.cloud_ptr,
                              indices);
      // downsample:
      CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());
      current_scan_filter_ptr_->Filter(cloud_data.cloud_ptr, filtered_cloud_ptr);

      Eigen::Matrix4f  temp_init_pose  =  Eigen::Matrix4f::Identity();     //  ndt 匹配得到的变换矩阵
       int iter_rate  =72;
       for(int i = 1; i <  iter_rate + 1;  i++){     //  分成72份
             double  angle =   2*M_PI /  72 * i;
             predict_pose  = init_pose; 
             Eigen::AngleAxisf rotation_vector(angle,  Eigen::Vector3f(0,0,1))  ;   // 绕z轴旋转  
             Eigen::Matrix3f    rotation_ =  rotation_vector.matrix();
            predict_pose.block<3,3>(0,0)  =  rotation_ *  predict_pose.block<3,3>(0,0);    

          // matching: 
            CloudData::CLOUD_PTR result_cloud_ptr(new CloudData::CLOUD());    
            registration_ptr_->ScanMatch(filtered_cloud_ptr, predict_pose,
                                        result_cloud_ptr, temp_init_pose);     
            
            float  score =   registration_ptr_->GetFitnessScore();         //  获取匹配分数
            std::cout  <<  "angle =  "<<  (angle * 57.3 ) << "  score  =  "   <<   score  <<std::endl; 
             if(score < 0.1){break;}   // 得分小于 0.1 跳出循环
       }
      predict_pose.block<3,3>(0,0)  =  temp_init_pose.block<3,3>(0,0);       // 获取配准质量高的姿态
     // matching: 
      CloudData::CLOUD_PTR result_cloud_ptr(new CloudData::CLOUD());    
      registration_ptr_->ScanMatch(filtered_cloud_ptr, predict_pose,
                                  result_cloud_ptr, temp_init_pose);     
      
      init_ori.block<3,3>(0,0)  =  temp_init_pose.block<3,3>(0,0);     //  更新init  pose的航向
}

3.2 获取第一帧gnss 和 point_cloud 数据，调用 init_ori 初始化

FILE: kitti_filtering_flow.cpp

这里参考 KITTIFilteringFlow::InitCalibration() 这部分的写法，接受到第一帧数据后，进行阻塞匹配，匹配完成后，清空数据buff，舍弃匹配过程中所有数据，更新数据进行下一步的定位。

bool  KITTIFilteringFlow::InitOrientation(){
     static bool InitOrientation_received = false;
     if(!InitOrientation_received){
          ReadData();      //  订阅传感器的话题，并将数据存储到buff 中
     }
     if ( (!InitOrientation_received) &&( HasData()) ) {
      //  use  pcl_ndt   registration  to  init  
      current_gnss_data_    =   gnss_data_buff_.front();        //   提取第一帧gnss数据，做位置初始化，提取local_map
      current_cloud_data_  =   cloud_data_buff_.front();      //  提取第一帧点云数据

      filtering_ptr_->InitOri(current_gnss_data_.pose,   current_cloud_data_, init_ori_);
      // std::cout << "init_ori_  "  <<  "\n"<< init_ori_  << std::endl;
      imu_raw_data_buff_.clear();     //    清空缓存数据
      cloud_data_buff_.clear();
      imu_synced_data_buff_.clear();
      pos_vel_data_buff_.clear();
      gnss_data_buff_.clear();
      InitOrientation_received  = true;
     std::cout << "------------------------------InitOrientation   SUCESS------------------------------"  << std::endl;
     }
    return  InitOrientation_received;
}

3.3 更新 current_gnss_data_.pose 为配准后的pose

FILE: kitti_filtering_flow.cpp KITTIFilteringFlow::InitLocalization(void)

  current_gnss_data_.pose.block<3,3>(0,0)  =   init_ori_.block<3,3>(0,0);     //  修正姿态

3.4 调用 InitOrientation

FILE: kitti_filtering_flow.cpp KITTIFilteringFlow::Run()

  if(!InitOrientation()){
    return  false;
  }

4. 实验结果

4.1 使用

roslaunch lidar_localization kitti_localization.launch
rosbag  play  kitti_lidar_only_2011_10_03_drive_0027_synced.bag

4.2 结果

在不同的地方进行播放地图，大致可以完成初始化

原点播放数据集

95s播放数据集合

200s播放数据集合

300s播放数据集合

5.存在问题

在播放100s的数据时发现，初始化结束后，定位跑飞了，原因在于播放100s数据集所在地方刚好在转角处(姿态改变方向的位置)，并且角度在230度附近，需要较长的初始化匹配时间，初始化成功后，当前载体的姿态已经换向和原来方向差异大

后续优化方向：

1.优化遍历角度的方法，可采用二叉树的思路，加快遍历速度，进而提高初始化姿态的效率

2.尝试loam、scancontext等匹配方法，加快匹配效率。

? edited by kaho 11.21