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