随着无人机应用在军事与民用领域的拓展, 基于图像的目标定位开始成为无人机应用的一个重要功能。基于图像的目标定位包括被动与主动2种方法。被动方法需要从图像中捕捉目标, 然后根据图象信息和其他的无人机参数进行定位^[1-2]。主动方法则利用光电设备上安装的激光测距设备测量目标至无人机的距离, 然后根据测距值以及无人机其他信息进行定位^[3-4]。由于具备激光测距辅助, 主动定位方法都够达到较高精度, 是目标定位方法的主流。

对于主动目标定位方法, 典型的应用场景是无人机根据设定的飞行航迹在目标周围飞行, 任务操作手视频上寻找目标。当这个目标出现在视野内时。操作手控制光电平台锁定并跟踪地面目标, 保持目标在视野中心位置^[5-9], 然后发射激光测量无人机与锁定目标之间的距离。目标定位软件根据无人机的姿态、光电载荷的方位角和高低角、摄像机参数以及激光测距值计算目标位置。这就是一个典型的定位过程^[6]。本文中, 我们将这个定位过程称为单点定位。多种方法被用于改进单点定位算法的精度, 如RLS滤波、偏差估计、飞行航迹选择以及风场估计^[6]等。一种传感器补偿的方法被用来减少输入角度的误差影响以改进单点定位的精度^[10]。

在单点定位的基础上, 施丽娟等又提出一种双目空间交会定位法^[7]。双目空间交会定位法是基于2个2次单点定位的结果。其主要思想是将第一次定位时飞机的位置、第二次定位时飞机的位置及目标位置视为一个三角形, 然后通过解三角形的方式来计算目标的定位, 其关健是计算激光束之间、以及激光束与地面的角度。

为了解决无人机姿态测量误差太大影响定位精度的问题, 王养柱等又提出一种多点定位的方法^[8]。这种方法与GPS定位的原理相似, 需要在至少3个空间距离较远的地方分别测量飞机至目标的距离。但定位过程仅需要无人机的位置和激光测距信息, 可以有效避免飞机姿态测量误差和光电设备姿态测量误差对精度的影响。因此这种方法具备较高的稳定性和精度。然而如后文所分析的, 这种多点定位方法, 出于线性化以简化计算增强实时性的目的, 对定位过程的观测模型进行了简化, 影响了最终估计的精度。

本文从分析多点定位的测量方程出发, 采用蒙特卡罗卡尔曼滤波的方式进行目标定位。相比传统多点定位方法, 这种方法能够有效提高目标定位精度的同时, 还能够给出对定位误差的估计, 使得飞行操作人员在控制无人机时可以根据对定位误差的要求和当时定位误差的估计判断侦查是否达到预期的目的。

1 目标定位问题描述

这里仅使用飞机位置和激光测距结果即飞机到目标的距离来估计目标位置, 以规避飞机和光电平台的姿态测量误差。

设目标位置x=(x_n, x_e, x_h)^T, 其中x_n、_xe、x_h分别表示目标的北向坐标, 东向坐标和高度。同样的无人机位置为u=(u_n, u_e, u_h)^T, 设其测量过程中具有均值为0、协方差矩阵为Σ_u的随机测量误差。由于目标静止, 有

(1)

式中，x^k、x^k-1分别表示在第k、k-1步的位置。

对于观测过程, 设z^k为第k步的激光测距值, 则观测方程为

(2)

式中，w表示激光器的随机测量误差, 假设其服从均值为0, 均方差为σ_l²的高斯分布。

2 传统多点定位算法

将方程(2) 写为标量形式, 将k移至下标, 并暂时忽略w, 由于目标位置不变, 有

(3)

将两边平方, 然后将(3) 式中第2至k式同时减去1式, 得

(4)

由于每次测量的z、u已知, 这是一个线性方程组, 可以简单的使用最小二乘法求解。

从这个过程可以看出, 传统的多点定位方法是通过对原有观测模型的简化, 即消去二次项的方式将非线性方程线性化, 从而绕开了非线性观测方程的问题, 使得问题可以用较为简单的方式求解。然而, 由于对观测模型的简化, 最小二乘法优化的目标已经不是原有观测方程的最小均方差了。所以其得到的结果, 也不是无人机目标定位观测模型下的最优解, 而仅仅是其简化模型的最优解。从这个角度讲, 传统的多点定位方法在精度上还有改进提高的空间。

