CN102707726B - 一种无人机目标定位方法 - Google Patents

Abstract

本发明提出一种无人机目标定位方法，属于目标定位技术领域。本发明首先确定目标所在位置，由飞机的导航系统得到飞机在n个位置的坐标，再由激光测距得到飞机n个位置到目标M的距离，列写出以目标坐标为未知量的n个方程，对这n个方程组成的方程组进行线性化并应用最小二乘法求解，得到目标M的坐标。本发明提供的目标定位方法适用于任何地形条件；误差来源少，只包括飞机位置误差和激光测距误差，可以获得较高的定位精度；而且算法简单，易于实现。

Description

一种无人机目标定位方法

技术领域

本发明提出一种无人机目标定位方法，属于目标定位技术领域。

背景技术

在现代战争中，导弹武器已成为左右战场局势、决定战争进程的一个重要因素。导弹武器的大量服役，扩大了作战空间，增强了战争的破坏力，加速了战争的进程，使战争在许多情况下成了导弹等高新技术武器的较量。目前导弹射程已越来越远，在解决了“飞到”的问题后，还必须解决“看到”的问题，才能达到“打到”的目的。利用无人机目标定位技术可以有效的获取远距离目标信息，为导弹进行超视距攻击提供目标指示。

目前国内外各种型号无人机进行目标定位主要有三种方法：基于姿态测量-激光测距的目标定位、基于共线构象原理的目标定位和基于DEM的目标定位。

现有无人机目标定位方法的不足：

（1）误差来源多，影响定位精度。三种方法都包括观测平台定位误差、姿态测量误差、激光测距误差、像点提取误差等等。

（2）对地形有要求。基于共线构象原理的目标定位方法对地形平坦的地区适用，复杂地形情况下，定位精度较差；基于DEM的目标定位方法，在地形中存在许多孤立点的情况下，定位精度比较低。

（3）对无人机的航路有要求。基于共线构象原理的目标定位方法要求在实施目标定位时，飞机尽量从目标上方飞过；基于DEM的目标定位方法要求目标的入射光线和地表法线的夹角尽量地小，否则容易造成死循环，因此需要针对侦察地区进行科学的航路规划。

发明内容

本发明提出一种无人机目标定位方法，是基于飞机坐标和激光测距的目标定位方法。

由于飞机坐标通常是在地球大地坐标系中给出，即由经度、纬度和高度表示。而本发明的解算过程均是在地球直角坐标系下进行的。所以在本发明中，未明确说明的情况下，均默认已将飞机坐标由地球大地坐标系转换为地球直角坐标系。

本发明提出一种无人机目标定位方法包括如下步骤：

第一步，在地地球直角坐标系下，选定目标M(x,y,z)^T，飞机在目标M上方飞行；

第二步，飞机在飞行过程中采集自身的n个位置P₁(x₁，y₁，z₁)^T、P2(x₂，y₂，z₂)^T、……P_n(x_n，y_n，z_n)^T，n≥3，n个位置的坐标由飞机的导航系统得到；

第三步，由激光测距得到位置点P₁、P₂……P_n到目标M的距离

列写出以目标M坐标(x,y,z)^T为未知量的n个距离方程，令

则有方程组：

$\left\{\begin{array}{c}{f}_1(x,y,z)=\stackrel{}{\left|\stackrel{\‾}{{\mathrm{MP}}_1}\right|}=\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}\\ f_2(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_2}\right|=\sqrt{{(x-x_2)}^2+{(y-y_2)}^2+{(z-z_2)}^2}\\ \·\·\·\·\·\·\\ f_n(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_n}\right|=\sqrt{{(x-x_n)}^2+{(y-y_n)}^2+{(z-z_n)}^2}\end{array}\right.---\left(1\right)$

第四步，求解所述距离方程组成的方程组，得到目标M的坐标值，即目标M的具体位置；

第五步，计算空间位置精度m_p的值，若当前的目标坐标满足空间位置精度要求，则以第四步中解算的坐标值为目标的最终坐标，定位结束，否则返回第二步，继续采集飞机位置。

