算法理论计算案例.docVIP

导航算法理论计算 1、捷联算法 1.1原理图 图1.1 捷联算法原理图 1.2捷联解算步骤 子午圈曲率半径 公式(1.1) 公式(1.2) 公式(1.3) 卯酉圈曲率半径 公式(1.4) 重力加速度计算 公式(1.5) 其中， 导航坐标系下位移角速率 由于飞行器的运动引起的地理坐标系相对地球坐标系的角速度 公式(1.6) 公式(1.7) 公式(1.8) 地球角速率 由于地球转动引起的地理坐标系的角速度 公式(1.9) 公式(1.10) 公式(1.11) 其中， 四元数法求姿态矩阵 公式(1.13) 公式(1.14) 公式(1.15) 公式(1.16) 公式(1.17) 公式(1.18) 其中， 姿态更新 俯仰角 ： 公式(1.19) 横滚角： 公式(1.20) 航向角： 公式(1.21) 速度更新 东向速度： 公式(1.22) 北向速度： 公式(1.23) 天向速度： 公式(1.24) 位置更新 经度： 公式(1.25) 纬度： 公式(1.26) 2、高动态捷联算法 当运载体存在线振动和角振动，或运载体做机动运动时，在姿态解算中会引起圆锥误差，在速度解算中会引起划桨误差，在位置解算中会引起涡卷误差，在相应算法中需要做严格的补偿。 旋转矢量法可采用多子样算法实现对不可交换误差做有效补偿，特别适用于角机动频繁激烈或存在严重角振动的运载体的姿态更新。姿态更新计算的四元数皮卡算法实质上就是单子样旋转矢量算法。因此，多子样旋转矢量算法比四元数皮卡算法的精度高。 2.1 基于旋转矢量法的姿态更新 由导航坐标系到载体坐标系的姿态矩阵： 构造四元数，由该四元数可确定出载体系至导航系的坐标变换矩阵为： 如果表征载体系至导航系的旋转四元数已确定，按上式便可计算出姿态阵。因此，捷联惯导中的姿态更新实质上是如何计算四元数。 记时刻机体坐标系至时刻机体坐标系的旋转四元数为，时刻导航坐标系至的旋转四元数为，时刻导航坐标系至的旋转四元数为，其中。则 其中 为至的等效旋转矢量，。 ⑴ 单子样旋转矢量算法（即为四元数法的毕卡逼近算法）： ⑵ 双子样旋转矢量算法： 其中“”表示矢量叉乘，、分别表示、两个时间段内的角增量。 ⑶ 三子样旋转矢量算法： 其中、、分别表示、、三个时间段内的角增量。 ⑷ 四子样旋转矢量算法： 其中、、、分别表示、、、四个时间段内的角增量。 旋转矢量构造的姿态变化四元数为： 其中，， 即： 依据公式可得： 于是求出时刻的姿态更新四元数，进一步求取姿态矩阵，获得此时刻的姿态角。 2.2捷联惯导系统的速度更新算法 取地理坐标系为导航坐标系，速度方程为： 式中，为载体在导航坐标系下的速度矢量；为载体的姿态矩阵；为加速度计输出的在载体坐标系下的比力；和分别是位置速率和地球自转速率；为导航坐标系下的重力加速度矢量。 当载体同时存在线运动和角运动时，采用速度增量解算必须补偿速度的旋转效应和划桨效应。速度旋转效应补偿项为： 其中是加速度计在时间段内输出的速度增量；是陀螺仪在时间段内输出的角度增量。旋转效应是由运载体的线运动方向在空间旋转引起的。 速度划桨效应补偿项的三子样算法为： 式中，是内角增量的三个等间隔采样值，是内速度增量的三个等间隔采样值。 2.3捷联惯导系统的位置更新 设位置更新周期为，和分别为时刻和时刻的导航坐标系，坐标系至坐标系的旋转矢量可写成： 其中，在计算