刘梓轩，孙永荣，曾庆化，赵科东

(南京航空航天大学自动化学院导航研究中心，南京 210016)

0 引言

2015年，X-47B型无人机首次实现了自主空中加油。目前，在预对接前的会合编队过程中，加油机与受油机间依赖于低延时战术瞄准网络技术数据链完成信息交互，通过传播双机的INS／GPS等导航信息实现高精度的相对位姿估计。在机间数据链中断情况下，常规的相对导航策略无法满足自主空中加油相对导航要求[1]，空中加油技术会合阶段作为典型的合作目标相对导航问题实际上转换为在没有通信条件或者目标未配置主动传感器的情况下的非合作目标相对定位问题[2]。对于中远距离的无通信空中加油相对导航，通过态势感知传感器对未知环境进行目标识别与引导[3]，保证实时、高可靠、高精度的相对状态估计是本文研究的重点。

在通信中断的情况下，无人机常用的相对导航测量传感器包括：以波达角度(Angle of Arrival，AOA)为量测的传感器，如光电传感器、红外搜索跟踪系统(Infrared Search and Track，IRST)等；以波达距离(Range of Arrival，ROA)为量测的传感器，如激光测距仪等[4]；兼具AOA／ROA的传感器，如激光雷达、合成孔径雷达等[5]。选择合适的态势感知传感器对未知环境进行探索是非常关键的。

对于非线性系统，扩展Kalman滤波(Extended Kalman Filter，EKF)是目前适用范围最广的非线性状态估计滤波算法[6-7]。但在线性化过程中，EKF对非线性函数的Taylor级数进行了一阶截断处理，忽略了其高阶项[8]。这种线性化处理在一些强非线性的情况下可能会使系统产生较大的误差，使得滤波收敛速度缓慢[9]。为了进一步提高算法性能，迭代扩展Kalman滤波(Iterated Extended Kalman Filter，IEKF)得以发展[10-11]。IEKF可以看作是Gauss-Newton法在最大似然估计求解近似问题上的应用[12]，利用可充分信任的量测信息不断迭代修正相对状态，但在迭代初值或协方差误差较大时，IEKF算法容易陷入局部极值点，明显降低了滤波器的精度[13]。IEKF的迭代更新过程实际是对最优问题的求解，在最优问题求解中，每一步迭代都希望可以使目标函数值降低。信赖域狗腿(Dog-Leg，DG)算法作为一种Gauss-Newton算法与梯度下降算法的混合算法[14]，从迭代初值开始先划定一个可以信赖的最大范围，再以当前的工作点作为原点在预设的最大范围内求解目标函数的最优解[15]，兼具了Gauss-Newton法与梯度下降法的优点。

本文在受油机与加油机间数据链中断的情况下，通过综合多个态势感知传感器提供的观测信息，以双机之间的直线距离、视线俯仰角与视线方位角作为传感器输出估计双机间的相对状态。同时，对于非线性系统的状态估计算法，考虑到IEKF算法在状态变量初始误差较大时滤波收敛不理想的问题，本文借鉴了DG算法的优化思想，将信赖域求解最优解过程引入IEKF算法中的迭代更新过程，提出了一种DG-IEKF(Dog-Leg Iterated Extended Kalman Filter)算法。最后，通过与EKF、IEKF算法的对比分析验证了本文算法的有效性。

1 测距测角信息下的相对导航系统模型

1.1 非合作目标CA模型

在地心地固坐标系下，定义受油机绝对位置坐标向量为Xf＝[xfyfzf]， 加油机绝对位置坐标向量为Xl＝[xlylzl]， 状态变量为x＝[ΔXΔX′ ΔX″]T。其中，ΔX＝Xl－Xf为受油机与加油机之间的相对位置， ΔX′和ΔX″分别为受油机与加油机之间的相对速度和相对加速度。在双机加油过程中，加油机一般在指定空域中进行匀速直线运动，而受油机一般以高于加油机的速度接近加油机，双机速度都较为稳定且机动较小。因此，在相对导航目标为非合作目标的情况下，选取匀加速(Constant Acceleration，CA)模型作为相对运动模型。离散化状态方程为

式(1)中，Fk｜k－1和Gk－1分别为状态转移矩阵和系统噪声矩阵，wk－1为3×1维的相互独立的零均值Gauss白噪声向量。其中，Fk｜k－1和Gk－1满足

式(2)中，I3为三阶单位矩阵，T为采样间隔。

1.2 测距测角信息下的量测方程

根据非合作目标定位原理，可以得到相对空间几何模型。本文相对导航的目的就是利用受油机上配置的主被动传感器测得与目标之间的距离与角度信息[ραβ]，计算出受油机与加油机之间的相对位置ΔX＝Xl－Xf＝[xl－xfyl－yfzlzf]T。 如图1所示，可得地心地固坐标系下受油机与加油机之间的空间几何关系

