CN114296477A - 一种面向空地协同作战的无人机动平台自主降落方法 - Google Patents

Abstract

本发明公开了一种面向空地协同作战的无人机动平台自主降落方法，属于多智能体目标识别、自主降落和运动控制领域，针对传统无人机自主降落时仅仅依靠视觉信息引导无人机降落而易受到环境光线的影响的缺点，引入了空地协同自主降落系统。本发明通过目标检测与坐标变化，获得无人机与动平台相对位置信息，在设计无人机自主降落策略时，不仅考虑了相对位置信息，而且通过无人车与无人机的协同，将无人车的实时速度反馈给无人机，最后在真实实验场景中动平台不同速度运动情况下进行了实验验证，无人机都能精准的降落到无人车上，表现出很好的稳定性和快速性。

Description

一种面向空地协同作战的无人机动平台自主降落方法

技术领域

本发明属于多智能体目标识别、自主降落和运动控制领域，尤其涉及一种面向空地协同作战的无人机动平台自主降落方法。

背景技术

由于现代战争战场形势多变、环境复杂，战争形态与作战样式也呈现多样化发展。空地协同作战作为未来战争重要的作战形式，将对无人机与无人车的自主化、智能化提出更高的要求。多旋翼无人机大多存在工作时间较短，负载较轻等问题，通过无人机与无人车的合理配合，可以弥补单一类型对象的不足，有效提升协同作业效能。一方面，当无人机燃料不足时，可以通过空地协同着陆，实现对无人机的自动补给功能。另一方面，当无人机出现意外情况而不能执行任务时，通过空地协同着陆，使无人机降落到无人车平台上进行检查和维修，从而快速再次作战，在提高任务执行效率方面具有重要意义。空地协同着陆控制是空地协同在作战的基础功能之一。现阶段针对无人机自主降落问题，研究人员大多着重于目标识别算法的研究、控制策略的研究以及无人机本身的研究。以视觉信息为主引导无人机自主着陆容易受到环境以及光线的影响，不能适应所有天气环境。而通过无人机与无人车之间的协同，可以更高效、更精准地实现无人机自主降落。此外，针对无人机自主降落问题的研究，大多局限与降落平台静止不动的情况。而在实际战场环境中，由于战斗需要，无人车一般具有一定的速度，这时研究无人机如何降落到移动平台上具有重要研究价值。

发明内容

本发明提供了一种面向空地协同作战的无人机动平台自主降落方法，解决空地协同作战中无人机移动平台自主降落问题，实现了无人机移动平台快速、稳定自主降落。

为达到以上目的，本发明采用以下技术方案：

一种面向空地协同作战的无人机动平台自主降落方法，包括以下步骤：

1)基于空地协同作战的无人机动平台自主降落问题构建系统；

2)推导目标坐标系与相机坐标、像素坐标系、物理成像坐标系之间的关系，并通过识别Aruco码解算其位置信息；

3)在获得无人机与动平台相对位置信息的条件下，额外引入动平台速度信息，设计无人机自主降落策略，引导无人机平稳降落。

以上所述步骤中，步骤1)中所述系统包含空地协同自主降落系统、基于mavros的无人机内部通信方式、无人机配备的硬件；

步骤2)中坐标转换包括以下步骤：

(a)世界坐标系到相机坐标系的转换

空间点P(X,Y,Z)在世界坐标系下的坐标为P_W(X_W,Y_W,Z_W)，相应的其在相机坐标系下的坐标为P_C(X_C,Y_C,Z_C)，两者之间的转换关系为：

其中，T为相机外参，由相机安装位置和方向决定，主要包括两部分R和t，R为正交旋转矩阵，由相机坐标系与世界坐标系的旋转欧拉角决定：

t为平移矩阵，由相机坐标系的坐标原点在世界坐标系的x、y、z方向的平移量t_x、t_y、t_z决定：

t＝[t_x t_y t_z]^T (3)

(b)相机坐标系到物理成像坐标系的转换

设P_W在物理成像坐标系下的坐标为P′(X′,Y′,Z′)，由相机的小孔成像模型可知：

Z′＝1 (6)

即：

