张彦涛,赵宏明,谭立龙,王 鹏,钟 山

(火箭军工程大学, 西安 710025)

0 引言

SINS以数字递推的方式实现载体定位,惯性测量组件的漂移误差在积分环节中逐渐积累,导致定位系统出现较大偏差[1]。缩小误差的方法一般分为两类:一是与卫星导航系统(GPS、GNSS、北斗等)组合构成组合导航系统[2],二是借助外部传感器的测量信息[3]。

增加外部传感器系统辅助SINS可以有效的避免辅助定位信息丢失,完成对车辆的精确定位[4]。传统的GPS/INS组合导航系统易受外界环境及器件漂移误差影响,难以准确建立模型实现最优滤波[5],激光多普勒测速仪(laser Doppler velocimeter,LDV)和里程计(odometer,OD)是SINS常用的外部辅助传感器[6],文中提出采用LDV与OD的不同测速原理和误差特性,二者之间相互配合,对载体精密测量,实现高精度实时定位。

1 速度测量原理

载体速度是导航系统需要精确测量的关键参数之一。载体速度的测量方法主要有4种:GPS、LDV、OD和加速度计[7]。LDV和OD直接安装在车辆上,自主性强,可靠性高。

1.1 LDV测速原理

LDV是利用多普勒效应直接测量载体的速度,通过测量发射激光与反射激光之间频率的差值,确定物体的速度。具有反应快、测速精度高等优点。

(1)

(2)

1.2 OD测速原理

OD用来进行位置导航,在以速度为观测量建立卡尔曼滤波观测方程时[8],OD测速误差会降低滤波的修正效果。

航位推算模型:

(3)

误差源有两个:安装角误差和刻度系数误差。φO经过标定精确后可以当成一个常数。δKO可以用一阶马尔科夫过程来表述:

(4)

式中：τ为马尔科夫过程相关时间;T为输出周期。

OD航位推算误差:

(5)

实际航位推算方程:

(6)

DR误差方程:

(7)

2 联邦卡尔曼滤波基本原理

卡尔曼滤波一般可以分为集中式滤波和分散式滤波[9]。集中式滤波计算量大,不利于实时计算,Li等[10]提出鲁棒性好的卡尔曼滤波器,但滤波器的容错性能差,不易诊断故障,造成导航系统输出可靠性差。文中采用分散式滤波,即联邦卡尔曼滤波来构建组合导航系统的滤波器[11]。

2.1 联邦滤波器算法

假设滤波过程中的总噪声Q-1分配到各个子滤波和主滤波器中,则有:

(8)

状态的初始信息按照信息分配原则进行分配:

(9)

因此信息总量在分配前后保持不变,若各子滤波器和主滤波器之间相互独立,则全局最优估计可表示为:

(10)

2.2 联邦卡尔曼滤波算法最优性

运用联邦卡尔曼滤波器进行估计时,子滤波器将滤波结果输出与主滤波器融合,再用融合的结果对子滤波器进行校正,得到最优的滤波结果[12]。

系统状态向量:

(11)

系统状态方程:

(12)

增广系统协方差矩阵:

(13)

其中,时间更新和量测更新之间相互独立。

1)时间更新

由式(12)得到:

(14)

则有:

(15)

由于公共噪声Q的存在,Pji≠0始终成立,即时间更新引入的各子滤波器状态相关。“方差上界”法可以消除引起的子滤波器状态相关问题。

初始协方差矩阵上界设置:

(16)

采用“方差上界”法处理后,各个子滤波器的量测更新和时间更新都可以独立,各子滤波器之间的状态估计也是独立的。

2)量测更新

系统量测更新方程:

(17)

当Pjl(-)增加γi倍时,Pjl(+)不会相应增加γi倍。解决此问题可以采用系统重置法:用全局滤波来重置滤波协方差和局部滤波。

经过重置后,协方差最优融合估计在任何情况下都是成立的。Pg(+)可以按照下式:

(18)

上述合成Pg(+)用到了主滤波器和子滤波器协方差阵之逆。局部估计结果虽是次优的,但全局滤波结果是最优的。

2.3 联邦滤波器结构与性能分析

在设计联邦卡尔曼滤波器的过程中,将全局总噪声Q-1分配至主滤波器和子滤波器中,βi取不同值可以得到不同类型的滤波器结构[13]。典型的联邦卡尔曼滤波器结构有6种,如表1所示。表中FDI是指系统故障检测隔离能力,FR是指系统故障恢复能力。

表1 六种联邦滤波结构性能分析表

3 自主定位系统滤波器设计

SINS精度随时间增加逐渐降低。通过运用联邦卡尔曼滤波技术,将LDV/OD测速传感器与车辆辅助信息应用到车载导航中,构建基于车辆约束模型的SINS/LDV/OD自主定位系统。

3.1 自主定位系统结构设计

SINS作为公共参考系统,添加LDV和OD测速传感器,构建联邦卡尔曼滤波器,如图1所示。

3个子滤波器采用集中滤波的方式估计导航误差,主滤波器融合各子滤波器结果,获得定位结果的最优估计,反馈到各子滤波器和SINS进行滤波器自校正,并输出滤波结果校正定位误差。[14]

3.2 子滤波器设计

根据SINS误差模型,选取位置、速度和姿态误差,并与陀螺和加速度计漂移构成15维状态矢量,构建SINS误差方程[4]。

图1 自主定位系统卡尔曼滤波器原理图

(19)

式中,状态向量XS有15个状态的列向量:

(20)

系数矩阵:

(21)

设Re=6 378 137.0 m,f=1/298.275,

ωie=15°/h,Rh=Re+h,R1=Re(1-2f+3fsin2L)+h,

R2=Re(1+fsin2L)+h。

系统噪声:

(22)

