基于滯后補償ESO的多旋翼SO(3)全姿態控制

朱玉穎 張果正 任哲昆

(西南科技大學信息工程學院 四川綿陽 621000)

在無人機上安裝的姿態傳感器以矢量方向測量的形式測量其姿態的部分信息，通過姿態估計器處理矢量信息得到機體的方向。傳感器測量和處理這些矢量時可能導致幾十毫秒(甚至更大)的延遲，這種延遲會對觀測器或濾波器的穩定性和魯棒性產生負面影響，如果它們沒有得到合適的補償，則會降低整個系統的性能。傳統的解決傳感器延遲的方法是采取一種具有所需性能的估計器進行無延遲測量，雖然這類改進在工程應用中比較普遍，但是它們需要復雜的穩定性分析和比較精細的增益調諧，這就導致了估計器的瞬態響應較差。目前應用比較廣泛的單步滯后無序量測濾波算法(OOSM-GMPHD)[1-2]雖然易于處理延遲的數據且比較靈活，但會占用較大的內存，對于大多數嵌入式觀測器設計難以實現。本文針對傳感器測量的延遲問題，提出了一個改進的擴展狀態觀測器，補償矢量測量中的延遲影響。本設計基于SO(3)[3-5]上精確的連續時間非線性姿態運動學，而不借助于參數化、線性化或離散化，有效地利用姿態運動學和矢量測量模型的對稱性設計一個簡單的通用擴張狀態觀測器，只需要非常小的計算能力，就能靈活地在嵌入式系統中實現，而且在此基礎上將姿態的R3×3旋轉矩陣的表示方法改進為SO(3)上的姿態分離來實現多旋翼無人機全姿態控制[6-9]。

1 擴張狀態觀測器滯后補償改進的實現

四旋翼飛行器在實際飛行過程中容易受到外界的干擾，為了提高系統的穩定性，人們通過在系統中設計觀測器來減小擾動的影響，但卻忽略了傳感器采集信號時的延遲時間，導致了觀測器輸出不準確[10-11]。本文將改進后的狀態觀測器應用在多旋翼姿態控制的角速度環中，改善控制效果。

1.1 狀態觀測器估計滯后原理

首先對傳感器采集信息的延遲問題建立了如圖1所示的簡單模型。該模型由預采樣延遲和后采樣延遲以及一個零階保持器組成。yi(t)為傳感器待采集的信號量，?i為預采樣延遲的時間，圖中的ZOH把第tki時刻的采樣信號值一直保持到第tki+1時刻的前一瞬時,把第tki+1時刻的采樣值一直保持到第tki+2時刻，以此類推，從而把一個脈沖信號序列變成一個連續的階梯信號。圖中右側的后采樣延遲，表示傳感器采集到的信號量轉化為用戶可以直接使用的數字量的延遲時間σi。通過以上的分析我們可以得到模型最后的輸出為：

圖1 傳感器采樣延遲模型Fig.1 Sampling delay model of the sensor

t∈[tki+σi,tki+1+σi]

(1)

由此可以看出，得到的輸出結果相對于原始信號有一定的延遲，因此狀態觀測器估計出的值也會有一定的延遲[12-13]。本文提出一種改良的狀態觀測器來解決延遲問題。

1.2 擴張狀態觀測器滯后補償改進

狀態觀測器滯后補償模型如圖2所示。本文設計了一種改良的狀態觀測器對傳感器預采樣和后采樣的延遲時間進行補償，并對系統中的狀態變量進行觀測，為方便說明，將改良后的狀態觀測器拆分成一個預測器和一個修正器。

圖2 狀態觀測器滯后補償模型Fig.2 Lag compensation model of state observer

(2)

其中：e為擴張狀態觀測器中間計算變量；Z1_delay為滯后的角速度估計狀態量；Z1為角速度狀態估計量；Z2為角加速度狀態估計量；Z3為系統擾動；?1,?2,?3為可調參數，b為可調參數補償因子，l為可調參數步長。因為內環本身就是一個包含力矩的動力學方程，所以系統存在擾動影響，因此設計出完整的姿態內環控制流程如圖3所示。

圖3 完整的姿態內環控制框圖Fig.3 Block diagram of complete attitude inner ring control

2 SO(3)上的全姿態控制

2.1 擴張狀態觀測器滯后補償改進

(3)

2.2 表示出飛行器SO(3)上的實際姿態

(4)

2.3 表示出飛行器SO(3)上的姿態誤差

圖4 SO(3)姿態矩陣 R3×3Fig.4 Attitude matrix R3×3 of SO(3)

(5)

(6)

對上述兩個姿態誤差向量進行相加即可得到姿態控制器3個通道的全部誤差公式：

(7)

通過以上姿態誤差運算即可解耦分離出俯仰、橫滾和航向上的姿態控制，然后分別對這3個姿態控制通道進行控制律設計即可。由于多旋翼飛行器姿態外環本身就是一個簡單的一階運動學模型，而且沒有任何內部、外部擾動，所以姿態外環的控制律設計使用P比例控制器就可以使得系統穩定，姿態外環的控制率可以設計為：

(8)

至此，狀態觀測器滯后問題的解決方案以及飛行器在SO(3)上的姿態外環控制器設計方法已經全部列出。通過以上內容的研究，將改良的狀態觀測器融入SO(3)的全姿態控制中，既可解決由于傳感器本身采樣帶來的狀態觀測器估計延遲問題，又可顯著提高無人機的控制效果。

3 實驗結果分析

本設計的實驗平臺是自制的350 mm軸距多旋翼無人機飛行器，其飛行器的實物圖片如圖5所示，飛行器結構參數如表1所示。

圖5 飛行器實物圖Fig.5 Aircraft object

表1 飛行器結構參數Table 1 Structural parameters of the aircraft

通過飛行器實際飛行測試驗證，本文提出的狀態觀測器滯后補償改進的方法有效解決了傳感器測量帶來的純滯后影響。傳統的觀測器當傳感器具有一定的采樣延遲時，觀測器的估計值具有一定的延遲，如圖6所示，在100 ms后估計值明顯滯后于測量值。而從滯后補償觀測器的角速度估計圖(圖7)可以看出，改進后的狀態觀測器估計出的角速度明顯超前于傳感器測量角速度，且在估計角速度數據穩定跟蹤傳感器測量角速度數據的前提下更加平滑。此外，在自研的飛行器實驗平臺上測量了一些必要的無人機結構參數輔助我們整定控制器參數。從懸停飛行的角度控制誤差圖(圖8)可以看出，無人機在懸停飛行的50 s中，姿態角控制誤差始終小于0.5度，由此證明本文提出的姿態控制器設計不僅從理論上可以實現，而且在飛行器實際飛行中也可以達到穩定的效果。

圖6 普通觀測器的角速度估計圖Fig.6 Angular velocity estimation of ordinary observer

4 結論

本文提出了一種適用于在延遲矢量測量下的姿態估計的狀態觀測器滯后補償改進方法。利用姿態運動學和系統輸出映射的對稱性，改進的滯后補償觀測器能夠重建矢量測量的連續時間無延遲估計。在SO(3)的全姿態控制中加入改良的狀態觀測器，通過理論分析與實驗證明該觀測器在采樣延遲較大的情況也能達到很好的滯后補償效果。通過該觀測器所得到的估計值相位超前于實際值，而且數據更加平滑，系統的控制效果明顯提升，在工程實際中具有較大的應用價值。