[發明專利]一種基于圓特征及P3P算法的飛行器位姿測量方法

在審

• 申請號: 201910705090.7
• 申請日: 2019-07-30
• 公開（公告）號: CN110500954A
• 公開（公告）日: 2019-11-26
• 發明（設計）人: 張莉君;郭輝;陳鵬;黃順
• 申請（專利權）人: 中國地質大學(武漢)
• 主分類號: G01B11/00
• 分類號: G01B11/00;G01C21/20;G06T7/70;G06T7/80;G06T7/136
• 代理公司: 42238 武漢知產時代知識產權代理有限公司
• 代理人: 鄒桂敏<國際申請>=<國際公布>=<進入

1.一種基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，包括以下步驟：

S1、利用單目相機拍攝飛行器的圖像，所述飛行器上布置有四個用于特征識別及提取的靶標，其中，確定每個靶心在世界坐標系中的坐標；

S2、對步驟S1中的圖像進行平滑濾波，然后利用閾值分割方法處理得到二值圖像；

S3、根據步驟S2中得到的二值圖像中的輪廓中的像素點的數量篩選出靶標的輪廓；

S4、在像素坐標系中，利用所述步驟S3中得到的靶標的輪廓，計算靶心的坐標；

S5、任意選取三個靶標，根據步驟S4中得到的像素坐標系中的靶心的坐標以及相機內參數、步驟S1中確定的世界坐標系中的靶心的坐標，計算位姿參數解集；

S6、構造最小化重投影誤差函數，利用未被選取的靶標的坐標信息，校驗步驟S5中解算的位姿參數解集，得到最接近真實情況的唯一解；

S7、通過步驟S6中得到的唯一解，計算飛行器的姿態參數以及飛行器相對于相機的位置參數。

2.根據權利要求1所述的基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，所述步驟S1中，先對所述單目相機進行標定，得到所述單目相機的內參數，然后利用所述標定后的單目相機進行圖像拍攝，所述單目相機拍攝得到的圖像為灰度圖；其中，所述單目相機與所述飛行器之間的距離控制在一定閾值內，優選為10米。

3.根據權利要求1所述的基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，所述步驟S1中，所述飛行器上布置的靶標為四個半徑不同的圓形含LED燈的有源靶標，其中，按照半徑的大小，對靶標依次編號；所述靶標的布置方式為：任意三個靶標的靶心不共線，四個靶標的靶心的連線呈平行四邊形分布。

4.根據權利要求1所述的基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，所述步驟S2中，采用基于亮度均衡處理的雙閾值分割方法處理圖像得到二值圖像。

5.根據權利要求1所述的基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，所述步驟S3中，所述篩選方式為：計算所述二值圖像中的輪廓個數N，以及每個輪廓上包含的像素點個數[m₁，m₂，…m_N]，設置閾值T₁、T₂，若有T₁＜m_i＜T₂，其中i為整數且1≤i≤N，則保留該輪廓，否則去掉該輪廓。

6.根據權利要求1所述的基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，所述步驟S4中，采用最小二乘擬合圓的方式求取靶心在像素坐標系中的坐標。

7.根據權利要求1所述的基于圓特征及P3P算法的飛行器位姿測量方法，其特征在于，所述步驟S5中，利用P3P算法計算飛行器的位姿參數解集；所述位姿參數解為一組世界坐標系相對于相機坐標系的旋轉矩陣和平移矩陣。