本發(fā)明屬于無人機控制領域，涉及一種旋翼式無人機的視覺著陸位姿估計方法。
背景技術：
：旋翼式無人機能夠?qū)崿F(xiàn)垂直起降、自由懸停以及低空低速飛行等多種飛行方式，并且具有成本低、功耗小、反應靈活等優(yōu)點。這些優(yōu)點使其在復雜環(huán)境中的服務和救援等領域都受到了廣泛的應用。在無人機的起飛、巡航、懸停、著陸四個階段中，著陸是其中最為重要的環(huán)節(jié)。目前，旋翼式無人機的自主著陸導航系統(tǒng)主要有GPS/INS導航系統(tǒng)、視覺導航系統(tǒng)、以及多傳感器融合導航系統(tǒng)。其中，利用GPS/INS等信號進行定位導航著陸精度不高，且GPS信號在室內(nèi)等場景中無法使用，多傳感器融合導航成本高，且存在累積誤差，而視覺導航具有成本低、精度高、抗干擾性強等優(yōu)點。旋翼式無人機的視覺導航大多以在著陸平臺放置合作目標的方式進行自主著陸，隨著計算機視覺的發(fā)展，國內(nèi)外已有很多利用視覺估計無人機自主著陸位姿的方法。位姿包括無人機相對于著陸點三維距離的位置參數(shù)和無人機的俯仰角、滾轉(zhuǎn)角和偏航角的姿態(tài)參數(shù)。不同的位姿估計方法在時間和精度上都存在著不足，如文獻利用Tsai提出的基于RAC的相機標定方法計算無人機的位姿參數(shù)，該方法需以迭代過程求解最優(yōu)解，且通常是求取局部最優(yōu)解，因此位置參數(shù)估計精度與時間效率不高；文獻設計了分級策略的圖標，利用快速四點方法實現(xiàn)3D位姿估計，該方法能夠?qū)崿F(xiàn)對位姿參數(shù)的快速求解，且高度計算精度較高，所需特征較少，但對于姿態(tài)角度的計算精度不足。技術實現(xiàn)要素：本發(fā)明的目的在于解決上述現(xiàn)有技術存在的缺陷，提供一種旋翼式無人機的視覺著陸位姿估計方法，提高了Tsai和快速四點方法估計位姿的精度，能夠滿足自主降落對無人機位姿參數(shù)獲取準確性和實時性的要求。為實現(xiàn)上述目的采用如下技術方案：一種旋翼式無人機的視覺著陸位姿估計方法，包括以下步驟：(1)設計合作目標；(2)特征提取與標記：通過機載視覺設備采集合作目標圖像，圖像采集后進行合作目標的特征提取，特征提取包括圖像預處理、目標域劃分、特征點提取；(2)位姿估計方法采用Tsai方法和快速四點方法兩種位姿估計方法的融合算法，根據(jù)合作目標的圖像坐標與世界坐標信息，利用Tsai方法計算無人機相對于合作目標坐標系的姿態(tài)角、快速四點方法計算無人機相對位置參數(shù)。進一步，所述的合作目標，考慮以下3點：(1)合作目標易區(qū)別于其它物體和環(huán)境，特征明顯，易于提取與識別；(2)合作目標中至少需包含5個特征點。，且每三個互不共線，以及其中四點能形成矩形；(3)合作目標的尺寸能適應無人機降落高度以及艦船等運動無人機著陸平臺上的要求，在降落距離范圍內(nèi)能夠使用的目標圖像占整幅圖像的10％-50％。進一步，圖像預處理、目標域劃分、特征點提取具體流程如下：(1)圖像預處理：使用最大類間方差(Otsu)算法求取最佳閾值對圖像進行自動閾值分割，獲取二值圖像；對二值圖像進行連通區(qū)域提取，去掉面積較小和長寬比相差過大的連通域，若無連通域，則認定圖像中無合作目標，調(diào)整無人機姿態(tài)重新獲取圖像檢索目標；(2)目標域劃分：利用每級六邊形合作目標質(zhì)心接近的特點，計算剩余連通區(qū)域的質(zhì)心距離Di,jDi,j＝[(xi-xj)2+(yi-yj)2]1/2(1)若其質(zhì)心距離Di,j小于設定閾值，則認定該連通區(qū)域為合作目標；進行合作目標二次篩選后，利用合作目標固定的面積比確定合作目標等級label；(3)特征點提?。捍_定著陸目標等級后，先對目標進行LSD直線提取[8]，利用圖像的梯度幅值和方向檢測直線l；若該級六邊形目標提取的直線數(shù)量Nl＜6，則選取label+1等級目標重新提取直線；若直線數(shù)量Nl≥6，再通過顏色信息對提取的直線進行標記；對標記的直線求交點并按順序排列p(ui,vi),i＝1,2,...6，作為計算位姿參數(shù)的特征點。進一步，Tsai姿態(tài)估計具體方法為：根據(jù)攝像機坐標系與世界坐標系關系式得到x=r1xw+r2yw+r3zw+Txy=r4xw+r5yw+r6zw+Tyz=r7xw+r8yw+r9zw+Tz---(3)]]>由徑向約束關系(RAC)得xy=xcyc=r1xw+r2yw+r3zw+Txr4xw+r5yw+r6zw+Ty---(4)]]>設定合作目標所在平面zw＝0，則上式可整理得[xwycywycyc-xwxc-ywxc]r1/Tyr2/TyTx/Tyr4/Tyr5/Ty=xc---(5)]]>每個特征點都可列出對應式(5)的方程，則用最小二乘法解超定方程組，N≥5得到未知參數(shù)，利用旋轉(zhuǎn)矩陣的性質(zhì)可求得旋轉(zhuǎn)矩陣RCW：進而求得俯仰角θ＝arcsin(-r3)、偏航角ψ＝arcsin(r2/cosθ)、翻滾角進一步，快速四點位置估計具體方法為：利用像頂點坐標pc(xc，yc，zc)，求得直線l在像平面SC內(nèi)的極坐標方程xcosθ+ysinθ＝ρ與平面的平面方程及平面法向量Aix+Biy+Ciz=0,ni→=(Ai,Bi,Ci),i=1,2,3,4---(7)]]>由于A、B、D、E分別是以及SABDE平面的交點，SABDE平面方程為Ax+By+Cz＝1，帶入平面方程便可求出其坐標可表示為Pi|C(XC,YC,ZC)=k·wi1|Wi|wi2|Wi|wi3|Wi|,i=1,2,3,4,k=1/C]]>Wi=mn1AiBiCiAi+1Bi+1Ci+1m=AC,n=BC---(8)]]>其中，wij是Wi的代數(shù)余子式；四面體ABDOC、BDEOC、ADEOC、ABEOC的體積為Vi，Sd為ABDE的面積，h為坐標原點OC到平面SABDE的距離，則四棱錐ABDEOC的體積V=Σi=14Vi=13Sdh---(9)]]>根據(jù)透視關系和立體幾何原理將已知量Sd帶入體積公式求出h以及矩形的4個頂點在攝像機坐標系下的坐標PC(XC，YC，ZC)；在上述步驟中，世界坐標系和攝像機坐標系的旋轉(zhuǎn)矩陣R已求出，根據(jù)OW在攝像機坐標系和世界坐標系下的不同坐標值PC和PW的轉(zhuǎn)化關系得相機到合作目標的相對位置TCW＝[Tx，Ty，Tz]：TCW＝PW-RCWPC(10)其中PC＝[0,0,0]T，本發(fā)明旋翼式無人機的視覺著陸位姿估計方法，將Tsai方法與快速四點方法融合起來計算位姿參數(shù)，該融合的方式既可以避免Tsai方法中迭代求解所消耗的時間，也能夠提高Tsai方法和快速四點方法中位姿參數(shù)計算的精度，并基于該方法設計了一種分級合作目標，該合作目標能適應不同降落高度下的位姿估計，最后進行了大量仿真實驗進行方法驗證。附圖說明圖1分級合作目標設計和特征點標記(a)合作目標等級標識(b)特征點順序標記圖2特征提取算法流程圖圖3不同位姿下的圖像處理結(jié)果圖4合作目標投影成像示意圖(a)偏航角；(b)X平移距離；(c)俯仰角；(d)Y平移距離；(e)翻滾角；(f)Z平移距離；圖5三種位姿估計方法的計算結(jié)果對比具體實施方式為使本發(fā)明的目的、技術方案和優(yōu)點更加清楚，下面本發(fā)明中的技術方案進行清楚、完整地描述，顯然，所描述的實施例是本發(fā)明一部分實施例，而不是全部的實施例?；诒景l(fā)明中的實施例，本領域普通技術人員在沒有作出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本發(fā)明保護的范圍。本發(fā)明根據(jù)上述兩種基于投影關系的位姿估計方法對圖像特征的要求以及普通合作目標對無人機適用距離限制的情況，在設計合作目標時，考慮了以下3點：(1)合作目標易區(qū)別于其它物體和環(huán)境，特征明顯，易于提取與識別；(2)合作目標中至少需包含5個特征點(每三個互不共線)以及其中四點能形成矩形；(3)合作目標的尺寸能適應無人機降落高度以及艦船等運動無人機著陸平臺上的要求，在降落距離范圍內(nèi)可使用的目標圖像占整幅圖像的10％-50％?；趯ι鲜鰲l件的考慮和已有的合作目標，設計了如圖1(a)所示的合作目標。該合作目標采用分級策略，將尺寸大小不同的標準六邊形合并，并按尺寸大小劃分等級，每一等級目標的6個頂點適應不同距離下的位姿估計，如圖1(b)所示。對于2m-100m范圍內(nèi)的著陸高度，共設計了5級不同大小的標準六邊形作為合作目標，其尺寸根據(jù)相機尺寸、焦距、無人機著陸范圍等參數(shù)計算，該圖標采用白色打底，用藍、綠、紅三種顏色進行特征點的順序標記，定義藍色為合作目標正方向。該圖標可用相應顏色的LED發(fā)光板替代，從而進行不同光照下的自主著陸。特征提取與標記在無人機的視覺自主著陸過程中，采用合作目標形式的視覺導航需將無人機粗定位到降落區(qū)域內(nèi)，通過機載視覺設備采集合作目標圖像，圖像采集后進行合作目標的特征提取，其特征點的提取精度直接影響位姿估計算法的精度。特征提取主要分為三部分：圖像預處理、目標域劃分、特征點提取。主要流程如下圖2所示。(1)圖像預處理：使用最大類間方差(Otsu)算法求取最佳閾值對圖像進行自動閾值分割，獲取二值圖像；對二值圖像進行連通區(qū)域提取，去掉面積較小和長寬比相差過大的連通域，若無連通域，則認定圖像中無合作目標，調(diào)整無人機姿態(tài)重新獲取圖像檢索目標。(2)目標域劃分：利用每級六邊形合作目標質(zhì)心接近的特點，計算剩余連通區(qū)域的質(zhì)心距離Di,jDi,j＝[(xi-xj)2+(yi-yj)2]1/2(1)若其質(zhì)心距離Di,j小于設定閾值，則認定該連通區(qū)域為合作目標；進行合作目標二次篩選后，利用合作目標固定的面積比確定合作目標等級label。(3)特征點提?。捍_定著陸目標等級后，先對目標進行LSD直線提取[8]，利用圖像的梯度幅值和方向檢測直線l；若該級六邊形目標提取的直線數(shù)量Nl＜6，則選取label+1等級目標重新提取直線；若直線數(shù)量Nl≥6，再通過顏色信息對提取的直線進行標記；對標記的直線求交點并按順序排列p(ui,vi),i＝1,2,...6，作為計算位姿參數(shù)的特征點。不同高度的特征提取結(jié)果如圖3所示，結(jié)果表明在不同高度下，均有不同等級的完整合作目標提供足夠的特征用于計算位姿。位姿估計方法本發(fā)明采用Tsai方法和快速四點方法兩種位姿估計方法的融合算法，根據(jù)合作目標的圖像坐標與世界坐標信息，利用Tsai方法計算無人機相對于合作目標坐標系的姿態(tài)角、快速四點方法計算無人機相對位置參數(shù)，該融合方法具有以下優(yōu)點：(1)利用Tsai方法中的徑向排列一致約束(RAC)建立坐標關系的方程求得姿態(tài)角，具有快速、誤差小的優(yōu)點，同時避免了Tsai方法中利用搜索優(yōu)化方式估計位置參數(shù)時，無法全局優(yōu)化收斂到最優(yōu)解且耗時長等問題；(2)在計算出姿態(tài)角的情況下，使用快速四點方法通過四個特征點與攝像機的投影幾何關系計算高度，其計算精度較高，且避免了該方法在計算姿態(tài)角的過程中對特征點坐標提取精度過度依賴，導致精度不足的問題；(3)融合方法所需特征較少，且全過程使用線性方程求解，運算量低，能夠?qū)崿F(xiàn)實時的位姿估計。本發(fā)明的融合算法步驟如下：本發(fā)明合作目標(六邊形)共有6個共面頂點進行姿態(tài)估計，由理想的透視投影關系，空間特征點世界坐標為Pi|w(xw，yw，zw),i＝1,2,...6，其中zw＝0，投影到攝像機圖像坐標為p(ui,vi),i＝1,2,...6(簡稱像頂點)，示意圖如圖4所示。利用攝像機已標定的內(nèi)參矩陣K，將像頂點的圖像坐標p(ui,vi)轉(zhuǎn)化為其在攝像機坐標系下的坐標Pi|C(xc，yc，zc)＝fK-1(ui,vi),i＝1,2,...6(2)4.1Tsai姿態(tài)估計根據(jù)攝像機坐標系與世界坐標系關系式可以得到x=r1xw+r2yw+r3zw+Txy=r4xw+r5yw+r6zw+Tyz=r7xw+r8yw+r9zw+Tz---(3)]]>由徑向約束關系(RAC)可得xy=xcyc=r1xw+r2yw+r3zw+Txr4xw+r5yw+r6zw+Ty---(4)]]>由于本發(fā)明設定合作目標所在平面zw＝0，則上式可整理得[xwycywycyc-xwxc-ywxc]r1/Tyr2/TyTx/Tyr4/Tyr5/Ty=xc---(5)]]>每個特征點都可列出對應式(5)的方程，則用最小二乘法解超定方程組(N≥5)得到未知參數(shù)，利用旋轉(zhuǎn)矩陣的性質(zhì)可求得旋轉(zhuǎn)矩陣RCW：進而求得俯仰角θ＝arcsin(-r3)、偏航角ψ＝arcsin(r2/cosθ)、翻滾角4.2快速四點位置估計利用像頂點坐標pc(xc，yc，zc)，求得直線l在像平面SC內(nèi)的極坐標方程xcosθ+ysinθ＝ρ與平面的平面方程及平面法向量Aix+Biy+Ciz=0,ni→=(Ai,Bi,Ci),i=1,2,3,4---(7)]]>由于A、B、D、E分別是以及SABDE平面的交點，SABDE平面方程為Ax+By+Cz＝1，帶入平面方程便可求出其坐標可表示為Pi|C(XC,YC,ZC)=k·wi1|Wi|wi2|Wi|wi3|Wi|,i=1,2,3,4,k=1/C]]>Wi=mn1AiBiCiAi+1Bi+1Ci+1m=AC,n=BC---(8)]]>其中，wij是Wi的代數(shù)余子式。四面體ABDOC、BDEOC、ADEOC、ABEOC的體積為Vi。Sd為ABDE的面積，h為坐標原點OC到平面SABDE的距離，則四棱錐ABDEOC的體積V=Σi=14Vi=13Sdh---(9)]]>根據(jù)透視關系和立體幾何原理將已知量Sd帶入體積公式便可求出h以及矩形的4個頂點在攝像機坐標系下的坐標PC(XC，YC，ZC)。在上述步驟中，世界坐標系和攝像機坐標系的旋轉(zhuǎn)矩陣R已求出，根據(jù)OW在攝像機坐標系和世界坐標系下的不同坐標值PC和PW的轉(zhuǎn)化關系可得相機到合作目標的相對位置TCW＝[Tx，Ty，Tz]：TCW＝PW-RCWPC(10)其中PC＝[0,0,0]T，實驗結(jié)果及分析為驗證上述提出的視覺位姿估計方法的有效性，將二軸云臺與攝像機結(jié)合，從不同位姿拍攝位于水平面的合作目標，并利用融合的位姿估計算法進行了大量試驗與對比。實驗采用的攝像機為Baumer高清面陣CCD攝像機、16mm定焦鏡頭，圖像大小為1292×960像素，使用Matlab2014a在配置為Intel(R)Pentium(R)CPUG2030@3.00GHz內(nèi)存4GB的計算機上編程運算?？紤]旋翼式無人機的垂直起降功能，實驗中，俯仰角和翻滾角的角度變化范圍為[-30°～30°]，步長為1.3°，距離變化范圍為[7m～50m]，我們選取10m、20m、28m、36m、44m五個高度進行結(jié)果統(tǒng)計，每個高度點選取不同位姿參數(shù)的20組數(shù)據(jù)統(tǒng)計誤差。具體計算結(jié)果見表1所示。表1位姿參數(shù)估計的平均誤差結(jié)果表明，本發(fā)明融合方法能夠?qū)崿F(xiàn)不同高度下的位姿參數(shù)估計，且角度誤差控制在2°以內(nèi)，位置誤差控制在4％以內(nèi)。為了驗證本發(fā)明融合方法在位姿估計上相對于其他兩種方法的優(yōu)勢，隨機抽取140組數(shù)據(jù)進行三種方法的位姿參數(shù)估計的精度與時間效率的對比，結(jié)果如表2、表3所示。表2三種方法位姿參數(shù)估計誤差結(jié)果比較表3三種位姿估計方法時間效率比較在精度上，本發(fā)明融合方法估計的姿態(tài)參數(shù)與快速四點方法相比誤差分別減少了1.8°、1.3°、4.82°；本發(fā)明融合方法估計的位置參數(shù)精度較Tsai方法與快速四點方法提高了12.13％、18.85％、13.12％和0.97％、6.49％、1.36％。圖5為其中45組數(shù)據(jù)的位姿參數(shù)真實值與估計值?？梢钥闯鲈谖恢脜?shù)估計上，本發(fā)明融合方法效果明顯好于Tsai方法。在時間效率上，如表3Tsai方法由于需迭代求解最優(yōu)解，耗時較長，難以滿足對位姿估計實時性的要求；而融合算法和快速四點方法求解過程無需迭代，耗時較短，能夠滿足位姿估計的實時性。最后應說明的是：以上實施例僅用以說明本發(fā)明的技術方案，而非對其限制；盡管參照前述實施例對本發(fā)明進行了詳細的說明，本領域的普通技術人員應當理解：其依然可以對前述各實施例所記載的技術方案進行修改，或者對其中部分技術特征進行等同替換；而這些修改或者替換，并不使相應技術方案的本質(zhì)脫離本發(fā)明各實施例技術方案的精神和范圍。當前第1頁1 2 3