GPS_SINS全组合导航系统的姿态组合算法_杜亚玲 - 副本

GPS/SINS全组合导航系统的姿态组合算法

杜亚玲　刘建业　熊智(南京航空航天大学自动化学院,南京210016) 姜涌(南京航空航天大学无人机研究院,南京210016)

摘要　利用卡尔曼滤波的GPS/SINS全组合导航系统中,将IMU量测的平台误差角简单的近似为姿态误差角,会带来较大的数学模型误差。文章通过分析姿态组合算法中平台误差角与姿态误差角物理意义的不同,得到了二者相互转换的关系式;从实际应用的角度出发,采用对观测向量预处理的方法,对姿态组合算法进行了改进,从而消除了数学模型误差,并且很容易进行工程实现。仿真结果表明使用改进后的姿态组合算法能够有效的提高全组合导航系统的精度。主题词　全球定位系统　捷联式惯性导航　组合导航　姿态参数　组合算法　仿真

1　引言传统的GPS/SINS全组合导航系统,都以GPS的速度位置测量值作为卡尔曼滤波器的观测量,

来估计惯性导航系统的误差状态量。但随着近年来GPS实时测姿技术的发展,利用GPS的姿态、速度和位置信息,进行GPS/SINS全组合正在逐渐成为研究的热点[1]。目前,对GPS/SINS全组合导航系统进行的研究,大都将IMU量测的平台误差角简单的近似为姿态误差角[2～4],用于卡尔曼滤波。这种简单的数学模型给组合系统带来一定的误差。2002年

Yang提出了一种详细推导GPS/INS姿态量测方程的方法[5],但是其方程复杂不适合工程应用,并且不能进行全姿态解算。本文根据平台误差角和姿态误差角物理意义的不同,推导出二者的变换矩阵。文章从实际应用的角度出发,没有将姿态量测方程复杂化,而是将这种思想体现在GPS观测量的预处理上。从而不仅大大简化了姿态量测方程,也解决了全姿态解算的问题,具有一定的工程实用价值。

2　GPS/SINS组合导航系统算法通过对GPS和SINS系统的性能及误差源的分析,系统采用18维误差状态变量的卡尔曼滤波器的闭环校正算法来实现[6]。卡尔曼滤波器主要由捷联惯性系统的误差方程及系统量测方程组成,

方程如下(注:以下推导均定义在东北天坐标系下)

平台误差角方程<・=δωnie+δωnen-(ωnie+ωnen)×<+εp(1)

速度误差方程δv・=fn×<-(2

δωnie+δωnen)×v-(2ωnie+ωn

en)×δv+Δp(2)

国防预研重点项目(41309030309)收稿日期:2004212202。收修改稿日期:2005204201

352006年2月第　1　期

中国空间科学技术

CHINESESPACESCIENCEANDTECHNOLOGY位置误差方程δL

・

=δ

v

n

Rm+h

δλ・=δveRn+hsecL+veRn+hsecLtanLδL

δh

・

=δvu

(3)

位置误差量测方程ZS=δS+v

S(4)

速度误差量测方程Zv=δv+v

v(5)

姿态角误差量测方程Zα=<+v

<(6)

