CN112577481B - 一种旋翼无人机地面目标定位方法 - Google Patents

Abstract

本发明公开了一种旋翼无人机地面目标定位方法，将经过目标检测算法输出的图像位置信息与无人机的姿态角、高度等信息结合起来，通过坐标转换关系计算出导航坐标系下目标与无人机之间的相对位置信息，然后结合无人机GPS经纬度信息，计算出目标的绝对位置信息；最终设计无人机的控制律，实现系统平台的目标定位，并将定位结果发送到地面站上。本方法定位精度较高、实时性好，且具有较好的鲁棒性和可行性。

Description

一种旋翼无人机地面目标定位方法

技术领域

本发明属于无人机技术领域，具体涉及一种无人机目标定位方法。

背景技术

由于视觉技术的发展，旋翼无人机的二次开发也迎来了新的高度。视觉传感器常采用小型摄像头，其具有质量小、体积小、功耗低等优点，适合应用于相对其它无人机负载能力较低的四旋翼无人机。因此，无人机目标检测技术已成为其发展的新亮点。目标对象检测的研究是计算机视觉(Computer Vision)领域的重要成员。目标检测是检测图像序列中目标对象的位置，即使在目标发生非刚体变化，光线变化，快速移动和运动模糊以及背景干扰的情况下仍能保持一定精度。它的广泛应用遍及军事、民用的各个领域，如侦察，精确制导和战况评估等领域。近几年无人机和计算机视觉都有很大的发展，这使得基于无人机的目标定位系统的实现成为可能；而在军事、工业、安防等领域的应用需求也使得无人机目标识别定位系统的实现变得迫切。

现有技术中的研究利用图像处理算法对目标图像特征进行处理，从而对目标进行检测和跟踪，但其研究对象大都是图像序列，很少将其应用于无人机系统平台上。而应用到无人机系统平台上的实现并没有将无人机传感器的信息与图像处理的结果充分结合起来，计算目标的绝对位置信息。此外，目前视觉采集和处理有两种方式：一种是将摄像头采集的图像信息通过无线图传传回地面处理设备进行处理，将处理之后的结果再通过无线数传发送给飞控，这种方式适用于图像复杂、计算量较大的情况，其缺点是图像和数据的传输存在延迟，并且容易受到干扰，导致信息传递出错；另一种则是选用质量轻、尺寸小的机载处理设备直接进行图像处理，避免了图像回传过程中的干扰，实时性高，但其处理性能不如电脑，不适宜做复杂的数据处理。

发明内容

为了克服现有技术的不足，本发明提供了一种旋翼无人机地面目标定位方法，将经过目标检测算法输出的图像位置信息与无人机的姿态角、高度等信息结合起来，通过坐标转换关系计算出导航坐标系下目标与无人机之间的相对位置信息，然后结合无人机GPS经纬度信息，计算出目标的绝对位置信息；最终设计无人机的控制律，实现系统平台的目标定位，并将定位结果发送到地面站上。本方法定位精度较高、实时性好，且具有较好的鲁棒性和可行性。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1：计算目标在导航坐标系下的位置；

定义目标位置为P₄＝[x_cn,y_cn,z_cn]；其中[x_cn,y_cn]为目标在导航坐标系下的位置坐标，z_cn为目标所在地面的海拔高度；

目标位置P₄由以下关系式表示：

P₄＝P₁+P₂+P₃ (1)

式中，向量P₁＝[x_an y_an z_an]^T表示无人机在导航坐标系下位置的三维坐标；

向量P₂＝[x₂ y₂ z₂]^T为无人机质心到摄像机光心的距离，且：

式中^bP₂＝[^bx₂,^by₂,^bz₂]为在机体坐标系下相机的安装位置三维坐标，

为机体坐标系到导航坐标系的转换矩阵；

向量P₃为相机到目标的距离，表示为：

式中^cP₃＝[x_c,y_c,z_c]为目标在相机坐标系下的位置三维坐标，

