熊 航，楊任農(nóng)，梁曉龍，侯岳奇，王維佳

空軍工程大學(xué) 空管領(lǐng)航學(xué)院，西安 710051

1 引言

無人機(jī)（UAV）小型化、自動(dòng)化和機(jī)載計(jì)算等能力的提升使其近年來在各領(lǐng)域得到廣泛應(yīng)用。其中，無人機(jī)對(duì)目標(biāo)的精確定位已經(jīng)成為重要研究方向。計(jì)算機(jī)視覺（Computer Vision）技術(shù)的發(fā)展為無人機(jī)快速、精確地完成目標(biāo)定位提供了可能[1]。

單目視覺（Monocular Vision）傳感器具有精確度高，成本低，且數(shù)據(jù)信息豐富等特點(diǎn)[2]。將單目視覺與無人機(jī)相結(jié)合，學(xué)者開展了大量利用機(jī)載CCD（Charge-Coupled Device）攝像頭進(jìn)行目標(biāo)定位的應(yīng)用研究。文獻(xiàn)[3-4]利用單目視覺實(shí)現(xiàn)了空中加油管錐套識(shí)別和三維定位，但算法依賴于加油管錐套的環(huán)形結(jié)構(gòu)，對(duì)其他形狀的目標(biāo)不具一般性。文獻(xiàn)[5]基于已知的農(nóng)田和標(biāo)志參數(shù)，利用無人機(jī)單目攝像頭對(duì)農(nóng)田飛艇進(jìn)行定位，不僅需要先驗(yàn)信息且地形環(huán)境的變化會(huì)直接導(dǎo)致定位精度波動(dòng)。文獻(xiàn)[6]矯正無人機(jī)單目攝相圖像的畸變后，根據(jù)參考物的尺寸完成了工地設(shè)施間的相對(duì)定位，但未完全反映三維空間的位置關(guān)系且需要已知的參考尺寸。以上特定場(chǎng)景的單目視覺三維定位，存在對(duì)先驗(yàn)背景信息依賴性強(qiáng)、定位精度受環(huán)境影響的問題。到達(dá)角（Angle of Arrival，AOA）定位[7]無需先驗(yàn)環(huán)境信息，且AOA定位精度只受傳感器自身位置誤差和量測(cè)基線角度偏差的影響。故基于AOA的無人機(jī)單目視覺定位為解決上述問題提供了新的途徑。

AOA定位的主要難點(diǎn)在于量測(cè)數(shù)據(jù)和目標(biāo)位置非線性相關(guān)，這對(duì)定位精度提出了較高的要求。文獻(xiàn)[7-11]進(jìn)行了提高二維平面AOA 定位精度的算法研究，但只適用于遠(yuǎn)距定位時(shí)忽略高度的情況，無法直接運(yùn)用于三維定位。在精度需求較高的三維場(chǎng)景，AOA 三維定位的方向角和俯仰角耦合于非線性量測(cè)方程中，因此對(duì)算法提出了更大挑戰(zhàn)。文獻(xiàn)[12]運(yùn)用工具變量（Instrumental Variable，IV）降低了三維定位偏差。文獻(xiàn)[13]提出了一種旋轉(zhuǎn)變量偽線性估計(jì)器（Rotation Variant Pseudolinear Estimator，RVPLE）利用傳感器幾何旋轉(zhuǎn)角之間的關(guān)系減小三維定位的偏差。文獻(xiàn)[14]提出了改進(jìn)的IV 估計(jì)，通過減去偏差估計(jì)值來補(bǔ)償偏差并取得了很好的效果。以上AOA三維定位算法均未考慮傳感器自身位置的定位誤差且AOA定位精度也與量測(cè)點(diǎn)的選取相關(guān)[15]。

