無人機動平臺著陸慣性/視覺位姿歧義校正算法

尚克軍，鄭 辛，王旒軍，扈光鋒，劉崇亮

（1.北京理工大學 自動化學院，北京 100086；2.北京自動化控制設(shè)備研究所，北京 100074）

長航時無人機由于其留空時間長，多采用太陽能結(jié)合儲能電池系統(tǒng)作為動力能源，因而要求結(jié)構(gòu)質(zhì)量盡量輕，結(jié)構(gòu)設(shè)計上多采用取消起落架配置，能夠滿足長航時無人機減小起飛重量、簡化結(jié)構(gòu)設(shè)計的需求[1]。在移動平臺上設(shè)置合作目標并敷設(shè)回收網(wǎng)，通過對無人機速度和位置的精確控制，使無人機平穩(wěn)降落至柔性的回收網(wǎng)中，實現(xiàn)無人機的回收，是有效可行的回收方式之一。

以視覺二維碼作為合作目標，使用單目相機只進行一次觀測即可解算出相機與二維碼之間的相對位置和姿態(tài)（即為典型的PnP（Perspective-n-Point）問題），具有很強的便利性和靈活性。視覺二維碼已成為機器人、無人車與無人機領(lǐng)域常用的位姿估計手段。

針對PnP 問題有許多經(jīng)典的解法，諸如使用三對點的P3P、直接線性變換（Direct linear transformation,DLT）、EPnP 和UPnP 等。由于單個二維碼是平面目標，四個三維角點滿足共面約束，二維碼平面與圖像平面之間變換為單應矩陣（Homography），進而使用二維碼進行位姿解算是平面位姿估計（Plane-based Pose Estimation,PPE）問題。

PPE 問題的求解包含兩類主要方法：其主流解法是首先估計二維碼平面與圖像平面之間變換為單應矩陣，然后使用單應陣分解（Homography Decomposition,HD）求得相對位姿的解析解；另一類常用求解方法將PPE 視為PnP 問題的特例直接使用PnP 求解方法計算相對位置和姿態(tài)。

理論上，使用任意三點不共線的四個共面點（如方形二維碼四個角點）進行6 自由度位姿解算具有唯一解。實際上，盡管PPE 問題有許多成熟的求解方法，但是基于二維碼的位姿解算深受位姿歧義的困擾，約25%情況位姿解算結(jié)果具有歧義性[2]。

目前解決PPE 問題位姿歧義有以下幾類常用方法：

1）添加額外二維碼組成二維碼序列[3]。一種方案是添加多個共面二維碼組成組合二維碼：如果各二維碼之間位置關(guān)系已知，則直接使用4n（n為檢測到的二維碼個數(shù)）個3D-2D 對應點對計算相對位姿。如果各二維碼之間位置關(guān)系未知，則需要先求出每個二維碼的位姿結(jié)果，然后再利用所有二維碼的位姿一致性選取正確的結(jié)果；另一種方案是添加多個非共面二維碼：如果一副圖像中識別出多個二維碼，則多個3D-2D點對打破了共面條件的約束，該PnP 問題具有唯一解。

需要注意的是，不論添加額外的共面還是非共面二維碼，能解決位姿歧義問題的前提是組合二維碼在圖像中成像面積足夠大，否則就會陷入單個二維碼位姿歧義的場景，多個二維碼與單個二維碼的作用效果沒有差別。

2）基于時間序列濾波的方法[4,5]。使用卡爾曼濾波器、擴展卡爾曼濾波器或粒子濾波器等方法建模相機運動模型，對當前位姿結(jié)果進行預測，選取符合模型的結(jié)果。

基于濾波的方法假設(shè)二維碼位姿的變化具有時間連續(xù)性，而位姿歧義會導致位姿解算結(jié)果隨機震蕩，導致不能建立準確的運動模型，使得濾波器能減少正確位姿之間錯誤的位姿估計，但是如果初始位姿估計錯誤，則濾波平滑后的相機運動軌跡可能一直為錯誤估計。

無人機動平臺著網(wǎng)回收場景下二維碼合作目標與機載相機都在運動，其相對位姿關(guān)系沒有確定的模型，并且著網(wǎng)回收過程起始于無人機與二維碼目標距離較遠的地方，位姿歧義可能頻繁出現(xiàn)，基于濾波方法結(jié)果的不確定性會導致災難性的后果。

