捷联惯导基本算法
惯性导航技术于20世纪50年代最初开始投入使用,可分为物理平台与模拟平台。物理平台就是平台式惯性导航系统(PINS)。模拟平台又称捷联式惯性导航系统(SINS),它以计算机为平台,随着MEMS与微处理器技术的飞速发展也得到了迅速推广,由于SINS结构简单、体积小的特点迅速受到普遍的关注。
1 姿态更新
姿态更新微分方程:
式中,×表示对某一矢量求反对称矩阵,wb表示载体坐标系b系相对于惯性坐标系i系的角速度在b系下的投影,wn角速度矢量的标注方法同上。
由上文介绍的四元数与姿态变换的关系可得:
式中所有矢量数据均使用四元数形式来表示。
wn可由与wig与wgn相加来表示:
上式中,wig与wgn分别由下式表示为:
式中,L表示g系下的纬度,N、W、U表示n系下的北向、西向与天向,v
表示n系下三个轴向上的速度,R分别为沿地球子午圈与卯酉圈的主曲率半径。
2 速度更新
速度更新微分方程为:
式中,a为加速度计的观测值,g为重力加速度在n系下的分量,wgnv为地球表面的载体运动产生的向心加速度;wigv表示载体与地球发生运动而产生的哥氏加速度;即:
普通民用等级的惯性传感器并不能对地球的自转有所反映,因此使用时需忽略地球自转速度与哥氏加速度对系统产生的影响,则对式(2.33)简化,速度解算微分方程改写如下:
3 位置更新
位置更新微分方程为:
式中,C为位置矩阵,一般采用西北天坐标系时,h表示当地海拔高度,经度与纬度L之间关系可用矩阵形式表示如下:
则经纬度可表示为:
组合导航基本技术
1 SINS/GNSS松组合模型
SINS与GNSS的松组合导航,根本上就是惯性器件与GNSS的姿态、速度及位置信息融合处理的一种组合导航方式。在SINS/GNSS松组合中, GNSS的输出用来为SINS的输出数据进行校正,且二者关系相对独立。松组合的基本工作原理如图所示:使用GNSS接收机与惯性元件测得的速度、位置信息分别作差,该差值即为观测值,之后通过组合滤波器得到惯性传感器的误差估计值,并将其反馈给SINS从而实现输出的矫正。
由上述介绍及原理图展示可以看出,松组合模式的导航系统可以对两系统进行互补,大幅度地提高了导航定位的输出精度,并且其组成结构较为简单,实现起来也较为容易。然而松组合模式的输出几乎不存在冗余信息,这就意味着难以及时对系统异常数据进行监测,并且接收机通过自身滤波输出速度及位置信息,与滤波器形成串联关系,这种情况容易影响观测噪声的相关性,即观测噪声不再属于白噪声。如果此类情况发生可能会导致滤波器输出的稳定性下降。
实际使用时
松组合模式的组合导航系统解算步骤如图所示。
√系统开始启动时,SINS输出初始姿态、位置及速度信息。
姿态信息的确定通过九轴传感器的融合互补补偿得到。使用加速度计输出对陀螺仪解算的横滚角与俯仰角进行校正,使用磁力计输出对陀螺仪解算的航向角进行校正,最终输出载体姿态。而速度及位置信息在初始时刻使用GNSS输出信息进行对准,当载体运动速度提高到某规定值后,直接使用GNSS输出的速度、位置信息作为速度及位置的初始值,即:
√将九轴传感器输出时间与GNSS输出时间进行比较,如果九轴传感器输出时间大则初始对准完成,反之,进行循环更新。
√使用当前时刻的九轴传感器输出进行解算,一方面通过滤波器输出姿态信息,另一方面经过滤波器预测,求得状态观测量与状态协方差矩阵。
将当前时刻九轴传感器与GNSS输出的时间信息进行比较,当满足:
√即当两系统输出时间最为接近时,开始进行融合解算,以此时获得的GNSS信号的速度及位置数据作为滤波器的观测量进行滤波解算。
反之则更新数据对下一时刻的SINS进行解算,并与该GNSS输出进行融合。如此循环比较并输出。
√输出当前时刻的速度及位置信息,同时将系统误差状态量反馈给SINS,对下时刻更新的数据进行补偿。通过以上循环算法最终实现导航过程。
未完待续。。。
To be continue…