以精確定位目標(biāo)為任務(wù)背景，選取小型旋翼無人機(jī)平臺(tái)，搭載CCD 云臺(tái)相機(jī)，基于AOA 原理利用單目視覺量測(cè)參數(shù)進(jìn)行定位，解決了傳統(tǒng)單目視覺定位對(duì)先驗(yàn)背景信息依賴性強(qiáng)以及環(huán)境變化對(duì)精度影響的問題。首先建立了單目視覺針孔成像與AOA三維定位參數(shù)的量測(cè)模型。然后考慮無人機(jī)自身GPS 定位誤差和角度量測(cè)誤差，先完成目標(biāo)二維坐標(biāo)的偽線性估計(jì)（Pseudolinear Estimator，PLE），得出目標(biāo)坐標(biāo)的初步估計(jì)值以計(jì)算量測(cè)點(diǎn)與目標(biāo)之間的距離，再代入三維偽線性方程通過加權(quán)最小二乘（Weighted Least-Squares，WLS）求解目標(biāo)的三維坐標(biāo)估計(jì)值。根據(jù)非等角分布選取量測(cè)點(diǎn)位置，設(shè)計(jì)旋翼無人機(jī)分時(shí)異地依次在規(guī)劃的量測(cè)點(diǎn)上懸停并通過云臺(tái)相機(jī)獲取目標(biāo)參數(shù)，實(shí)現(xiàn)了單目視覺的多點(diǎn)量測(cè)。最后，設(shè)計(jì)了無人機(jī)單目視覺定位的任務(wù)流程。仿真與實(shí)驗(yàn)部分驗(yàn)證了方法的有效性。

2 量測(cè)模型

AOA 定位中，目標(biāo)與量測(cè)點(diǎn)連線的空間角度和量測(cè)點(diǎn)位置信息為定位所需參數(shù)。單目攝像機(jī)的成像模型解決了從圖像中獲取目標(biāo)到達(dá)角的問題，無人機(jī)在量測(cè)點(diǎn)處的GPS 參數(shù)視為量測(cè)點(diǎn)的估計(jì)位置。角度與位置量測(cè)值均含噪聲誤差。

2.1 單目視覺量測(cè)模型

以相機(jī)光心為原點(diǎn)建立攝像坐標(biāo)系Oc-XcYcZc，OcXc光軸與成像平面的交點(diǎn)o為像平面上圖像坐標(biāo)系原點(diǎn)，單個(gè)攝像機(jī)與目標(biāo)成像關(guān)系如圖1所示。

圖1 針孔成像模型

根據(jù)針孔成像模型，目標(biāo)p在圖像坐標(biāo)系yoz的像為Oc p連線與像平面的交點(diǎn)q，坐標(biāo)為(yl,zl)。設(shè)Oc p在攝像坐標(biāo)系中的方向角為θl（光軸為0°，順時(shí)針為正），俯仰角為φl（水平為0°，向上為正），光心到像平面的距離為等效焦距f，由圖1中幾何關(guān)系可得[16]：

圖像坐標(biāo)(yl,zl)到像素坐標(biāo)(ul,vl)轉(zhuǎn)換公式為：

dy、dz為單位像素在坐標(biāo)軸方向上的物理尺寸；u0、v0為圖像坐標(biāo)系原點(diǎn)o的像素單位坐標(biāo)。式（2）代入式（1）得：

根據(jù)式（3），若已知ul、vl、u0、v0和等效焦距f與像元大小dy、dz之比，即可求得像素點(diǎn)在攝像坐標(biāo)系中的方向角和俯仰角。

2.2 AOA量測(cè)模型

如圖2 所示，單架旋翼無人機(jī)搭載CCD 云臺(tái)相機(jī)，分時(shí)異地懸停于量測(cè)點(diǎn)，對(duì)目標(biāo)進(jìn)行N次成像拍攝。設(shè)在世界坐標(biāo)系下，目標(biāo)的真實(shí)坐標(biāo)；將無人機(jī)和云臺(tái)相機(jī)的實(shí)時(shí)位置視為重合一點(diǎn)，無人機(jī)位置坐標(biāo)即為第i次量測(cè)點(diǎn)坐標(biāo)，無人機(jī)真實(shí)位置，實(shí)際量測(cè)坐標(biāo)由GPS 測(cè)量得出ui=[xi,yi,zi]T。N次測(cè)量無人機(jī)真實(shí)坐標(biāo)實(shí)際量測(cè)坐標(biāo)(u1,u2,…,uN)，定義坐標(biāo)向量考慮GPS誤差有：

圖2 三維定位場(chǎng)景

Δu=是服從零均值高斯分布的隨機(jī)向量，方差為，協(xié)方差矩陣為Qu。同時(shí)，定義U=[u1,u2,…,uN]T。

