無人機高機動抗擾軌跡跟蹤控制方法

王英勛 宋欣嶼 趙江 蔡志浩

(北京航空航天大學 自動化科學與電氣工程學院, 北京 100083)

近年來,四旋翼無人機以其獨特的外形、結構與飛行方式逐漸成為國內外關注的熱點。 與常規布局的直升機相比,四旋翼無人機機械結構簡單,成本較低,易于維護,4 個螺旋槳對稱分布,使得四旋翼無人機的機動能力更強,靜態盤旋的穩定性更好,也更容易實現機型的微小型化。 四旋翼無人機特別適合在近地面環境(如室內、城市和叢林等)中執行監視、偵察等任務,具有廣闊的應用前景[1]。 在進行這些任務時,往往需要無人機進行高速度、高加速度、高角速度、高角加速度的運動,然而,四旋翼在高機動飛行時,系統呈現嚴重的非線性和非定常特性,氣動阻力難以建模,給控制器設計帶來了很大的難度。 此外,精準的跟蹤高機動軌跡還需要參考其高階時間導數,即加加速度和加加加速度。 傳統的控制方法難以解決建模不準和擾動的問題,也無法考慮軌跡的高階時間導數,在高機動飛行時控制效果不佳,難以跟蹤機動性較強的軌跡[2]。

非線性動態逆控制(nonlinear dynamic inversion, NDI)通過對動態系統求逆來實現非線性控制系統反饋線性化,是非線性控制中的一種較為有效的方法[3],在飛行器控制領域被廣泛應用[4],對非線性控制系統具有很好的跟蹤性能。但是NDI 對建模誤差十分敏感,控制器的魯棒性較低[5]。 針對這一問題,Sieberling 等[6]在NDI 基礎上提出了增量非線性動態逆控制(incremental nonlinear dynamic inversion, INDI),通過角加速度反饋解決了NDI 魯棒性不足的問題,實現了對固定翼無人機姿態的控制,但只進行了仿真,未進行試飛測試。 Simplício 等[7]將INDI 用于 直 升 機 懸停控制,驗證了INDI 對模型的依賴性較低,具有較強的魯棒性,但不適用于軌跡跟蹤控制。 Lu等[8]用INDI 實現了固定翼有人機作動器故障時的軌跡跟蹤控制。 近年來,INDI 逐漸被用于四旋翼無人機的控制領域[9-14],但在估計角加速度時,均采用了延遲較大的方法,對控制精度有著較大的影響。

角加速度不易測量[11,15],角加速度計是測量飛行器角加速度最直接的裝置,但由于成本和飛行器尺寸的限制,還未應用于小型無人機[16-18]。雖然也可以通過線加速度計間接測量角加速度值,但需要在機身上合理布置至少3 組加速度計,難以實現[19-21]。 除卻測量角加速度外,國內外也提出了多種數值估計方法用以計算角加速度的估計值。 最常見的方法是對角速度進行微分獲取角加速度的估計值,但其會放大噪聲的影響,為此引入低通濾波器,但又會帶來一定的延遲。 為了在不引入延遲的同時抑制噪聲,Valiviita 和Ovaska[22]提出了一種預測低通濾波器,雖然取得了很好的效果,但其假設加速度可以通過分段低次多項式準確近似,不適用于機動性較強的無人機。Smeur 等[9]使用低通濾波同時處理角加速度和控制輸入,實現了滯后同步,雖然看起來很簡單,但滯后時間是不確定的。 此外,卡爾曼濾波器基于以高斯白噪聲為輸入的恒定加速度模型[23-25],但實際上無人機的飛行并不一定表現出類似的穩定隨機過程[26-27]。

本文主要創新點如下:①設計了基于INDI 方法的軌跡跟蹤控制器,為減小軌跡跟蹤的延遲,引入了微分平坦前饋。 ②由于角加速度無法直接獲得,INDI 方法對其又非常敏感,本文設計了多種角加速度估計方法,并通過飛行試驗選擇了效果最佳的估計方法。 ③通過飛行試驗驗證了所提控制器具有高精度的高機動軌跡跟蹤效果,且具有較強的抗擾能力。

1 預備知識

1.1 無人機模型

本文研究對象為小型四旋翼無人機,慣性參考坐標系選擇北東地坐標系,其正交基為(ix,iy,iz),機體坐標系固定在四旋翼飛行器上,坐標原點與其質心重合,如圖1 所示。