图1 相对导航空间几何关系示意图Fig.1 Schematic diagram of relative navigation geometry

由式(3)可以建立在测距测角信息下的相对导航系统量测方程

2 基于信赖域狗腿算法优化的IEKF算法

2.1 IEKF算法与Gauss-Newton算法的一致性分析

式(6)中，P为状态变量协方差阵。将量测与状态变量估计值组合构造一个伪观测量，并对量测矩阵也进行相应的构造

根据式(7)可得

用最大似然函数L(x)表示Z关于状态变量x的概率密度，有

定义

由式(11)可得，IEKF迭代更新的最大似然目标函数与非线性最小二乘问题的目标函数是等价的。至此，将IEKF的量测更新过程转化为求解非线性最小二乘问题。

Gauss-Newton算法对式(11)的解为

对式(12)进行整理，可得

式(13)中，根据矩阵逆的性质可得

式(13)与IEKF状态递推公式完全一致，IEKF算法可以看作Gauss-Newton算法在最大似然估计求解近似问题上的应用。而Gauss-Newton算法只有在满足一定局部性条件的情况下才能稳定收敛，这也解释了IEKF算法在初始误差较大情况下可能失效的原因。因此，作为一种Gauss-Newton算法的改进显式信赖域算法，将狗腿算法引入IEKF算法，可以有效优化IEKF算法中的状态变量迭代更新过程，增强算法的收敛性与稳定性。

2.2 信赖域狗腿算法

信赖域算法是一种最优化方法，能够保证目标函数最优化总体收敛。本文的信赖域狗腿算法为梯度下降算法与Gauss-Newton算法的混合算法，在求解函数极小值的最优化算法中，每一次迭代都要求目标函数是下降的，从初始点开始先划定一个可以信赖的最大位移量，在预设的最大范围内求解目标函数的最优解，并根据实际的下降量自适应地划定信赖域范围。信赖域狗腿算法的基本思想是：通过比较梯度下降算法的迭代步长、Gauss-Newton算法的迭代步长与划定的信赖域范围来计算迭代增量。在迭代初值误差较大时，通过接近梯度下降算法的迭代步长快速下降，有助于快速收敛；在接近最优值时，通过接近Gauss-Newton算法的迭代步长逼近最优值，避免了梯度震荡。由此，减少了函数陷入局部极小值的情况。

假设非线性最小二乘的目标函数为

定义

根据式(12)，可得Gauss-Newton算法的迭代步长为

根据梯度下降算法的原理，迭代下降方向为

选取合适的步进长度，从而得到最大程度的迭代下降。

假设f(x+τhsd)≈f(x)+τ[f′(x)]Thsd， 代入式(15)可得

结合式(18)与式(20)，可得梯度下降算法的迭代步长为

因此，Δ、τhsd、hgn与hdl的关系示意图如图2所示，通过比较Δ、τhsd、hgn来确定DG算法最终迭代步长hdl

在计算出DG算法迭代步长后，还需衡量近似模型对于真实模型的准确性，使用预测下降量与实际下降量的比值进行评判，并对信赖域Δ进行自适应调整。

图2 迭代步长关系示意图Fig.2 Diagram of iteration step relationship

近似模型的下降量为

定义比值

式(24)中，阻尼因子q用于衡量二次模型与目标函数的逼近程度。

根据所得到的q来进行DG算法的迭代更新，并自适应调整下次迭代的信赖域半径Δ：

1)当q≤0时：xi+1＝xi

当前迭代步长不合理，缩小信赖域半径，使得Δi+1＝Δi／2， 重新计算试探当前迭代步长。

2)当q＞0时：xi+1＝xi+hdl

若q＞0.75，二阶近似效果较好，可适当扩大信赖域半径，使得Δi+1＝2Δi。

若0.25≤q≤0.75，保持信赖域半径不变，使得Δi+1＝Δi。

若q＜0.25，二阶近似效果较差，可适当缩小信赖域半径，使得Δi+1＝Δi／2。

2.3 DG-IEKF状态更新方程与协方差更新方程的推导

通过借鉴DG算法的思路，并分析IEKF算法状态变量的迭代更新过程，推导DG-IEKF算法的状态更新方程与协方差更新方程。

结合式(12)与式(16)，可得

结合式(17)与式(25)，可得

结合式(21)与式(25)，可得

根据式(22)，可确定DG-IEKF算法的迭代步长hdl。计算阻尼因子q来衡量二次模型的近似程度，来判断步长的有效性

结合式(22)与式(28)，可得DG-IEKF算法的状态更新方程