为相机坐标系到机体坐标系的转换矩阵；

从而有：

最终得到如下表达式：

式中M₁为相机内参矩阵，u、v为目标在像素坐标系下的位置坐标，z_d为图像深度；

令

则：

从而图像深度信息计算如下：

最终得到目标在导航坐标系下的位置坐标为：

其中

和

为

矩阵第一行和第二行的数据，C(1,:)和C(2,:)为C矩阵第一行和第二行的数据；

步骤2：计算目标的绝对位置信息；

设无人机出发点O的经纬度坐标为(B₁,L₁)，目标点C的经纬度坐标为(B₂,L₂)，其中B为纬度，L为经度；定义OC为等角航线，OE和CD为两条平行圈，OD和EC为两条子午线；A为方向角，即无人机相对于目标的方向；

在椭球面微分三角形OEC中，存在以下关系式：

cos A ds＝MdB (9)

sin A ds＝N cos B dL (10)

其中M为子午线曲率半径，M＝a(1-e²)/(1-e²sin²B)^3/2；N为卯酉圈曲率半径，N＝a/(1-e²sin²B)^1/2；a为椭球的长半轴，e为椭球的第一偏心率；s为等角航线弧长；

将式(9)和式(10)相除，得：

对式(12)积分得：

L₂-L₁＝tan A(q₂-q₁) (13)

其中：

对式(9)积分得：

式中

为两点间的子午线弧长；

定义X(B)为自赤道至大地纬度B处的子午线弧长，计算公式为：

X(B)＝a(1-e²)(αB+βsin2B+γsin4B+δsin6B+εsin8B) (16)

其中：

则由式(15)得：

X(B₂)-X(B₁)＝s cos A (18)

根据符号形式的子午线弧长反解公式，直接求出目标点C的纬度，计算如下：

其中

求出B₂后带入式(14)求出q₂，联合式(13)和式(14)，最终求得目标点C的经度如下：

L₂＝L₁+tan A(q₂-q₁) (22)

当A＝90°时，tan A为无穷大，无法采用式(13)求解目标点经度；等角航线弧长即为起始点所在纬线圈的弧长，即：

s＝N₁cos B₁(L₂-L₁) (23)

又因为N₁＝a/(1-e²sin²B₁)^1/2，则目标点C的经纬度坐标：

步骤3：设计无人机的控制律；

步骤3-1：建立旋翼无人机的六自由度运动方程：

式中，I_x、I_y、I_z分别为旋翼无人机的总体转动惯量在机体轴三轴上的分量，U_T,U_φ,U_θ,U_ψ分别为旋翼无人机在高度、滚转、俯仰和偏航方向的控制量，J_r为旋翼绕转轴的转动惯量；[φ θ ψ]为无人机最终输出姿态角；h为无人机当前高度；x和y分别为无人机当前水平位置；Ω_G是旋翼叶片转动速率，g是重力加速度，z是北东地坐标系下的高度；

步骤3-2：姿态控制量设计；

在步骤1的基础上，得到滚转、俯仰和偏航方向三个姿态角的控制量为：

其中[φ_g θ_g ψ_g]为期望姿态角，

将式(26)改写成普通PID控制律的形式为：

式中[p q r]为无人机输出角速率；

步骤3-3：高度控制量设计；

由无人机六自由度运动方程得到高度方向控制量UT为：

其中m是无人机总质量；

最终设计的高度方向控制量为：

其中h_g为无人机期望高度，h为无人机当前高度；

步骤3-4：最终得到位置控制律如下；

其中x_g和y_g分别为无人机水平期望位置，

和

分别为无人机水平期望速度，v_x和v_y分别为无人机当前水平速度，φ_d和θ_d分别为姿态控制器的期望输入量，u_x和u_y分别为水平方向上等效控制量。

本发明的有益效果如下：

1、本发明采用机载处理的方式，使数据处理更靠近数据来源，相比较数传传输，不存在传输时延，同时也不会受到外界的干扰。

