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

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

Info

Publication number
CN115388890A
CN115388890A CN202210901076.6A CN202210901076A CN115388890A CN 115388890 A CN115388890 A CN 115388890A CN 202210901076 A CN202210901076 A CN 202210901076A CN 115388890 A CN115388890 A CN 115388890A
Authority
CN
China
Prior art keywords
target
coordinate system
model
unmanned aerial
aerial vehicle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210901076.6A
Other languages
English (en)
Inventor
牛轶峰
林博森
吴立珍
马兆伟
贾圣德
王菖
李�杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202210901076.6A priority Critical patent/CN115388890A/zh
Publication of CN115388890A publication Critical patent/CN115388890A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

本发明公开了一种基于视觉的多无人机协同对地目标定位方法，其包括：步骤S1：建立坐标系；步骤S2：基于多无人机对目标协同观测几何学模型的目标位置初值计算：利用多无人机对目标的观测信息，利用坐标变换将多架无人机观测量统一到世界坐标系下，再利用对目标观测的几何学模型计算目标初始位置；步骤S3：基于端到端滤波模型的协同目标位置与状态估计。本发明具有原理简单、应用范围广、定位精度高等优点。

Description

基于视觉的多无人机协同对地目标定位方法
技术领域
本发明主要涉及到无人机技术领域，特指一种基于视觉的多无人机协同对地目标定位方法，可用于具有视觉传感器的无人机集群对地面目标进行持续定位。
背景技术
近年来，无人机被广泛的运用在民用和军用领域，执行特定区域搜寻救援、目标侦查、物资运输与投递、灾害侦查等任务。无人机完成这些任务的一个重要的前提条件是对地面目标进行准确的目标定位。
一般来说，无人机是利用自身传感器对目标进行观测，获得目标精确的地理空间状态信息；如目标在某地理坐标系下的位置、速度等信息，这个过程被称为目标定位。
目前，现有技术中无人机对地目标定位的方法主要有两类：
第一类是有源定位方法，使用激光测距仪等主动对地面目标发射信号的设备，通过测量地面目标与无人机间的距离实现目标定位，这种方法定位精度高，但受制于激光测距仪的作用距离。
第二类是无源定位方法，使用视觉等被动传感器，结合无人机位置与姿态信息，利用无人机和目标间的几何关系解算目标位置，这种方法中无人机不主动发射辐射源隐蔽性更强，且视觉传感器成本更低。无源定位方法的难点在于降低无人机位置姿态信息的误差对目标定位精度的影响。
发明内容
本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、应用范围广、定位精度高的基于视觉的多无人机协同对地目标定位方法。
为解决上述技术问题，本发明采用以下技术方案：
一种基于视觉的多无人机协同对地目标定位方法，其包括：
步骤S1：建立坐标系；
步骤S2：基于多无人机对目标协同观测几何学模型的目标位置初值计算：利用多无人机对目标的观测信息，利用坐标变换将多架无人机观测量统一到世界坐标系下，再利用对目标观测的几何学模型计算目标初始位置；
步骤S3：基于端到端滤波模型的协同目标位置与状态估计。
作为本发明的进一步改进：所述坐标系包括建立大地坐标系、世界坐标系、机载北东地坐标系、机体坐标系、吊舱坐标系、相机坐标系、图像坐标系中的一种或多种。
作为本发明的进一步改进：所述大地坐标系{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)。
作为本发明的进一步改进：所述步骤S2包括：
步骤S201：利用目标在图像坐标系{Ii}下的位置
Figure BDA0003770952300000021
计算从相机光心到目标的视轴线单位向量
Figure BDA0003770952300000022
在相机坐标系{Ci}下的表示；
步骤S202：结合云台俯仰、偏航欧拉角
Figure BDA0003770952300000023
无人机滚转、俯仰、偏航欧拉角
Figure BDA0003770952300000024
计算无人机i的对目标观测的单位视轴向量在世界坐标系{W}下的表示
Figure BDA0003770952300000025
步骤S203：结合无人机相机位置的实际测量值
Figure BDA0003770952300000026
获得视轴线LOSi的表示；
步骤S204：利用无人机对目标协同观测几何学模型，求解目标位置初始值
Figure BDA0003770952300000027
作为本发明的进一步改进：所述步骤S201中，将相机视为针孔相机模型，计算从相机光心到目标的视轴线单位向量在相机坐标系{Ci}下的表示：
Figure BDA0003770952300000028
其中fi和di是与相机焦距和像素大小相关的相机内参。
作为本发明的进一步改进：所述步骤S202中，计算无人机i的对目标观测的单位视轴向量在世界坐标系{W}的表示：
Figure BDA0003770952300000029
其中：
Figure BDA00037709523000000210
Figure BDA00037709523000000211
其中
Figure BDA00037709523000000212
分别为云台俯仰、偏航欧拉角；
Figure BDA00037709523000000213
为无人机滚转、俯仰、偏航欧拉角；
Figure BDA00037709523000000214
由从吊舱基座几何中心到无人机中的旋转关系得到，可以通过外参标定获得；
Figure BDA00037709523000000215
为单位对角阵。
作为本发明的进一步改进：所述步骤S203中，视轴线LOSi表示为：
Figure BDA0003770952300000031
其中
Figure BDA0003770952300000032
是无人机相机位置的实际测量值，
Figure BDA0003770952300000033
是实际情况下包含了观测误差和传感器噪声的视轴向量表示。
作为本发明的进一步改进：所述步骤S3中，基于端到端滤波模型的协同目标位置与状态估计是利用交互多模型无迹卡尔曼滤波建立从“目标观测的图像位置”端到“目标在世界坐标系下的位置”端的端到端目标位置与状态估计算法，持续获得精确的目标定位结果。
作为本发明的进一步改进：所述步骤S3包括：
步骤S301：初始化系统初始状态
Figure BDA0003770952300000034
初始化状态协方差P0、过程噪声协方差Q、观测噪声协方差R、模型间的一步转移概率矩阵t和模型概率矩阵q；
步骤S302：设当前时刻为k；对每一个模型，计算模型输入交互初始状态
Figure BDA0003770952300000035
和协方差矩阵
Figure BDA0003770952300000036
步骤S303：对每一个模型，分别执行Sigma点计算过程；其中对于模型j，利用初始状态
Figure BDA0003770952300000037
和协方差矩阵
Figure BDA0003770952300000038
计算Sigma点集
Figure BDA0003770952300000039
和Sigma点集的协方差权重Wc和均值权重Wm；
步骤S304：对每一个模型，分别执行状态预测过程；对于模型j，对所有的Sigma点利用状态模型进行一步预测过程；计算Sigma点集的均值
Figure BDA00037709523000000310
及协方差矩阵
Figure BDA00037709523000000311
步骤S305：对每一个模型，分别执行量测更新过程；其中对于模型j，计算量测的Sigma点集
Figure BDA00037709523000000312
计算量测值Sigma点集的均值
Figure BDA00037709523000000313
量测更新值的协方差矩阵
Figure BDA00037709523000000314
步骤S306：对每一个模型，分别执行增益计算过程；其中对于模型j，计算系统状态与量测间的交互协方差矩阵
Figure BDA00037709523000000315
计算系统的Kalman增益
Figure BDA00037709523000000316
步骤S307：对每一个模型，分别执行滤波更新过程；其中对于模型j，结合k时刻的量测值
Figure BDA00037709523000000317
计算状态估计
Figure BDA00037709523000000318
和协方差矩阵
Figure BDA00037709523000000319
步骤S308：对每一个模型，分别计算每个模型对应滤波器的似然函数；其中对于模型j，计算k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数
Figure BDA00037709523000000320
计算模型j在k时刻的概率
Figure BDA00037709523000000321
步骤S309：利用所有模型的状态估计和概率，计算k时刻的多模型交互后的联合状态估计
Figure BDA00037709523000000322
和协方差矩阵Ok；利用状态估计值
Figure BDA00037709523000000323
即可获得目标在世界坐标系下的位置
Figure BDA00037709523000000324
并计算目标的地理位置
Figure BDA00037709523000000325
步骤S310：以一个时间间隔Δt重复步骤S302-步骤S309的过程，直到完成目标定位任务。
与现有技术相比，本发明的优点就在于：
1、本发明的基于视觉的多无人机协同对地目标定位方法，原理简单、应用范围广、定位精度高，通过构建一个多无人机协同对地目标定位方法，能够融合多无人机对地面目标的多维观测降低目标定位误差，使无人机实现对地面目标的精确持续定位。
2、本发明的基于视觉的多无人机协同对地目标定位方法，可以解决多无人机以协同的方式对地面目标进行精确定位的问题。通过基于多无人机协同观测几何学模型的目标位置计算，能够利用多架无人机对目标的观测，获得对目标位置的较为准确的初始估计。通过建立端到端的滤波模型的协同目标位置与状态估计，能够利用无人机对目标的持续观测，有效降低传感器误差对目标定位精度的影响，获得持续的、准确的目标位置估计。
附图说明
图1是本发明方法的流程示意图。
图2是本发明在具体应用实例中坐标系建立结果示意图。
图3是本发明在具体应用实例中相机坐标系下的物像位置示意图。
图4是本发明在具体应用实例中多无人机协同目标定位几何学模型示意图。
具体实施方式
以下将结合说明书附图和具体实施例对本发明做进一步详细说明。
本发明先定义系统的状态向量X和量测向量Z为：
X＝[x,y,z,vx,vy,vz,ax,ay,az]T
Z＝[u1,v1,…,ui,vi,…,un,vn]T
其中(x,y,z)定义为目标在世界坐标系三轴下的位置
Figure BDA0003770952300000041
(vx,vy,vz)和(ax,ay,az)分别是目标在三个轴下的速度和加速度。针对n架无人机集群中的无人机i，(ui,vi)是目标在图像坐标系下的像素位置。
对于无人机i，非线性观测模型Zk|k-1i＝hki(Xk|k-1)为：
Figure BDA0003770952300000042
其中
Figure BDA0003770952300000043
从相机光心到吊舱基座的几何中心的变换矩阵，
Figure BDA0003770952300000044
为从吊舱基座几何中心到无人机机体坐标系原点的变换矩阵，
Figure BDA0003770952300000045
为从无人机机体坐标系原点到机载北东地坐标系原点的变化矩阵，
Figure BDA0003770952300000046
为从机载北东地坐标系原点到世界坐标系原点的变换矩阵。
如图1所示，本发明的基于视觉的多无人机协同对地目标定位方法，包括：
步骤S1：建立坐标系；
参见图2，在具体应用实例中，所述坐标系包括建立大地坐标系、世界坐标系、机载北东地坐标系、机体坐标系、吊舱坐标系、相机坐标系、图像坐标系中的一种或多种；其中：
大地坐标系{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)。
步骤S2：基于多无人机对目标协同观测几何学模型的目标位置初值计算：利用多无人机对目标的观测信息，利用坐标变换将多架无人机观测量统一到世界坐标系下，再利用对目标观测的几何学模型计算目标初始位置。
步骤S3：基于端到端滤波模型的协同目标位置与状态估计。
在具体应用实例中，所述步骤S2的流程包括：
步骤S201：根据式(1)，利用目标在图像坐标系{Ii}下的位置
Figure BDA0003770952300000051
计算从相机光心到目标的视轴线单位向量
Figure BDA0003770952300000058
在相机坐标系{Ci}下的表示，如图3所示。
步骤S202：根据式(2)，结合云台俯仰、偏航欧拉角
Figure BDA0003770952300000052
无人机滚转、俯仰、偏航欧拉角
Figure BDA0003770952300000053
计算无人机i的对目标观测的单位视轴向量在世界坐标系{W}下的表示
Figure BDA0003770952300000054
步骤S203：根据式(5)，结合无人机相机位置的实际测量值
Figure BDA0003770952300000055
获得视轴线LOSi的表示。步骤S204：根据式(8)，利用无人机对目标协同观测几何学模型，求解目标位置初始值
Figure BDA0003770952300000056
如图4所示。
在上述过程中，计算无人机对目标观测的视轴线，具体内容为：
对于n架无人机集群中的无人机i，目标在图像坐标系{Ii}下的位置
Figure BDA0003770952300000057
将相机视为针孔相机模型，计算从相机光心到目标的视轴线单位向量在相机坐标系{Ci}下的表示：
Figure BDA0003770952300000061
其中fi和di是与相机焦距和像素大小相关的相机内参。
计算无人机i的对目标观测的单位视轴向量在世界坐标系{W}的表示：
Figure BDA0003770952300000062
其中：
Figure BDA0003770952300000063
Figure BDA0003770952300000064
其中
Figure BDA0003770952300000065
分别为云台俯仰、偏航欧拉角；
Figure BDA0003770952300000066
为无人机滚转、俯仰、偏航欧拉角；
Figure BDA0003770952300000067
由从吊舱基座几何中心到无人机中的旋转关系得到，可以通过外参标定获得；
Figure BDA0003770952300000068
为单位对角阵。
在上述过程中，基于多无人机协同观测几何学模型的目标位置计算，具体内容为：
考虑到对目标观测的视轴线起始于无人机机载相机光心终止于目标位置，多架无人机的视轴射线将会交于目标一点。但是在现实条件下，观测误差和传感器噪声会导致视轴射线不完全相交。在这种情况下，视轴线LOSi可以表示为：
Figure BDA0003770952300000069
其中
Figure BDA00037709523000000610
是无人机相机位置的实际测量值，
Figure BDA00037709523000000611
是实际情况下包含了观测误差和传感器噪声的视轴向量表示。
设目标在世界坐标系下的位置为
Figure BDA00037709523000000612
则
Figure BDA00037709523000000613
到各个视轴射线LOSi的距离之和D可以表示为：
Figure BDA00037709523000000614
其中
Figure BDA00037709523000000615
是目标位置
Figure BDA00037709523000000616
投影到视轴射线LOSi的垂足位置。
Figure BDA00037709523000000617
将式(7)带入式(6)，分别求距离的和D对
Figure BDA00037709523000000618
和
Figure BDA00037709523000000619
的偏导。当D对
Figure BDA00037709523000000620
和
Figure BDA00037709523000000621
的偏导全为0的时候，获得目标在世界坐标系{W}下的位置估计
Figure BDA00037709523000000622
Figure BDA00037709523000000623
根据世界坐标系原点的经纬、纬度和大地高坐标，可以获得目标在大地坐标系{O}下的位置。
在具体应用实例中，所述步骤S3的流程包括：
步骤S301：利用(2)步骤4的结果，初始化系统初始状态
Figure BDA0003770952300000071
初始化状态协方差P0、过程噪声协方差Q、观测噪声协方差R、模型间的一步转移概率矩阵t和模型概率矩阵q。
步骤S302：设当前时刻为k。根据式(11)、(12)，对每一个模型，计算模型输入交互初始状态
Figure BDA0003770952300000072
和协方差矩阵
Figure BDA0003770952300000073
步骤S303：根据式(15)、(16)，对每一个模型，分别执行Sigma点计算过程。其中对于模型j，利用初始状态
Figure BDA0003770952300000074
和协方差矩阵
Figure BDA0003770952300000075
计算Sigma点集
Figure BDA0003770952300000076
和Sigma点集的协方差权重Wc和均值权重Wm。
步骤S304：对每一个模型，分别执行状态预测过程。对于模型j，根据式(17)，对所有的Sigma点利用状态模型进行一步预测过程。根据式(21)、(22)，计算Sigma点集的均值
Figure BDA0003770952300000077
及协方差矩阵
Figure BDA0003770952300000078
步骤S305：对每一个模型，分别执行量测更新过程。其中对于模型j，根据式(23)，计算量测的Sigma点集
Figure BDA0003770952300000079
根据式(29)，计算量测值Sigma点集的均值
Figure BDA00037709523000000710
根据式(30)，量测更新值的协方差矩阵
Figure BDA00037709523000000711
步骤S306：对每一个模型，分别执行增益计算过程。其中对于模型j，根据式(31)，计算系统状态与量测间的交互协方差矩阵
Figure BDA00037709523000000712
根据式(32)，计算系统的Kalman增益
Figure BDA00037709523000000713
步骤S307：对每一个模型，分别执行滤波更新过程。其中对于模型j，根据式(33)、(34)，结合k时刻的量测值
Figure BDA00037709523000000714
计算状态估计
Figure BDA00037709523000000715
和协方差矩阵
Figure BDA00037709523000000716
步骤S308：对每一个模型，分别计算每个模型对应滤波器的似然函数。其中对于模型j，根据式(35)，计算k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数
Figure BDA00037709523000000717
根据式(36)，计算模型j在k时刻的概率
Figure BDA00037709523000000718
步骤S309：利用所有模型的状态估计和概率，根据式(37)、(38)，计算k时刻的多模型交互后的联合状态估计
Figure BDA00037709523000000719
和协方差矩阵Pk。利用状态估计值
Figure BDA00037709523000000720
即可获得目标在世界坐标系下的位置
Figure BDA00037709523000000721
并计算目标的地理位置
Figure BDA00037709523000000722
步骤S310：以一个时间间隔Δt重复步骤S302-步骤S309的过程，直到完成目标定位任务。
在上述过程中，基于端到端滤波模型的协同目标位置与状态估计：利用交互多模型无迹卡尔曼滤波建立从“目标观测的图像位置”端到“目标在世界坐标系下的位置”端的端到端目标位置与状态估计算法，持续获得精确的目标定位结果。具体内容为：
定义系统的状态向量X和量测向量Z为：
X＝[x,y,z,vx,vy,vz,ax,ay,az]T (9)
Z＝[u1,v1,…,ui,vi,…,un,vn]T (10)
其中(x,y,z)定义为目标在世界坐标系{W}三轴下的位置
Figure BDA0003770952300000081
(vx,vy,vz)和(ax,ay,az)分别是目标在三个轴下的速度和加速度。针对n架无人机集群中的无人机i，(ui,vi)是目标在图像坐标系{Ii}下的像素位置。
依据多无人机对目标协同观测几何学模型，初始化系统初始状态
Figure BDA0003770952300000082
状态协方差P0、过程噪声协方差Q及观测噪声协方差R。设置多模型间的一步转移概率矩阵t和模型概率矩阵q。
设一共有r＝3个模型，对于其中的模型j，利用其在k-1时刻的最优状态估计
Figure BDA0003770952300000083
和协方差矩阵
Figure BDA0003770952300000084
计算模型输入交互初始状态
Figure BDA0003770952300000085
和协方差矩阵
Figure BDA0003770952300000086
Figure BDA0003770952300000087
Figure BDA0003770952300000088
其中
Figure BDA0003770952300000089
是模型l到模型j的一步转移概率，有：
Figure BDA00037709523000000810
Figure BDA00037709523000000811
对于所有r个模型，分别执行Sigma点计算过程。其中对于模型j，利用初始状态
Figure BDA00037709523000000812
和协方差矩阵
Figure BDA00037709523000000813
计算Sigma点集
Figure BDA00037709523000000814
和Sigma点集的协方差权重Wc和均值权重Wm：
Figure BDA00037709523000000815
Figure BDA00037709523000000816
其中L是系统状态的维数，本算法中L＝9；λ＝α2(L+κ)-L为缩放因子；α决定了Sigma点的散步程度，应该取一个小的正值(本算法中α＝0.01)；κ默认为0；β用来描述系统状态的分布情况，在Gaussian早上条件下β＝2为最优值。
对于所有r个模型，分别执行状态预测过程。其中对于模型j，对所有的Sigma点利用状态模型进行一步预测过程：
Figure BDA00037709523000000817
其中Aj为模型j的系统状态转移矩阵。共使用三种运动模型，分别为匀速运动模型、匀加速运动模型和匀速转弯模型。这三种模型的状态转移矩阵A1＝ACV、A2＝ACA、A3＝ACT如下：
Figure BDA0003770952300000091
Figure BDA0003770952300000092
Figure BDA0003770952300000093
其中Δt是时刻k-1到时刻k的时间间隔，ω是目标在水平XW-YW面目标转弯的角速度。
计算Sigma点集的均值
Figure BDA0003770952300000094
及协方差矩阵
Figure BDA0003770952300000095
Figure BDA0003770952300000096
Figure BDA0003770952300000097
对于所有r个模型，分别执行量测更新过程。其中对于模型j，计算量测的Sigma点集。
Figure BDA0003770952300000098
对于无人机i，非线性观测模型Zk|k-1i＝hki(Xk|k-1)为：
Figure BDA0003770952300000099
其中：
Figure BDA0003770952300000101
Figure BDA0003770952300000102
Figure BDA0003770952300000103
Figure BDA0003770952300000104
其中
Figure BDA0003770952300000105
从相机光心到吊舱基座的几何中心的平移向量，
Figure BDA0003770952300000106
为从吊舱基座几何中心到无人机中心的平移向量，
Figure BDA0003770952300000107
为从无人机重心到世界坐标系原点的平移向量。
计算量测值Sigma点集的均值
Figure BDA0003770952300000108
Figure BDA0003770952300000109
量测更新值的协方差矩阵为：
Figure BDA00037709523000001010
对于所有r个模型，分别执行增益计算过程。其中对于模型j，系统状态与量测间的交互协方差矩阵
Figure BDA00037709523000001011
为：
Figure BDA00037709523000001012
系统的Kalman增益
Figure BDA00037709523000001013
可以计算为：
Figure BDA00037709523000001014
对于所有r个模型，分别执行滤波更新过程。其中对于模型j，利用Kalman增益
Figure BDA00037709523000001015
可以计算得到状态估计
Figure BDA00037709523000001016
和协方差矩阵
Figure BDA00037709523000001017
Figure BDA00037709523000001018
Figure BDA00037709523000001019
其中
Figure BDA00037709523000001020
是在k时刻的量测值。
对于所有r个模型，分别计算每个模型对应滤波器的似然函数。对于模型j，k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数
Figure BDA00037709523000001021
为：
Figure BDA00037709523000001022
则模型j在k时刻的概率
Figure BDA0003770952300000111
为：
Figure BDA0003770952300000112
利用所有r个模型的状态估计和概率，计算k时刻的多模型交互后的联合状态估计
Figure BDA0003770952300000113
和协方差矩阵Pk：
Figure BDA0003770952300000114
Figure BDA0003770952300000115
即为算法在时刻k的状态估计和协方差矩阵。利用状态估计值
Figure BDA0003770952300000116
即可获得目标在世界坐标系{W}下的位置
Figure BDA0003770952300000117
和目标状态，进而可进一步计算目标的地理位置
Figure BDA0003770952300000118
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (9)

