基于人工標志的單目視覺下無人機位姿估計

吳 妍，吳 芬，戚國慶

（南京理工大學 江蘇 南京 210094）

基于人工標志的單目視覺下無人機位姿估計

吳 妍，吳 芬，戚國慶

（南京理工大學 江蘇 南京 210094）

就無人機的自定位問題，文中提出一種數字圖像處理結合2D-3D場景重建的位姿估計算法。在已知人工標志世界坐標的前提下，對于單目視覺下機載非線性相機拍攝的單幀圖像，利用圖像處理技術為位姿解算提供特征點數據。位姿解算提供了一種改進的N點透視算法，該算法結合正交迭代算法，以N點透視算法計算的旋轉矩陣值作為迭代算法初值，迭代得到最終估計結果。通過仿真實驗和實物實驗驗證了算法的可行性。文中提出的算法，較N點透視算法，提高了位姿估計的精度且收斂速度快，給無人機室內位姿估計提供了的一種新的解決思路。

自定位；非線性；人工標志；N點透視；正交迭代

無人機憑借結構穩定、可操作性高、無人駕駛等特點，在航拍勘測、目標定位跟蹤等領域獲得了廣泛應用。在利用無人機對地面/海上等目標的定位與跟蹤、無人機自身位姿控制、無人機測量、無人機自主降落等問題中，無人機自身位姿的確定，成為研究的前提，是不可回避的一大難題。

目前，無人機自身的位姿信息主要依靠GPS[1]和IMU[2]獲取。GPS受環境與氣候的干擾嚴重，尤其在室內，無人機無法接收到GPS信號，因此無法獲取自身的位置信息[3]。而IMU是根據加速度計和陀螺儀的姿態變化積分獲取位姿信息的。實際應用中會產生累積誤差從而降低位姿估計的精度。隨著CCD器材等視覺傳感器的制作工藝日趨精良，利用視覺傳感器獲取的信息進行位姿估計成為一種有利的替代手段[4]。

基于視覺的無人機位姿估計[5-7]，減少了攜帶的設備且價格低；同時，視覺信號穩定，魯棒性強，不存在累積誤差。利用視覺信息實現無人機的位姿估計，無疑是實現無人機自主位姿估計的又一有力技術手段，尤其是在室內無法獲取GPS信號的區域[8-9]。

針對室內無人機的位姿估計，文中提出了一種基于數字圖像處理技術和2D-3D場景重建的位姿解算方法。事先在實驗室內鋪設信息已知的人工標志，利用圖像處理技術檢測并提取人工標志的特征點信息，在此基礎上，借助坐標系轉換以及攝像機成像模型，建立圖像坐標系2維平面到3維世界坐標下的轉換模型，即位姿解算模型，從而解算出無人機自身位姿信息。

1 系統結構

文中以常見的四旋翼無人機作為研究對象，考慮系統的實時性與低復雜性，選用藍色雙三角平面標志物作為人工標志，在無人機上固定相機，對機載相機拍攝的圖片進行圖像處理，將處理結果和已知世界坐標系下的圖像坐標作為位姿解算模型的輸入，解算無人機位姿。假設室內無人機飛行區域中都能通過機載相機觀測到架設的人工標志。位姿估計示意圖如圖1所示。

圖1 位姿估計的示意圖

視覺位姿估計系統由兩個模塊構成：圖像處理以及位姿解算。圖像處理模塊完成人工標志的檢測與提取。利用顏色特征完成人工標志的檢測，借助Harris角點檢測、Hough直線擬合獲取特征點的提取。圖像處理的流程如圖2所示。

圖2 圖像處理流程圖

位姿解算，實際就是對于機載相機的外參數的估計。在完成攝像機內參數標定的前提下，利用點透視算法求解一個粗精度的位姿將其作為正交迭代算法[10]的初始值，經迭代求解位姿。下文對位姿解算原理進行詳細介紹。

2 位姿估計原理

位姿解算，是對世界坐標系下信息已知的人工標志進行成像信息進行特征分析處理求解。這里對坐標系轉換以及攝像機成像原理進行如下說明。

2.1 攝像機模型