2、本发明将目标检测算法与定位算法结合起来，同时根据实际试飞任务设计控制律，充分利用系统平台的各个模块的输出数据，实现对特定目标的定位，定位结果与GPS精度密切相关，GPS定位精度越高，目标定位结果越准确。

3、本发明将目标识别结果和定位误差数据通过数传传送到地面站上，实时显示目标定位结果，增强人机交互体验。

附图说明

图1为本发明定位系统软件设计框架。

图2为本发明方法目标检测制导模型。

图3为本发明方法椭球面上等角航线微分三角形。

图4为本发明实施例期望滚转角为10度时仿真曲线。

图5为本发明实施例期望俯仰角为10度时仿真曲线。

图6为本发明实施例期望偏航角为10度时仿真曲线。

图7为本发明实施例t＝0时刻期望高度为5m，t＝2s时期望位置为(3m,2m)的仿真曲线。

图8为本发明实施例自主航点模式下无人机实际轨迹与目标航线之间的对比曲线。

图9为本发明实施例实际试飞采集的视频帧目标检测结果。

图10为本发明实施例两次实验地面站上显示的目标识别结果与定位误差。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

如图1所示，一种旋翼无人机地面目标定位方法，包括如以下步骤：

步骤1：计算目标在导航坐标系下的位置；

综合考虑实际试飞任务以及机载处理器的算力，选用Tiny-YOlOV3作为旋翼无人机目标检测的算法；

由于目标检测算法最终输出的是目标物体在像素坐标系下的像素坐标，因此要想计算地面目标的具体位置，需要先建立一个目标图像的制导模型，建立模型图如图2所示；

定义目标位置为P₄＝[x_cn,y_cn,z_cn]；其中[x_cn,y_cn]为目标在导航坐标系下的位置坐标，z_cn为目标所在地面的海拔高度；

目标位置P₄由以下关系式表示：

P₄＝P₁+P₂+P₃ (1)

式中，向量P₁＝[x_an y_an z_an]^T表示无人机在导航坐标系下位置的三维坐标；

向量P₂＝[x₂ y₂ z₂]^T为无人机质心到摄像机光心的距离，且：

式中^bP₂＝[^bx₂,^by₂,^bz₂]为在机体坐标系下相机的安装位置三维坐标，

为机体坐标系到导航坐标系的转换矩阵；

向量P₃为相机到目标的距离，表示为：

式中^cP₃＝[x_c,y_c,z_c]为目标在相机坐标系下的位置三维坐标，

为相机坐标系到机体坐标系的转换矩阵；

从而有：

最终得到如下表达式：

式中M₁为相机内参矩阵，u、v为目标在像素坐标系下的位置坐标，z_d为图像深度；

令

则：

从而图像深度信息计算如下：

最终得到目标在导航坐标系下的位置坐标为：

其中

和

为

矩阵第一行和第二行的数据，C(1,:)和C(2,:)为C矩阵第一行和第二行的数据；

步骤2：计算目标的绝对位置信息；

如图3所示，OC为一等角航线，设无人机出发点O的坐标为(B₁,L₁)，目标点C的坐标为(B₂,L₂)，其中B为纬度，L为经度，A为方向角，即无人机相对于目标的方向，OE和CD为两条平行圈，OD和EC为两条子午线。

在椭球面微分三角形OEC中，存在以下关系式：

cos A ds＝MdB (9)

sin A ds＝N cos B dL (10)

其中M为子午线曲率半径，M＝a(1-e²)/(1-e²sin²B)^3/2；N为卯酉圈曲率半径，N＝a/(1-e²sin²B)^1/2；a为椭球的长半轴，e为椭球的第一偏心率；s为等角航线弧长；

将式(9)和式(10)相除，得：

对式(12)积分得：

L₂-L₁＝tanA(q₂-q₁) (13)

其中：

对式(9)积分得：

式中

为两点间的子午线弧长；