3）利用深度信息矯正位姿歧義[6]。使用雙目或RGBD 相機可以獲取二維碼目標的深度信息，融合二維碼的深度信息可提升位姿估計精度并解決位姿歧義問題。

但是目前RGBD 相機有效作用距離為5 m 左右，且在室外日光條件下深度傳感器無效。無人機動平臺著網(wǎng)回收場景起始于無人機與二維碼目標距離較遠的地方，遠超出雙目或RGBD 相機有效作用距離，不能消除位姿歧義。

本文針對無人機移動平臺著網(wǎng)回收應用場景，通過構(gòu)建非共面二維碼合作目標，并融合IMU 傳感器信息，提出了一套基于二維碼的慣性/視覺位姿歧義校正算法，重點解決無人機回收過程中位姿解算歧義問題，提升無人機動平臺回收的導航精度、魯棒性與實用性。

1 二維碼合作目標設(shè)計

目前常用的二維碼大致可分為方形二維碼、圓形二維碼和異形二維碼等。方形二維碼具有正方形的外邊界，一般由黑色邊框包圍，內(nèi)部含有二進制或特定視覺模式等編碼信息，利用方形二維碼外邊界的四個角點可進行位姿解算。常見的方形二維碼有ARToolKit、AprilTags[7]和ArUco[8]等；圓形二維碼使用圓輪廓作為識別特征，編碼信息包含于圓形扇面或者同心圓環(huán)中。圓形二維碼通常只有圓心點特征，單個圓形碼不能進行位姿解算。常見的圓形二維碼有Intersense 和CCTag[9]等；除方形和圓形二維碼外，還有一些不規(guī)則形狀的二維碼，如STag[10]和TopoTag[11]等。

方形碼由于其具備簡單的編碼方式，并且單個碼即可提供四個角點用于位姿估計，在增強現(xiàn)實和機器人技術(shù)等領(lǐng)域應用廣泛，在各型二維碼中占主導地位。本文基于方形碼設(shè)計了一套組合二維碼。

該組合碼包含三個方形二維碼，二維碼內(nèi)部使用簡單的二進制編碼，并且以不同的打印尺寸組合成立體碼，其主要設(shè)計理念如下：

1) 為了適應無人機著網(wǎng)回收遠距離（≥20m）識別定位的需求，二維碼內(nèi)部使用簡單的9位二進制編碼，易于遠距離分辨識別；

2) 無人機著網(wǎng)回收過程末段二維碼與機載相機的距離較近，可能會使單個二維碼成像不完整，導致識別與位姿解算失敗。本文使用不同尺寸的二維碼以一定的空間關(guān)系排布形成組合二維碼（見圖1(a)），只要識別大于或等于一個任意二維碼就能確定相機與組合碼的相對位姿關(guān)系。較大尺寸的2#碼主要用于遠距離識別定位，而0#和1#碼設(shè)計較小的尺寸主要用于近處的識別定位，組合二維碼詳細尺寸見圖1。該組合二維碼同時具備作用距離遠和近處防遮擋的特性；

3) 使用單幅圖像中共面的特征點進行位姿估計會得到四個不同的解，根據(jù)“Cheirability”約束可以剔除其中的兩個錯誤解，最終獲得兩個可能的解。大多數(shù)視角下可以使用最小化重投影誤差判別兩個解中正確的一個，然而當相機與二維碼距離較遠時，利用重投影誤差等手段不足以判別兩個解的正確性，從而導致姿態(tài)的歧義[13]。本文通過將0#和1#碼上抬45 °構(gòu)建非共面組合二維碼，并結(jié)合IMU 傳感器，用于優(yōu)化相對位姿解算并解決位姿歧義問題（詳細分析見第2 節(jié)）。

圖1 組合二維碼設(shè)計示意圖Fig.1 Schematic diagram of marker map design

2 位姿歧義校正算法

2.1 位姿歧義問題