攝像機模型，是對攝像機成像過程的數學表達。物體在圖像坐標上的像點與攝像機內部參數以及物體的空間相對位置有關。理想的攝像機模型為針孔模型，但實際中，相機一般由于鏡頭畸變呈非線性。非線性相機模型為：

式（1）中 f為焦距，xc，yc，zc為攝像機坐標系下坐標，xI，yI為圖像坐標系下坐標。 k1，k2為畸變系數，取二階徑向模型。攝像機的焦距，像元尺寸，光心，偏移量，不垂直度，畸變系數等內部參數都是未知的，要實現位姿估計還需對其標定。

2.2 坐標系轉換

位姿解算本質上是2D-3D的場景重建，在此過程中，涉及到世界坐標系、機體坐標系、攝像機坐標系及圖像坐標系的相互轉換。

系統將無人機與攝像機固連，只需解算出攝像機在世界坐標系下的位姿，即認為獲取無人機的位姿。下文的機體坐標系，均以攝像機坐標系取代。就研究涉及的三個坐標系間的轉換關系介紹如下：

1）世界坐標系-攝像機坐標系

世界坐標系到攝像機坐標系的轉換如圖3所示。

圖3 世界坐標系-攝像機坐標系

兩不同的右手坐標系可通過旋轉平移轉換重合。旋轉變換可視為三軸以一定順序分別旋轉的合運動。取繞各軸逆時針旋轉的角度為正，按照X-YZ旋轉順序，得到符合系統的旋轉矩陣：

式（3）中 準，θ，ψ 對應橫滾角、俯仰角、偏航角。下文用r11-r33代替R矩陣的各個元素。

2）攝像機坐標系-圖像坐標系

攝像機坐標系到圖像坐標系的轉換是基于攝像機模型的。上一章節已給出詳細的闡述

其中 u，v 是圖像坐標，xc，yc，zc是攝像機的坐標，u0，v0為圖像中心，fx，fy為像素單位下的焦距，k1，k2為畸變系數。

2.3 位姿估計算法

1）N點透視算法

考慮到無人機系統機動性較大，位姿估計需兼顧實時性，不宜取過多特征點。本系統擬采用平面標志物的6個特征點分析解算。理論上，對于平面目標，只需要4個共面且任意三點不在一條直線上的點集即可確定出無人機的位姿[11]。

對于圖像處理后獲取的點坐標進行畸變校正。將校正后的坐標ui，vi代入模型。從攝像機坐標系到圖像坐標系，是一個3D-2D的轉換過程。轉換中，深度信息丟失，但是可以在求解過程中消去該變量。求解的方程組如下：為歸一化的攝像機坐標。 假設 tz=1，有 8個未知量，根據PnP理論和線性代數知識，需要任意三點不共線的4個點才能求解。對于式（5）可記為：Am=b。通過最小二乘法可以解出 m=（ATA）-1ATb。結合旋轉矩陣的單位性和正交性可以求解出tz、RJS、TJS。

至此，位姿估計的初始值求解完成。

2）正交迭代算法

對于N點透視算法，各個特征點提取都存在著量測誤差，估計精度隨之降低。基于此，本文參考了一種正交迭代思想[12-13]來對N點透視算法的估計值進行進一步優化。

已知任意空間點與其成像在同一條過光心的直線上[14]。利用這個性質，以量測像點到實際像點的距離構造誤差方程。假設人工標志的特征點在世界坐標系下的坐標為wi，根據提取的圖像坐標解算出投影在歸一化平面上的像點vi。基于文獻[13]的物空間誤差模型建立目標函數如下：

令E（R，T）值最小的R，T為最優估計。對于這兩個參數的最小平方的估計，文獻[11]給出利用奇異值分解法的求解方案。旋轉矩陣的精度與平移向量精度息息相關，利用迭代算法計算得到旋轉矩陣R*。在此基礎，計算關于R*的T的最優解作為平移變量T的初始值：

根據vi=R*wi+T，更新vi，迭代若干次，計算使E最小的R和T。其中最優R*的求解。采用SVD分解法[11，13-15]進行求解。

3）位姿求解

在旋轉矩陣和平移向量求解完成后，位姿估計就十分直觀了。對于位置的求解，根據世界坐標-攝像機坐標系的轉換關系以及旋轉矩陣的定義，可以得到位置t和姿態角為：

3 實驗驗證