其中f为焦距，这样得到了空间点P(X,Y,Z)在物理成像平面上的坐标P′(X′,Y′,Z′)；

(c)物理成像坐标系到像素坐标的转换

设物理成像平面上的点P′对应于像素坐标系下的坐标为P_ix(u,v)，则：

其中α、β为图像单位长度上的像素点的个数，C_x为u方向上的像素点的偏移量，C_y为v方向上的像素点的偏移量；

进一步得到：

用矩阵的形式表示为：

其中，f_x为相机x方向的焦距，f_y为相机y方向上的焦距；

(d)世界坐标系到像素坐标系的转换

世界坐标系到像素坐标系的转换关系为：

其中，K为相机内参：

即：

上式中从左到右参数依次为像素坐标点P_ix，相机内参矩阵K，外参矩阵T(变换矩阵)，世界坐标系下的点的坐标P_W，相机的外参矩阵T由相机安装位置和方向决定，我们所需要做的就是估计出相机内参K中的四个参数f_x、f_y、C_x和C_y；

另外，相机成像的理想位置和实际位置存在误差，分为径向失真和切向失真，径向畸变是由于光线在远离透镜中心的地方偏折更大，矫正公式为：

x_corrected＝x(1+k₁r²+k₂r⁴+k₃r⁶) (14)

y_corrected＝y(1+k₁r²+k₂r⁴+k₃r⁶) (15)

切向畸变是由于透镜不完全平行于图像平面而产生的误差。矫正公式为：

x_corrected＝x+[2p₁xy+p₂(r²+2x²)] (16)

y_corrected＝y+[2p₂xy+p₁(r²+2y²)] (17)

其中k₁、k₂、k₃、p₁、p₂为相机的畸变参数。相机内参K和畸变参数可以使用ROS的标定工具对USB摄像头进行标定；

步骤2)中通过识别Aruco码解算位置信息，具体包括以下步骤：

(ⅰ)创建Aruco码：通过drawMarker()函数生成用来；

(ⅱ)检测Aruco码：给定一个Aruco码的图像，检测程序可以返回检测到的Aruco码的列表，每个检测到的Aruco码列表包括：图像四个角的位置和marker的Id；

Aruco码的检测的主要过程包括阈值化处理、轮廓滤波(舍弃无用信息，筛选出候选Aruco码)、比特拉提取(分析每个候选的比特位，来确定它们是不是marker)和获取Aruco码的Id，通过上述的运行过程，检测到图像中所有的markar，并且得到了每个markar的id、角点像素坐标以及Rvec和Tvec；最后需要计算markar在相机坐标系下的位姿。

步骤3)具体包括以下步骤：无人机通过检测、识别无人车上的Aruco码并锁定无人车，然后通过坐标变化解算出无人车与无人机的相对位置或距离信息；另外无人机与无人车进行通信，无人车发布自己全局坐标系下的速度信息并由无人机接收，降落控制策略将融合这两种信息，向无人机发出相应的速度控制指令，最后由无人机的执行单元去执行，最终实现无人机的自主降落功能；

通过步骤2)Aruco码的检测和位置估计，可以得到相机坐标系下Aruco码的位置信息，假设在时刻t无人机的位置为p_无人机＝(x_(t),y_(t),z_(t))，在时刻t无人车的位置为：p_无人车＝(x′_(t),y′_(t),z′_(t))，那么无人机与无人车的相对位置为：

Δp_车-机＝(Δx_(t),Δy_(t),Δz_(t))＝(x′_(t)-x_(t),y′_(t)-y_(t),z′_(t)-z_(t)) (25)

默认情况下，认为无人机和无人车的世界坐标系为东北天(ENU)坐标系，无人车在t时刻的世界坐标系下的速度为：

V_无人车(t)＝(u_(t),v_(t)) (26)

为了让无人机更加精准迅速的降落到移动无人车上，仅仅依靠目标识别算法得到的相对距离信息是不够的，这时无人机若能够获得无人车的速度信息，并根据无人车的速度大小调整自身追踪降落的过程，那么就可以提高自主降落的表现和性能，此时，设计的无人机降落策略设计为：

