朱玉穎 張果正 任哲昆

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

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

1 擴張狀態(tài)觀測器滯后補償改進的實現(xiàn)

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

1.1 狀態(tài)觀測器估計滯后原理

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

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

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

(1)

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

1.2 擴張狀態(tài)觀測器滯后補償改進

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

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

(2)

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

圖3 完整的姿態(tài)內(nèi)環(huán)控制框圖Fig.3 Block diagram of complete attitude inner ring control

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

2.1 擴張狀態(tài)觀測器滯后補償改進

(3)

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

(4)

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

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

(5)

(6)

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

(7)

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

(8)

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

3 實驗結(jié)果分析

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

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

表1 飛行器結(jié)構(gòu)參數(shù)Table 1 Structural parameters of the aircraft

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

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

4 結(jié)論

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