基于混合濾波與多環(huán)控制的無(wú)人機(jī)跟降方法

摘" 要： 針對(duì)無(wú)人機(jī)在復(fù)雜環(huán)境下，利用傳統(tǒng)的單環(huán)控制與卡爾曼濾波進(jìn)行目標(biāo)跟蹤與降落任務(wù)時(shí)，出現(xiàn)跟蹤目標(biāo)丟失、降落精確性不足的問(wèn)題，提出一種基于混合濾波和多環(huán)控制的狀態(tài)估計(jì)與控制算法。利用卡爾曼濾波和擴(kuò)展卡爾曼濾波的混合濾波方法對(duì)移動(dòng)降落平臺(tái)進(jìn)行狀態(tài)估計(jì)，同時(shí)通過(guò)速度PI控制環(huán)、姿態(tài)PID控制環(huán)、位置PID控制環(huán)及加速度PID控制環(huán)來(lái)完成無(wú)人機(jī)的跟蹤與降落任務(wù)。仿真試驗(yàn)結(jié)果表明，在面對(duì)簡(jiǎn)易或復(fù)雜的環(huán)境時(shí)，該方法都具有較好的跟蹤性能和更高的降落精度，并能夠應(yīng)用在多種環(huán)境下的無(wú)人機(jī)自主追蹤和降落作業(yè)中。

關(guān)鍵詞： 混合濾波； 多環(huán)控制； 無(wú)人機(jī)； 擴(kuò)展卡爾曼濾波； PID控制； 位姿估計(jì)； 移動(dòng)目標(biāo)跟蹤

中圖分類號(hào)： TN713?34； V249.122+.5" " " " " " " " " "文獻(xiàn)標(biāo)識(shí)碼： A" " " " " " " "文章編號(hào)： 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" 引" 言

無(wú)人機(jī)自主追蹤和降落技術(shù)[1?3]是無(wú)人機(jī)領(lǐng)域的關(guān)鍵技術(shù)之一。該技術(shù)使無(wú)人機(jī)能夠在沒(méi)有人為干預(yù)的情況下追蹤特定目標(biāo)，并能夠在指定的位置安全降落。然而在高度無(wú)人化的無(wú)人機(jī)作業(yè)過(guò)程中，因跟丟追蹤目標(biāo)、降落誤差過(guò)大等因素導(dǎo)致的無(wú)人機(jī)炸機(jī)事故屢見(jiàn)不鮮。因此，研究如何提升無(wú)人機(jī)自主追蹤和降落技術(shù)的穩(wěn)定性與精確性是必要的。

無(wú)人機(jī)追蹤與降落的關(guān)鍵之一在于獲取準(zhǔn)確的位姿信息。I. U. Jan等人設(shè)計(jì)了一個(gè)無(wú)人機(jī)自主著陸控制系統(tǒng)。該系統(tǒng)利用卡爾曼濾波器處理視覺(jué)信息來(lái)控制直升機(jī)相對(duì)于目標(biāo)的位置、高度和方向，以實(shí)現(xiàn)安全準(zhǔn)確的著陸[4]。He Y等人創(chuàng)新性地將特征點(diǎn)坐標(biāo)求解應(yīng)用于姿態(tài)估計(jì)，提高了無(wú)人機(jī)的實(shí)時(shí)性能，以及準(zhǔn)確、快速識(shí)別著陸標(biāo)志的能力[5]。P. R. Grobler等人利用擴(kuò)展卡爾曼濾波器濾波GPS數(shù)據(jù)來(lái)估計(jì)無(wú)人機(jī)的位置[6]。Feng B等人用紅外熱成像視覺(jué)代替可見(jiàn)光視覺(jué)，提升目標(biāo)識(shí)別率，并通過(guò)圖像處理獲得無(wú)人艇與無(wú)人機(jī)的相對(duì)距離信息[7]。

