文档介绍：捷联惯导算法心得
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,根据这个来纠正估算航向。。
------------------------------------------------------
来点干货,注意以下的欧拉角都是这样的顺序:先航向-再俯仰-然后横滚
公式截图来自:袁信、郑锷的《捷联式惯性导航原理》,邓正隆的《惯性技术》。--------------------------------------------------
根据加计计算初始欧拉角
这个无论欧拉角算法还是四元数算法还是方向余弦矩阵都需要,因为加计和电子罗盘给出欧拉角的描述方式比较方便。 = atan2(, ); = -asin( / ACCEL_1G);
ACCEL_1G ^2,,--------------------------------------------------
欧拉角微分方程
如果用欧拉角算法,那么这个公式就够了,不需要来回转换。
矩阵上到下三个角度(希腊字母)是roll pitch和yaw,公式最左边的上面带点的三个是本次更新后的角度,不带点的是上个更新周期算出来的角度。
Wx,y,z是roll pitch和yaw方向的三个陀螺在这个周期转动过的角度,单位为弧度,计算为间隔时间T*陀螺角速度,*=.
--------------------------------------------------
以下是四元数四元数初始化------------