IMU角度跳变问题

前言

在融合定位实验中偶然发现IMU的角度存在跳变现象，本文主要记录问题分析和解决方法。

一、IMU角度跳变现象

imu_driver对原始数据进行解析后，以ROS标准的IMU_msg格式发布出来，然后融合定位模块订阅该msg，对其中的IMU四元数通过Eigen中的函数转成欧拉角，但欧拉角时不时的发生180度跳变，比如-124.5度会突然跳变至55.5度，运行一定角度后，又会再次跳回来，此时查看四元数并无很大的变化。

二、问题分析

订阅IMU_topic后直接调用Eigen中的函数将四元数转成欧拉角。
原程序是先记录第一条msg的角度，再将后续收到的msg的角度值与第一条作差，得到相对的偏航角，用于机器人控制。部分代码如下：

Eigen::Vector3d MsgEuler = MsgQuat.matrix().eulerAngles(0,1,2);
if(mbImuTfFromImu2BaseLink) {Eigen::AngleAxisd TfPitch(mImuTfPitch / 180.0 * M_PI, Eigen::Vector3d(1, 0, 0));Eigen::AngleAxisd TfRoll(mImuTfRoll / 180.0 * M_PI, Eigen::Vector3d(0, 1, 0));Eigen::AngleAxisd TfYaw(mImuTfYaw / 180.0 * M_PI, Eigen::Vector3d(0, 0, 1));ImuQuat = Eigen::Quaterniond(mImuFirstQuat * TfRoll.matrix() * TfPitch.matrix() * TfYaw.matrix()).inverse() * Eigen::Quaterniond(MsgQuat * TfRoll.matrix() * TfPitch.matrix() * TfYaw.matrix());
} else {Eigen::AngleAxisd TfPitch((mImuTfPitch - 180.0) / 180.0 * M_PI, Eigen::Vector3d(1, 0, 0));Eigen::AngleAxisd TfRoll((mImuTfRoll - 180.0) / 180.0 * M_PI, Eigen::Vector3d(0, 1, 0));Eigen::AngleAxisd TfYaw((mImuTfYaw - 180.0) / 180.0 * M_PI, Eigen::Vector3d(0, 0, 1));ImuQuat = Eigen::Quaterniond(mImuFirstQuat * TfRoll.matrix() * TfPitch.matrix() * TfYaw.matrix()).inverse() * Eigen::Quaterniond(MsgQuat * TfRoll.matrix() * TfPitch.matrix() * TfYaw.matrix());

然而采用这种计算方法将四元数换算成欧拉角存在角度跳变问题，比如当前角度为-124.5度，突然跳变至55.5度，但看前四列的四元数并未有巨大变化。

查阅相关资料后发现，同一个四元数，可以用2个欧拉角来表示，而这个方法得到的结果有可能是用转角大于2PI的方式表达的。比如我在RPY角度里输入[177.13，144.47，-124.55]，画出此时向量，结果点击apply，此时角度自动变成[-2.87，35.53，55.45]，再看角度和四元数与上图跳变一致，印证了这个说法。

三、改进

为了避免这个问题，可以用以下方法

static void toEulerAngle(const Eigen::Quaterniond& q, double& roll, double& pitch, double& yaw)
{// roll (x-axis rotation)double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());roll = atan2(sinr_cosp, cosr_cosp);// pitch (y-axis rotation)double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());if (fabs(sinp) >= 1)pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of rangeelsepitch = asin(sinp);// yaw (z-axis rotation)double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());yaw = atan2(siny_cosp, cosy_cosp);

这里求RY是用atan2()函数，该函数值域为[-pi,pi]可以覆盖roll、yaw整个角度变化范围，无问题；而asin()函数值域为[-pi/2,pi/2]无法完全覆盖pitch角范围，定义域为[-1,1]，若直接使用，便会出问题。因此在求出sinp时，首先会对其进行判断，看是否超出范围，若超出，则用90度代替，并取此时sinp的符号。
顺便讲讲copysign(x,y)函数，就是返回带有符号y的数字( x )

float x =  2.5;
float y = -2;
copysign(x, y);    
OUTPUT：-2.5

也就是说如果上述sinp符号为正，且超出范围，则最后程序输出+pi/2。

总结

最终通过上述方法，转换后的yaw角再无跳变现象，因此工程问题得以解决。如果单从问题本身来讲，对四元数的理解仍然不够深刻，后续需要进一步研究。