小型無人旋翼機自主著陸位姿估計方法
【專利摘要】本發明涉及一種小型旋翼機自主著陸位姿估計方法。該方法通過搭載裝有電動可調焦液體鏡頭的相機來解決旋翼機在著陸過程中由于機載相機所拍攝圖像出現離焦現象而出現位姿估計不準的問題，提高無人旋翼機的著陸精度。其實現操作步驟為:（1）離線標定相機可變內參;（2）根據著陸地標特征點在線自標定得到相機外參數獲取無人旋翼機相對于著陸地標的位姿。該方法可保證無人旋翼機在自主著陸過程中的位姿估計都是基于對焦圖像，能夠提高其在著陸過程中的位姿估計精度，增加視覺輔助著陸范圍，保障了無人旋翼機自主著陸的準確性，提高了著陸系統的安全性。
【專利說明】
小型無人旋翼機自主著陸位姿估計方法
技術領域
[0001] 本發明涉及無人飛行器技術領域，尤其是一種小型旋翼機自主著陸位姿估計方 法，著重于基于單目視覺信息的UAV姿態參數估計。 技術背景
[0002] 小型旋翼機因為可以自由地實現懸停以及空間中的自由移動，具有很大的靈活 性。此外，因為它結構簡單，機械穩定性好，所以成本低廉、性價比很高。主要應用于玩具、航 模、航拍，新的應用也在不斷的拓展之中。小型旋翼機的自主著陸是指依賴機載的導航設備 和飛行控制系統來進行定位導航并最終控制旋翼機在特定著陸區域降落的過程。這極大得 拓展了小型旋翼機被應用于執行任務過程中的靈活性和智能性。而要實現自主著陸，小型 旋翼機就必須具備自主導航能力，需要實時地估計其相對于特定著陸區域的姿態參數。在 著陸控制過程中利用單目視覺提取著陸地標的特征點利用此圖像特征來進行位置姿態估 計成為近來小型旋翼機自主著陸過程中位姿估計常用到的一種方法，但是由于往往用于相 機景深的限制，位姿估計的地標圖像并非是成像最清晰的圖像甚至是模糊的圖像這就導致 位姿估計存在一定的偏差，而位姿估計的精度直接影響到旋翼機的控制精度。
【發明內容】
[0003] 為了克服由于著陸地標圖像離焦而導致的位姿估計存在偏差的問題，本發明提供 一種可有效提高基于視覺信息的小型旋翼機自主著陸的位姿估計的方法。 為達到上述目的，本發明的構思是:在旋翼機自主著陸的過程中通過自動變焦技術使 機載相機拍攝到的進行位姿估計的著陸地標圖像都是最佳的對焦圖像，而傳統的自動變焦 系統因體積龐大很難在小型旋翼機上得到應用，本發明采用一種電動可調焦液體鏡頭來實 現機載相機的自動對焦，這種電動可調焦液體鏡頭因為沒有任何機械移動部件僅靠 USB串 口驅動器產生不同的電流便可改變鏡頭形狀從而實現變焦，因而使整個變焦系統結構緊 湊，功耗較小。而由于鏡頭焦距的變化帶來的成像系統內參的變化可先行離線標定建立內 參表。在自主著陸過程中僅需將相應內參帶入公式即可計算出相機外參即旋翼機相對于著 陸地標的位姿。
[0004] 根據上述發明構思，本發明采用下述技術方案： 一種小型無人旋翼機自主著陸位姿估計方法，其特征在于包含以下步驟： (1)遍歷機載相機鏡頭焦距，拍攝清晰的標定圖像，計算出標定圖像中特征點的圖像坐 標，帶入計算公式獲取機載相機內部參數表，包含以下步驟： 1. 改變電動可調焦液體鏡頭驅動電流，改變其焦距：由USB串口驅動器通過改變其驅動 電流來改變電動可調焦液體鏡頭的焦距； 2. 使用圖像清晰度評價函數，確定標定板成像最清晰時的位置，采集此時的標定板圖 像:清晰度評價函數是對圖像的清晰度進行定量描述的函數，典型的清晰度評價函數曲線 如圖4所示。當電動可調焦液體鏡頭焦距確定以后可通過前后移動標定板來確定標定板成 像最清晰時的位置，采集此處標定板的圖像，改變標定板位姿得到此處不同視角的標定板 圖像(建議多于5幅）； 3.根據從多幅標定板圖像中提取到的特征點的像素坐標，及已知的標定板上特征點的 物理坐標計算相機的內部參數;重復步驟1至3直至遍歷全部鏡頭焦距，從而建立起相 機的內部參數表。
[0005] (2)外參數在線標定，獲取旋翼機著陸位姿:無人旋翼機著陸過程中，采集清晰著 陸地標圖像，計算著陸地標特征點圖像坐標，帶入公式計算出旋翼機的著陸位姿： 1. 在旋翼機著陸的過程中，利用合焦的著陸地標圖像計算旋翼機此時相對于著陸地 標的位姿相對于傳統定焦相機景深范圍內拍攝得到的著陸地標圖像估計可以提高位姿的 估計精度，利用基于電動可調焦鏡頭的自動對焦方法包含以下步驟： a .確定液態鏡頭驅動器電流改變方向； b. 按照確定的電流改變方向通過驅動器改變方向通過驅動器改變電動可調焦鏡頭驅 動電流； c. 利用清晰度評價函數對電流改變前后對焦區域圖像的清晰度值進行評價，判斷清 晰度值是否連續出現下降的情況，若是則進入步驟d，否則返回步驟b; d. 改變驅動器電流的改變方向，減小電流該變量； e. 判斷電流改變量是否達到一預設最小值，若是則液體鏡頭驅動器此時驅動電流對 應的焦距即為該對焦控制方法找到的成像系統拍攝目標物合焦時的焦距，記錄下此時的驅 動電流，結束對焦。否則，返回步驟b; 2. 利用步驟①中得到的合焦時的驅動電流及步驟(1)中得到的相機內部參數表，查到 此時的相機內部參數； 3. 通過拍攝得到的著陸地標圖像，進行特征點的提取，獲取這些特征點的圖像坐標， 再根據已知的特征點在物理世界中的坐標帶入公式計算出映射矩陣，并根據步驟②中得到 的相機內參計算出相機的外參，即此時旋翼機相對于著陸地標的位姿。
[0006] 實施本發明與現有技術相比較，具有如下顯而易見的突出實質性特點和顯著技術 進步:本發明用于旋翼機著陸位姿估計的著陸地標圖像皆是合焦圖像，可以有效減少旋翼 機在著陸過程中利用單目視覺進行位姿估計的誤差，所得數據用于旋翼機的著陸控制中可 以實現旋翼機的自主精確著陸，所述基于電動可調焦液體鏡頭的合焦著陸地標獲取方法， 對焦速度快，對焦準確可以滿足旋翼機的自主實時著陸控制。
【附圖說明】
[0007] 圖1為本發明實施例一提供的一種小型旋翼機自主著陸位姿估計方法 圖2為本發明實施例二提供的一種小型旋翼機自主著陸位姿估計方法 圖3a為本發明實施例二用于獲取相機內部參數表的標定板示意圖 圖3b為本發明實施例二所設計的著陸地標圖像 圖4 一典型的清晰度評價函數曲線 圖5為實施例的自動對焦方法的流程圖。
【具體實施方式】
[0008] 為使本發明的目的、技術方案、和優點更加清楚，下面結合附圖對本發明的優選實施例 進一步地詳細描述。
[0009] 實施例一 參見圖1，本發明實施例提供了一種小型旋翼機自主著陸過程中的位姿估計方法，包 括： 101:遍歷機載相機鏡頭焦距，在相機每一焦距下拍攝若干幅不同視角清晰的標定圖 像，根據從標定圖像中提取到的特征點及其對應的物理世界坐標得到相機的內參，建立起 相機的內部參數表； 其中，在具體實現101中包括:利用清晰度評價函數確定在當前相機焦距下標定板在待 標定相機中成像最清晰時的位置;在該位置上拍攝若干幅不同視角的標定板圖像;從這些 圖像上進行特征點的提取，并根據相應的三維空間坐標計算出相機在此焦距下的內部參 數，最終形成該相機的內部參數表。
[0010] 102:在旋翼機著陸過程中，通過自動對焦算法獲取清晰著陸地標圖像，根據從著 陸地標圖像中提取到的特征點坐標及其在物理世界中的坐標計算出相機外參即旋翼機的 位姿，用于飛控系統精確控制旋翼機的精確著陸； 其中，在具體實現102中包括:利用基于電動可調焦液體鏡頭的自動對焦方法來實時 獲取清晰的著陸地標圖像;從著陸地標圖像上提取特征點，確定它們的圖像坐標并根據它 們已知的物理世界的坐標計算計算出相機外參即旋翼機的位姿，用于飛控系統精確控制旋 翼機著陸。
[0011] 本發明實施例利用基于電動可調焦液體鏡頭的機載相機自動對焦方法使得旋翼 機在著陸過程中采集到的用于位姿估計的著陸地標圖像皆是合焦圖像，可以有效減少旋翼 機在著陸過程中利用單目視覺進行位姿估計的誤差，所得數據用于旋翼機的著陸控制中可 以實現旋翼機的自主精確著陸，其中基于電動可調焦液體鏡頭的自動對焦方法，對焦速度 快，對焦準確可以滿足旋翼機的自主實時著陸控制。
[0012]實施例二 參見圖2,本發明實施例二提供了一種小型旋翼機自主著陸過程中的位姿估計方法，包 括： 20:獲取相機內部參數表： 包括： 201:改變相機焦距，根據清晰度評價函數確定標定板成像清晰時所在位置:圖像的清 晰度可由清晰度評價函數來進行評判，參見圖4為一典型的清晰度評價函數曲線，圖中曲線 峰值處所對應的標定板位置可認為是標定板在相機當前焦距下成像最清晰時的位置。常用 的圖像清晰度值計算方法有很多如灰度變化函數、梯度函數、圖像灰度熵函數、頻域類函數 等。在本實施例的成像系統中對不同模糊程度成像的圖像進行評價后發現"歸一化方差法" 產生的評價曲線較其它計算方法具有更好的單峰性、抗噪性，的計算公如下：
其中，H為圖像的高度，W位圖像的寬度，y位圖像的平均像素值。
[0013] 202:拍攝不同視角的標定板圖像:參見圖3a為本實施例中所用到的標定板，需要 以不同位姿擺放標定板進行拍攝(建議大于等于5幅）以減小相機內參的標定誤差，由于標 定板位置距離相機較遠，可自己制作較大的標定板圖像用于標定，圖3a為一方格尺寸為 8cm，角點數位9 X 6的棋盤格標定板。
[0014] 203:提取特征點并計算對應點的圖像坐標:標定板中的棋盤格角點較明顯，比較 容易提取。可以利用Harris、Shi-Tomasi等角點檢測算法對角點進行提取，并得到角點相應 的圖像坐標。
[0015] 204:計算相機內參，最終得到該相機的可變內部參數表：由步驟203中得到的角 點圖像坐標及其已知的在物理世界中的坐標，可由張氏標定法得到相機內參，并最終建立 起與相機焦距(電動可調焦液體鏡頭驅動電流)想對應的相機內部參數表。
[0016] 21:獲取旋翼機著陸位姿： 包括： 211:使用自動對焦算法獲取清晰著陸地標圖像:本實施例的自動對焦算法包括以下 步驟： (1) 選擇對焦區域 (2) 對焦 (3) 計算對焦區域清晰度值 (4) 計算計算該清晰度值與上次對焦成功時儲存的圖像清晰度值的絕對差值與其百分 比:若百分比超出預設閾值，則返回步驟(2)。其中步驟(2)包括以下步驟，參閱圖5: a. 確定液態鏡頭驅動器電流改變方向，計算當前對焦區域的清晰度值記為vl，然后在 當前液體鏡頭驅動電流的基礎上增加一預設較大值large (a)并計算對焦區域的清晰度值 記為v2,若v2大于vl則確定驅動器電流改變方向為正向（增大液體可變焦鏡頭驅動電流，減 小其焦距），否則驅動器電流改變方向為負向（減小液體可變焦鏡頭驅動電流，增大其焦 距）； b. 按照步驟a中確定的驅動器電流改變方向以預設變量medium來改變驅動器驅動電 流； c. 對電流改變前后對焦區域圖像的清晰度值進行評價直至清晰度值連續n次出現下 降的情況，若是則說明成像系統已經錯過最佳對焦焦距，進入步驟d，否則返回步驟b; d. 改變驅動器電流改變方向，并在medium的基礎上減小一值step作為驅動器的電流 改變量； e. 判斷medium是否達到一最小值minimum，若是則液體鏡頭驅動器此時驅動電流對應 的焦距即為該對焦控制方法尋找到的成像系統成像系統最佳時的焦距，記錄下此時的驅動 電流，結束對焦。否則返回步驟b; 根據實驗數據，large = 31mA, medium = 21mA，step = 5mA，n = 2，minimum = lmA〇
[0017] 212:提取特征點并計算對應點的圖像坐標:標定板中的棋盤格角點較明顯，比較 容易提取。可以利用Harris、Shi-Tomasi等角點檢測算法對角點進行提取，并得到角點相應 的圖像坐標。參閱圖3b，為本實施例所設計的"H"型著陸地標，共計12個特征點； 213:由步驟212中得到的角點圖像坐標及其已知的在物理世界中的坐標，計算出映射 矩陣H，再由步驟211中記錄的驅動電流從步驟20中建立起來的相機內部參數表中查到對應 的相機內參A，由張氏標定法計算出相機的外部參數，即旋翼機的位姿。
[0018]本發明實施例利用基于電動可調焦液體鏡頭的機載相機自動對焦方法使得旋翼 機在著陸過程中采集到的用于位姿估計的著陸地標圖像皆是合焦圖像，可以有效減少旋翼 機在著陸過程中利用單目視覺進行位姿估計的誤差，所得數據用于旋翼機的著陸控制中可 以實現旋翼機的自主精確著陸，其中基于電動可調焦液體鏡頭的自動對焦方法，對焦速度 快，對焦準確可以滿足旋翼機的自主實時著陸控制。
【主權項】
1. 一種小型旋翼機自主著陸位姿估計新方法，其特征在于使用配有電動可調焦液體鏡 頭的機載相機解決由于旋翼機著陸過程中拍攝的著陸地標圖像可能出現的離焦現象而引 起的位姿估計不準確的問題，提高無人旋翼機的著陸精度，其實現操作步驟如下： (1) 遍歷機載相機鏡頭焦距，拍攝清晰的標定圖像，計算出標定圖像中特征點的圖像坐 標，帶入計算公式獲取機載相機內部參數表； (2) 外參數在線標定，獲取旋翼機著陸位姿:無人旋翼機著陸過程中，采集清晰著陸地 標圖像，計算著陸地標特征點圖像坐標，帶入公式計算出旋翼機的著陸位姿，無人機的著陸 位姿信息傳送給飛控系統實現旋翼機的準確著陸。2. 根據權利要求1所述的基于裝有電動可調焦液體鏡頭機載相機的小型旋翼機自主著 陸位姿估計方法，其特征在于所述步驟(1)相機內部參數表的獲取采用下列具體步驟： a. 改變電動可調焦液體鏡頭驅動電流，改變其焦距； b. 使用圖像清晰度評價函數，確定標定板成像最清晰時的位置，采集此時的標定板圖 像； c. 根據從標定板中提取到的特征點的像素坐標，帶入公式計算此時的相機內部參數； 重復步驟a至c直至遍歷全部鏡頭焦距，從而建立起相機的內部參數表。3. 根據權利要求1所述的基于裝有電動可調焦液體鏡頭機載相機的小型旋翼機自主著 陸位姿估計方法，其特征在于所述步驟(2)旋翼機著陸位姿獲取采用下列具體步驟： a. 利用圖像自動對焦方法在旋翼機自主著陸過程中始終使著陸地標成像清晰； b. 根據清晰標定圖像拍攝時記錄的驅動電流值，在已建立起來的相機內參表中查到此 時相機的內參，并根據從著陸地標中提取到的特征點對應的像素坐標計算相機外參，即得 到旋翼機在著陸過程中相對于著陸地標的位姿，旋翼機的飛控系統可利用此位姿信息實現 精準的著陸控制。4. 根據權利要求3所述的小型旋翼機自主著陸位姿估計方法，其特征在于所述步驟（1) 中的對焦控制算法，所采用的著陸地標成像最清晰時的焦距搜索算法包括以下步驟： a. 確定液態鏡頭驅動器電流改變方向； b. 按照確定的電流改變方向通過驅動器改變電動可調焦鏡頭驅動電流； c. 對電流改變前后對焦區域圖像的清晰度值進行評價判斷清晰度值是否連續出現下 降的情況，若是進入步驟d否則返回步驟b; d. 改變驅動器電流改變方向，減小電流改變量； e. 判斷電流改變量是否達到一預設最小值，若是則液體鏡頭驅動器此時驅動電流對 應的焦距即為該對焦控制方法尋找到的成像系統成像系統最佳時的焦距，結束對焦，否則 返回步驟b。
【文檔編號】G05D1/08GK105929837SQ201610254133
【公開日】2016年9月7日
【申請日】2016年4月23日
【發明人】李恒宇, 劉航, 程洪濤, 謝少榮, 羅均
【申請人】上海大學