一种基于ROS的双云台无人机自主降落方法

一种基于ros的双云台无人机自主降落方法
技术领域
1.本发明涉及无人机技术领域，特别是涉及一种无人机自主降落控制系统。


背景技术：

2.无人机(unmanned aerial vehicle，uav)，是无人驾驶飞机的简称。飞机上安装有自动驾驶仪、程序控制装置等设备，在地面安装雷达等设备完成对无人机的跟踪、遥控等任务，与重载飞机相比具有体积小、使用方便的优点。广泛应用于电力高空巡检作业、煤矿塌陷救援工作、森林防火巡查工作、核酸检测样箱运输工作、植保农药喷洒、施肥作业等方面，都充分体现了无人机在实际应用中的重要意义，由此可见，无人机的市场呈现不断拓宽的趋势，具有重大的经济与军事意义。
3.在各类新兴技术不断发展下，许多问题亟待解决。尤其针对四旋翼无人机续航性能欠缺，与地面设备(自走式施肥机，地面移动充电设备)等协作完成充电、施肥、救援等任务后的回收又称为关键技术之一。自主降落过程中主要用到的传感器技术包括：gnss定位导航系统、激光雷达、超宽带uwb、gps定位导航技术等。但以上定位技术都存在一定的不足，针对电磁干扰、风扰动等情况易引起信号丢失，加大误差而导致坠机的风险，在无人机动态降落过程中需要精准与更稳定的降落方式。所以开展对无人机自主降落方法的关键技术研究意义重大。


技术实现要素：

