黃夢(mèng)淵，王耀力，張祉怡，常 青

（1.太原理工大學(xué)信息與計(jì)算機(jī)學(xué)院，山西太原 030024；2.北京師范大學(xué) 人工智能學(xué)院，北京 100875）

無(wú)人機(jī)廣泛應(yīng)用在鐵路檢測(cè)[1-3]、快遞包裹[4-5]等多個(gè)領(lǐng)域。室外風(fēng)場(chǎng)存在會(huì)影響飛行器的飛行姿態(tài)和穩(wěn)定控制性能。使用飛行器實(shí)物驗(yàn)證控制器抗風(fēng)干擾效果時(shí)，容易出現(xiàn)失控，產(chǎn)生危險(xiǎn)，增加飛行器研究成本。

國(guó)內(nèi)外學(xué)者對(duì)相關(guān)內(nèi)容進(jìn)行了廣泛研究。文獻(xiàn)[6]采用LabVIEW 仿真軟件來(lái)顯示飛行器的飛行姿態(tài)，但無(wú)法模擬風(fēng)場(chǎng)環(huán)境。文獻(xiàn)[7-12]采用EKF濾波對(duì)風(fēng)環(huán)境中的飛行器姿態(tài)進(jìn)行實(shí)時(shí)估計(jì)，但抗風(fēng)干擾能力差。

該文主要研究風(fēng)場(chǎng)環(huán)境下的旋翼動(dòng)力學(xué)模型；設(shè)計(jì)雙自適應(yīng)衰減擴(kuò)展卡爾曼濾波算法(Dual Adaptive Fading Extended Kalman Filter，DAFEKF)來(lái)提高姿態(tài)數(shù)據(jù)的濾波精度，并結(jié)合PID 控制器來(lái)增強(qiáng)系統(tǒng)的抗風(fēng)干擾能力；對(duì)室外風(fēng)場(chǎng)環(huán)境進(jìn)行分析，建立3D風(fēng)場(chǎng)仿真模型，進(jìn)一步驗(yàn)證系統(tǒng)的控制效果。

1 風(fēng)場(chǎng)模型

在自然界中往往存在著多種類型的風(fēng)，通常有平均風(fēng)、風(fēng)切變、紊流風(fēng)、陣風(fēng)等。但四旋翼飛行器在室外飛行時(shí)，通常作用在四旋翼上的主要是紊流風(fēng)和陣風(fēng)。對(duì)于紊流風(fēng)，一般用Dryden 模型來(lái)產(chǎn)生紊流風(fēng)。將高斯白噪聲序列通過(guò)成形濾波器形成有色噪聲序列，實(shí)現(xiàn)對(duì)大氣紊流的建模。

Dryden 模型的時(shí)間頻譜表示為[13]：

其中，σμ、σv、σω分別表示縱軸、橫軸和豎軸方向的紊流強(qiáng)度，Lμ、Lv、Lω分別表示三軸方向的紊流尺度，v代表旋翼飛行器的飛行速度。由于旋翼飛行器一般都在低空飛行，則依據(jù)8785C 規(guī)定，紊流強(qiáng)度和紊流尺度可用以下公式得出[13]：

其中，h代表飛行器的飛行高度；u20代表高度為6.096 m 的風(fēng)速大小。

將單位強(qiáng)度的白噪聲序列通過(guò)一個(gè)強(qiáng)度為G(s)的成形濾波器，產(chǎn)生的輸出序列的頻譜函數(shù)為：

其中，*代表共軛。根據(jù)式（3）對(duì)式（1）進(jìn)行分解，可以得到成形濾波器的傳遞函數(shù)為[13]:

離散突風(fēng)是指在各個(gè)方向上出現(xiàn)忽大忽小的風(fēng)，又稱陣風(fēng)。通常用半波離散突風(fēng)模型表示[14]，如式（1）：

其中，dm為陣風(fēng)尺度范圍，為陣風(fēng)峰值，x為距離陣風(fēng)中心的距離。

將紊流風(fēng)和陣風(fēng)相結(jié)合可得到真實(shí)風(fēng)場(chǎng)環(huán)境，如式（6）：

