基于D-GPS/IMU的组合导航方法研究与分析

张长远

【摘 要】 研究了一种新的双GPS单元与IMU惯性测量单元组合结构（D-GPS/IMU）的导航应用问题；基于D-GPS/IMU的动力模型结构，分析了其状态空间的可观测性能，提出并证明了系统完全可观测的条件；最后，通过仿真实验，结果表明，该方法收敛稳定且快、组合误差小，具有一定的实用性。

【关键词】 组合导航 可观测性 扩展卡尔曼滤波 自适应

一、D-GPS/IMU动力模型的构造

D-GPS/IMU的结构组成如图1所示。其中的{I}为惯性坐标系，它的坐标原点在GPS基准天线处；{b}为机体坐标系，它的坐标原点位于IMU的几何中心并与IMU的坐标轴平行。P1、P2、r分别表示在{I}中GPS1、GPS2和IMU的向量；e1、e2、△e分别表示在{b}中GPS1、GPS2和两卫星间连线的向量。{b}系相对于{i}的方向用四元素q=[qTvqTo]T来表示，其中的下标v和o分别表示四元素的矢量部分和标量部分。则由四元素表示的载体旋转矩阵A（q）为：

A（q）=（2q2v-1）I3+2qo[qvX]+2qvqTv （1）

其中：[·X]表示向量的叉乘矩阵形式（反对称矩阵），In表示nXn的单位矩阵。

由图1知，GPS天线相对于坐标系{i}的位置矢量计算公式为：

定义：D-GPS/IMU模型的状态向量为x=[qTv、rT、rT、bT]T，其中b为陀螺漂移，非线性系统的观测方程为： z=h（x）+v

二、D-GPS/IMU组合导航方法的观测性分析

组合导航系统的观测性能对系统量测结果的输出会产生很大的影响。不完全可观测的系统会引起闭环回路的量测结果估计失效，同时不能保证输出结果收敛到期望真值。最大效率的利用KF算法要求系统是完全可观测的。

对于线性时变系统，当且仅当它的观测矩阵满秩的时候系统是全局可观测的。如果组合导航系统全局可观测，则输出的估计误差仅仅为系统噪声，而状态初始值对系统产生的误差影响将逐步消失，这样就不会出现原时变系统模型的不稳定的行为特征，增强了系统的稳定性，下面对D-GPS/IMU结构模型的观测性进行分析。

D-GPS/IMU观测性

如果矩阵A可逆，则可以通过对V的第四行左乘[e1X] A-1加到第三行，变换为三角矩阵，因为该三角矩阵的所有对角线子矩阵都是可逆的所以观测矩阵V满秩。

推论：如果连接两个GPS天线间的基线向量△e和加速度向量q不平行，则D-GPS/IMU模型（11）为完全可观测系统。

证明：如果△ea，则D-GPS/IMU模型为完全可观测系统（A为满秩矩阵）。如果A不满秩，则一定存在非零向量ξ≠0，使Aξ=0成立，变换为：

[ξx]△e-λ△e=0 （7a）

其中：λ=ξT（△e·a） （7b）

式（7a）为反对称矩阵[ξX]的特征方程表达式。根据反对称矩阵的实特征值仅为零的性质知，λ=0，ξ=△e代入式（7b），得出：△e·（△e·a）=0 （7c）

仅仅当非零向量△e和加速度向量a平行时，式（7c）才会成立，这与前提条件矛盾，因此△ξ=0不可能成立，即D-GPS/IMU模型为完全可观测系统（A为满秩矩阵）。

同时，△e和a之间的夹角可以通过下式两个向量之间点乘公式来求出：

可以实时地通过检测θ的角度变化来判断D-GPS/IMU系统模型的可观测性能。

三、仿真分析

按照相同的飞行轨迹，采用以下两种不同的结构模型和处理算法进行对比分析验证：仿真飞行路线和参数设置为：初始位置（34.2 108.9 0），初始速度为0m/s，初始姿态误差角为2'，陀螺和加计的噪声方差分别为σg=2·10-5rad/ s，σa=0.25m/s2， 位置和速度初始误差分别为5m和0.1m/s，IMU和GPS的采样速率为20Hz和1Hz，算法滤波周期为1s，滑动长度为L=5且θ≠0。经过爬升、转弯、匀速平飞、加速爬升等飞行阶段，仿真时间为600s，其飞行轨迹如图2所示。

在两种结构模型中系统状态解算后的速度和位置误差是直接可观测的，而姿态误差有着不同的可观测性。所以，可将姿态误差估计作为参考状态量。D-GPS/IMU结构模型的姿态误差估计控制在±0.1°以内，航向误差估计控制在±0.2°以内。在仿真的200～300 s和500～550s中，仿真飞行轨迹的航向变化剧烈（即两次大转弯），使D-GPS/IMU解算的姿态误差角估计在仿真飞行中都保持稳定收敛性和较小的估计误差。

四、结束语

（1）建立了D-gps/IMU的非线性化的动力学模型，并应用线性化处理方法建立了相应的线性化的动力学模型；（2）分别对D-gps/IMU和S-gps-IMU两种结构模型的状态空间的现测性能进行了分析，确定了D-gps/IMU完全可现测的条件；（3）EKF算法中的增益值根据定义的GPS量测残差值进行了调整，残差协方差应用区间反向平滑策略，来提高算法收敛速度和稳定性；通过仿真实验来说明本文的结构模型和算法的有效性。D-gps/IMU模型在满足一定条件时为可观测系统，但增加了硬件成本，同时算法的复杂度也增加了，下一步将在提升算法效率和减小硬件成本上进行优化处理。

参 考 文 献

[1]张涛，徐晓苏，李瑶，宫淑萍.基于惯导及水下声学辅助系统的AUV容错导航技术[J].中国惯性技术学报，2013，04：512-516.

[2]任沁源.基于视觉信息的微小型无人直升机地标识别与位姿估计研究[D].浙江大学，2008.