基于混合濾波與多環控制的無人機跟降方法

摘" 要： 針對無人機在復雜環境下，利用傳統的單環控制與卡爾曼濾波進行目標跟蹤與降落任務時，出現跟蹤目標丟失、降落精確性不足的問題，提出一種基于混合濾波和多環控制的狀態估計與控制算法。利用卡爾曼濾波和擴展卡爾曼濾波的混合濾波方法對移動降落平臺進行狀態估計，同時通過速度PI控制環、姿態PID控制環、位置PID控制環及加速度PID控制環來完成無人機的跟蹤與降落任務。仿真試驗結果表明，在面對簡易或復雜的環境時，該方法都具有較好的跟蹤性能和更高的降落精度，并能夠應用在多種環境下的無人機自主追蹤和降落作業中。

關鍵詞： 混合濾波； 多環控制； 無人機； 擴展卡爾曼濾波； PID控制； 位姿估計； 移動目標跟蹤

中圖分類號： TN713?34； V249.122+.5" " " " " " " " " "文獻標識碼： A" " " " " " " "文章編號： 1004?373X（2024）14?0108?07

Method for unmanned aerial vehicle tracking and landing based on hybrid

filtering and multi?ring control

REN Qianqian， WANG Xiaosong， ZHENG Enhui

（School of Mechanical and Electrical Engineering， China Jiliang University， Hangzhou 310018， China）

Abstract： A state estimation and control algorithm based on hybrid filtering and multi loop control is proposed to address the problems of target loss and insufficient landing accuracy when unmanned aerial vehicles uses traditional single loop control and Kalman filtering for target tracking and landing tasks in complex environments. The hybrid filtering method of Kalman filter and extended Kalman filter is used to estimate the state of the mobile landing platform， and the tracking and landing tasks of the unmanned aerial vehicles are completed by means of the speed PI control loop， attitude PID control loop， position PID control loop， and acceleration PID control loop. The simulation experimental results show that when facing both simple and complex environments， this method has good tracking performance and higher landing accuracy， and can be applied to autonomous tracking and landing operations of unmanned aerial vehicles in various environments.

Keywords： hybrid filtering; multi?loop control; unmanned aerial vehicle; extended Kalman filtering; PID control; pose estimation; moving target tracking

0" 引" 言

無人機自主追蹤和降落技術[1?3]是無人機領域的關鍵技術之一。該技術使無人機能夠在沒有人為干預的情況下追蹤特定目標，并能夠在指定的位置安全降落。然而在高度無人化的無人機作業過程中，因跟丟追蹤目標、降落誤差過大等因素導致的無人機炸機事故屢見不鮮。因此，研究如何提升無人機自主追蹤和降落技術的穩定性與精確性是必要的。

無人機追蹤與降落的關鍵之一在于獲取準確的位姿信息。I. U. Jan等人設計了一個無人機自主著陸控制系統。該系統利用卡爾曼濾波器處理視覺信息來控制直升機相對于目標的位置、高度和方向，以實現安全準確的著陸[4]。He Y等人創新性地將特征點坐標求解應用于姿態估計，提高了無人機的實時性能，以及準確、快速識別著陸標志的能力[5]。P. R. Grobler等人利用擴展卡爾曼濾波器濾波GPS數據來估計無人機的位置[6]。Feng B等人用紅外熱成像視覺代替可見光視覺，提升目標識別率，并通過圖像處理獲得無人艇與無人機的相對距離信息[7]。

上述提到的方法在位置空間上都存在一定的局限性，它們只能應用于目標位置絕對靜止的情況。C. Y." Kim等人提出了一種新型的無人機穩健著陸系統，采用超寬帶傳感器來補償攝像機有限的視場，還開發了擴展卡爾曼濾波器，融合從各種傳感器獲得的信息來估計移動平臺的相對位置[8]。該系統只在簡易的環境下做了相關的仿真實驗，并沒有探討復雜情況下該系統的性能。文獻[9]中將全向相機的測量數據與適當的動態模型相結合，以估算移動降落平臺的位置和速度。文獻[10]中使用安裝在地面車輛上的激光雷達進行激光雷達引導著陸，并利用聚類算法估計無人機相對于著陸平臺的位置。