上述提到的方法在位置空間上都存在一定的局限性，它們只能應(yīng)用于目標(biāo)位置絕對(duì)靜止的情況。C. Y." Kim等人提出了一種新型的無(wú)人機(jī)穩(wěn)健著陸系統(tǒng)，采用超寬帶傳感器來(lái)補(bǔ)償攝像機(jī)有限的視場(chǎng)，還開(kāi)發(fā)了擴(kuò)展卡爾曼濾波器，融合從各種傳感器獲得的信息來(lái)估計(jì)移動(dòng)平臺(tái)的相對(duì)位置[8]。該系統(tǒng)只在簡(jiǎn)易的環(huán)境下做了相關(guān)的仿真實(shí)驗(yàn)，并沒(méi)有探討復(fù)雜情況下該系統(tǒng)的性能。文獻(xiàn)[9]中將全向相機(jī)的測(cè)量數(shù)據(jù)與適當(dāng)?shù)膭?dòng)態(tài)模型相結(jié)合，以估算移動(dòng)降落平臺(tái)的位置和速度。文獻(xiàn)[10]中使用安裝在地面車輛上的激光雷達(dá)進(jìn)行激光雷達(dá)引導(dǎo)著陸，并利用聚類算法估計(jì)無(wú)人機(jī)相對(duì)于著陸平臺(tái)的位置。

一個(gè)魯棒的自主追蹤降落系統(tǒng)只有良好的位姿估計(jì)精確度而沒(méi)有穩(wěn)定的控制方案是不行的。孫凱等人提出了一個(gè)分階段降落策略來(lái)實(shí)現(xiàn)無(wú)人機(jī)的自主跟蹤著陸任務(wù)[11]。夏知?jiǎng)俚热死眠z傳算法優(yōu)化后的單級(jí)PID控制器來(lái)實(shí)現(xiàn)對(duì)無(wú)人機(jī)的姿態(tài)控制[12]。蘇贇等人提出一種自適應(yīng)串級(jí)PID算法控制無(wú)人機(jī)的姿態(tài)，從而實(shí)現(xiàn)對(duì)移動(dòng)降落平臺(tái)的跟蹤[13]。

針對(duì)上述方案各自存在的局限性，本文綜合提出了一個(gè)基于混合濾波與多環(huán)控制的無(wú)人機(jī)跟蹤和降落的方法。首先利用卡爾曼濾波和擴(kuò)展卡爾曼濾波的混合濾波模式來(lái)處理雙目攝像頭采集到的Apriltag碼信息，從而估計(jì)無(wú)人機(jī)的相對(duì)姿態(tài)；再利用串并級(jí)的多環(huán)控制器來(lái)調(diào)整無(wú)人機(jī)的飛行姿態(tài)，最終實(shí)現(xiàn)自主追蹤著陸的任務(wù)。試驗(yàn)結(jié)果表明，該方法具有良好的穩(wěn)定性和降落精確度。

本文的主要貢獻(xiàn)有如下三點(diǎn)。

1） 針對(duì)無(wú)人機(jī)追蹤與降落任務(wù)設(shè)計(jì)了非線性與線性狀態(tài)融合的無(wú)人機(jī)動(dòng)態(tài)模型，該動(dòng)態(tài)模型充分地描述了無(wú)人機(jī)在執(zhí)行飛行任務(wù)時(shí)的姿態(tài)信息。

2） 新穎地提出了一個(gè)混合濾波框架，該框架可以用于無(wú)人機(jī)的實(shí)時(shí)姿態(tài)評(píng)估。

3） 設(shè)計(jì)了一個(gè)串并級(jí)共存的多環(huán)控制器，用于精準(zhǔn)地控制無(wú)人機(jī)的飛行狀態(tài)。

1" 問(wèn)題描述與建模

1.1" 問(wèn)題描述

狀態(tài)估計(jì)問(wèn)題和位姿控制問(wèn)題是無(wú)人機(jī)跟蹤與著陸方案設(shè)計(jì)的兩大要點(diǎn)。在無(wú)人機(jī)的飛行過(guò)程中，精確的位姿估計(jì)和穩(wěn)定的飛行控制都是必不可少的。

