组合导航算法设计

INS/GPS组合导航算法设计1 引言目前单一导航系统难以满足实际要求，把两种或多种导航系统组合起来，应用最优估计理论，形成最优组合导航系统，使组合后的导航系统在精度和可靠性都有所提高。

本课题研究飞行器GPS/INS组合导航算法，通过对飞行器INS/GPS 组合导航算法设计，以VC++6.0为平台组建INS/GPS组合导航仿真系统，对组合导航算法进行实现。

并对飞行器的飞行状态进行仿真，仿真前预先设定飞行器的飞行参数（包括平飞、加速、减速、上升、下降、转弯等飞行动作以及每个动作开始结束的时间），通过设计的组合导航仿真系统得到飞行器的位置、速度、姿态角信息。

并通过MATLAB对INS/GPS组合导航解算出来的数据与预先设定的实际飞行数据进行比较分析。

惯性导航系统的优点是：（1）自主性强，它可以不依赖任何外界系统得支持，单独进行导航。

（2）不受环境、载体运动和无线电干扰的影响，可连续输出包括基准在内的全部导航参数，实时导航数据更新率高。

（3）具备很好的短期精度和稳定性。

其主要缺点是导航定位误差随时间增长，难以长时间的独立工作。

全球定位系统是一种高精度的全球三维实时导航的卫星导航系统，其导航定位的全球性、高精度、误差不随实践积累的优点，但是GPS系统也存在一些不足之处，主要是：GPS接收机的工作受飞行机动影响，当载体的机动超过GPS接收机的动态范围时，GPS接收机会失锁，从而不能工作，或者动态误差过大，超过允许值，不能使用。

且GPS接收机的更新频率较低（1HZ），难以满足实时控制的要求。

抗干扰能力差。

此外GPS导航受制于人。

因此GPS系统一般作为理想的辅助导航设备使用。

GPS/惯性组合导航，克服了各自的缺点，取长补短，可以构成一个比较理想的导航系统，GPS/惯性组合导航可以大大降低导航系统的成本。

随着MEMS技术的发展，惯导成本的降低都是组合导航系统发展的优势所在。

我们用组合导航算法将惯性导航单元的信息和GPS的信息进行综合，来补偿惯性元件的误差，修正速度、姿态信号，从而构成一个精度适中、结构紧凑、成本低廉的导航系统。

可用于飞行器的导航。

2 INS/GPS组合系统主要功能和算法选择INS/GPS组合导航系统的主要功能为：a）实时测量飞行器的角速率、加速度、速度、位置以及姿态，完成测量信息的采集和处理；b）完成惯性导航解算；c）完成惯性/卫星组合导航解算。

滤波器算法选择组合滤波器是组合导航系统的核心，其本身性能的优劣将直接影响组合导航的质量。

Kalman滤波算法是最为常用和成熟的滤波算法，它作为一种现代最优估计理论，采用了状态空间和递推计算形式，可对一般线性系统进行线性最小方差估计，并且具有数据存储量小，易于计算机实现等优点，在INS/GPS组合导航领域获得了广泛的应用。

因此系统采用集中式Kalman滤波器进行惯性导航系统和GPS测量与导航信息的综合。

3组合卡尔曼滤波算法设计组合卡尔曼滤波器设计的主要内容包括：确定真实系统的状态；列写系统状态方程；列写量测方程；编制滤波仿真程序；设计降阶滤波器。

组合导航系统的实现和组合效果的充分发挥，很大程度上决定于状态的选择、模型方程的建立和简化、参数的选择和滤波算法设计等。

组合卡尔曼滤波器设计的主要步骤包括：1、针对所选择的系统和测量方法，确定系统状态，以及尽可能真实地描述系统动态特性的状态方程和量测方程；2、根据系统噪声和量测噪声的统计特性，确定噪声的方差强度阵Q和R；3、连续系统离散化；4、确定滤波方程的计算方法；5、编制仿真计算程序，模拟滤波效果，初步确定滤波周期及系统其他参数，按状态协方差阵校核组合系统达到的精度；6、在组合系统满足任务要求精度的前提下，设计导航计算机简化滤波计算方法。

