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

空軍工程大學 空管領航學院，西安 710051

1 引言

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

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

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

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

2 量測模型

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

2.1 單目視覺量測模型

以相機光心為原點建立攝像坐標系Oc-XcYcZc，OcXc光軸與成像平面的交點o為像平面上圖像坐標系原點，單個攝像機與目標成像關系如圖1所示。

圖1 針孔成像模型

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

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

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

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

2.2 AOA量測模型

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

圖2 三維定位場景

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

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

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

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

3 方法設計

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

3.1 三維定位算法

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

3.1.1 二維偽線性估計

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

最小二乘解為：

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

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

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

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

建立三維偽線性方程：

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

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

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

其中：

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

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

3.2 量測點選取

AOA 定位精度不僅受定位算法影響，也與量測點選取有關[15]。設定無人機飛行高度不變，在水平面內(nèi)選取N處量測點，則θi與的選取決定了所選量測點的空間位置。基于克拉美羅界（Cramer-Rao Bound，CRB）理論，平面內(nèi)FIM矩陣J及其特征值det(J)可表示為：

則CRB可表示為：

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

得出N處量測點的兩種分布，等角分布（UAA）與非等角分布（Non-UAA）的角度關系分別為：

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

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

圖3 量測點分布

3.3 任務流程設計

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

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

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

步驟3在初始位置附近區(qū)域隨機選取一組量測點，搜索識別目標進行量測，得出目標位置估計值；

步驟4依照非等角分布規(guī)劃N處量測點坐標；

步驟5無人機飛行至下一量測點；

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

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

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

4 仿真與實驗

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

4.1 算法仿真

設目標坐標為原點[0,0,0]T，在以原點為中心200 m×200 m×200 m 的立方體上半部分生成N處量測點坐標。設量測噪聲均不相關，則所有協(xié)方差矩陣為對角陣，且無人機和云臺相機的測量噪聲方差在測量過程中的各量測點均相同。根據(jù)GPS 和云臺相機設備出廠參數(shù)，取無人機位置測量噪聲標準差σx=σy=0.5 m，σz=0.1 m ，角度測量噪聲標準差σ=σ=0.2°。基于以上噪

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

（1）令量測點數(shù)量N=5，隨機生成坐標，求取定位偏差均值，驗證算法定位精度；

（2）改變量測點數(shù)量，隨機生成坐標，求解量測點數(shù)量對定位精度的影響；

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

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

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

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

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

圖4 平均偏差收斂情況

圖5 量測點數(shù)量對定位精度影響

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

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

圖6 無人機高度對定位精度影響

4.2 實驗驗證

如圖7 所示，搭建四旋翼無人機為量測平臺，地面站為控制處理中心。每處量測點懸停穩(wěn)定時，GPS 信息、高度、航向角和機載云臺角度信息以及云臺相機對靶標的拍照圖像實時發(fā)送至地面站的通信模塊和數(shù)字圖傳并存儲[18]。

圖7 實驗驗證平臺

10個靶標在150 m×50 m場地上分散布設，選取靶標1作為原點[0,0,0]T建立東北天（ENU）坐標系O-XYZ，得到所有靶標的位置關系如圖8所示。

圖8 靶標示意圖

靶標的絕對坐標由差分GPS 測量，高度均設為0 m。真實坐標如表1所示。

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

表1 靶標在東北天坐標系的真實坐標 m

表2 量測點坐標 m

表3 定位結(jié)果 m

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

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

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

5 結(jié)束語

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

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