V_无人机(t)＝(V_x(t),V_y(t),V_z(t)) (27)

V_x(t)＝K₁*Δx_(t)+kx*u_(t) (28)

V_y(t)＝K₂*Δy_(t)+ky*v_(t) (29)

其中，V_x、V_y、V_z分别为无人机世界坐标系下x轴、y轴、z轴方向的速度，K₁、K₂、K₃分别为无人机x、y、z方向的速度控制系数，kx和ky分别为x方向和y方向无人机与无人车的协同速度系数，h₁为最低降落距离，表示无人机与无人车之间高度方向的最小距离值；h₂为最低高度，表示无人机在世界坐标系下的最低飞行高度，在无人机降落过程中，若无人机与无人车高度方向的距离低于h₁或无人机的飞行高度低于h₂，则认为无人机已经到达最低降落高度，进入land模式，完成自主降落过程。

另外，设计空地协同自主降落精度指标E为：

其中，(x_c,y_c)为Aruco码的中心位置坐标，(x′_(T),y′_(T))为无人机降落点坐标。

有益效果：本发明提供了一种面向空地协同作战的无人机动平台自主降落方法，针对传统无人机自主降落时仅仅依靠视觉信息引导无人机降落而易受到环境光线的影响的缺点，引入了空地协同自主降落系统，在设计无人机自主降落策略时，不仅考虑了相对位置信息，而且通过无人车与无人机的协同，将无人车的实时速度反馈给无人机，最后在真实实验场景中动平台不同速度运动情况下进行了实验验证，无人机都能精准的降落到无人车上，表现出很好的稳定性和快速性。本发明解决空地协同作战中无人机移动平台自主降落问题，实现了无人机移动平台快速、稳定自主降落。

附图说明

图1为空地协同自主降落系统示意图；

图2为基于mavros的无人机内部通信示意图；

图3为无人机硬件示意图；

图4为坐标系转化关系示意图；

图5为Aruco码的检测过程图；

图6为世界坐标系与相机坐标系相对关系示意图；

图7为空地协同自主降落原理图；

图8为控制策略流程图；

图9为无人机与单目相机和动平台与Aruco安装位置图；

图10为单目相机旗格盘标定板示意图；

图11为单目相机图像识别信息示意图；

图12为二维码检测结果图；

图13为空地协同试验场地示意图；

图14为无人机动平台降落过程图；

图15为无人机动平台降落数据曲线示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明：

1、空地协同自主降落系统构建

如图1所示，空地协同问题模型由一架无人机、一辆无人车、两个遥控器、大功率路由器、分别装有地面站和NOMACHINA通信软件的两台笔记本电脑构成。

在室内环境下，无人机的WiFi模块、无人车的WiFi模块以及笔记本连接同一路由器。其中，无人机可装载T265双目相机、D435i深度相机、激光雷达等外部传感器进行环境感知与定位。无人机上安装的WiFi数剧传输模块，通过串口和飞控连接，通过WiFi无线信号连接路由器，安装有地面站的电脑通过网线或者WiFi无线信号连接路由器，从而组成通信网络。地面站通过IP地址方式连接无人机上的WiFi数传模块，可以实时监控无人机的状态，并且对无人机进行参数修改、飞行模式控制、位置指令发布等功能。无人车具有与无人机相同的连接方式，无人机与无人车之间采用ROS系统下的多机协同的通信方式进行信息传输。无人机与无人车之间可以互相传输图像信息，实现获取环境信息的交互。并且在无人车顶部位置布置了Aruco码，便于无人机的识别和降落。无人机与无人车的系统环境方面，采用的是ubuntu18.04的系统以及ROS melodic版本.

通过mavros实现无人机内外部模块的通信如图2所示。mavros包为使用MAVLink通信协议的设备提供了通信驱动，此外它还给地面站如QGroundControl提供了UDP MAVLink桥接。在下图中，通过MAVLink通讯协议，实现PX4 on Pixhawk和上位机(如英伟达tx2、Nano)之间以及上位机和地面站之间的通信。

