组合导航载体轨迹发生器的设计与实现

季云健

（杭州万向职业技术学院，浙江 杭州 31000）

0 引言

随着卫星导航系统和传感技术的发展，低价捷惯性导航得到广泛应用[1]。通过实地测量获取惯导元件输出数据，受试验周期、成本、环境等多种因素影响，难度较大[2-3]。为避免实地测量周期长、成本高、易受干扰等，越来越多的学者通过建立飞行轨迹模型来获取惯导元件的输出数据[4]。轨迹发生器是惯性导航系统进行算法验证和仿真的基础，用于产生仿真所需要的传感器数据和BDS参数[5]。

本文设计一种通过建立控制方程计算飞行器飞行状态，生成理想轨迹的轨迹发生器，对组合导航系统中惯性测量单元（IMU）[6]、BDS位置和速度等多类型传感器数据生成机理进行分析,设计了捷联惯性组合导航轨迹发生器。利用轨迹发生器生成的仿真数据进行组合导航解算,导航结果验证了所设计的轨迹发生器的正确性和有效性。

1 基本理论

1.1 载体轨迹发生器原理

轨迹发生器用于产生模拟场景中各个时刻载体的导航参数:位置、速度、姿态、比力加速度积分增量和角速度积分增量[7]。加速度增量和角速度增量的估计是设计轨迹发生器的的关键。为了实现误差控制，本文计算飞行器飞行状态，生成理想轨迹的轨迹发生器。具体步骤如下。

1）直线运动（静止/匀速）阶段，俯仰角、航向角、横滚角、加速度为0。

2）加速减速运动阶段，飞行器姿态角不变，三方向加速度任取一个常值。

3）上升/下降运动阶段，载体俯仰角以等角速度增大。

4）沿轴滚准运动阶段，载体俯仰角和航向角不变，横滚角以等速度递加，三方向加速度不变。

5）转弯运动阶段，载体航向角以等角速度增大。

1.2 标准SRCKF算法

标准SRCKF算法包括初始化、时间更新和量测更新三个步骤[8]：

1）初始化

假设n维系统状态x的初始概率分布满足

其中，S0是初始协方差矩阵P0的平方根，通过楚列斯基分解chol(· )计算得到，需要注意的是，在SRCKF 中得到不断更新的协方差矩阵是Sk|k，而在CKF中不断更新的协方差矩阵是Pk|k。

2）时间更新（预测过程）

计算k- 1时刻的容积点Xk-1|k-1：

其中，[ 1]i为n维向量，是n×n维单位矩阵In×n的第i列。

将容积点代入非线性系统方程，计算预测容积点Xi,k|k-1和预测值：

更新状态协方差矩阵平方根

其中，SQ,k-1为系统噪声协方差矩阵Qk-1的平方根，即满足为矩阵三角化算法，一般使用QR 分解实现矩阵三角化的表达式如下：

3）量测更新

计算k时刻的预测容积点Xk|k-1：

将预测容积点代入非线性观测方程，计算观测量的预测值

计算观测量预测值协方差矩阵平方根

其中，SR,k为系统观测量协方差矩阵Rk的平方根，即满足Rk=SR,k·，可以仿照使用楚列斯基分解计算SR,k，Zk|k-1的表达式如下：

计算滤波器增益矩阵Kk：

使用观测值zk完成状态量测更新

更新状态协方差矩阵平方根

1.3 载体轨迹发生器

假设载体为某飞行器，仿真过程包含了飞行器从静止到滑跑、上升、转弯和平飞等全过程，将仿真时间设定为1小时，飞行器的运动状态设计如表1所示。

表1 飞行器运动状态及相关参数设计

根据上述飞行器状态设计，生成的飞行器姿态角速率、姿态角、加速度和速度如图1所示。

图1 飞行器姿态角速率、姿态角、加速度和速度的生成

图2 仿真生成的飞行器轨迹图

图3 仿真生成的加速度计输出和陀螺输出图

