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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1 二維碼合作目標(biāo)設(shè)計(jì)

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

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

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

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

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

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

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

2 位姿歧義校正算法

2.1 位姿歧義問題

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

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

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

2.2 位姿解算與歧義校正

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

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

2.2.1 角點(diǎn)坐標(biāo)優(yōu)化

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

考慮到二維碼四個角點(diǎn)為其四條邊線交點(diǎn)的事實(shí)，單純通過設(shè)定近鄰窗口進(jìn)行角點(diǎn)的亞像素優(yōu)化可能不滿足二維碼設(shè)計(jì)時的幾何約束。本文使用二維碼每個邊上的輪廓點(diǎn)進(jìn)行最小二乘擬合獲取四條邊的直線方程，然后通過兩兩直線求交點(diǎn)計(jì)算四個角點(diǎn)的亞像素坐標(biāo)。角點(diǎn)坐標(biāo)優(yōu)化效果如圖3中所示，紅色角點(diǎn)為二維碼識別步驟獲得的粗略坐標(biāo)，綠色點(diǎn)為亞像素優(yōu)化后的角點(diǎn)坐標(biāo)，可以看出經(jīng)過坐標(biāo)優(yōu)化，角點(diǎn)坐標(biāo)更加準(zhǔn)確，可以提升位姿解算精度，并在一定程度上避免位姿歧義的產(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 對應(yīng)點(diǎn)對計(jì)算平面目標(biāo)三維位姿的快速求解方法。IPPE 針對PPE問題進(jìn)行優(yōu)化，對PPE 問題進(jìn)行解析求解，其求解速度比通用PnP 求解方法要快得多，并且大多數(shù)情況下精度也更高。此外，IPPE 算法在處理位姿歧義情況時具有顯著的優(yōu)勢：IPPE 算法總是返回兩個候選位姿解算結(jié)果及其各自的重投影誤差，并且位姿結(jié)果使用重投影誤差進(jìn)行排序，第一個返回結(jié)果具有較小的重投影誤差。如果第二個結(jié)果的重投影誤差比第一個要大得多，則選取第一個解作為位姿解算結(jié)果。如果兩個解的重投影誤差差別不大，則面臨位姿歧義的情況，僅依靠重投影誤差不能判別兩個可能解的正確性，事實(shí)上，由于此時重投影誤差包含較大噪聲，正確的位姿結(jié)果可能還具有較大的重投影誤差，需要額外的約束條件來判別正確的位姿結(jié)果。

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

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

式中f x,fy分別為相機(jī)焦距，[p x,py]T為相機(jī)主點(diǎn)坐標(biāo)。

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

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

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

2.2.3 位姿歧義校正

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

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

式中pi,i∈ [0,1,2,3]為原始圖像坐標(biāo)系的二維碼亞像素角點(diǎn)坐標(biāo)。

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

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

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

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

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

點(diǎn)V在相機(jī)坐標(biāo)系下的坐標(biāo)V′為

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

點(diǎn)V在圖像平面的投影點(diǎn)pv為：

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

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

記圖像坐標(biāo)系中x軸與y軸向量為：

式中·符號表示向量點(diǎn)積。

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

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)設(shè)置

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

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

3.2 定位精度分析

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

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

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

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

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

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

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

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

4 結(jié) 論

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