邢伯陽，潘峰，2，王位，馮肖雪，＊

1.北京理工大學(xué) 自動化學(xué)院，北京 1000812.北京理工大學(xué) 昆明產(chǎn)業(yè)技術(shù)研究院，昆明 650106

四旋翼飛行器自主著陸技術(shù)對其在自動充電和物流配送等領(lǐng)域的應(yīng)用都有著非常重要的研究意義。目前已有許多研究成果實現(xiàn)了在靜止目標(biāo)上的降落，文獻(xiàn)［1］基于全球定位系統(tǒng)（GPS）和慣導(dǎo)傳感器（IMU）作為機(jī)載導(dǎo)航設(shè)備，結(jié)合著陸點的激光反射裝置，使用降落目標(biāo)的經(jīng)緯度作為返航降落點實現(xiàn)了高精度的自動降落，另外還有采用視覺里程計［2］等機(jī)器視覺技術(shù)替代GPS的方案。基于可靠性、成本和計算量的綜合考慮，采用二維地標(biāo)引導(dǎo)的自主降落系統(tǒng)備受關(guān)注，其核心是使用二維地標(biāo)提供飛行器機(jī)載相機(jī)相對自身的實時位姿，從而輔助飛行器自主降落。如文獻(xiàn)［3］采用了一種雙圓環(huán)圖案的地標(biāo)為無人機(jī)自主降落提供可靠的視覺定位數(shù)據(jù)。文獻(xiàn)［4］在圓環(huán)地標(biāo)內(nèi)增加字符不對稱設(shè)計，實現(xiàn)了基于單一圓環(huán)對相機(jī)六自由度（6DoF）位姿的估計。另外還有一些研究成果采用二維碼實現(xiàn)了在靜止目標(biāo)上的可靠降落［5］。

相比于靜止目標(biāo)，在車輛或船舶等動平臺上的自主降落更具實用價值。其最大的難點在于降落目標(biāo)的位置會不斷改變，因此對動平臺精確位姿的測量和如何規(guī)劃出高效降落軌跡是其中的難點，目前相關(guān)文獻(xiàn)主要通過3方面策略來實現(xiàn)在動平臺上的地標(biāo)引導(dǎo)降落。

1）二維地標(biāo)定位技術(shù)

二維地標(biāo)定位已被廣泛應(yīng)用于靜目標(biāo)降落系統(tǒng)中，許多學(xué)者進(jìn)一步對現(xiàn)有二維地標(biāo)做出改進(jìn)以獲得更可靠的識別和更連續(xù)的定位數(shù)據(jù)。文獻(xiàn)［6］使用紅藍(lán)色塊地標(biāo)，實現(xiàn)了在動平臺低慢速和直線運動下的飛行器自主降落，但由于地標(biāo)尺寸固定使得其存在較大死區(qū)，因此在低空降落中往往因為地標(biāo)不完整識別而導(dǎo)致降落失敗。為減小地標(biāo)定位死區(qū)，研究人員提出采用多尺寸地標(biāo)組合或動態(tài)調(diào)整地標(biāo)大小的方式，文獻(xiàn)［7］在二維碼內(nèi)部編碼空白處增加小尺寸二維碼有效減小了定位死區(qū)，文獻(xiàn)［8］采用多個不同尺寸二維碼并排布置實現(xiàn)了在不同距離上高精度的相機(jī)定位。文獻(xiàn)［9］則直接采用一個顯示屏依據(jù)其離飛行器的距離來動態(tài)改變地標(biāo)的尺寸。

2）動平臺狀態(tài)估計技術(shù)

由于二維地標(biāo)需要完整識別才能提供定位數(shù)據(jù)，因此在實際應(yīng)用中其數(shù)據(jù)往往不連續(xù)并且存在噪聲。有學(xué)者提出通過建立動平臺運動模型結(jié)合異構(gòu)傳感器信息來估計其精確連續(xù)的位姿。文獻(xiàn)［10］建立了動平臺的線性運動模型，將GPS數(shù)據(jù)與碼盤數(shù)據(jù)融合實現(xiàn)了對動平臺連續(xù)位姿的在線估計。類似的系統(tǒng)在文獻(xiàn)［11］中也被提出，但作者使用二維碼來代替GPS并基于平臺直線運動假設(shè)設(shè)計運動模型。為解決動平臺運動中的非線性問題，一些學(xué)者使用擴(kuò)展卡爾曼濾波器（EKF）［12］設(shè)計狀態(tài)估計器得到了更精確的位姿估計結(jié)果。

3）降落軌跡規(guī)劃技術(shù)

在得到動平臺精確位姿估計后，最直接的方式是采用位置閉環(huán)控制飛行器降落。文獻(xiàn)［13］采用線性二次型控制器進(jìn)行位置閉環(huán)控制實現(xiàn)了飛行器初步的動平臺降落，該系統(tǒng)簡單易于實現(xiàn)但未考慮動平臺存在突發(fā)機(jī)動的情況導(dǎo)致降落成功率較低。因此一些研究學(xué)者提出采用最優(yōu)控制理論來設(shè)計高效降落軌跡的方案，降落軌跡除了需要準(zhǔn)確通過預(yù)設(shè)航點外還需要盡可能地減少降落能量消耗，文獻(xiàn)［14］采用最小Snap指標(biāo)來設(shè)計降落軌跡，并基于最小值原理求解出其最優(yōu)解。文獻(xiàn)［15］則將降落軌跡規(guī)劃考慮為二次型最優(yōu)化問題并設(shè)計算法實現(xiàn)了在復(fù)雜環(huán)境下的軌跡規(guī)劃，但由于其采用不等式約束使得計算量較大難以在低成本嵌入式平臺上保證實時性。

綜上，動平臺精確降落對視覺定位的準(zhǔn)確性、狀態(tài)估計的魯棒性以及降落策略的高效性都有較高要求?，F(xiàn)有系統(tǒng)存在地標(biāo)定位死區(qū)大，狀態(tài)估計不精確、魯棒性差以及降落策略簡單和軌跡規(guī)劃算法計算量大等問題。為解決以上問題本文設(shè)計了一個基于復(fù)合地標(biāo)導(dǎo)航的動平臺優(yōu)化降落系統(tǒng)。所提系統(tǒng)采用圓環(huán)和二維碼構(gòu)成復(fù)合地標(biāo)來實現(xiàn)對機(jī)載相機(jī)的實時定位。通過建立精確的運動模型并利用擴(kuò)展卡爾曼濾波理論實現(xiàn)對動平臺在車輪打滑或碼盤標(biāo)定不精確等情況下的精確位姿估計。最終以最小軌跡Jerk指標(biāo)設(shè)計降落軌跡保證快速高效的降落。通過仿真和實際降落實驗驗證了所提系統(tǒng)具有估計動平臺連續(xù)實時位姿估計的能力，基于該系統(tǒng)實現(xiàn)了飛行器在動平臺處于直線或圓形軌跡運動下的平穩(wěn)可靠降落。

1 基于復(fù)合地標(biāo)的相機(jī)定位算法

1.1 復(fù)合地標(biāo)設(shè)計與識別算法

為實現(xiàn)對動平臺離飛行器在不同距離上的連續(xù)精確定位，本文設(shè)計了如圖1所示的C2D地標(biāo)并將其布置于動平臺頂部。C2D地標(biāo)由圓環(huán)子地標(biāo)（T-C）和二維碼子地標(biāo)（T-D）組成，其中 T-C地標(biāo)帶有缺口設(shè)計，其外圓直徑為do，內(nèi)圓直徑為di；T-D地標(biāo)布置于T-C地標(biāo)圓心處其尺寸為d2D。C2D地標(biāo)識別流程如圖2所示。

圖1 C2D地標(biāo)Fig.1 C2Dlandmark

圖2 C2D地標(biāo)識別流程Fig.2 Detection flow chart of C2Dlandmark