本发明的优点在于：

(1)本发明提出的无人机目标定位方法，适用于任何地形条件；

(2)本发明提出的无人机目标定位方法，误差来源少，只包括飞机位置误差和激光测距误差，可以获得较高的定位精度；

(3)本发明提出的无人机目标定位方法，算法简单，易于实现；

(4)本发明提出的无人机目标定位方法，无需对无人机的航路进行特别规划，保证所选取的三个以上飞机位置至少3个位置不共线。

附图说明

图1为本发明提出的无人机目标定位方法的原理图；

图2为实施例中飞机飞行轨迹在地球直角坐标系XOY面的投影。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明提供一种无人机目标定位方法，结合附图1，具体步骤如下：

第一步，选定目标M(x,y,z)^T，无人机（后称飞机）在目标M上方飞行。

第二步，飞机在飞行过程中采集自身的n个位置P₁(x₁，y₁，z₁)^T、P₂(x₂，y₂，z₂)^T、……P_n(x_n，y_n，z_n)^T，n≥3。n个位置的坐标由飞机的导航系统得到。

如图1所示，飞机航路不能为直线，n个飞机位置中至少应有三个位置不在同一直线上。飞机的最优航路为在目标上方盘旋飞行。

第三步，由激光测距得到位置点P₁、P₂……P_n到目标M的距离

则可以列写出以目标M坐标(x,y,z)^T为未知量的n个距离方程,

令 $f_i(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_i}\right|,i=\mathrm{1,2},\·\·\·n,$ 则有：

$\left\{\begin{array}{c}{f}_1(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_1}\right|=\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}\\ f_2(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_2}\right|=\sqrt{{(x-x_2)}^2+{(y-y_2)}^2+{(z-z_2)}^2}\\ \·\·\·\·\·\·\\ f_n(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_n}\right|=\sqrt{{(x-x_n)}^2+{(y-y_n)}^2+{(z-z_n)}^2}\end{array}\right.---\left(1\right)$

第四步，求解所述距离方程，得到目标M的坐标值，即目标M的具体位置。

取M₀＝(x⁰,y⁰,z⁰)^T为目标M的初始位置（所述的初始位置坐标可以根据第三步中位置点P₁、P₂……P_n坐标取平均得到），对f_i(x,y,z)（i＝1,2,…n）在点M0＝(x⁰,y⁰,z⁰)^T处用泰勒级数展开，并取一次近似表达式（即线性化表达式）：

