算法理论计算汇总

1、导航算法理论计算1、捷联算法1.1原理图图1.1 捷联算法原理图1.2捷联解算步骤1) 子午圈曲率半径 公式(1.1) 公式(1.2) 公式(1.3)2) 卯酉圈曲率半径 公式(1.4)3) 重力加速度计算 公式(1.5)其中，4) 导航坐标系下位移角速率由于飞行器的运动引起的地理坐标系相对地球坐标系的角速度 公式(1.6) 公式(1.7) 公式(1.8)5) 地球角速率由于地球转动引起的地理坐标系的角速度 公式(1.9) 公式(1.10) 公式(1.11)其中， 6) 四元数法求姿态矩阵 公式(1.13) 公式(1.14) 公式(1.15) 公式(1.16) 公式(1.17) 公式(1.18

2、)其中，7) 姿态更新俯仰角 ： 公式(1.19)横滚角： 公式(1.20)航向角： 公式(1.21)8) 速度更新 东向速度：公式(1.22)北向速度：公式(1.23)天向速度：公式(1.24)9) 位置更新 经度： 公式(1.25)纬度： 公式(1.26) 2、高动态捷联算法当运载体存在线振动和角振动，或运载体做机动运动时，在姿态解算中会引起圆锥误差，在速度解算中会引起划桨误差，在位置解算中会引起涡卷误差，在相应算法中需要做严格的补偿。旋转矢量法可采用多子样算法实现对不可交换误差做有效补偿，特别适用于角机动频繁激烈或存在严重角振动的运载体的姿态更新。姿态更新计算的四元数皮卡算法实质上就是单

3、子样旋转矢量算法。因此，多子样旋转矢量算法比四元数皮卡算法的精度高。2.1 基于旋转矢量法的姿态更新由导航坐标系到载体坐标系的姿态矩阵： 构造四元数，由该四元数可确定出载体系至导航系的坐标变换矩阵为： 如果表征载体系至导航系的旋转四元数已确定，按上式便可计算出姿态阵。因此，捷联惯导中的姿态更新实质上是如何计算四元数。记时刻机体坐标系至时刻机体坐标系的旋转四元数为，时刻导航坐标系至的旋转四元数为，时刻导航坐标系至的旋转四元数为，其中。则 其中 为至的等效旋转矢量，。 单子样旋转矢量算法（即为四元数法的毕卡逼近算法）： 双子样旋转矢量算法：其中“”表示矢量叉乘，、分别表示、两个时间段内的角增量。

4、三子样旋转矢量算法：其中、分别表示、三个时间段内的角增量。 四子样旋转矢量算法：其中、分别表示、四个时间段内的角增量。旋转矢量构造的姿态变化四元数为：其中， 即：依据公式可得：于是求出时刻的姿态更新四元数，进一步求取姿态矩阵，获得此时刻的姿态角。2.2捷联惯导系统的速度更新算法取地理坐标系为导航坐标系，速度方程为： 式中，为载体在导航坐标系下的速度矢量；为载体的姿态矩阵；为加速度计输出的在载体坐标系下的比力；和分别是位置速率和地球自转速率；为导航坐标系下的重力加速度矢量。当载体同时存在线运动和角运动时，采用速度增量解算必须补偿速度的旋转效应和划桨效应。速度旋转效应补偿项为： 其中是加速度计在时

5、间段内输出的速度增量；是陀螺仪在时间段内输出的角度增量。旋转效应是由运载体的线运动方向在空间旋转引起的。速度划桨效应补偿项的三子样算法为： 式中，是内角增量的三个等间隔采样值，是内速度增量的三个等间隔采样值。2.3捷联惯导系统的位置更新设位置更新周期为，和分别为时刻和时刻的导航坐标系，坐标系至坐标系的旋转矢量可写成： 其中，在计算时所用的纬度是在时间点上纬度的线性外推值： 若已求得，则有 由于很小，所以上式可取一阶近似： 时刻的位置矩阵可按下式确定： 式中为上一位置更新时间点上确定的位置矩阵。位置矩阵与经度和纬度关系如下： 高度求取如下： 式中，是沿导航坐标系z轴的分量。3、静态初始对准3.1