一個魯棒的自主追蹤降落系統只有良好的位姿估計精確度而沒有穩定的控制方案是不行的。孫凱等人提出了一個分階段降落策略來實現無人機的自主跟蹤著陸任務[11]。夏知勝等人利用遺傳算法優化后的單級PID控制器來實現對無人機的姿態控制[12]。蘇贇等人提出一種自適應串級PID算法控制無人機的姿態，從而實現對移動降落平臺的跟蹤[13]。

針對上述方案各自存在的局限性，本文綜合提出了一個基于混合濾波與多環控制的無人機跟蹤和降落的方法。首先利用卡爾曼濾波和擴展卡爾曼濾波的混合濾波模式來處理雙目攝像頭采集到的Apriltag碼信息，從而估計無人機的相對姿態；再利用串并級的多環控制器來調整無人機的飛行姿態，最終實現自主追蹤著陸的任務。試驗結果表明，該方法具有良好的穩定性和降落精確度。

本文的主要貢獻有如下三點。

1） 針對無人機追蹤與降落任務設計了非線性與線性狀態融合的無人機動態模型，該動態模型充分地描述了無人機在執行飛行任務時的姿態信息。

2） 新穎地提出了一個混合濾波框架，該框架可以用于無人機的實時姿態評估。

3） 設計了一個串并級共存的多環控制器，用于精準地控制無人機的飛行狀態。

1" 問題描述與建模

1.1" 問題描述

狀態估計問題和位姿控制問題是無人機跟蹤與著陸方案設計的兩大要點。在無人機的飛行過程中，精確的位姿估計和穩定的飛行控制都是必不可少的。

無人機的狀態估計是利用當前時刻t之前的所有無人機控制信息[μ1：t]和傳感器觀測信息[z1：t]來估計無人機當前時刻的狀態[dt]，該狀態包括無人機的位置、姿態和速度等信息。根據貝葉斯濾波的原理[14]，將狀態估計問題分為兩個步驟進行。

1） 預測。預測即在獲得當前時刻t的無人機搭載的外部傳感器觀測信息[zt]之前，對無人機的狀態[dt]進行估計。在已知[z1：t-1]和[μ1：t]的情況下，通過計算[dt]的置信度[bel（dt）]來量化預測結果。該置信度也被稱為先驗概率。

[beldt=pdtdt-1，z1：t-1，μ1：tp（dt-1z1：t-1，μ1：t）ddt-1]

（1）

為了方便求解，做了馬爾可夫假設，即在狀態量具備完整性的特征時，系統當前時刻的狀態只與上一時刻有關。[bel（dt）]進一步簡化為：

[beldt=pdtxt-1，μtbel（dt-1）ddt-1] （2）

式中：[bel（dt-1）]代表已知[z1：t-1]和[μ1：t-1]的情況下[xt-1]的置信度，其在測量更新步驟中有詳細的解釋。

2） 測量更新。更新即在獲得當前時刻之前所有的無人機控制信息[μ1：t]和傳感器觀測信息[z1：t]后（包括當前時刻t的無人機搭載的外部傳感器觀測信息[zt]），通過計算條件概率[pdtz1：t，μ1：t]來得到置信度[bel（dt）]，即后驗概率。

[bel（dt）=λp（ztdt，z1：t-1，μ1：t）pdtz1：t-1，μ1：t] （3）

式中[λ]是與[dt]無關的常量。

同樣地，利用馬爾可夫假設，[beldt]被進一步簡化為：

[beldt=λp（ztdt） beldt] （4）

將無人機控制信息和觀測信息作為輸入，進行不斷預測和測量更新的迭代，就能得到每個時刻無人機的狀態信息。在這個過程中，如何選擇合適的狀態轉移概率[pdtdt-1，μt]和測量概率[p（ztdt）]是問題的關鍵，然而當前主流的濾波方法都需要在高斯空間下進行。為了保證后驗概率[beldt]在任何時刻t內都滿足高斯分布，狀態轉移概率和測量概率都必須是帶有隨機高斯噪聲的線性或非線性函數，且初始置信度[beld0]也要滿足正態分布。飛行器的姿態控制的目標是通過調整每個電機的轉速，使得飛行器保持期望的姿態。在本文中將t時刻無人機的狀態向量[dt]定義為：