$\left\{\begin{array}{c}{f}_1(x,y,z)=f_1(x^0,y^0,z^0)+\frac{{\∂f}_1}{\∂x}{|}_{(x^0,y^0,z^0)}(x-x^0)+\frac{{\∂f}_1}{\∂y}{|}_{(x^0,y^0,z^0)}(y-y^0)+\frac{{\∂f}_1}{\∂z}{|}_{(x^0,y^0,z^0)}(z-z^0)\\ f_2(x,y,z)=f_2(x^0,y^0,z^0)+\frac{{\∂f}^2}{\∂x}{|}_{(x^0,y^0,z^0)}(x-x^0)+\frac{{\∂f}^2}{\∂y}{|}_{(x^0,y^0,z^0)}(y-y^0)+\frac{{\∂f}^2}{\∂z}{|}_{(x^0,y^0,z^0)}(z-z^0)\\ \·\·\·\·\·\·\\ f_n(x,y,z)=f_n(x^0,y^0,z^0)+\frac{{\∂f}_n}{\∂x}{|}_{(x^0,y^0,z^0)}(x-x^0)+\frac{{\∂f}_n}{\∂y}{|}_{(x^0,y^0,z^0)}(y-y^0)+\frac{{\∂f}_n}{\∂z}{|}_{(x^0,y^0,z^0)}(z-z^0)\end{array}\right.---\left(2\right)$

其中i＝1,2,…n，

$\frac{{\∂f}_i}{\∂x}{|}_{(x^0,y^0,z^0)}=\frac{x^0-x_i}{f_i(x^0,y^0,z^0)}$

$\frac{{\∂f}_i}{\∂y}{|}_{(x^0,y^0,z^0)}=\frac{y^0-y_i}{f_i(x^0,y^0,z^0)}$

$\frac{{\∂f}_i}{\∂z}{|}_{(x^0,y^0,z^0)}=\frac{z^0-z_i}{f_i(x^0,y^0,z^0)}$

$f_i(x^0,y^0,z^0)=\left|\stackrel{\‾}{M_0{P}_i}\right|=\sqrt{{(x^0-x_i)}^2+{(y^0-y_i)}^2{(z^0-z_i)}^2}$

将方程组（2）表示为矩阵形式：Am＝b

其中，

$A=\left(\begin{array}{ccc}\frac{x^0-x_1}{f_1(x^0,y^0,z^0)}& \frac{y^0-y_1}{f_1(x^0,y^0,z^0)}& \frac{z^0-z_1}{f_1(x^0,y^0,z^0)}\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ \frac{x^0-x_n}{f_n(x^0,y^0,z^0)}& \frac{y^0-y_n}{f_n(x^0,y^0,z^0)}& \frac{z^0-z_n}{f_n(x^0,y^0,z^0)}\end{array}\right)=\left(\begin{array}{c}\frac{\stackrel{\‾}{M_0{P}_1}}{\left|\stackrel{\‾}{M_0{P}_1}\right|}\\ \·\\ \·\\ \·\\ \frac{\stackrel{\‾}{M_0{P}_n}}{\left|\stackrel{\‾}{M_0{P}_n}\right|}\end{array}\right)$

$m=\left(\begin{array}{c}x-x^0\\ y-y^0\\ z-z^0\end{array}\right)=(\stackrel{\‾}{\mathrm{OM}}-\stackrel{\‾}{{\mathrm{OM}}_0})$

$b=\left(\begin{array}{c}{f}_1(x,y,z)-f_1(x^0,y^0,z^0)\\ \·\\ \·\\ \·\\ f_n(x,y,z)-f_n(x^0,y^0,z^0)\end{array}\right)=\left(\begin{array}{c}{L}_1-\left|\stackrel{\‾}{M_0{P}_1}\right|\\ \·\\ \·\\ \·\\ L_n-\left|\stackrel{\‾}{M_0{P}_n}\right|\end{array}\right)$

其中，

和

分别为地心O到目标的当前位置M和初始位置M₀的向量。

对Am＝b应用最小二乘法，即M＝(A^TA)^-1A^Tb+M₀，对M＝(A^TA)^-1A^Tb+M₀采用迭代法解算，即第一次求解后，利用得到的目标坐标(x,y,z)^T更新目标坐标的初始值(x⁰,y⁰,z⁰)^T，再重新对方程组进行求解，反复进行这一解算过程，直到相邻两次解算的目标坐标的差的模小于设定阈值为止，即第k+1次迭代得到的目标坐标与第k次迭代得到的目标坐标的差小于设定阈值，

k为正整数，代表迭代的次数。设定阈值一般取为0.01米。

第五步，计算空间位置精度m_p的值，若当前的目标坐标满足空间位置精度要求，则以第四步中解算的坐标值为目标的最终坐标，定位结束，否则返回第二步，继续采集飞机位置，继续采集的飞机位置与前面飞机位置一起参与运算。

目标空间位置精度与空间（三维）位置精度因子PDOP的大小有关。在激光测距误差σ₀确定的情况下，使PDOP的数值尽量地减少，可以提高空间位置精度。

令 $Q={\left(A^{T}A\right)}^{-1}=\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right],$ Q为权系数矩阵（亦称协调因素阵）。

PDOP由下式定义：

$\mathrm{PDOP}={(q_{11}+q_{22}+q_{33})}^{\frac{1}{2}}$

空间位置精度m_p为：

m_p＝σ₀·PDOP

其中σ₀为激光测距误差。

应用本发明提供的无人机目标定位方法进行仿真定位，仿真条件如下：

