一种基于LED圆环检测的四旋翼无人机相对位姿估计方法与流程

一种基于led圆环检测的四旋翼无人机相对位姿估计方法
技术领域
[0001]
本发明属于四旋翼无人机技术领域，特别涉及到一种四旋翼无人机相对位姿估计方法。


背景技术：

[0002]
近年来，无人机的应用愈加广泛，多机协作配合的任务也越来越多，如无人机编队表演、多无人机环境探索等。多无人机进行配合需要获取相互的位置信息，用于运动规划和避免相互碰撞，从而完成指定的任务。因此相对位姿估计对与多无人机协作任务具有重要意义。
[0003]
无人机的相对位姿获取可分为两种，一种依赖gps(global positioning system，全球定位系统)或gps-rtk(gps-real-time kinematic，一种采用载波相位动态实时差分的定位方法)提供的位姿信息，一种是无人机主动感知周围环境的相对位姿估计方法。gps可以为四旋翼提供定位信息，但其定位精度通常只能达到米级，并且gps的动态响应能力差，容易因建筑物遮挡等外界因素丢失卫星信号，因此不能满足多机协作的要求。而gps-rtk定位精度高，可以达到厘米级别，但是成本高昂，并且需要提前架设基站，在很多场景中无法使用。而无人机主动感知周围环境的相对位姿估计方法依赖自身传感器，对外部依赖程度小，适应性强，因此很多研究人员对此开展了研究。
[0004]
文献(c.forster,m.pizzoli and d.scaramuzza.svo:fast semi-direct monocular visual odometry[c].ieee international conference on robotics and automation(icra),hong kong,2014:15-22.)提出一种半直接的单目视觉里程计算法，通过摄像头采集环境图像，利用帧与帧之间特征点的光度误差计算帧间位姿，并对多个关键帧之间的特征点重投影误差进行优化，得到更精确的无人机位姿估计。该方法速度快，适合无人机的实时位姿估计，但其位姿估计的全局坐标系由算法进行初始化时决定，在多无人机协作飞行时，各飞机的全局坐标系不是统一的，因此无法直接计算无人机之间的相对位姿。
[0005]
文献(马俊杰,黄大庆,仇男豪,等.基于合作目标识别的无人机相对位姿估计[j].电子设计工程,2020,028(010):1-6.)提出的基于合作目标识别的无人机相对位姿估计方法，设计了由大写字母h和圆形边缘组成的合作目标，并使用改进的yolo网络在摄像头图像中提取合作目标，结合透视投影原理，解算无人机与合作目标的位置关系。该方法将无人机的定位问题转换为合作目标的检测识别任务，从而可完成无人机相对于合作目标的位姿估计。但是该方法需要事先安置合作目标装置，并且无人机活动范围受合作目标装置的制约，因此无法扩展到无固定合作目标的多无人机相对位姿估计。


技术实现要素：