1.一种基于视觉的多无人机协同对地目标定位方法，其特征在于，包括：
步骤S1：建立坐标系；
步骤S2：基于多无人机对目标协同观测几何学模型的目标位置初值计算：利用多无人机对目标的观测信息，利用坐标变换将多架无人机观测量统一到世界坐标系下，再利用对目标观测的几何学模型计算目标初始位置；
步骤S3：基于端到端滤波模型的协同目标位置与状态估计。
2.根据权利要求1所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述坐标系包括建立大地坐标系、世界坐标系、机载北东地坐标系、机体坐标系、吊舱坐标系、相机坐标系、图像坐标系中的一种或多种。
3.根据权利要求2所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述大地坐标系{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)。
4.根据权利要求1或2或3所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述步骤S2包括：
步骤S201：利用目标在图像坐标系{Ii}下的位置
Figure FDA0003770952290000011
计算从相机光心到目标的视轴线单位向量
Figure FDA0003770952290000012
在相机坐标系{Ci}下的表示；
步骤S202：结合云台俯仰、偏航欧拉角
Figure FDA0003770952290000013
无人机滚转、俯仰、偏航欧拉角
Figure FDA0003770952290000014
计算无人机i的对目标观测的单位视轴向量在世界坐标系{W}下的表示
Figure FDA0003770952290000015
步骤S203：结合无人机相机位置的实际测量值
Figure FDA0003770952290000016
获得视轴线LOSi的表示；
步骤S204：利用无人机对目标协同观测几何学模型，求解目标位置初始值
Figure FDA0003770952290000017
5.根据权利要求4所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述步骤S201中，将相机视为针孔相机模型，计算从相机光心到目标的视轴线单位向量在相机坐标系{Ci}下的表示：
Figure FDA0003770952290000021
其中fi和di是与相机焦距和像素大小相关的相机内参。
6.根据权利要求4所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述步骤S202中，计算无人机i的对目标观测的单位视轴向量在世界坐标系{W}的表示：
Figure FDA0003770952290000022
其中：
Figure FDA0003770952290000023
Figure FDA0003770952290000024
其中
Figure FDA0003770952290000025
分别为云台俯仰、偏航欧拉角；
Figure FDA0003770952290000026
为无人机滚转、俯仰、偏航欧拉角；
Figure FDA0003770952290000027
由从吊舱基座几何中心到无人机中的旋转关系得到，可以通过外参标定获得；
Figure FDA0003770952290000028
为单位对角阵。
7.根据权利要求4所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述步骤S203中，视轴线LOSi表示为：
Figure FDA0003770952290000029
其中
Figure FDA00037709522900000210
是无人机相机位置的实际测量值，
Figure FDA00037709522900000211
是实际情况下包含了观测误差和传感器噪声的视轴向量表示。
8.根据权利要求1或2或3所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述步骤S3中，基于端到端滤波模型的协同目标位置与状态估计是利用交互多模型无迹卡尔曼滤波建立从“目标观测的图像位置”端到“目标在世界坐标系下的位置”端的端到端目标位置与状态估计算法，持续获得精确的目标定位结果。
9.根据权利要求8所述的基于视觉的多无人机协同对地目标定位方法，其特征在于，所述步骤S3包括：
步骤S301：初始化系统初始状态
Figure FDA00037709522900000212
初始化状态协方差P0、过程噪声协方差Q、观测噪声协方差R、模型间的一步转移概率矩阵t和模型概率矩阵q；
步骤S302：设当前时刻为k；对每一个模型，计算模型输入交互初始状态
Figure FDA00037709522900000213
和协方差矩阵
Figure FDA00037709522900000214
步骤S303：对每一个模型，分别执行Sigma点计算过程；其中对于模型j，利用初始状态
Figure FDA00037709522900000215
和协方差矩阵
Figure FDA00037709522900000216
计算Sigma点集
Figure FDA00037709522900000217
和Sigma点集的协方差权重Wc和均值权重Wm；
步骤S304：对每一个模型，分别执行状态预测过程；对于模型j，对所有的Sigma点利用状态模型进行一步预测过程；计算Sigma点集的均值
Figure FDA0003770952290000031
及协方差矩阵
Figure FDA0003770952290000032
步骤S305：对每一个模型，分别执行量测更新过程；其中对于模型j，计算量测的Sigma点集
Figure FDA0003770952290000033
计算量测值Sigma点集的均值
Figure FDA0003770952290000034
量测更新值的协方差矩阵
Figure FDA0003770952290000035
步骤S306：对每一个模型，分别执行增益计算过程；其中对于模型j，计算系统状态与量测间的交互协方差矩阵
Figure FDA0003770952290000036
计算系统的Kalman增益
Figure FDA0003770952290000037
步骤S307：对每一个模型，分别执行滤波更新过程；其中对于模型j，结合k时刻的量测值
Figure FDA0003770952290000038
计算状态估计
Figure FDA0003770952290000039
和协方差矩阵
Figure FDA00037709522900000310
步骤S308：对每一个模型，分别计算每个模型对应滤波器的似然函数；其中对于模型j，计算k时刻下模型代表的目标运动模型与目标真实运动模型间的似然函数
Figure FDA00037709522900000311
计算模型j在k时刻的概率
Figure FDA00037709522900000312
步骤S309：利用所有模型的状态估计和概率，计算k时刻的多模型交互后的联合状态估计
Figure FDA00037709522900000313
和协方差矩阵Pk；利用状态估计值
Figure FDA00037709522900000314
即可获得目标在世界坐标系下的位置
Figure FDA00037709522900000315
并计算目标的地理位置
Figure FDA00037709522900000316
步骤S310：以一个时间间隔Δt重复步骤S302-步骤S309的过程，直到完成目标定位任务。
CN202210901076.6A 2022-07-28 2022-07-28 基于视觉的多无人机协同对地目标定位方法 Pending CN115388890A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210901076.6A CN115388890A (zh) 2022-07-28 2022-07-28 基于视觉的多无人机协同对地目标定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210901076.6A CN115388890A (zh) 2022-07-28 2022-07-28 基于视觉的多无人机协同对地目标定位方法