无人机的配备的硬件如图3所示，包括激光雷达、单目相机、T265双目相机、GPS、PIXHAWK4飞控板、英伟达TX2板载计算机、数传、高KV电机、4S电池等。其中，激光雷达、双目相机用来室内环境的建图和定位；单目相机用来进行目标的识别与检测；GPS模块用来室外环境的定位；PIXHAWAK4飞控板用来解算无人机的速度信息、位置信息、加速度信息等，将其转化为电机的转速，并由电调、电机等执行机构去执行；数传模块用来无人机与地面站之间的数据传输，包括无人机的状态信息和地面站的控制指令等。

2、基于单目相机的无人机目标检测

2.1坐标转换

(a)世界坐标系到相机坐标系的转换

如图4所示，世界坐标系以图形标志的中心点为原点o₀，平行于标志平面的横向和纵向分别为x₀轴和y₀轴，z₀轴垂直于o₀x₀y₀平面向下。相机坐标系以相机的光点为原点，平行于图像平面的x轴和y轴分别为X_C轴和Y_C轴，以摄像机的光轴为Z_C轴。

假设世界坐标系下空间一点的坐标为P_W(X_W,Y_W,Z_W)，相应的其在相机坐标系下的坐标为P_C(X_C,Y_C,Z_C)，它们之间的转换关系为：

其中，T为相机外参，由相机安装位置和方向决定，主要包括两部分R和t。R为正交旋转矩阵，由相机坐标系与世界坐标系的旋转欧拉角决定：

t为平移矩阵，由相机坐标系的坐标原点在世界坐标系的x、y、z方向的平移量t_x、t_y、t_z决定：

t＝[t_x t_y t_z]^T (3)

(b)相机坐标系到物理成像坐标系的转换

物理成像坐标系O′X′Y′Z′以光轴与图像的交点为原点O′，以平行于数字图像的行向方向与列向方向分为X′轴和Y′轴；设P_W在物理成像坐标系下的坐标为P′(X′,Y′,Z′)，由相机的小孔成像模型可知：

Z′＝1 (6)

即：

其中f为焦距。这样得到了空间点P(X,Y,Z)在物理成像平面上的坐标P′(X′,Y′,Z′)。

(c)物理成像坐标系到像素坐标的转换

图像像素坐标系O_iuv以像素图像的左上角顶点为原点，以数字图像的正左方和正下方为u轴和v轴。由于物理成像坐标系的原点在区域中心处，而像素坐标系的原点在左上角，且坐标单位不同。所以需要进行坐标转化，并将物理成像坐标系的长度单位转化为像素坐标系下的像素单位。

设物理成像平面上的点P′对应于像素坐标系下的坐标为P_ix(u,v)。则：

其中α、β为图像单位长度上的像素点的个数，C_x为u方向上的像素点的偏移量，C_y为v方向上的像素点的偏移量。

进一步得到：

用矩阵的形式表示为：

其中，f_x相机x方向的焦距，f_y为相机y方向上的焦距。令K为相机内参：

(d)世界坐标系到像素坐标系的转换

世界坐标系到像素坐标系的转换关系为：

即：

上式中从左到右参数依次为像素坐标点P_ix，相机内参矩阵K，外参矩阵T(变换矩阵)，世界坐标系下的点的坐标P_W。相机的外参矩阵T由相机安装位置和方向决定。我们所需要做的就是估计出相机内参K中的四个参数f_x、f_y、C_x和C_y。

另外，相机成像的理想位置和实际位置存在误差，分为径向失真和切向失真。径向畸变是由于光线在远离透镜中心的地方偏折更大。矫正公式为：

x_corrected＝x(1+k₁r²+k₂r⁴+k₃r⁶) (14)

y_corrected＝y(1+k₁r²+k₂r⁴+k₃r⁶) (15)

切向畸变是由于透镜不完全平行于图像平面而产生的误差。矫正公式为：

x_corrected＝x+[2p₁xy+p₂(r²+2x²)] (16)

y_corrected＝y+[2p₂xy+p₁(r²+2y²)] (17)

其中k₁、k₂、k₃、p₁、p₂为相机的畸变参数。相机内参K和畸变参数可以使用ROS的标定工具对USB摄像头进行标定。

2.2 Aruco码的识别与位置解算