根据式(15)，令F′(x)＝0， 求取极值点，可得

由线性一阶近似可得

将式(31)代入式(30)，可得

整理可得

由式(9)可知：Z－g(x)～N(O12×1，C)， 可得DG-IEKF算法的协方差阵更新方程

2.4 DG-IEKF算法流程

根据上文推导的DG-IEKF算法的状态更新方程与协方差更新方程，可得DG-IEKF算法流程如图3所示。其中，迭代终止门限值μ、η和最大迭代次数γ依赖于实际经验设定。

图3 DG-IEKF算法流程图Fig.3 Flowchart of DG-IEKF algorithm

3 实验与分析

为了验证DG-IEKF算法的有效性，本文采用Monte-Carlo方法开展仿真验证，共进行了100次仿真。与EKF算法、IEKF算法进行了比较，通过应用平均绝对误差(Mean Absolute Error，MAE)和均方根误差(Root Mean Square Error，RMSE)来评价算法精度。

3.1 仿真条件

设置仿真时间为300s，设置测距测角传感器精度[σρσασβ]T为[30m 2mrad 2mrad]T，传感器量测更新频率为2Hz，采样频率为2Hz；受油机初始位置为[118° 31.8649° 8000m]T，机体系下初始速度为[0m／s 50m／s 0m／s]T，初始加速度 为[0m／s20m／s20m／s2]T， 初 始 姿 态 为[0° 0° 0°]T；加油机初始位置为[118.0225°31.8640° 7795.35m]T，机体系下初始速度为[0m／s 50m／s 0m／s]T，初始加速度为[0m／s20m／s20m／s2]T，初始姿态为[0° 0° 0°]T。加油机向东匀速直线飞行，受油机在机体系下的飞行轨迹参数如表1所示。地心地固坐标系下，双机相对运动轨迹如图4所示。

表1 受油机飞行轨迹参数Table 1 Parameters of oilreceiver flight trajectory

图4 双机相对运动轨迹Fig.4 Diagram of two aircrafts'relative motion trajectory

3.2 仿真结果分析

采用Monte-Carlo方法统计这100次的仿真结果。通过对每次采样的100组数据取平均值绘制曲线，相对位置误差曲线如图5(a)所示，相对位置RMSE曲线如图5(b)所示；相对速度误差曲线如图6(a)所示，相对速度RMSE曲线如图6(b)所示。

图5 相对位置误差与相对位置RMSE对比示意图Fig.5 Comparison diagram of relative position error and relative position RMSE

图6 相对速度误差与相对速度RMSE对比示意图Fig.6 Comparison diagram of relative speed error and relative speed RMSE

由图5(a)、图6(a)可知，在相对导航信息初始对准存在较大误差时，本文的DG-IEKF算法能够更有效、更准确地逼近相对位置真实值，具有更快的收敛速度。在受油机轨迹发生较大变化的50s～100s内，EKF算法出现了数次较大的毛刺，而DG-IEKF算法则相对更加平稳，避免了陷入局部极值点的情况。图5(b)、图6(b)分别为相对位置与相对速度的RMSE对比曲线，代表DG-IEKF算法的绿色曲线在全仿真阶段的误差总体上低于EKF算法与IEKF算法，对比可知DG-IEKF算法具有较好的稳定性。综合图5、图6可以看出，在相对位置与相对速度方面，DG-IEKF算法要优于EKF与IEKF算法。

表2为三种算法对相对导航状态的误差统计，分别计算了三种算法在综合MAE与综合RMSE下x轴、y轴与z轴的相对状态精度。综合RMSE基于L2范数计算，对偏差的敏感性更高。与EKF算法相比，DG-IEKF算法在x轴、y轴、z轴上的相对位置MAE精度分别提高了21.17%、20.42%、18.42%，相对位置RMSE精度分别提高了37.18%、36.05%、33.42%；相对速度MAE精度分别提高了12.55%、11.76%、11.33%，相对速度RMSE精度分别提高了20.57%、18.84%、18.19%。

表2 MAE与RMSE误差统计对比Table 2 Comparison of MAE and RMSE error statistics

4 结论

针对相对导航数据链中断的情况，本文提出了一种在测距测角信息下基于狗腿算法优化的DGIEKF状态估计算法，通过态势感知传感器辅助双机进行相对导航，接续常规相对导航策略完成相对导航任务。该算法借鉴信赖域优化思想改进IEKF中状态迭代更新过程，以提高相对导航滤波器的精度与可靠性。仿真实验结果表明，本文的DG-IEKF算法与现广泛使用的非线性滤波EKF算法相比，在RMSE方面，相对位置综合精度提高了35.55%，相对速度综合精度提高了19.20%，能够在常规导航策略失效时稳定可靠地完成相对导航任务。