[0006]
为解决现有技术不足，本发明提供一种基于led圆环检测的四旋翼无人机相对位姿估计方法。通过在四旋翼无人机桨叶的上表面安置led灯，利用四旋翼桨叶的旋转特性构
造led圆环，位于上方的无人机通过下视的摄像头对led圆环进行检测，进而估计出相对位姿。本发明所需要硬件简单，led灯和摄像头均为常见硬件，不需布置外部设备，适应性强。
[0007]
本发明技术方案：
[0008]
一种基于led圆环检测的四旋翼无人机相对位姿估计方法，步骤如下：
[0009]
(1)四旋翼无人机桨叶led圆环的设计
[0010]
四旋翼无人机的四个电机相连可以构成一个矩形，在飞行过程中电机带动桨叶高速转动提供无人机运动的动力。利用此特性，在四个桨叶的上表面距离旋转中心r处安置小型led灯及供电电路。在四旋翼无人机起飞前点亮led灯，当桨叶高速运动时，从上方俯视将观察到四个分别以四个电机为中心、以r为半径的led圆环。为了辨别机头的方向，可对led圆环的颜色进行调整，比如将四旋翼无人机机头右前方的led圆环设置成特定的颜色，以区别其他三个圆环，从而标志出机头的方向。
[0011]
(2)led圆环的检测
[0012]
首先，需要对机载摄像头相机进行标定，此摄像头将获取视野中其他四旋翼无人机的led圆环图片。通过标定获取摄像头的内参和畸变参数等，并联合标定摄像头和四旋翼无人机，得到摄像头坐标系和四旋翼无人机机体坐标系的转换矩阵。
[0013]
位于上方的四旋翼无人机的下视摄像头将拍摄到下方四旋翼无人机及其桨叶上表面的led圆环，利用图像处理的方法从摄像头图像中提取圆环。首先，去除图像中噪声的干扰，对摄像头获取的图片进行高斯滤波去噪，并使用canny算子进行边缘检测。其次，利用霍夫变换检测初步提取出图片中的led圆环边缘，由于真实场景中可能会存在其他圆形物体，因此需要对提取的圆环进行筛选。无人机的四个圆环具有相同的尺寸，并且圆心位置比较靠近，因此对初步提取出的圆环进行半径筛选，并对每个圆环计算其周围圆环的数量，最终筛选出无人机的四个led圆环。在原图像中对四个圆的颜色进行检测，以确定机头的朝向。
[0014]
(3)相对位姿估计
[0015]
对无人机建立机体坐标系，通过物理测量的方法，获取桨叶的旋转中心在机体坐标系中的三维坐标。在摄像头图像中提取到的led圆环的中心即是桨叶中心在二维图像中的投影。将桨叶中心的三维坐标、图像中对应的二维坐标、摄像头的内参以及畸变参数，代入pnp(pespective-n-point)模型求解方程，即可解出上方四旋翼无人机的下视摄像头在下方被检测圆环的无人机的机身坐标系中的位姿，再结合摄像头与机身的联合标定得到的坐标系转换矩阵，即可得到下方四旋翼无人机在上方四旋翼无人机机身坐标系中的位姿。设四旋翼无人机a位于四旋翼无人机b的上方，a通过下视摄像头c估计b的相对位姿，空间某一点p在a的机身坐标系的坐标为p
a
，在b的机身坐标系中的坐标为p
b
，在c的坐标系中的坐标为p
c
，c坐标系与a坐标系之间的转换矩阵为r
ac
。通过联合标定摄像头和四旋翼无人机得到如下转换关系：
[0016]
p
a
＝r
ac
p
c
[0017]
而通过求解pnp问题，可得到如下转换关系，其中r
bc
表示坐标系c与坐标系b之间的转换矩阵：
[0018]
p
b
＝r
bc
p
c
[0019]
由这两个公式可得到p
a
与p
b
之间的转换关系，也就是a、b两个机身坐标系之间的转
换关系，即a、b间的相对位姿，如下式所示，其中r
′
bc
表示r
bc
的逆矩阵：
[0020]
p
a
＝r
ac
r
′
bc
p
b
[0021]
本发明的有益效果：本发明设计的桨叶led圆环特征明显，易于提取，增强了相对位姿估计的鲁棒性和速度。同时，所需的传感器等硬件成本较低，重量小，无人机有更好的续航能力。
附图说明
[0022]
图1为本发明的流程图。
[0023]
图2为led圆环实拍图片。
[0024]
图3为图像边缘提取结果。
[0025]
图4为led圆环提取及筛选结果。
[0026]
图5为圆环提取结果与原图像对比。
具体实施方式
[0027]
以下结合技术方案和附图详细叙述本发明的具体实施方式。
[0028]
本发明选择使用的四旋翼无人机包含pixhawk4飞控、摄像头、机载计算机(arm或x86)、安置有led等的桨叶等。其中，摄像头负责获取led圆环图像，机载计算机负责图像处理、坐标系转换计算等，桨叶上表面的led灯将在桨叶旋转时显示出圆环图案。
[0029]
(1)四旋翼无人机桨叶led圆环的设计
[0030]
为提高检测精度，桨叶上的led圆环半径应尽可能大，led距离桨叶旋转中心的距离即是led圆环的半径，因此led灯安置在远离桨叶中心的桨叶上表面位置。四个桨叶led圆环的半径都设置为r＝7.7cm(r值由测量得出)，设置机身右前方的led圆环颜色与其他三个圆环颜色不同以标识机头方向。四旋翼无人机起飞前打开led灯开关，在飞行过程中，从上方俯视将看到led灯旋转构成的圆环，实景照片如图2所示。
[0031]
(2)led圆环检测
[0032]
位于上方的四旋翼无人机通过摄像头可观测到下方无人机桨叶的led圆环，图片中的圆环包含下方无人机四个电机的二维位置信息和机身的朝向信息。由于图片中存在噪声干扰，首先对图片进行高斯滤波去噪。其次使用canny算子对去噪后的图像进行边缘检测等处理。边缘检测的结果如图3所示。在边缘检测的结果图上利用霍夫变换提取圆环，由于场景中可能会有圆形物体以及环境复杂时会出现误提取，因此需要对圆环进行筛选。首先，桨叶上表面的四个led圆环半径相同，利用半径尺寸信息对圆环进行第一次筛选，如果一个圆环找不到其他三个半径相同(或半径相差在一定阈值内)的圆环，那么将此圆环删除。其次，led圆环位置相近，设置距离阈值为n＝6(n的数值根据无人机硬件尺寸决定)倍的圆环半径，如果一个圆环与其他圆环的圆心距离小于阈值，说明这两个圆环可能是四个led圆环中的两个，进行记录。如果一个圆环能找到三个满足此条件的其他圆环，说明此圆环是led圆环。圆环的中心即是桨叶旋转中心在图片中的二维投影。led圆环的检测结果如图4所示，led圆环检测结果与原图像的对比如图5所示。
[0033]
(3)相对位姿估计
[0034]
通过对四旋翼无人机和下视摄像头进行联合标定，可得到无人机机体坐标系和摄
像头坐标系的转换关系。通过物理测量可得到四个桨叶的旋转中心在机身坐标系中的三维坐标。对下视摄像头进行标定得到其内参和畸变参数等。
[0035]
至此，通过将下视摄像头内参和畸变参数、四个电机的三维坐标、四个电机在图像中的二维投影坐标(led圆环的中心)代入pnp问题模型，即可求解出下视摄像头与下方无人机机身坐标系的相对位姿关系，而下视摄像头与四旋翼无人机联合标定的时候已经得到了二者坐标系之间的转换关系，因此，可推导出上下两个四旋翼无人机之间的位姿关系，即估计出两个无人机的相对位姿。以图5为例，相对位姿的计算结果为(x＝0.38m,y＝0.39m,z＝-4.43m,roll＝0.02
°
,pitch＝0.32
°
,yaw＝5.3
°
)，计算时间为18ms，满足实时计算要求。