3.1 仿真實驗

在Matlab2012b平臺上，對位姿估計算法進行仿真實驗。通過給定姿態角位置和攝像機模型求解出對應的圖上坐標，對求解的圖上坐標加不同強度（單位為像素）的隨機噪聲 q×（rand（1）-0.5），通過位姿估計算法，將結果與參考值對比。仿真實驗中影響位姿估計精度的因素，可以歸納為：量測噪聲強度、人工標志距離光軸的遠近、人工標志的大小/距攝像機距離以及人工標志的特征點數。下文就以上因素給出仿真結果。

1）量測噪聲強度——取6個特征點作為人工標志的輸入，保持位置一定，對于噪聲強度從0-10，以0.5為間隔，每個強度取1 000次估計結果的平均值。在不同強度的噪聲下，解算出來的位姿誤差如如圖4所示，隨著噪聲的增強，姿態角和位置估計的誤差逐漸增大。在噪聲強度為10像素之內，角度偏差最大為2°。當無人機距人工標志物的距離在1 600 mm左右，對于強度為10像素的隨機噪聲會造成最大x方向2cm的偏差，y方向6 cm的偏差，z方向4 cm的偏差。

圖4 不同噪聲強度下位姿估計誤差

圖5 距光軸遠近不同對位姿估計的影響

2）人工標志距離光軸的遠近——仿真取相同噪聲條件下，初始狀態為[-pi，0，0]時，可視為距光軸為0，橫滾角以0.1 rad遞增，距離逐漸變大，位姿解算的誤差如圖5所示。人工標志距光軸的距離隨著roll角的減小而增大，位姿估計誤差隨著光軸的距離的增大而減小。

圖6 人工標志大小/相機距離對位姿估計的影響

圖7 特征點個數對位姿估計的影響

3）人工標志的大小/距攝像機距離——簡單地，取相機位于目標的正上方，在相同的噪聲強度下，同樣大小的人工標志，取不同的高度，即對應不同的得到的仿真結果如圖6所示。結果表明，距離越大位姿估計精度越低。

4）平面特征點的個數——平面特征點至少要4個，考慮到這里要保證其他因素不變，取標志物上的4/5/6個特征點觀察一下趨勢，仿真結果如圖7所示。特征點給的個數越多，位姿估計精度越高，但實際系統中太多的特征點在圖像處理中會耗時多，無法滿足實時性的要求。

5）除此以外，仿真實驗比較了本文算法和單獨的

N點透視算法的估計精度。對于不同噪聲強度下，取：

作為衡量位置和角度與標準值的平均偏差，仿真結果如圖8。

實驗結果表明，加入正交迭代，對于位姿估計的精度有明顯提升，尤其是在噪聲強度大的時候，效果更明顯。

圖8 兩種算法的位姿誤差

3.2 實物實驗

由于無人機在室內無法獲取GPS信號，因此利用VICON紅外三維運動捕捉系統，來獲取無人機的位置姿態信息，以此作為無人機位姿的標準值。實物實驗選取的機載相機型號為Gopro hero4 silver。采用加州理工大學開發的camera calibration toolbox在MATLAB2012b下進行棋盤標定，標定結果如表1所示。

圖9 圖像處理過程

表1 攝像機參數標定結果

實物實驗使用的人工標志物為藍色的挖空的三角形圖像，對機載相機拍攝的圖像做離線的圖像處理，由于篇幅限制，截取的部分圖像處理的過程如圖9所示。

人工標志固定，選取無人機任意位置拍攝的兩組圖片進行位姿解算驗證算法。相機拍攝的兩組位姿圖像如圖10所示。解算結果如表2所示。

圖10 兩組位姿圖像

表2 位姿解算結果

實驗表明，該位姿估計算法能實現無人機的位姿估計，并達到一定的精度。在實物實驗中，出現了上文例子2中，偏航角的解算出復數形式，即死鎖問題，這是由于歐拉角表示的旋轉矩陣的局限，這一問題可通過四元數法表示的旋轉矩陣進行修正。

4 結 論