(1)飞机在目标上方（近似正上方）盘旋飞行，盘旋半径约为7500米，飞行高度约为7000米，飞行速度大小为100米/秒；

(2)飞机盘旋一周约需500秒，每一秒记录一次飞机位置坐标，共记录500次；

(3)飞机位置误差(δx;δy;δz)为服从零均值高斯分布的随机变量,δx～N(0,8.6),δy～N(0,8.6)， $\mathrm{\δz}~N(0,\frac{40}{3})$ （单位：米）。

(4)飞机飞行轨迹在地球直角坐标系XOY面的投影如图2所示。M为目标位置在XOY的投影，C为M正上方7000米处的点在XOY面的投影。

进行10万次仿真，对应的可计算出10万个目标定位误差。对这10万个定位误差进行统计分析，可得目标位置的50%圆概率误差CEP≈2.1米。

可见，本发明提供的目标定位方法可以提供精度至少2.1米的定位结果，可以获得较高的定位精度。

Claims (3)

1.一种无人机目标定位方法，其特征在于：

第一步，在地球直角坐标系下，选定目标M(x,y,z)^T，飞机在目标M上方飞行；

第二步，飞机在飞行过程中采集自身的n个位置P₁(x₁，y₁，z₁)^T、P₂(x₂，y₂，z₂)^T、……P_n(x_n，y_n，z_n)^T，n≥3，n个位置的坐标由飞机的导航系统得到；所述的n个位置中至少应有三个位置不在同一直线上，飞机的飞行航路为在目标上方盘旋飞行；

第三步，由激光测距得到位置点P₁、P₂……P_n到目标M的距离

列写出以目标M坐标(x,y,z)^T为未知量的n个距离方程，令

i＝1，2，…n，则有方程组：

$\left\{\begin{array}{c}{f}_1(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_1}\right|=\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}\\ f_2(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_2}\right|=\sqrt{{(x-x_2)}^2+{(y-y_2)}^2+{(z-z_2)}^2}\\ \·\·\·\·\·\·\\ f_n(x,y,z)=\left|\stackrel{\‾}{{\mathrm{MP}}_n}\right|=\sqrt{{(x-x_n)}^2+{(y-y_n)}^2+{(z-z_n)}^2}\end{array}\right.---\left(1\right)$

第四步，求解所述距离方程组成的方程组，得到目标M的坐标值，即目标M的具体位置；

第五步，计算空间位置精度m_p的值，若当前的目标坐标满足空间位置精度要求，则以第四步中解算的坐标值为目标的最终坐标，定位结束，否则返回第二步，继续采集飞机位置；

第四步求解方程组的步骤具体如下：

取M₀＝(x⁰,y⁰,z⁰)^T为目标M的初始位置，对f_i(x,y,z)在点M₀＝(x⁰,y⁰,z⁰)^T处用泰勒级数展开，并取一次近似表达式：

