一种无人机光电平台对地面静止目标定位方法及系统与流程

本发明涉及目标定位领域，特别是涉及一种无人机光电平台对地面静止目标定位方法及系统。

背景技术：

目标定位的最终目的是求解出地面目标的经度、纬度和高程。传统的目标定位算法解算过程涉及到多个坐标系的转换，然后，结合无人机姿态角、光电平台姿态角，计算出目标位置坐标。由于该算法引入了无人机姿态角、光电平台姿态角测量误差，导致定位精度不高。基于激光测距值多点目标定位滤波算法具有误差来源少、定位精度高的优点，可以实现对静止目标位置精确计算。

目前，用于无人机测距固定目标定位估计较好的算法是伪线性化的线性最小二乘法。现有的算法对固定目标进行定位估计时，是从伪线性化后的线性角度出发的。具体步骤为：1、建立关于无人机位置序列、目标位置及激光测距值序列的非线性方程组；2、对测量序列进行伪线性化变形处理，得到有关无人机实时位置序列、目标位置及激光测距值序列的线性方程组；3、利用最小二乘法求解线性方程组得到目标的位置估计。

在伪线性化过程中对观测方程进行了变形和简化，利用最小二乘法得到的最优估计不再是原有观测方程的最小均方差，而是变形简化后方程组的最小均方差，这导致定位结果存在一定的偏差。也就是说现有基于伪线性化的最小二乘法的目标定位算法的问题是目标定位精度较差。

技术实现要素：

本发明的目的是提供一种无人机光电平台对地面静止目标定位方法及系统，能够大幅提高固定目标定位精度。

为实现上述目的，本发明提供了如下方案：

一种无人机光电平台对地面静止目标定位方法，包括：

获取无人机位置序列和激光测距值序列；

根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组；

根据所述非线性方程组建立伪线性化最小二乘模型，得到目标定位估计的粗解；

建立非线性高斯牛顿迭代模型；

根据所述粗解和所述非线性高斯牛顿迭代模型求解目标位置的估计值。

可选的，所述根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组，具体包括：

根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组：

其中，xei、yei、zei分别为无人机n个航迹点坐标，di为n次激光测距值，i＝1,2,…,n，xe、ye、ze分别为大地直角坐标系下的坐标。

可选的，所述根据所述非线性方程组建立伪线性化最小二乘模型，得到目标定位估计的粗解，具体包括：

将所述非线性方程组中di²和d1²相减，i＝2,…,n，得到下式：

式中，i＝2,…,n；

将上式变形得到：

将上式写成最小二乘形式：

ax＝b

其中：

确定目标点o在大地直角坐标系中的坐标最小二乘解：

xls＝(a^ta)^-1a^tb

所述坐标最小二乘解为粗解。

可选的，所述建立非线性高斯牛顿迭代模型，具体包括：

建立包含目标位置x的非线性高斯牛顿迭代模型

其中，x为大地直角坐标系中目标o的坐标估计，ε^k为残差向量，x^k为初始值，每迭代一次的结果就是x^k+1，jk为fi(x)在x^k处taylor展开的jacobian矩阵，fi(x)为观测方程。

可选的，所述根据所述粗解和所述非线性高斯牛顿迭代模型求解目标位置的估计值，具体包括：

将所述粗解作为所述非线性高斯牛顿迭代的估计初值进行迭代，求解目标位置的估计值。

一种无人机光电平台对地面静止目标定位系统，包括：

获取模块，用于获取无人机位置序列和激光测距值序列；

非线性方程组建立模块，用于根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组；

粗解确定模块，用于根据所述非线性方程组建立伪线性化最小二乘模型，得到目标定位估计的粗解；

非线性高斯牛顿迭代模型建立模块，用于建立非线性高斯牛顿迭代模型；

求解模块，用于根据所述粗解和所述非线性高斯牛顿迭代模型求解目标位置的估计值。

可选的，所述非线性方程组建立模块具体包括：

非线性方程组建立单元，用于根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组：

其中，xei、yei、zei分别为无人机n个航迹点坐标，di为n次激光测距值，i＝1,2,…,n，xe、ye、ze分别为大地直角坐标系下的坐标。

可选的，所述粗解确定模块具体包括：

做差单元，用于将所述非线性方程组中di²和d1²相减，i＝2,…,n，得到下式：

式中，i＝2,…,n；

变形单元，用于将上式变形得到：

将上式写成最小二乘形式：

ax＝b

其中：

粗解确定单元，用于确定目标点o在大地直角坐标系中的坐标最小二乘解：