6、粗对准基本原理捷联系统是将IMU与载体固联，安装方式对应的坐标系为右前上。载体静止水平时，导航坐标系下的载体三轴的加速度值为，当存在姿态角时，有载体坐标系与导航坐标系的关系如下所示： 公式(3.1) 公式(3.2) 载体坐标系下x轴的加表测量值; 载体坐标系下y轴的加表测量值; 载体坐标系下z轴的加表测量值; 载体俯仰角 载体横滚角 载体偏航角这样通过以下公式可求出水平的两个姿态角： 公式(3.3) 公式(3.5)3.2精对准基本原理精对准是粗对准的继续，主要任务是在尽可能使“数学平台”趋于水平状态的同时，能够自动检测出陀螺的漂移，并精确的计算出姿态角，为导航计算提供准确的初始条件。精对准与组

7、合导航的思想一致，构建状态误差模型，引入GPS的速度观测信息，构建观测方程，对初始的姿态角进行修正。选取的状态向量为： x轴速度误差 y轴速度误差 俯仰误差角 横滚误差角 偏航误差角 x轴加表零偏 y轴加表误差 x轴陀螺零偏 y轴陀螺零偏 z轴陀螺零偏状态方程： 公式(3.6)公式(3.7)其中： 地球自转角速率 载体坐标系到导航坐标系的转换矩阵 x轴加表噪声 y轴加表噪声 x轴陀螺噪声 y轴陀螺噪声 z轴陀螺噪声观测向量： 公式(3.8)观测方程： 公式(3.9) 公式(3.10)1) 系统方程离散化 公式(3.11) 公式(3.12)表示滤波周期。2) 采用卡尔曼滤波进行数据融合采用Kal

8、man滤波对模型进行组合解算获得姿态角信息，解算方程组如下所示： 公式(3.13) 状态误差方差阵 观测误差方差阵 估计误差方差阵 最优增益矩阵或者加权矩阵4、松耦合设计图4.1 松组合导航设计流程图松组合导航设计即利用IMU的信息和GPS的信息相结合，在捷联解算计算出的导航信息与GPS接收的速度和位置信息，利用Kalman滤波算法进行数据融合，估计出导航信息误差值，再采用闭环修正法对捷联解算的导航信息进行修正，获得更准确的导航信息。捷联解算的周期比GPS数据的周期短，因此需要判断GPS数据是否有效，当GPS数据无效时，仅进行捷联解算，图中表示为黑色的箭头流程，输出为纯捷联解算结果;当GPS数

9、据有效时，进行组合导航，算出姿态误差、速度误差和位置误差，反馈给系统进行修正，最后获得导航数据，图中表示为红色的箭头流程，输出为组合导航结果。考虑到车载组合导航系统，测量水平的速度和位置信息，对高度不做要求，去掉高度相关参数，因此可以选用十二维系统，降低维数，减少计算量，也可以减轻数字数据处理运算负担。而对于飞行载体，则不可忽略高度值，需要完整的三维信息，选用十五维导航系统。下面分别对十二维和十五维算法的原理进行阐述，同时验证算法的有效性。4.1十二维组合导航系统设计选取的状态向量为： 纬度误差 经度误差 其他变量与初始对准定义一致。状态方程： 公式(4.1) 观测向量：观测方程： 公式(4.

10、2)4.2十五维组合导航系统设计选取的状态向量为： 状态方程： 公式(4.3) 观测向量：观测方程： 公式(4.4) 公式(4.5)5、紧耦合设计与松耦合相比，紧耦合是双向信息传输的，即一方面GPS信号用于修正INS；另一方面，INS信号在卫星星历的辅助下，也用于计算载体相对于GPS卫星的伪距和伪距率，并用该信息辅助GPS信号的接受和锁相过程，以提高GPS的接收精度和动态性能。紧耦合GPS/INS组合导航系统的结构示意图如图5.1所示。图5.1紧耦合GPS/INS组合导航系统结构示意图紧耦合GPS/SINS组合导航系统取消了GPS接收机内部的卡尔曼滤波器，将其功能放在SINS/GPS组合导航卡

