基于距離信息的無人機碰撞預測方案*

王長坤

（陸軍工程大學，江蘇 南京 210001）

0 引言

無人機安全問題是實現無人機集群飛行的前提。集群內部的避撞問題是困擾眾多學者的難題。在全球衛星導航系統（Global Navigation Satellite System，GNSS）環境中，個體獲取充足的狀態信息，通過路徑規劃和軌跡規劃可以實現個體和集群的安全飛行[1-2]。但是，在未知環境下，尤其是惡劣環境如森林、深山、室內等衛星拒止條件下，個體難以獲取位置服務，難以實現運動規劃。在衛星拒止環境中，通過添加傳感器獲取需要的狀態信息，慣導設備和GNSS 的組合導航是一種常用的解決方案[3]；有學者利用慣導結合藍牙測距和交互個體狀態信息來實現個體避撞[4]；視覺傳感器的發展更加成熟，也是實現個體避撞的有效手段之一[5]。

傳感器的增加在獲取更多的信息的同時，也給平臺帶來了更大的計算負擔和能源消耗，隨之增加了傳感器之間的相互干擾。利用盡可能少的傳感器實現無人機安全飛行是值得探究的課題。協同定位是解決據拒止環境難題的手段之一。它通過個體之間的信息交互，使得由于信息不足無法定位的節點滿足定位條件[6]。相比于其他感知方式，協同定位主要依靠通信設備測距，沒有視距、光線的環境限制，且有全向感知的優勢。

協同定位中常用藍牙或（Ultra-Wide Band，UWB)作為感知手段。文獻[7]中利用藍牙進行測距，利用測距信息進行碰撞預測，個體通過執行停止飛行、反向飛行或側向飛行的策略來避撞。UWB 由于精度高、優異的抗多徑效果被越來越多的用于距離感知。文獻[8]中在多節點勻速直線運動場景中，以多個靜態節點為錨點，利用UWB 在節點之間測距，利用測距信息進行碰撞預測，但需要犧牲多個節點充當錨點。文獻[9]利用MDS 變化估計相對速度，依賴距離的二階導數，受誤差影響較大。

以上文獻中有的是將測距信息作為一種輔助信息，結合速度方位等一起實現避撞；有的是使用多個靜止節點完成定位，利用位置制定避撞策略。這些方法證明了僅基于距離信息可以完成個體避撞，但是在陌生環境下移動的個體中并不適用，因此需要一種新的碰撞預測方案。

本文探究在多個體移動的場景中僅利用測距信息如何來實現碰撞預警，主要工作如下：

（1）提出一種僅基于距離信息的碰撞預測方案，分析在不同運動模型中預測效果差別和克拉美羅界下界；

（2）使用矩陣填充算法，即利用協同交互的測距信息填補缺失的距離信息，輔助完成碰撞預測；

（3）在運動模型未知時，使用均值檢驗進行運動模型識別，減小預測誤差。

1 系統模型

1.1 碰撞預測系統概述

無人機集群在未知環境中執行任務時，由于個體目標或者速度差異，可能會存在碰撞的危險。本文將無人機作為質點沒有考慮其飛行特性，個體利用UWB 通信設備測量彼此距離，個體之間交互測距信息。此處簡化集群中個體的運動模型為勻速運動（Constant Velocity，CV）和勻加速運動模型（Constant Acceleration，CA）兩種。多無人機碰撞場景，如圖1 所示。

圖1 多無人機碰撞場景

圖2 為本文提出的碰撞預測方案的系統流程圖。先通過測量和交互獲取距離信息矩陣，但通信測距可能會存在節點之間信息丟失的問題，此時無法獲取距離信息。為解決此問題，本文在碰撞預測方案中加入矩陣填充算法，在判斷距離矩陣出現缺失時，利用冗余的測距信息完成缺失信息填充。若是沒有缺失則直接進行下一步；接著利用運動模型檢測算法識別正確的運動模型，最后依據選擇的運動模型，匹配合適的碰撞預測方法進行碰撞預測。

圖2 碰撞預測方案系統流程

1.2 運動模型

運動模型如圖3 所示。

圖3 運動預測模型

1.2.1 勻速運動（CV）

如圖3（a）所示，個體A和個體B保持勻速運動，實際速度為vA和vB，初始位置為XA和XB，則二者的相對速度為v。假設在某一時刻二者最短距離為dm，當前位置到達最短距離時間為tmin。

