深蓝学院-多传感器融合定位课程-第6章-惯性导航解算及误差模型

专栏文章:

深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第3章-3D激光里程计II_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第4章-点云地图构建及基于地图的定位_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第5章-惯性导航原理及误差分析_goldqiu的博客-CSDN博客

github保存了相关代码和学习笔记：

https://github.com/goldqiu/Shenlan-Course-Multi-Sensor-Fusion-for-Localization-Mapping/tree/main/%E7%AC%AC6%E7%AB%A0%EF%BC%9A%E6%83%AF%E6%80%A7%E5%AF%BC%E8%88%AA%E8%A7%A3%E7%AE%97%E5%8F%8A%E8%AF%AF%E5%B7%AE%E6%A8%A1%E5%9E%8B

第6章：惯性导航解算及误差分析

课程内容

代码、PPT、视频见文件夹

课程笔记

三维运动描述基础知识

多传感器融合中三维运动的导航信息包含姿态、速度、位置，其中姿态的处理最为复杂，也最为核心。 姿态有三种表示形式：欧拉角、旋转矩阵、四元数，此外还有等效旋转矢量，但它一般在中间计算过程中使用。

参考文献： 1. Quaternion kinematics for the error-state Kalman filter

1. 《捷联惯导算法与组合导航原理》（严恭敏等著）

姿态描述方法

欧拉角

欧拉角等同于把姿态绕三次不同轴旋转。 不同的旋转顺序会得到不同的欧拉角，常见的有： a. 机器人坐标系：xyz分别对应前左上，旋转顺序为z-y-x； b. 惯性导航坐标系：xyz分别对应右前上，旋转顺序为z-x-y； c. 另一种惯性导航坐标系：xyz分别对应前右下，旋转顺序为z-y-x。

思考：为什么不同的坐标系定义下，会选择不同的旋转顺序？

万向锁：当载体处在某个姿态时，会产生奇异性问题，导致丢失一个自由度。 不同的旋转顺序下，产生万向锁时所处的姿态不同。

不同坐标系定义下，选择不同旋转顺序的原因：其本质都是按照“航向->俯仰->横滚”的顺序旋转，因为此时万向锁出现在俯仰为90°时的情况，而多数载体出现该姿态的几率最小。 需要注意的是，欧拉角有明确的物理意义，不随坐标系定义的不同而改变： a. 俯仰角：载体抬头为正，低头为负； b. 横滚角：向右滚为正，向左滚为负； c. 航向角：机器人中，一般以逆时针旋转为正，顺时针旋转为负。(在与地理系相关的惯性导航中，常以北偏东为正，北偏西为负，遇到时需要注意)

由于欧拉角必然产生奇异性，因此一般只用它做人机交互的显示，而不用来做姿态解算。

旋转矩阵

​

旋转矩阵是单位正交矩阵，即行列式为1，且逆为转置。 优点： a. 没有奇异性，适合用于解算； 缺点： a. 用9个元素表示3个自由度，会增加计算复杂度； b. 为了保持正交性，一般更新完毕后，要重新做正交化

四元数

​

​

​

等效旋转矢量

​

​

各描述方法之间的关系

​

​

​

​

​

三维运动的微分性质

旋转矩阵微分方程

​

四元数微分方程

​

​

​

等效旋转矢量微分方程

​

惯性导航解算概述

1)目的：利用IMU测量的角速度、加速度，根据上一时刻导航信息，推算出当前时刻导航信息，包括姿态解算、速 度解算、位置解算。

2)方法：由姿态、速度、位置的微分方程，推导出它们的解，并转变成离散时间下的近似形式，从而可以在离散时 间采样下，完成导航信息求解。

3)优点：IMU不受外界信号干扰，可以得到稳定、平滑的导航结果。 4)缺点：误差随时间累计，一般时间越长，累计误差越大。因此需要融合，而导航解算(本小节)及其误差分析(下一 小节)，是融合的基础。

姿态更新

​

​

​

​

​

惯性导航解算总结

​

惯性导航误差分析

误差方程推导方法

​

姿态误差方程

​

​

​

速度误差方程

​

位置误差方程

​

bias误差方程

​

惯性导航误差分析总结

​

作业

利用IMU仿真数据，进行惯性导航解算，并通过计算与ground truth之间的误差，对比欧拉法与中 值法的精度差异。

评价标准：

1) 及格：根据课程给定的数据，完成基于中值法的解算；

2) 良好：根据课程给定的数据，完成基于中值法、欧拉法的解算，并对精度做对比分析；

3) 优秀：利用IMU仿真程序，自己生成不同运动状况(静止、匀速、加减速、快速转弯等)的仿真数 据，对比两种解算方法精度差异与运动状况的关系，并给出原因分析。

IMU数据仿真程序地址：https://github.com/Aceinna/gnss-ins-sim

附加题(不参与作业考核)：

1. 四阶龙格库塔法也是一种导航解算方法，但多数情况下并没有必要，在剧烈运动情况下才会产生精度差异，所以本课程并没有讲解。 感兴趣或有疑问的，可以通过仿真证明这一点，即利用IMU仿真程序产生不同运动状况下的仿真数据，对比四阶龙格库塔方法与中值法的精度差异。
2. 从惯性导航的误差方程里，可以看出导航误差与器件误差(陀螺仪或加速度计bias等)以及初始导航误差(初始姿态误差、速度误差、位置误差等)的关系。 例如，假设初始姿态为单位阵，当 x 加速度计有零偏 ，且没有其他器件误差和初始导航误差时，x 方向的速度误差、位置误差随时间的变化分别为 。 请仿照上述例子，自行推导导航误差与其他器件误差和初始导航误差的关系函数，并用仿真数据证明该函数关系正确。

参考博客

使用示例教程, 基于Python的开源GNSS/INS仿真