云臺(tái)旋轉(zhuǎn)角(θci,φci)是云臺(tái)自身測(cè)量參數(shù)，目標(biāo)成像角(θli,φli)通過式（3）獲取，量測(cè)點(diǎn)的方位角θi=θci+θli和俯仰角φi=φci+φli即為兩者之和。設(shè)角度真實(shí)值為和，則：

θ=[θ1,θ2,…,θN]T，φ=[φ1,φ2,…,φN]T為角度量測(cè)向量，為真實(shí)值；n=[n1,n2,…,nN]T，ω=[ω1,ω2,…,ωN]T是零均值高斯噪聲，方差和協(xié)方差矩陣分別為。

式（4）、（5）、（6）即為AOA三維定位所需量測(cè)參數(shù)。

3 方法設(shè)計(jì)

利用量測(cè)參數(shù)，在三維坐標(biāo)系中同時(shí)考慮無人機(jī)自身位置誤差和量測(cè)角度誤差進(jìn)行AOA 定位，并基于Fisher 信息矩陣（Fisher Information Matrix，F(xiàn)IM）討論定位過程中量測(cè)點(diǎn)選取的問題，最后設(shè)計(jì)無人機(jī)單目視覺定位流程。

3.1 三維定位算法

建立三維偽線性方程需要已知量測(cè)點(diǎn)與目標(biāo)之間的距離di。故先進(jìn)行二維方向的偽線性估計(jì)，得出目標(biāo)坐標(biāo)的初步估計(jì)值?，求出距離估計(jì)值，再代入三維偽線性方程，運(yùn)用加權(quán)最小二乘求得最終定位坐標(biāo)，使其偏差最小。

3.1.1 二維偽線性估計(jì)

將無人機(jī)第i次測(cè)量處ui與目標(biāo)的位置關(guān)系由三維投影至xoy平面，ui(1:2)取ui的前兩個(gè)元素，得到關(guān)于目標(biāo)實(shí)際二維坐標(biāo)的偽線性方程[17]：

最小二乘解為：

通過算術(shù)平均得到z坐標(biāo)：

式（8）、（9）的結(jié)果并未考慮量測(cè)噪聲和無人機(jī)自身位置偏差，故其估計(jì)值只作為下一步三維偽線性方程中估算量測(cè)點(diǎn)和目標(biāo)之間距離的條件使用。

3.1.2 三維加權(quán)最小二乘估計(jì)

在3.1.1 節(jié)的基礎(chǔ)上，同時(shí)考慮角度量測(cè)噪聲和無人機(jī)位置誤差，定義：

建立三維偽線性方程：

其中，右式2N×3 矩陣G和2N×1 向量h分別為：

1 為全1向量，⊙為哈達(dá)馬積，U=[u1,u2,…,uN]T。

左式中，ε=[nT,ωT,ΔuT]T，協(xié)方差矩陣Q=diag(Qn,Qω,Qu)；2N×5N矩陣R為：

其中：

為目標(biāo)與量測(cè)點(diǎn)之間的估計(jì)距離，利用目標(biāo)位置估計(jì)值?與無人機(jī)位置量測(cè)值得出。運(yùn)用加權(quán)最小二乘優(yōu)化得到式的解：

其中加權(quán)矩陣W=(RQRT)-1。

3.2 量測(cè)點(diǎn)選取

AOA 定位精度不僅受定位算法影響，也與量測(cè)點(diǎn)選取有關(guān)[15]。設(shè)定無人機(jī)飛行高度不變，在水平面內(nèi)選取N處量測(cè)點(diǎn)，則θi與的選取決定了所選量測(cè)點(diǎn)的空間位置?；诳死懒_界（Cramer-Rao Bound，CRB）理論，平面內(nèi)FIM矩陣J及其特征值det(J)可表示為：

則CRB可表示為：

設(shè)目標(biāo)函數(shù)為f(θ)且量測(cè)點(diǎn)與目標(biāo)距離固定，使得CRB 最小的角度θi分布即為所選取的N處量測(cè)點(diǎn)分布。FIM最大特征值對(duì)應(yīng)CRB跡的最小值，則優(yōu)化問題轉(zhuǎn)化為：