xls＝(a^ta)^-1a^tb

所述坐标最小二乘解为粗解。

可选的，所述非线性高斯牛顿迭代模型建立模块具体包括：

非线性高斯牛顿迭代模型建立单元，用于建立包含目标位置x的非线性高斯牛顿迭代模型

其中，x为大地直角坐标系中目标o的坐标估计，ε^k为残差向量，x^k为初始值，每迭代一次的结果就是x^k+1，jk为fi(x)在x^k处taylor展开的jacobian矩阵，fi(x)为观测方程。

可选的，所述求解模块具体包括：

求解单元，用于将所述粗解作为所述非线性高斯牛顿迭代的估计初值进行迭代，求解目标位置的估计值。

根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供一种无人机光电平台对地面静止目标定位方法，通过建立基于激光测距值的非线性高斯牛顿迭代模型，然后利用伪线性化的线性最小二乘的粗解作为非线性牛顿迭代法的初值进行目标定位估计。依照本发明方法能够大幅提高固定目标定位精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明无人机光电平台对地面静止目标定位方法流程图；

图2为本发明无人机光电平台对地面静止目标定位系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种无人机光电平台对地面静止目标定位方法及系统，能够大幅提高固定目标定位精度。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1：

图1为本发明无人机光电平台对地面静止目标定位方法流程图。如图1所示，一种无人机光电平台对地面静止目标定位方法，包括：

步骤101：获取无人机位置序列和激光测距值序列。

步骤102：根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组，具体包括：

根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组：

其中，xei、yei、zei分别为无人机n个航迹点坐标，di为n次激光测距值，i＝1,2,…,n，xe、ye、ze分别为大地直角坐标系下的坐标

步骤103：根据所述非线性方程组建立伪线性化最小二乘模型，得到目标定位估计的粗解，具体包括：

将所述非线性方程组中di²和d1²相减，i＝2,…,n，得到下式：

式中，i＝2,…,n；

将上式变形得到：

将上式写成最小二乘形式：

ax＝b

其中：

确定目标点o在大地直角坐标系中的坐标最小二乘解：

xls＝(a^ta)^-1a^tb

所述坐标最小二乘解为粗解。

步骤104：建立非线性高斯牛顿迭代模型，具体包括：

建立包含目标位置x的非线性高斯牛顿迭代模型

其中，x为大地直角坐标系中目标o的坐标估计，ε^k为残差向量，x^k为初始值，每迭代一次的结果就是x^k+1，jk为fi(x)在x^k处taylor展开的jacobian矩阵，fi(x)为观测方程。

无人机非线性测距定位方程可以表示为：

d＝fi(x)+w

式中，d为观测向量，fi(x)为式(1)构成的观测方程，w为量测噪声。

w＝[w1,w2,…,wi]^t

为求最优估计x^*，需要使残差向量ε＝d-fi(x^*)的平方和s(x)＝ε^tε最小，即：

x^*＝argmins(x)

利用高斯牛顿法迭代求解x^*。首先对上述定位方程进行线性化处理，在初值x＝x^k处对d做一阶taylor展开，得到：

d＝fi(x^k)+fi′(x^k)(x-x^k)

＝fi′(x^k)x-(fi′(x^k)x^k-fi(x^k))

其中，令jk＝fi′(x^k)，jk为fi(x)在x^k处taylor展开的jacobian矩阵。

此时残差可表示为ε(x^k)＝fi(x^k)-d，残差向量记为ε^k。

对s(x)求导并使导数为0，可得：

若观测量n>3且j满秩时，则得到：

步骤105：根据所述粗解和所述非线性高斯牛顿迭代模型求解目标位置的估计值，具体包括：

将所述粗解作为所述非线性高斯牛顿迭代的估计初值进行迭代，求解目标位置的估计值。

实施例2：

图2为本发明无人机光电平台对地面静止目标定位系统结构图。如图2所示，一种无人机光电平台对地面静止目标定位系统，包括：

获取模块201，用于获取无人机位置序列和激光测距值序列。

非线性方程组建立模块202，用于根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组。

粗解确定模块203，用于根据所述非线性方程组建立伪线性化最小二乘模型，得到目标定位估计的粗解。

非线性高斯牛顿迭代模型建立模块204，用于建立非线性高斯牛顿迭代模型。

求解模块205，用于根据所述粗解和所述非线性高斯牛顿迭代模型求解目标位置的估计值。

所述非线性方程组建立模块202具体包括：