以平面二維碼為合作目標進行位姿解算時，多數(shù)情況下通過比較返回位姿結(jié)果的重投影誤差并使用最小重投影誤差可以判別正確的位姿解，但是當二維碼合作目標尺寸較小或相機與二維碼的距離遠遠大于相機的焦距時，二維碼角點檢測結(jié)果不可避免地攜帶較大的噪聲干擾。這時，二維碼在成像平面投影透視效應很弱（Weak-Perspective），二維碼平面與圖像平面之間單應矩陣近似為仿射變換（Affine Transformation），僅利用重投影誤差手段不足以判別兩個可能解的正確性，從而導致位姿解算的歧義。

如圖2所示，紅色圓區(qū)域表示二維碼角點可能的檢測誤差范圍，左右兩種顏色的立方體表示兩個可能的位姿結(jié)果T和T′（T與T′是以相機光心和二維碼中心連線為法線的平面的一對翻轉(zhuǎn)）。由于角點成像噪聲影響，無法區(qū)分這兩種位姿投影產(chǎn)生的成像結(jié)果，位姿解算結(jié)果會在T和T′這兩種結(jié)果之間隨機震蕩，使得位姿解算具有歧義性。此種情況下，盡管2D 像素位置幾乎沒有差別，但解算得出的兩3D 位姿差別較大。因此，通過隨機選取一個解或者計算兩個解的中間值都不是可行的方案。

圖2 位姿歧義示意圖Fig.2 Pose ambiguity

2.2 位姿解算與歧義校正

針對無人機動平臺著網(wǎng)回收應用場景，本文通過添加額外的二維碼構(gòu)建組合二維碼，并將0#和1#碼上抬45 °構(gòu)建非共面二維碼序列，使用非共面3D-2D 點對打破共面約束，用于優(yōu)化較近距離（≤10m）位姿解算。此外，通過融合IMU 傳感器引入姿態(tài)約束，解決無人機著網(wǎng)回收過程中的相對位姿解算歧義問題。

使用文獻[3]方法進行二維碼的檢測識別，獲取各二維碼角點坐標以進行后續(xù)的位姿解算。位姿解算與歧義校正流程如圖3所示，主要包含二維碼角點坐標優(yōu)化、極小化平面位姿估計算法（IPPE）、位姿歧義校正和位姿優(yōu)化等步驟。

2.2.1 角點坐標優(yōu)化

精確的角點坐標有利于提升位姿解算的精度，并能一定程度抑制位姿歧義的發(fā)生。二維碼識別步驟獲取了各二維碼粗略的角點坐標，攜帶較大的噪聲干擾，需要進行角點亞像素坐標優(yōu)化。

考慮到二維碼四個角點為其四條邊線交點的事實，單純通過設(shè)定近鄰窗口進行角點的亞像素優(yōu)化可能不滿足二維碼設(shè)計時的幾何約束。本文使用二維碼每個邊上的輪廓點進行最小二乘擬合獲取四條邊的直線方程，然后通過兩兩直線求交點計算四個角點的亞像素坐標。角點坐標優(yōu)化效果如圖3中所示，紅色角點為二維碼識別步驟獲得的粗略坐標，綠色點為亞像素優(yōu)化后的角點坐標，可以看出經(jīng)過坐標優(yōu)化，角點坐標更加準確，可以提升位姿解算精度，并在一定程度上避免位姿歧義的產(chǎn)生。

圖3 位姿解算與歧義校正處理流程Fig.3 Pose estimation and ambiguity correction

2.2.2 IPPE 位姿解算

IPPE（Infinitesimal Plane-based Pose Estimation）算法[14]是一種利用n（n≥ 4）個3D-2D 對應點對計算平面目標三維位姿的快速求解方法。IPPE 針對PPE問題進行優(yōu)化，對PPE 問題進行解析求解，其求解速度比通用PnP 求解方法要快得多，并且大多數(shù)情況下精度也更高。此外，IPPE 算法在處理位姿歧義情況時具有顯著的優(yōu)勢：IPPE 算法總是返回兩個候選位姿解算結(jié)果及其各自的重投影誤差，并且位姿結(jié)果使用重投影誤差進行排序，第一個返回結(jié)果具有較小的重投影誤差。如果第二個結(jié)果的重投影誤差比第一個要大得多，則選取第一個解作為位姿解算結(jié)果。如果兩個解的重投影誤差差別不大，則面臨位姿歧義的情況，僅依靠重投影誤差不能判別兩個可能解的正確性，事實上，由于此時重投影誤差包含較大噪聲，正確的位姿結(jié)果可能還具有較大的重投影誤差，需要額外的約束條件來判別正確的位姿結(jié)果。

