一种单机航迹上基于不同时刻观测点的目标快速定位方法

摘要

本发明提供一种单机航迹上基于不同时刻观测点的目标快速定位方法，由地面控制站通过无线数传链路与机载自驾仪进行通信，控制无人机飞行。无人机在飞行的过程中，将机载视频通过图传链路实时传输到图像地面站。图像地面站根据目标点在图像中的位置进行计算，结合飞机的位置、姿态及云台转动角度信息，通过目标快速定位算法进行目标位置估算。本发明弥补了目标点在无人机坐标系中的位置无法精确获取的不足，大大提高了目标定位的适用范围。同时，无人机多点协同定位能有效提高目标探测的准确性，改良单点、单传感器工作的缺陷，提高系统工作的可靠性、容错性。

说明书

技术领域

本发明属于无人机技术领域，具体涉及一种单机航迹上基于不同时刻观测点的目 标快速定位方法。

背景技术

无人机在海上应急搜索、靶场残骸搜索等方面的应用日益广泛，如对MH370、导弹 残骸搜索等。为了进一步提高无人机执行搜索与定位任务的效果，需要研究无人机的目标 快速定位问题。关于无人机的目标快速定位问题，现有技术存在的缺陷主要表现在以下方 面：

一方面，无人侦察机现在一般采用的定位方法为单站测角测距定位方法，实际使 用过程中暴露出了几个难以解决的问题，具体表现为：(1)目标定位过程局限明显。无人机 现有系统结构在目标跟踪和定位阶段对飞机有两个必要约束条件，即跟踪阶段的盘旋和定 位时的过顶，但从安全性上看，在现代高技术战场上几乎不可能。(2)对多目标连续定位能 力不足。无人侦察机系统在特殊情况下需要对多个目标进行定位校射，一般也是采用以连 续完成多个单目标定位校射任务的简单累加，耗时一般比较长，效率较低，风险大。(3)定位 精度不高。虽然国内针对军用无人机电视图像定位精度不高的问题做了大量的研究，但是， 一直没有找到行之有效的技术方案。现有的共线定位方法，定位精度远不能满足多种任务 的要求。

另一方面，无人机空间两点交会定位的方法，在一定程度上提高了无人机电视图 像的定位精度，但是在飞行实验中，多次定位误差还是很大，依然不能满足实际运用需求。

还有就是，空间三点定位的思想，通过GPS定位获得三次或多次无人机的空中位 置，通过激光战场关键点目标进行三次或多次测距，求解距离方程，获得关键点目标的坐 标。从本质上来说，这种技术方案是一种针对战场单目标的定位算法。但计算方法繁琐，测 距误差较大，需要预先已知目标的近似高程，定位精度不高。

发明内容

针对现有技术存在的缺陷，本发明提供一种单机航迹上基于不同时刻观测点的目 标快速定位方法，方法的原理示意图如图1所示：单个无人机沿着航迹飞行，结合不同时刻 飞机在不同位置观测到的目标信息，进行多点目标快速定位。

本发明的工作流程图如图2所示：由地面控制站通过无线数传链路与机载自驾仪 进行通信，控制无人机飞行。无人机在飞行的过程中，将机载视频通过图传链路实时传输到 图像地面站。图像地面站根据目标点在图像中的位置进行计算，结合飞机的位置、姿态及云 台转动角度信息，通过目标快速定位算法进行目标位置估算。

一种单机航迹上基于不同时刻观测点的目标快速定位方法，包括以下步骤：

S1.根据相机视场中每像素所对应的物理尺寸及相机焦距，计算出三个观测位置 与目标点连线的空间观测射线在像空间坐标系下的方向向量；

S2.依据云台的俯仰角、方向角以及飞机的姿态角信息，将三条空间观测射线的方 向向量统一到同一北东地坐标系中，进而可得出用直线的标准式表达的三条空间观测射 线；

S3.利用平行于地面的一高度平面去截三条空间观测射线，得到高度平面与三条 空间观测射线间的三个交点，根据三个交点的坐标可计算三个交点所围成的三角形面积， 并将三角形面积作为目标函数；

S4.利用求解无约束优化问题的算法，对目标函数最优值进行搜索，得到相对应目 标点的估计高度；