圖1 中所示的單位矢量是機體坐標系下的基向量,形成從機體坐標系到慣性參考坐標系的轉換旋轉矩陣R=(bxbybz)∈SO(3),即

圖1 四旋翼及機體坐標系定義Fig.1 Quadrotor with body-fixed reference system

四旋翼無人機的質心運動模型為

式中:x和v分別為慣性參考坐標系下的位置和速度。 加速度主要考慮3 項:①向下的重力加速度g;②推重比τ為總推力T和機體質量m之比,由于推力矢量沿機體坐標系的bz的反方向,四旋翼向前、向后或側向加速時必須進行俯仰或滾轉運動;③外部干擾力矢量fext,其包含所有作用于機體的其他力(如空氣阻力)。

假定每個螺旋槳軸均與bz軸完美平行,用Ti和Qi來描述每個電機產生的推力和扭矩,則有

式中:ωi＞0(i=1,2,3,4)為每個電機的旋轉速度;kτ為電機推力系數;kq為電機扭矩系數。

電機的序號和旋轉方向如圖2 所示。

圖2 旋翼序號及旋轉方向Fig.2 Rotor serial number and rotation direction

1.2 微分平坦前饋

控制器的目標是精準地跟蹤由式(12)定義的參考軌跡:

σref(t)由4 個微分平坦輸出組成,即慣性參考坐標系下的四旋翼位置xref(t) ∈R3和機身偏航角ψref(t)∈T,T 表示圓組。

假設xref的前四階導數存在且連續,ψref前二階導數存在且連續。 在慣性參考坐標系中,xref對時間求導依次產生參考速度vref、參考加速度aref、參考加加速度jref和參考加加加速度sref,即

通過上面的推導,可以將軌跡跟蹤問題重新構造為狀態跟蹤問題,將推導出的參考角速度Ωref和參考角加速度Ω·ref關于參考軌跡加加加速度sref、加加速度jref、偏航角速度?ψref和偏航角加速度¨ψref的表達式,作為軌跡跟蹤控制中的前饋輸入。

1.3 增量非線性動態逆控制原理

常規非線性系統的狀態空間表達式可以寫為

式中:x(t)為狀態向量;u(t)為控制輸入向量;y(t)為輸出向量。 不同于NDI 方法將整個系統的動態特性求逆,INDI 方法在當前時刻的狀態x0和控制輸入u0下對系統的動態特性進行線性化并求逆。

使用一階泰勒級數展開式重寫式(26),忽略其他高階項可得

2 基于前饋INDI 的抗擾控制器設計

總體控制框架如圖3 所示。 其中,無人機的加速度由加速度計測得,用ab表示在機體坐標系下未經過濾波的加速度測量值,用a表示慣性參考坐標系下經過重力加速度校正的加速度,則有

圖3 總體控制框圖Fig.3 Overall control diagram

無人機的位置信息由OptiTrack 提供,為了融合加速度計和OptiTrack 的信息,采用了擴展卡爾曼濾波(EKF)方法,獲得了經過EKF 處理后的位置xf、速度vf、加速度af。 角速度由陀螺儀測量得到,根據角速度信息和角加速度估計方法得到角加速度的估計值Ω·f。

2.1 位置和速度控制

位置和速度控制基于2 個串聯的PD 控制器,所得的控制器表達式為

式中:Kx為位置環對角增益矩陣;Kv為速度環對角增益矩陣;下標ref 表示從參考軌跡獲得的參考值,下標c 表示控制器計算的命令值。 式中的前2 項確保跟蹤參考位置和參考速度,最后1 項作為前饋輸入以確保跟蹤參考加速度,得到的加速度命令用于計算推力和姿態命令。

位置和速度控制原理框圖如圖4 所示。

圖4 位置和速度控制Fig.4 Position and velocity control

2.2 INDI 加速度控制

加速度控制原理框圖見圖3 左側虛線部分。

2.3 姿態和角速度控制

式(36)和已知的偏航角參考值ψref共同構成姿態角指令:

式中:τf為根據電機轉速估計值和推力模型估算的當前推重比。

式(34)增量性質使控制器可以在存在干擾或建模誤差的情況下實現對加速度的控制,所需的推重比為

姿態和角速度控制的原理框圖如圖5 所示。

圖5 姿態和角速度控制Fig.5 Attitude and angular rate control

2.4 INDI 角加速度控制

由式(5)可知

式中:Mf為機體坐標系下根據轉速指令計算的控制力矩。

將式(39)代入式(5)可得