最接近點（Closest Point of Approach，CPA）技術[10]是利用多個時刻鄰居個體的位置來進行預測，得到兩者之間最短距離和到達最短距離的時間，從而利用最短距離與預設的安全距離比較判斷是否發生碰撞。由于此處只有距離信息，無法正常使用CPA 檢測技術。但是，CPA 的實質是獲取最短距離值。參考此技術，利用多個時刻距離值求得最短距離進行碰撞檢測。

圖3（a）中左邊是在已知個體位置的坐標系中的情況，右邊是相對速度、最短距離以及到達時間的關系圖。假設參數α=(tmin,dm,v)，個體進行勻速運動，以此刻為起點，經歷時間t后，獲取的測量距離信息為d(t)，測距噪聲為ω，則有：

在式（1）中距離與時間的關系是一個二次方程，則參數α=(tmin,dm,v)與方程系數關系如下：

由此可以知參數α是決定距離變化的變量，無需知道個體的自身位置、速度以及正北方向也可以進行碰撞預測，從而擺脫絕對坐標系的限制。在進行預測時，使用的依然是式（1）的形式，并沒有使用式（2）的二階函數處理數據。因為模型中對參數有著更多的約束（如參數大于等于0 的默認約束，不會出現參數是負數的不合理情況）。

1.2.2 勻加速運動（CA）

在勻加速度場景中，預測方式變得不一樣，如圖3（b）所示。對于非直線運動，由于a和v都是矢量，無法用關系式直接計算出。

假設：

在非直線運動中存在6 個未知量，分別為β=(x0,y0,vx,vy,ax,ay)，分別是初始位置坐標、初始速度矢量和加速度矢量，由此可以得到：

k1=在相對運動中為已知量，即為初始測量的距離值d0的平方。此時，可以利用初始距離對相對位置(x0,y0)進行初始化(d0,0)，以簡化公式。參數β實際只有4 個未知量。在利用式（7）進行預測時，依然可以利用距離信息獲取距離隨時間的變化方程，實現碰撞預測。

只有當相對加速度和相對速度方向一致的特殊情況，會有式（2）的特殊公式。假設相對加速度恒定為a，初始相對速度為v，此時參數變為β=(tmin,dm,v,a)，距離變化的系統方程為：

如圖4 所示，利用獲取的測量數據（帶噪聲標準差0.1）進行回歸分析，在預測的曲線中獲取最短距離。為了不引入真實距離和噪聲的交叉項，這里直接使用獲取的測距值，不做平方處理。

圖4 CA 模型的預測示例

2 理論知識

2.1 運動模型識別

本文場景中存在勻速運動（CV）和勻加速運動模型（CA）兩種運動模型。勻速運動可以看作勻加速運動的特殊情況（加速度為0），數學公式中，勻速運動模型是二階函數，勻加速運動模型是四階函數。在數據處理時，對于同一段測距數據，使用更高階的模型，數據擬合誤差會更小，但是本文的最終目的是通過獲取的距離變化函數預測個體之間最短距離，即距離函數的最小值，并不是擬合誤差越小越好，所以不能用任一種模型同時預測兩種運動的碰撞參數。

為了驗證上文結論，進行仿真實驗。設置兩個運動個體，速度設置4 m/s，距離設置為30 m，分別設置CV 場景和CA 場景，加速度只設置其中一個。圖5（a）實驗為CV 場景，圖5（b)實驗為CA 場景，其中一個個體設置有加速度，結果如圖5 所示。

圖5 利用CV 和CA 模型預測CA 運動和CV 運動

在利用不匹配運動模型擬合時，利用CA 模型預測CV 模型，如圖5（a）所示；或是用CV 模型預測CA 模型，如圖5（b）所示，預測精度較差。由此可知，模型的正確選擇對預測的結果有重要影響，在預測前需要進行模型選擇。

本文希望通過對距離信息進行均值檢驗來識別運動模型。文獻[11-12]中使用t值檢驗和序貫檢測的方式進行多模型識別，給本文進行模式識別提供了研究思路。

對于距離信息處理使用的是最小二乘法，其中距離信息的量測方程為：

式中：D為測量距離值；H為測量矩陣；ω為測量噪聲。因此，只需要求得：

可以利用兩種運動模型進行碰撞預測，分別產生CV 和CA 模型的距離平方估計值和

在假設噪聲ω分布滿足ω～N(μ0,σ2)，均值為μ0，此時有：

可知，距離平方差依然滿足ω～N(μ0,σ2)。

對于距離平方差均值，有：

由CV 和CA 的運動模型式（2）和式（9）可知，距離平方差均值相同，但是均值不同，故而可以利用均值檢驗進行運動模型選擇。