$\left\{\begin{array}{c}{f}_1(x,y,z)=f_1(x^0,y^0,z^0)+\frac{{\∂f}_1}{\∂x}{|}_{(x^0,y^0,z^0)}(x-x^0)+\frac{\∂f_1}{\∂y}{|}_{(x^0,y^0,z^0)}(y-y^0)+\frac{\∂f_1}{\∂z}{|}_{(x^0,y^0,z^0)}(z-z^0)\\ f_2(x,y,z)=f_2(x^0,y^0,z^0)+\frac{{\∂f}_2}{\∂x}{|}_{(x^0,y^0,z^0)}(x-x^0)+\frac{{\∂f}_2}{\∂y}{|}_{(x^0,y^0,z^0)}(y-y^0)+\frac{{\∂f}_2}{\∂z}{|}_{(x^0,y^0,z^0)}(z-z^0)\\ \·\·\·\·\·\·\\ f_n(x,y,z)=f_n(x^0,y^0,z^0)+\frac{{\∂f}_n}{\∂x}{|}_{(x^0,y^0,z^0)}(x-x^0)+\frac{\∂f_n}{\∂y}{|}_{(x^0,y^0,z^0)}(y-y^0)+\frac{{\∂f}_n}{\∂z}{|}_{(x^0,y^0,z^0)}(z-z^0)\end{array}\right.---\left(2\right)$

其中i＝1，2，…n，

$\frac{{\∂f}_i}{\∂x}{|}_{(x^0,y^0,z^0)}=\frac{x^0-x_i}{f_i(x^0,y^0,z^0)}$

$\frac{{\∂f}_i}{\∂y}{|}_{(x^0,y^0,z^0)}=\frac{y^0-y_i}{f_i(x^0,y^0,z^0)}$

$\frac{{\∂f}_i}{\∂z}{|}_{(x^0,y^0,z^0)}=\frac{z^0-z_i}{f_i(x^0,y^0,z^0)}$

$f_i(x^0,y^0,z^0)=\left|\stackrel{\‾}{M_0{P}_i}\right|=\sqrt{{(x^0-x_i)}^2+{(y^0-y_i)}^2{(z^0-z_i)}^2}$

将方程组（2）表示为矩阵形式：Am＝b

其中，

$A=\left(\begin{array}{ccc}\frac{x^0-x_1}{f_1(x^0,y^0,z^0)}& \frac{y^0-y_1}{f_1(x^0,y^0,z^0)}& \frac{z^0-z_1}{f_1(x^0,y^0,z^0)}\\ \·& \·& \·\\ \·& \·& \·\\ \·& \·& \·\\ \frac{x^0-x_n}{f_n(x^0,y^0,z^0)}& \frac{y^0-y_n}{f_n(x^0,y^0,z^0)}& \frac{z^0-z_n}{f_n(x^0,y^0,z^0)}\end{array}\right)=\left(\begin{array}{c}\frac{\stackrel{\‾}{M_0{P}_1}}{\left|\stackrel{\‾}{M_0{P}_1}\right|}\\ \·\\ \·\\ \·\\ \frac{\stackrel{\‾}{M_0{P}_n}}{\left|\stackrel{\‾}{M_0{P}_n}\right|}\end{array}\right)$

$m=\left(\begin{array}{c}x-x^0\\ y-y^0\\ z-z^0\end{array}\right)=(\stackrel{\‾}{\mathrm{OM}}-\stackrel{\‾}{{\mathrm{OM}}_0})$

$b=\left(\begin{array}{c}{f}_1(x,y,z)-f_1(x^0,y^0,z^0)\\ \·\\ \·\\ \·\\ f_n(x,y,z)-f_n(x^0,y^0,z^0)\end{array}\right)=\left(\begin{array}{c}{L}_1-\left|\stackrel{\‾}{M_0{P}_1}\right|\\ \·\\ \·\\ \·\\ L_n-\left|\stackrel{\‾}{M_0{P}_n}\right|\end{array}\right)$

其中，

和

分别为地心O到目标的当前位置M和初始位置M₀的向量；

对Am＝b应用最小二乘法，即M＝(A^TA)^-1A^Tb+M₀，对M＝(A^TA)^-1A^Tb+M₀采用迭代法解算，即第一次求解后，利用得到的目标坐标(x,y,z)^T更新目标坐标的初始值(x⁰,y⁰,z⁰)^T，再重新对方程组进行求解，反复进行这一解算过程，直到相邻两次解算的目标坐标的差的模小于设定阈值为止。

2.根据权利要求1所述的一种无人机目标定位方法，其特征在于：所述的设定阈值取为0.01米。

3.根据权利要求1所述的一种无人机目标定位方法，其特征在于：所述的空间位置精度mp与空间位置精度因子PDOP的大小有关，即：

m_p=σ_o·PDOP,

其中σ₀为激光测距误差，令 $Q={\left(A^{T}A\right)}^{-1}=\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right],$ Q为权系数矩阵，PDOP由下式定义：

$\mathrm{PDOP}={(q_{11}+q_{22}+q_{33})}^{\frac{1}{2}}.$

Images