由于角加速度無法直接獲得,需要通過角加速度估計方法進行估算,Ω·f有一定時間延遲,式(41)中所有變量應為同一時刻,否則該式是錯誤的,造成系統振蕩發散。 因此,在利用當前時刻電機轉速估算控制力矩Mf時,應使其延遲與角加速度估計延遲保持一致。 角加速度控制原理框圖見圖3 中間虛線部分。

2.5 電機轉速估計

由于缺少電機轉速的傳感器和電調的轉速反饋,引入作動器模型以獲得當前時刻的電機轉速的估計值。 電機轉速指令與其響應之間的動態關系可以表示為連續的一階傳遞函數。

式中:ωci(i=1,2,3,4)為每個電機的轉速指令;τa為可以通過參數辨識獲得的作動器時間常數。

根據估計的轉速和1.1 節中的無人機模型,即可估算無人機當前時刻的推重比τf和控制力矩Mf。

2.6 基于互補濾波的低延遲角加速度估計

INDI 基于加速度反饋,降低了對模型氣動參數的敏感性,提高了控制的魯棒性,因此,加速度值的準確性和實時性是實現上述方法的基礎。 在實際應用中,小型無人機由于體積和成本的原因,主要裝備的是慣性傳感器(inertial measurement unit,IMU),無法直接獲得角加速度,因此需要設計延遲低、精度高的方法對角加速度進行估計。

2.6.1 基于模型預測法的角加速度估計

根據飛行器的動力學方程(5),可以通過理論估計方法計算出角加速度信號,如下:

由于實際中無法對飛行器的參數和空氣動力學準確建模,控制力矩會受到模型不確定性的影響,無法準確估計,此外系統擾動無法準確建模,模型預測方法無法考慮實際飛行中外界干擾的影響,外界干擾的影響會導致估計結果偏離真值。雖然該方法的魯棒性較差,在實際中不能很好地估計角加速度,但其不使用微分運算,也不需要低通濾波器,因此噪聲很小,相位滯后幾乎為零。 由于模型預測方法是一種基于系統非線性動力學方程的理論估計方法,其動態響應非常快。

2.6.2 基于微分法和低通濾波的角加速度估計

微分法是現實中估計角加速度信號最直接和最常用的方法。 由于微分運算對系統噪聲有嚴重的放大作用,會導致信號性能變差,通常使用二階巴特沃斯低通濾波器(low-pass filter, LPF) 對IMU 信號進行濾波,以減輕機身振動和其他噪聲的影響,再進行微分運算獲取角加速度。

式中:ωn為低通濾波器的截止頻率。

很難找到一種低通濾波器,既能表現出小的相位滯后,又能抑制微分操作引起的放大噪聲。但該方法不依賴于系統模型參數,其估計結果可以有效地反映外界干擾和系統不確定性的影響,從而能夠真實地估計出實際角加速度信號的變化。

2.6.3 基于互補濾波的角加速度估計

模型預測方法具有噪聲少、相位滯后小的優點,可以彌補微分法的不足。 微分法可以估計實際角加速度信號的真值,彌補模型預測估計結果的誤差。 因此,基于互補濾波(complementary filter, CF)理論的思想對估計結果進行數據融合,可以得到較好的估計結果。

通過推導,角加速度的最終估計值如下:

圖6 互補濾波Fig.6 Complementary filter

從方程(48)可以看出,T(s)為一個高通濾波器,其保留了模型預測方法的有益的高通特性,并抑制影響低頻不確定性和干擾;S(s)為一個低通濾波 器,可 濾除Ω·^D(s)的高頻噪聲并保留信號的低通特性。 由于G(s)是一個積分器,G(s)Ω·^D(s)

是可以通過IMU 測量的角速度信號。 因此,IMU的測量值可以直接用于數據融合方法的實際實現中,巧妙地避免了整個估計過程中的微分運算。因此,該方法不會遇到由差分操作引起的噪聲放大問題。

T(s) +S(s) =1 是互補性的體現。 它將每種方法的有利部分保留在最終估計結果中,并確保截止頻率彼此相同。 因此,微分法的相位滯后問題通過補償模型預測方法的高通特性來解決。 通過角速度信號的反饋,可以消除外界干擾和模型不確定性的影響。 綜上所述,該估計方法的最大優點是在估計過程中巧妙避免了微分運算,有效降低噪聲的放大效應,提高信號性能。