11、尔曼滤波器中实现，能更精确地对惯导的位置和速度误差以及惯性传感器的误差进行估计；并且该系统将惯导产生的载体位置和速度信息馈送到GPS接收机的载波跟踪环和码跟踪环中，提高了GPS接收机的抗干扰能力；同时由于紧耦合系统的量测值直接取自GPS接收机的直接观测量，即卫星与载体之间的伪距和伪距率信息，无量测输入相关问题，且在GPS可见卫星个数小于四颗时仍然能够提供一定精度的导航参数。与松耦合组合导航系统一样，紧耦合系统中的捷联解算的周期同样比GPS数据的周期短，因此需要判断GPS数据是否有效，当GPS数据无效时，仅进行捷联解算，当GPS数据有效时，进行组合导航，算出姿态误差、速度误差和位置误差反馈给系统

12、进行修正，最后获得导航数据。紧耦合组合导航系统的设计流程图如图5.2所示。图5.2 松组合导航设计流程图5.1二十维紧耦合组合导航系统的状态方程紧耦合SINS/GPS组合导航系统状态方程中的变量为SINS误差状态和GPS误差状态的组合，其中SINS的误差状态包括数学平台误差角、速度误差和位置误差以及惯性仪表误差；GPS的误差状态通常取两个与时间有关的误差：一个是与时钟钟差等效的距离误差，另一个是与时钟频率误差等效的距离率误差。当取地理坐标系（东北天坐标系）为导航坐标系时，捷联惯导系统的数学平台误差角方程为： (5-1)由比力方程可以得到速度误差方程为： (5-2)位置误差方程为： (5-3)陀

13、螺仪和加速度计的测量误差包括安装误差、刻度因子误差和随机误差，前两项可以通过转台试验标定并补偿，这里只考虑随机误差。通常取陀螺漂移为： (5-4)式中为随机常值漂移；为一阶马尔科夫过程；为高斯白噪声。陀螺误差模型可表达为： (5-5)式中为相关时间，为白噪声。加速度计的测量误差为零偏和随机噪声，其中随机噪声可表示为： (5-6)式中为相关时间，为白噪声。GPS的误差状态和的微分方程为： (5-7)将SINS的误差方程式(5-1) 式(5-6)与GPS误差方程式(5-7)合并，写为矩阵形式，即可得到紧耦合SINS/GPS组合导航系统的状态方程： (5-8)其中：为紧耦合SINS/GPS组合导航系

14、统的状态变量；为阶的系统状态矩阵 、为阶的动态噪声矩阵、为11维的过程白噪声矢量。5.2二十维紧耦合组合导航系统的观测方程在组合导航系统中，假设通过捷联解算得到的SINS的位置为，SINS位置的坐标真值为，由卫星星历确定的GPS卫星位置为。由SINS载体到GPS卫星的距离可由下式表达： (5-9)将在处展开成泰勒级数可得： (5-10)其中GPS系统的测码伪距是由卫星发射的测距码（C/A码或P码）到用户接收机的传播时间（时间延迟）乘以光速所获得的距离观测量，如下式所示： (5-11)式中为卫星到用户机的几何距离；是与时钟钟差等效的距离误差；为电离层折射对测码伪距的影响；为大气对流层折射对测码伪距的影响。将电离层误差和对流层误差看作高斯白噪声，式(5-11)可写为： (5-12)则紧耦合系统的伪距差量测方程为： (5-13)捷联惯性导航系统安