3 卡尔曼滤波与蒙特卡罗方法

卡尔曼滤波的核心是在动态过程中用递推方法不断估计状态量, 观测量的均值, 协方差矩阵以及它们之间的协方差矩阵, 再利用这些协方差矩阵计算卡尔曼增益, 并以之为权更新状态量估计。对于线性和非线性系统, 采用不同的方法计算均值和协方差衍生出了不同类型的卡尔曼滤波。

对于线性系统, 所有均值和协方差矩阵可以解析地算出。卡尔曼滤波器对于线性系统也是最小方差意义下的最优滤波器。对于非线性系统, 可以对动态方程和观测方程进行泰勒展开近似, 从而可以解析地计算均值和均方差, 即扩展卡尔曼滤波器(EKF); 也可以通过指定一组关于多维高斯分布均值对称的矢量点, 将这些选中的点集输入非线性变换中, 利用输出结果点的加权和, 可以得到更好的变换后的均值和协方差估计, 即无迹卡尔曼滤波(UKF); 最后, 直接根据上一步均值和均方差生成大量随机点, 将随机点输入非线性变换, 再利用输出结果计算变换后的均值和协方差, 即蒙特卡罗卡尔曼滤波(MCKF)。

对于非线性卡尔曼滤波, 蒙特卡罗卡尔曼滤波在随机样本足够高的情况下可以达到任意精度^[11-12], 缺点在于需要的计算量相对较大, 但在本文的实例中可以看到, 在当前的主流硬件配置条件下, 设置合适的样本量, 完全可以同时满足精度和实时性的要求。

应用于卡尔曼滤波过程时, 蒙特卡罗方法既可以用于动态过程与观测过程的递推估计过程中, 也可以用于滤波器初值的选择过程, 尤其是可以给出状态估计的初始误差估计。

本问题中, 动态方程非常简单, 但观测方程非线性, 且涉及不断变化的无人机位置。同时, 无人机位置也有测量误差, 即观测方程不仅是时变的, 而且具有一定的随机性。使用扩展卡尔曼滤波和无迹卡尔曼滤波都较难处理这种情况, 较为稳妥的选择是蒙特卡罗卡尔曼滤波, 因为使用蒙特卡罗模拟时, 不仅可以生成目标位置的随机点集, 还可以生成无人机位置的随机点集, 从而给出更准确的测量误差信息。

4 蒙特卡罗卡尔曼滤波目标定位

首先是计算滤波器初值。设传统的单点定位方法可以表示成定位函数L(·), 即

(5)

式中，θ_u、ϕ_u、ψ_u分别为无人机的俯仰角, 滚转角和航向角, θ_p, ψ_p为任务设备的俯仰角和方位角, z为激光测距值。整个定位过程假设目标由任务设备跟踪锁定, 处于画面中心, 且焦距f为固定已知值。

假设定位函数L(·)的每个输入都带有零均值, 均方差已知且相互独立的随机高斯噪声。针对每个输入参数, 利用随机数生成器可以生成包含N个样本的点集。例如, 对俯仰角θ_u, 可以生成如下样本集

(6)

式中, θ_{u, j}为第j个样本, 代表均值为、均方差为σ_θu²的正态分布, 为俯仰角测量值, σ_θu²测量误差均方差。将这些点集输入L(·), 得到目标定位的初始样本x_j⁰

(7)

由此可以得到关于目标定位的初始样本集{x_j⁰|j=1, 2, …, N}以及目标定位的初始估计和协方差估计

(8)

(9)

得到初始估计后, 就可以进行迭代过程。注意, 当进入迭代过程时, 初始信息包括以及样本集{x_j⁰}。迭代过程可以分为3个步骤:生成状态预测, 生成观测值样本预测, 以及更新卡尔曼滤波估计。下面分别描述。