3.1组合模式设计GPS接收机和惯性导航系统选用基于位置、速度的松组合方式。

图3-1为位置、速度信息组合的原理框图。

用GPS和惯导各自输出的位置和速度信息差值作为量测值，经组合卡尔曼滤波，估计惯性器件和惯性导航的误差，然后对器件误差和导航误差进行校正，以提高惯性导航的精度。

图3-1位置、速度组合原理图松组合中，GPS 和惯导仍独立工作，组合作用主要表现为用GPS 辅助惯导；组合系统的输出是通过融合GPS 的导航输出和SINS 的导航输出而得到；能提供冗余度，有界的位置、速度和姿态估计，还提供高数据刷新率的与制导控制方程相配的平动和转动信息。

位置、速度信息组合的主要优点是组合工作比较简单，便于工程实现；而且两个系统仍可独立工作，使导航信息有一定余度。

3.2 INS/GPS 组合导航系统的估计方法组合导航系统利用间接法进行导航参数的估计。

间接法以组合导航系统中某一导航系统输出的导航参数的误差为滤波器状态，滤波器估值的主要部分是导航参数误差估值，然后用估值校正状态。

间接法估计时一般系统方程和量测方程是线性的。

图3-2给出了SINS/GPS 组合导航系统间接估计方法的原理框图。

导航参数输出出校正馈校正图3-2 INS/GPS 组合导航系统间接估计方法原理框图 3.3 组合滤波器的校正方法采用间接估计法的组合导航，系统的状态都是误差状态∆X ，利用状态估值ˆ∆X去对原系统进行校正的方法有两种：输出校正和反馈校正。

图3-2所示为SINS/GPS 输出和反馈校正滤波示意图。

输出校正是用导航参数误差的估值ˆ∆X作为惯导系统输出的校正量。

反馈校正是将惯导系统参数误差I ∆X 的估计值反馈到惯导系统和其余子系统内，对误差状态进行校正。

从形式上看，输出校正仅校正惯导系统的输出量，反馈校正则校正惯导系统的内部状态。

从数学模型的推导结果可以证明，如果滤波器是最优滤波器，即它的状态基本上包括了真实系统的所有状态，状态方程真实地描述了真实系统的变化规律，则输出校正和反馈校正具有同样精度，组合系统具有相同的效果。

然而，在考虑到一些实际情况时，两种校正方式仍有较大区别。

输出校正的优点是工程实现比较方便，组合滤波器的故障不会影响惯导系统的工作。

缺点是由于输出校正的滤波器所估计的状态是未经校正的导航参数误差，而惯导系统误差是随时间增长的，滤波器的数学模型建立在误差为小量且取一阶近似的基础上，因此在长时间工作时，由于惯导误差不再是小量，会使滤波方程出现模型误差，使滤波精度下降。

反馈校正的滤波器所估计的状态是经过校正的导航参数误差，在反馈校正后，惯导系统的输出就是组合系统的输出，误差始终保持为小量，克服了输出校正的缺点，因此可以认为利用反馈校正的系统状态方程，更能接近真实地反映系统误差状态的动态过程，可认为没有模型误差。

反馈校正的缺点是工程实现没有输出校正简单，且滤波器故障直接污染惯导系统输出，使系统可靠性降低。

综上所述，校正方法选择的原则是：只要状态能够具体实施反馈校正，组合导航系统尽量采用反馈校正。

在实际应用中，如果惯导系统精度较高，且连续工作时间不长，则采用输出校正。

反之，如果惯导系统精度较差，且连续工作时间又长，则需采用反馈校正。

由于低成本MEMS 惯导精确较差，因此组合导航需要采用反馈校正的方式。

