農業植保無人機變結構線性滾動時域估計器研究

楚紅雨 倪俊超 常志遠 邵延華 張曉強

(西南科技大學信息工程學院， 綿陽 621010)

0 引言

植保無人機的航跡規劃是保證其完成植保作業任務的關鍵技術，高精度、低延時的位置估計算法一直是該領域的研究重點[1-2]。位置估計器需要融合多傳感器的異構數據，例如，植保無人機的定位可以通過融合嵌入式傳感器提供的測量數據，然后估計出植保無人機實時的位置信息。當位置估計器融合慣導數據和GPS導航數據時，數據采樣過程中存在多傳感器采樣頻率不同而造成的延時，從而影響位置信息估計的準確性[3-4]。

目前，國內外學者對多傳感器采樣頻率不同造成的延時問題展開了研究。文獻[5]提出使用預測器來補償延時的方法，但是將預測器應用到系統中會增加計算時間和內存使用率，不適用于計算資源有限、實時性較高的飛控系統。文獻[6]提出了3種不同的優化方法處理多目標無序測量造成的延時問題，其中CISI (Complete in-sequence information)方法效果最好，但是該方法結構比較復雜。文獻[7]根據歷史測量值，利用其中的延時測量值重新估計整個后驗狀態序列，通過數值模擬的方法進行驗證。文獻[8]設計了一個正則化方法，并在滾動時域估計器(MHE)中給出一個具體的代價函數，根據丟失的數據自適應調整權重來處理延時問題，但該方法只針對單采樣頻率的延時，對于多采樣頻率的延時尚有待驗證。文獻[9]提出了一種用于處理時滯測量的MHE，但是在快速實時在線處理和傳感器多采樣頻率方面需要進一步研究。

針對以上問題，本文利用變結構線性滾動時域估計器(VSL-MHE)來解決因多傳感器采樣頻率不同而造成的延時問題，以提高植保無人機位置估計器的準確性和魯棒性。

1 植保無人機動力學模型

1.1 坐標系定義

圖1 植保無人機坐標系系統Fig.1 Coordinate system of plant protection UAV

1.2 動力學模型

使用牛頓-歐拉法對植保無人機進行動力學建模，動力學模型表征了飛行器受力和運動之間的關系。完整的動力學模型形式過于復雜，文獻[10]中對四旋翼無人機動力學模型進行了簡化，具體形式為

(1)

式中sW——植保無人機的位置

vW——植保無人機的速度

aW——導航坐標系下的實際加速度

式(1)中，加速度和位置的測量模型為

(2)

式中aB——機體坐標系下的加速度測量值

abias——加速度偏移值

aτ——加速度計測量值中的高斯白噪聲

sτ——GPS和氣壓計測量值中包含的高斯白噪聲

(3)

xk=Axk-1+Buk-1+Nwk-1

(4)

式中A——狀態轉移矩陣

B——輸入矩陣

N——過程噪聲矩陣

(5)

測量方程可表示為

zk=Cxk+νk

(6)

式中νk——測量噪聲，νk∈R3×1

2 位置估計器設計

2.1 滾動時域估計器

滾動時域估計器(MHE)是一種基于時域的優化方法，利用過去有限的測量值進行估計，其窗口的最后一個輸出作為當前狀態估計[11]。通過將估計問題轉化成優化問題進行求解，可以顯式地表示約束，并求解狀態估計量。通常這些約束也更能反映實際情況，因此可以提供更多信息，縮小估計器的搜索空間，提高搜索效率，使其能夠在嵌入式處理器上高效可靠地運行，估計植保無人機的實時位置。

將式(4)、(6)離散化，可表示為

(7)

式中uk——k時刻的輸入矢量，uk∈R3×1

zk——k時刻的測量矢量，zk∈R3×1

對狀態估計量施加約束

(8)

式中X——狀態可行集

sv,i——植保無人機的位置約束值

vw,i——植保無人機的速度約束值

at,i——加速度偏移的約束值

式(7)中的xk為9維狀態，包含3維位置、3維速度和3維加速度偏移。假設過程噪聲wk和測量噪聲νk為零均值高斯白噪聲，其協方差矩陣分別為Qk和Rk。