2 風(fēng)場(chǎng)中的旋翼動(dòng)力學(xué)模型

2.1 坐標(biāo)系的建立

四旋翼飛行器在風(fēng)場(chǎng)環(huán)境下進(jìn)行動(dòng)力學(xué)建模時(shí)，首先建立相應(yīng)的坐標(biāo)系。該文采取了慣性坐標(biāo)系O3X3Y3Z3、機(jī)體坐標(biāo)系O2X2Y2Z2、氣流坐標(biāo)系O1X1Y1Z1。X軸為四旋翼的飛行速度方向，Y軸由右手定則確定，Z軸垂直水平面向上。姿態(tài)角度θ、Ψ、φ為俯仰角、偏航角、滾轉(zhuǎn)角。氣流角α、β為仰角、側(cè)滑角。

2.2 動(dòng)力學(xué)建模

通過(guò)對(duì)四旋翼運(yùn)動(dòng)過(guò)程分析可知，四旋翼運(yùn)動(dòng)狀態(tài)一般分為平移運(yùn)動(dòng)和轉(zhuǎn)動(dòng)運(yùn)動(dòng)。因此，采用Newton-Euler 法對(duì)風(fēng)場(chǎng)環(huán)境下的四旋翼的平移運(yùn)動(dòng)和轉(zhuǎn)動(dòng)運(yùn)動(dòng)進(jìn)行建模。

由空氣動(dòng)力學(xué)可知四旋翼所受風(fēng)場(chǎng)阻力為[15]：

其中，CW為風(fēng)場(chǎng)阻力系數(shù)，S為機(jī)身的有效面積，Vm為慣性坐標(biāo)系下的風(fēng)速大小。

因此，風(fēng)場(chǎng)環(huán)境下四旋翼的平移運(yùn)動(dòng)和轉(zhuǎn)動(dòng)運(yùn)動(dòng)方程為：

其中，k為升力系數(shù)，kd為阻力，Ω為電機(jī)轉(zhuǎn)速。

3 DAFEKF算法設(shè)計(jì)

在室外，由于風(fēng)干擾因素的存在，要實(shí)現(xiàn)四旋翼的穩(wěn)定飛行，就必須及時(shí)地獲取飛行器的位置和姿態(tài)信息。通過(guò)IMU 獲取的姿態(tài)數(shù)據(jù)具有大量噪聲，而EKF 濾波具有除去噪聲和對(duì)姿態(tài)進(jìn)行實(shí)時(shí)估計(jì)的功能。但EKF 也存在缺陷，需要對(duì)其進(jìn)行改進(jìn)。

3.1 EKF算法

四旋翼飛行器的非線性系統(tǒng)[16]如式（10）：

其中，Xk和Zk分別代表系統(tǒng)狀態(tài)量和觀測(cè)量；f(·) 和h(·) 代表非線性函數(shù)；B為控制輸入矩陣；wk-1和vk分別代表過(guò)程噪聲和測(cè)量噪聲。

根據(jù)EKF 理論可知，EKF 濾波算法[17]具體實(shí)現(xiàn)過(guò)程如下：

其中，R和Q分別代表測(cè)量噪聲協(xié)方差和狀態(tài)噪聲協(xié)方差。

在擴(kuò)展卡爾曼濾波算法中，不僅存在目前濾波數(shù)據(jù)對(duì)歷史數(shù)據(jù)的過(guò)度依賴問(wèn)題，而且R和Q通常為固定值。但是，四旋翼在室外飛行時(shí)，由于風(fēng)場(chǎng)的影響，測(cè)量噪聲和狀態(tài)噪聲通常并不是固定值，這使得EKF 算法的濾波精度變低，抗風(fēng)干擾能力變差。因此，設(shè)計(jì)了DAFEKF 算法。

3.2 DAFEKF算法

DAFEKF 算法不僅設(shè)計(jì)了自適應(yīng)衰減因子，利用該因子來(lái)減小歷史數(shù)據(jù)對(duì)目前濾波數(shù)據(jù)的影響，同時(shí)，添加自適應(yīng)因子來(lái)不斷對(duì)當(dāng)前的Q和R進(jìn)行調(diào)整，改善濾波精度，提高抗風(fēng)干擾能力。

