基于SCKF和姿态估计的SINS/GPS在线对准方法

张桓瑞，刘向龙，邵洪峰

(1. 北京航空航天大学 仪器科学与光电工程学院, 北京 100191；2.交通运输部科学研究院 城市公共交通智能化交通运输行业重点实验室,北京 100029；3.北京航空航天大学 仪器科学与光电工程学院, 北京 100191)

0 引言

随着微机电系统(MEMS)传感技术不断发展，基于MEMS技术的惯性测量单元(IMU) 已在小型无人机及战术武器等领域得到了广泛应用。目前，MEMS惯性器件精度不高，一般MEMS陀螺精度大多在每小时几十度到100°，因此，微惯性测量单元(MIMU)进行空中对准时常处于大失准角状态下。目前对于大失准角下的空中对准研究主要有：

1) 对大失准角下的姿态误差模型进行研究[1-6]。

2) 基于最优估计的初始对准法(OBA)将初始对准转换为通过使用量测信息对初始姿态的最优估计问题。

2013年，吴等[7]提出了一种基于最优估计的空中粗对准法，但此方法对使用惯性器件精度要求较高。2017年，CHANG等[8]提出了一种基于间接卡尔曼滤波的OBA算法，先使用姿态估计的方法将姿态误差缩小到小角度，然后使用线性卡尔曼滤波进行精对准，此方法可用于MEMS惯导系统,但对准时间较长。CUI等[9]基于OBA法构建了非线性的量测模型来估计代表初始姿态转换矩阵的罗德里格参数，并通过全球定位系统(GPS)提供的速度位置信息实时计算当前时刻的载体姿态。此方法使用二阶扩展卡尔曼滤波(2nd-EKF)将非线性量测模型线性化，需要计算二阶雅克比矩阵,计算量较大。

本文采用CUI等提出的滤波模型，引进了平方根容积卡尔曼滤波来处理非线性量测模型，得到了更快的收敛速度，并保证滤波过程的稳定。

1 姿态估计算法

1.1 坐标系定义

载体坐标系(b系)：坐标系原点在载体的质心，y轴为载体纵轴，x轴指向载体右侧与y轴垂直，z轴和x,y轴组成右手坐标系。

导航坐标系(n系)：坐标系原点在载体的质心，x轴指向地理北向，y轴指向地理东向，z轴指向天向。

初始导航坐标系(in系)：惯性坐标系与初始时刻的导航坐标系重合。

初始载体坐标系(ib系)：惯性坐标系与初始时刻的载体坐标系重合。

1.2 初始姿态估计方法

根据矩阵链式乘法法则，t时刻载体到导航坐标系的姿态转换矩阵可分解成3个部分：

(1)

(2)

(3)

(4)

式中：L0，λ0分别为初始位置纬度、经度；Lk，λk分别为当前时刻纬度、经度;δλ为t时间内经度变化值,且δλ=λk-λ0+ωiet,ωie为地球自转角速度。以上数据均可从GPS获取。

1.3 基于罗德里格参数的误差模型

(5)

式中Vin，Vib分别为in、ib系下的比力积分所得值，且

(6)

(7)

考虑误差可得：

(8)

(9)

式中δVib(tk)为误差项。

将初始姿态转换矩阵用罗德里格参数l表示可得：

(10)

将式(10)代入式(9)整理可得：

l+l×δVib(tk)+wtk

(11)

Dtk=Stk×l+l×δVib(tk)+wtk

(12)

式(12)即为关于初始罗德里格参数的量测方程，此方程为非线性方程。估计出最优的罗德里格参数后，可通过式(10)求得初始姿态转换矩阵，从而求出当前时刻的姿态转换矩阵。

(13)

假定陀螺加速度计的误差模型分别如下：

(14)

(15)

(16)

由式(6)、(8)可得：

(17)

将式(17)左、右两边对t求导，代入式(13)、(15)可得：

(18)

(19)

惯组陀螺加速度计的常值误差微分为0有：

(20)

(21)

(22)

2 平方根容积卡尔曼滤波器

一般离散非线性系统为