4 INS/GPS 组合导航算法的设计 4.1 姿态矩阵的计算姿态矩阵是指从导航坐标系(n 系)到载体坐标系(b 系)的变换矩阵，当采用“东北天”地理坐标系为导航坐标系时，姿态矩阵为cos cos sin sin sin cos sin sin sin cos sin cos cos sin cos cos sin sin cos cos sin sin sin sin cos sin cos cos cos b n C γψγθψγψγθψγθθψθψθγψγθψγψγθψγθ+-+-⎡⎤⎢⎥=⎢⎥⎢⎥---⎣⎦ (4.1) 式中，ψ为方位角，θ为俯仰角，γ为横滚角，这三个角称为载体的姿态角. 当MIMU 固连的载体姿态发生变化时，MIMU 中的陀螺仪就能敏感出相应的角速率，姿态矩阵bn C 随之发生了变化，其微分方程为b b bn nb n C C =-Ω (4.2)式中，bnb Ω为角速度Tb bx by bz nb nbnbnbωωωω⎡⎤=⎣⎦构成的反对称阵；x ,y ,z 为右前上方向。

捷联惯导姿态矩阵的即时修正就是实时地给出捷联矩阵，它是捷联惯导的关键任务，而这要通过一定地算法来完成。

进行姿态矩阵即时修正的算法很多，主要有欧拉角法、方向余弦算法、四元数法等。

由于四元数算数法计算量小，存储容量小，仅需要进行简单的四元数规范化处理便可以保证姿态矩阵的正交性，因而成为目前实际应用中普遍采用的姿态计算方法。

单位四元数可用如下形式的来描述：01012323q q Q q q i q j q k q q ⎡⎤⎢⎥⎢⎥= +++=⎢⎥⎢⎥⎣⎦(4.3) 这个四元数的范数为： 222201231Q q q q q = + + + = (4.4)称作“规范化”四元数。

我们可以通过求解一个四元数来计算导航参数。

在捷联导航中，要求载体系到导航系的转换矩阵，要解下列四元数的运动方程：12Q Q =Ω (4.5) 式中，000bx by bz nb nb nb bx bz by nbnbnbby bz bx nb nb nb bz by bxnbnbnbωωωωωωωωωωωω⎡⎤---⎢⎥-⎢⎥Ω=⎢⎥-⎢⎥-⎢⎥⎣⎦，0123q q Q q q ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ (4.6) Q 是从载体系到导航系的转动四元数；bnb ω表示载体坐标系相对导航坐标系的转动角速度在载体坐标系上的投影，它由陀螺仪的输出变换而得。

它与其它速率的关系是：()b b b b b n b b n nnb ib in ib n in ib n ie en C C ωωωωωωωω=-=-=-+ (4.7)式中，b ib ω即为角速率陀螺的输出，n ie ω为地球角速率，是已知的，n en ω为导航坐标系相对地球的角速度，它可以由瞬时速度nen v 求得。

设bx ω、by ω、bz ω为角速率陀螺直接测得的载体相对惯性空间的绝对角速率，则0(cos )sin tan cos sin tan Nbx M nb bx by b E nb by n e N bz nb bz e E N NM bx bE by n e N bz Ee N v R h v C L R h L v L R h v R h v C L R h v L L R h ωωωωωωωωωωωωω⎡⎤-⎢⎥+⎢⎥⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=-+⎢⎥⎢⎥⎢⎥⎢⎥+⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎢⎥⎢⎥+⎣⎦⎡⎤-⎢⎥+⎢⎥⎡⎤⎢⎥⎢⎥=-+⎢⎥⎢⎥+⎢⎥⎢⎥⎣⎦⎢⎥+⎢⎥+⎣⎦(4.8) 式中，E v 、N v 、U v 为东北天的速度，M R 、N R 为地球子午面和卯酉面曲率半径，L 、λ、h 为纬度、经度、高度。