得出N處量測(cè)點(diǎn)的兩種分布，等角分布（UAA）與非等角分布（Non-UAA）的角度關(guān)系分別為：

無人機(jī)在第i個(gè)量測(cè)點(diǎn)獲取參數(shù)后，保持與目標(biāo)距離不變，與目標(biāo)連線角度分別偏轉(zhuǎn)直至分別繞目標(biāo)一周和半周后完成量測(cè)過程。

兩種量測(cè)點(diǎn)選取均可得到最優(yōu)分布[15]，但考慮非等角分布所需無人機(jī)飛行航時(shí)更短，因此選取Non-UAA進(jìn)行定位，如圖3所示。

圖3 量測(cè)點(diǎn)分布

3.3 任務(wù)流程設(shè)計(jì)

根據(jù)量測(cè)點(diǎn)的分布選取和旋翼無人機(jī)平臺(tái)CCD云臺(tái)相機(jī)工作原理，設(shè)計(jì)基于AOA 的無人機(jī)單目視覺三維定位流程如下：

步驟1設(shè)置初始懸停點(diǎn)參數(shù)，載入無人機(jī)并發(fā)送指令，起飛升空至設(shè)定位置；

步驟2判斷有無目標(biāo)坐標(biāo)的先驗(yàn)信息，若有，則轉(zhuǎn)到步驟4；

步驟3在初始位置附近區(qū)域隨機(jī)選取一組量測(cè)點(diǎn)，搜索識(shí)別目標(biāo)進(jìn)行量測(cè)，得出目標(biāo)位置估計(jì)值；

步驟4依照非等角分布規(guī)劃N處量測(cè)點(diǎn)坐標(biāo)；

步驟5無人機(jī)飛行至下一量測(cè)點(diǎn)；

步驟6云臺(tái)相機(jī)搜索識(shí)別目標(biāo)，穩(wěn)定懸停后，計(jì)算并記錄角度參數(shù)θi、φi以及GPS定位參數(shù)；

步驟7判斷是否飛行遍歷所有量測(cè)點(diǎn)，若否，則轉(zhuǎn)到步驟5；

步驟8將N組量測(cè)參數(shù)代入式（8）～（17），得到目標(biāo)坐標(biāo)定位值。

4 仿真與實(shí)驗(yàn)

為驗(yàn)證方法有效性，分別進(jìn)行算法仿真與實(shí)驗(yàn)驗(yàn)證。仿真環(huán)境為i7-8565U，主頻1.8 GHz，8 GB 內(nèi)存，基于MATLAB 2017a環(huán)境代入實(shí)際參數(shù)進(jìn)行仿真。實(shí)驗(yàn)平臺(tái)主要組成為：飛行控制器、機(jī)載計(jì)算平臺(tái)、通信模塊、圖傳模塊、云臺(tái)相機(jī)和地面站。

4.1 算法仿真

設(shè)目標(biāo)坐標(biāo)為原點(diǎn)[0,0,0]T，在以原點(diǎn)為中心200 m×200 m×200 m 的立方體上半部分生成N處量測(cè)點(diǎn)坐標(biāo)。設(shè)量測(cè)噪聲均不相關(guān)，則所有協(xié)方差矩陣為對(duì)角陣，且無人機(jī)和云臺(tái)相機(jī)的測(cè)量噪聲方差在測(cè)量過程中的各量測(cè)點(diǎn)均相同。根據(jù)GPS 和云臺(tái)相機(jī)設(shè)備出廠參數(shù)，取無人機(jī)位置測(cè)量噪聲標(biāo)準(zhǔn)差σx=σy=0.5 m，σz=0.1 m ，角度測(cè)量噪聲標(biāo)準(zhǔn)差σ=σ=0.2°?；谝陨显?/p>

nω聲參數(shù)，以目標(biāo)定位估計(jì)值與真實(shí)值之間的距離偏差為定位精度衡量標(biāo)準(zhǔn)，本節(jié)進(jìn)行三組算法仿真：

（1）令量測(cè)點(diǎn)數(shù)量N=5，隨機(jī)生成坐標(biāo)，求取定位偏差均值，驗(yàn)證算法定位精度；

（2）改變量測(cè)點(diǎn)數(shù)量，隨機(jī)生成坐標(biāo)，求解量測(cè)點(diǎn)數(shù)量對(duì)定位精度的影響；