此處選擇使用CV 模型進行檢驗：

因為加權最小二乘法能夠很好地處理噪聲帶來的異常數據，在此處用它與普通最小二乘法進行對比。對于加權最小二乘優化公式為：

2.2 克拉美-羅界（CRLB）

克拉美-羅界（Cramer-Rao Lower Bounds，CRLB）是針對參數估計問題提出的，為任何無偏估計量的方差確定了一個下限。無偏估計量的方差只能無限制的逼近CRLB，而不會低于CRLB。此節利用克拉美羅界對基于距離預測的誤差下界進行估計[8]。

對于勻速運動，有對數似然函數：

對于CA 運動，以直線運動的特殊情況為例，式（8）費舍爾信息矩陣變為：

假設此時有f(β)=d(t)-ω，則有：

則最短距離的方差估計界限為：

2.3 矩陣填充算法

當群體中少數節點之間由于通信丟包、信號遮擋等因素導致測距信息缺失時，使用矩陣填充的方法獲取測距信息，選用的算法是經典的交替最小二乘法（Alternating Least Squares，ALS），并在其中使用交替方向乘子法（Alternating Direction Method of Multipliers，ADMM）優化算法進行優化[13]。此處假設距離平方歐式矩陣為D，先對D進行雙中心化，得到先通過矩陣分解降低D矩陣維數，將其分解為兩個秩為k的低秩矩陣Um×k和Vn×k，Ω 為群體中個體的集合，帶有正則項λ的目標函數為：

此凸優化問題的增廣拉格朗日函數表達式如下：

因此，ADMM 算法的迭代步驟如下：

3 仿真結果

針對上述提出的碰撞預測方案進行仿真驗證，分別對單運動模型和多運動模型場景進行仿真，分析運動模型檢測方法的性能，并驗證當個體之間部分距離信息缺失時使用矩陣填充方法的預測效果。

3.1 運動模型檢測

仿真實驗設置為個體A 和個體B 初始速度為2 m/s，這里需要控制個體之間的最小距離。不同的距離下效果并不一樣，因為要預測碰撞，所以實驗中最短距離根據安全距離要求設置為3 m，主要為了測試在碰撞的界限處的預測效果，加速度設置在0 ＜a≤0.6m/s2。

結果如圖6 所示。

圖6 在較小加速度時，模型檢測方法效果測試

當加速度較小時，兩種模型的分界并不明顯，存在較大的誤判危險。從模型判斷角度看，當加速度小于一定數值時成功率較低，在此階段的模型判斷準確度較低。同時，加權最小二乘在此誤差條件下，從成功率變化角度看，效果不如普通最小二乘。在圖6 加速度范圍內，相同加速度普通最小二乘檢測成功率更高。運動模型檢測方法可以在一定的加速度范圍內成功識別運動模式。仿真中，普通的最小二乘法在加速度0.2 m/s2之后達到80%識別成功率，在0.3 m/s2之后達到90%識別成功率。

3.2 單運動模型下的預測誤差

3.2.1 入射角和預測數據量分析

此處不僅測試預測方案的效果，還對影響效果的多種因素進行分析。仿真設置兩節點相對運動,速度大小設置為4 m/s，距離設置為40 m，蒙特卡洛循環次數為100，入射角度隨機，其余信息未知。

固定其他條件，預測的最短距離誤差與入射的相對角度關系如圖7 所示。同理，預測誤差與預測的數據量關系如圖8 所示。

圖7 入射角度變化

圖8 預測數據量變化

在入射角度越小時，相同距離下，最短距離越小，此時預測的誤差逐漸增大；在圖8 中可以預測數據量在大于4 s 后預測效果更好。以上分析可以看出，在預測數據量選擇時，選用連續5 s 內的預測數據量較為合適。

在圖7和圖8中，在入射角接近0點時，克拉美-羅界趨向于無窮大，此時是由于最短距離趨近于0，從式（12）中看費舍爾矩陣趨近于不可逆，表示距離值的PDF 對最短距離依賴性變小，直至與之不相關（趨于無窮大）。但是，實際仿真中，當最短距離為0 時仿真精度并沒有變得很大。這并不是仿真出錯，因為CRLB 是理想的理論下界，隨著仿真次數的增加誤差會接近CRLB。但是，這里并沒有足夠的仿真次數。