状态预测, 即根据动态方程进行一步预测, 输入包括上一轮迭代的估计均值, 协方差P_xx^k-1和样本集{x_j^k-1}, 对于本问题, 由于状态不变, 有

(10)

(11)

(12)

接下来为观测值样本预测, 即根据观测方程预测观测值以及计算相关协方差, 由于观测方程中涉及飞机位置的测量, 所以首先要生成关于飞机位置的样本集

(13)

式中，为本轮迭代时的飞机位置测量值, Σ_u为飞机定位误差协方差矩阵, 设其为

(14)

σ_n、σ_e、σ_h取决于无人机定位设备性能指标。

生成飞机位置样本集后, 就可以根据观测方程生成观测值样本集

(15)

(16)

(17)

(18)

注意和均为标量。

最后, 进行卡尔曼滤波状态估计更新, 包括计算卡尔曼增益、计算估计值、估计协方差以及根据估计值和协方差生成新的估计样本集

(19)

(20)

(21)

(22)

式中，z_o^k为第k步的实际激光测距值。

由此, 完成了从第k-1步到第k步的递推。以作为估计结果, 同时P_xx^k给出了对估计结果的误差估计。

5 试验验证

为了验证这种方法的性能和实时性, 我们将2次实际飞行的数据代入本方法, 并将结果与传统多点定位的结果做对比。本试验的硬件配置为CPU: Intel i5-6300HQ, 内存8G, 软件环境为Matlab。

2次飞行试验分别在不同地点进行。飞行航线均为绕已知位置目标以场高约2 000~2500 m, 半径约1 500 m圆盘飞行。任务操作人员控制任务设备将已知目标锁定在画面中心, 此时打开激光测距, 以每秒一次的频率开始记录相关数据, 包括飞机位置, 飞机姿态, 任务设备方位俯仰角度, 激光测距值, 摄像机焦距等, 持续一个激光测距周期。2次飞行的激光测距周期分别为约3分钟和约1分半钟。之后对飞行数据采用目标定位算法估计, 并与真实值比较得出定位误差。

对于本问题中的蒙特卡罗卡尔曼滤波, 样本数量N取1 000。其他误差设定如表 1所示。

另外, 由于传统空间三角形单点定位方法每个时间点都能产生一个定位结果, 最终评价性能的方法是采用CEP或RMS统计的方法。然而蒙特卡罗卡尔曼滤波采用递推的方式, 本质上是利用一段飞行的所有数据递推产生一个定位结果, 所以, 评价性能的方法是考察最终估计的精度。

5.1 飞行试验A

已知目标为图 1中画面中心浮桥一端的铁桩。无人机绕目标飞行的轨迹示意如图 2所示, 目标位于坐标原点。

图 3为蒙特卡罗卡尔曼滤波在飞行试验A中目标定位过程的示意图。

图中, 以目标真实位置为坐标原点, 并用“*”标出。圆圈及连线代表每隔2 s的目标位置估计及估计变化轨迹。灰色虚线代表每隔20秒目标估计的误差椭圆, 随时间变化由浅变深。误差椭圆根据卡尔曼滤波输出的目标位置估计及协方差P_zz^k绘制, 代表在第k秒滤波器认为目标真实位置有95%的概率位于该椭圆内。直观地看, 随着时间推进, 误差椭圆的面积逐渐变小, 代表目标位置估计的品质逐渐提升。第201 s的误差椭圆由黑色实线画出, 可以看出其接近半径3 m左右的圆形, 表明在第201 s, 对目标位置的估计已经较为精确。

图 4给出滤波器对目标位置估计与真实目标位置距离, 即实际估计误差。可以看出从实际过程看, 对目标位置点估计的误差并不总是单调递减的, 其中在50秒前后误差的增长对应于前图中估计位置由下至上运动的过程。最终的点估计据真实位置误差为1.582 m。

