旋翼飛行器抗風(fēng)擾控制器設(shè)計與風(fēng)場環(huán)境建模

黃夢淵，王耀力，張祉怡，常 青

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

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

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

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

1 風(fēng)場模型

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

Dryden 模型的時間頻譜表示為[13]：

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

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

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

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

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

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

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

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

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

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

2.2 動力學(xué)建模

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

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

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

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

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

3 DAFEKF算法設(shè)計

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

3.1 EKF算法

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

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

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

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

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

3.2 DAFEKF算法

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

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

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

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

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

4 算法驗證與分析

該文分別對風(fēng)場和風(fēng)環(huán)境下的四旋翼動力學(xué)進(jìn)行建模，設(shè)計了DAFEKF 濾波算法的狀態(tài)估計器，并與PID 控制器相結(jié)合。將建立的風(fēng)場模型應(yīng)用到四旋翼系統(tǒng)，通過MATLAB/Simulink 對整個系統(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è)計的帶有自適應(yīng)因子的R和Q以及自適應(yīng)衰減因子λk的PID-DAFEKF 控制系統(tǒng)與經(jīng)典的R和Q為固定值的PID-EKF 控制系統(tǒng)，以及魯棒性強(qiáng)的滑模控制系統(tǒng)對風(fēng)場環(huán)境中四旋翼實現(xiàn)定點懸停的控制效果進(jìn)行對比。設(shè)置時間為30 s，對比結(jié)果如圖1 和圖2所示。

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

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

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

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

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

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

5 平臺仿真

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

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

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

圖3 定點懸停流程圖

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

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

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

5.2 軌跡跟蹤

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

圖5 軌跡跟蹤

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

6 結(jié)束語

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