將組合二維碼坐標系設(shè)定為世界坐標系（見圖1），設(shè)各二維碼坐標系到世界坐標系的變換矩陣為Ti w,i= 0,1,2s 。假設(shè)某張圖像中檢測識別到n個（n≥1）二維碼，首先使用IPPE 算法對每個檢測到的二維碼計算相對位姿，IPPE 返回兩個可能的解，記為Ti與Ti′,i∈ [0,1,2]，兩個解對應的重投影誤差分別記為ei與ei′,i∈ [0,1,2]。

接下來使用所有檢測到角點的平均重投影誤差初步篩選位姿解算結(jié)果。設(shè)相機內(nèi)參矩陣：

式中f x,fy分別為相機焦距，[p x,py]T為相機主點坐標。

記所有檢測到的原始圖像坐標系二維碼角點坐標為p j,j= 0,1 ??? 4n，對應的二維碼角點世界坐標為Pj,j= 0,1 ??? 4n，定義單個二維碼平均重投影誤差：

式中d=(k1,k2,p1,p2,k3)為畸變參數(shù)，f(K,d,Pj,Ti wT)使用變換矩陣Ti wT將世界坐標系3D點投影為圖像坐標系2D 點。

選取使所有的3D-2D點對平均重投影誤差最小的二維碼位姿T*與T*′作為最優(yōu)候選解算結(jié)果。

2.2.3 位姿歧義校正

比 較最 優(yōu) 候選位姿T*與T*′對 應的e*與e*′，令σ=e*′ /e*。當σ≥5.0（較大閾值）時認為位姿解算結(jié)果沒有歧義，否則進行位姿歧義校正。本文提出了引入機載IMU 水平姿態(tài)約束進行二維碼位姿歧義校正算法。

如圖4(a)所示，設(shè)最優(yōu)候選位姿對應二維碼中心在圖像平面的投影為pm：

式中pi,i∈ [0,1,2,3]為原始圖像坐標系的二維碼亞像素角點坐標。

設(shè)過相機光心的鉛垂線在圖像平面的投影點為pv，使用北東地（NED）坐標系下過光心的鉛垂線上一點V= [0,0,1]T求取pv。

相機與IMU 的坐標軸定義與安裝關(guān)系如圖4(a)所示，相機與鉛錘面的夾角為30 °，則IMU 到相機的方向余弦矩陣為：

式中，R(·)為旋轉(zhuǎn)矩陣。

設(shè) IMU 的滾動角為φ,φ∈[ -π,π]，俯仰角為θ,θ∈ [ -π/ 2,π/2]，航向角為ψ,ψ∈[ -π,π]，則北東地坐標系到IMU 的方向余弦矩陣為：

式中，R(·)為旋轉(zhuǎn)矩陣。

點V在相機坐標系下的坐標V′為

由于算法只關(guān)心IMU 提供的水平姿態(tài)，不依賴其航向角，因此，計算時可將ψ置為零，所以本文歧義校正算法不依賴于GPS 信息且消費級別的IMU 即可滿足算法需求。

點V在圖像平面的投影點pv為：

式中K為相機內(nèi)參矩陣，d為相機畸變參數(shù)，repj()表示投影變換，將相機坐標系3D 點投影到圖像坐標系2D 點。需要說明的是投影點pv坐標值可以超出圖像邊界而不影響后續(xù)計算結(jié)果。

