一、背景

上一次，我们使用Autoware成功进行了ndt定位和导航，但是GNSS初始定位以及修正功能没有正常生效。行走途中遇到剧烈颠簸、非常狭小的空间、遮挡严重、或其他ndt局部定位特征不足时，ndt_matching会找不到自己位置导致定位飞。

由于rtk定位精度<10cm，在信号好且途中定位质量差或初始化定位失败时，很有必要使用GNSS进行修正下定位。本文就针对这块单独讲下怎么处理。

二、定位原理

由于小车底盘老跑偏，odom论速计不准，本文主要使用IMU+RTK。

 

三、启用GNSS定位

1. 修改tix2tfpose配置

fix2tfpose包通过订阅fix，发布gps定位topic为/gnss_pose。我们启用GNSS时，除了use_gnss设置为1以外，还需要修改gps fix的topic名称：

# 修改fix2tfpose的gps topic为外部参数传入
vim Autoware/src/autoware/core_perception/gnss_localizer/nodes/fix2tfpose/fix2tfpose.cpp
    private_nh.getParam("gps_topic", _gps_topic);
    pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1000); 
    stat_publisher = nh.advertise<std_msgs::Bool>("/gnss_stat", 1000);
    ros::Subscriber gnss_pose_subscriber = nh.subscribe(_gps_topic, 100, GNSSCallback);

vim Autoware/src/autoware/core_perception/gnss_localizer/launch/fix2tfpose.launch
    <arg name="plane" default="7"/>
    <arg name="gps_topic" default="gps/fix"/>
    <node pkg="gnss_localizer" type="fix2tfpose" name="fix2tfpose" output="log">
        <param name="plane" value="$(arg plane)"/>
        <param name="gps_topic" value="$(arg gps_topic)"/>
    </node>

# 编译
cd ~/Autoware
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=gnss_localizer

# 运行测试确认节点订阅关系正常
roslaunch gnss_localizer fix2tfpose.launch plane:=7 gps_topic:=/gps/fix
rosrun rqt_graph rqt_graph

# 检查/gnss_pose是否正常输出
rostopic echo /gnss_pose

2. 验证GNSS定位是否生效

启动播包和定位可以看到fix2pose已经生效，但是输出的/gnss_pose与map frame有一个距离：

# 启动播包、gnss定位
roslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=/home/autoware/shared_dir/bag/map-8-jhy-gps/map-8-jhy-gps-241203-2301.bag

# 查看/gnss_pose与/ndt_pose发布的数据是否基本一致，如果基本一致，那么成功（我们这里还对不上，别着急)
rostopic echo /gnss_pose
rostopic echo /ndt_pose

 

四、GNSS定位与NDT定位位姿的旋转矩阵

刚才我们已经尝试让Autoware的GNSS定位生效了，但是它的位置与map位置无法匹配，因此我们还需要计算GNSS定位与NDT定位位姿的旋转矩阵，让GNSS和NDT Matching的定位能匹配上。

1. 修改 geo_pos_conv.cpp 初始化度分位置