S5.当观测点的数目多于三个，空间观测射线多于三条时，对多条空间观测射线利 用排列组合的原理获得种空间观测射线的组合方式，n大于3，每种组合方式中的三条空 间观测射线进行步骤S1至S4的操作，得到相应的目标点的估计高度，对所有组合方式中得 到的目标点的估计高度求和后取平均值，得到平均估计高度；所有组合方式得到的多个目 标点的估计高度中，与平均估计高度最接近的那一组组合方式得到的目标点估计高度，即 为理想的估计高度；

S6.利用平行于地面的且高度为S5中得到的理想的估计高度的高度平面、理想的 估计高度对应的那组三条空间观测射线以及高度平面与三条空间观测射线间的三个交点， 利用中心坐标公式，求出目标点的坐标，并将坐标转换到大地坐标系中。

本发明的优点包括：弥补了目标点在无人机坐标系中的位置无法精确获取的不 足，大大提高了目标定位的适用范围。同时，无人机多点协同定位能有效提高目标探测的准 确性，改良单点、单传感器工作的缺陷，提高系统工作的可靠性、容错性。另外，基于某准则 的最速下降法简单可靠，适用性强，结果较为准确。

附图说明

图1是本发明的应用场景示意图

图2是本发明的流程图

图3是本发明原理框图

具体实施方式

本发明提出的一种单机航迹上基于不同时刻观测点的目标快速定位方法，涉及多 个坐标系之间转换。因此，首先需要明确以下几个坐标系的定义：

a、地心惯性坐标系：GPS采集的数据是基于地心惯性坐标系定义的，其原点位于地 球质心，z轴为地球自转轴，向上为正，x轴指向格林尼治起始子午面与地球赤道的交点E，y 轴与x轴、z轴组成右手直角坐标系。通常表示为o_E-x_Iy_Iz_I。

b、像空间坐标系：其定义如下：原点位于图像中心点，x轴平行于像平面主纵线指 向摄影方向，z轴正方向垂直于像平面向下，y轴平行于主横线且与其余三轴组成右手系，用 o_p-x_py_pz_p表示。

c、机体坐标系：其原点位于机体的质心，x轴正方向指向机头方向，y轴垂直于机身 对称面向右，z轴指向下与其余两轴构成右手系。通常表示为o_b-x_by_bz_b。

d、云台坐标系：其原点定义在云台转动的中心处，x、y、z各轴均与机体坐标系平 行,用o'_b-x_b'y_b'z_b'表示。

e、北东地坐标系：也是将不同位置的观测信息最终统一在一起的坐标系，其原点 定义在基准点处，在该方案实施中，一般将某观测点定为基准点，x轴正方向指向水平正北 方向，z轴沿当地参考椭球面法线，向下为正，y轴与其余两轴构成右手直角坐标系，通常表 示为o₁-x₁y₁z₁。

具体地，一种单机航迹上基于不同时刻观测点的目标快速定位方法，包括以下步 骤：

(1)由目标点在视频图像中的像素坐标(u,v)，通过如下坐标变换公式得到以mm为 单位的像空间坐标系下的坐标(x,y)：

$\left(\begin{array}{c}x=(u-u_0)*\Delta x;\\ y=(v-v_0)*\Delta y;\end{array}\right)$

式中，Δx为x轴方向上每个像素所对应的物理尺寸，Δy为y轴方向上每个像素所 对应的物理尺寸，u₀、v₀为图像中心处的像素坐标；

将像空间坐标系下的空间观测射线的方向向量表示为(x,y,f)，其中f为相机焦 距。

(2)根据云台的俯仰角p和方位角q，在像空间坐标系中，绕z_p轴转角度q，再绕y'_b轴 转角度p，从而得到由像坐标系至云台坐标系的转换矩阵

进而根据飞机的三个姿态角—俯仰角θ、滚转角φ、偏航角ψ，按照滚转、俯仰、偏航 的顺序求出云台坐标系至北东地坐标系的旋转矩阵

$M_{b^{\prime }}^n=\left(\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& \cos \theta \end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& cos\phi & sin\phi \\ 0& -\sin \theta & cos\phi \end{array}\right)$

综上，将方向向量旋转至原点在当前时刻飞机质心的北东地坐标系中的方向 向量的计算过程如下：