采集實飛數據,分別用較為常用的直接微分法、卡爾曼濾波方法和互補濾波方法進行角加速度估計。 直接微分法由于噪聲被放大,根據多次試驗,選擇了延遲相對較小且對噪聲抑制效果較好的、截止頻率為70 rad/s 的二階巴特沃斯低通濾波器濾波處理。 卡爾曼濾波器中由于控制器的控制周期為250 Hz,因此ΔT=0.004 s。 互補濾波器的H(s)根據試驗反復調整到最佳效果。三者對角加速度估計結果如圖7 所示,其中虛線為真實值,紅色實線為互補濾波方法估計結果,藍色虛線為卡爾曼濾波方法估計結果,綠色點線為對角速度直接微分后低通濾波的估計結果。

圖7 角加速度估計結果Fig.7 Angular acceleration estimation results

低通濾波方法雖然抑制噪聲的效果很好,但其時間延遲較大,卡爾曼濾波方法雖然可以實時計算最優卡爾曼增益,但由于其計算角加速度預測值時采用了線性模型,在姿態變化劇烈時測量值與預測值誤差較大,也導致了時間延遲較大。與卡爾曼濾波和微分后低通濾波方法相比,互補濾波方法雖然計算量更大,但其抑制噪聲的能力更強,時間延遲也更小。 因此,在實現高機動軌跡跟蹤控制時,采用互補濾波方法獲得角加速度反饋以提高INDI 方法的精度。

3 飛行試驗

為了驗證本文所設計控制器的精度和抗擾性能,采用圖1 中所示四旋翼無人機進行飛行試驗。該四旋翼重667 g,推進系統由T-MOTOR F1507 3 800 kV 電機、T-MOTOR T3140 槳葉和T-MOTOR V45A V2 電調組成,相鄰電機相距15 cm,由一個4S LiPo 電池供電。 飛控選用Pixraptor,控制頻率為250 Hz,飛機的姿態、角速度、加速度等由其內部IMU 測量得到,位置信息由120 Hz 的OptiTrack動作捕捉系統提供。 利用EKF 方法對OptiTrack 的位置信息和IMU 的加速度信息進行處理融合,得到融合后的位置、速度和加速度信息,根據試驗選擇延遲最小、精度最高的互補濾波方法對角加速度進行估計。

首先,使跟蹤速度和加速度都很大的高機動軌跡驗證其對高機動軌跡的跟蹤能力;其次,通過附加超過飛行器正面面積4 倍以上的紙板增大飛行過程中的阻力,增大其模型誤差,驗證其對內部干擾的魯棒性;然后,利用風扇測試其在5 m/s 的風干擾中的懸停能力,進一步展示了對外部干擾的魯棒性;最后,將設計的控制器性能與常規PID控制器進行對比分析。

3.1 軌跡跟蹤試驗

為了保證試驗的安全,軌跡跟蹤的起始和終止狀態飛機應保持懸停,根據參考文獻[27]中的Minimum snap 方法,設計了不同加速度、速度的8 字形參考軌跡,分別利用前饋PID 方法和本文所設計的前饋INDI 方法對其進行跟蹤。 當參考軌跡的最大速度為4. 481 m/s,最大加速度為5.117 m/s2時,前饋PID 方法和前饋INDI 方法均能很好地跟蹤參考軌跡,但前饋INDI 方法跟蹤的均方根誤差遠小于前饋PID 方法。 試驗結果如圖8和表1 所示。

圖8 低速軌跡跟蹤曲線Fig.8 Low speed trajectory tracking curves

表1 低速軌跡跟蹤效果Table 1 Low speed trajectory tracking performance

在跟蹤最大速度為6. 098 m/s、最大加速度為12.648 m/s2的8 字形軌跡時,前饋PID 方法的跟蹤誤差為17.238 cm,而前饋INDI 方法僅為7.175 cm。 此時,前饋PID 方法控制飛行器進行軌跡跟蹤時,姿態較為振蕩,穩定性下降。 試驗結果如圖9和表2 所示。

表2 中速軌跡跟蹤效果Table 2 Medium speed trajectory tracking performance

圖9 中速軌跡跟蹤曲線Fig.9 Medium speed trajectory tracking curves

當參考軌跡的速度和加速度進一步提高時,前饋PID 方法無法跟蹤,而前饋INDI 方法仍然能保持很高的精度。 采用不同的角加速度估計方法跟蹤最大速度10.18 m/s、最大加速度15.36 m/s2的參考軌跡時,跟蹤結果如圖10 所示,飛行過程中的位置誤差、偏航角誤差、速度和加速度曲線如圖11所示,試驗數據如表3 所示。