4.本发明目的是提供一种基于ros的双云台无人机自主降落方法，该方法基于视觉定位技术，目标跟踪技术，设计复合的aruco定位标记，使用改进的kcf目标跟踪技术，结合双环pid控制器实现无人机动态降落，有效解决背景技术中所描述的目标跟踪问题及自主降落定位问题。
5.本发明的技术方案是：一种基于ros的双云台无人机自主降落方法，包含以下步骤：
6.步骤1，基于视觉的前视相机无人机目标跟踪技术，进行无人机对移动目标的远距离目标跟踪，针对应用场景中背景复杂，安全性高及变尺度性能要求，选取符合要求的目标跟踪算法，验证无人机自主降落过程中的远距离目标跟踪；
7.步骤2，基于双环pid的轨迹跟踪策略，作为无人机自主降落的理论基础，根据无人机降落特点及目标移动特点，外环pid控制器的输出作为内环控制器的输入值，内环控制器的输出传递给执行机构，从而保证控制系统具有较好的控制效果，使其在轨迹跟踪过程中的稳定性有较大提升；
8.步骤3，在步骤2的基础上，基于视觉的目标定位技术，使用下视相机获取俯视图像和视觉标识确定目标降落区域；
9.步骤4，根据所述俯视图像和视觉标识确定目标降落区，确定降落区的位置信息，并根据位置信息启动自主降落控制指令，使用单级pid控制策略控制无人机速度实现降落。
10.所述步骤1中，无人机前视相机第一帧图像的获取使用图像处理技术进行轮廓提取。步骤一包含以下步骤：
11.步骤1.1：获取第一帧目标图像；
12.步骤1.2：在当前帧目标区域附近采样，使用改进kcf目标跟踪器进行移动平台的位置预测；
13.步骤1.3：在目标位置已知的情况下，使用改进kcf目标跟踪器的尺度滤波器寻找响应最大的尺度，在小范围内实现尺度自适应；
14.步骤1.4：在新一帧提取训练样本时，尺度估计滤波器将每次更新的尺度变化传递到下一帧位置跟踪滤波器中进行目标跟踪，根据上一帧尺度变化动态的调整当前帧的训练尺寸。
15.所述步骤1.1包括以下步骤：
16.步骤1.1.1：使用onmouse(event,x,y,flags,param)函数获取第一帧图像；
17.步骤1.1.2：灰度化与二值化；
18.步骤1.1.3：使用findcontours()找出轮廓。
19.所述步骤1.2包括以下步骤：
20.步骤1.2.1：使用岭回归做分类，采用最小二乘法逼近拟合函数计算系统响应值，得到回归标签矩阵；
21.步骤1.2.2：通过循环位移构建目标物位于跟踪框不同位置的训练样本集，产生循环矩阵；
22.步骤1.2.3：根据训练样本集及标签训练一个线性回归模型；
23.步骤1.2.4：引入核函数对非线性模型进行回归；
24.步骤1.2.5：利用傅里叶反变换求出在待测样本集中回归响应值最大的样本位置。
25.所述步骤1.3包含以下步骤：
26.步骤1.3.1：使用岭回归求解最优尺度相关滤波器
27.步骤1.3.2：利用尺度相关滤波器以当前位置为中心点，获取不同尺度的候选目标，使用目标样本候选参数原则anp
×an
r选取合适尺度，式中p和r分别表示在前一帧的宽和高，尺度引子a＝1.05。
28.所述步骤2基于双环pid的轨迹跟踪策略，作为无人机自主降落的理论基础，根据无人机降落特点及目标移动特点，外环pid控制器的输出作为内环控制器的输入值，内环控制器的输出传递给执行机构，从而保证控制系统具有较好的控制效果，使其在轨迹跟踪过程中的稳定性有较大提升：步骤2包含以下步骤：
29.步骤2.1：外环控制器用于控制无人机与目标间的距离，内环为速度控制环，通过确定目标跟踪框中心位置到无人机机载相机图像中心位置的像素距离来确定无人机的速度，其控制关系式为
30.步骤2.2：当前视相机成像平面上目标占比越来越大，且无人机与目标水平距离x小于预设阈值，无人机下视相机节点启动。
31.所述步骤3：在步骤2的基础上，基于视觉的目标定位技术，使用下视相机获取俯视
图像和视觉标识确定目标降落区域：包含以下步骤：
32.步骤3.1：无人机自主降落标识设计；
33.步骤3.2：视觉定位算法从坐标转换原理入手，进行空间位姿公式推导；
34.步骤3.3：图像域处理；
35.步骤3.4：引入aruco类目标定位码进行标志检测与识别；
36.步骤3.5：相对位姿解算,使用pnp类定位算法求解出相机对于移动目标的三维空间坐标系的旋转向量和平移向量，最后计算欧拉角，计算出无人机相对aruco的偏移量，最后得出无人机相较于降落板的相对位置。
37.所述步骤3.1中的降落标识码要能保证无人机能够精准降落。
38.所述步骤3.1中的标识码具备以下三个条件：(1)简单性(2)易于区分(3)易于识别。
39.所述步骤3.1设计复合降落标志尺寸为1.4
×
0.8m。较大码的尺寸为0.6
×
0.6m，其余小码的尺寸为0.16666
×
0.16666m。当标识检测程序运行时，每次只处理一个marker，如果同时检测到多个marker时，marker的优先级设置为：2》(1,5,3,6)》7,理想情况下，无人机远距离检测到7号码，进行位置估计和调整，通过冗余码1、3、5、6进行精细化矫正，使得无人机精准降落于标识码2。
40.所述步骤3.2中包含以下步骤：
41.步骤3.2.1：无人机自主降落过程中对其位置进行估计，进行坐标系的转换；涉及到的坐标系包含世界坐标系、机体坐标系、摄像机坐标系与图像坐标系；
42.步骤3.2.2：构建相机成像模型—针孔成像模型；
43.步骤3.2.3：根据步骤3.1.1与3.1.2所述建立相机坐标系下的坐标对应像点在图像坐标系下的坐标映射关系；
44.步骤3.2.4：由建立的坐标系关系得到无人机在世界坐标系下的位置与姿态，并获取移动目标在世界坐标系下的坐标p2。
45.所述步骤3.3中包含以下步骤：
46.步骤3.3.1：图像灰度化，使用cvtcolor(img,imggray,cv_bgr2gray)函数将采集的彩色图像转换为灰度图；
47.步骤3.3.2：图像阈值化，使用threshold(imggray,result,100,255,cv_thresh_binary)函数进行阈值化操作，相机拍摄下存在较复杂的噪声和复杂背景，阈值化可以有效的去除噪声和分割背景，降低图像处理的复杂度；
48.步骤3.3.3：轮廓提取，使用findcontours(thresh1,contours2,hierarchy2,retr_tree,chain_approx_none)函数将aruco通过轮廓提取掌握图形基本构造；
49.步骤3.3.4：四边形筛选，使用findsquares(const umat&image,vector《vector《point》》&squares)函数进行四边形筛选，当摄像机从不同的角度摄取标记时，获得的标记的图像总是四边形，所以需要对提取的轮廓做进一步的滤波操作来提取出候选标记；
50.步骤3.3.5：透视变换，将相机视角下获取的图像进行图片修正，得到标记正视图。将相机视角下获取的图像进行图片修正，得到标记正视图。使用cvgetperspectivetransform()函数由三对点计算仿射变换，得到3
×
3的矩阵；使用函数cvwarpperspective()对图像做仿射变换。
51.所述步骤3.5包含以下步骤：
52.步骤3.5.1：使用步骤3.2.1所述的坐标转换关系，得到世界坐标系下的点与像素坐标系下点的转换关系
53.步骤3.5.2：利用pnp类定位算法——直接线性化方法(dlt)获取物体位姿，本发明中的合作标记每个marker都有四个明显的角点，其四个角点在像素坐标系中的坐标和世界坐标系的点坐标已知，求解旋转矩阵r与平移矩阵t；
54.步骤3.5.3：计算偏移量，根据刚体变换pc＝r
×
pw+t,其中，pc代表aruco码对象在相机坐标系下的坐标，pw代表aruco码对象在世界坐标系下的坐标，从相机坐标系映射到世界坐标系，以无人机单目相机所在的位置为相机坐标系原点，就可以计算出aruco码对象在相机坐标系下的坐标，如此，就可以计算出无人机相对于aruco码的偏移量；
55.步骤3.5.4：使p＝0，则意味着aruco码对象移到了相机坐标系的原点，求出来的pw代表了相机在世界坐标系中的位置，0＝pwr+t，相机在世界坐标系下的坐标变换公式为pw＝-r-1
t；
56.步骤3.5.5：已经知道相机在世界坐标系下的坐标的情况下，世界坐标系原点的位置在aruco码对象处，就可以解算出无人机相较于aruco码的偏航角。
57.所述步骤4根据所述俯视图像和视觉标识确定目标降落区，确定降落区的位置信息，并根据位置信息启动自主降落控制指令；可根据无人机通过gps，惯性里程计，气压计等融合获取实机坐标p1，通过下视相机节点获取移动平台在涉及坐标系下的实时三维坐标位置p2。使用单级pid控制方式，上述两坐标系的差值为无人机与地面移动平台的位置偏差，作为pid自主降落控制器的输入变量。以此无人机实时调整自身位置，逐步跟进并靠近降落板，待机体与降落板距离达到预设阈值，无人机上锁降落。
58.本发明的有益效果是：基于目标跟踪技术，实现无人机对目标的实时跟踪，解决gps受干扰情况下及目标突然加速或减速情况下的目标丢失问题；基于视觉定位技术，通过设计复合aruco识别标志及算法解算无人机识别标志的相对位置参数，实现无人机的姿态调整；采用双环pid控制器，稳定控制无人机的目标跟踪速度；采用双云台的工作机制，有效的避免了在ros环境下的节点冲突问题，满足在复杂工作条件下的高时效性，为精准降落提供了必要的条件，使无人机的降落进度有了明显的提高。
附图说明
59.图1为本发明方法的降落标识图；
60.图2为本发明方法的软件框架图；
61.图3为本发明方法的流程图；
62.图4为本发明方法的相机坐标系与世界坐标关系图；
63.图5为本发明的跟踪双环pid控制系统框图。
具体实施方式
64.下面将结合附图，对本发明中的技术方案进行清楚、完整地描述。
65.一种基于ros的双云台无人机自主降落方法，包含以下步骤：
66.步骤1，基于视觉的前视相机无人机目标跟踪技术，进行无人机对移动目标的远距离目标跟踪，针对应用场景中背景复杂，安全性高及变尺度性能要求，选取符合要求的目标跟踪算法，验证无人机自主降落过程中的远距离目标跟踪；
67.步骤2，基于pid的轨迹跟踪策略，作为无人机自主降落的理论基础，根据无人机降落特点及目标移动特点，外环pid控制器的输出作为内环控制器的设定值，内环控制器的输出传递给执行机构，从而保证控制系统具有较好的控制效果，使其在轨迹跟踪过程中的稳定性有较大提升；
68.步骤3，在步骤2的基础上，基于视觉的目标定位技术，使用下视相机获取俯视图像和视觉标识确定目标降落区域；
69.步骤4，根据所述俯视图像和视觉标识确定目标降落区，确定降落区的位置信息，并根据位置信息启动自主降落控制指令，使用单级pid控制策略控制无人机速度实现降落。
70.首先针对本发明设计易于识别地降落标志码，保证无人机能够精准降落。所设计标识码具备以下三个条件：(1)简单性(2)易于区分(3)易于识别。
71.本发明所采用复合降落标志尺寸为1.4
×
0.8m。较大码的尺寸为0.6
×
0.6m，其余小码的尺寸为0.16666
×
0.16666m。当标识检测程序运行时，每次只处理一个marker，如果同时检测到多个marker时，marker的优先级设置为：2》(1,5,3,6)》7,理想情况下，无人机远距离检测到7号码，进行位置估计和调整，通过冗余码1、3、5、6进行精细化矫正，使得无人机精准降落于标识码2。
72.所述步骤1中，无人机前视相机第一帧图像的获取使用图像处理技术进行轮廓提取。
73.步骤一包含以下步骤：
74.步骤1.1：获取第一帧目标图像；
75.步骤1.2：在当前帧目标区域附近采样，使用改进kcf目标跟踪器进行移动平台的位置预测；
76.步骤1.3：在目标位置已知的情况下，使用改进kcf目标跟踪器的尺度滤波器寻找响应最大的尺度，在小范围内实现尺度自适应；
77.步骤1.4：在新一帧提取训练样本时，尺度估计滤波器将每次更新的尺度变化传递到下一帧位置跟踪滤波器中进行目标跟踪，根据上一帧尺度变化动态的调整当前帧的训练尺寸。
78.所述步骤1.1包括以下步骤：
79.步骤1.1.1：使用onmouse(event,x,y,flags,param)函数获取第一帧图像；
80.步骤1.1.2：灰度化与二值化；
81.步骤1.1.3：使用findcontours()找出轮廓。
82.所述步骤1.2包括以下步骤：
83.步骤1.2.1：使用岭回归做分类，采用最小二乘法逼近拟合函数计算系统响应值，得到回归标签矩阵；
84.步骤1.2.2：通过循环位移构建目标物位于跟踪框不同位置的训练样本集，产生循环矩阵；
85.步骤1.2.3：根据训练样本集及标签训练一个线性回归模型；
86.步骤1.2.4：引入核函数对非线性模型进行回归；
87.步骤1.2.5：利用傅里叶反变换求出在待测样本集中回归响应值最大的样本位置。
88.所述步骤1.3包含以下步骤：
89.步骤1.3.1：使用岭回归求解最优尺度相关滤波器
90.步骤1.3.2：利用尺度相关滤波器以当前位置为中心点，获取不同尺度的候选目标，使用目标样本候选参数原则anp
×an
r选取合适尺度，式中p和r分别表示在前一帧的宽和高，尺度引子a＝1.05。
91.所述步骤2基于pid的轨迹跟踪策略，作为无人机自主降落的理论基础，根据无人机降落特点及目标移动特点，外环pid控制器的输出作为内环控制器的输入值，内环控制器的输出传递给执行机构，从而保证控制系统具有较好的控制效果，使其在轨迹跟踪过程中的稳定性有较大提升。
92.步骤2包含以下步骤：
93.步骤2.1：目标跟踪框中心位置到无人机机载相机图像中心位置的像素距离来确定无人机的速度，其控制关系式为
94.步骤2.2：当前视相机成像平面上目标占比越来越大，且无人机与目标水平距离x小于预设阈值，无人机下视相机节点启动。
95.所述步骤3：在步骤2的基础上，基于视觉的目标定位技术，使用下视相机获取俯视图像和视觉标识，确定目标降落区域：包含以下步骤：
96.步骤3.1:无人机自主降落标识设计；
97.步骤3.2:视觉定位算法从坐标转换原理入手，进行空间位姿公式推导；
98.步骤3.3:图像预处理；
99.步骤3.4:引入aruco类目标定位码进行标志检测与识别；
100.步骤3.5:相对位姿解算,使用pnp类定位算法求解出相机对于移动目标的三维空间坐标系的旋转向量和平移向量，最后计算欧拉角，计算出无人机相对aruco的偏移量，最后得出无人机相较于降落板的相对位置。
101.所述步骤3.1中的降落标识码要能保证无人机能够精准降落。
102.所述步骤3.1所设计标识码具备以下三个条件：(1)简单性(2)易于区分(3)易于识别。
103.所述步骤3.1设计复合降落标志尺寸为1.4
×
0.8m。较大码的尺寸为0.6
×
0.6m，其余小码的尺寸为0.16666
×
0.16666m。当标识检测程序运行时，每次只处理一个marker，如果同时检测到多个marker时，marker的优先级设置为：2》(1,5,3,6)》7,理想情况下，无人机远距离检测到7号码，进行位置估计和调整，通过冗余码1、3、5、6进行精细化矫正，使得无人机精准降落于标识码2。
104.所述步骤3.2中包含以下步骤：
105.步骤3.2.1：无人机自主降落过程中对其位置进行估计，进行坐标系的转换；涉及到的坐标系包含世界坐标系、机体坐标系、摄像机坐标系与图像坐标系。
106.世界坐标系到相机坐标系的转换：[x
c y
c zc]
t
＝r[x
w y
w zw]
t
+t。
[0107]
像素坐标系到图像坐标系的转换：u＝x/dx+u0；v＝y/dy+v0。
[0108]
图像坐标系到相机坐标系的转换：x＝f
×
xc/zc；y＝f
×
yc/zc；
[0109]
步骤3.2.2：构建相机成像模型—针孔成像模型；
[0110]
步骤3.2.3：根据步骤3.1.1与3.1.2所述建立相机坐标系下的坐标对应像点在图像坐标系下的坐标映射关系；
[0111]
步骤3.2.4：由建立的坐标系关系得到无人机在世界坐标系下的位置与姿态，并获取移动目标在世界坐标系下的坐标p2。
[0112]
所述步骤3.3中包含以下步骤：
[0113]
步骤3.3.1：图像灰度化，使用cvtcolor(img,imggray,cv_bgr2gray)函数将采集的彩色图像转换为灰度图；
[0114]
步骤3.3.2：图像阈值化，使用threshold(imggray,result,100,255,cv_thresh_binary)函数进行阈值化操作，相机拍摄下存在较复杂的噪声和复杂背景，阈值化可以有效的去除噪声和分割背景，降低图像处理的复杂度；
[0115]
步骤3.3.3：轮廓提取，使用findcontours(thresh1,contours2,hierarchy2,retr_tree,chain_approx_none)函数将aruco通过轮廓提取掌握图形基本构造；
[0116]
步骤3.3.4：四边形筛选，使用findsquares(const umat&image,vector《vector《point》》&squares)函数进行四边形筛选，当摄像机从不同的角度摄取标记时，获得的标记的图像总是四边形，所以需要对提取的轮廓做进一步的滤波操作来提取出候选标记；
[0117]
步骤3.3.5：透视变换，将相机视角下获取的图像进行图片修正，得到标记正视图。使用cvgetperspectivetransform()函数由三对点计算仿射变换，得到3
×
3的矩阵；使用函数cvwarpperspective()对图像做仿射变换。
[0118]
所述步骤3.5的具体实施步骤包含以下步骤：
[0119]
步骤3.5.1：使用步骤3.2.1所述的坐标转换关系，得到世界坐标系下的点与像素坐标系下点的转换关系
[0120]
步骤3.5.2：利用pnp类定位算法——直接线性化方法(dlt)获取物体位姿，本发明中的合作标记每个marker都有四个明显的角点，其四个角点在像素坐标系中的坐标和世界坐标系的点坐标已知，求解旋转矩阵r与平移矩阵t；
[0121]
步骤3.5.3：计算偏移量，根据刚体变换pc＝r
×
pw+t,其中，pc代表aruco码对象在相机坐标系下的坐标，pw代表aruco码对象在世界坐标系下的坐标，从相机坐标系映射到世界坐标系，以无人机单目相机所在的位置为相机坐标系原点，就可以计算出aruco码对象在相机坐标系下的坐标，如此，就可以计算出无人机相对于aruco码的偏移量。
[0122]
步骤3.5.4：使p＝0，则意味着aruco码对象移到了相机坐标系的原点，求出来的pw代表了相机在世界坐标系中的位置，0＝pwr+t，相机在世界坐标系下的坐标变换公式为pw＝-r-1
t；
[0123]
步骤3.5.5：已经知道相机在世界坐标系下的坐标的情况下，世界坐标系原点的位置在aruco码对象处，就可以解算出无人机相较于aruco码的偏航角；
[0124]
步骤3.5.6：解算复合降落标志精准降落标识位置平均值，将解算后的位置发送给飞控端，控制无人机完成动态降落。
[0125]
所述步骤4根据所述俯视图像和视觉标识确定目标降落区，确定降落区的位置信息，并根据位置信息启动自主降落控制指令；可根据无人机通过gps，惯性里程计，气压计等融合获取实机坐标p1，通过下视相机节点获取移动平台在涉及坐标系下的实时三维坐标位置p2，使用单级pid控制方式，上述两坐标系的差值为无人机与地面移动平台的位置偏差，作为pid自主降落控制器的输入变量，以此无人机实时调整自身位置，逐步跟进并靠近降落板，待机体与降落板距离达到预设阈值，无人机上锁降落。
[0126]
实施例。
[0127]
本发明一种基于ros的双云台无人机自主降落方法，软件控制框图如图2所示，本系统主要为软件系统构成，由单目相机实时获取图像信息，飞控端解算位姿信息，从而实现无人机姿态调整，使其逼近移动平台，最终达到了降落与跟踪目的。
[0128]
本发明一种基于ros的双云台无人机自主降落方法，根据流程图，在安装有ros、mavros、mavlink的ubuntu18.04系统下启动终端，待mavros连接成功，将无人机起飞至固定高度h，在目标跟踪阶段：启动目标跟踪节点，待移动平台进入前视相机视野，无人机进行目标检测，并使用轨迹追踪控制策略跟踪移动平台运动，待无人机距移动平台水平距离1.2米时，启动下视相机节点，进入自主降落阶段：启动目标检测节点进行复合标志码的检测，利用复合降落标志与目标定位算法进行无人机近距离位姿估计，并启动自主降落节点，使其边跟踪边降落。
[0129]
考虑双云台无人机自主降落过程分为5个阶段：起飞阶段、目标跟踪阶段、轨迹跟踪阶段、降落定位阶段、降落阶段。首先由终端控制无人机完成起飞阶段。因此程序要包含终端命令起飞功能。
[0130]
第二阶段为目标跟踪阶段，在这一阶段中，需要待无人机起飞悬停后，启动前视相机节点，进行图像第一帧的获取，待移动平台进入视野，使用上述方式获取第一帧图像，并使改进kcf目标跟踪技术进行目标下一帧的位置预测及尺度估计，保证稳定的目标跟踪功能。
[0131]
第三阶段为目标轨迹跟踪阶段，确定目标跟踪框中心位置到无人机机载相机图像中心位置的像素距离来确定无人机的速度；当前视相机成像平面上目标占比越来越大，且无人机与目标水平距离x小于预设阈值，无人机下视相机节点启动。
[0132]
第四阶段，，无人机启动下视相机节点，在相机视野范围内借助降落标识码进行精确定位，且无人机通过定位不断调整自身位姿，逐步靠近设定降落码上方。
[0133]
最后无人机降落阶段，根据设定降落要求，无人机时刻检测降落码与无人机的垂直距离，当无人机在降落码上方高度达到给定阈值时，自动切换至降上锁模式完成降落。
[0134]
综上所述，本发明采用双云台工作机制，首先采用改进kcf的目标跟踪技术，实时跟踪移动平台，并结合pid轨迹跟踪算法实现了无人机动态降落过程中对目标的稳定跟踪；其次采用aruco的复合定位标记，在无人机低空降落过程中，能够精确定位降落码，并降落
至要求码中。此种降落方法在提高无人机动态降落过程中的精度具有重要意义。
[0135]
本发明申请人结合说明书附图对本发明的实施例做了详细的说明与描述，经过众多仿真实验表明，该发明具有一定的应用价值与研究意义。