为了较为直观地描述卡尔曼滤波器自身估计品质, 我们着重考察其协方差矩阵P_xx^k的特征。首先, 由于不考察目标高度误差, 我们只提取出P_xx^k前两维的部分。由统计及线性代数知识可知, 协方差矩阵的特征值表示随机变量在正交的主方向上的均方差。以本文的情况, P_xx^k前两维特征值的平方根与误差椭圆的半长轴与半短轴的长度呈比例关系。当误差椭圆的概率门限为95%时, 其半长轴为P_xx^k前两维的较大特征值平方根的2倍, 半短轴为较小特征值平方根的2倍。所以, 为了直观地表现卡尔曼滤波估计品质的变化, 我们将P_xx^k前两维的较大特征值的平方根在图 5中绘制出来, 作为卡尔曼滤波误差估计。

可以看出, 误差估计的范围是单调递减的, 代表卡尔曼滤波的精度随着动态过程不断提高。注意到50秒前后点估计误差的值明显大于误差估计, 但也没有超过误差估计的两倍, 即其仍处于95%误差椭圆范围内。

这里卡尔曼滤波的误差估计与点估计误差精度变化的总趋势都是逐渐变小。但点估计在误差范围内存在跳动的可能, 不过随着迭代过程进展, 最终还是会收敛到真实目标附近。

试验A中卡尔曼滤波迭代201次, 共用时0.330 7 s, 实时性能够满足使用要求。

对比采用原有多点定位的结果, 将飞行数据代入后, 定位结果误差为18.886 m。

5.2 飞行试验B

本次试验由于飞行条件的限制, 整个定位过程只进行了约四分之一圆周, 目标为图 6中房屋右下角的位置。飞行轨迹如图 7, 目标位于坐标原点。

卡尔曼滤波的结果如图 8至图10所示。由于飞行时间短, 误差椭圆的形状仍未收敛成类圆形。由于航线设定是绕目标的圆形, 所以在如试验A中绕目标一周后, 估计误差在各个方向较为对称, 误差椭圆的形状比较接近圆形。而在本试验中, 由于只进行了四分之一圆弧的飞行, 估计结果还有进一步收敛的空间。也就是说, 如果能进行整个圆盘旋, 估计精度会进一步提高。

试验B中卡尔曼滤波迭代87次, 共用时0.065 8 s, 实时性能够满足使用要求。

点估计的精度最终为7.752 m。采用同样的飞行数据, 多点定位结果误差为28.728 m。

5.3 数据比较与讨论

表 2比较了蒙特卡罗卡尔曼滤波与传统多点定位结果的误差比较。2种方法都采用相同的数据, 具备可比性。显然, MCKF给出的结果大大优于传统多点定位方法的定位结果。

一般来讲, MCKF给出的结果会优于多点定位的结果, 原因在于多点定位方法为了将测量模型线性化, 采用将测量方程先平方再相减的方式消去了二次项, 使在进行之后的最小二乘估计时所用的代价函数发生了变化。严格地讲, 多点定位方法给出的结果已经不是原测量方程意义下的最优解了, 只是其简化线性模型意义下的最优解。相比之下, 本方法直接针对原测量方程进行优化, 从使用模型的角度已经在理论上优于传统多点定位方法, 故而能得到更精确的结果。

同时, 卡尔曼滤波也会给出优于传统的单点定位结果, 因为其初值就是用空间三角形方法产生的, 而其后的不断迭代保证了估计精度不断改善。

在实际应用中, 还存在类似试验B中飞行限制和精度要求的矛盾, 这就需要根据实际情况具体安排。反过来讲, 基于卡尔曼滤波的目标定位提供了一种根据精度需要调整飞行航路规划的可能。如果对目标位置精度要求高, 则安排较长时间的盘旋以达到要求精度; 如果对目标位置精度要求不那么高, 或因目标威胁或其他风险无法长时间在目标周边飞行, 则仅需要从目标周边“掠过”, 用较短时间给出结果。

6 结论

本文提出了一种基于蒙特卡罗卡尔曼滤波的无人机目标定位方法。本方法仅利用无人机位置和激光测距值递推得到目标位置, 从而避免了因无人机角度测量误差引起的定位误差。通过2组飞行试验结果表明, 本方法在满足实时性要求的同时, 相比传统多点定位方法将定位误差由18.886 m和28.728 m分别减小至1.582 m和7.752 m, 定位精度有较大提高。同时, 本方法还可以实时输出对目标位置的误差估计, 作为飞行指挥决策的依据。