定义X(B)为自赤道至大地纬度B处的子午线弧长，计算公式为：

X(B)＝a(1-e²)(αB+βsin2B+γsin4B+δsin6B+εsin8B) (16)

其中：

则由式(15)得：

X(B₂)-X(B₁)＝s cos A (18)

根据符号形式的子午线弧长反解公式，直接求出目标点C的纬度，计算如下：

其中

求出B₂后带入式(14)求出q₂，联合式(13)和式(14)，最终求得目标点C的经度如下：

L₂＝L₁+tan A(q₂-q₁) (22)

当A＝90°时，tan A为无穷大，无法采用式(13)求解目标点经度；等角航线弧长即为起始点所在纬线圈的弧长，即：

s＝N₁cos B₁(L₂-L₁) (23)

又因为N₁＝a/(1-e²sin²B₁)^1/2，则目标点C的经纬度坐标：

因此可以根据以上计算方法通过无人机的GPS经纬度信息计算出目标点的经纬度信息，即绝对位置信息。

步骤3：设计无人机的控制律；

步骤3-1：建立旋翼无人机的六自由度运动方程：

式中，I_x、I_y、I_z分别为旋翼无人机的总体转动惯量在机体轴三轴上的分量，U_T,U_φ,U_θ,U_ψ分别为旋翼无人机在高度、滚转、俯仰和偏航方向的控制量，J_r为旋翼绕转轴的转动惯量；[φ θ ψ]为无人机最终输出姿态角；h为无人机当前高度；x和y分别为无人机当前水平位置；Ω_G是旋翼叶片转动速率，g是重力加速度，z是北东地坐标系下的高度；

步骤3-2：姿态控制量设计；

在旋翼无人机建模的基础上，通过分析滚转、俯仰和偏航通道的开环传递函数，引入零点配置使系统稳定，最终通过分析根轨迹图得到姿态角的控制量为：

其中[φ_g θ_g ψ_g]为期望姿态角，

将式(26)改写成普通PID控制律的形式为：

式中[p q r]为无人机输出角速率；

步骤3-3：高度控制量设计；

由无人机六自由度运动方程得到高度方向控制量UT为：

其中m是无人机总质量；

最终设计的高度方向控制量为：

其中h_g为无人机期望高度，h为无人机当前高度；

步骤3-4：最终得到位置控制律如下；

其中x_g和y_g分别为无人机水平期望位置，

和

分别为无人机水平期望速度，v_x和v_y分别为无人机当前水平速度，φ_d和θ_d分别为姿态控制器的期望输入量，u_x和u_y分别为水平方向上等效控制量。

上述方法的设计与实现过程进一步可概括为：首先根据系统机载处理器的性能选取合适的目标检测算法，将检测结果与无人机传感器数据结合起来，计算出目标与无人机在导航坐标系下的相对位置；然后在导航坐标系下目标位置解算的基础上，将无人机GPS的信息考虑进来，通过等角航线的正向解法计算目标的绝对位置信息，实现目标的定位；最后根据实际试飞任务，包括自稳、悬停、定高以及航点模式，设计无人机的控制律，实现系统平台的目标定位验证，并将识别结果和定位误差发送到地面站上。

具体实施例：

目前深度学习在目标检测和识别方面主要有两大“流派”，候选框和回归法。候选框流派主要使用某种算法获取主体所在的候选区域，然后再对这块区域进行分类，以Faster RCNN/SPP/R-FCN为代表；回归法则直接进行BBox回归与主体分类，以YOLO/SSD为代表。区域候选算法实质上还是采用滑动窗口遍历图像的方法，在检测过程中会产生非常多的候选窗口，因此算法的计算量非常大，并不能达到实时检测的目的。回归法相比较区域候选算法大大减少了计算量，在保证精度的前提下，在带有GTX TITAN X的服务器上，均可以达到实时检测的效果，这也为在拥有GPU的嵌入式上部署深度网络模型提供了可能。

YOLO全名You Only Look Once,历经三次演变为现在的YOLOV3。YOLOV3使用多个独立的Logistic分类器替代了YOLOV2中的softmax分类器，Anchor box聚类了9个，而不是YOLOV2版中的5个，而且每个尺度预测3个BBox，基础网络使用了Darknet-53。此外还有一个简化版本的Tiny-YOlOV3网络，与YOLOV3类似，网络中使用3×3和1×1两种大小的卷积核，卷积层包含卷积操作、批归一化处理和Leaky Relu激活函数三种基本操作。Tiny-YOLOV3主干网络主要有7个3×3的卷积层及6个最大池化层，前5个最大池化层的步长为2，最后一个步长为1。当输入大小为416×416的图像时，整体网络的输出为13×13的特征图，经过上采样之后与第5个卷积层的输出进行张量拼接，再进行卷积运算得到26×26大小的特征图，最终网络输出两个不同尺度的特征图。

相比较YOLOV3网络，Tiny-YOlOV3网络结构更加简单，需要训练的参数大大减少，特别适合于工程实现。尤其在无人机进行目标检测时，由于机载处理器算力不足，无法运行深层的卷积神经网络模型，而针对无人机的目标检测又需要兼顾准确率和实时性的要求，那么Tiny-YOlOV3网络便成了首要选择。

对采用的四旋翼无人机进行建模分析，选取无人机总质量m＝1.235kg,四旋翼无人机的总体转动惯量在机体轴三轴上的分量I_x＝0.016kg·m²、I_y＝0.016kg·m²、I_z＝0.032kg·m²,旋翼绕转轴的转动惯量J_r＝5.6953e-05kg·m²，旋翼无人机状态初值为0作为控制器设计的基础数据，基于上述仿真条件，设计无人机的控制器，仿真结果如图4-图8所示。

从图4-图8的仿真结果可以看出，旋翼无人机对姿态和位置的控制不存在超调，调节时间迅速，航线控制时与目标航线存在一定的偏差，但整体偏差不大，能够很好地跟踪目标航线。因此设计的控制器能够实现基本的试飞任务，包括自稳、定高、悬停以及航线控制，能够为无人机的实际目标定位任务的实现提供保障。

图9、图10和表1为实际试飞验证的目标定位结果，目标小车的实际位置由GPS定位获得，即经度108.7555392，纬度34.0335743。进行十次试飞实验，目标定位结果如表1所示，展示了十次试飞定位结果与目标实际位置之间的定位误差。地面站定位结果显示如图10所示，平均定位误差基本在GPS误差允许的范围内，定位效果较好。

表1目标定位结果

综合各个仿真结果以及实际试飞定位结果，证明了本发明所设计的旋翼无人机地面目标定位算法的设计与实现方法的有效性，能够在机载处理器上通过运行目标检测算法实时处理图像数据，并通过检测结果与无人机数据的融合实现无人机对给定目标的定位，同时能够通过设计控制律变换任务场景，实现特定任务下目标绝对位置的计算，具有很强的鲁棒性。

Claims (1)

1.一种旋翼无人机地面目标定位方法，其特征在于，包括以下步骤：

步骤1：计算目标在导航坐标系下的位置；

定义目标位置为P₄＝[x_cn，y_cn，z_cn]；其中[x_cn，y_cn]为目标在导航坐标系下的位置坐标，z_cn为目标所在地面的海拔高度；

目标位置P₄由以下关系式表示：

P₄＝P₁+P₂+P₃ (1)

式中，向量P₁＝[x_an y_an z_an]^T表示无人机在导航坐标系下位置的三维坐标；

向量P₂＝[x₂ y₂ z₂]^T为无人机质心到摄像机光心的距离，且：

式中^bP₂＝[^bx₂，^by₂，^bz₂]为在机体坐标系下相机的安装位置三维坐标，

为机体坐标系到导航坐标系的转换矩阵；

向量P₃为相机到目标的距离，表示为：