由EKF 理論可知，采用新息序列來(lái)描述實(shí)際測(cè)量值與預(yù)測(cè)測(cè)量值之間的誤差協(xié)方差[18]如式（12）：

對(duì)R提取的自適應(yīng)因子為：

對(duì)Q提取的自適應(yīng)因子為：

設(shè)計(jì)的自適應(yīng)衰減因子λk如下：

4 算法驗(yàn)證與分析

該文分別對(duì)風(fēng)場(chǎng)和風(fēng)環(huán)境下的四旋翼動(dòng)力學(xué)進(jìn)行建模，設(shè)計(jì)了DAFEKF 濾波算法的狀態(tài)估計(jì)器，并與PID 控制器相結(jié)合。將建立的風(fēng)場(chǎng)模型應(yīng)用到四旋翼系統(tǒng)，通過(guò)MATLAB/Simulink 對(duì)整個(gè)系統(tǒng)進(jìn)行建模。

設(shè)置四旋翼飛行器的初始位置X=[0 0 0]，期望位置X=[6 6 15]，初始姿態(tài)角η=[0 0 0]，期望姿態(tài)角η=[0 0 0.5]，風(fēng)速為5 m/s。將新設(shè)計(jì)的帶有自適應(yīng)因子的R和Q以及自適應(yīng)衰減因子λk的PID-DAFEKF 控制系統(tǒng)與經(jīng)典的R和Q為固定值的PID-EKF 控制系統(tǒng)，以及魯棒性強(qiáng)的滑?？刂葡到y(tǒng)對(duì)風(fēng)場(chǎng)環(huán)境中四旋翼實(shí)現(xiàn)定點(diǎn)懸停的控制效果進(jìn)行對(duì)比。設(shè)置時(shí)間為30 s，對(duì)比結(jié)果如圖1 和圖2所示。

圖1 三軸位移輸出曲線和三軸位移絕對(duì)誤差值

圖2 姿態(tài)角輸出曲線和偏航角誤差絕對(duì)值

由圖1 可知，在風(fēng)場(chǎng)環(huán)境中，分別給四旋翼3 個(gè)方向設(shè)定期望值，并使用3 種不同的控制方式來(lái)實(shí)現(xiàn)四旋翼的定點(diǎn)懸停和抗風(fēng)干擾控制。采用滑?？刂扑惴ê?，三軸位移的最大絕對(duì)誤差分別約為0.58 m、0.59 m、0.81 m。雖然滑模有較強(qiáng)的魯棒性，但未對(duì)姿態(tài)數(shù)據(jù)進(jìn)行估計(jì)。并且，由于風(fēng)場(chǎng)的不斷擾動(dòng)，使得滑模產(chǎn)生的抖動(dòng)比較大，抗風(fēng)干擾能力差。采用PID-EKF 算法后，不僅對(duì)飛行器姿態(tài)進(jìn)行實(shí)時(shí)估計(jì)，還對(duì)姿態(tài)數(shù)據(jù)進(jìn)行濾波處理，使得三軸位移的最大絕對(duì)誤差分別約為0.2 m、0.32 m、0.45 m。誤差相對(duì)減小，響應(yīng)速度變快。采用PID-DAFEKF 算法后，對(duì)測(cè)量噪聲和狀態(tài)噪聲不斷在線更新，并引入自適應(yīng)衰減因子，減少當(dāng)前濾波值對(duì)陳舊數(shù)據(jù)的過(guò)渡依賴，使得三軸位移的最大絕對(duì)誤差分別約為0.05 m、0.06 m、0.06 m。提高了濾波精度，抗風(fēng)干擾能力更強(qiáng)，使飛行器的懸停更加穩(wěn)定，更加接近期望值。