表3 高機動軌跡跟蹤效果Table 3 Aggressive trajectory tracking performance

圖10 高機動軌跡跟蹤曲線Fig.10 Aggressive trajectory tracking curves

圖11 高機動軌跡跟蹤曲線Fig.11 Aggressive trajectory tracking curves

跟蹤過程中,基于互補濾波的前饋INDI 控制方法的跟蹤誤差僅為10.310 cm,偏航角的均方根誤差為2.645°。 飛行器的最大速度為10.416 m/s,平均速度為 4. 654 m/s, 最大加速度為17.119 m/s2,平均加速度為7.434 m/s2。 可見設計的基于互補濾波的前饋INDI 控制器可以跟蹤高機動軌跡,有著較高的精度。

3.2 抗擾試驗

3.2.1 內部干擾

在飛機下方懸掛如圖12 所示面積大于其4 倍的硬紙板,增大其運動時的空氣阻力,增大其與仿真模型的不匹配度,以測試該控制器對內部模型誤差干擾的魯棒性。

圖12 懸掛紙板的四旋翼無人機Fig.12 Quadrotor with cardboard drag plate

分別用微分平坦前饋PID 控制器和本文所設計的控制器控制四旋翼跟蹤最大速度4.07 m/s、最大加速度8. 45 m/s2的軌跡,效果如圖13 所示,飛行過程中的位置誤差、偏航角誤差、速度和加速度曲線如圖14 所示。

圖13 帶紙板軌跡跟蹤曲線Fig.13 Tajectory tracking curves with cardboard drag plate

圖14 帶紙板軌跡跟蹤曲線Fig.14 Trajectory tracking curves with cardboard drag plate

前饋PID 控制器跟蹤該軌跡時,均方根誤差為8. 091 cm,但加入紙板后,均方根誤差變為10.00 cm,精度有所下降。 前饋INDI 控制器跟蹤軌跡時,均方根誤差為4.232 cm,加入紙板后為4.494 cm,變化不明顯。 其他參數如表4 所示。 可見,前饋INDI 控制器不僅精度更高,對模型誤差的容忍性也更強,對內部干擾有著優秀的抗擾能力。

表4 軌跡跟蹤效果Table 4 Trajectory tracking performance

3.2.2 外界干擾

為了驗證前饋INDI 控制器對外界干擾的抗擾能力,將功率為280 W 的華生FL750 工業風扇置于飛機懸停點前方2 m 處(見圖15),用風速儀測得此處風速約為5 m/s,進行懸停抗擾試驗,得到的懸停效果如圖16 所示。

圖15 懸停抗擾試驗Fig.15 Hover test with disturbance

圖16 懸停抗擾曲線Fig.16 Curves for hover with disturbance

前饋PID 控制器控制四旋翼進行懸停時,風扇啟動后位置和偏航角誤差都顯著增大,位置最大誤差達12.273 cm。 前饋INDI 控制懸停加入風干擾后,位置誤差雖有所增大,但位置誤差最大時也僅為4. 502 cm,風干擾對其懸停的影響不大。 可見對于外界干擾,前饋INDI 方法的魯棒性明顯優于前饋PID 方法。 其他參數如表5所示。

表5 懸停抗擾效果Table 5 Performance for hover with disturbance

綜上所述,不論是對于建模不準確引起的內部干擾,還是外界陣風干擾,前饋INDI 方法都表現出了優秀的抗擾能力。

4 結 論

1) 設計的前饋INDI 控制器在跟蹤高機動軌跡時具有較高的控制精度,在跟蹤最大速度為10.416 m/s,最大加速度為17.119 m/s2,平均速度為4.654 m/s,平均加速度為7.434 m/s2的高機動軌跡時,均方根誤差僅為10.310 cm。

2) 該控制器對內部擾動具有較強的抗擾能力,加入紙板增大模型誤差后,跟蹤最大速度為4.034 m/s,最大加速度為9.299 m/s2的軌跡時,均方根誤差由4.232 cm 變為4.494 cm,無明顯增大。

3) 該控制器對外界風干擾具有較強的抗干擾能力,令控制器控制飛行器保持懸停,在其懸停正前方2 m 處引入風速為5 m/s 的風干擾,位置均方根誤差仍能保持在1.199 cm。

飛行試驗表明,本文設計的基于互補濾波的前饋INDI 軌跡跟蹤控制器可以控制飛行器精準、快速跟蹤參考軌跡,在高機動飛行時仍保持極高的控制精度,對外界風干擾和內部模型誤差有著較強的抗擾能力。