参考文章:
LIO_SAM_6AXIS 适配香港城市数据集UrbanNav - 知乎
参考代码:
GitHub - JokerJohn/LIO_SAM_6AXIS: LIO_SAM for 6-axis IMU and GNSS.
GitHub - nkymzsy/LIO-SAM-MID360
前言:
LIO-SAM中源代码使用的9轴IMU,在这里我们想要LIO-SAM代码适配6轴IMU,首先我们要了解9轴IMU和6轴IMU区别是什么?6轴IMU能够测量的量有:xyz方向上的线角速度和xyz方向上的角速度,9轴IMU能够测量的量有:xyz方向上的线速度、xyz方向上的角速度和当前时刻IMU的RPY值(欧拉角)。
因此在9轴IMU中,欧拉角可以通过两种方式获取:直接从传感器测量的原始数据计算得到的欧拉角(即RPY值),或者通过对传感器的测量数据进行积分得到的欧拉角(即预积分欧拉角)。
而6轴IMU中,欧拉角只能通过预积分的方式获取。
坐标系:
在九轴IMU中多了一个磁力计,磁力计的坐标系和加速度计和陀螺仪的坐标系一般为IMU的本体坐标系。
LIO-SAM-6轴IMU思路:
了解了6轴IMU和9轴IMU的区别后,我们就可以去查看LIO-SAM中哪里用到了9轴IMU中的RPY数据。
源头:ImageProjection::imuHandler() 和IMUPreintegration::imuHandler() 中调用imuConverter()从这里接受的imu原始数据,并将imu的RPY保存下来给后面的函数使用。
去向:mapOptimization中使用。
通过读取9轴IMU中的RPY的数据流发现,imu的原始RPY在两个环节进行了使用:
- 1)作为 第一帧 雷达的初始姿态角
- 2)用于scan-to-map预估的位姿和IMU的RPY数据进行姿态角融合
LIO-SAM-6轴方法:
基于上述分析,为了尽可能减少对源代码的改动,针对imu的原始RPY在两个地方的使用方法,在原理上我们只需要做出以下两点改动:
- 1)每一帧的IMU的RPY值我们用单位矩阵代替。
- 2)在进行姿态角融合时,将IMU的RPY权重设置为0。
基于以上原理我们不需要改动LIO-SAM的结构,只需要做出以下主要修改即可:
在params.yaml中:
#1.设置IMU的RPY值为单位矩阵
extrinsicRPY: [1, 0, 0,
0, 1, 0,
0, 0, 1]
#2.设置IMU的RPY权重为0
imuRPYWeight: 0.00
在utility.h文件imuConverter函数中:
#设置imu的RPY值为单位向量,extQRPY是单位矩阵转化的四元数,前面还有一些操作
if (imuType == “”6轴“”)
{
q_final = extQRPY;
q_final.normalize();
# 6轴imu本来没有orientation这个数据,这里人为设置。
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
}
在mapOptmization.cpp文件publishOdometry()函数中:
if (cloudInfo.imuAvailable == true)
{
// 如果俯仰角度小于这个值,对roll角和pitch角进行加权平均一下
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = 0.1; #6轴IMu这里权重应该设置成0
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid
// roll姿态角加权平均
transformQuaternion.setRPY(roll, 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
roll = rollMid
// pitch姿态角加权平均
transformQuaternion.setRPY(0, pitch, 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
pitch = pitchMid;
}
}