前文的數據量并沒有控制與最短距離點的距離，通過克拉美-羅界分析可知，采樣數據距離最短距離點越近，預測誤差越小。為了控制每組數據同樣接近最短距離點，實驗設置如下。假設使用最短距離前的15 s 內60 個數據，為了排除距離最近點差別的因素，選取數據為10～15 s 內全部數據，5～15 s 內間隔1 個取值，0～15 s 內間隔2 個取值，最后選取全部數據對比。噪聲速度設置與3.2.1相同。

數據反映：3 組和4 組反映在最后一次采樣點與最近距離點距離相同時，使用的歷史采樣數據越多，預測越準確；在上述條件不變時，同時保持采樣量相同，1～3 組反應在相鄰數據差值越大，預測的RMSE 越小，即相對運動速度較大時預測更加準確。

表1 蒙特卡洛100 次循環，不同的數據類型下最短距離預測的RMSE

3.2.2 測距噪聲分析

在固定角度和預測數據量情況下，噪聲和預測誤差的關系如圖9 所示，分別為預測到達最短距離時間和最短距離的誤差分析。隨著噪聲的增大，預測的克拉美羅-界線性增大，但是實際預測值并不是線性增大。

圖9 噪聲對預測結果影響分析

由以上分析可知，對預測精度影響因素較多，主要分為兩類：一是引起參數θ變化，影響預測誤差，如相對速度大小、入射角度、預測時間等；二是噪聲影響原始數據準確性，引起預測精度變化，如測距誤差、信息丟失等。其中，入射角度是無法控制的因素。但是，對于噪聲和預測的數據量可以進行設置，以減小預測誤差。噪聲方面主要依賴傳感器的能力。預測的數量設置不可過大，否則當前狀態改變后難以即使更新預測。

3.3 矩陣填充預測效果

使用蒙特卡洛實驗法循環次數為500，節點數為6 個。假設每次實驗在任意單對節點之間出現數據丟失，相對速度設置為4 m/s，相對距離設置為40 m，最短距離隨機設置為0～10 m，預設安全距離3 m，測距噪聲ω～N(0,0.12)，判斷閾值和數據量設置如前文實驗。在不同噪聲條件下，測距信息丟失節點的碰撞預測效果如圖10 所示。

圖10 矩陣填充預測效果

由圖10 可見，隨著噪聲增加預測效果越來越差，在噪聲方差為0.1 時，預測成功率達到84%左右。在使用矩陣填充的數據和正常獲取的數據進行碰撞預測時，二者效果相差不大。可見，利用填充數據進行預測的方法是可行的，且效果與原始數據相當。這一方法不僅可以解決測距信息丟失的問題，而且對碰撞預測效果影響較小。

3.4 多運動模型下預測效果

對于個體狀態發生改變時，預測效果變化并未可知，本節測試在運動狀態變化時預測效果變化。實驗設置：選用6 節點分別設置編號，速度大小設置4 m/s，運動方向隨機設置，判斷閾值和數據量設置如上；初始位置隨機分布在50 m 的圓周內，測距噪聲ω～N(0,0.12)，運動時間設置為15 s；在15 s的運動中，2 號節點的運動狀態在0～6.5 s 內勻速運動，在6.5～15 s 內勻加速運動，加速度大小為1 m/s2。這里的誤差選用距離平方的均方差作為衡量1 號節點和2 號節點最短距離預測結果如圖11所示。圖11 中還將CA 和CV 模型作為對比，在預測的前段和后端效果較好，在中間段由于預測數據里既有勻速運動的測距值也有加速運動的測距值。本文的預測方法需要單一的運動模型下的測距數據才能進行預測，所以預測精度較差。結果顯示，提出的預測方案可以有效識別運動模型，做出碰撞預測。但是，提出的預測方法也存在缺陷：只適合運動模型相對簡單的場景；在實時多變的場景，會由于數據信息的混雜導致預測誤差難以接受。

圖11 復雜運動模型下的預測效果

4 結語

本文探究在僅有距離信息條件下個體之間的避撞預測方法，提出CA 或CV 未知運動模型下碰撞預測的方案，分析得出不同運動模型的預測方法互用會使得預測效果差，因此使用均值檢驗的方法進行運動模型識別。對于測距信息丟失無法完成預測的問題，使用矩陣填充算法填補缺失的測距信息，之后使用克拉美-羅界的分析運動參數對預測結果的影響。仿真表明，使用矩陣填充的碰撞預測效果與原始數據效果相當，在復雜運動中，提出的碰撞預測方案效果較于單模型的預測效果更好。但是，本文依然存在不足，如當加速度較小時個體難以正確判斷出運動模型。因此，下一步可能需要引入額外的傳感信息。