由圖2 可知，在風(fēng)場(chǎng)環(huán)境中，設(shè)定期望偏航角為0.5 rad。采用滑模控制算法后，X軸和Y軸方向上的位移抖動(dòng)性比較大，使得俯仰角和滾轉(zhuǎn)角的波動(dòng)比較大，飛行器穩(wěn)定性差，同時(shí)，偏航角也產(chǎn)生了較大誤差，最大絕對(duì)誤差值約為0.12 rad。采用PID-EKF算法后，對(duì)飛行器的姿態(tài)進(jìn)行估計(jì)，并對(duì)姿態(tài)數(shù)據(jù)進(jìn)行了濾波處理，使得俯仰角和滾轉(zhuǎn)角的波動(dòng)減小，也使得偏航角的誤差減小，最大誤差絕對(duì)值約為0.1 rad。最后，采用新設(shè)計(jì)的PID-DAFEKF 算法后，對(duì)測(cè)量噪聲和狀態(tài)噪聲不斷在線更新，并引入自適應(yīng)衰減因子，使得姿態(tài)估計(jì)更加準(zhǔn)確，濾波效果更好，偏航角的輸出更加接近期望值，最大絕對(duì)誤差幾乎為零。整體抗干擾能力好。位移與偏航角均方根誤差統(tǒng)計(jì)如表1 所示。

表1 位移與偏航角均方根誤差統(tǒng)計(jì)

由表1 可知，在風(fēng)場(chǎng)環(huán)境下，采用滑?？刂破骱?，位移與偏航角分別與期望值產(chǎn)生一定偏差。當(dāng)使用PID-EKF 算法對(duì)飛行器姿態(tài)進(jìn)行估計(jì)以及對(duì)姿態(tài)數(shù)據(jù)進(jìn)行濾波后，使得RMSE 分別減少了49%、55.7%、49.2%、52.8%。通過(guò)新設(shè)計(jì)的PID-DAFEKF算法對(duì)EKF改進(jìn)后，RMSE分別減少了81.2%、82.7%、84.6%、83.2%，其使飛行器的穩(wěn)定性更好，抗風(fēng)干擾能力更強(qiáng)。

5 平臺(tái)仿真

在前面使用MATLAB/Simulink 對(duì)新設(shè)計(jì)算法的可行性進(jìn)行驗(yàn)證，結(jié)果表明，該算法的抗風(fēng)干擾效果好，為了進(jìn)一步驗(yàn)證該算法的合理性、有效性和可視化四旋翼飛行器在風(fēng)場(chǎng)環(huán)境中的穩(wěn)定性，一般采用飛行器實(shí)物進(jìn)行抗風(fēng)干擾實(shí)驗(yàn)，但容易出現(xiàn)失控、墜機(jī)等現(xiàn)象，增加飛行器研究成本。因此，在開(kāi)源仿真平臺(tái)建立3D 風(fēng)場(chǎng)環(huán)境模型來(lái)實(shí)現(xiàn)四旋翼在仿真平臺(tái)的風(fēng)環(huán)境中進(jìn)行抗風(fēng)干擾測(cè)試，代替使用四旋翼實(shí)物進(jìn)行抗風(fēng)干擾測(cè)試。實(shí)現(xiàn)四旋翼在風(fēng)場(chǎng)中飛行姿態(tài)的可視化，降低飛行器研究成本。

5.1 3D風(fēng)環(huán)境建模

該文采用Gazebo 三維動(dòng)態(tài)仿真平臺(tái)，模擬復(fù)雜的室內(nèi)外環(huán)境、機(jī)器人和飛行器。根據(jù)前面建立的風(fēng)場(chǎng)數(shù)學(xué)模型，在Gazebo 仿真平臺(tái)的世界模型中建立風(fēng)世界，將Gazebo 仿真平臺(tái)中的空世界轉(zhuǎn)變成風(fēng)世界。并通過(guò)mavros 和mavlink 來(lái)實(shí)現(xiàn)Gazebo 仿真平臺(tái)與飛控平臺(tái)相連通。在Gazebo 仿真平臺(tái)模擬的風(fēng)環(huán)境中，使用編寫(xiě)的Ros 代碼對(duì)四旋翼進(jìn)行目標(biāo)位置設(shè)置，為X=[6 6 15]，并利用飛控平臺(tái)的外部控制模式來(lái)控制Gazebo 仿真平臺(tái)中的四旋翼，實(shí)現(xiàn)定點(diǎn)懸停。使四旋翼飛行器在風(fēng)場(chǎng)環(huán)境中的穩(wěn)定性和飛行姿態(tài)可視化。在風(fēng)世界模型中分別對(duì)X軸和Y軸方向設(shè)置風(fēng)速為5 m/s 的風(fēng)場(chǎng)。定點(diǎn)懸停流程圖如圖3 所示，在X軸和Y軸風(fēng)場(chǎng)環(huán)境下的姿態(tài)角如圖4 所示。

