一种用于无人机视觉着陆的地标跟踪控制系统及方法与流程

1.本发明涉及一种用于无人机视觉着陆的地标跟踪控制系统及方法，属于计算机控制技术领域。


背景技术：

2.无人机视觉着陆控制是无人机研究中最重要的任务之一。无人机在巡检、返航、充电等任务时，都需要无人机能够精准地实现视觉着陆。
3.要实现精准的视觉着陆，需要保证实时准确的着陆地标检测及跟踪和无人机的稳定控制。为了在性能受限的无人机控制板上保证检测速度，检测算法的复杂度不能太高。而为了保证无人机控制稳定，跟踪算法要足够可靠，控制量要足够平滑。因此，如何在地标检测的实时性、跟踪的准确性和无人机控制的鲁棒性之间权衡就显得尤为重要。


技术实现要素：

4.本发明的目的在于，克服现有技术存在的缺陷，解决如下技术问题：1.现有的无人机视觉着陆存在着陆地标检测不稳定的问题。无人机视觉着陆地标主要采用apriltag检测算法。apriltag检测算法定位依赖于对图像中边缘的检测，当图像分辨率不高或者目标较远时，目标的边缘特征很难被精确捕获；此外，当无人机处于飞行状态时，采集到的图像会有频繁的抖动，目标边缘特征会被进一步破坏，严重影响apriltag检测算法的检测性能。如cn202010978134。2.现有的视觉着陆地标单一、图案简单，满足不了个性化的定制需要。由于视觉检测算法的限制，着陆地标普遍采用h图案、圆等边缘或形状特征明显的图案。一方面这些基本图案很难满足个性化的定制需要；另一方面，由于图案特征单一，检测的结果往往包含更多的噪声。如cn201911136463。3.现有的无人机视觉着陆方案检测范围较小，短时间内无法实现大范围的目标搜索。由于现有的技术方案普遍将摄像头固定在无人机底部，搜索范围固定，需要配合无人机的运动才能实现较大范围的着陆地标检测。
5.本发明具体采用如下技术方案：一种用于无人机视觉着陆的地标跟踪控制系统，包括：
6.地标姿态获取模块，具体执行：由nanodet目标检测算法和apriltag检测算法得到地标相对于相机的姿态信息，所述地标相对于相机的姿态信息包括地标相对于相机的三维位置信息p
target
和角度信息w
target
；
7.云台姿态获取模块，具体执行：由云台自身的惯性传感单元得到云台姿态角w
gimbal
，所述云台姿态角w
gimbal
包含云台的俯仰角、偏航角和横滚角；
8.无人机状态获取模块，具体执行：无人机携带的传感器测量得到无人机状态信息s
drone
，所述无人机状态信息s
drone
包含无人机实时姿态及高度飞行数据；
9.控制信息生成模块，具体执行：将所述地标姿态获取模块、所述云台姿态获取模块和所述无人机状态获取模块获取的信息分别生成包含云台姿态的控制量无人机
姿态控制量和无人机速度控制量的控制量信息；所述控制量信息表示为：
10.u＝f(p
target
,w
target
,w
gimbal
,s
drone
)
11.其中，f表示包含扩展卡尔曼滤波算法、pid控制算法和视觉伺服算法在内的控制算法。
12.本发明还提出一种基于所述用于无人机视觉着陆的地标跟踪控制系统的控制方法，包括如下步骤：
13.步骤ss1：无人机准备着陆，采用nanodet目标检测算法判断是否发现着陆地标，若判定为是，则转入步骤ss2，否则判定为否，转动云台查找目标；
14.步骤ss2：采用扩展卡尔曼滤波算法进行目标跟踪；
15.步骤ss3：控制云台，使目标处于视野中心；
16.步骤ss4：根据云台姿态信息，计算目标相当于无人机的位置信息；
17.步骤ss5：控制无人机，使无人机处于目标上方；固定云台姿态，使相机保持竖直向下；利用视觉伺服控制使无人机向目标移动，同时调整姿态，若判定位置误差与姿态误差小于指定阈值，则进行着陆，否则返回继续步骤ss5。
18.作为一种较佳的实施例，所述步骤ss1具体包括：无人机处于着陆点附近并悬停，控制位于无人机底部的3轴无刷云台在扇形范围内查找着陆地标；通过不断调节云台的俯仰角和横滚角，逐渐扩大搜索区域，云台采用nanodet目标检测算法通过shufflenet多层卷积神经网络提取图像特征，通过特征金字塔网络融合多尺度的所述图像特征，通过浅层的分类网络和回归网络完成地标的识别和定位。
19.作为一种较佳的实施例，所述步骤ss2具体包括：当在视野中检测到着陆地标时，采用扩展卡尔曼滤波算法对地标进行跟踪，考虑无人机在跟踪目标时偏航角的变化，将目标的角度和角速度也放入扩展卡尔曼滤波的状态空间中，即状态向量采用目标在图像视野中的像素位置、速度、角度和角速度，测量值采用nanodet目标检测算法的得到的像素位置信息。
20.作为一种较佳的实施例，所述步骤ss2具体还包括：以目标左上角为例，对应的状态量表示为：
[0021][0022]
其中，p
x
和py分别表示t时刻目标左上角在图像水平方向和垂直方向的像素位置，v表示当前的线速度，ψ表示当前目标移动方向与水平方向的夹角，表示当前的角速度；目标在k+1时刻的状态由k时刻的状态预测得到，对应的状态量预测公式为：
[0023][0024]
其中：
[0025]
[0026][0027]
为简化模型，将k时刻到k+1时刻的目标在图像中的速度和角速度变化考虑在模型的过程噪声中，在状态转移方程中不考虑目标加速度和角加速度均为0，即：
[0028][0029][0030][0031]
在过程噪声中，假设目标在图像中移动的加速度噪声w
a,k
和角加速度噪声符合0均值的高斯分布，即：
[0032][0033][0034]
由于nanodet目标检测算法的检测结果是目标的左上角和右下角像素坐标，因此对应的测量公式为：
[0035]
zk＝hxk+vk[0036]
其中，zk表示nanodet目标检测算法的检测结果，h＝(1,0,0,0,0；0,1,0,0,0),vk表示测量噪声。
[0037]
作为一种较佳的实施例，所述步骤ss3具体包括：为在跟踪过程中保证地标不丢失，在通过nanodet目标检测算法和扩展卡尔曼滤波算法跟踪到地标后，需要调整无人机底部的云台，让地标处于视野的正中间。
[0038]
作为一种较佳的实施例，所述步骤ss3具体还包括：以3轴无刷云台为例，地标在图像中水平方向的偏移对应着云台横滚角的变化，在竖直方向的偏移对应着云台俯仰角的变化；设图像的宽高分别为w和h，相机在水平方向和竖直方向的视场角分别为θ
x
和θy；假设某一时刻图像中的目标中心点坐标为(x
t
,y
t
)，目标到摄像头的直线距离为r，目标相对于相机在水平方向和竖直方向的偏角为α和β,利用三角函数得到如下关系：
[0039]
w＝2krtanθ
x
[0040]
h＝2krtanθy[0041]
x
t
＝krtanα
[0042]yt
＝krtanβ
[0043]
其中，k表示由三维空间到像素平面的缩放系数，由相机内部参数确定，联立上式得到：
[0044][0045][0046]
设当前云台跟踪的角度误差e＝(α,β),针对云台控制设计如下pid控制器：
[0047][0048]
其中，k
p
、ki、kd分别表示云台控制的比例、微分和积分系数，u为最终的云台角度变化控制量。
[0049]
作为一种较佳的实施例，所述步骤ss4具体包括：
[0050]
根据云台当前的姿态信息和目标在相机中的位置信息计算当前目标相对于无人机的位置信息，以地面地标为例，设目标相对于无人机的位置为(x,y,z),对应的计算公式为：
[0051]
x＝ztanα
[0052]
y＝ztanβ
[0053][0054]
其中，α和β为目标在相机中水平方向和竖直方向的偏角；d为目标到相机的距离，由目标相对于相机的姿态信息计算得到。
[0055]
作为一种较佳的实施例，所述步骤ss5具体包括：
[0056]
当无人机获取到目标的位置信息后，云台开始跟踪目标，无人机具体的控制方式取决于云台相机当前的姿态角信息；当云台相机未处于竖直状态时，只对无人机施加pid位置控制，输入的误差信号为目标相对于无人机的三维位置信息与目标位置信息的偏差，即：
[0057]
e＝(x
current-x
target
,y
current-y
target
,z
current-z
target
)
t
[0058][0059]
为进一步平滑控制曲线，并限制输出控制的响应范围，采用如下函数对控制量进行映射：
[0060][0061]
其中，u
in
是pid位置控制的输出；u
out
是经映射后得到的最终位置控制量；u
max
表示作用域无人机的最大控制量；λ为衰减因子，决定控制量的衰减速度；
[0062]
当无人机逐步逼近地标正上方时，云台的俯仰角和横滚角趋于0，进入apriltag检测算法的有效检测范围，此时无人机进入视觉着陆阶段，保持相机视角竖直向下，采用扩展卡尔曼滤波器对目标在图像中的4个顶点进行跟踪，并根据相机内参和apriltag检测算法在图像中的像素位置信息精确计算出相当于无人机的三维空间位置信息；由于当距离着陆
标志较近时，无人机的速度已趋于平稳，采用视觉伺服控制算法对无人机施加位置控制及角度控制，此时的输入误差信号为当前地标在图像中的像素坐标与地标三维空间位置映射到图像中的像素坐标偏差，即：
[0063]
e(t)＝s[m(t),a]-s
*
[0064]
其中，s表示地标的4个顶点在像平面上的坐标，s
*
表示地标的目标位置在像平面上的位置，m表示目标在图像中的像素坐标，a表示相机内部参数，用于将像素坐标映射到像平面上。
[0065]
作为一种较佳的实施例，所述步骤ss5具体还包括：根据误差输入采用的视觉伺服控制率由以下公式计算得到：
[0066][0067]
其中，vc表示所需的相机线速度和角速度向量；l
x
表示图像雅可比矩阵，由相机内参和像素坐标确定，负责将像素坐标系中的速度变换映射到相机坐标系中；λ表示视觉伺服增益，决定控制力的大小。
[0068]
本发明所达到的有益效果：1.本发明提出的一种用于无人机视觉着陆的地标跟踪控制系统及方法，通过深度学习结合apriltag检测算法，深度学习检测对小目标和目标模糊抖动鲁棒性好，可以在远距离和无人机速度变化较快时实现有效的检测；apriltag检测算法可获取精确的目标姿态信息，可在距离目标较近时实现精准的姿态控制；两者想结合可兼顾远距离和近距离的着陆标志检测。2.本发明提出的一种用于无人机视觉着陆的地标跟踪控制系统及方法配合深度学习目标检测技术，目标不再局限于apriltag和简单的图案，可针对不同的使用场景实现个性化定制。3.本发明提出的一种用于无人机视觉着陆的地标跟踪控制系统及方法配合三轴云台可实现大范围的目标搜索，提高视觉着陆可靠性的同时也减少了对其他定位模块的依赖(如gps、惯性传感单元等)。4.本发明通过由nanodet目标检测算法和apriltag检测算法得到地标相对于相机的姿态信息，所述地标相对于相机的姿态信息包括地标相对于相机的三维位置信息p
target
和角度信息w
target
；由云台自身的惯性传感单元得到云台姿态角w
gimbal
，所述云台姿态角w
gimbal
包含云台的俯仰角、偏航角和横滚角；无人机携带的传感器测量得到无人机状态信息s
drone
，所述无人机状态信息s
drone
包含无人机实时姿态及高度飞行数据；将所述地标姿态获取模块、所述云台姿态获取模块和所述无人机状态获取模块获取的信息分别生成包含云台姿态的控制量无人机姿态控制量和无人机速度控制量的控制量信息，从整体上解决了现有的无人机视觉着陆存在着陆地标检测不稳定的问题、现有的视觉着陆地标单一、图案简单，满足不了个性化的定制需要以及现有的无人机视觉着陆方案检测范围较小，短时间内无法实现大范围的目标搜索的技术需求。
附图说明
[0069]
图1是本发明的一种用于无人机视觉着陆的地标跟踪控制系统的原理拓扑示意图；
[0070]
图2是本发明的一种用于无人机视觉着陆的地标跟踪控制方法的流程图。
具体实施方式
[0071]
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0072]
实施例1：如图1所示，本发明提出一种用于无人机视觉着陆的地标跟踪控制系统，包括：
[0073]
地标姿态获取模块，具体执行：由nanodet目标检测算法和apriltag检测算法得到地标相对于相机的姿态信息，所述地标相对于相机的姿态信息包括地标相对于相机的三维位置信息p
target
和角度信息w
target
；
[0074]
云台姿态获取模块，具体执行：由云台自身的惯性传感单元得到云台姿态角w
gimbal
，所述云台姿态角w
gimbal
包含云台的俯仰角、偏航角和横滚角；
[0075]
无人机状态获取模块，具体执行：无人机携带的传感器测量得到无人机状态信息s
drone
，所述无人机状态信息s
drone
包含无人机实时姿态及高度飞行数据；
[0076]
控制信息生成模块，具体执行：将所述地标姿态获取模块、所述云台姿态获取模块和所述无人机状态获取模块获取的信息分别生成包含云台姿态的控制量无人机姿态控制量和无人机速度控制量的控制量信息；所述控制量信息表示为：
[0077]
u＝f(p
target
,w
target
,w
gimbal
,s
drone
)
[0078]
其中，f表示包含扩展卡尔曼滤波算法、pid控制算法和视觉伺服算法在内的控制算法。
[0079]
实施例2：如图2所示，本发明提出一种用于无人机视觉着陆的地标跟踪控制方法，具体包括如下6个步骤。
[0080]
1.无人机处于着陆点附近并悬停，控制位于无人机底部的3轴无刷云台在扇形范围内查找着陆地标。通过不断调节云台的俯仰角和横滚角，可以逐渐扩大搜索区域。此时由于距离目标较远，地标在视野中很小，检测算法采用轻量级的nanodet模型。模型通过shufflenet多层卷积神经网络自动提取图像特征，通过特征金字塔网络融合多尺度的图像特征，通过浅层的分类网络和回归网络完成地标的识别和定位。
[0081]
2.当在视野中检测到着陆地标时，采用扩展卡尔曼滤波对地标进行跟踪，考虑到无人机在跟踪目标时偏航角的变化，将目标的角度和角速度也放入扩展卡尔曼滤波的状态空间中，即状态向量采用目标在图像视野中的像素位置、速度、角度和角速度，测量值采用nanodet目标检测的得到的像素位置信息。以目标左上角为例，对应的状态量表示为
[0082][0083]
其中p
x
和py分别表示t时刻目标左上角在图像水平方向和垂直方向的像素位置，v表示当前的线速度，ψ表示当前目标移动方向与水平方向的夹角，表示当前的角速度。目标在k+1时刻的状态可由k时刻的状态预测得到，对应的预测公式为：
[0084][0085]
其中：
[0086][0087][0088]
为模型的简化，将k时刻到k+1时刻的目标在图像中的速度和角速度变化考虑在模型的过程噪声中，在状态转移方程中不考虑目标加速度和角加速度均为0，即：
[0089][0090][0091][0092]
在过程噪声中，假设目标在图像中移动的加速度噪声w
a,k
和角加速度噪声符合0均值的高斯分布，即：
[0093][0094][0095]
由于nanodet检测的结果是目标的左上角和右下角像素坐标，因此对应的测量公式为：
[0096]
zk＝hxk+vk[0097]
其中，zk表示nanodet的检测结果，h＝(1,0,0,0,0；0,1,0,0,0),vk表示测量噪声。
[0098]
3.为了在跟踪过程中保证地标不丢失，在通过上述nanodet检测模型和扩展卡尔曼滤波跟踪到地标后，需要调整无人机底部的云台，让地标处于视野的正中间。以常见的3轴无刷云台为例，地标在图像中水平方向的偏移对应着云台横滚角的变化，在竖直方向的偏移对应着云台俯仰角的变化。设图像的宽高分别为w和h,相机在水平方向和谁知方向的视场角分别为θ
x
和θy；假设某一时刻图像中的目标中心点坐标为(xt,yt)，目标到摄像头的直线距离为r，目标相对于相机在水平方向和竖直方向的偏角为α和β,利用三角函数可以得到如下关系：
[0099]
w＝2krtanθ
x
[0100]
h＝2krtanθy[0101]
xt＝krtanα
[0102]
yt＝krtanβ
[0103]
其中，k表示由三维空间到像素平面的缩放系数，由相机内参确定。根据上式可以得到：
[0104][0105][0106]
设当前云台跟踪的角度误差e＝(α,β),可针对云台控制设计如下pid控制器：
[0107][0108]
其中，k
p
、ki、kd分别表示云台控制的比例、微分和积分系数，u为最终的云台角度变化控制量。
[0109]
4.根据云台当前的姿态信息和目标在相机中的位置信息计算当前对目标相对于无人机的位置信息，以地面地标为例，设目标相对于无人机的位置为(x,y,z),对应的计算公式为：
[0110]
x＝ztanα
[0111]
y＝ztanβ
[0112][0113]
其中，α和β为目标在相机中水平方向和竖直方向的偏角；d为目标到相机的距离，由目标相对于相机的姿态信息计算得到。
[0114]
5.当无人机获取到目标的位置信息后，云台开始跟踪目标，无人机具体的控制方式取决于云台相机当前的姿态角信息。
[0115]
当云台相机未处于竖直状态时，只对无人机施加pid位置控制，输入的误差信号为目标相对于无人机的三维位置信息与目标位置的偏差，即：
[0116]
e＝(x
current-x
target
,y
current-y
target
,z
current-z
target
)
t
[0117][0118]
为进一步平滑控制曲线，并限制输出控制的响应范围，采用如下函数对控制量进行映射：
[0119][0120]
其中，u
in
是pid位置控制的输出；u
out
是经映射后得到的最终位置控制量；u
max
表示作用域无人机的最大控制量；λ为衰减因子，决定控制量的衰减速度。
[0121]
当逐渐靠近着陆标志时，进入apriltag的有效检测范围，采用扩展卡尔曼滤波器对目标在图像中的4个顶点进行跟踪，并可以根据相机内参和apriltag在图像中的像素位置信息精确计算出相当于无人机的三维空间位置信息。由于当距离着陆标志较近时，无人机的速度已趋于平稳，这阶段ariltag目标检测鲁棒性较强，不再依赖于深度学习。采用视觉伺服控制对无人机施加位置控制及角度控制，此时的输入误差信号为当前地标在图像中
的像素坐标与地标三维空间位置映射到图像中的像素坐标偏差，即：
[0122]
e(t)＝s[m(t),a]-s
*
[0123]
其中，s表示地标的4个顶点在像平面上的坐标，s
*
表示地标的目标位置在像平面上的位置，m表示目标在图像中的像素坐标，a表示相机内参，用于将像素坐标映射到像平面上。根据误差输入采用的视觉伺服控制率由以下公式计算得到：
[0124][0125]
其中，vc表示所需的相机线速度和角速度向量；l
x
表示图像雅可比矩阵，由相机内参和像素坐标确定，负责将像素坐标系中的速度变换映射到相机坐标系中；λ表示视觉伺服增益，决定控制力的大小。
[0126]
6.无人机根据视觉伺服空置率快速到达目标位置，当误差低于指定的阈值时控制无人机着陆。
[0127]
相比于现有技术，本发明的优点在于：1.本发明提出的一种用于无人机视觉着陆的地标跟踪控制系统及方法，通过深度学习结合apriltag检测算法，深度学习检测对小目标和目标模糊抖动鲁棒性好，可以在远距离和无人机速度变化较快时实现有效的检测；apriltag检测算法可获取精确的目标姿态信息，可在距离目标较近时实现精准的姿态控制；两者想结合可兼顾远距离和近距离的着陆标志检测。2.本发明提出的一种用于无人机视觉着陆的地标跟踪控制系统及方法配合深度学习目标检测技术，目标不再局限于apriltag和简单的图案，可针对不同的使用场景实现个性化定制。3.本发明提出的一种用于无人机视觉着陆的地标跟踪控制系统及方法配合三轴云台可实现大范围的目标搜索，提高视觉着陆可靠性的同时也减少了对其他定位模块的依赖(如gps、惯性传感单元等)。
[0128]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0129]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0130]
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽
管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。