（3）量測(cè)點(diǎn)在水平面內(nèi)非等角分布的坐標(biāo)不變，改變量測(cè)點(diǎn)高度坐標(biāo)，仿真求解最佳定位高度。

仿真1仿真次數(shù)1 至1 000 逐次設(shè)定，求解每組仿真次數(shù)下的平均定位偏差。

平均定位偏差隨仿真次數(shù)變化情況如圖4 所示。200 m×200 m×100 m 空間范圍內(nèi)的平均定位偏差在0.4～0.7 m浮動(dòng)且逐漸收斂。

仿真2改變量測(cè)點(diǎn)數(shù)量N從2 到100，每組仿真1 000次。

量測(cè)點(diǎn)數(shù)量N變化時(shí)，平均定位偏差值的變化情況如圖5 所示。隨量測(cè)點(diǎn)數(shù)量增加，平均定位偏差由1.1 m 降至 0.2 m。其中，0～10 區(qū)間內(nèi)趨勢(shì)變化快，10～100區(qū)間內(nèi)趨勢(shì)平緩?？紤]合理減少定位時(shí)長(zhǎng)和數(shù)據(jù)處理復(fù)雜度，選取N=10 為實(shí)驗(yàn)參數(shù)。

圖4 平均偏差收斂情況

圖5 量測(cè)點(diǎn)數(shù)量對(duì)定位精度影響

仿真3設(shè)定N=10，在水平方向按圖3所示的非等角分布取10 處量測(cè)點(diǎn)的坐標(biāo)固定。改變量測(cè)點(diǎn)高度5 m至150 m，每組仿真10 000次。

量測(cè)點(diǎn)高度變化時(shí)平均定位偏差值變化如圖6 所示。高度在50 m 以下時(shí)定位偏差保持在0.37 m 附近。50 m以上高度，隨著量測(cè)點(diǎn)與目標(biāo)高度距離增大，定位精度不斷降低。綜合考慮無人機(jī)實(shí)際飛行安全高度以及在定位偏差盡量小的情況下擴(kuò)大搜索范圍（即航拍視野），選取60 m作為量測(cè)點(diǎn)高度。

圖6 無人機(jī)高度對(duì)定位精度影響

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

如圖7 所示，搭建四旋翼無人機(jī)為量測(cè)平臺(tái)，地面站為控制處理中心。每處量測(cè)點(diǎn)懸停穩(wěn)定時(shí)，GPS 信息、高度、航向角和機(jī)載云臺(tái)角度信息以及云臺(tái)相機(jī)對(duì)靶標(biāo)的拍照?qǐng)D像實(shí)時(shí)發(fā)送至地面站的通信模塊和數(shù)字圖傳并存儲(chǔ)[18]。

圖7 實(shí)驗(yàn)驗(yàn)證平臺(tái)

10個(gè)靶標(biāo)在150 m×50 m場(chǎng)地上分散布設(shè)，選取靶標(biāo)1作為原點(diǎn)[0,0,0]T建立東北天（ENU）坐標(biāo)系O-XYZ，得到所有靶標(biāo)的位置關(guān)系如圖8所示。

圖8 靶標(biāo)示意圖

靶標(biāo)的絕對(duì)坐標(biāo)由差分GPS 測(cè)量，高度均設(shè)為0 m。真實(shí)坐標(biāo)如表1所示。

考慮風(fēng)力等天氣因素對(duì)實(shí)驗(yàn)的影響，選擇正午晴朗時(shí)分進(jìn)行無人機(jī)飛行定位實(shí)驗(yàn)。根據(jù)4.1 節(jié)中的仿真結(jié)果，設(shè)定無人機(jī)飛行高度為60 m，初始位置在靶標(biāo)群南向，量測(cè)點(diǎn)以半徑為100 m 的半圓按非等角分布。云臺(tái)相機(jī)采取跟隨機(jī)頭模式，故無人機(jī)航向角θc（正北為0°，順時(shí)針為正）即為云臺(tái)航向角，俯仰角φc（水平為0°，向上為正）由云臺(tái)自身獲取。無人機(jī)在10處量測(cè)點(diǎn)處進(jìn)行搜索、識(shí)別、拍攝，通過圖傳和數(shù)傳將圖像與無人機(jī)的經(jīng)度(lon)、緯度(lat)和高度(alt)信息傳至地面站。