Publications (1)

Publication Number Publication Date
CN115388890A true CN115388890A (zh) 2022-11-25

Family

ID=84116581

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210901076.6A Pending CN115388890A (zh) 2022-07-28 2022-07-28 基于视觉的多无人机协同对地目标定位方法

Country Status (1)

Country Link
CN (1) CN115388890A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116192571A (zh) * 2023-02-06 2023-05-30 中国人民解放军火箭军工程大学 一种波束抖动效应下无人机isac信道估计方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116192571A (zh) * 2023-02-06 2023-05-30 中国人民解放军火箭军工程大学 一种波束抖动效应下无人机isac信道估计方法
CN116192571B (zh) * 2023-02-06 2024-03-08 中国人民解放军火箭军工程大学 一种波束抖动效应下无人机isac信道估计方法

Similar Documents

Publication Publication Date Title
CN107727079B (zh) 一种微小型无人机全捷联下视相机的目标定位方法
CN109709537B (zh) 一种基于卫星编队的非合作目标位置速度跟踪方法
CN110487267B (zh) 一种基于vio＆uwb松组合的无人机导航系统及方法
CN111366148B (zh) 适用于机载光电观瞄系统多次观察的目标定位方法
CN112197761B (zh) 一种高精度多旋翼机协同定位方法及系统
CN109269512B (zh) 行星着陆图像与测距融合的相对导航方法
CN105698762A (zh) 一种单机航迹上基于不同时刻观测点的目标快速定位方法
CN111102981B (zh) 一种基于ukf的高精度卫星相对导航方法
Xu et al. Target location of unmanned aerial vehicles based on the electro-optical stabilization and tracking platform
CN111273312B (zh) 一种智能车辆定位与回环检测方法
CN105043392A (zh) 一种飞行器位姿确定方法及装置
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN107656286A (zh) 大倾斜远端观测环境下目标定位方法及系统
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
Lee et al. Vision-based terrain referenced navigation for unmanned aerial vehicles using homography relationship
CN111238469A (zh) 一种基于惯性/数据链的无人机编队相对导航方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CN115388890A (zh) 基于视觉的多无人机协同对地目标定位方法
CN113408623B (zh) 非合作目标柔性附着多节点融合估计方法
CN113022898B (zh) 弱引力环境柔性附着系统状态估计方法
Mostafa et al. Optical flow based approach for vision aided inertial navigation using regression trees
CN113340272A (zh) 一种基于无人机微群的地面目标实时定位方法
CN113405560A (zh) 车辆定位和路径规划统一建模方法
CN116184430B (zh) 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法
CN115079229A (zh) 一种基于单目相机的无人机对地目标定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination