邢伯陽(yáng)，潘峰，2，王位，馮肖雪，＊

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

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

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

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

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

2）動(dòng)平臺(tái)狀態(tài)估計(jì)技術(shù)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

首先，使用相機(jī)內(nèi)參將橢圓投影轉(zhuǎn)換到歸一化相機(jī)坐標(biāo)系下并由識(shí)別結(jié)果計(jì)算出橢圓圓心和長(zhǎng)短軸參數(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ù)號(hào)，同時(shí)單一橢圓投影無(wú)法得到地標(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兩個(gè)約束條件，并篩選出兩組奇異解。隨后本文基于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)識(shí)別到T-D地標(biāo)時(shí)可直接選擇奇異解中與其位姿結(jié)果相近的作為正解。

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

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

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

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

由式（1）～式（10）可以得到 T-D地標(biāo)和 T-C地標(biāo)坐標(biāo)系下的相機(jī)位姿估計(jì)。若某一子地標(biāo)被單獨(dú)識(shí)別則C2D地標(biāo)定位結(jié)果等于該子地標(biāo)的定位結(jié)果。若兩個(gè)子地標(biāo)同時(shí)被識(shí)別時(shí)，本文采用T-D地標(biāo)4個(gè)頂點(diǎn)的平均二次投影誤差構(gòu)建融合權(quán)重來(lái)對(duì)二者定位結(jié)果進(jìn)行線性加權(quán)融合（近距離時(shí)T-D地標(biāo)角點(diǎn)檢測(cè)精度高則權(quán)重大，遠(yuǎn)距離時(shí)T-D地標(biāo)角點(diǎn)檢測(cè)精度下降則權(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個(gè)頂點(diǎn)二次投影誤差平均值；emax為設(shè)定的最大投影誤差。

2 動(dòng)平臺(tái)位姿估計(jì)算法

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

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

圖3中的三輪全向平臺(tái)正運(yùn)動(dòng)學(xué)驅(qū)動(dòng)模型［18］為

圖3 動(dòng)平臺(tái)坐標(biāo)系示意圖Fig.3 Coordination of moving platform

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

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

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

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

1）方便引入其他傳感器來(lái)測(cè)量vmx、vmy、ω，如采用陀螺儀來(lái)測(cè)量動(dòng)平臺(tái)角速度，采用光流傳感器來(lái)測(cè)量動(dòng)平臺(tái)更精確的線速度。

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

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

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

式中：T為采樣周期。基于EKF濾波算法k＋1時(shí)刻系統(tǒng)狀態(tài)Xk＋1的預(yù)測(cè)協(xié)方差矩陣P－k＋1為

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

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

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

式中：σk＋1為量測(cè)噪聲。C2D地標(biāo)僅能提供｛m｝系 下 的 相 機(jī) 位 姿 ζC2D（k）＝ ［xC2D（k），yC2D（k），φC2D（k）］T，因此需要使用四旋翼飛行器全局狀態(tài)估計(jì)ζq（k）＝ ［xq（k），yq（k），φq（k）］T進(jìn)行坐標(biāo)變換，則 C2D 地標(biāo)提供的量測(cè)向量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｝系下的朝向，另外量測(cè)數(shù)據(jù)中還包括了碼盤測(cè)量到的各輪線速度，對(duì)于碼盤數(shù)據(jù)滯后本文采用文獻(xiàn)［19］所提出的環(huán)形隊(duì)列和時(shí)間戳技術(shù)進(jìn)行補(bǔ)償，則 碼 盤 提 供 的 量 測(cè) 向 量 為 ze（k＋1）＝［w1（k＋1），w2（k＋1），w3（k＋1）］T，最終定義濾波器量測(cè)向量為zk＋1＝ ［zTc（k＋1），zTe（k＋1）］T，根據(jù) EKF濾波算法其預(yù)測(cè)值為

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

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

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

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

3 動(dòng)平臺(tái)降落規(guī)劃算法

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

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

1）跟蹤邊界點(diǎn)P1（t）

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

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

2）降落切換點(diǎn)P2（t）

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)與分析

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

4.1 C2D地標(biāo)定位實(shí)驗(yàn)

本文首先對(duì)C2D地標(biāo)定位性能進(jìn)行驗(yàn)證。圖6（a）中給出了相機(jī)在｛m｝系下的各地標(biāo)提供的3D定位結(jié)果，圖中曲線恒為零時(shí)表示該地標(biāo)無(wú)法識(shí)別，可見(jiàn)T-C地標(biāo)和T-D地標(biāo)都僅能實(shí)現(xiàn)相機(jī)在｛m｝系下部分區(qū)域的定位，而所提C2D地標(biāo)定位算法則實(shí)現(xiàn)了相機(jī)在｛m｝系中較完整的3D定位。圖6（b）給出了C2D地標(biāo)距離相機(jī)不同直線距離上的定位誤差曲線（圖中定位誤差為1時(shí)表示該地標(biāo)無(wú)法識(shí)別），其中三角形標(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)定位曲線，可看到其通過(guò)綜合T-C地標(biāo)和T-D地標(biāo)結(jié)果實(shí)現(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)估計(jì)仿真實(shí)驗(yàn)

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

圖7 動(dòng)平臺(tái)位置估計(jì)結(jié)果Fig.7 Estimated position of moving platform

圖8 動(dòng)平臺(tái)速度估計(jì)結(jié)果Fig.8 Estimated speed of moving platform

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

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

4.3 動(dòng)平臺(tái)降落實(shí)驗(yàn)

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

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

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

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

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

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

圖11 動(dòng)平臺(tái)車輪打滑實(shí)驗(yàn)結(jié)果Fig.11 Experimental results of wheel-slip

5 結(jié) 論

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

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

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

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