式中　p代表平台坐标系,n代表导航坐标系,<=[误差角,εp为等效陀螺漂移,δv=[δve　δvn　δvu]T表示惯导系统速度误差,󰂈p为加速度计量测误差,δS=[δL　δλ　δh]表示惯导系统位置误差,vS代表位置量测噪声,vv代表速度量测噪声,

v<

代表平台误差角量测噪声。

陀螺噪声模型考虑为随机常数和一阶马尔柯夫过程,加速度计误差考虑为一阶马尔柯夫过程,

利用状态扩充法,系统状态矢量确定为　X18×1=[εbxεbyεbzεrxεryεrz󰂈x󰂈y󰂈z]T

(7)

GPS/SINS全组合的观测量共有9维,由GPS和捷联惯导输出的位置、速度和姿态信息的差值组成:

ZS=[LI-LG λI-λG hI-hG]T(8)

Zv=[vIE-vGE vIN-vGN vIU-vGU]T(9)

Zα≈[γI-γG θI-θG ψI-ψG]T(10)

由公式(6)和公式(10)得到姿态量测方程为

Zα(t)=δγδθδψ(11)

由于状态向量[[δγ　δθ　δψ]T表示的是系统姿态误差角,二者的物理意义不同,具有一定的转换关系。一般的姿

态组合方法都忽略了二者的差异,简单视为约等的关系,这会给系统带来一定的数学模型误差。

3　姿态组合算法的改进

311　算法改进思路为了使系统姿态量测方程中状态向量和量测向量的物理意义统一起来,系统采用将观测量转换为平台误差角后,再进行姿态组合的方法。这种观测向量预处理后进行组合的方法,能够消除姿态组合的数学模型误差,从而有效提高系统精度。经过预处理的观测值表示为Z′α(t)=[<′e　<′n　<′u]

T

(12)

得到新的姿态组合量测方程为Z′α(t)

=

e

<′n

<′u

(13)

新的姿态组合算法的重点和难点在于观测向量的预处理方法,即将原有的表示姿态误差角的观测向量,转换为表示数学平台误差角的物理量的过程。

45 中国空间科学技术 2006年2月312　观测向量的预处理捷联惯导系统数学平台误差角的定义为:平台坐标系(p系)和导航坐标系(n系)之间存在的误差角。姿态误差角定义为:真实的姿态角与实际量测的姿态角之间的差值。本文根据捷联惯导数学平台误差角和姿态角的关系,可得如下关于坐标转换矩阵的方程:

Cpn=(Cbp)TCbn(14)

式中　C

pn为平台误差角矩阵,Cbn为理想的姿态矩阵,Cb

p为根据IMU量测值得到的姿态矩阵。

假定数学平台误差角矢量<=[u]

T,姿态角矢量α=[θ,γ,ψ]T

,实际量测姿态角矢量

α′=[θ′,γ′,ψ′]T,姿态误差角矢量9α=[9θ,9γ,9ψ]T。

由于平台误差角的存在,实质上p系并不是正交坐标系。根据非正交变换关系,在数学平台误差角矢量和姿态误差角矢量为小量的前提下,可列写平台误差角矩阵为[7]

Cpn=1n

-e

e1

(15)

根据东北天坐标系下姿态角定义,可列写方向余弦阵Cbn、(Cbp)T为(Cbp)T=cosγ′cosψ′+sinγ′sinθ′sinψ′cosθ′sinψ′sinγ′cosψ′-cosγ′sinθ′sinψ′-cosγ′sinψ′+sinγ′sinθ′sinψ′cosθ′cosψ′-sinγ′sinψ′-cosγ′sinθ′cosψ′-sinγ′cosθ′sinθ′cosγ′cosθ′(16)

Cbp=cosγcosψ+sinγsinθsinψ-cosγsinψ+sinγsinθsinψ-sinγcosθcosθsinψcosθcosψsinθsinγcosψ-cosγsinθsinψ-sinγsinψ-cosγsinθcosψcosγcosθ(17)

根据“实际的姿态角=理想的姿态角+姿态误差角”的关系,得γ′<′ψ′=γ+δγ<+δ<ψ+δψ(18)

将式(15)～(18)代入式(14),进行矩阵运算。在运算过程中,由于姿态误差角为小量,所以近似认为cosδγ≈cosδθ≈cosδψ≈1,sinδγ≈δγ,sinδθ≈δθ,sinδψ≈δψ;并忽略二阶以上微量。得到数学平台误差角和姿态误差角的关系式如下<′e

<′n

<′u

=-cosθsinψ-cosψ0-cosθcosψsinψ0-sinθ01δγδθδψ(19)

严格来讲,式中量测噪声v<=[δγG　δθG　δψG]T也应同理转换为v′<=[δUG]

T

,

但是由于本身就是小量,进行转换的意义不大,所以近似认为v′α≈vα。

4　仿真及结论本文利用蒙特卡罗方法,对静态和动态下的GPS/SINS全组合导航系统进行了仿真分析。设定系统器件精度:陀螺精度5(°)/h。加速度计精度为10

-4

g;GPS数据刷新频率为1Hz,GPS姿

态误差012°,速度误差为012m/s,水平位置误差为20m,高度误差为50m。设定初始条件为:载体姿态水平、航向指北(90°),速度为零。文章对每种情况下的系统仿真20次,对导航参数进行统计分析。静态仿真持续3000s,文章对改进姿态组合算法和未加改进的GPS/SINS全组合导航系统进行

552006年2月 中国空间科学技术