無(wú)人機(jī)的狀態(tài)估計(jì)是利用當(dāng)前時(shí)刻t之前的所有無(wú)人機(jī)控制信息[μ1：t]和傳感器觀測(cè)信息[z1：t]來(lái)估計(jì)無(wú)人機(jī)當(dāng)前時(shí)刻的狀態(tài)[dt]，該狀態(tài)包括無(wú)人機(jī)的位置、姿態(tài)和速度等信息。根據(jù)貝葉斯濾波的原理[14]，將狀態(tài)估計(jì)問(wèn)題分為兩個(gè)步驟進(jìn)行。

1） 預(yù)測(cè)。預(yù)測(cè)即在獲得當(dāng)前時(shí)刻t的無(wú)人機(jī)搭載的外部傳感器觀測(cè)信息[zt]之前，對(duì)無(wú)人機(jī)的狀態(tài)[dt]進(jìn)行估計(jì)。在已知[z1：t-1]和[μ1：t]的情況下，通過(guò)計(jì)算[dt]的置信度[bel（dt）]來(lái)量化預(yù)測(cè)結(jié)果。該置信度也被稱為先驗(yàn)概率。

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

（1）

為了方便求解，做了馬爾可夫假設(shè)，即在狀態(tài)量具備完整性的特征時(shí)，系統(tǒng)當(dāng)前時(shí)刻的狀態(tài)只與上一時(shí)刻有關(guān)。[bel（dt）]進(jìn)一步簡(jiǎn)化為：

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

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

2） 測(cè)量更新。更新即在獲得當(dāng)前時(shí)刻之前所有的無(wú)人機(jī)控制信息[μ1：t]和傳感器觀測(cè)信息[z1：t]后（包括當(dāng)前時(shí)刻t的無(wú)人機(jī)搭載的外部傳感器觀測(cè)信息[zt]），通過(guò)計(jì)算條件概率[pdtz1：t，μ1：t]來(lái)得到置信度[bel（dt）]，即后驗(yàn)概率。

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

式中[λ]是與[dt]無(wú)關(guān)的常量。

同樣地，利用馬爾可夫假設(shè)，[beldt]被進(jìn)一步簡(jiǎn)化為：

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

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

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

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

1.2" 四旋翼無(wú)人機(jī)的動(dòng)態(tài)模型

無(wú)人機(jī)的狀態(tài)估計(jì)和位姿控制的前提是建立一個(gè)準(zhǔn)確的動(dòng)態(tài)模型，即狀態(tài)轉(zhuǎn)換方程。對(duì)于一個(gè)連續(xù)的狀態(tài)空間，系統(tǒng)的狀態(tài)轉(zhuǎn)移方程表達(dá)為：

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

式中：[Δt]為兩個(gè)相鄰無(wú)人機(jī)狀態(tài)[dt]和[dt-1]的過(guò)渡時(shí)間；[ξt]是均值為0、方差為[Rt]的高斯隨機(jī)變量，其含義為由狀態(tài)轉(zhuǎn)移引發(fā)的不確定性。引入該高斯隨機(jī)變量的意義在于為后續(xù)的混合濾波估計(jì)位姿做技術(shù)性鋪墊。在無(wú)人機(jī)的運(yùn)動(dòng)過(guò)程中，[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]為作用在機(jī)體的總拉力；[（rx，ry，rz）]為螺旋槳在笛卡爾坐標(biāo)系下產(chǎn)生的力矩；m為機(jī)體的總質(zhì)量；g為重力加速度；[JRP]是電機(jī)轉(zhuǎn)子和螺旋槳繞軸的總轉(zhuǎn)動(dòng)慣量；[（Ixx，Iyy，Izz）]是慣性矩陣[J]的對(duì)角值。[Ω]的表達(dá)式為：

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

式中：[Ω1]～[Ω4]分別為4個(gè)螺旋槳的轉(zhuǎn)速。

1.3" 無(wú)人機(jī)的觀測(cè)模型

本文探討的觀測(cè)方程是根據(jù)如下背景展開(kāi)的。