式中^cP₃＝[x_c，y_c，z_c]为目标在相机坐标系下的位置三维坐标，

为相机坐标系到机体坐标系的转换矩阵；

从而有：

最终得到如下表达式：

式中M₁为相机内参矩阵，u、v为目标在像素坐标系下的位置坐标，z_d为图像深度；

令

则：

从而图像深度信息计算如下：

最终得到目标在导航坐标系下的位置坐标为：

其中

和

为

矩阵第一行和第二行的数据，C(1，：)和C(2，：)为C矩阵第一行和第二行的数据；

步骤2：计算目标的绝对位置信息；

设无人机出发点O的经纬度坐标为(B₁，L₁)，目标点C的经纬度坐标为(B₂，L₂)，其中B为纬度，L为经度；定义OC为等角航线，OE和CD为两条平行圈，OD和EC为两条子午线；A为方向角，即无人机相对于目标的方向；

在椭球面微分三角形OEC中，存在以下关系式：

cos A ds＝MdB (9)

sin A ds＝N cos B dL (10)

其中M为子午线曲率半径，M＝a(1-e²)/(1-e²sin²B)^3/2；N为卯酉圈曲率半径，N＝a/(1-e²sin²B)^1/2；a为椭球的长半轴，e为椭球的第一偏心率；s为等角航线弧长；

将式(9)和式(10)相除，得：

对式(12)积分得：

L₂-L₁＝tanA(q₂-q₁) (13)

其中：

对式(9)积分得：

式中

为两点间的子午线弧长；

定义X(B)为自赤道至大地纬度B处的子午线弧长，计算公式为：

X(B)＝a(1-e²)(αB+βsin 2B+γ sin 4B+δsin 6B+ε sin 8B) (16)

其中：

则由式(15)得：

X(B₂)-X(B₁)＝s cos A (18)

根据符号形式的子午线弧长反解公式，直接求出目标点C的纬度，计算如下：

其中

求出B₂后带入式(14)求出q₂，联合式(13)和式(14)，最终求得目标点C的经度如下：

L₂＝L₁+tan A(q₂-q₁) (22)

当A＝90°时，tanA为无穷大，无法采用式(13)求解目标点经度；等角航线弧长即为起始点所在纬线圈的弧长，即：

s＝N₁cos B₁(L₂-L₁) (23)

又因为N₁＝a/(1-e²sin²B₁)^1/2，则目标点C的经纬度坐标：

步骤3：设计无人机的控制律；

步骤3-1：建立旋翼无人机的六自由度运动方程：

式中，I_x、I_y、I_z分别为旋翼无人机的总体转动惯量在机体轴三轴上的分量，U_T，U_φ，U_θ，U_ψ分别为旋翼无人机在高度、滚转、俯仰和偏航方向的控制量，J_r为旋翼绕转轴的转动惯量；[φ θ ψ]为无人机最终输出姿态角；h为无人机当前高度；x和y分别为无人机当前水平位置；Ω_G是旋翼叶片转动速率，g是重力加速度，z是北东地坐标系下的高度；

步骤3-2：姿态控制量设计；

在步骤1的基础上，得到滚转、俯仰和偏航方向三个姿态角的控制量为：

其中[φ_g θ_g ψ_g]为期望姿态角，

将式(26)改写成普通PID控制律的形式为：

式中[p q r]为无人机输出角速率；

步骤3-3：高度控制量设计；

由无人机六自由度运动方程得到高度方向控制量U_T为：

其中m是无人机总质量；

最终设计的高度方向控制量为：

其中h_g为无人机期望高度，h为无人机当前高度；

步骤3-4：最终得到位置控制律如下；

其中x_g和y_g分别为无人机水平期望位置，

和

分别为无人机水平期望速度，v_x和v_y分别为无人机当前水平速度，φ_d和θ_d分别为姿态控制器的期望输入量，u_x和u_y分别为水平方向上等效控制量。

Images