为了确保无人机识别降落平台，采用了基于Aruco码的目标识别方法。Aruco模块主要基于ArUco library，是一个二维码识别中被广泛使用的库。一个Aruco码是一个二进制平方标记，外围都有一组黑色边框，同时内部有着确定该标记ID的二维矩阵组合而成。黑色的边框能加速标记在图像中的检测速度，内部的二维编码能唯一识别该标记，同时进行错误检测和错误修复。

Opencv中Aruco识别的过程为：

1、创建Aruco码。通过drawMarker()函数生成用来。

2、检测Aruco码。给定一个Aruco码的图像，检测程序可以返回检测到的Aruco码的列表。每个检测到的Aruco码列表包括：图像四个角的位置和marker的Id。

Aruco码的检测主要流程为如图5所示。检测的主要过程包括阈值化处理、轮廓滤波(舍弃无用信息，筛选出候选Aruco码)、比特拉提取(分析每个候选的比特位，来确定它们是不是marker)和获取Aruco码的Id。通过上图的运行过程，检测到了图像中所有的markar，并且得到了每个markar的id、角点像素坐标以及Rvec和Tvec；最后需要计算markar在相机坐标系下的位姿。

以二维码的坐标系为世界坐标系，如图6所示。则二维码四个角点的三维空间坐标分别为A＝(-s/2,s/2,0)^T、B＝(s/2,s/2,0)^T、C＝(s/2,-s/2,0)^T、D＝(-s/2,-s/2,0)^T。假设它们在图像中对应投影点的像素坐标分别为a＝(u_a,v_a)^T、b＝(u_b,v_b)^T、c＝(u_c,v_c)^T、d＝(u_d,v_d)^T。相机的内参矩阵K已知，由公式(12)可知，三维空间坐标与像素坐标之间的转换关系为(以A-a为例)：

为了简化表示，令

则(1000)式可表示为：

最后一行把z消去得到两个约束：

于是有：

旋转矩阵R∈SO(3)R\in\text{SO}(3)R∈SO(3)是一个单位正交阵，自身具有六个约束：

这样，我们便可以用二维码的三个角点加上旋转矩阵自身的六个约束求得R、t。

最后，利用求得的外参T和公式(1)，便可将二维码的坐标信息转化为相机坐标系下的坐标。

3、自主降落控制策略设计

传统无人机自主降落策略一般利用视觉信息，通过目标识别解算降落平台的位置信息，从而引导无人机自主降落。但是，当降落平台速度和位置发生变化时，由于目标识别与无人机飞行动作的滞后，无人机降落的准确性和成功率往往不能保证。所以本发明针对传统做法的缺陷，基于空地协同降落系统，设计了更加稳定可靠的自主降落策略。

为了充分发挥空地协同系统的特点，无人机在自主降落过程中将与移动无人车互相通信和协同配合实现无人机的精准降落。自主降落的思想如图7所示，无人机通过检测、识别无人车上的Aruco码并锁定无人车，然后通过坐标变化等解算出无人车与无人机的相对位置或距离信息；另外无人机将与无人车进行通信，无人车发布自己全局坐标系下的速度信息并由无人机接收。降落控制策略将融合这两种信息，向无人机发出相应的速度控制指令。最后由无人机的执行单元去执行，最终实现无人机的自主降落功能。

由步骤2可以知道，通过Aruco码的检测和位置估计，可以得到相机坐标系下Aruco码的位置信息。假设在时刻t无人机的位置为p_无人机＝(x_(t),y_(t),z_(t))，在时刻t无人车的位置为：p_无人车＝(x′_(t),y′_(t),z′_(t))。那么无人机与无人车的相对位置为：

Δp_车-机＝(Δx_(t),Δy_(t),Δz_(t))＝(x′_(t)-x_(t),y′_(t)-y_(t),z′_(t)-z_(t)) (25)

默认情况下，认为无人机和无人车的世界坐标系为东北天(ENU)坐标系。无人车在t时刻的世界坐标系下的速度为：

V_无人车(t)＝(u_(t),v_(t)) (26)