對式(7)中的每個工作點線性化，可導出測量序列{zk}的線性描述為

(9)

式(9)中狀態估計可以表述為MHE問題，即

(10)

式中，x0是初始狀態，該問題將受到約束限制，其狀態變量xj在狀態可行集X中的取值為xj∈X(j=0,1,…,k-1)。

式(10)中的代價函數表示為

Γk(x0,{wj}(j=0,1,…,k-1))

(11)

式中S——測量部分范數權重矩陣

L——噪聲部分范數權重矩陣

隨著時間的推移，式(10)中將獲得傳感器更多的信息，計算的復雜度隨著時間的增加而增加，這將使得有限資源下在線計算的實時性無法保證。為了避免這一問題，文獻[12]提出了一種固定維數的滾動時域估計方法。通過該方法，代價函數(11)可以改寫為

(12)

將式(12)作為MHE中的代價函數，MHE的計算量隨著采樣次數線性增加，當到達窗口NP的邊界時，MHE的計算量基本保持不變。

上述算法在實際應用過程中，由于多傳感器數據的采樣之間存在延時，會造成位置估計信息不準確，使得植保無人機定位精度變差，導致作業過程中出現噴灑面不均勻的情況。

2.2 變結構滾動時域估計器

測量數據存在延時，會影響到控制器的性能。如果MHE具有可變結構，便可以處理由于多傳感器采樣頻率不同而造成的數據之間延時問題。如圖2所示，通過零階保持塊(ZOH)對IMU、氣壓計和GPS進行采樣，在位置融合周期內，IMU采樣頻率大于氣壓計和GPS的采樣頻率。位置融合更新的時間點上GPS相對于IMU的延遲為d1,d2，…,dk。

圖2 傳感器數據采樣延遲示意圖Fig.2 Delay of sensors data acquisition

圖3 考慮數據延時的系統結構示意圖Fig.3 System block diagram with data delay

(13)

式中H——狀態轉移矩陣，NP-dn為與GPS輸出之后最接近的慣導輸出時刻

Θ——與H相對應的控制矩陣

按序展開后的H與Θ矩陣序列分別為

(14)

(15)

dj為k時刻窗口NP內的延遲位置，當在j序列上dj為0時，則表示需要將對應的Sk進行更新。

綜上所述，實際應用中，VSL-MHE的運行過程如圖4所示。

圖4 VSL-MHE算法運行過程Fig.4 Iterative process of VSL-MHE algorithm

值得注意的是，矩陣S接收到的新測量矢量值是變化的，但其維數保持不變。如果一個測量值在k時刻到達，并且該值在窗口NP內，則通過將矩陣S的塊移動到窗口NP中相應的位置，完成矩陣S的更新。

3 算法驗證與飛行實驗

當前循環迭代卡爾曼濾波器(CIEKF)是多旋翼無人機常用的位置估計方法[13]，為了評估位置估計器的性能，將CIEKF、MHE與VSL-MHE進行比較，使用均方根誤差(RMSE)來度量估計器的精度，定義為

(16)

式中δ∈{CIEKF,MHE,VSL-MHE}，NR為采樣次數，在采樣范圍[0,NR-1]內的實際值和估計值分別表示為xi和i(i=0,1,…,NR-1)，為了比較上述兩種位置估計器在相同傳感器延時條件下的性能，定義ηδ為

(17)

如果ηδ為負數，則表示VSL-MHE估計的位置精度高于MHE和CIEKF，值越小表征位置估計器的性能越好。

3.1 實驗平臺

為了驗證VSL-MHE的精度，在室內使用OptiTrack運動捕捉系統，為植保無人機提供可調輸出頻率的模擬GPS信息，同時也可獲得植保無人機的精確位置。在該系統下對基于VSL-MHE、MHE和CIEKF的位置估計算法進行定量評估。運動捕捉系統的主要組成部分包括：12個Flex3紅外攝像頭、無線路由器、標記點、工作站和光學運動捕捉軟件[14]。通過OptiTrack可實時獲取植保無人機的位置信息，精度可達0.1 mm。利用無線路由器將位置數據傳輸到樹莓派3B+，進行坐標轉換后發送到飛控系統。圖5為室內模擬系統結構圖。實驗場地長8 m、寬8 m、高3 m，為了模擬GPS的發送頻率，設置運動捕捉系統的位置發送頻率為2～20 Hz。