非线性方程组建立单元，用于根据所述无人机位置序列和所述激光测距值序列，建立有关目标位置的非线性方程组：

其中，xei、yei、zei分别为无人机n个航迹点坐标，di为n次激光测距值，i＝1,2,…,n，xe、ye、ze分别为大地直角坐标系下的坐标。

所述粗解确定模块203具体包括：

做差单元，用于将所述非线性方程组中di²和d1²相减，i＝2,…,n，得到下式：

式中，i＝2,…,n；

变形单元，用于将上式变形得到：

将上式写成最小二乘形式：

ax＝b

其中：

粗解确定单元，用于确定目标点o在大地直角坐标系中的坐标最小二乘解：

xls＝(a^ta)^-1a^tb

所述坐标最小二乘解为粗解。

所述非线性高斯牛顿迭代模型建立模块204具体包括：

非线性高斯牛顿迭代模型建立单元，用于建立包含目标位置x的非线性高斯牛顿迭代模型

其中，x为大地直角坐标系中目标o的坐标估计，ε^k为残差向量，x^k为初始值，每迭代一次的结果就是x^k+1，jk为fi(x)在x^k处taylor展开的jacobian矩阵，fi(x)为观测方程。

所述求解模块205具体包括：

求解单元，用于将所述粗解作为所述非线性高斯牛顿迭代的估计初值进行迭代，求解目标位置的估计值。

实施例3:

1、根据无人机位置序列和激光测距值序列建立有关目标位置的非线性方程组：

假设目标o在大地直角坐标系中坐标为to(xo,yo,zo)，无人机n个航迹点坐标为tei(xei,yei,zei)，n次激光测距值为di，i＝1,2,…,n。则有：

2、建立伪线性化最小二乘模型并得到目标定位估计的粗解：

为得到伪线性观测方程，利用式(1)中di²和d1²相减，i＝2,…,n，可得：

式中，i＝2,…,n。

变形可得：

式(3)是一个线性方程组，可写成最小二乘形式：

ax＝b(5)

其中：

则目标点o在大地直角坐标系中的坐标最小二乘解(即粗解)为：

xls＝(a^ta)^-1a^tb(6)

3、建立非线性高斯牛顿迭代模型

无人机非线性测距定位方程可以表示为：

d＝fi(x)+w(7)

式中，d为观测向量，fi(x)为式(1)构成的观测方程，w为量测噪声。

w＝[w1,w2,…,wi]^t

为求最优估计x^*，需要使残差向量ε＝d-fi(x^*)的平方和s(x)＝ε^tε最小，即：

x^*＝argmins(x)(8)

利用高斯牛顿法迭代求解x^*。首先对式(7)进行线性化处理，在初值x＝x^k处对d做一阶taylor展开，得到：

其中，令jk＝fi′(x^k)，jk为fi(x)在x^k处taylor展开的jacobian矩阵。

此时残差可表示为ε(x^k)＝fi(x^k)-d，残差向量记为ε^k。

对s(x)求导并使导数为0，可得：

若观测量n>3且j满秩时，则得到：

4、将步骤2的粗解作为步骤3的初值进行迭代求解目标的位置估计

迭代过程如下：

第一步：利用式(6)对前m个观测量求解得到目标位置的最小二乘粗解，作为初始估计值x0，初始迭代次数k＝0，迭代步长为1，设置迭代阈值δ作为停止迭代的条件；

第二步：计算第k次迭代的残差函数ε(x^k)，得到残差向量ε^k；

第三步：判断||ε^k||≤δ是否成立，若成立，则停止迭代；若不成立，由式(10)计算在x^k处taylor展开jacobian矩阵jk；

第四步：令k＝k+1，由式(12)更新x^k+1，返回步骤二。

本发明提供的无人机光电平台的固定目标定位算法采用了更接近实际的非线性模型，避免了伪线性化的最小二乘的最优估计不再是原有观测方程的最小均方差的问题。同时本方法利用高斯牛顿进行迭代求解，计算量小、收敛速度快并且精度高，但是高斯牛顿法对初值精度有一定要求，迭代初值不能和真实值偏差过大。所以混合算法中将伪线性化的最小二乘的估计粗解作为高斯牛顿迭代的初值，满足了高斯牛顿法对初值精度的要求，得到的目标定位估计具有明显优于传统方法的定位精度。

从非线性角度实现目标定位估计还有线性近似法和牛顿迭代法，但这两种方法存在定位精度低、结果不收敛、对初值要求高、hessian阵奇异、计算量大等问题。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。