水平穩定平臺的姿態解算與控制策略

李凌云，蘆海超，李芳芳，吳秦英

（西安石油大學電子工程學院，西安710065）

0 引言

穩定平臺可分為單軸穩定平臺、雙軸穩定平臺以及三軸（俯仰軸、滾轉軸、方位軸）穩定平臺。其中忽略俯仰通道和滾轉通道——僅考慮繞方位軸的兩自由度水平穩定平臺具有結構簡單、使用靈活等優點，因此具有廣泛的應用背景。在石油工業中，出現了利用全景圖像捕捉設備進行油氣井探測獲得巖層、裂縫等重要信息，進而完成油氣井三維重建的圖形圖像新技術。其工作過程中要求承載圖像捕捉設備的穩定云臺要能夠克服井下惡劣條件及隨機擾動等因素的影響，保持穩定的姿態以獲取高質量的原始圖像信息；此外，穩定平臺也被廣泛應用在水平導向鉆井中：鉆具姿態的實時測量是實現導向的關鍵因素，這些姿態信息包括工具面向角、傾斜角和方位角，它們是由安裝在鉆具內的隨鉆測量單元獲得的，穩定平臺提供的方位角正好可以獲得鉆具的姿態信息[1]；在隨鉆測量技術中，要在鉆進的同時實現各個參數的連續測量，獲得準確的工程參數，穩定平臺的作用必不可少。在安防監管、交通監控等民用領域中，隨著被監控對象的多樣化，現有的固定監控設備已不能很好的滿足人們對動態監控的需求[2]，可移動監控設備將是未來的重要發展方向。可移動的設備如何克服運動過程中振動等外來擾動因素帶來的影響，始終保持監控畫質清晰和精準對焦，也需要應用高性能的穩定平臺來解決[3]。

1 平臺的姿態解算算法

目前載體的姿態解算方法主要有：歐拉法、方向余弦法與四元數法[4]。由于歐拉法的每個計算方程中均含有三角函數，并且會出現“奇點”。故常用后兩種方法來解算出載體的姿態角度。

1.1 方向余弦法

剛體在三維空間中的運動可逆向分解為依次繞三個旋轉軸的旋轉，各軸旋轉角（歐拉角）分別為：俯仰角θ、橫滾角φ 和方位角ψ，其分別繞各自的旋轉軸旋轉后的旋轉向量分別為Tx、Ty與Tz：

方向余弦矩陣可由三維空間中的任意向量R( x,y,z )按照Z-Y-X 的順序旋轉后得到：

穩定平臺在三維空間中的姿態可用方向余弦或者歐拉角表示，用后者表示的方法具有非常直觀的意義。但從式（2）看出，方向余弦矩陣中的9 個元素均為三角函數，在直接參與運算的情況下，其計算量非常大，嚴重影響控制器的工作效率，并不適合在工程實際中應用。相比而言，四元數具有運算量小、計算精度高、不存在奇異性等優點[5]，可預先求取四元數，然后再計算四元數旋轉矩陣與方向余弦矩陣之間的對等關系來提取平臺的歐拉角。

1.2 四元數法及四元數更新

四元數由一個實數和三個虛數構成[5-6]：

由文獻[7]可知，捷聯姿態矩陣與四元數有如下關系：

其中M 稱為四元數的旋轉矩陣，可以看出，矩陣M 與方向余弦矩陣有一一對應的關系，這樣就建立起四元數組成的矩陣M 與方向余弦矩陣之間的關系。由式（2）和式（4）可以通過計算反三角函數求取出三個歐拉角，如下：

故只需知道四元數的四個參數就能解算出平臺最后的姿態角度。

然而平臺時刻處于動態之中，故四元數是一個隨時間變化的函數。建立四元數微分方程如下：

其中，w 為載體坐標系中繞軸向旋轉的角速率，將式（8）展開：

式（10）中，T、T+ΔT 分別表示姿態解算的當前時刻和下一時刻。從上式可看出，理論上只需得到3 個繞軸角速率便可通過四元數法求出平臺的歐拉角。然而，選用的陀螺儀存在積分累積誤差，隨著運行時間的加長，將產生嚴重的漂移，使計算的姿態角存在較大的誤差，最終導致平臺失控。利用加速度計對陀螺儀進行補償可以很好地解決這個問題。

將重力加速度從大地坐標系旋轉到平臺的載體坐標系中，再將其與歸一化的加速度計值做外積[9]，將得到外積誤差融合到陀螺儀值中便可實現對陀螺儀的積分累積誤差進行補償。歸一化后重力加速度g?b如下：

