目录1.连续时间下三维运动的微分性质1.1 旋转矩阵的微分方程1.2 四元数的微分方程1.3 旋转向量的微分方程2.惯性导航解算2.1 姿态更新2.2 速度更新2.3 位置更新3.惯性导航误差分析3.1 姿态误差微分方程3.2 速度误差微分方程3.3 位置误差方程3.4 bias误差3.5 惯性导航误差分析总结注本篇笔记的主体内容来源于对深蓝学院《多传感器融合定位》课程的学习推荐有一定基础的SLAM 初学者学习该课程。1.连续时间下三维运动的微分性质1.1 旋转矩阵的微分方程假设世界坐标系(w系)中有一个固定不动的矢量它在载体坐标系(b系)下表示为则有等式两边同时对时间进行微分可得由于在w系是固定不动的但载体是在运动的所以可以看作是相对载体b系以相反的角速度在旋转的则其中代表载体旋转角速度在b系下的表示即陀螺仪的角速度测量。将二者带入原式则1.2 四元数的微分方程四元数乘法其中可以推出使用四元数表示两个矢量和的转换其中表示四元数乘法为的逆共轭。等式两边同时右乘易证与抵消则等式两边同时对时间求微分得其中可以转换为此时认为和两个纯虚四元数即实部为0。则再对等式两边同时左乘则再将上式改写为忽略实部则可得此时得到了上式的虚部还需要考虑其实部的形式。对其实部的推导如下假设对应的旋转向量为其模长为单位方向向量为则可得因为所以则的实部为0则从而得到四元数在连续时间下的微分方程1.3 旋转向量的微分方程此处直接给出结论为了化简取出其中的高阶项2.惯性导航解算目的利用IMU测量的角速度、加速度根据上一时刻导航信息推算出当前时刻导航信息包括姿态解算、速度解算、位置解算。方法由姿态、速度、位置的连续时间下的微分方程推导出其离散时间下的帧间位姿递推的近似形式从而可以在离散时间采样下完成导航信息求解。2.1 姿态更新1帧间相对旋转向量在旋转向量的微分方程中对于右侧第二项根据叉乘的定义可知当叉乘的两个矢量之间方向重合时则结果为0。假设IMU频率很高则k-1时刻到k时刻的时间极短可认为两个矢量接近重合。因此旋转矢量微分方程可进一步化简为其中当前时刻的陀螺仪读数是陀螺仪的零偏。再进行离散化。欧拉法中值法2旋转矩阵更新根据前面推导旋转矩阵的微分方程为则k-1时刻到k时刻的相对旋转为对指数部分进行离散化可以得到指数映射部分可以直接使用Sophus库的exp()方法进行也可以使用罗德里格斯公式近似则k-1到k时刻的姿态更新为3四元数更新根据前面推导四元数的微分方程为代表四元数乘法。微分方程的矩阵形式为则其中对指数函数进行泰勒展开并忽略高阶项可得则因为所以所以2.2 速度更新易知速度的微分方程为其中为当前位姿由姿态更新部分解算得到为重力加速度为当前加速度计的测量值是加速度计的零偏。因为加速度计测量的是“比力”所以需要减去重力加速度。则该微分方程的通解形式对应的基于中值法的速度更新形式为2.3 位置更新位置微分方程为其通解形式为此处v指的是该时间段内的平均速度该形式对应的基于中值法的离散形式为进一步3.惯性导航误差分析由于imu数据带有误差并且在离散化惯性解算过程中也会引入误差所以需要估计误差。在惯性导航中我们无法直接算出真实值Truth只能维护一个推算值名义值Nominal State。假设名义值理想值误差即此时再根据状态量的微分方程推导其误差的微分方程。误差微分方程描述了推算值名义值与真实值理想值之间的偏差即误差随时间变化的速率和规律。状态量误差微分方程是误差状态卡尔曼滤波ESKF的预测模型基础滤波器只有依赖它才能计算出系统随时间累积的不确定性协方差从而决定后续观测时对IMU数据的信任程度。它通过对微小误差量进行一阶近似将载体原本极其复杂的非线性运动完美转化为了线性系统满足了卡尔曼滤波严格的数学前提。它不仅在物理上明确了各状态误差如姿态误差如何污染速度的交叉传播机制还在数学上巧妙解决了三维姿态描述如四元数带来的自由度冗余与奇异性问题。3.1 姿态误差微分方程1不考虑误差的微分方程2考虑误差的微分方程3带误差的名义值与理想值之间的关系其中为陀螺仪白噪音为陀螺仪在t时刻的零偏。并且其中为姿态误差对应的旋转向量为小量。4将3中的关系代入2其中5将1中的关系代入4等式两边同乘再将四元数乘法转换为矩阵乘法令则由于则化简得再将代入得再化简忽略其中的二阶小量得注这里推导旋转向量的误差微分方程是因为后续直接使用旋转向量作为滤波或图优化中的待优化变量而非四元数q。3.2 速度误差微分方程1) 不考虑误差时的微分方程暂时忽略重力向量g2) 考虑误差时的微分方程3) 带误差的名义值与理想值之间的关系其中为加速度计的测量白噪音为加速度计的零偏。4将3代入25将1带入4化简方程并忽略二阶小量得3.3 位置误差方程1) 不考虑误差时的微分方程2) 考虑误差时的微分方程3带误差的名义值与理想值之间的关系4将3代入25把1带入4化简得3.4 bias误差在IMU精度较高时bias认为是常值即有但机器人领域所用的mems多数达不到这种精度因为角速度随机游走和加速度随机游走较大因此误差方程常写为和分别为加速度计和陀螺仪零偏的变化率。3.5 惯性导航误差分析总结