[发明专利]一种基于LED圆环检测的四旋翼无人机相对位姿估计方法

权利要求书

1.一种基于LED圆环检测的四旋翼无人机相对位姿估计方法，其特征在于，步骤如下：

(1)四旋翼无人机桨叶LED圆环的设计

四旋翼无人机的四个电机相连构成一个矩形，在飞行过程中电机带动桨叶高速转动提供无人机运动的动力；利用此特性，在四个桨叶的上表面距离旋转中心r处安置小型LED灯及供电电路；在四旋翼无人机起飞前点亮LED灯，当桨叶高速运动时，从上方俯视将观察到四个分别以四个电机为中心、以r为半径的LED圆环；对LED圆环的颜色进行调整，以辨别机头的方向；

(2)LED圆环的检测

首先，需要对机载摄像头相机进行标定，此摄像头将获取视野中其他四旋翼无人机的LED圆环图片；通过标定获取摄像头的内参和畸变参数，并联合标定摄像头和四旋翼无人机，得到摄像头坐标系和四旋翼无人机机体坐标系的转换矩阵；

位于上方的四旋翼无人机的下视摄像头将拍摄到位于下方的四旋翼无人机及其桨叶上表面的LED圆环，利用图像处理的方法从摄像头图像中提取圆环；首先，去除图像中噪声的干扰，对摄像头获取的图片进行高斯滤波去噪，并使用Canny算子进行边缘检测；其次，利用霍夫变换检测初步提取出图片中的LED圆环边缘，利用四旋翼无人机的四个圆环具有相同的尺寸，以及圆心位置相对靠近的特点，对边缘提取后的圆环进行筛选，除去真实场景中存在其他圆形物体，筛选出无人机的四个LED圆环，在原图像中对四个圆的颜色进行检测，以确定机头的朝向；

(3)相对位姿估计

对四旋翼无人机建立机体坐标系，通过物理测量的方法，获取桨叶的旋转中心在机体坐标系中的三维坐标；在摄像头图像中提取到的LED圆环的中心即是桨叶中心在二维图像中的投影；将桨叶中心的三维坐标、摄像头图像中对应的二维坐标、摄像头的内参以及畸变参数，代入PnP模型求解方程，即可解出上方四旋翼无人机的下视摄像头在下方被检测圆环的无人机的机身坐标系中的位姿，再结合摄像头与机身的联合标定得到的坐标系转换矩阵，即可得到下方四旋翼无人机在上方四旋翼无人机机身坐标系中的位姿。

2.根据权利要求1所述的一种基于LED圆环检测的四旋翼无人机相对位姿估计方法，其特征在于，所述步骤(2)中，对边缘提取后的圆环进行筛选包括两种方式：

(1)根据四个圆环具有相同的尺寸的特点进行筛选，具体为：计算每一个圆环i与其他圆环的半径之比，如果比值在0.9～1.1之间，视为两圆环半径近似相等，如果与圆环i半径近似相等的圆环数量小于3，说明圆环i不是无人机桨叶上的LED圆环，筛除圆环i；

(2)根据圆心位置相对靠近的特点进行筛选，具体为：计算每一个圆环i的圆心与其他圆环圆心的欧式距离，如果计算结果小于圆环i半径的6倍，视为两圆环位置相对靠近，如果与圆环i位置相对靠近的圆环数量不等于3，说明圆环i不是无人机桨叶上的LED圆环，删除圆环i。

3.根据权利要求1或2所述的一种基于LED圆环检测的四旋翼无人机相对位姿估计方法，其特征在于，所述步骤(3)中，求解位姿时，上方四旋翼无人机、下方被检测圆环的无人机、上方四旋翼无人机下视摄像头三者坐标系之间的转换矩阵根据如下步骤获得：

设四旋翼无人机A位于四旋翼无人机B的上方，A通过下视摄像头C估计B的相对位姿，空间某一点P在A的机身坐标系的坐标为P_A，在B的机身坐标系中的坐标为P_B，在C的坐标系中的坐标为P_C，C坐标系与A坐标系之间的转换矩阵为R_AC；通过联合标定摄像头和四旋翼无人机得到如下转换关系：

P_A＝R_ACP_C

而通过求解PnP问题，可得到如下转换关系，其中R_BC表示坐标系C与坐标系B之间的转换矩阵：

P_B＝R_BCP_C

由这两个公式可得到P_A与P_B之间的转换关系，也就是A、B两个机身坐标系之间的转换关系，即A、B间的相对位姿，如下式所示，其中R′_BC表示R_BC的逆矩阵：

P_A＝R_ACR′_BCP_B。