基于视觉的多无人机协同对地目标定位方法

1.本发明主要涉及到无人机技术领域，特指一种基于视觉的多无人机协同对地目标定位方法，可用于具有视觉传感器的无人机集群对地面目标进行持续定位。


背景技术：

2.近年来，无人机被广泛的运用在民用和军用领域，执行特定区域搜寻救援、目标侦查、物资运输与投递、灾害侦查等任务。无人机完成这些任务的一个重要的前提条件是对地面目标进行准确的目标定位。
3.一般来说，无人机是利用自身传感器对目标进行观测，获得目标精确的地理空间状态信息；如目标在某地理坐标系下的位置、速度等信息，这个过程被称为目标定位。
4.目前，现有技术中无人机对地目标定位的方法主要有两类：
5.第一类是有源定位方法，使用激光测距仪等主动对地面目标发射信号的设备，通过测量地面目标与无人机间的距离实现目标定位，这种方法定位精度高，但受制于激光测距仪的作用距离。
6.第二类是无源定位方法，使用视觉等被动传感器，结合无人机位置与姿态信息，利用无人机和目标间的几何关系解算目标位置，这种方法中无人机不主动发射辐射源隐蔽性更强，且视觉传感器成本更低。无源定位方法的难点在于降低无人机位置姿态信息的误差对目标定位精度的影响。


技术实现要素：

