IMU的数学模型与误差标定问题
IMU数学模型
加速度计
首先，对于世界坐标系，一般我们会使用最常见的东北天(ENU)坐标系G（无关远点位置，只与姿态有关）。
在这个坐标系中，重力加速度为
此时，假设IMU坐标系就是ENU坐标系，则，静止时有（其中
是测量值）：
所以，不静止时：（此处对a和g符号不做区分标记，因为假设body系与Global系一样）
由上可知，其实在物体做*落体时imu测量的加速度才是0，静止时反而是
，这个是由加速度计的测量原理决定的。
上面讲的是在IMU坐标系也是ENU坐标系时的情况（此时位置无关，只关乎姿态）。大多数实际应用中，IMU坐标系（Body）一般是与ENU坐标系有一个姿态的变化的。此时，得到的理论测量值为：
此处
是将Global坐标转换到Body坐标姿态的旋转矩阵。此处可以看出，global坐标系的位置与body坐标系的位置与在两个系下测量的加速度大小无关。但是，与姿态有关。
陀螺仪
相比较于加速度计，陀螺仪相对简单。如果不考虑误差，则
我们会发现此处并没有像加速度计一样，将global坐标系下的角速度转换到陀螺仪的测量值，而是直接使用body系下的角速度。这是因为旋转叠加时（比如四元数和旋转矩阵表示姿态时），全局姿态是直接乘以body系下的更新量的就可以得到新的全局姿态的。
恢复运动轨迹
imu最后输出的是一个离散的加速度、角速度序列。我们想做的是利用这些恢复出运动的轨迹（也就是一个位姿的序列）。
下面会介绍两种离散积分的方法。欧拉法与中值法。
这两种方法，都是已知了时刻的位姿，
时刻与
时刻的测量值（加速度与角速度）。目的是求得
时刻的位姿。
欧拉法
欧拉法，是直接使用时刻的测量值
来积分。
其中
中值法
中值法，使用两个相邻时刻到
的位姿是用两个时刻的测量值
的平均值来离散积分。
其中
代码
for (int i = 0; i < imudata.size()-1; ++i) {
MotionData imupose = imudata[i];
MotionData imupose1 = imudata[i+1];
/*
// 欧拉积分
//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = acc_w * dt + Vw;
Qwb = Qwb * dq;
Qwb.normalize();
*/
/// 中值积分
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = (imupose.imu_gyro + imupose1.imu_gyro)*dt/4.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
Eigen::Quaterniond Qwb1 = Qwb * dq;
Qwb1.normalize();
Eigen::Vector3d acc_w = (Qwb * imupose.imu_acc + Qwb1 * imupose1.imu_acc)*0.5 + gw;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Vw = acc_w * dt + Vw;
Qwb = Qwb1;
//存储位姿
save_points<<imupose.timestamp<<" "
<<Qwb.w()<<" "
<<Qwb.x()<<" "
<<Qwb.y()<<" "
<<Qwb.z()<<" "
<<Pwb(0)<<" "
<<Pwb(1)<<" "
<<Pwb(2)<<" "
<<std::endl;
}
此处推荐一个生成imu数据、加噪声与测试的工具，这里中值积分与欧拉积分可以参考上面的我的代码，因为工具里可能不一定提供了。
旋转积分
上面恢复姿态轨迹的积分用了四元数的形式，下面我想多介绍几个
此处是imu的测量结果，也就是局部角速度。因为这个旋转积分的结果是姿态，因此下面会分别介绍使用四元数、SO3还有欧拉角下的积分方式。
- 四元数的形式：
-
形式：
- 欧拉角形式：
我们可以发现，这三种方法，对于全局姿态的变化，旋转矩阵与四元数都是直接使用imu的测量（也就是局部角速度）进行更新的。只有在欧拉角的形式里，
表示将IMU body坐标系下的角速度转化成欧拉角速度。此处可以推导出
欧拉角速度到imu输出角速度
这个挺有必要的，因为用欧拉角来表示姿态还是很方便的。
-
step1:绕着惯性坐标系的z轴旋转，得到新的坐标系
-
step2:绕着新坐标系
的y轴旋转得到坐标系
-
step3:绕着新坐标系
的x轴旋转得到坐标系
，
就是我们的body坐标系
欧拉角速度到body角速度：
上面取逆就得到，如下body到欧拉角的变换：
误差与标定
加速度计和陀螺仪的误差可以分为确定性误差与随机误差。
确定性误差
确定性误差可以事先标定确定，包括：bias，scale …
bias
理论上，当没有外部作用时，IMU传感器的输出应该为0。但是，实际上数据存在一个偏置b。
scale
scale可以看成是实际数值和传感器输出值之间的比值。
Nonorthogonality/Misalignment Errors
在多轴IMU传感器制作的时候，由于制作工艺的问题的问题，会使得xyzxyz轴可能不垂直，这个也叫轴间误差。
轴间误差使得本来x轴的分量会对测量到的y轴与z轴的分量有影响。将其与scale误差相结合，会得到如下的测量与实际的对应关系。
其他确定性误差
bias与scale的误差是会受温度影响的，并且在运行中也许也会改变。
确定新误差的标定（六面法）
以加速度计为例，陀螺仪同理
指将加速度计的3个轴分别朝上或者朝下水平放置一段时间（对于陀螺仪就是在三个旋转轴上正反旋转，不过需要高精度转台），采集六个面的数据完成标定。
3个轴都是正交时
其中，为加速度计某个轴的测量值，
为当地的重力加速度。
当具有轴间误差时
此时实际加速度和测量值之间的关系为：
水平静止放置6面的时候，加速度的理论值为：
对应的测量值矩阵L:
利用最小二乘就能够把12个变量求出来。
随机误差
随机误差主要有两部分，一个是高斯白噪声，一个是bias随机游走。
高斯白噪声
IMU数据连续时间上受到一个均值为0，方差为，各时刻之间相互独立的高斯过程
:
其中表示狄拉克函数。
不过需要说明的是，实际上，IMU传感器获取的数据为离散采样，离散和连续高斯白噪声的方差之间存在如下转换关系：
即离散的序列的方差比连续的方差大
倍（传感器的采样时间）。
bias随机游走
通常使用维纳过程来建模bias随时间连续变化的过程，离散时间下称之为随机游走。
bias的变化的导数是其中是方差为1的白噪声。
同样，离散和连续之间的转换为：
bias随机游走的噪声方差离散的序列比连续的大
随机误差的标定（艾伦方差标定）
Allan方差法是20世纪60年代由美国国家标准局的David Allan提出的，它是一种基于时域的分析方法。具体流程如下：
-
保持传感器绝对静止获取数据
-
对数据进行分段，设置时间段的时长，如下图所示。
-
将传感器数据按照时间段进行平均。
-
计算方差，绘制艾伦曲线。
- 此处的艾伦方差的计算公式如下（将每个时间段长度作为一个变量，将每个时间段的数据求均值，计算方差）：
- 忽略其他噪声的影响，Allan方差可以近似为各种噪声的和，化简为：
其中，Q:量化噪声误差系数；N：角速度随机游走误差系数；B：零偏不稳定性误差系数；K：速率随机游走误差系数；R：速率斜坡误差系数
(ps:其中表格中B那一项是乘法不是除法，写错了)
这里，绘制出来的艾伦曲线如下图所示：
其中t=1，斜率为-0.5处纵坐标的值为高斯白噪声方差，斜率为0.5，t=3处的纵坐标的值为随机游走方差。
下面给出两个github上的比较好用的标定工具。
-
imu_utils，额注意，这个工具的结果关于bias那一项输出的是bias稳定性的方差，不是随机游走的方差，因此，我们需要此工具生成的艾伦方差曲线自行完成bias随机游走方差的获得。。
加上误差模型后的理论测量值
- 加速度计
- 陀螺仪
- 低端传感器，可能会出现加速度影响陀螺仪的值的情况，也就是下面的第二项：
上一篇： ps画线条用什么工具？