为了让无人机更加精准迅速的降落到移动无人车上，仅仅依靠目标识别算法得到的相对距离信息是不够的。这时无人机若能够获得无人车的速度信息，并根据无人车的速度大小调整自身追踪降落的过程，那么就可以提高自主降落的表现和性能。此时，设计的无人机降落策略设计为：

V_无人机(t)＝(V_x(t),V_y(t),V_z(t)) (27)

V_x(t)＝K₁*Δx_(t)+kx*u_(t) (28)

V_y(t)＝K₂*Δy_(t)+ky*v_(t) (29)

其中，V_x、V_y、V_z分别为无人机世界坐标系下x轴、y轴、z轴方向的速度。K₁、K₂、K₃分别为无人机x、y、z方向的速度控制系数。kx和ky分别为x方向和y方向无人机与无人车的协同速度系数。h₁为最低降落距离，表示无人机与无人车之间高度方向的最小距离值；h₂为最低高度，表示无人机在世界坐标系下的最低飞行高度。在无人机降落过程中，若无人机与无人车高度方向的距离低于h₁或无人机的飞行高度低于h₂，则认为无人机已经到达最低降落高度，进入land模式，完成自主降落过程，控制策略流程图如图8所示。

另外，设计空地协同自主降落精度指标E为：

其中，(x_c,y_c)为Aruco码的中心位置坐标，(x′_(T),y′_(T))为无人机降落点坐标。

4、实验验证

4.1相机标定与检测

将相机安装在无人机机架侧方，朝向垂直向下，将Aruco码安置在无人车的顶部，无人机的相机视角范围为θ≤90°，如图9所示，

图10为相机标定所使用到的棋盘格标定板，通过相机标定，得到相机的内参f_x＝678.064156，f_y＝680.164067，C_x＝285.672484，C_y＝198.627123，畸变参数为：k₁＝-0.443024，k₂＝0.231290，k₃＝0.000767，p₁＝0.001554，p₂＝0.000000。

运行无人机二维码检测模块，打开rqt_image_view，并订阅无人机相机图像，得到图11所示的画面，可以看到无人机以及识别到了二维码，并呈现其坐标系等信息。

下一步，通过在终端输入rostopic echo/object_detection/landpad_det，便可以得到如下信息，图12中“position：[0.1064，0.0289，0.7446]”中的三个数据分别表示二维码在相机坐标系下x方向、y方向、z方向的位置信息，此信息将作为无人机自主降落策略的输入，引导无人机精准降落。

4.2基于空地协同系统的无人机自主降落实验

为了测试空地协同系统无人机自主降落的有效性，设计了基于动平台匀速直线运动下的无人机自主降落场景并进行相应的实验。

实验场地如图13所示，场地的长5.4m，宽3.6m，高2.8m。由于场地有限，加上小车具有一定的高度，无人机飞行高度不能过高，将其飞行高度限制在1.5m以下。设定小车的速度不超过20cm/s。

经过调试，无人机的参数选择为：

K₁＝0.8；K₂＝0.8；K₃＝0.05；kx＝1.1；ky＝1.1；h₁＝0.2；h₂＝0.3；起飞高度0.7m

实验过程如图14所示，无人机将在无人车上一依次实现起飞和降落。在无人机到达指定高度后，小车将启动自身控制指令，保持x方向10cm/s的速度匀速行驶，并不断发布自身速度信息。无人机在检测到小车二维码后，开启自主降落过程。在这个过程中，无人机不断订阅无人车的速度信息和来自单目相机的相对位置信息，并处理转化为自身速度控制指令。最终实现精准自主降落。

无人机降落过程相关数据如图15所示：纵坐标为高度、速度、相对位置，横坐标为时间，在(a)图中，drone_state/velocity[0]、/drone_state/velocity[1]、/drone_state/velocity[2]分别代表无人机全局坐标系下x方向、y方向、z方向的位置。在(b)图中，/object_detection/landpad_det/position[0]、/object_detection/landpad_det/position[1]、/object_detection/landpad_det/position[2]分别代表无人机全局坐标系下x方向、y方向、z方向的速度。在(c)中，drone_state/position[0]、drone_state/position[1]、drone_state/position[2]分别代表相机坐标系下无人机与小车二维码x方向、y方向、z方向的相对距离。可以看到无人机在20秒左右开始起飞；28秒时到达指定高度0.7m。这时无人机执行自主降落控制策略，将无人车的速度信息和位置信息作为输入，输出相应的速度指令。由于存在噪声，造成无人机速度曲线的波动，但是总体趋势表现出控制策略的有效性。位置曲线总体非常平滑，起飞和降落的总时间为26秒，其中降落时间15秒。经过测量与计算，无人机降落精度为14.86cm。