7.本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、应用范围广、定位精度高的基于视觉的多无人机协同对地目标定位方法。
8.为解决上述技术问题，本发明采用以下技术方案：
9.一种基于视觉的多无人机协同对地目标定位方法，其包括：
10.步骤s1：建立坐标系；
11.步骤s2：基于多无人机对目标协同观测几何学模型的目标位置初值计算：利用多无人机对目标的观测信息，利用坐标变换将多架无人机观测量统一到世界坐标系下，再利用对目标观测的几何学模型计算目标初始位置；
12.步骤s3：基于端到端滤波模型的协同目标位置与状态估计。
13.作为本发明的进一步改进：所述坐标系包括建立大地坐标系、世界坐标系、机载北东地坐标系、机体坐标系、吊舱坐标系、相机坐标系、图像坐标系中的一种或多种。
14.作为本发明的进一步改进：所述大地坐标系{o}采用国际地球参考系统wgs-84系统；所述世界坐标系{w}的原点是无人机的起始位置，xw轴指向地理北方向、yw轴指向地理东方向、zw指向地心方向；所述机载北东地坐标系{n}的原点位于无人机的质心，xn、yn、zn轴与世界坐标系的xw、yw、zw轴平行；所述无人机的机体坐标系({b}原点位于无人机的质心，xb轴指向无人机的机头方向，yb轴指向无人机的右翼，zb轴和xb、yb轴组成右手坐标系指向无人
机的底部；所述吊舱坐标系{g}原点位于吊舱基座的几何中心，xg轴指向无人机的前向，yg轴指向无人机的右向，zg轴和xg、yg轴组成右手坐标系指向无人机的底部；所述相机坐标系{c}原点与相机的光心重合，zc轴指向相机的光轴方向，xc轴指向右向，yc轴指向下方；所述图像坐标系{i}原点在图像的中心，xi轴指向图像的右向，yi轴指向图像的下方，目标在图像坐标系中的坐标指的是图像矩阵中的像素位置(u,v)。
15.作为本发明的进一步改进：所述步骤s2包括：
16.步骤s201：利用目标在图像坐标系{ii}下的位置计算从相机光心到目标的视轴线单位向量在相机坐标系{ci}下的表示；
17.步骤s202：结合云台俯仰、偏航欧拉角无人机滚转、俯仰、偏航欧拉角计算无人机i的对目标观测的单位视轴向量在世界坐标系{w}下的表示
18.步骤s203：结合无人机相机位置的实际测量值获得视轴线losi的表示；
19.步骤s204：利用无人机对目标协同观测几何学模型，求解目标位置初始值
20.作为本发明的进一步改进：所述步骤s201中，将相机视为针孔相机模型，计算从相机光心到目标的视轴线单位向量在相机坐标系{ci}下的表示：
[0021][0022]
其中fi和di是与相机焦距和像素大小相关的相机内参。
[0023]
作为本发明的进一步改进：所述步骤s202中，计算无人机i的对目标观测的单位视轴向量在世界坐标系{w}的表示：
[0024][0025]
其中：
[0026][0027][0028]
其中分别为云台俯仰、偏航欧拉角；为无人机滚转、俯仰、偏航欧拉角；由从吊舱基座几何中心到无人机中的旋转关系得到，可以通过外参标定获得；为单位对角阵。
[0029]
作为本发明的进一步改进：所述步骤s203中，视轴线losi表示为：
[0030][0031]
其中是无人机相机位置的实际测量值，
是实际情况下包含了观测误差和传感器噪声的视轴向量表示。
[0032]
作为本发明的进一步改进：所述步骤s3中，基于端到端滤波模型的协同目标位置与状态估计是利用交互多模型无迹卡尔曼滤波建立从“目标观测的图像位置”端到“目标在世界坐标系下的位置”端的端到端目标位置与状态估计算法，持续获得精确的目标定位结果。
[0033]
作为本发明的进一步改进：所述步骤s3包括：
[0034]
步骤s301：初始化系统初始状态初始化状态协方差p0、过程噪声协方差q、观测噪声协方差r、模型间的一步转移概率矩阵t和模型概率矩阵q；
[0035]
步骤s302：设当前时刻为k；对每一个模型，计算模型输入交互初始状态和协方差矩阵
[0036]
步骤s303：对每一个模型，分别执行sigma点计算过程；其中对于模型j，利用初始状态和协方差矩阵计算sigma点集和sigma点集的协方差权重wc和均值权重wm；
[0037]
步骤s304：对每一个模型，分别执行状态预测过程；对于模型j，对所有的sigma点利用状态模型进行一步预测过程；计算sigma点集的均值及协方差矩阵
[0038]
步骤s305：对每一个模型，分别执行量测更新过程；其中对于模型j，计算量测的sigma点集计算量测值sigma点集的均值量测更新值的协方差矩阵
[0039]
步骤s306：对每一个模型，分别执行增益计算过程；其中对于模型j，计算系统状态与量测间的交互协方差矩阵计算系统的kalman增益
[0040]
步骤s307：对每一个模型，分别执行滤波更新过程；其中对于模型j，结合k时刻的量测值计算状态估计和协方差矩阵
[0041]
步骤s308：对每一个模型，分别计算每个模型对应滤波器的似然函数；其中对于模型j，计算k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数计算模型j在k时刻的概率
[0042]
步骤s309：利用所有模型的状态估计和概率，计算k时刻的多模型交互后的联合状态估计和协方差矩阵ok；利用状态估计值即可获得目标在世界坐标系下的位置并计算目标的地理位置
[0043]
步骤s310：以一个时间间隔δt重复步骤s302-步骤s309的过程，直到完成目标定位任务。
[0044]
与现有技术相比，本发明的优点就在于：
[0045]
1、本发明的基于视觉的多无人机协同对地目标定位方法，原理简单、应用范围广、定位精度高，通过构建一个多无人机协同对地目标定位方法，能够融合多无人机对地面目标的多维观测降低目标定位误差，使无人机实现对地面目标的精确持续定位。
[0046]
2、本发明的基于视觉的多无人机协同对地目标定位方法，可以解决多无人机以协同的方式对地面目标进行精确定位的问题。通过基于多无人机协同观测几何学模型的目标位置计算，能够利用多架无人机对目标的观测，获得对目标位置的较为准确的初始估计。通过建立端到端的滤波模型的协同目标位置与状态估计，能够利用无人机对目标的持续观测，有效降低传感器误差对目标定位精度的影响，获得持续的、准确的目标位置估计。
附图说明
[0047]
图1是本发明方法的流程示意图。
[0048]
图2是本发明在具体应用实例中坐标系建立结果示意图。
[0049]
图3是本发明在具体应用实例中相机坐标系下的物像位置示意图。
[0050]
图4是本发明在具体应用实例中多无人机协同目标定位几何学模型示意图。
具体实施方式
[0051]
以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
[0052]
本发明先定义系统的状态向量x和量测向量z为：
[0053]
x＝[x,y,z,v
x
,vy,vz,a
x
,ay,az]
t
[0054]
z＝[u1,v1,
…
,ui,vi,
…
,un,vn]
t
[0055]
其中(x,y,z)定义为目标在世界坐标系三轴下的位置(v
x
,vy,vz)和(a
x
,ay,az)分别是目标在三个轴下的速度和加速度。针对n架无人机集群中的无人机i，(ui,vi)是目标在图像坐标系下的像素位置。
[0056]
对于无人机i，非线性观测模型z
k|k-1i
＝h
ki
(x
k|k-1
)为：
[0057][0058]
其中从相机光心到吊舱基座的几何中心的变换矩阵，为从吊舱基座几何中心到无人机机体坐标系原点的变换矩阵，为从无人机机体坐标系原点到机载北东地坐标系原点的变化矩阵，为从机载北东地坐标系原点到世界坐标系原点的变换矩阵。
[0059]
如图1所示，本发明的基于视觉的多无人机协同对地目标定位方法，包括：
[0060]
步骤s1：建立坐标系；
[0061]
参见图2，在具体应用实例中，所述坐标系包括建立大地坐标系、世界坐标系、机载北东地坐标系、机体坐标系、吊舱坐标系、相机坐标系、图像坐标系中的一种或多种；其中：
[0062]
大地坐标系{o}采用国际地球参考系统wgs-84系统；
[0063]
世界坐标系{w}的原点是无人机的起始位置，xw轴指向地理北方向、yw轴指向地理东方向、zw指向地心方向；
[0064]
机载北东地坐标系{n}的原点位于无人机的质心，xn、yn、zn轴与世界坐标系的xw、yw、zw轴平行；
[0065]
无人机的机体坐标系({b}原点位于无人机的质心，xb轴指向无人机的机头方向，yb轴指向无人机的右翼，zb轴和xb、yb轴组成右手坐标系指向无人机的底部。
[0066]
吊舱坐标系{g}原点位于吊舱基座的几何中心，xg轴指向无人机的前向，yg轴指向无人机的右向，zg轴和xg、yg轴组成右手坐标系指向无人机的底部；
[0067]
相机坐标系{c}原点与相机的光心重合，zc轴指向相机的光轴方向，xc轴指向右向，yc轴指向下方；
[0068]
图像坐标系{i}原点在图像的中心，xi轴指向图像的右向，yi轴指向图像的下方，目标在图像坐标系中的坐标指的是图像矩阵中的像素位置(u,v)。
[0069]
步骤s2：基于多无人机对目标协同观测几何学模型的目标位置初值计算：利用多无人机对目标的观测信息，利用坐标变换将多架无人机观测量统一到世界坐标系下，再利用对目标观测的几何学模型计算目标初始位置。
[0070]
步骤s3：基于端到端滤波模型的协同目标位置与状态估计。
[0071]
在具体应用实例中，所述步骤s2的流程包括：
[0072]
步骤s201：根据式(1)，利用目标在图像坐标系{ii}下的位置计算从相机光心到目标的视轴线单位向量在相机坐标系{ci}下的表示，如图3所示。
[0073]
步骤s202：根据式(2)，结合云台俯仰、偏航欧拉角无人机滚转、俯仰、偏航欧拉角计算无人机i的对目标观测的单位视轴向量在世界坐标系{w}下的表示
[0074]
步骤s203：根据式(5)，结合无人机相机位置的实际测量值获得视轴线losi的表示。步骤s204：根据式(8)，利用无人机对目标协同观测几何学模型，求解目标位置初始值如图4所示。
[0075]
在上述过程中，计算无人机对目标观测的视轴线，具体内容为：
[0076]
对于n架无人机集群中的无人机i，目标在图像坐标系{ii}下的位置
[0077]
将相机视为针孔相机模型，计算从相机光心到目标的视轴线单位向量在相机坐标系{ci}下的表示：
[0078][0079]
其中fi和di是与相机焦距和像素大小相关的相机内参。
[0080]
计算无人机i的对目标观测的单位视轴向量在世界坐标系{w}的表示：
[0081][0082]
其中：
[0083]
[0084][0085]
其中分别为云台俯仰、偏航欧拉角；为无人机滚转、俯仰、偏航欧拉角；由从吊舱基座几何中心到无人机中的旋转关系得到，可以通过外参标定获得；为单位对角阵。
[0086]
在上述过程中，基于多无人机协同观测几何学模型的目标位置计算，具体内容为：
[0087]
考虑到对目标观测的视轴线起始于无人机机载相机光心终止于目标位置，多架无人机的视轴射线将会交于目标一点。但是在现实条件下，观测误差和传感器噪声会导致视轴射线不完全相交。在这种情况下，视轴线losi可以表示为：
[0088][0089]
其中是无人机相机位置的实际测量值，是实际情况下包含了观测误差和传感器噪声的视轴向量表示。
[0090]
设目标在世界坐标系下的位置为则到各个视轴射线losi的距离之和d可以表示为：
[0091][0092]
其中是目标位置投影到视轴射线losi的垂足位置。
[0093][0094]
将式(7)带入式(6)，分别求距离的和d对和的偏导。当d对和的偏导全为0的时候，获得目标在世界坐标系{w}下的位置估计
[0095][0096]
根据世界坐标系原点的经纬、纬度和大地高坐标，可以获得目标在大地坐标系{o}下的位置。
[0097]
在具体应用实例中，所述步骤s3的流程包括：
[0098]
步骤s301：利用(2)步骤4的结果，初始化系统初始状态初始化状态协方差p0、过程噪声协方差q、观测噪声协方差r、模型间的一步转移概率矩阵t和模型概率矩阵q。
[0099]
步骤s302：设当前时刻为k。根据式(11)、(12)，对每一个模型，计算模型输入交互初始状态和协方差矩阵
[0100]
步骤s303：根据式(15)、(16)，对每一个模型，分别执行sigma点计算过程。其中对于模型j，利用初始状态和协方差矩阵计算sigma点集和sigma点集的协方差权重wc和均值权重wm。
[0101]
步骤s304：对每一个模型，分别执行状态预测过程。对于模型j，根据式(17)，对所有的sigma点利用状态模型进行一步预测过程。根据式(21)、(22)，计算sigma点集的均值及协方差矩阵
[0102]
步骤s305：对每一个模型，分别执行量测更新过程。其中对于模型j，根据式(23)，计算量测的sigma点集根据式(29)，计算量测值sigma点集的均值根据式(30)，量测更新值的协方差矩阵
[0103]
步骤s306：对每一个模型，分别执行增益计算过程。其中对于模型j，根据式(31)，计算系统状态与量测间的交互协方差矩阵根据式(32)，计算系统的kalman增益
[0104]
步骤s307：对每一个模型，分别执行滤波更新过程。其中对于模型j，根据式(33)、(34)，结合k时刻的量测值计算状态估计和协方差矩阵
[0105]
步骤s308：对每一个模型，分别计算每个模型对应滤波器的似然函数。其中对于模型j，根据式(35)，计算k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数根据式(36)，计算模型j在k时刻的概率
[0106]
步骤s309：利用所有模型的状态估计和概率，根据式(37)、(38)，计算k时刻的多模型交互后的联合状态估计和协方差矩阵pk。利用状态估计值即可获得目标在世界坐标系下的位置并计算目标的地理位置
[0107]
步骤s310：以一个时间间隔δt重复步骤s302-步骤s309的过程，直到完成目标定位任务。
[0108]
在上述过程中，基于端到端滤波模型的协同目标位置与状态估计：利用交互多模型无迹卡尔曼滤波建立从“目标观测的图像位置”端到“目标在世界坐标系下的位置”端的端到端目标位置与状态估计算法，持续获得精确的目标定位结果。具体内容为：
[0109]
定义系统的状态向量x和量测向量z为：
[0110]
x＝[x,y,z,v
x
,vy,vz,a
x
,ay,az]
t
ꢀꢀꢀ
(9)
[0111]
z＝[u1,v1,
…
,ui,vi,
…
,un,vn]
t
ꢀꢀꢀ
(10)
[0112]
其中(x,y,z)定义为目标在世界坐标系{w}三轴下的位置(v
x
,vy,vz)和(a
x
,ay,az)分别是目标在三个轴下的速度和加速度。针对n架无人机集群中的无人机i，(ui,vi)是目标在图像坐标系{ii}下的像素位置。
[0113]
依据多无人机对目标协同观测几何学模型，初始化系统初始状态状态协方差p0、过程噪声协方差q及观测噪声协方差r。设置多模型间的一步转移概率矩阵t和模型概率矩阵q。
[0114]
设一共有r＝3个模型，对于其中的模型j，利用其在k-1时刻的最优状态估计
和协方差矩阵计算模型输入交互初始状态和协方差矩阵
[0115][0116][0117]
其中是模型l到模型j的一步转移概率，有：
[0118][0119][0120]
对于所有r个模型，分别执行sigma点计算过程。其中对于模型j，利用初始状态和协方差矩阵计算sigma点集和sigma点集的协方差权重wc和均值权重wm：
[0121][0122][0123]
其中l是系统状态的维数，本算法中l＝9；λ＝α2(l κ)-l为缩放因子；α决定了sigma点的散步程度，应该取一个小的正值(本算法中α＝0.01)；κ默认为0；β用来描述系统状态的分布情况，在gaussian早上条件下β＝2为最优值。
[0124]
对于所有r个模型，分别执行状态预测过程。其中对于模型j，对所有的sigma点利用状态模型进行一步预测过程：
[0125][0126]
其中aj为模型j的系统状态转移矩阵。共使用三种运动模型，分别为匀速运动模型、匀加速运动模型和匀速转弯模型。这三种模型的状态转移矩阵a1＝a
cv
、a2＝a
ca
、a3＝a
ct
如下：
[0127]
[0128][0129][0130]
其中δt是时刻k-1到时刻k的时间间隔，ω是目标在水平x
w-yw面目标转弯的角速度。
[0131]
计算sigma点集的均值及协方差矩阵
[0132][0133][0134]
对于所有r个模型，分别执行量测更新过程。其中对于模型j，计算量测的sigma点集。
[0135][0136]
对于无人机i，非线性观测模型z
k|k-1i
＝h
ki
(x
k|k-1
)为：
[0137][0138]
其中：
[0139][0140][0141]
[0142][0143]
其中从相机光心到吊舱基座的几何中心的平移向量，为从吊舱基座几何中心到无人机中心的平移向量，为从无人机重心到世界坐标系原点的平移向量。
[0144]
计算量测值sigma点集的均值
[0145][0146]
量测更新值的协方差矩阵为：
[0147][0148]
对于所有r个模型，分别执行增益计算过程。其中对于模型j，系统状态与量测间的交互协方差矩阵为：
[0149][0150]
系统的kalman增益可以计算为：
[0151][0152]
对于所有r个模型，分别执行滤波更新过程。其中对于模型j，利用kalman增益可以计算得到状态估计和协方差矩阵
[0153][0154][0155]
其中是在k时刻的量测值。
[0156]
对于所有r个模型，分别计算每个模型对应滤波器的似然函数。对于模型j，k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数为：
[0157][0158]
则模型j在k时刻的概率为：
[0159][0160]
利用所有r个模型的状态估计和概率，计算k时刻的多模型交互后的联合状态估计和协方差矩阵pk：
[0161]
[0162][0163]
即为算法在时刻k的状态估计和协方差矩阵。利用状态估计值即可获得目标在世界坐标系{w}下的位置和目标状态，进而可进一步计算目标的地理位置
[0164]
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。