Autoware中使用IMU为Ndt配准提供初值

最近在使用Autoware中发现，在进行Ndt_matching的过程中，可以选用IMU为每次ndt算法的迭代过程提供初始值，然后根据初始值来进行点云配准，整理了一下笔记，具体代码流程如下：

首先可以选用GNSS或输入坐标用作初始对准

param_callback

利用参数初始化当前节点，包括初始位置（如果没有收到initial pose的消息就是000，000）

map_callback

new_ndt.setInputTarget(map_ptr);

局部变量new_ndt设置好目标点云（全局地图）、迭代次数等参数

ndt = new_ndt;

将全局ndt对象赋值初始化。

imu_callback

这个是imu数据的回调函数，但是里面的处理方式非常奇怪，使用了IMU姿态的四元数解算出RPY角，再和上一次的RPY角作差，从而计算出这两次间隔之间的角速度，但是角速度不是可以直接从IMU中读取出来么。。

静态局部变量previous_time = current_time初始化为第一次接收到消息的时间

diff_time = (current_time - previous_time)

tf::quaternionMsgToTF(input->orientation, imu_orientation);

从输入中得到四元数，转换成TF形式的四元数，再getRPY得到RPY角Imu_raw,yaw,pitch

wrapToPmPi(imu_roll) 把欧拉角转换到[-pi,pi]范围

局部变量static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;初始化值为第一次得到的imu_roll\pitch\yaw

CalcDiffForRadianl两次的角度 做差值并转换到[-pi,pi]

全局变量msg::imu，用订阅到的imu消息中线加速度赋值此变量的线加速度，用角度差值/时间差得到角速度，用订阅的消息中的消息头赋值此变量

这个IMU处理函数默认之选用了X轴的线加速度，但实际运行过程中最好把Y轴的线加速度注释解开。

角速度如果可以从IMU中直接读取，建议直接改写成使用读取的角速度，而不必再通过两次姿态四元数换算角速度。

imu_calc

• • 开头依然是和imu_callback做了一样的时间差
  • 利用全局变量msg::imu中保存的角速度乘以时间差的到diff_imu_RPY
  • 利用diff_imuRPY对全局变量current_pose的角度进行更新
  • 计算三个方向线加速度，积分，更新全局current_velocity_imuxyz、offset_imu_xyzRPY
  • 对全局current pose的位置坐标进行更新
  • 结尾对static局部变量previous time更新

结尾对static局部变量previous_*进行更新

Points_callback（ndt配准模块）

其中会选用odom/imu的值作为predict pose，ndt初始值，

1.使用0，0，0或GNSSpose进行初始化，得到initpose

2.根据上一时刻的速度和位置，再加上当前时刻与上时刻的时间差进行积分计算，得到predict pose ，待定

3.判断是否使用imu和odom（应该是轮速里程计），然后决定用哪个位姿作为当前时刻的预测位姿（predict pose for ndt）。

4.利用当前的预测位姿（6dof）转换成旋转矩阵，作为ndt的初始变换矩阵

5.对当前的点云进行配准与打分输出，得到ndt变换矩阵t（点云到lidar），再利用tf变换转移到机器人坐标系t2（lidar到baselink），最终利用t2得到变换后位姿ndt_pose

6.计算ndtpose和predictposeforndt的误差，通过阈值判断，误差小于阈值说明配准成功，没有发生大的跳跃，可以用ndtpose，若误差过大说明发生跳跃，则用预测值（速度积分）的位姿

7.利用当前位姿和上一时刻位姿，更新当前速度值和加速度值，发布当前位置和底盘速度等数据

8.输出日志