为了表现出无人机空地协同自主降落的稳定性和精确性，我们分别测试了动平台在不同速度下无人机自主降落的时间和精度。在动平台10cm/s和20cm/s速度情况下分别测试10次，得到表4-1结果。在动平台10cm/s的情况下，无人机降落的成功率为百分之百，平均降落精度为13.25cm，平均降落时间为15.44秒。而在动平台20cm/s的情况下，虽然降落误差和时间都稍有提高，无人机仍然具有很高的成功率。

表1动平台不同速度情况下无人机降落测试结果

小车速度(cm/s)	10	20
			平均误差(cm)	13.25	16.09
平均降落时间(秒)	15.44	17.13
			成功率	100％	90％

综上，本发明设计的基于空地协同作战的无人机动平台自主降落系统和控制策略在实验中表现良好，具有一定的稳定性、快速性以及精准性。

以上为本发明的优选实施方式，但是本发明不只限于以上实施方式。对于本技术领域的普通技术人员而言，在其知识范围内以及不脱离本发明原理的前提下，可以对本发明作出若干改进，这些也视作本发明的保护范围。

Claims (9)

1.一种面向空地协同作战的无人机动平台自主降落方法，其特征在于，包括以下步骤：

1)基于空地协同作战的无人机动平台自主降落问题构建系统；

2)推导目标坐标系与相机坐标、像素坐标系、物理成像坐标系之间的关系，并通过识别Aruco码解算其位置信息；

3)在获得无人机与动平台相对位置信息的条件下，额外引入动平台速度信息，设计无人机自主降落策略，引导无人机平稳降落。

2.根据权利要求1所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，步骤1)中所述系统包含空地协同自主降落系统、基于mavros的无人机内部通信方式、无人机配备的硬件。

3.根据权利要求1所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，步骤2)中坐标转换包括以下步骤：

(a)世界坐标系到相机坐标系的转换

空间点P(X，Y，Z)在世界坐标系下的坐标为P_W(X_W，Y_W，Z_W)，相应的其在相机坐标系下的坐标为P_C(X_C，Y_C，Z_C)，两者之间的转换关系为：

其中，T为相机外参，由相机安装位置和方向决定，主要包括两部分R和t，R为正交旋转矩阵，由相机坐标系与世界坐标系的旋转欧拉角决定：

t为平移矩阵，由相机坐标系的坐标原点在世界坐标系的x、y、z方向的平移量t_x、t_y、t_z决定：

t＝[t_xt_yt_z]^T (3)

(b)相机坐标系到物理成像坐标系的转换

设P_W在物理成像坐标系下的坐标为P′(X′，Y′，Z′)，由相机的小孔成像模型可知：

Z′＝1 (6)

即：

其中f为焦距，这样得到了空间点P(X，Y，Z)在物理成像平面上的坐标P′(X′，Y′，Z′)；

(c)物理成像坐标系到像素坐标的转换

设物理成像平面上的点P′对应于像素坐标系下的坐标为P_ix(u，v)，则：

其中α、β为图像单位长度上的像素点的个数，C_x为u方向上的像素点的偏移量，C_y为v方向上的像素点的偏移量；

得到：

用矩阵的形式表示为：

其中，f_x为相机x方向的焦距，f_y为相机y方向上的焦距；

(d)世界坐标系到像素坐标系的转换

世界坐标系到像素坐标系的转换关系为：

其中，K为相机内参：

即：

上式中从左到右参数依次为像素坐标点P_ix，相机内参矩阵K，外参矩阵T(变换矩阵)，世界坐标系下的点的坐标P_W，相机的外参矩阵T由相机安装位置和方向决定，估计出相机内参K中的四个参数f_x、f_y、C_x和C_y。