[dt=xt，yt，zt，?t，θt，ψtT] （5）

式中：[（xt，yt，zt）]代表t時刻無人機在笛卡爾坐標系下的位置信息；[（?t，θt，ψt）]代表t時刻無人機的橫滾、俯仰和偏航角。

1.2" 四旋翼無人機的動態模型

無人機的狀態估計和位姿控制的前提是建立一個準確的動態模型，即狀態轉換方程。對于一個連續的狀態空間，系統的狀態轉移方程表達為：

[dt+1=gdt，μt+1+ξt=12fdt，μt+1Δt2+d'tΔt+ξt] （6）

式中：[Δt]為兩個相鄰無人機狀態[dt]和[dt-1]的過渡時間；[ξt]是均值為0、方差為[Rt]的高斯隨機變量，其含義為由狀態轉移引發的不確定性。引入該高斯隨機變量的意義在于為后續的混合濾波估計位姿做技術性鋪墊。在無人機的運動過程中，[fdt，μt+1=xt，yt，zt，?t，θt，ψtT]表示為：

[-fm（cosψtsinθtcos?t+sinψtsin?t）-fm（sinψtsinθtcos?t-cosψtsin?t）g-fmcos?tcosθt1Ixx[rx+θtψtIyy-Izz-JRPθtΩ]1Iyy[ry+?tψtIzz-Ixx+JRP?tΩ]1Izz[rz+?tθtIxx-Iyy]] （7）

式中：[f]為作用在機體的總拉力；[（rx，ry，rz）]為螺旋槳在笛卡爾坐標系下產生的力矩；m為機體的總質量；g為重力加速度；[JRP]是電機轉子和螺旋槳繞軸的總轉動慣量；[（Ixx，Iyy，Izz）]是慣性矩陣[J]的對角值。[Ω]的表達式為：

[Ω=Ω1-Ω2+Ω3-Ω4] （8）

式中：[Ω1]～[Ω4]分別為4個螺旋槳的轉速。

1.3" 無人機的觀測模型

本文探討的觀測方程是根據如下背景展開的。

無人機為搭載著單目相機的四旋翼飛行器，地面移動降落平臺上貼有Apriltag標簽[15]，無人機利用相機識別標簽來進行高精度的定位。

考慮到標簽上所有的點都與z軸共面，不妨設被相機觀測到的標簽上的任意一點[Q=（Xw，Yw，1）T]，[Q]點對應的像素坐標[q=（xv，yv）T]。則根據空間三維點與圖像點的對應關系，有如下等式：

[sxvyv1=KR'tXwYw1=HXwYw1] （9）

式中：s是比例縮放過程中的尺度因子；K是相機的內參矩陣；[R'=[r1，r2]]，是從世界坐標系到相機坐標系的旋轉矩陣R的前兩列所構成的矩陣。

通過識別標簽可以得到標簽角點的像素坐標，并利用標簽邊長和標簽中心的世界坐標計算出對應的世界坐標。將成對的像素坐標與世界坐標代入式（9），可以求得[R'：t]，特別地[r3]可以通過[r1]和[r2]的叉乘求解。由于攝像機固定安裝在無人機上，無人機體坐標系相對于相機坐標系的旋轉矩陣[Rcu]和平移量[tcu]可以通過實際的測量獲得。最終，無人機相對于世界坐標系下的位姿轉換公式為：

[Rwu=RTRcutwu=RT（tcu-t）] （10）

由此，可以得出t觀測量[Qt]與無人機狀態[dt]之間的觀測方程，如下：

[Qt=hdt+οt=Rwu-1dt[1：3]-twu+οt] （11）

式中：[οt]是均值為0、方差為[Ot]的高斯隨機變量；[dt[1：3]]是t時刻無人機狀態[dt]的位置向量。

至此，無人機的狀態轉移方程與觀測模型如式（6）和式（11）所示。

2" 基于KF和EKF的混合濾波方案

在討論混合濾波方案之前，要引入一個簡易的無人機的線性化動態模型。假設無人機總是在微小的時間段[Δtρ]內做勻速運動，則無人機的狀態轉移方程可以簡化為：

[dt+1=dt+AΔtρΔtρ+ξt] （12）

式中A為6×6的對角矩陣。

[AΔtρ=diag（vxt，vyt，vzt，pt，qt，rt）]" （13）

在本文中，無人機的狀態轉移模型有兩種：一種是非線性化的復雜模型，如式（6）所示；另一種是線性化的簡易模型，如式（12）所示。兩種模型在一定條件下可以相互轉換，這樣可以使估計過程既體現出穩定性也展現出快速性。混合濾波框架如圖1所示。在不斷地預測和測量更新的迭代過程中，基于兩種狀態方程來結合卡爾曼濾波[16?17]和擴展卡爾曼濾波[18?19]，針對每一時刻的無人機狀態進行預測。其預測結果作為更新過程的輸入。然而由于其觀測模型的線性結構，利用卡爾曼濾波來更新結果，更新量又作為下一時刻預測過程的輸入，如此循環往復，將準確地預估無人機的實時位姿。

假設無人機的狀態轉移方程為線性化的，應用卡爾曼濾波得到狀態轉移概率[pk]：

[pk=12πRtexp-12（dt+1-dt-AΔtρΔtρ）T·" " " " R-1t（dt+1-dt-AΔtρΔtρ）] （14）

假設無人機的狀態轉移方程為非線性化的，應用擴展卡爾曼濾波得到狀態轉移概率[pe]：

[pe=12πRtexp-12dt-gμt-1，μt-Gtxt-1-μt-1T·" " " R-1t（dt-gμt-1，μt-Gtxt-1-μt-1] （15）

式中[Gt]為[gd，μ]在[μt-1，μt]的一階泰勒展開系數所構成的雅可比矩陣。

[Gt=?gd，μ?dμt-1，μt] （16）

將狀態模型的變化轉換為系統狀態轉移概率的變化。因此，得到基于混合濾波的先驗概率。

[beldt=max （pe，pk）bel（dt-1） ddt-1] （17）

[beldt]服從高斯分布，其均值[ζt]為：

[ζt=gζt-1，μt，" " " " pe≥pkζt-1+AΔtρΔtρ，" pelt;pk] （18）

其方差[Σt]為：

[Σt=GtΣt-1GTt+Rt，" pe≥pkΣt-1+Rt，" " " " " "pelt;pk] （19）

根據線性的無人機觀測模型，如式（11）所示，利用卡爾曼濾波得到系統的測量概率[pQtdt]：

[pQtdt=12πOtexp-12（Qt-Rwu-1（dt[1：3]-twu））T·" " " " " " " " " " "O-1t（Qt-Rwu-1（dt1：3-twu））] （20）

根據測量概率得到系統的后驗概率[beldt]：

[beldt=λp（Qtdt） beldt] （21）

式中[beldt]同樣服從高斯分布，其均值[ζt]為：

[ζt=ζt+KtQt-Rwu-1dt1：3-twu] （22）

其方差[Σt]為：

[Σt=（I-KtRwu-1）Σt] （23）

式（22）和式（23）中的卡爾曼系數[Kt]為：

[Kt=ΣtRwu（Rwu-1ΣtRwu+Ot）-1] （24）

3" 基于串并級的多環控制器設計

完成無人機的狀態估計工作后，針對無人機的飛行控制，采用四環的串級和并級相融合的控制模式，其控制原理圖如圖2所示。控制框架由速度環、姿態環、位置環和加速度環構成。其中，速度PI控制環和姿態PID控制環組成一個串級結構，位置環和加速度環組成一個串級結構。兩個串級結構并行組成一個并級結構。

通過處理無人機上搭載的單目相機拍攝的實時貼有Apriltag標簽的移動平臺照片，可以得到無人機和移動降落平臺之間的相對位置[Δx]。位置指令是參考混合濾波估計無人機在[t]時刻的位置[dt1：3=[xt，yt，zt]T]后，向無人機下達的跟隨或者降落情況下的期望位置[pt]，位置PID環以位置指令作為輸入，[Δx]作為反饋量對無人機的實際位置進行PID控制。位置PID輸出期望位置加速度值作為位置加速度PID環的輸入。速度指令是參考混合濾波估計無人機在t時刻的姿態[dt4：6=[?t，θt，ψt]T]后，向無人機下達的跟隨或者降落情況下的期望位置[st]，速度PI環以速度指令作為輸入，位置加速度PID環的輸出和慣性導航測量無人機的線速度同時作為反饋量，對無人機的實時位姿進行PI調節。在這個過程中，引入一個姿態PID環的串級結構，聯合該結構對無人機的電機進行轉速調節。其中姿態PID環的輸入為速度PI環的輸出，其反饋量為慣性導航測量無人機的角速度和角度。

在無人機對于移動目標的跟蹤過程中，四環控制全部處于收斂狀態。特別地，在降落過程中，位置PID環對于z軸上分量的調節處于線性發散的狀態，當無人機的位置下降到一定高度時再回歸到收斂狀態，當無人機與目標達到相對靜止的狀態時關閉發動機。至此，無人機的跟蹤降落任務已完成。

4" 仿真實驗

本文的仿真平臺采用Gazebo和Rviz相結合的模式。首先，利用Gazebo監測無人機和移動平臺的相對位置關系，模擬無人機實時跟蹤移動目標并自主降落的全過程。其次，利用Rviz體現出各個坐標系直接的位置關系，并在image窗口體現相機跟蹤Apriltag碼的過程。無人機和小車的模型利用Urdf模型制作方法制作。仿真平臺如圖3所示。

利用上述搭建的仿真平臺進行了一系列實驗，模擬了無人機追蹤移動平臺并能夠在其上安全降落的場景。在這些實驗中，本文方法將與常規方法，即擴展卡爾曼濾波和單環PID進行比較，并且移動平臺的運動路徑分別設置為直線和圓形。目標運動軌跡為圓形時，兩種方法的無人機的跟蹤降落狀態如圖4所示。

針對圓形跟蹤的效果，如圖4a）和圖4b）所示，本文方法與傳統方法的優劣顯而易見，應用本文方法可以在跟蹤過程中讓無人機與移動目標貼合緊密；然而傳統方法使得無人機的位置比較松散，無法與移動目標貼合。針對圓形降落的效果，如圖4c）和圖4d）所示，本文方法可以實現精準降落，而傳統方法的降落誤差較大。

當目標運動軌跡為直線時，假設移動目標沿y軸以0.5 m/s的速度進行勻速運動。兩種方法的無人機的跟蹤降落狀態如圖5所示。

針對直線跟蹤的效果，如圖5a）和圖5b）所示，本文方法在跟蹤效果上要優于傳統方法。傳統方法在x軸上與追蹤目標有著明顯的偏移量。針對直線降落的效果，如圖5c）和圖5d）所示，本文方法降落精度明顯較高。為了進一步定量地分析兩種方法的優劣，做了10組實驗，其反饋的追蹤、降落時的平均誤差和降落平均時間分別如表1、表2所示。

根據表1的無人機跟蹤誤差數據，可以看出本文方法在面對目標軌跡為圓形和直線時都有著更小的跟蹤誤差，無人機在跟蹤時能更加貼合目標平臺。通過分析表2的無人機降落誤差和時間的數據，本文方法無論是在降落時間還是降落精準度上都有著明顯的優勢。

5" 結" 語

本文針對如何提升無人機自主追蹤著陸時的精度和速度的問題，提出一種基于混合濾波與多環控制的方案，并通過仿真實驗驗證該方法的有效性和魯棒性。結果表明本文方法在跟蹤和降落方面都較傳統的方法更有優勢。關于后續的工作，考慮引入更復雜的目標平臺的移動軌跡或感興趣區域來進一步提升跟蹤速度，并將該方案運用到真實的實驗環境中去。

參考文獻

[1] 鐘春來，楊洋，曹立佳，等.基于視覺的無人機自主著陸研究綜述[J].航空兵器，2023，30（5）：104?114.

[2] 韓鵬，耿增顯，劉宏，等.無人機視覺識別與移動追蹤技術綜述[J].飛航導彈，2018（10）：24?28.

[3] 趙良玉，李丹，趙辰悅，等.無人機自主降落標識檢測方法若干研究進展[J].航空學報，2022，43（9）：274?289.

[4] JAN I U， KHAN M U， IQBAL N. Combined application of kalman filtering and correlation towards autonomous helicopter landing [C]// 2009 2nd International Conference on Computer， Control and Communication. [S.l.]： IEEE， 2009： 1?6.

[5] HE Y， ZENG Z， LI Z， et al. A new vision?based method of autonomous landing for UAVs [C]// 2023 9th International Conference on Electrical Engineering， Control and Robotics. Wuhan： IEEE， 2023： 1?6.

[6] GROBLER P R， JORDAAN H W. Autonomous vision based landing strategy for a rotary wing UAV [C]// 2020 International SAUPEC/RobMech/PRASA Conference. Cape Town， South Africa： IEEE， 2020： 1?6.

[7] FENG B， YANG X， WANG R， et al. Design and implementation of autonomous takeoff and landing UAV system for USV platform [C]// 2022 International Conference on Cyber?Physical Social Intelligence （ICCSI）. [S.l.]： IEEE， 2022： 292?296.

[8] KIM C Y， LEE E C M， CHOI J H， et al. ROLAND： robust landing of UAV on moving platform using object detection and UWB based extended kalman filter [C]// 2021 21st International Conference on Control， Automation and Systems. [S.l.]： IEEE， 2021： 249?254.

[9] KIM J W， JUNG Y， LEE D. Outdoor autonomous landing on a moving platform for quadrotors using an omnidirectional camera [C]// 2014 International Conference on Unmanned Aircraft Systems （ICUAS）. Orlando， FL， USA： IEEE， 2014： 1243?1252.

[10] AOKI N， ISHIGAMI G. Autonomous tracking and landing of an unmanned aerial vehicle on a ground vehicle in rough terrain [J]. Advanced robotics， 2023， 37（5）： 344?355.

[11] 孫凱，于洋，馮迎賓.基于視覺的自主無人機目標跟蹤著陸方法研究[J].沈陽理工大學學報，2022，41（5）：21?28.

[12] 夏知勝，陳誠，楊愛斌.基于遺傳算法優化PID的固定翼無人機俯仰控制設計[J].科技創新與應用，2023，13（34）：41?44.

[13] 蘇贇，王挺，姚辰，等.基于合作目標的無人機目標跟蹤方法[J].機器人，2019，41（4）：425?432.

[14] QU Congshan， XU Hualong， TANG Ying. A survey of nonlinear Bayesian filtering algorithms [J]. Electronics optics amp; control， 2008， 15（8）： 64?69.

[15] WANG J， OLSON E. AprilTag 2： efficient and robust fiducial detection [C]// 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems. Daejeon， South Korea： IEEE， 2016： 4193?4198.

[16] CAO Y， YAN P， LIN S， et al. KF?based MPC for the cascaded headbox system in papermaking process [C]// 2017 36th Chinese Control Conference. [S.l.]： IEEE， 2017： 4369?4374.

[17] DUTTA A， DAS M. ECG signal denoising using adaptive unscented kalman filter [C]// 2022 IEEE Conference on Interdisciplinary Approaches in Technology and Management for Social Innovation （IATMSI）. Gwalior， India： IEEE， 2022： 1?6.

[18] WANG H， WANG J， QU L， et al. Simultaneous localization and mapping based on multilevel?EKF [C]// 2011 IEEE International Conference on Mechatronics and Automation. Beijing： IEEE， 2011： 2254?2258.

[19] LUO L， CHEN G. Research on SOC estimation of lithium battery for electric vehicle based on EKF algorithm [C]// 2021 International Conference on Power System Technology. Kunming： IEEE， 2021： 820?823.