geo_pos_conv.cpp 主要实现地理位置转换器的功能，将纬度、经度、高度坐标(LLH) 转化为笛卡尔坐标 (XYZ) 。使用“set_plane”函数根据 runtime 设置的参数输入对应纬度和经度设置平面，获得的 XYZ 坐标。
修改 void geo_pos_conv::set_plane(int num)函数，̀set_plane` 函数重载采用纬度 (`lat`) 和经度 (`lon`) 值，并相应地设置成员变量 `m_PLato` 和 `m_PLo`。根据整数 num 的值输出不同平面对应的预定义值设置纬度和经度。按照以下方式修改 num=7 时的代码块：

vim Autoware/src/autoware/common/gnss/src/geo_pos_conv.cpp

void geo_pos_conv::set_plane(int num) {
  ...
  else if (num == 7) {
    /*lon_deg = 36;
    lon_min = 0;
    lat_deg = 137;
    lat_min = 10;*/
    // 这里修改为你自己所在地的经纬度分
    lon_deg = 40;
    lon_min = 2;
    lat_deg = 116;
    lat_min = 15; 
  ...

 }

# 修改完需要先编译以下，等下计算旋转矩阵要用到 
cd ~/Autoware
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=gnss

例如我所在地区的wgs84经纬度为longitude,latitude: 116.260100°,40.048550°，将经纬度转换度分秒的方式：

• 将度(DDD)换算成度分秒(DMS)。
  • 东经 116.260100 度：转换方法是将 116.260100 整数位不变取 116(度)；0.260100*60=15.6060265，取整数位 15(分)；0.6060265*60=36.36159再取整数位 36(秒)。故对应度分秒为：东经 116 度 15 分 36 秒。
  • 北纬40.048550 度：整数位不变取 40(度)；0.048550*60=2.913，取整数位 2(分)；0.913*60=54.78，取整为54(秒)。故对应度分秒为：40 度 2 分 54 秒。
• 也可以使用在线转换工具：https://www.lddgo.net/convert/degree-convert
  • 输入116.260100°,40.048550°，点击“度->度分秒”转换按钮，得到116°15’36”,40°2’54”
• 这里的度分并不要求必须是精确的起点坐标，只要是所在城市的坐标，基本就问题不大。

2. 计算GNSS与地图的旋转矩阵

只修改 geo_pos_conv.cpp 文件地图和 pcd 点云还是无法对齐，需要加入旋转矩阵，将源坐标系（map）中的点变换到目标坐标系中。

每个地理位置的旋转矩阵参数都是不同的。要获取旋转矩阵的步骤，需要先打开地图定位功能，可以使用数据包定位，也可以使用小车完成实时定位。下面以 rosbag 为例，简述操作步骤。

2.1 确保ndt matching处于定位成功状态

# 启动播包、定位模块（注意此时需要先关闭GNSS定位功能）
roslaunch autolaunch run.launch map_name:=jingheyuan rviz:=true use_gnss:=0 bag_file:=/home/autoware/shared_dir/bag/map-8-jhy-gps/map-8-jhy-gps-241203-2301.bag

# 检查use_gnss参数状态
rosparam get /ndt_matching/use_gnss
  0

# 确保rtk是生效的（status >= 2）
rostopic echo /gps/fix

如果ndt不能自行定位，先手动设置下初始位姿，确保地图处于匹配状态后，按空格暂定播包：

2.2 计算旋转矩阵

启动计算旋转矩阵节点（此时数据包还处于暂停播放状态）：

rosrun turn_on_wheeltec_robot gnss_matrix.py

这个节点的功能是等待接收到足够数量的样本（/ndt_pose 和/gnss_pose 的话题数据，所使用的数据包需要录制大概三分钟），执行矩阵计算（刚性变换）并打印结果。

2.3. 继续播放bag，输出旋转矩阵

开始播放数据，等待3分钟左右，gnss_matrix.py会输出旋转矩阵：

我们需要的是最下边的r martrix、t matrix，它是：

r matrix:
[[ 0.99708656 -0.07450855 -0.016336  ]
 [ 0.06207712  0.91707773 -0.39384626]
 [ 0.0443263   0.39168472  0.91903115]]
t matrix:
[ -729.95410472 -1585.00749103  -732.72186379]

*注：如果出现 nan 无效值，只需要再执行一次以上所有步骤重新计算即可。

2.4 设置旋转矩阵

根据 gnss_matrix 节点算出的旋转矩阵，填入 geo_pos_conv.cpp 文件中，修改末尾处的 R 和 T 参数。

vim Autoware/src/autoware/common/gnss/src/geo_pos_conv.cpp

  #if 1
    // debug gnss localtor
    Eigen::Matrix3f R;
    Eigen::RowVector3f T;
    Eigen::RowVector3f m_3;
    Eigen::RowVector3f m_4;
    
    // 原始笛卡尔坐标（ m_y, m_x, m_z）分配给m_3向量，赋值顺序为y,x,z
    m_3 << m_y, m_x, m_z;

    /*
    R <<  1, 0, 0,    
          0, 1, 0,
          0, 0,1;
    
    T << 0, 0, 0;     
    */
    // 旋转矩阵R是一个3x3矩阵，每行代表旋转后变换轴的方向
    R << 0.99708656, -0.07450855, -0.016336,
        0.06207712,  0.91707773, -0.39384626,
        0.0443263,   0.39168472,  0.91903115;
    // 平移动向量T表示旋转后应用于坐标的位移
    T << -729.95410472, -1585.00749103,  -732.72186379;

    // 变换后的坐标m_4是通过将原始坐标m_3乘以喜欢转矩阵R.transpose()的转置，然后加上平移向量得到的T
    m_4 = m_3 * R.transpose() + T;

    // 将转换后的坐标m_4分配回m_x，m_y,m_z变量，赋值顺序为y,x,z
    m_y = m_4[0];
    m_x = m_4[1];
    m_z = m_4[2];
  #endif

2.5 编译

修改完成后，单独编译该功能包：

cd ~/Autoware
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=gnss

3. NDT+GNSS联合定位验证

3.1 GNSS定位验证

# 启动播包、gnss定位
roslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=/home/autoware/shared_dir/bag/map-8-jhy-gps/map-8-jhy-gps-241203-2301.bag

可以看到虽然不像之前偏离那么远了，但是依然是有偏差的，可能是初始化的时候gnss定位不准，我这里等播包一定gnss稳定后再进行矩阵计算：

注：重跑时，一定要先恢复R/原始值，编译后再跑gnss_matrix（否则跑出来会是相对之前旋转的基础上再旋转的值）

R <<  1, 0, 0,
        0, 1, 0,
        0, 0,1;
  
  T << 0, 0, 0;

得到一组新的矩阵：

r matrix:
[[ 0.99961893  0.01157622 -0.02505958]
 [-0.00929102  0.99594709  0.08945991]
 [ 0.02599362 -0.08919299  0.99567512]]

t matrix:
[ -877.05005961 -1675.08312445    91.9169504 ]

设置并编译gnss后确认效果：

  // 旋转矩阵R是一个3x3矩阵，每行代表旋转后变换轴的方向
  R << 0.99961893, 0.01157622, -0.02505958,
       -0.00929102, 0.99594709, 0.08945991,
       0.02599362, -0.08919299, 0.99567512;
  // 平移动向量T表示旋转后应用于坐标的位移
  T << -877.05005961, -1675.08312445, 91.9169504;

*注：gps的定位，不是很稳定，有时可以跟ndt完全匹配，有时会偏移到两侧，距离再5-20cm左右波动，像是RTK定位精度问题。

3.2 手动修正矩阵

这里如果相对关系很稳定的话，也可以手动微调平移动向量T来解决：

• 播包后暂停，查看gps_pose位置与实际地图位置的相对偏移关系，修改偏移参数
•  R值决定gnss_pose相对地图的旋转角度：
  • 这个我发现好像在平地上的话用默认的就可以，所以我先把这里恢复成默认值编译，再单独调T矩阵
  • 这里不用旋转角度的原因是我的GNSS+IMU比较便宜，它自己不太容易找到方向，但是可以确定位置，所以就不配置旋转角了
•  T值的三个值决定gnss_pose在地图中的偏移位置：
  • 第一个值：南北距离，值越大越靠北（相对2D地图也就是越靠上），单位米
  • 第二个值：东西距离，值越大越靠东（相对2D地图也就是越靠右），单位米
  • 第三个值：相对地面高度，值越大高度越高（打开rviz 3D视角确认gap），单位米

最终，我还是自己手动标了一个矩阵，编译后，初始化定位需要先让车走上几米，他即可自己初始化定位成功；行驶途中估计把当前位置改称成错的，基本5s之内可以根据GNSS坐标自动修正回来：

  R <<  1, 0, 0,
        0, 1, 0,
        0, 0,1;
  T << -858.55005961, -1687.08312445, -36.5;

*注：需要5s的原因是我的GNSS+IMU航向角数据特别差，找不着北，只能根据大概10-20厘米误差的方位辛苦ndt去matching了。

这个实际上车发现，这个方法有改善但是不多，走完全程的定位稳定性相较不使用gnss更差了。其他地图有坡度或高度差的地方，这样的方法依然比较难match上。经过研究，在这一步的基础上再做一些修改即可达到比较好的效果。

五、改善GNSS定位修正方法

上边的方法，在pc上仿真效果还可以，但是实际使用时，当出现ndt定位不成功，会频繁的ndt和gnss的定位来回切无法成功修正，rviz画面闪瞎我的狗眼，想手动点下都不行，突发奇想，为啥子不让它自动执行一个这个操作呢？来动手试一试：

vim Autoware/src/autoware/core_perception/lidar_localizer/nodes/ndt_matching/ndt_matching.cpp

    static double _gnss_reinit_fitness = 1.0; 
    static void gnss_callback(const geometry_msgs::PoseStamped::ConstPtr& input)
    {
      ...
      // 启用GNSS但尚未初始化位姿，或ndt定位质量太差（/ndt_stat.score>500，分数越大matching质量越差）时
      if ((_use_gnss == 1 && init_pos_set == 0) || fitness_score >= _gnss_reinit_fitness)
      {
        // 直接使用gnss当前位姿态重置初始位姿，以修正定位飘问题（发送topic而不是直接调用initialpose_callback的原因是要同步通知给op_global_planner）
        geometry_msgs::PoseWithCovarianceStamped initialpose_msg;
        initialpose_msg.header = input->header;
        initialpose_msg.pose.pose = input->pose;
        initialpose_msg.pose.covariance[0] = 0.25;
        initialpose_msg.pose.covariance[6 * 1 + 1] = 0.25;
        initialpose_msg.pose.covariance[6 * 5 + 5] = 0.06853892326654787;
        initialpose_pub.publish(initialpose_msg);
        init_pos_set = 1;
        // !!!注释掉这个if里的其他代码，增加上边这部分，如果区分初始化/行驶过程总的场景区别处理下效果会更好
      }
      ...
     int main(){
         private_nh.param<double>("gnss_reinit_fitness", _gnss_reinit_fitness, 1.0);
     }
    }

vim Autoware/src/autolaunch/launch/localization.launch
    <include file="$(find lidar_localizer)/launch/ndt_matching.launch">
        ...
        <arg name="gnss_reinit_fitness" default="1.0" /> <!-- ndt_matching分数大于1.0时，基本就已经飘了，这时应该用gnss_pose重置一下 -->
    </include>
# 编译
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lidar_localizer

# 启动播包、ndt+gnss定位
roslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=/home/autoware/shared_dir/bag/map-8-jhy-gps/map-8-jhy-gps-241203-2301.bag

# 查看ndt+gnss pose、定位质量
rostopic echo /ndt_pose
rostopic echo /gnss_pose
rostopic echo /ndt_stat    # 看score，>1.0基本都是平飘了

重新试下，从之前ndt pose与gnss pose反复拉扯的状态，到偏移后大概率可以重定位成功（gnss pose距离真实位置偏差2米范围内，且周围有足够的特征时），速度也比之间快了很多。

最终的节点关系图：

六、总结

今天还是很有收获的，GNSS定位的介入对NDT match特征不足时定位飘问题有改善，GNSS修正定位的主要影响因素如下：

• GNSS信号质量需要有保障，不能偏差太大，至少需要确保gnss pose与实际位置偏差再1-2米内，20cm内效果最佳；
• GNSS修正后，NDT match需要周围有足够的特征，如果特征不够，NDT match会再次飘，并被GNSS再次修正，直到match score<1.0；
• 初始GNSS定位时，车辆最好先向前运动1-2米，这样GNSS将或获得稳定的航向，对自动GNSS初始定位成功很有帮助；这个问题也可以通过带稳定航向的9轴ARHS来解决（实际上，很多号称9轴的IMU，实际只能用到VRU，磁力航向基本不可用，打开ARHS不但航向不准，反而会导致连6轴指标也不满足了，谁有可靠性强的便宜9轴IMU请给我留言，谢谢）。

今天可以快乐的睡觉了。

 

参考：

Autoware 笔记 No. 5——基于GNSS的定位

导航中通过“initialpose“设置机器人位姿

欢迎关注下方“非著名资深码农“公众号进行交流～