文中對無人機室內飛行時的位姿實時獲取提供了一種由粗到精的位姿估計方法。根據不同位姿下機載相機下的人工標志不同的成像信息，在世界坐標系和圖像坐標系下建立轉換關系，利用點透視算法解位姿初值代入正交迭代算法，最終實現位姿的精確解算。經實驗驗證，在噪聲影響較小的情況下，本算法能實現室內無人機位姿的實時高精度估計。但該算法是基于點特征，受噪聲影響較大，一旦噪聲過大，則估計不準確，下一步人工目標的特征設計可朝著曲線特征[16]以及與IMU信息融合的方向改進。

[1]孫罡.低成本微小型無人機慣性組合導航技術研究[D].南京：南京理工大學，2014.

[2]沈浩.無人機MIMU/GPS組合導航系統研究[D].哈爾濱：哈爾濱工程大學，2012.

[3]張勇.基于合作目標的無人機位姿估計算法研究[D].南京：南京航空航天大學，2010.

[4]竇川川.基于慣性傳感和視覺方法的無人直升機位姿估計比較研究[D].哈爾濱：哈爾濱工業大學，2013.

[5]Li J H，Xie X Z.Study of Monocular Measuring Technique Based on Homography Matrix[M].Berlin Heidelberg:Springer 2011:316-324.

[6]Jayatilleke L，Zhang N.Landmark-based localization for Unmanned Aerial Vehicles[C].//Systems Conference （SysCon）， 2013 IEEE International IEEE，2013:448-451.

[7]Zhu L， Fan B，Du Y， et al.A tracking and locating method for UAVs Vision System[C].//Information and Automation （ICIA）， 2010 IEEE International Conference onIEEE，2010:1953-1957.

[8]Wang T，Zhang Y，Wang C，et al.Indoor visual navigation system based on paired-landmark for smallUAVs [C].Robotics and Biomimetics（ROBIO）， 2014 IEEE International Conference onIEEE，2014:1703-1708.

[9]楊世保.無人機著陸位姿參數視覺估計研究[D].南京：南京航空航天大學，2012.

[10]李鑫，龍古燦，劉進博，等.相機位姿估計的加速正交迭代算法[J].光學學報，2015（1）:266-273.

[11]王鵬，孫長庫，張子淼.單目視覺位姿測量的線性求解[J].儀器儀表學報， 2011，32（5）:1126-1131.

[12]Morbidi F，Mariottini G L.Active Target Tracking and Cooperative Localization for Teams of Aerial Vehicles[J].IEEE Transactions on Control Systems Technology， 2013，21（5）:1694-1707.

[13]Mirota D J， Ishii M， Hager G D.Vision-based navigation in image-guided interventions.[J].Annual Review of Biomedical Engineering，2011，13（13）:297-319.

[14]李飛，唐大全，畢波，等.基于點線對應的無人機位姿估計算法 [J].計算機測量與控制，2011，19(12):3108-3110.

[15]許允喜，蔣云良，陳方，等.基于點線對應的多攝像機全局位姿估計迭代算法[J].光子學報，2010，39（10）:1881-1888.

[16]尚曉紅.一種基于圓或平行線特征的位姿估計方法[D].昆明：云南大學，2012.

Pose estimation of UAV based on artificial marks with monocular vision

WU Yan， WU Fen， QI Guo-qing
（Nanjing University of Science and Technology， Nanjing 210094， China）

This article proposes a pose estimation algorithm with a combination of digital image processing and 2D-3D scene reconstruction to deal with UAV self localization.Under the premise of the known world coordinates of the artificial mark，we obtain the information from the images taken by a nonlinear onboard camera and take some methods of digital image processing to provide feature point datas for subsequent calculation.Pose estimation provides a improved N-point perspective algorithm which combines orthogonal iteration.N-point perspective calculates attitude angles matrix as the initial values of orthogonal iteration.The feasibility of the algorithm is verified by simulation experiments and real experiments.Compared with N-point perspective algorithm，this method improves the pose estimation accuracy and fast convergence and provide a new idea for the UAV indoor pose estimation.

self localization； nonlinear； artificial marks； PnP； orthogonal iteration

TN29

A

1674－6236（2017）12-0143-06

2016-08-04稿件編號：201608037

國家自然科學基金項目（61104186；61273076）

吳妍（1992—），女，江蘇南通人，碩士研究生。研究方向：四旋翼無人機的定位與導航、圖像處理等。