點p m,pv的俯視示意圖如圖4(b)所示(以2#碼為例)。記二維碼坐標系x軸與二維碼邊線交點的圖像坐標為px，y軸與二維碼邊線交點的圖像坐標為py，則：

記圖像坐標系中x軸與y軸向量為：

式中·符號表示向量點積。

將二維碼位姿矩陣分解為旋轉(zhuǎn)與平移兩部分：

則相機在二維碼坐標系下的平移向量可表示為tc=-RTt= [tx,t y,tz]T。由著網(wǎng)回收場景硬件配置可知，二維碼在相機視野中時tz≥ 0恒成立。利用p m,pv的相對坐標關(guān)系即cosα,cosβ的符號可以確定t x,ty的符號，進而可以從T*與T*′中選取正確的位姿解。

由圖4(b)中所示點pv與點pm的坐標關(guān)系可知，cosα> 0,cosβ＜ 0。設(shè)T*與T*′對應的相機在二維碼坐標系下的平移向量分別為t c與tc′，t c與tc′ 的坐標分布如圖5(a)中所示，tc位于第四象限，tc′位于第二象限。由cosα> 0,cosβ＜ 0可知真實的平移向量位于第四象限，則與tc對應的T*為正確的位姿結(jié)果。

圖4 位姿歧義校正算法示意圖Fig.4 Pose ambiguity correction

由于IMU 精度限制和相機IMU 安裝誤差等原因，存在這樣的情況：相機在二維碼坐標系下的平移t c與tc′某個軸的分量值非常小，如圖4(b)中pv′點所示。pv′點y軸坐標值接近于零，其值可能主要由誤差組成，導致計算值與真實值反號，如圖5(b)所示。這時t c與tc′ 均不滿足 IMU 約束條件，需要特殊處理。選取cosα,cosβ中絕對值較大的一個，優(yōu)先保證與其對應的坐標軸分量，放寬另一個軸的符號使其滿足最接近的真實解。

圖5 通過IMU 約束選取正確結(jié)果示意圖Fig.5 Select the correct pose through IMU constraints

2.2.4 L-M 位姿優(yōu)化

經(jīng)過位姿解算與歧義校正步驟獲取了正確的組合二維碼位姿解算結(jié)果，為了充分利用所有檢測到的二維碼信息，利用所有二維碼角點構(gòu)建位姿優(yōu)化問題，對IPPE 解算結(jié)果進行微調(diào)。Levenberg-Marquadt[15]性能優(yōu)異，是常用的非線性優(yōu)化方法，本文使用L-M 算法進行位姿優(yōu)化。

設(shè)檢測識別到二維碼個數(shù)為n，記所有檢測到的原始圖像坐標系二維碼角點坐標為p j,j= 0,1 ??? 4n，對應的世界坐標為Pj,j= 0,1 ??? 4n，經(jīng)過位姿解算與歧義校正得到的初始位姿解為T*，位姿優(yōu)化的目標函數(shù)為：

式中K為相機內(nèi)參矩陣，d為畸變參數(shù)，i為T*對應的二維碼ID，f(K,d,Pj,Ti wT)將世界坐標系3D點投影為圖像坐標系2D 點。

在實際應用中，由于可能存在異常二維碼識別的情況，將導致優(yōu)化中存在一條誤差很大的邊，它的梯度也很大。如果對這種錯誤的邊不加限制，將會導致整體優(yōu)化結(jié)果偏向于誤匹配的邊，導致優(yōu)化結(jié)果失敗。本文使用Huber 魯棒核函數(shù)解決這個問題：

可以看到，當誤差e>δ時，誤差函數(shù)增長由二次形式變?yōu)橐淮?，有效地限制了梯度的最大值，同時，Huber 核函數(shù)又是光滑的，便于求導。

3 實驗驗證

3.1 實驗設(shè)置

以大疆經(jīng)緯M600 Pro 旋翼無人機為飛行載體，搭載英偉達Jetson TX2 嵌入式平臺、Bobcat1921 可見光相機、SBG Ellipse-D IMU 和OEM615 衛(wèi)星接收機等對本文算法進行實驗驗證?？梢姽庀鄼C最大分辨率為1920 ×1080，順時針旋轉(zhuǎn)90 °安裝，其全局快門（Global shutter）設(shè)計有利于高速飛行時圖像采集，相機鏡頭焦距f=8mm，實驗時F 數(shù)（F No.）設(shè)為4。組合二維碼由PP 板打印安裝并放置于實驗場地地面，二維碼上方設(shè)置紅色回收網(wǎng)以模擬回收平臺。算法定位精度使用差分衛(wèi)星（DGPS）接收機來評價，差分接收機由機載OEM615 移動站和地面OEM615 基站組成。