(23)

式中：xk为k时刻估计状态量；f为k-1时刻到k时刻的状态转移函数；Γk为噪声驱动函数；wk-1为k-1时刻的随机噪声；zk为k时刻量测量；h为量测函数；vk为量测噪声。

(24)

式(24)积分可使用Spherical-Radial容积准则来进行计算，对于一般高斯分布有[10]：

(25)

平方根容积卡尔曼滤波算法过程如下：

1) 初始化。

Sk-1=chol(Pk-1)

(26)

2) 时间更新。

a. 构造容积点：

Xi,k-1|k-1=Sk-1ξi+xk-1

(27)

b. 用状态方程传播容积点：

(28)

c. 估计k时刻的状态一步预测值：

(29)

d. 计算误差协方差平方根的一步预测：

(30)

(31)

3) 量测更新。

a. 使用误差协方差平方根的一步预测构造容积点：

Xi,k|k-1=Sk|k-1ξi+xk|k-1

(32)

b. 使用量测方程传播容积点：

(33)

c. 计算量测量的一步预测值：

(34)

d. 计算量测自相关协方差的平方根：

(35)

(36)

e. 计算状态和量测互相关协方差的平方根：

(37)

(38)

f. 计算k时刻的滤波增益：

Wk=Pxz,k|k-1/[Szz,k|k-1·(Szz,k|k-1)T]

(39)

g. 计算k时刻的状态估计值：

xk=xk|k-1+Wk(zk-zk|k-1)

(40)

h. 计算k时刻的误差协方差平方根：

(41)

图1 方法流程图

3 半实物仿真结果

为验证此方法对低成本战术武器和民用车辆导航的有效性，本文对以上两种情况分别进行了半实物仿真。采集了MTI-3 MEMS惯导的随机噪声数据，并将其加入预设轨迹数据中。MTI-3 MEMS惯导参数如表1所示。

表1 MTI-3 MEMS惯导参数(g=9.8 m/s2)

仿真过程中的参数设置如下：

(42)

通常，短距离制导武器的飞行时间在60 s内，飞行距离在20 km内。根据短距离制导武器轨迹特性，半实物仿真使用的轨迹姿态与速度变化如图2、3所示。初始姿态估计结果如图4、5所示。

图2 制导武器仿真轨迹姿态变化

图3 制导武器仿真轨迹速度变化

图4 制导武器初始姿态估计结果

图5 制导武器姿态误差

由图4可看出，对3个初始姿态角的估计均在10 s内完成收敛。其中航向角估计误差在0.1°内；俯仰角估计误差在0.2°内；横滚角误差在0.5°内。由图5可看出，本文方法可在25 s内完成姿态角的估计航向角和俯仰角的误差均在0.1°内，横滚角误差在0.3°内。

对于低成本民用车辆导航，设置了如下半实物仿真轨迹进行验证。图6为车辆东向、北向速度及航向角变化示意图。其中15～25 s为匀加速直线运动，故东、北向速度发生变化但航向角未发生改变。

图6 车辆仿真速度航向变化

图7为车辆仿真初始姿态估计结果。由图可知，对3个初始姿态角的估计均在15 s内完成收敛。其中航向角估计误差在1°内；俯仰角估计误差在0.5°内；横滚角误差在0.5°内。图8 车辆仿真姿态误差。由图可看出，在30 s内，航向角误差在0.2°内，俯仰角和横滚角误差在1°内，可满足低成本民用车辆的对准需求。

图7 车辆仿真初始姿态估计结果

图8 车辆仿真姿态误差

4 结束语

本文提出了基于平方根容积卡尔曼滤波器和姿态估计法的SINS/GPS在线对准方法，并针对短距离制导武器及低成本车载导航系统进行了半实物仿真。该方法可在25 s左右完成在线对准，其中短距离制导武器仿真结果航向角及俯仰角误差在0.1°内，横滚角误差在0.3°内；低成本车载导航系统仿真结果航向角误差在0.2°内，俯仰角及横滚角误差在1°内，可以满足制导武器及低成本民用车辆的对准需求。