[發明專利]一種基于LED圓環檢測的四旋翼無人機相對位姿估計方法

有效

• 申請號: 202011135322.9
• 申請日: 2020-10-22
• 公開（公告）號: CN112308900B
• 公開（公告）日: 2022-10-21
• 發明（設計）人: 閆飛;初玉濱;莊嚴
• 申請（專利權）人: 大連理工大學
• 主分類號: G06T7/62
• 分類號: G06T7/62;G06T7/73;G06T7/80;G06F30/15;B64C27/08
• 代理公司: 大連理工大學專利中心 21200
• 代理人: 劉秋彤;梅洪玉

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。