圖3 定點(diǎn)懸停流程圖

圖4 風(fēng)場(chǎng)環(huán)境中的姿態(tài)角

圖3 流程圖顯示了通過(guò)Ros 代碼實(shí)現(xiàn)Gazebo 中的四旋翼的定點(diǎn)懸停控制。

由圖4 可知，在Gazebo 中首先引入沿X軸方向的風(fēng)場(chǎng)，仿真平臺(tái)中的四旋翼在俯仰方向產(chǎn)生傾斜，并不斷抖動(dòng)，俯仰角在0.15 rad 附近不斷變化。然后引入沿Y軸方向的風(fēng)場(chǎng)，仿真平臺(tái)中的四旋翼在滾轉(zhuǎn)方向產(chǎn)生傾斜，并不斷抖動(dòng)，滾轉(zhuǎn)角在0.17 rad 附近不斷變化。

5.2 軌跡跟蹤

完成Gazebo 仿真平臺(tái)中風(fēng)世界的建立，并與飛控平臺(tái)相連通之后，通過(guò)QGC 地面站設(shè)置航點(diǎn)位置及航點(diǎn)任務(wù)，進(jìn)行航跡規(guī)劃。在mission 模式下啟動(dòng)四旋翼飛行器，使四旋翼飛行器在風(fēng)場(chǎng)環(huán)境中進(jìn)行軌跡跟蹤，結(jié)果如圖5 所示。

圖5 軌跡跟蹤

在Gazebo仿真平臺(tái)的風(fēng)世界中，引入風(fēng)速為5 m/s的風(fēng)場(chǎng)，設(shè)置飛行高度為15 m，并啟動(dòng)仿真平臺(tái)中的四旋翼在風(fēng)場(chǎng)環(huán)境中進(jìn)行航跡跟蹤。分析飛行器的飛行日志，由圖5 仿真結(jié)果可知，由于風(fēng)場(chǎng)干擾，飛行器在3D 風(fēng)場(chǎng)環(huán)境下的實(shí)際飛行軌跡與期望軌跡產(chǎn)生偏離。旋翼飛行器在俯仰方向和滾轉(zhuǎn)方向產(chǎn)生傾斜并不斷抖動(dòng)；經(jīng)過(guò)新設(shè)計(jì)的PID-DAFEKF 濾波算法處理后，在風(fēng)場(chǎng)環(huán)境中，飛行器的實(shí)際飛行軌跡與期望飛行軌跡相吻合，軌跡跟蹤結(jié)果精確，四旋翼飛行器在風(fēng)場(chǎng)環(huán)境中能穩(wěn)定飛行，具有良好的抗風(fēng)干擾能力和跟蹤效果。

6 結(jié)束語(yǔ)

根據(jù)四旋翼飛行器在室外飛行時(shí)所受的風(fēng)場(chǎng)干擾，設(shè)計(jì)了PID 位置控制器與DAFEKF 濾波算法相結(jié)合的控制系統(tǒng)。在開(kāi)源仿真平臺(tái)中建立3D 風(fēng)場(chǎng)環(huán)境模型，實(shí)現(xiàn)通過(guò)仿真平臺(tái)中的風(fēng)環(huán)境來(lái)驗(yàn)證抗風(fēng)干擾算法的有效性。實(shí)驗(yàn)結(jié)果顯示，設(shè)計(jì)的控制系統(tǒng)不僅具有很好的抗風(fēng)干擾能力，而且開(kāi)源仿真平臺(tái)建立的3D 風(fēng)場(chǎng)環(huán)境能很好模擬飛行器在室外飛行時(shí)所受到的風(fēng)場(chǎng)干擾。