噪声矩阵的系数矩阵:

(23)

1)SINS/LDV子滤波器

该子滤波器采用集中滤波的方式,选取SINS的误差方程(19)和LDV的误差方程作为子滤波器的状态方程[15]。以速度差值作为该子滤波器的观测量,建立子系统量测方程。

状态方程:

(24)

量测方程:

(25)

(26)

2)SINS/OD子滤波器

在将OD与SINS组合,采用集中滤波的方式建立子滤波器时,选取SINS的误差方程和DR的误差方程作为子滤波器的状态方程[16]。以SINS与DR之间的位置的差值作为该子滤波器的观测量,建立子系统量测方程。

状态方程:

(27)

量测方程:

(28)

(29)

3.3 主滤波器设计

1)系统离散化

卡尔曼滤波建立的状态方程和量测方程是连续的,因此首先要对建立的状态方程和量测方程进行离散化处理。

(30)

F(t)化成Φk,k-1:

(31)

2)信息分配

(32)

3)主系统滤波

子系统采用集中式卡尔曼滤波法,滤波周期为1 s;主滤波器10 s钟进行一次信息融合计算,融合算法:

(33)

3.4 系统容错性设计

容错系统的具体结构如图2所示,可以看出该容错系统主要包括子系统故障的检测、隔离和容错综合算法。

检测故障的子滤波器并进行隔离,其余的子滤波器误差估计值进入主滤波器,进行系统的误差估计,同时利用主滤波器的结果对故障子滤波器进行故障修复和检测。当故障子滤波系统恢复正常后接入系统,将子滤波结果输入到主滤波器,系统继续提供精确的导航信息。

图2 基于载体约束的SINS/LDV/OD容错系统结构

(34)

(35)

1)当子滤波器1失效时,主滤波器估计误差:

(36)

(37)

估计失效子系统1:

(38)

2)当子滤波器2失效时,主滤波器估计误差:

(39)

(40)

估计失效子系统2:

(41)

3)当子滤波器3失效时,主滤波器估计误差:

(42)

(43)

估计失效子系统3:

(44)

4 试验检验

为验证系统有效性及定位精度,在西安地区进行跑车试验。同时将OD安装在试验车辆上。OD刻度系数误差是0.013 09,滚转角误差0.700 31′,偏航角误差0.720 31′,俯仰角误差-0.085 59′。SINS进行5 min初始对准,起步2 min内对OD进行刻度系数误差标定。在固定时间内特定传感器停止采集数据,其它传感器正常工作。

跑车试验从经纬度(108.77°,34.03°)处出发,至经纬度(108.96°,34.31°)处结束,总路程48.4 km,用时107 min,其中前420 s静止,行驶路线如图3所示。

图3 跑车试验轨迹图

图4表示实际跑车中位置误差。可以看出,试验中经度误差不超过20 m,纬度误差不超过20 m,高度误差不超过10 m。

图4 跑车试验位置误差

试验过程中,模拟自主定位系统中LDV故障,其它设备正常工作。跑车试验1 500～1 800 s时,不记录LDV采集数据。子滤波器1的输出位置误差、速度误差如图5所示。

子滤波器1故障时,对LDV误差进行估计。由结果可知经度误差不超过15 m,纬度误差不超过20 m,高度误差不超过10 m,可以看出东向和北向速度误差不超过0.1 m/s,天向速度误差不超过0.05 m/s。

图5 子滤波器1位置误差、速度误差

模拟自主定位系统中OD故障,其它设备正常工作。跑车试验2 500～2 800 s时,不记录OD采集数据。子滤波器3的输出结果如图6所示。

图6 子滤波器2位置误差、速度误差

子滤波器2故障时,对航位推算误差进行估计。可知经度误差不超过15 m,纬度误差不超过25 m,高度误差不超过5 m,东向和北向速度误差不超过0.1 m/s,天向速度误差不超过0.05 m/s。

模拟自主定位系统中角度传感器故障,其它设备正常。在3 500 s至3 800 s时,不记录角度传感器采集数据。子滤波器3输出的位置误差、速度误差如图7所示。

图7 子滤波器3位置误差、速度误差

子滤波器3故障时,对其误差进行估计。经度误差不超过20 m,纬度误差不超过25 m,高度误差不超过3 m,东向和北向速度误差不超过0.1 m/s,天向速度误差不超过0.05 m/s。

模拟自主定位系统正常工作,主滤波器的位置误差、速度误差如图8所示。

联邦滤波器正常工作时,对自主定位系统的定位误差进行估计。从图8可以看出经度误差不超过15 m,纬度误差不超过20 m,高度误差不超过5 m,东向、北向和天向速度误差不超过0.05 m/s。

在图5～图8中,子滤波器故障时,信息融合技术对子滤波器的误差进行估计,补偿子滤波器的位置和速度误差。自主定位系统误差保持在一个合理的范围内,确保了自主定位系统正常工作。

分析试验结果可知,LDV比OD故障引起的误差小,对组合导航产生的影响也小。同时,组合系统定位定向精度随时间趋于稳定,但误差一直存在。分析设计的组合定位系统,发现建立的融合算法误差方程中存在缺陷,导致实际补偿与理论计算存在偏差,融合算法需要进一步改进。

图8 主滤波器位置误差、速度误差

5 结束语

文中主要阐述了LDV/OD测速原理,分析其误差来源。根据LDV和OD的误差源,建立SINS、LDV和OD的误差方程,介绍了联邦卡尔曼滤波器基本原理,设计了SINS/LDV/OD自主定位系统的联邦卡尔曼滤波器,并给出其状态方程和量测方程以及系统容错型设计。跑车试验结果表明经度误差不超过20 m,纬度误差不超过20 m,高度误差不超过10 m,误差合理,设计有效。