1.4 理想惯性传感器数据

根据设计的飞行器姿态角速率、姿态角、加速度和速度，生成理想的惯性传感器数据。惯性传感器数据[9]包括三个陀螺和三个加速度计。

定义导航坐标系n为东-北-天坐标系，定义载体坐标系b为右-前-上坐标系，定义由导航坐标系到载体坐标系的旋转由三次坐标旋转组成，第一次为绕z轴旋转，第二次绕x轴旋转，第三次绕y轴旋转

的方向余弦矩阵为

其中，θ为俯仰角，γ为横滚角，ϕ为航向角。在轨迹生成中，得到的速度是载体系下的速度，根据飞行器姿态和上述的方向余弦矩阵定义，可以将载体系下的速度转换为导航系下的速度，包括北向速度vN、东向速度vE和天向速度vN，对其积分后可得到载体的纬度L、经度λ和高度h。

载体坐标系b相对于导航坐标系n的旋转角速度计算公式如下

其中，dθ/dt，dγ/dt和dϕ/dt分别表示俯仰角速率、横滚角速率和航向角速率，矩阵M的定义如下

陀螺输出的角速度为载体系相对于惯性空间的转动角速度在载体坐标系下的投影，其计算方法为

其中，Ω为地球自转角速率，RN为子午圈半径，RE为卯酉圈半径。

将前后两次载体系下的速度求差并除以仿真补偿后，得到载体系下的加速度，加速度计所测量的载体系下的比力计算公式为

其中为地球重力加速度矢量。仿真得到的飞行器轨迹、加速度计输出和陀螺输出如下图所示：

1.5 含有误差的惯性传感器数据生成

为了使仿真贴近实际，需要在生成的惯性传感器数据中加入误差。向理想传感器数据中加入的误差包括固定误差[10]和随机误差[11]。

固定误差项包括：零偏项、标度因数误差和惯性传感器安装误差。固定误差项设置如表2所示。

表2 固定误差项参数设置

惯性传感器的随机误差包括：随机零偏、一阶马尔可夫噪声和白噪声。随机误差参数设置如表3所示。

表3 随机误差参数设置

仿真生成的惯性传感器误差如图4所示。

图4 仿真生成的惯性传感器误差

1.6 BDS仿真数据生成

根据载体轨迹设计中生成的纬度、经度和高度、东向速度、北向速度和天向速度，向其中加入误差得到BDS仿真数据，BDS的误差参数设置如表4所示。

表4 BDS的误差参数设置（0.01m/s）

表5 BDS位置粗差（50m）

仿真生成的BDS位置和速度如图5所示。

图5 仿真生成的BDS位置和速度图

2 仿真验证及分析

为验证轨迹发生器的正确性，将以CKF为模型得到标准CKF组合导航结果，组合导航与飞行过程真实轨迹对比如图6所示。

图6 标准CKF导航轨迹图

图7 未加入误差卡尔曼滤波组合导航纬度、经度、高度、位置误差图

现在向BDS 位置观测量加入粗差，在[800,900]平飞阶段，加入100秒位置误差，如下所示。

图8 加入误差下卡尔曼滤波组合导航纬度、经度、高度、位置误差图

由上述实验结果可以看到，从第800秒（0.22h）开始加入BDS 位置粗差，至900 秒（0.25h）撤销粗差，组合导航定位精度受到了严重的影响。水平位置精度在粗差撤销后180秒回归正常水平，而高度精度在粗差撤销后越1600秒才回归正常水平，该现象与惯性导航系统的天向通道本身不稳定的特性有关。

3 结束语

本文利用轨迹发生器产生的轨迹数据和导航算法进行了组合验证，结果表明了本文设计的有效性。但当BDS位置观测量中含有粗差时，给组合导航精度带来了不可接受的显著影响，因此必须设法使组合导航滤波器具有一定的抗差自适应能力。为解决这一问题，接下来将深入研究抗差自适应容积卡尔曼滤波器。