表1 靶標(biāo)在東北天坐標(biāo)系的真實(shí)坐標(biāo) m

表2 量測(cè)點(diǎn)坐標(biāo) m

表3 定位結(jié)果 m

采集圖像中10個(gè)靶標(biāo)的像素坐標(biāo)，根據(jù)單目視覺模型求得靶標(biāo)相對(duì)云臺(tái)的角度信息θli、φli(i=1,2,…,10)，其中事先測(cè)定相機(jī)的等效焦距與像元大小比值參數(shù)為(3.68/3)×103。結(jié)合云臺(tái)角度和靶標(biāo)相對(duì)云臺(tái)角度得到10處量測(cè)點(diǎn)相對(duì)靶標(biāo)的角度。經(jīng)無人機(jī)GPS和高度信息轉(zhuǎn)換后，10 處量測(cè)點(diǎn)在O-XYZ中的坐標(biāo)如表2 所示。將角度信息及表2坐標(biāo)代入4.1節(jié)算法求得10個(gè)靶標(biāo)的定位坐標(biāo)，結(jié)果與真實(shí)值比較如表3所示。

如表3所示，對(duì)靶標(biāo)的定位距離偏差最小為0.22 m，最大為1.37 m，平均為0.827 m。4.1節(jié)仿真中，量測(cè)點(diǎn)數(shù)量為10，高度為60 m情況下的平均定位偏差在0.37～0.4 m區(qū)間。實(shí)驗(yàn)取得了不錯(cuò)的定位效果。

與仿真結(jié)果相比，實(shí)驗(yàn)定位誤差有所增大，可能與實(shí)際量測(cè)量過程中無人機(jī)GPS定位偏差較大、云臺(tái)相機(jī)受環(huán)境不確定影響導(dǎo)致角度抖動(dòng)過大，或者采集靶標(biāo)的像素坐標(biāo)過程中誤差偏大有關(guān)。實(shí)際誤差與仿真精度差異的根本原因在于實(shí)際量測(cè)過程中環(huán)境、人為因素帶來的噪聲誤差協(xié)方差矩陣的不確定性，GPS和云臺(tái)設(shè)備出廠參數(shù)與真實(shí)的無人機(jī)坐標(biāo)量測(cè)誤噪聲Δu的協(xié)方差矩陣Qu以及角度量測(cè)噪聲n、ω的協(xié)方差矩陣Qn、Qω存在偏差，導(dǎo)致最終計(jì)算的估計(jì)值與仿真結(jié)果有一定差異。

5 結(jié)束語

針對(duì)現(xiàn)有無人機(jī)單目視覺定位方法中對(duì)先驗(yàn)信息依賴和地形環(huán)境對(duì)精度影響的問題，提出了基于AOA的無人機(jī)單目視覺三維定位。建立了量測(cè)模型，采用二維偽線性估計(jì)基礎(chǔ)上的三維加權(quán)最小二乘算法以及非等角分布量測(cè)點(diǎn)選取方法，設(shè)計(jì)了任務(wù)流程并進(jìn)行了算法仿真與實(shí)驗(yàn)驗(yàn)證。設(shè)定條件下，算法仿真的定位距離偏差大約為0.37 m；實(shí)機(jī)實(shí)驗(yàn)中對(duì)靶標(biāo)的定位保持在1 m左右，驗(yàn)證了方法的有效性，同時(shí)分析了實(shí)驗(yàn)與仿真之間精度差異的根本原因。

為減小定位方法的系統(tǒng)誤差，后續(xù)計(jì)劃開展以下工作：（1）改進(jìn)AOA三維定位算法；（2）研究三維定位的最優(yōu)量測(cè)點(diǎn)（最優(yōu)構(gòu)型）選取方法；（3）結(jié)合集群化思想，將無人機(jī)單目視覺分時(shí)異地的量測(cè)點(diǎn)定位方式拓展為無人機(jī)集群的多點(diǎn)、同時(shí)的協(xié)同定位，增加量測(cè)點(diǎn)數(shù)量并提高定位實(shí)時(shí)性。