圖6 實驗無人機平臺及回收硬件設(shè)置Fig.6 Experimental UAV platform and recycling setup

3.2 定位精度分析

定位精度如圖7所示，“DGPS”曲線表示差分GPS定位結(jié)果，為定位真值，“OpenCV-PnP”曲線表示使用OpenCV 庫solvePnP 函數(shù)解算結(jié)果，“Our-IPPE”曲線表示使用去除位姿歧義校正步驟的位姿解算流程計算結(jié)果，“Our-IPPE-disambiguation”曲線表示添加位姿歧義校正步驟后計算結(jié)果。

圖7 定位精度曲線Fig.7 Positioning accuracy curve

由圖7可知（時間區(qū)間[28585,28605]秒無人機飛越二維碼合作目標，二維碼不可見）“OpenCV-PnP”方法解算結(jié)果在x,y,z軸方向上均出現(xiàn)了大的坐標值反轉(zhuǎn)（歧義），并在x軸分量上有許多小的坐標值歧義存在；“Our-IPPE”方法消除了大部分的小尺度坐標值歧義，但是仍舊含有大的坐標歧義存在；由“Our-IPPE-disambiguation”曲線可知，在添加位姿歧義校正步驟后有效地杜絕了各坐標軸方向上的解算歧義，實現(xiàn)了無人機定位結(jié)果的平滑與大幅精度提升。

為了對定位精度進行量化分析，引入絕對軌跡誤差（Absolute Pose Error,APE）根據(jù)時間差直接測量真實軌跡和估計軌跡點之間的差異。統(tǒng)計其均方根誤差（Root Mean Square Error,RMSE）、平均誤差（Mean）、中值（Median）、標準差（Std）、誤差最小值（Min）和最大值（Max）等，各誤差統(tǒng)計數(shù)據(jù)見表1。

由誤差統(tǒng)計結(jié)果可知，由于位姿解算歧義的存在，“OpenCV-PnP”與“Our-IPPE”方法具有非常大的誤差最大值，在破壞位姿解算連續(xù)性的同時導致多項誤差統(tǒng)計指標增大；“Our-IPPE-disambiguation”在標準差和均方根誤差等多項結(jié)果中均具有最小定位誤差，在整個無人機回收實驗過程中取得了0.245 m 的標準差與0.022 m 的最小定位誤差，具有非常好的定位結(jié)果。

表1 各方案定位誤差統(tǒng)計(單位：m)Tab.1 Positioning error statistics (unit: m)

使用本文提出的“Our-IPPE-disambiguation”完備位姿解算與歧義校正方案在多架次飛行實驗中的定位數(shù)據(jù)統(tǒng)計如表2所示?？梢钥闯?，三架次飛行實驗導航作用距離均大于30 m，回收全過程標準差均0.3 m左右，最小定位誤差僅為0.02 m 左右，定位精度完全滿足無人機動平臺著網(wǎng)回收控制需求，且各飛行架次定位結(jié)果差異不大，證明本文提出的導航算法具有較好的魯棒性。

表2 多架次數(shù)據(jù)統(tǒng)計(單位：m)Tab.2 Multiple flight data statistics (unit: m)

4 結(jié) 論

本文針對無人機動平臺著網(wǎng)回收應用場景，提出了一套基于二維碼合作目標的慣性/視覺位姿歧義校正解決方案。設(shè)計了易于遠距離辨識的9 位編碼防遮擋方形二維碼序列，并將0#與1#二維碼抬起一定角度構(gòu)建了非共面組合二維碼，用于優(yōu)化相對位姿解算與位姿歧義問題。通過二維碼角點坐標優(yōu)化、IPPE 位姿解算、位姿歧義校正和L-M 位姿優(yōu)化等步驟實現(xiàn)了快速魯棒位姿解算與歧義校正。室外多架次飛行實驗表明本文方法相較于OpenCV 庫與原生IPPE 算法消除了位姿解算歧義，提升了定位精度與魯棒性，導航作用距離大于30 m，回收末段最小定位誤差僅為2 cm左右，完全滿足無人機動平臺著網(wǎng)回收控制需求，具有較好的應用前景。