捷联惯导算法心得论述.pdfVIP

捷联惯导算法心得 1、四个概念：“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用 的系数。 地理坐标系：东、北、天，以下简称地理。在这个坐标系里有重力永远是（0,0,1g）， 地磁永远是（0,1,x）（地磁的垂直不关心）两个三维向量。 机体坐标系：以下简称机体，上面有陀螺、加计、电子罗盘传感器，三个三维向 量。 换算公式：以下简称公式，公式就是描述机体姿态的表达方法，一般都是用以地 理为基准，从地理换算到机体的公式，有四元数、欧拉角、方向余弦矩阵。 换算公式的系数：以下简称系数，四元数的 q0123、欧拉角的 ROLL/PITCH/YAW、 余弦矩阵的 9 个数。系数就是描述机体姿态的表达方法的具体数值。 姿态，其实就是公式+系数的组合，一般经常用人容易理解的公式“欧拉角”表示， 系数就是横滚 xx 度俯仰 xx 度航向 xx 度。 2、五个数据源：重力、地磁、陀螺、加计、电子罗盘，前两个来自地理，后三 个来自机体。 3、陀螺向量：基于机体，也在机体上积分，因为地理上无参考数据源，所以很 独立，直接在公式的老系数上积分，得到新系数。 狭义上的捷联惯导算法，就是指这个陀螺积分公式，也分为欧拉角、方向余弦矩 阵、四元数，他们的积分算法有增量法、数值积分法（X 阶龙格-库塔）等等 4、加计向量、重力向量：加计基于机体，重力基于地理，重力向量（0,0,1g） 用公式换算到机体，与机体的加计向量算出误差。理论上应该没有误差，这误差 逆向思维一下，其实就是换算公式的系数误差。所以这误差可用于纠正公式的系 数（横滚、俯仰），也就是姿态。 5、电子罗盘向量、地磁向量：同上，只不过要砍掉地理上的垂直向量，因为无 用。只留下地理水平面上的向量。误差可以用来纠正公式的系数（航向）。 6、就这样，系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代 表的姿态也在不断更新。 如果积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角 输出控制 PID（因为角度比较直观），那就需要有个四元数系数到欧拉角系数的 转换。常用的三种公式，它们之间都有转换算法。 再搞个直白一点的例子： 机体好似一条船，地理就是那地图，姿态就是航向（船头在地图上的方位），重 力和地磁是地图上的灯塔，陀螺/积分公式是舵手，加计和电子罗盘是瞭望手。 舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开， 觉得转了 90 度弯，那就会往东开。 当然如果舵手很牛逼，也许能估计很准确，维持很长时间。不过只信任舵手，肯 定会迷路，所以一般都有地图和瞭望手来观察误差。 瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的 X 方位。 然而看到实际灯塔在船的 Y 方位，那肯定船的当前航向有偏差了，偏差就是 ERR=X-Y。 舵手收到瞭望手给的 ERR 报告，觉得可靠，那就听个 90%*ERR，觉得天气不好、 地图误差大，那就听个 10%*ERR，根据这个来纠正估算航向。。 ------------------------------------------------------ 来点干货，注意以下的欧拉角都是这样的顺序：先航向-再俯仰-然后横 滚 公式截图来自：袁信、郑锷的《捷联式惯性导航原理》，邓正隆的《惯性技术》。 -------------------------------------------------- 根据加计计算初始欧拉角 这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要，因为加计和电子 罗盘给出欧拉角的描述方式比较方便。 imu.euler.x = atan2(imu.accel.y, imu.accel.z); imu.euler.y = -asin(imu.accel.x / ACCEL_1G); ACCEL_1G 为 9.81 米/秒^2，accel.xyz 的都为这个单位，算出来的 euler.xyz 单 位是弧度 航向 imu.euler.z 可以用电子罗盘计算 -------------------------------------------------- 欧拉角微分方程 如果用欧拉角算法，那么这个公式就够了，不需要来回转换。 矩阵上到下三个角度（希腊字母）是 roll pitch 和 yaw，