[發明專利]視覺輔助微型無人機室內飛行三維姿態算法

在審

• 申請號: 201911415820.6
• 申請日: 2019-12-31
• 公開（公告）號: CN111024091A
• 公開（公告）日: 2020-04-17
• 發明（設計）人: 李德輝;王冠林;史海慶;鄧成端
• 申請（專利權）人: 北京智新翼行科技有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 常州信策知識產權代理事務所(普通合伙) 32352
• 代理人: 于桂賢

本發明提供一種視覺輔助微型無人機室內飛行三維姿態算法，可以實現無人機室內飛行時俯仰角、橫滾角和航向角的純自主解算。當磁力計數據有效時，利用IMU和磁力計的數據實現無人機三維姿態的解算，并實現水平姿態角解算和航向角解算的解耦；當磁力計數據失效時，僅用IMU數據實現三維姿態解算，并切斷加速度計對航向角解算的影響，確保航向角解算在無磁力計輔助時維持續航時間內精度要求；當無人機在室內走廊飛行時，通過前視單目相機模塊實現狹道檢測，提取出無人機偏離廊道線的角度，并用此偏離角通過擴展卡爾曼濾波實現對無人機航向角累積誤差的修正。

技術領域

本發明涉及無人機技術領域，特別是涉及一種視覺輔助微型無人機室內飛行三維姿態算法。

背景技術

微型無人機由于其體積小、重量輕和隱蔽的特點，非常適用于室內環境的飛行應用，如室內偵查、搜救和環境探測等。但微型無人機也因為自身體積、重量和計算能力的限制，無法采用體積、重量和功耗有更高要求的導航傳感器以及計算量大的視覺導航算法來實現室內自主飛行。同時，室內磁場環境復雜，磁力計在室內應用受到很大限制，無人機的姿態解算將會受到干擾、精度降低，甚至航向角解算錯誤。因此，微型無人機室內飛行時，三維姿態角的解算也是一個需要解決的關鍵問題。

發明內容

本發明所要解決的技術問題是：為了克服現有技術中的不足，本發明提供一種視覺輔助微型無人機室內飛行三維姿態算法。

本發明解決其技術問題所要采用的技術方案是：一種視覺輔助微型無人機室內飛行三維姿態算法，包含磁場正常環境下無人機三維姿態角的解算，在解算過程中對水平姿態角和航向角解算進行解耦；磁場異常環境下無人機三維姿態角的解算；以及走廊飛行視覺輔助航向角修正；具體包括以下步驟：

S1：無人機起飛前機頭朝向與室內任一走廊方向平行，上電初始化；獲取IMU和三軸磁力計輸出數據；同時，視覺模塊利用狹道檢測方法對前方廊道開始檢測，并計算無人機偏離廊道線的角度；其中，視覺模塊包括前視單目相機和圖像處理計算機。

S2：根據磁力計輸出計算出起飛點地磁感應強度，將地磁感應強度數值與設定范圍比較，當起飛點地磁感應強度在設定范圍以內，則判定為飛行環境磁場正常，以磁力計給出的磁航向作為初始航向角，進入飛行狀態，并執行S3；當起飛點地磁感應強度在設定范圍以外，則判定為飛行環境磁場異常，將初始航向角設為0，進入飛行狀態，并執行S4；

S3：無人機在磁場正常環境下飛行過程中，計算飛行中的地磁感應強度，并判斷飛行途中地磁感應強度是否在設定范圍內，如果不在設定范圍內則判定為飛行環境磁場由正常轉為異常，進入步驟S4；如果在設定范圍內，則利用IMU與磁力計共9軸傳感器對無人機三維姿態角進行測量，并對水平姿態角解算和航向角解算進行解耦，然后進入步驟S5；

S4：只利用IMU數據解算姿態四元數和三維姿態角，然后進入步驟S5；

S5：讀取步驟S1中視覺模塊輸出的無人機偏離廊道線的角度，判斷是否有視覺航向測量更新，如果有更新，則進行EKF數據融合實現航向角修正并輸出三維姿態；如果無更新，則直接輸出三維姿態角。

進一步，步驟S1中計算無人機偏離廊道線角度的過程具體為：無人機在走廊飛行時，通過前視單目相機對前方廊道進行拍照獲得前視圖像，并將前視圖像發送至圖像處理計算機，圖像處理計算機利用前視圖像通過Hough變換實現狹道檢測，從而得到像素坐標系中廊道線的直線方程；再結合無人機俯仰角數據以及相機標定得到的內參數，通過三角關系，計算出無人機偏離廊道線的角度。

進一步，步驟S2中判斷磁場是否正常包括以下步驟：

步驟S2.1：對磁力計的輸出進行標定，獲得正常地磁感應強度H_E，并將H_E±5％作為地磁感應強度的設定范圍，將該設定范圍保存在無人機中；