植保無人機實驗平臺如圖6所示，采用四旋翼結構，每個旋翼可提供9.8 N的升力，總共可以提供最大39.2 N升力，能持續飛行15 min。植保無人機的結構參數如表1所示，位置輸入單元使用Neo-

圖5 室內模擬系統結構Fig.5 Structural diagram of indoor simulation system

圖6 植保無人機實驗平臺Fig.6 Experiment platform of plant protection UAV1.噴頭1 2.水泵 3.飛控系統 4.樹莓派3B+ 5.GPS模塊 6.噴頭2 7.水箱

參數數值空載質量/kg2.0滿載質量/kg3.5飛行器軸距/cm50螺旋槳槳徑/cm11電池容量/(A·h)6飛行速度(室內/室外)/(m·s-1)0.2/2.0

M8N GPS模塊，機載慣導單元為MTI-2模塊，該模塊集成了三軸加速度計、三軸陀螺儀和三軸磁力計[15]。高度測量模塊為MS5611，垂直精度可達10 cm。

利用GPS和慣導數據，在飛控系統上運行VSL-MHE、MHE和CIEKF算法實現位置估計解算，引導植保無人機完成作業任務，分別在室內和室外兩種環境下進行驗證。

3.2 室內仿真飛行測試

植保無人機在OptiTrack運動捕捉系統下測試位置估計算法，圖7為植保無人機室內飛行測試場景。

圖7 運動捕捉系統輔助下的室內飛行測試場景Fig.7 Indoor flight under motion capture system

為了模擬GPS數據的延時，OptiTrack運動捕捉系統的位置輸出頻率分別為2、5、10、20 Hz，并加入零均值高斯白噪聲干擾。其中2 Hz的模擬GPS輸出頻率相對于機載慣導的延時時間差最高約為500 ms。

植保無人機分別在VSL-MHE、MHE和CIEKF位置估計器的輔助下完成定點懸停[16]，每次懸停時間5 min，懸停高度為1.5 m，分別在發送頻率為2、5、10、20 Hz下測試懸停精度。

(18)

評估兩組位置估計器的性能，圖8展示了植保無人機懸停于設定位置時GPS不同發送頻率導致的最大位置偏差。

圖8 不同位置發送頻率下的最大偏差Fig.8 Maximum deviation under different frequencies

從圖8分析可知，位置發送頻率從20 Hz到2 Hz逐漸降低時，3種位置估計器估計的位置最大偏差也同時增加，當頻率為2 Hz時，CIEKF、MHE和VSL-MHE位置估計器在x軸和y軸方向估計的位置最大偏差都達到最大值，分別為102、124 mm，94、103 mm，65、71 mm，VSL-MHE位置估計值相比于MHE和CIEKF的位置估計值，在x軸方向和y軸方向的偏差分別降低29、32 mm和37、53 mm。在圖8中x和y軸的位置最大偏差中，2 Hz的模擬GPS輸出頻率造成的位置偏差明顯大于其他頻率，說明位置估計中慣導測量值的融合計算需要使用GPS信息進行修正，且在一定范圍內GPS的輸出頻率越高修正速度越快，引起的位置偏差也越小。20 Hz頻率下，CIEKF、MHE和VSL-MHE估計的位置最大偏差在x軸方向和y軸方向分別為11、24 mm，9、21 mm，8、15 mm，遠小于2 Hz頻率下的位置最大偏差。上述分析表明，當GPS的信息輸出頻率因受到干擾而降低時，相對于CIEKF和MHE，使用本文中設計的VSL-MHE位置估計算法能夠減小位置偏差，提高定位精度。

3.3 室外飛行測試

上述通過位置最大偏差實驗證明了VSL-MHE算法的有效性，為了進一步在真實環境下驗證算法的魯棒性，在室外進行了MHE、VSL-MHE和CIEKF位置估計算法的對比測試。圖9為植保無人機室外飛行實驗環境。

圖9 室外飛行實驗環境Fig.9 Environment of outdoor flight test