再與加速度做外積，得外積誤差c→：

其中，加速度計在載體坐標系中的加速度計值分別用abx、aby、abz表示。重力加速度旋轉到載體坐標系中的值分別用gbx、gby、gbz表示。由于加速度計與陀螺儀均固定在同一個載體上，最終將該誤差融合到陀螺儀的值上更新四元數即可實現對陀螺儀積分累積誤差的補償。

2 執行機構的控制策略

平臺選用的執行機構為180 度的數字舵機，其采用PWM 信號進行控制。高電平脈寬T 與舵機目標角γ 之間有如下關系：

舵機本身為伺服機構，當它沒有轉到指定位置時，將會全速向目標位置轉動。當其到達了指定位置，將會自動保持該位置，除非指定位置發生改變，否則舵機將會一直保持當前的位置不變，所以對舵機而言，PWM信號提供的是指定位置，跟蹤運動則要靠舵機本身。但僅僅知道式（13）是行不通的，因為舵機是按照PWM信號來實現轉動，所以必須要找到高電平脈沖寬度T與PWM 控制信號之間的對應關系。規定PWM 周期為20ms，將20ms 分為14400 份，則將式（13）轉換為：

此處的γ 是經過增量型PID 算法的輸出值，PWMvalue 為當前的PWM 值，即舵機現在位置對應的PWM 值，初始值設定為1080（對應1.5ms），至于為什么不用PWMvalue=1080+γ*8 現解釋如下：

因為舵機與平臺固聯，而平臺是處在動態當中，所以PWM 的值必定是上一個時刻的值。此處的γ 是經過PID 增量式算法的輸出值。

穩定平臺在水平面內的運動分為原地旋轉運動與水平平移運動。在此，僅對原地旋轉運動進行分析。

如圖1（a）所示，將姿態檢測模塊放于舵機轉軸上，舵機與平臺固聯，假設舵機轉軸基準位置設在1.5ms處，將點劃線部分看成是一個隨平臺運動的基準坐標系。當底座左旋+45°時（b），姿態模塊檢測出此時的姿態角，將其與期望角度做差，該偏差值送給控制器進行增量型PID 算法后，得出的控制量驅動舵機轉軸右旋補償平臺底座的左旋，使舵機轉軸始終指向期望位置。

3 系統測試

為驗證本文所采用的姿態解算算法以及控制策略的有效性，對水平穩定平臺的原地旋轉運動進行測試。主控制器選用STM32f013C8T6；姿態檢測單元選用MPU6050；SR-1501 型數字舵機作為執行機構。利用串口將姿態數據導入電腦后，用MATLAB 繪制平臺響應曲線并對其進行分析。

（1）定值控制時的系統響應

假設上電后舵機處在初始位置（方位角0°），以表1 期望角度為例（僅給出期望角度為+30°的響應曲線如圖2 所示）。

表1 定值控制系統響應情況記錄表

從表1 可以看出，在給定期望角度的情況下，除了在期望角度為60°時，系統的調節時間過長，其他給定的期望角度情況下系統上電后從基準位置轉到期望位置的時間短、超調量小、穩態誤差低。

圖1 平臺底座旋轉示意圖

圖2 期望角度為+30°

（2）人為干擾時的系統響應

在給定期望角度后，并隨機加入人為干擾，平臺響應情況如表2 所示。僅給出期望角度為+30°干擾響應曲線如圖3 所示。

圖3 期望角度為+30°時的干擾響應曲線

表2 人為干擾時系統響應情況記錄表

從表2 可以看出，在給定期望角度的情況下，人為轉動平臺，會給平臺帶來干擾，但平臺最終能夠憑借自身的旋轉回到期望位置，克服了人為干擾，基本滿足系統控制要求。

4 結語

水平穩定平臺一直是隨動跟蹤系統的重要研究方向之一，而平臺的姿態解算與控制策略是其重要組成部分。本文分析了方向余弦矩陣與四元數之間一一對應的關系，用四元數法解算出平臺的姿態角；研究了平臺執行機構的控制策略，從平臺的響應時間可以看出，無論是在定值下還是加入人為干擾，平臺都能回到指定位置，實現對目標的穩定跟蹤，證明了姿態解算算法以及控制策略的有效性。但是，執行機構的響應時間是制約平臺快速性的主要因素之一，下一步主要的研究工作是如何選取性能優良的執行機構來減少系統的響應時間。