根据上述方向向量，同时对应的观测点的坐标可知为(x_i0,y_i0,z_i0),i＝1,2,3，则 空间三条观测射线用直线的标准表达式可表示为：

$\frac{x-x_{i0}}{p_i}=\frac{y-y_{i0}}{q_i}=\frac{z-z_{i0}}{r_i}$

(3)利用平行于地面的高度平面h去截三条空间观测射线，高度平面与三条空间观 测射线的交点坐标为由三个交点的坐标值，可计算 三点围成的三角形面积s：$s=\frac{1}{2}\left|x_1{y}_2-x_2{y}_1+x_3{y}_1-x_1{y}_3+x_2{y}_3-x_3{y}_2\right|;$设定目标函数为s²，当 目标函数取到最小值时，对应的高度h为目标点估计高度。

(4)最快下降法是求解无约束优化问题经典方法之一，利用最快下降法求s²的最 小值，关键是用负梯度方向作为搜索方向进行运算求解。

根据上述三角形面积计算公式，若直接由s²的表达式对h求导进而计算梯度，表达 式较复杂，对于之后的搜索也很不利。因此利用导数的定义式$\underset{\Delta h\to 0}{\lim }=\underset{\Delta h\to 0}{\lim }\frac{f(h_0+\Delta h)-f\left(h_0\right)}{\Delta h}$计算梯度，确定搜索方向。

搜索方向确定后，利用某准则对最优值进行搜索，可选取Armijo准则、Wolfe准则 或精确先搜索方法，各准则之间具体求解过程不同，但方法基本一致，在本发明中选取 Armijo搜索方法为例进行介绍，设定此时：

h'＝h-f'*bⁱ

代入到三角形面积计算公式计算得到s'，式子中i是搜索步长，σ、b为搜索方法的 参数。当s'≤s+σ*bⁱ*[-(f')²]时结束搜索，输出此时的高度h为目标点的估计高度。

(5)在定位过程中，因受云台转动角度、飞机飞行不稳定等因素的影响，会在某些 测量位置出现测量野值的情况。因此，仅利用三个测量点定位不能保证定位的精度，需要利 用多点定位，对目标点进行筛选，得到更为准确的目标。

当观测点的数目多于三个，空间观测射线多于三条时，对多条空间观测射线利用 排列组合的原理获得种空间观测射线的组合方式，n大于3，所每种组合方式中的三条空 间观测射线进行步骤S1至S4的操作，得到相应的目标点的估计高度，对所有组合方式中得 到的目标点的估计高度求和后取平均值，得到平均估计高度；所有组合方式得到的多个目 标点的估计高度中，与平均估计高度最接近的那一组组合方式得到的目标点估计高度，即 为理想的目标点估计高度。

比如有四个观测点，空间观测射线为4条,4条空间观测射线进行排列组合有种 组合方式，对这四种组合方式分别进行步骤S1至S4的快速定位运算，解出4个目标点的估计 高度，对这四个目标点的估计高度取平均值，得到将四个目标点的估计高度分别与平均 值做差，与平均值最接近的即可认为是理想的目标点估计高度。

(6)根据上述理想的目标点的估计高度，利用理想的目标点估计高度对应的高度 平面与理想的目标点估计高度对应的三条空间观测射线间的三个交点的坐标值，根据中心 坐标公式求出目标点的坐标：

$\left(\begin{array}{c}x=\frac{x_1+x_2+x_3}{3}\\ y=\frac{y_1+y_2+y_3}{3}\\ z=\frac{z_1+z_2+z_3}{3}\end{array}\right)$

(7)结合飞机的GPS坐标即可求得目标点在大地坐标系下的坐标，具体如下：

$\left(\begin{array}{c}Lat+aim=Lat\_\mathrm{int}+\frac{x}{R_N+H}\\ Lon\_aim=Lon\_\mathrm{int}+\frac{y}{(R_E+H)*\cos (Lat\_\mathrm{int})}\\ H=H\_\mathrm{int}-z\end{array}\right)$

其中Lat_int、Lon_int、H_int分别代表原点坐标在大地坐标系下的纬度、经度和 高度，通常在建立空间射线观测方程时选取某观测点为坐标原点，R_N、R_E分别为子午圈、卯酉 圈曲率半径。