算法首先對原始圖像進(jìn)行如圖2（a）所示的6×6模板高斯濾波和自適應(yīng)二值化處理［16］，然后對圖像中T-D地標(biāo)進(jìn)行識別。采用Canny算子提取輪廓（圖2（b）所示）并在輪廓集合中篩選凸四邊形擬合結(jié)果。圖2（c）～圖2（d）對四邊形內(nèi)部編碼信息進(jìn)行校驗并確定T-D地標(biāo)編號和正確朝向，最后對識別到T-D地標(biāo)的頂點進(jìn)行亞像素檢測得到4個頂點精確的2D-3D匹配關(guān)系（圖2（f）所示）；對T-C地標(biāo)來說，首先對剩余輪廓進(jìn)行如圖2（g）所示的分段采樣，進(jìn)一步對采樣點集合進(jìn)行橢圓擬合得到如圖2（h）所示的橢圓集合。依據(jù)T-C地標(biāo)設(shè)計參數(shù)、橢圓中心誤差、長軸角度誤差和長短軸比例誤差加權(quán)得到綜合匹配誤差，最終選擇集合中2個綜合匹配誤差最小的橢圓作為內(nèi)外環(huán)識別結(jié)果（如圖2（i）所示）。另外，當(dāng)已識別到T-D地標(biāo)時可直接使用其中心點像素位置來快速篩選橢圓集合。為方便識別缺口位置，本文使用內(nèi)外環(huán)橢圓與圓環(huán)輪廓構(gòu)成如圖2（j）所示的缺口二值化圖，并通過檢測其中凸四邊形輪廓來確定缺口位置。

1.2 基于C2D地標(biāo)的相機(jī)位姿估計

C2D地標(biāo)由T-D地標(biāo)和T-C地標(biāo)構(gòu)成，其中T-D地標(biāo)提供近距離時的相機(jī)定位，T-C地標(biāo)提供遠(yuǎn)距離時的相機(jī)定位，二者使用的相機(jī)位姿估計算法如下所述。

1）基于T-D地標(biāo)的相機(jī)6DoF位姿估計

基于所提識別算法可以得到T-D地標(biāo)4個頂點的2D-3D匹配關(guān)系，本文使用高效N點透視定位（EPNP）算法［17］來快速地計算出相機(jī)在 T-D地標(biāo)坐標(biāo)系下的旋轉(zhuǎn)矩陣Rd和平移向量Td。

2）基本點基于T-C地標(biāo)的相機(jī)6DoF位姿估計

在完成對T-C地標(biāo)的識別后可以通過其外環(huán)橢圓投影估計出其在相機(jī)坐標(biāo)系下5DoF位姿，進(jìn)一步結(jié)合缺口來對地標(biāo)進(jìn)行朝向估計從而得到其完整的6DoF位姿。為不失一般性，首先給出由任意橢圓投影估計地標(biāo)5DoF位姿的算法。

首先，使用相機(jī)內(nèi)參將橢圓投影轉(zhuǎn)換到歸一化相機(jī)坐標(biāo)系下并由識別結(jié)果計算出橢圓圓心和長短軸參數(shù)Li（i＝1，2，…，6），則可構(gòu)建橢圓參數(shù)方程：

進(jìn)一步可以得到橢圓圓錐投影矩陣Q為［4］

式中：f為攝像頭焦距。

設(shè)λ1、λ2和λ3為Q 矩陣的特征值，ε2、ε3為λ2、λ3的特征向量。假設(shè)λ1＞λ2＞0＞λ3，則圓環(huán)平面在相機(jī)坐標(biāo)系下的角度和位置為

式中：d為圓環(huán)直徑。

式（3）和式（4）中S1、S2和S3未定正負(fù)號，同時單一橢圓投影無法得到地標(biāo)朝向，因此基于式（3）和式（4）只能得到相機(jī)5DoF旋轉(zhuǎn)矩陣rc和平移向量tc的奇異解：

基于地標(biāo)在相機(jī)前方且地標(biāo)z軸朝向相機(jī)光心能構(gòu)造n［0，0，1］T＜0和t［0，0，1］T＞0兩個約束條件，并篩選出兩組奇異解。隨后本文基于T-D地標(biāo)定位數(shù)據(jù)和機(jī)載IMU數(shù)據(jù)進(jìn)行二次篩選。

1）由于T-D地標(biāo)坐標(biāo)系和T-C地標(biāo)坐標(biāo)系重合，因此T-D地標(biāo)在相機(jī)坐標(biāo)系下的位姿應(yīng)當(dāng)與T-C地標(biāo)一致，則當(dāng)識別到T-D地標(biāo)時可直接選擇奇異解中與其位姿結(jié)果相近的作為正解。

2）在T-D地標(biāo)無法識別時，本文采用IMU解算姿態(tài)角作為篩選條件，假設(shè)動平臺處于水平地面并忽略平臺傾斜引起的誤差，由于相機(jī)與IMU相對安裝關(guān)系RIc已知，則可采用RIMU＝RIcRI作為判斷條件對奇異解進(jìn)行篩選，RI為相機(jī)在全局坐標(biāo)下的旋轉(zhuǎn)矩陣。為簡化奇異解篩選過程，本文直接采用歐拉角作為比較對象，則已知旋轉(zhuǎn)矩陣R＊和平移向量t＊由它們計算出的相機(jī)在歐拉坐標(biāo)系下的位姿為ζ＊為

定義基于式（7）最終篩選出的T-C地標(biāo)坐標(biāo)系下的相機(jī)5DoF位姿正解為r＊c、t＊c，進(jìn)一步為得到其完整的6DoF位姿，本文使用缺口中心像素和T-C地標(biāo)中心像素來近似計算朝向：

式中：［Scx，Scy］T為 T-C 地標(biāo)中心像素坐標(biāo)，［Hx，Hy］T為缺口中心點像素坐標(biāo)，則 T-C地標(biāo)坐標(biāo)系下相機(jī)完整6DoF位姿為

3）基于C2D地標(biāo)的相機(jī)位姿估計

由式（1）～式（10）可以得到 T-D地標(biāo)和 T-C地標(biāo)坐標(biāo)系下的相機(jī)位姿估計。若某一子地標(biāo)被單獨識別則C2D地標(biāo)定位結(jié)果等于該子地標(biāo)的定位結(jié)果。若兩個子地標(biāo)同時被識別時，本文采用T-D地標(biāo)4個頂點的平均二次投影誤差構(gòu)建融合權(quán)重來對二者定位結(jié)果進(jìn)行線性加權(quán)融合（近距離時T-D地標(biāo)角點檢測精度高則權(quán)重大，遠(yuǎn)距離時T-D地標(biāo)角點檢測精度下降則權(quán)重小）。定義由T-D地標(biāo)得到的相機(jī)位姿為ζ2D，由T-C地標(biāo)得到的相機(jī)位姿為ζc，則最終由C2D地標(biāo)提供的相機(jī)位姿ζC2D＝［xC2D，yC2D，φC2D］T為

式中：e2D為T-D地標(biāo)4個頂點二次投影誤差平均值；emax為設(shè)定的最大投影誤差。

2 動平臺位姿估計算法

為了得到動平臺精確連續(xù)的位姿和速度估計，本文基于擴(kuò)展卡爾曼濾波器（EKF）來設(shè)計動平臺狀態(tài)估計器。下文采用三輪全向底盤作為動平臺來介紹狀態(tài)估計算法，其坐標(biāo)系描述如圖3所示。

圖3中動平臺機(jī)體半徑為L，頂部布置有C2D地標(biāo)。從圖3中可以看到動平臺機(jī)體坐標(biāo)系與C2D坐標(biāo)系對齊，定義機(jī)體坐標(biāo)系為｛m｝，其原點在機(jī)體中心，ym軸指向機(jī)頭和缺口方向，xm和zm軸滿足右手法則；定義動平臺在全局坐標(biāo)系｛n｝下的位置為 ［x，y，φ］T，機(jī)頭朝向為φ，則可得到三輪全向底盤運動模型為

圖3中的三輪全向平臺正運動學(xué)驅(qū)動模型［18］為

圖3 動平臺坐標(biāo)系示意圖Fig.3 Coordination of moving platform

式中：wi（i＝1，2，3）為各輪線速度；vmx和vmy為動平臺機(jī)體線速度；ω為機(jī)體角速度；F為驅(qū)動變換矩陣。

2.1 狀態(tài)預(yù)測

在理想情況下，基于式（13）和式（14）僅利用碼盤數(shù)據(jù)就能實現(xiàn)對動平臺位姿的估計，但是由于碼盤數(shù)據(jù)存在噪聲和誤差，最終造成位姿估計結(jié)果會存在累計誤差。同時在實際應(yīng)用中動平臺往往在復(fù)雜地面上移動，其存在著車輪打滑等未知因素，如動平臺某輪懸空于地面縫隙或者動平臺被障礙物阻塞等情況都會造成碼盤數(shù)據(jù)與真實數(shù)據(jù)的不符，如直接使用包含測量偏差數(shù)據(jù)進(jìn)行狀態(tài)預(yù)測勢必會產(chǎn)生嚴(yán)重的誤差并最終導(dǎo)致降落失敗，因此為解決該問題本文進(jìn)一步對碼盤偏差進(jìn)行建模。

定義動平臺系統(tǒng)狀態(tài)為X＝ ［x，y，φ，vmx，vmy，ω，e1，e2，e3］T，其包括動平臺在｛n｝系下的位置和朝向，｛m｝系下的速度和角速度以及各碼盤測量偏差。另外，基于以下兩點考慮，本文將機(jī)體速度vmx、vmy包含在系統(tǒng)狀態(tài)中：

1）方便引入其他傳感器來測量vmx、vmy、ω，如采用陀螺儀來測量動平臺角速度，采用光流傳感器來測量動平臺更精確的線速度。

2）由于碼盤數(shù)據(jù)需要通訊傳輸，而通訊造成的延時更容易在量測更新中通過時間戳或環(huán)形隊列進(jìn)行補償。

綜上，結(jié)合動平臺非線性模型和系統(tǒng)狀態(tài)構(gòu)建濾波器狀態(tài)轉(zhuǎn)移方程為

式中：ηk為系統(tǒng)過程噪聲，本文假設(shè)其為高斯白噪聲（協(xié)方差矩陣為Qk），則k＋1時刻Xk＋1的預(yù)測值為

式中：T為采樣周期?；贓KF濾波算法k＋1時刻系統(tǒng)狀態(tài)Xk＋1的預(yù)測協(xié)方差矩陣P－k＋1為

式中：fx為f（·）的雅克比矩陣。

2.2 狀態(tài)量測更新

量測更新使用C2D地標(biāo)定位結(jié)果和碼盤速度測量值來對預(yù)測狀態(tài)進(jìn)行修正，則量測向量zk＋1和量測方程h（·）間的關(guān)系為

式中：σk＋1為量測噪聲。C2D地標(biāo)僅能提供｛m｝系 下 的 相 機(jī) 位 姿 ζC2D（k）＝ ［xC2D（k），yC2D（k），φC2D（k）］T，因此需要使用四旋翼飛行器全局狀態(tài)估計ζq（k）＝ ［xq（k），yq（k），φq（k）］T進(jìn)行坐標(biāo)變換，則 C2D 地標(biāo)提供的量測向量zc（k＋1）＝ ［xm（k＋1），ym（k＋1），φm（k＋1）］T 為

式中：ψ（k＋1）＝φq（k＋1）－φC2D（k＋1）為 C2D 地標(biāo)在｛n｝系下的朝向，另外量測數(shù)據(jù)中還包括了碼盤測量到的各輪線速度，對于碼盤數(shù)據(jù)滯后本文采用文獻(xiàn)［19］所提出的環(huán)形隊列和時間戳技術(shù)進(jìn)行補償，則 碼 盤 提 供 的 量 測 向 量 為 ze（k＋1）＝［w1（k＋1），w2（k＋1），w3（k＋1）］T，最終定義濾波器量測向量為zk＋1＝ ［zTc（k＋1），zTe（k＋1）］T，根據(jù) EKF濾波算法其預(yù)測值為

定義殘差向量νk＋1為量測向量和其預(yù)測值之差，則殘差向量和其近似協(xié)方差矩陣為

式中：hx為h（·）的雅克比矩陣；Rk＋1為量測噪聲σk＋1的協(xié)方差矩陣。則卡爾曼增益為

最終，動平臺修正后的狀態(tài)估計和協(xié)方差矩陣為

式中：I為單位矩陣。綜上，基于式（15）～式（25）可以實現(xiàn)對動平臺連續(xù)狀態(tài)的在線估計，并為后續(xù)降落規(guī)劃提供可靠的反饋數(shù)據(jù)。

3 動平臺降落規(guī)劃算法

為實現(xiàn)高效可靠的動平臺降落，本文以最小化軌跡Jerk指標(biāo)來設(shè)計降落軌跡，所提算法同時考慮降落過程中動平臺機(jī)動和降落能量消耗問題，降落軌跡示意圖如圖4所示，圖中黑色軌跡為動平臺運動軌跡，紅色軌跡為四旋翼飛行器降落軌跡，藍(lán)色箭頭為動平臺當(dāng)前速度矢量Vc。降落開始后，飛行器首先采用圖像跟蹤的方式從遠(yuǎn)處快速逼近動平臺，在達(dá)到切換邊界點P1（t）后基于動平臺和飛行器實時位姿估計規(guī)劃降落軌跡。降落軌跡除了需要保證飛行器在動平臺后方P2（t）航點處穩(wěn)定跟隨外還需要以最小能量消耗為指標(biāo)降落在P3（t）航點處，所提優(yōu)化降落策略如下所述。

圖4 降落軌跡示意圖Fig.4 Landing trajectory

1）跟蹤邊界點P1（t）

在識別到動平臺后，飛行器基于圖像跟蹤的方式向動平臺直線逼近。其以C2D地標(biāo)中心像素位置與圖像中心作反饋控制，x軸誤差控制四旋翼飛行器航向，y軸誤差控制云臺俯仰角。逼近中飛行器保持高度H1，前進(jìn)速度V1。定義R1為最小逼近距離，D（t）為飛行器離動平臺的實時直線距離：

式中：＾zq（t）為飛行器當(dāng)前估計高度；θp（t）為云臺俯仰角度；ρ（0＜ρ＜1）為可調(diào)權(quán)重參數(shù)，則當(dāng)D（t）小于R1時進(jìn)行模式切換。

2）降落切換點P2（t）

該模式下飛行器需要規(guī)劃降落軌跡實現(xiàn)在動平臺后方的穩(wěn)定跟隨，因此本文選擇動平臺當(dāng)前速度矢量Vc（t）反方向延長線與圓環(huán)C2交點作為P2（t）＝ ［x2（t），y2（t），z2（t）］T航點，則其坐標(biāo)為

3）降落目標(biāo)點P3（t）

在經(jīng)過P2（t）航點后飛行器進(jìn)入最終降落階段，本文基于最小能量消耗來動態(tài)選擇P3（t）降落點，首先以時間間隔dT來預(yù)測動平臺未來n·dT（n＝1，2，…，m）時刻的位置作為候選降落點。通過對候選軌跡的Jerk進(jìn)行積分作為其能量消耗［20］，并選擇能量消耗最少的作為降落航點P3（t）＝ ［x3（t），y3（t），z3（t）］T。

綜上，所提降落軌跡不但要滿足航點約束外還需具有最小能量消耗，該問題為一個典型的二次最優(yōu)規(guī)劃問題。為簡化計算本文對三維軌跡獨立進(jìn)行規(guī)劃，考慮某一維軌跡可由n階多項式表示，其多項式參數(shù)向量為p ＝ ［p0，p1，…，pn］T，采用最小化軌跡Jerk指標(biāo)設(shè)計降落軌跡的數(shù)學(xué)描述為

式中：k為軌跡分段數(shù)量；ti（i＝1，2，…，k）為各段軌跡始末時間。

進(jìn)一步由航點信息構(gòu)建如式（30）所示的約束，軌跡起始點為飛行器當(dāng)前位置，軌跡中點需通過P2（t）航點，軌跡末端終止在P3（t）航點，另外在P2（t）航點前后兩段軌跡的位置、速度和加速度應(yīng)當(dāng)連續(xù)，則以x軸軌跡規(guī)劃為例其等式約束如下：

進(jìn)一步結(jié)合本文第1節(jié)所提地標(biāo)定位算法和第2節(jié)所提動平臺狀態(tài)估計算法給出如圖5所示的降落系統(tǒng)設(shè)計。其采用層次化和模塊化設(shè)計理念，包括：① 傳感器層由多種異構(gòu)傳感器組成包括所提C2D地標(biāo)、碼盤、慣導(dǎo)傳感器和二維碼陣列定位系統(tǒng)；② 數(shù)據(jù)融合層采用異構(gòu)傳感器數(shù)據(jù)和所提狀態(tài)估計算法對飛行器和動平臺的位姿進(jìn)行在線估計；③ 決策層結(jié)合動平臺與飛行器的實時狀態(tài)估計執(zhí)行所提降落策略，并規(guī)劃降落軌跡；④ 控制層實現(xiàn)飛行器對降落軌跡的跟蹤控制，同時使用C2D地標(biāo)像素識別結(jié)果完成云臺對目標(biāo)的跟蹤對準(zhǔn)控制。

圖5 降落系統(tǒng)框架圖Fig.5 Flow chart of landing system

4 實驗與分析

為驗證本文所提優(yōu)化降落系統(tǒng)，本文搭建了一個四旋翼飛行器和三輪全向移動平臺。飛行器軸距為330mm，使用8045螺旋槳和980kV的無刷電機(jī)，整機(jī)重量為1.2kg，續(xù)航時間為15min。由式（19）可知動平臺在｛n｝系下的量測向量需使用飛行器全局位姿進(jìn)行坐標(biāo)系變換，為獲得飛行器精確的位姿數(shù)據(jù)，實驗使用文獻(xiàn)［21］中提出的二維碼陣列定位系統(tǒng)實現(xiàn)飛行器導(dǎo)航和定位。為降低圖像抖動并實現(xiàn)可靠圖像跟蹤，飛行器前方安裝有兩軸云臺和PS2Eye相機(jī)（可提供320×240，60FPS的圖像數(shù)據(jù)）。所提地標(biāo)定位算法，動平臺狀態(tài)估計算法和降落軌跡規(guī)劃算法均在機(jī)載Odroid-Xu4處理器中運行。三輪全向底盤高度為20cm，半徑L為35cm，C2D地標(biāo)外環(huán)直徑do為55cm，di為40cm，二維碼尺寸d2D為34cm。飛行器通過2.4G射頻基站與動平臺通訊，頻率為50Hz，丟包率小于5%。

4.1 C2D地標(biāo)定位實驗

本文首先對C2D地標(biāo)定位性能進(jìn)行驗證。圖6（a）中給出了相機(jī)在｛m｝系下的各地標(biāo)提供的3D定位結(jié)果，圖中曲線恒為零時表示該地標(biāo)無法識別，可見T-C地標(biāo)和T-D地標(biāo)都僅能實現(xiàn)相機(jī)在｛m｝系下部分區(qū)域的定位，而所提C2D地標(biāo)定位算法則實現(xiàn)了相機(jī)在｛m｝系中較完整的3D定位。圖6（b）給出了C2D地標(biāo)距離相機(jī)不同直線距離上的定位誤差曲線（圖中定位誤差為1時表示該地標(biāo)無法識別），其中三角形標(biāo)記線為T-D地標(biāo)定位誤差曲線，可以看到其提供了0.5～3.5m間的相機(jī)定位數(shù)據(jù)，其定位誤差隨距離增大而增加；紅色矩形標(biāo)記線為T-C地標(biāo)定位誤差曲線，可以看到其提供了1.5～6.0m的相機(jī)定位數(shù)據(jù)，并且其定位精度在不同距離上變化不大；黑色十字標(biāo)記線為C2D地標(biāo)定位曲線，可看到其通過綜合T-C地標(biāo)和T-D地標(biāo)結(jié)果實現(xiàn)了距離在0.5～6.0m內(nèi)的相機(jī)綜合完整定位。復(fù)合地標(biāo)既適用于近距離定位也適用于遠(yuǎn)距離定位，定位精度最高可達(dá)到0.1m。

圖6 C2D地標(biāo)定位結(jié)果圖Fig.6 Localization results of C2Dlandmark

4.2 狀態(tài)估計仿真實驗

為驗證所提動平臺位姿估計算法，本文設(shè)計如下仿真實驗。實驗中飛行器懸停在坐標(biāo)點（0，0，3）m處，動平臺以（1，3，0）m為圓心進(jìn)行半徑為2.0m的圓形軌跡運動，速度矢量模值為1.0m／s。為對比算法性能，實驗與文獻(xiàn)［11］中所提出未考慮測量偏差的EKF算法進(jìn)行對比。實驗中為碼盤1添加緩增測量偏差；在7s時為碼盤2突加0.2m／s固定測量偏差，碼盤3添加－0.4m／s固定測量偏差；在17s后為碼盤2和碼盤3添加幅值為0.4m／s的正弦偏差，實驗結(jié)果如圖7和圖8所示。

圖7 動平臺位置估計結(jié)果Fig.7 Estimated position of moving platform

圖8 動平臺速度估計結(jié)果Fig.8 Estimated speed of moving platform

圖7 （a）為動平臺位置估計結(jié)果，圖中紅色虛線為動平臺真實軌跡，綠色標(biāo)記為視覺定位軌跡，藍(lán)色虛線為對比算法估計結(jié)果，黑色實線為所提算法估計結(jié)果，從圖中可以看到所提算法能較好地估計出動平臺的移動軌跡而對比算法則出現(xiàn)較大的偏差。圖7（b）中給出了位置估計誤差曲線，可以看到所提算法位置估計誤差均在0.5m以內(nèi)，而對比算法估計誤差曲線存在較大的波動。

圖8（a）中給出了動平臺速度估計結(jié)果，圖中紅色虛線為動平臺真實速度。藍(lán)色點劃線為對比算法估計結(jié)果，其存在較大滯后和誤差。黑色實現(xiàn)為所提算法估計結(jié)果，可以看在碼盤存在測量偏差的情況下所提算法仍能較好地估計動平臺運動速度。圖8（b）給出了碼盤測量偏差估計結(jié)果，圖中點劃線為仿真添加測量偏差，實線為所提算法估計測量偏差，可以看到所提算法能較好地估計出碼盤測量偏差。

4.3 動平臺降落實驗

為驗證所提降落策略和降落軌跡規(guī)劃算法，本文設(shè)計了動平臺處于直線運動和圓周運動下的飛行器實際降落實驗，實驗中使用文獻(xiàn)［21］所提二維碼陣列定位系統(tǒng)為飛行器提供精確定位數(shù)據(jù)，相關(guān)實驗參數(shù)如表1所示。圖9和圖10給出了降落實驗結(jié)果，兩實驗中動平臺移動速度矢量模值均為0.35m／s，其中圓形軌跡半徑為2.0m。

表1 降落規(guī)劃實驗參數(shù)Table 1 Parameters of landing experiment

圖9 直線軌跡降落實驗結(jié)果Fig.9 Experimental results of linear motion landing

圖9 （a）為動平臺進(jìn)行直線軌跡移動下的降落結(jié)果，在圖9（a）中1處動平臺首次識別目標(biāo)后初始化狀態(tài)估計器，在圖9（a）中2處飛行器從圖像跟蹤切換到軌跡規(guī)劃模式，在圖9（a）中3處進(jìn)行降落軌跡規(guī)劃，最終在圖9（a）中4處飛行器持續(xù)進(jìn)行降落軌跡規(guī)劃直到降落成功。圖9（b）為降落3D軌跡還原圖，可以看到動平臺估計軌跡符合其直線運動。圖10為動平臺進(jìn)行圓形軌跡移動下的降落結(jié)果，從3D軌跡還原圖中可以看到動平臺估計運動軌跡符合所設(shè)定的圓形軌跡運動。

圖11為動平臺受墻壁阻礙導(dǎo)致車輪打滑的實驗結(jié)果，從圖中可以看到由于打滑造成其受阻礙時碼盤仍有速度測量值，如直接使用該測量值對動平臺速度和位置進(jìn)行預(yù)測將造成較大誤差。從圖中可以看出各碼盤偏差估計值幾乎趨近于碼盤測量值，因此基于所提運動模型可以保證狀態(tài)預(yù)測時機(jī)體速度趨近于零，符合動平臺受阻的實際情況。

圖10 圓形軌跡降落實驗結(jié)果Fig.10 Experimental results of circular motion landing

圖11 動平臺車輪打滑實驗結(jié)果Fig.11 Experimental results of wheel-slip

5 結(jié) 論

本文設(shè)計了一個基于復(fù)合地標(biāo)導(dǎo)航的優(yōu)化降落系統(tǒng)，其采用圓環(huán)和二維碼構(gòu)成復(fù)合地標(biāo)實現(xiàn)對機(jī)載相機(jī)的精確定位，基于狀態(tài)估計理論來估計動平臺實時位姿并進(jìn)一步規(guī)劃降落軌跡實現(xiàn)飛行器的可靠降落，通過實驗驗證可以得出如下結(jié)論：

1）復(fù)合地標(biāo)結(jié)構(gòu)提高了地標(biāo)定位的有效范圍、降低了定位死區(qū)，最終實現(xiàn)相機(jī)距離在0.5～6.0m內(nèi)的綜合完整定位并具有最高0.1m的定位精度。

2）所提狀態(tài)估計算法考慮了碼盤未知測量偏差提高了系統(tǒng)在車輪打滑或碼盤不精確標(biāo)定時的位姿估計精度，位置估計誤差控制在0.5m內(nèi)并能為降落規(guī)劃器提供動平臺精確連續(xù)的速度估計值。

3）降落軌跡依據(jù)飛行器和動平臺實時位姿估計值進(jìn)行在線動態(tài)規(guī)劃，以最小化軌跡Jerk為目標(biāo)達(dá)到了高效、可靠的降落性能，完成了動平臺處于直線和圓周運動下的降落任務(wù)。