基于復合地標導航的動平臺四旋翼飛行器自主優化降落技術

2019-07-18 03:49:50邢伯陽潘峰王位馮肖雪

航空學報 2019年6期

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

1.北京理工大學自動化學院，北京 1000812.北京理工大學昆明產業技術研究院，昆明 650106

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

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

1）二維地標定位技術

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

2）動平臺狀態估計技術

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

3）降落軌跡規劃技術

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

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

1 基于復合地標的相機定位算法

1.1 復合地標設計與識別算法

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

圖1 C2D地標Fig.1 C2Dlandmark

圖2 C2D地標識別流程Fig.2 Detection flow chart of C2Dlandmark

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

1.2 基于C2D地標的相機位姿估計

C2D地標由T-D地標和T-C地標構成，其中T-D地標提供近距離時的相機定位，T-C地標提供遠距離時的相機定位，二者使用的相機位姿估計算法如下所述。

1）基于T-D地標的相機6DoF位姿估計

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

2）基本點基于T-C地標的相機6DoF位姿估計

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

首先，使用相機內參將橢圓投影轉換到歸一化相機坐標系下并由識別結果計算出橢圓圓心和長短軸參數Li（i＝1，2，…，6），則可構建橢圓參數方程：

進一步可以得到橢圓圓錐投影矩陣Q為［4］

式中：f為攝像頭焦距。

設λ1、λ2和λ3為Q 矩陣的特征值，ε2、ε3為λ2、λ3的特征向量。假設λ1＞λ2＞0＞λ3，則圓環平面在相機坐標系下的角度和位置為

式中：d為圓環直徑。

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

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

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

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

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

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

3）基于C2D地標的相機位姿估計

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

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

2 動平臺位姿估計算法

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

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

圖3中的三輪全向平臺正運動學驅動模型［18］為

圖3 動平臺坐標系示意圖Fig.3 Coordination of moving platform

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

2.1 狀態預測

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

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

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

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

綜上，結合動平臺非線性模型和系統狀態構建濾波器狀態轉移方程為

式中：ηk為系統過程噪聲，本文假設其為高斯白噪聲（協方差矩陣為Qk），則k＋1時刻Xk＋1的預測值為

式中：T為采樣周期。基于EKF濾波算法k＋1時刻系統狀態Xk＋1的預測協方差矩陣P－k＋1為

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

2.2 狀態量測更新

量測更新使用C2D地標定位結果和碼盤速度測量值來對預測狀態進行修正，則量測向量zk＋1和量測方程h（·）間的關系為

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

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

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

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

最終，動平臺修正后的狀態估計和協方差矩陣為

式中：I為單位矩陣。綜上，基于式（15）～式（25）可以實現對動平臺連續狀態的在線估計，并為后續降落規劃提供可靠的反饋數據。

3 動平臺降落規劃算法

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

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

1）跟蹤邊界點P1（t）

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

式中：＾zq（t）為飛行器當前估計高度；θp（t）為云臺俯仰角度；ρ（0＜ρ＜1）為可調權重參數，則當D（t）小于R1時進行模式切換。

2）降落切換點P2（t）

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

3）降落目標點P3（t）

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

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

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

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

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

圖5 降落系統框架圖Fig.5 Flow chart of landing system

4 實驗與分析

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

4.1 C2D地標定位實驗

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

圖6 C2D地標定位結果圖Fig.6 Localization results of C2Dlandmark

4.2 狀態估計仿真實驗

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

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

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

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

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

4.3 動平臺降落實驗

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

表1 降落規劃實驗參數Table 1 Parameters of landing experiment

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

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

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