在室外已知環境下進行植保無人機的位置估計器性能測試，確定衡量飛行方式優劣的標準有時間、能耗和路程[17]。考慮本實驗中植保無人機的系統參數和測試對比實驗的一致性，采用圖10所示的40 m×30 m范圍的往復式航線[18-19]。在地面站中設置往復式航線，將其同步到飛行控制系統。該航線單節長30 m，相鄰航線間距8 m，飛行高度為3 m。在相同的條件下對MHE、VSL-MHE和CIEKF位置估計算法分別進行8次實驗，實驗數據以50 Hz的頻率存儲在機載SD卡用于后續的數據分析。

圖10 室外飛行軌跡示意圖Fig.10 Outdoor flight path diagram

實驗中以時間間隔tf位置估計器的輸出值與位置設定值的均方根誤差RMSE來定量衡量位置偏離程度，RMSE值越大代表位置估計器性能越差[20]。

圖11中展示了8次實驗數據的誤差比較。從圖11中分析可知，植保無人機x、y軸方向位置RMSE由大到小為：CIEKF、MHE、VSL-MHE，表明VSL-MHE位置估計算法相對于CIEKF能夠提高位置估計精度，并且對比MHE，由于使用了變結構的方法，VSL-MHE的位置估計精度比MHE有所提高。x軸方向位置RMSE值小于y軸方向位置RMSE值，是由于x軸方向的航線遠小于y軸方向的航線。圖12為兩組對比實驗，分別為VSL-MHE與MHE之間x軸方向與y軸方向的ηδ值，VSL-MHE與CIEKF之間x軸方向與y軸方向的ηδ值。圖12的實驗結果同樣表明了VSL-MHE的位置估計精度要高于MHE和CIEKF的位置估計精度。

圖11 植保無人機位置的RMSEFig.11 RMSE of UAV position

圖12 3種位置估計器之間的ηδFig.12 ηδ between three position estimators

分別將8次VSL-MHE、MHE和CIEKF位置估計數據經過均值處理，處理過程定義為

(19)

式中，P∈{CIEKF,VSL-MHE,MHE}，在T次實驗內，x軸和y軸方向的位置均值表示為(Px,Py)。圖13為3組實驗的位置估計精度對比圖。

圖13 位置估計器精度對比Fig.13 Accuracy comparison of position estimator

從圖13中可知，植保無人機在拐點處，偏航需要旋轉90°，此時造成的位置偏差最大。因為在植保無人機的姿態控制中，偏航角響應時間大于橫滾角和俯仰角的響應時間，一般情況下控制植保無人機偏航旋轉10°需要的時間是控制橫滾角和俯仰角的2倍[21]。CIEKF估計出來的位置信息，在實際飛行中與實際位置在x軸方向和y軸方向的最大偏差分別為1.1 m和0.95 m，而MHE和VSL-MHE估計的位置信息與實際位置在x軸方向和y軸方向的最大偏差分別為0.87、0.62 m和0.48、0.59 m。所以在整個室外飛行過程中，VSL-MHE為植保無人機提供的位置精度高于CIEKF和MHE。即使GPS模塊的數據輸出頻率只有10 Hz，通過VSL-MHE仍能進行較為準確的狀態估計，體現了室外真實作業環境中該算法的有效性。

4 結束語

為了解決多傳感器采樣頻率不同造成的延時影響，設計了一種變結構線性滾動時域估計器，用于提高植保無人機位置估計的準確性。通過將VSL-MHE位置估計器與CIEKF和MHE位置估計器進行比較，對估計精度進行分析。室內場景下飛行測試表明，隨著位置輸出頻率由20 Hz降低到2 Hz，位置估計器的精度也隨之下降，與CIEKF、MHE位置估計器對比，VSL-MHE能很好地抑制延時的影響，驗證了VSL-MHE位置估計器的位置估計精度。在室外真實作業場景下，將3組位置估計器用于植保無人機定位，分別進行了8次實驗，并計算ηδ值，結果表明ηδ均為負值，表明VSL-MHE位置估計器的精度優于CIEKF和MHE。利用VSL-MHE設計的位置估計器具有精度高、魯棒性強的特點，適用于植保無人機對室外動態性能要求較高的位置信息的解算。