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