4.根据权利要求3所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，相机成像的理想位置和实际位置存在误差，分为径向失真和切向失真，径向畸变是由于光线在远离透镜中心的地方偏折更大，矫正公式为：

x_corrected＝x(1+k₁r²+k₂r⁴+k₃r⁶) (14)

y_corrected＝y(1+k₁r²+k₂r⁴+k₃r⁶) (15)

切向畸变是由于透镜不完全平行于图像平面而产生的误差。矫正公式为：

x_corrected＝x+[2p₁xy+p₂(r²+2x²)] (16)

y_corrected＝y+[2p₂xy+p₁(r²+2y²)] (17)

其中k₁、k₂、k₃、p₁、p₂为相机的畸变参数。

5.根据权利要求1所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，步骤2)中通过识别Aruco码解算位置信息，具体包括以下步骤：

(i)创建Aruco码：通过drawMarker()函数生成用来；

(ii)检测Aruco码：阈值化处理、含弃无用信息，筛选出候选Aruco码、分析每个候选的比特位，来确定它们是不是marker、获取Aruco码的Id，通过上述的运行过程，检测到图像中所有的markar，并且得到了每个markar的id、角点像素坐标以及Rvec和Tvec；最后需要计算markar在相机坐标系下的位姿。

6.根据权利要求5所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，步骤(ii)具体包括：以二维码的坐标系为世界坐标系，则二维码四个角点的三维空间坐标分别为A＝(-s/2，s/2，0)^T、B＝(s/2，s/2，0)^T、C＝(s/2，-s/2，0)^T、D＝(-s/2，-s/2，0)^T，假设其在图像中对应投影点的像素坐标分别为a＝(u_a，v_a)^T、b＝(u_b，v_b)^T、c＝(u_c，v_c)^T、d＝(u_d，v_d)^T，相机的内参矩阵K已知，三维空间坐标与像素坐标之间的转换关系为：

为了简化表示，令

则(1000)式可表示为：

最后一行把z消去得到两个约束：

于是有：

旋转矩阵R∈SO(3)R\in\text{SO}(3)R∈SO(3)是一个单位正交阵，自身具有六个约束：

通过二维码的三个角点加上旋转矩阵自身的六个约束求得R、t；

最后，利用求得的外参T和公式(1)，便可将二维码的坐标信息转化为相机坐标系下的坐标。

7.根据权利要求1所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，步骤3)具体包括以下步骤：无人机通过检测、识别无人车上的Aruco码并锁定无人车，然后通过坐标变化解算出无人车与无人机的相对位置或距离信息；另外无人机与无人车进行通信，无人车发布自己全局坐标系下的速度信息并由无人机接收，降落控制策略将融合这两种信息，向无人机发出相应的速度控制指令，最后由无人机的执行单元去执行，最终实现无人机的自主降落功能。

8.根据权利要求1或7所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，所述无人机降落策略设计为：

V_无人机(t)＝(V_x(t)，V_y(t)，V_z(t)) (27)

V_x(t)＝K₁*Δx_(t)+kx*u_(t) (28)

V_y(t)＝K₂*Δy_(t)+ky*v_(t) (29)

其中，V_x、V_y、V_z分别为t时刻无人机世界坐标系下x轴、y轴、z轴方向的速度，K₁、K₂、K₃分别为无人机x、y、z方向的速度控制系数，kx和ky分别为x方向和y方向无人机与无人车的协同速度系数，h₁为最低降落距离，表示无人机与无人车之间高度方向的最小距离值；h₂为最低高度，表示无人机在世界坐标系下的最低飞行高度，在无人机降落过程中，若无人机与无人车高度方向的距离低于h₁或无人机的飞行高度低于h₂，则认为无人机已经到达最低降落高度，进入land模式，完成自主降落过程。

9.根据权利要求1所述的面向空地协同作战的无人机动平台自主降落方法，其特征在于，设计空地协同自主降落精度指标E为：

其中，(x_c，y_c)为Aruco码的中心位置坐标，(x′_(T)，y′_(T))为无人机降落点坐标。

Images