無(wú)人機(jī)為搭載著單目相機(jī)的四旋翼飛行器，地面移動(dòng)降落平臺(tái)上貼有Apriltag標(biāo)簽[15]，無(wú)人機(jī)利用相機(jī)識(shí)別標(biāo)簽來(lái)進(jìn)行高精度的定位。

考慮到標(biāo)簽上所有的點(diǎn)都與z軸共面，不妨設(shè)被相機(jī)觀測(cè)到的標(biāo)簽上的任意一點(diǎn)[Q=（Xw，Yw，1）T]，[Q]點(diǎn)對(duì)應(yīng)的像素坐標(biāo)[q=（xv，yv）T]。則根據(jù)空間三維點(diǎn)與圖像點(diǎn)的對(duì)應(yīng)關(guān)系，有如下等式：

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

式中：s是比例縮放過(guò)程中的尺度因子；K是相機(jī)的內(nèi)參矩陣；[R'=[r1，r2]]，是從世界坐標(biāo)系到相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣R的前兩列所構(gòu)成的矩陣。

通過(guò)識(shí)別標(biāo)簽可以得到標(biāo)簽角點(diǎn)的像素坐標(biāo)，并利用標(biāo)簽邊長(zhǎng)和標(biāo)簽中心的世界坐標(biāo)計(jì)算出對(duì)應(yīng)的世界坐標(biāo)。將成對(duì)的像素坐標(biāo)與世界坐標(biāo)代入式（9），可以求得[R'：t]，特別地[r3]可以通過(guò)[r1]和[r2]的叉乘求解。由于攝像機(jī)固定安裝在無(wú)人機(jī)上，無(wú)人機(jī)體坐標(biāo)系相對(duì)于相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣[Rcu]和平移量[tcu]可以通過(guò)實(shí)際的測(cè)量獲得。最終，無(wú)人機(jī)相對(duì)于世界坐標(biāo)系下的位姿轉(zhuǎn)換公式為：

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

由此，可以得出t觀測(cè)量[Qt]與無(wú)人機(jī)狀態(tài)[dt]之間的觀測(cè)方程，如下：

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

式中：[οt]是均值為0、方差為[Ot]的高斯隨機(jī)變量；[dt[1：3]]是t時(shí)刻無(wú)人機(jī)狀態(tài)[dt]的位置向量。

至此，無(wú)人機(jī)的狀態(tài)轉(zhuǎn)移方程與觀測(cè)模型如式（6）和式（11）所示。

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

在討論混合濾波方案之前，要引入一個(gè)簡(jiǎn)易的無(wú)人機(jī)的線性化動(dòng)態(tài)模型。假設(shè)無(wú)人機(jī)總是在微小的時(shí)間段[Δtρ]內(nèi)做勻速運(yùn)動(dòng)，則無(wú)人機(jī)的狀態(tài)轉(zhuǎn)移方程可以簡(jiǎn)化為：

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

式中A為6×6的對(duì)角矩陣。

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

在本文中，無(wú)人機(jī)的狀態(tài)轉(zhuǎn)移模型有兩種：一種是非線性化的復(fù)雜模型，如式（6）所示；另一種是線性化的簡(jiǎn)易模型，如式（12）所示。兩種模型在一定條件下可以相互轉(zhuǎn)換，這樣可以使估計(jì)過(guò)程既體現(xiàn)出穩(wěn)定性也展現(xiàn)出快速性。混合濾波框架如圖1所示。在不斷地預(yù)測(cè)和測(cè)量更新的迭代過(guò)程中，基于兩種狀態(tài)方程來(lái)結(jié)合卡爾曼濾波[16?17]和擴(kuò)展卡爾曼濾波[18?19]，針對(duì)每一時(shí)刻的無(wú)人機(jī)狀態(tài)進(jìn)行預(yù)測(cè)。其預(yù)測(cè)結(jié)果作為更新過(guò)程的輸入。然而由于其觀測(cè)模型的線性結(jié)構(gòu)，利用卡爾曼濾波來(lái)更新結(jié)果，更新量又作為下一時(shí)刻預(yù)測(cè)過(guò)程的輸入，如此循環(huán)往復(fù)，將準(zhǔn)確地預(yù)估無(wú)人機(jī)的實(shí)時(shí)位姿。

假設(shè)無(wú)人機(jī)的狀態(tài)轉(zhuǎn)移方程為線性化的，應(yīng)用卡爾曼濾波得到狀態(tài)轉(zhuǎn)移概率[pk]：

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

假設(shè)無(wú)人機(jī)的狀態(tài)轉(zhuǎn)移方程為非線性化的，應(yīng)用擴(kuò)展卡爾曼濾波得到狀態(tài)轉(zhuǎn)移概率[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]的一階泰勒展開(kāi)系數(shù)所構(gòu)成的雅可比矩陣。

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

將狀態(tài)模型的變化轉(zhuǎn)換為系統(tǒng)狀態(tài)轉(zhuǎn)移概率的變化。因此，得到基于混合濾波的先驗(yàn)概率。

[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）

根據(jù)線性的無(wú)人機(jī)觀測(cè)模型，如式（11）所示，利用卡爾曼濾波得到系統(tǒng)的測(cè)量概率[pQtdt]：

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

根據(jù)測(cè)量概率得到系統(tǒng)的后驗(yàn)概率[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）中的卡爾曼系數(shù)[Kt]為：

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

3" 基于串并級(jí)的多環(huán)控制器設(shè)計(jì)

完成無(wú)人機(jī)的狀態(tài)估計(jì)工作后，針對(duì)無(wú)人機(jī)的飛行控制，采用四環(huán)的串級(jí)和并級(jí)相融合的控制模式，其控制原理圖如圖2所示。控制框架由速度環(huán)、姿態(tài)環(huán)、位置環(huán)和加速度環(huán)構(gòu)成。其中，速度PI控制環(huán)和姿態(tài)PID控制環(huán)組成一個(gè)串級(jí)結(jié)構(gòu)，位置環(huán)和加速度環(huán)組成一個(gè)串級(jí)結(jié)構(gòu)。兩個(gè)串級(jí)結(jié)構(gòu)并行組成一個(gè)并級(jí)結(jié)構(gòu)。

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

在無(wú)人機(jī)對(duì)于移動(dòng)目標(biāo)的跟蹤過(guò)程中，四環(huán)控制全部處于收斂狀態(tài)。特別地，在降落過(guò)程中，位置PID環(huán)對(duì)于z軸上分量的調(diào)節(jié)處于線性發(fā)散的狀態(tài)，當(dāng)無(wú)人機(jī)的位置下降到一定高度時(shí)再回歸到收斂狀態(tài)，當(dāng)無(wú)人機(jī)與目標(biāo)達(dá)到相對(duì)靜止的狀態(tài)時(shí)關(guān)閉發(fā)動(dòng)機(jī)。至此，無(wú)人機(jī)的跟蹤降落任務(wù)已完成。

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

本文的仿真平臺(tái)采用Gazebo和Rviz相結(jié)合的模式。首先，利用Gazebo監(jiān)測(cè)無(wú)人機(jī)和移動(dòng)平臺(tái)的相對(duì)位置關(guān)系，模擬無(wú)人機(jī)實(shí)時(shí)跟蹤移動(dòng)目標(biāo)并自主降落的全過(guò)程。其次，利用Rviz體現(xiàn)出各個(gè)坐標(biāo)系直接的位置關(guān)系，并在image窗口體現(xiàn)相機(jī)跟蹤Apriltag碼的過(guò)程。無(wú)人機(jī)和小車的模型利用Urdf模型制作方法制作。仿真平臺(tái)如圖3所示。

利用上述搭建的仿真平臺(tái)進(jìn)行了一系列實(shí)驗(yàn)，模擬了無(wú)人機(jī)追蹤移動(dòng)平臺(tái)并能夠在其上安全降落的場(chǎng)景。在這些實(shí)驗(yàn)中，本文方法將與常規(guī)方法，即擴(kuò)展卡爾曼濾波和單環(huán)PID進(jìn)行比較，并且移動(dòng)平臺(tái)的運(yùn)動(dòng)路徑分別設(shè)置為直線和圓形。目標(biāo)運(yùn)動(dòng)軌跡為圓形時(shí)，兩種方法的無(wú)人機(jī)的跟蹤降落狀態(tài)如圖4所示。

針對(duì)圓形跟蹤的效果，如圖4a）和圖4b）所示，本文方法與傳統(tǒng)方法的優(yōu)劣顯而易見(jiàn)，應(yīng)用本文方法可以在跟蹤過(guò)程中讓無(wú)人機(jī)與移動(dòng)目標(biāo)貼合緊密；然而傳統(tǒng)方法使得無(wú)人機(jī)的位置比較松散，無(wú)法與移動(dòng)目標(biāo)貼合。針對(duì)圓形降落的效果，如圖4c）和圖4d）所示，本文方法可以實(shí)現(xiàn)精準(zhǔn)降落，而傳統(tǒng)方法的降落誤差較大。

當(dāng)目標(biāo)運(yùn)動(dòng)軌跡為直線時(shí)，假設(shè)移動(dòng)目標(biāo)沿y軸以0.5 m/s的速度進(jìn)行勻速運(yùn)動(dòng)。兩種方法的無(wú)人機(jī)的跟蹤降落狀態(tài)如圖5所示。

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

根據(jù)表1的無(wú)人機(jī)跟蹤誤差數(shù)據(jù)，可以看出本文方法在面對(duì)目標(biāo)軌跡為圓形和直線時(shí)都有著更小的跟蹤誤差，無(wú)人機(jī)在跟蹤時(shí)能更加貼合目標(biāo)平臺(tái)。通過(guò)分析表2的無(wú)人機(jī)降落誤差和時(shí)間的數(shù)據(jù)，本文方法無(wú)論是在降落時(shí)間還是降落精準(zhǔn)度上都有著明顯的優(yōu)勢(shì)。

5" 結(jié)" 語(yǔ)

本文針對(duì)如何提升無(wú)人機(jī)自主追蹤著陸時(shí)的精度和速度的問(wèn)題，提出一種基于混合濾波與多環(huán)控制的方案，并通過(guò)仿真實(shí)驗(yàn)驗(yàn)證該方法的有效性和魯棒性。結(jié)果表明本文方法在跟蹤和降落方面都較傳統(tǒng)的方法更有優(yōu)勢(shì)。關(guān)于后續(xù)的工作，考慮引入更復(fù)雜的目標(biāo)平臺(tái)的移動(dòng)軌跡或感興趣區(qū)域來(lái)進(jìn)一步提升跟蹤速度，并將該方案運(yùn)用到真實(shí)的實(shí)驗(yàn)環(huán)境中去。

參考文獻(xiàn)

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

[2] 韓鵬，耿增顯，劉宏，等.無(wú)人機(jī)視覺(jué)識(shí)別與移動(dòng)追蹤技術(shù)綜述[J].飛航導(dǎo)彈，2018（10）：24?28.

[3] 趙良玉，李丹，趙辰悅，等.無(wú)人機(jī)自主降落標(biāo)識(shí)檢測(cè)方法若干研究進(jìn)展[J].航空學(xué)報(bào)，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] 孫凱，于洋，馮迎賓.基于視覺(jué)的自主無(wú)人機(jī)目標(biāo)跟蹤著陸方法研究[J].沈陽(yáng)理工大學(xué)學(xué)報(bào)，2022，41（5）：21?28.

[12] 夏知?jiǎng)伲愓\(chéng)，楊愛(ài)斌.基于遺傳算法優(yōu)化PID的固定翼無(wú)人機(jī)俯仰控制設(shè)計(jì)[J].科技創(chuàng)新與應(yīng)用，2023，13（34）：41?44.

[13] 蘇贇，王挺，姚辰，等.基于合作目標(biāo)的無(wú)人機(jī)目標(biāo)跟蹤方法[J].機(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.