線性自抗擾的增穩(wěn)云臺控制系統(tǒng)研究與實現(xiàn)

黃海潤，施振華，蘇成悅，何家俊，麥偉圖

（1.廣東工業(yè)大學 物理與光電工程學院，廣東 廣州 510006；2.廣州杰超科技有限公司，廣東 廣州 510006）

0 引 言

無人系統(tǒng)中，云臺的控制策略應用較多的仍然是傳統(tǒng)的PID 控制算法，但該算法對無人系統(tǒng)運動中產(chǎn)生的隨機干擾響應較慢。基于傳統(tǒng)PID 控制算法，韓京清提出一種自抗干擾控制算法ADRC[1]。Mehran 等人通過帶寬參數(shù)化的方法，將ADRC 簡化為線性自抗干擾控制LADRC，減少了需要整定的控制參數(shù)，以便于將ADRC控制算法更好地推廣應用到實際的工程中[2]。

本文基于LADRC 控制算法并結合IMU 傳感器，實現(xiàn)無人系統(tǒng)云臺自行穩(wěn)定的功能，并使用FOC 矢量控制電機驅動算法攝像機云臺機動結構進行控制。

1 無人系統(tǒng)增穩(wěn)云臺結構與總體設計

1.1 增穩(wěn)云臺結構模型簡化

無人系統(tǒng)的增穩(wěn)云臺的簡化結構如圖1 所示，其由3 個云臺電機、內(nèi)外框骨架以及底座與減震結構[3]組成。為了保證攝像機能穩(wěn)定地采集圖像，分別使用3 個電機對攝像機相互垂直的三軸姿態(tài)角（Yaw、Pitch、Roll）進行控制。

圖1 無人系統(tǒng)增穩(wěn)云臺機械結構示意圖

為保證云臺結構的穩(wěn)定性與Pitch 軸的電機均勻受力，采用雙臂外框的機械結構設計。

1.2 無人系統(tǒng)增穩(wěn)云臺總體設計框圖

整個無人系統(tǒng)增穩(wěn)云臺包括三個部分：云臺姿態(tài)控制核心、FOC 關節(jié)電機驅動以及與上位機和遙控器的通信，總體設計框圖如圖2 所示。主控核心MCU 通過SPI通信協(xié)議與無人系統(tǒng)增穩(wěn)云臺內(nèi)框中的MPU6000 六軸運動傳感器通信，獲取內(nèi)框中攝像機的三軸角速度和三軸加速度。主控對傳感器的原始數(shù)據(jù)進行處理，使用四元數(shù)法進行姿態(tài)解算，得到云臺的Roll、Pitch、Yaw 的歐拉角數(shù)據(jù)。主控結合RC 遙控的控制命令與當前的姿態(tài)角數(shù)據(jù)，通過LADRC 算法得到云臺電機的速度目標值。

圖2 無人系統(tǒng)增穩(wěn)云臺總體設計示意圖

當前無人系統(tǒng)云臺普遍采用三相無刷電機作為驅動機構，其驅動方式多為開環(huán)的SPWM 波控制方式。SPWM 開環(huán)控制三相無刷電機具有無需匹配電機參數(shù)、硬件成本低、對控制器性能要求低的優(yōu)點，但SPWM 開環(huán)控制存在控制精度差、響應速度慢等問題。而對三相無刷電機采用磁場矢量定向控制策略（Field-Oriented Control, FOC）則可以避免以上問題。

主控通過RS 485 總線協(xié)議將電機的速度目標值發(fā)送至單軸FOC 云臺電機驅動器，驅動器驅動無刷電機轉動，調整攝像機的姿態(tài)。

2 無人系統(tǒng)增穩(wěn)云臺硬件設計

2.1 核心控制器硬件電路設計

本文設計選用STM32F103T8U6 作為云臺的控制核心，設計兩個串口分別用于調試或接收鏡頭球機的控制信號和向電機發(fā)送控制信號。

調試/控制信號接口通過USB 轉串口模塊連接到計算機PC 端，在串口調試助手中打印程序運行的Log 日志以及運行狀態(tài)，便于調試時使用。同時，可以接收鏡頭機芯發(fā)來的控制信號。

電機控制串口的通信線由于需要通過云臺電機中間走線，易受到電機運行時產(chǎn)生的電磁波干擾，因此通過MAX13487 芯片將TTL 電平的串口通信信號轉為RS 485 差分通信信號后，通過SH1.0 接線端子引出。無人系統(tǒng)增穩(wěn)云臺主控硬件設計框架圖如圖3 所示。

圖3 無人系統(tǒng)增穩(wěn)云臺主控硬件設計框架圖

為了兼容航模，常用遙控RC 信號控制增穩(wěn)云臺角度的功能，通過排針將STM32 帶有PWM 捕獲功能的引腳引出，期間使用RC 濾波電路對信號進行濾波處理。

系統(tǒng)總電源采用12 V 供電，除了通過接線端子輸出給電機供電，還通過HT7850 芯片與RT9013-3.3 V 芯片將12 V 電源穩(wěn)壓成5 V 與3.3 V，給MCU 等設備供電。

2.2 FOC 電機驅動硬件設計

無人系統(tǒng)增穩(wěn)云臺電機驅動采用機電一體化設計，將驅動電路嵌入云臺電機中。 電機驅動采用STM32F103C8T6 作為主控MCU，主控實物圖如圖4 所示?？紤]到云臺電機后蓋空間狹小的情況，電機驅動使用德州儀器TI 公司的DRV8313 三相電機驅動方案。DRV8313 電機驅動在大小只有6 mm×8 mm 的芯片內(nèi)集成了MOS 管與MOS 驅動，還具有過流保護、短路保護、欠壓鎖閉和過溫保護等功能。通過INA240APWR 芯片對三相電機的驅動下橋臂采樣電阻兩端的微小電壓進行放大，并通過STM32 的ADC 功能對放大后的模擬電壓信號進行采樣后轉換成數(shù)字信號，供FOC 電流環(huán)控制算法處理。由于矢量控制算法需要知道三相無刷電機的實時轉子位置，故本文選用TLE5012B 磁編碼器對轉子的位置進行追蹤。電機驅動硬件設計框圖如圖5所示。

圖4 無人系統(tǒng)增穩(wěn)云臺主控實物圖

圖5 無人系統(tǒng)增穩(wěn)云臺電機驅動硬件設計框架圖

3 無人系統(tǒng)增穩(wěn)云臺控制軟件設計

FOC 控制策略通過采樣高精度編碼器的數(shù)據(jù)獲取電機轉子的實時位置，從而驅動電機三相驅動合成的輸出力矩方向垂直于轉子。這使得電機的轉矩脈動更小、響應速度快，并具有更高的控制精度[4]，同時可為攝像頭的圖像采集帶來更加平穩(wěn)的畫面。

3.1 FOC 電機控制算法

本文設計中，執(zhí)行機構無刷直流電機采用FOC 電機驅動算法。整體采用串級PID 控制結構，外環(huán)為速度控制環(huán)，內(nèi)環(huán)為電流控制環(huán)。FOC 電機控制算法框圖如圖6 所示。

圖6 FOC 電機控制算法框架圖

對模擬PID 離散化后的PID 算法數(shù)學表達式如下：

式中：KP為比例系數(shù)；KI為積分系數(shù)；KD為微分系數(shù)；e(n)=期望值-實際值。

通過采集三相電機的相電流后，結合從磁編碼器得到的電機轉子位置信息進行一次Clark 變換和一次Park變換，可以得到在同步旋轉坐標系下的q軸電流iq和d軸電流id。電磁轉矩的大小與iq的數(shù)值成正比，因此可以通過閉環(huán)控制iq的值來控制電機的電磁輸出轉矩[4]。

采用id=0 的控制模式，電流環(huán)中iq與id經(jīng)過內(nèi)環(huán)PID，id期望值為0，iq的期望值由速度環(huán)的PID 經(jīng)過計算后得出。內(nèi)環(huán)PID 計算后得到目標值Vq、Vd，結合轉子當前的轉子角度對Vq、Vd進行反Park 變換后，得到靜止坐標系下的Vα和Vβ。

Vα和Vβ再經(jīng)過一次反Clark 變換，可以變?yōu)槿嚯姍C的期望電壓Va、Vb、Vc。之后經(jīng)由SVM 機制[5]更新到各相的PWM 輸出，最后由三相逆變橋作用到電機上。

電機驅動的速度環(huán)則以串口發(fā)來的速度值為目標，結合編碼器前饋的角度數(shù)據(jù)微分作為當前轉速，通過PID 控制器控制[6]。

3.2 FreeRTOS 任務調度

無人系統(tǒng)增穩(wěn)云臺控制軟件部分在ARM-Cortex-M3 開發(fā)平臺上設計，使用FreeRTOS 實時操作系統(tǒng)提高CPU 的使用效率，減輕復雜任務的開發(fā)難度。

表1 中列出了設計中的6 個線程以及各個線程負責的任務。

表1 線程及其任務

3.3 四元數(shù)姿態(tài)解算算法

本文設計中使用的MPU6000 六軸姿態(tài)傳感器內(nèi)自帶的DMP（Digital Motion Processor）功能模塊。通過對MPU6000 的DMP 模塊進行配置便可以直接讀出姿態(tài)歐拉角數(shù)據(jù)。

DMP 模塊減少了MCU 的計算資源，從而能夠直接獲取到IMU 的歐拉角姿態(tài)數(shù)據(jù)。但是DMP 模塊輸出姿態(tài)角的頻率為200 Hz，也就是每5 ms 輸出一次姿態(tài)數(shù)據(jù)。而本文的LADRC 控制器的處理頻率是1 kHz，DMP模塊輸出的速度無法滿足系統(tǒng)閉環(huán)處理的速度。因此采取直接獲取原始數(shù)據(jù)，通過Mahoney 互補濾波算法[7]進行姿態(tài)解算的方法獲取歐拉角數(shù)據(jù)。

3.3.1 建立坐標系

建立兩個坐標系：地理坐標系E與機體坐標系B0。地理坐標系E也稱為導航坐標系，其中X軸指向東，Y軸指向北，Z軸指向天。機體坐標系B0是與機體同步變化，其中X軸指向機體正前方、Y軸指向機體正右方、Z軸機體垂直正上方。姿態(tài)解算其實就是描述地理坐標系到機體坐標系的變化過程。

根據(jù)文獻[7]可得到由地理坐標系E到機體坐標系B0的姿態(tài)變換矩陣CBE為：

用四元數(shù)的形式表示CBE為：

對比式（2）和式（3）可得四元數(shù)轉歐拉角的公式為：

3.3.2 更新四元數(shù)姿態(tài)數(shù)據(jù)

利用加速度對陀螺儀數(shù)據(jù)進行糾正之后，獲取得到的數(shù)據(jù)是在機體坐標系下的三軸旋轉角速度。為了將其更新至四元數(shù)中，需要從四元數(shù)的導數(shù)入手[8]。

四元數(shù)機體三角表達形式如下：

對其進行求導得：

式中，wx、wy、wz為機體坐標系下的角速度。用一階龍格-庫塔法對進行求解可得：

到此便獲得了四元數(shù)與機體三軸角速度的關系，在周期性獲取角速度數(shù)據(jù)并利用重力加速度對其進行糾正后，通過式（7）可以獲取四元數(shù)的最新狀態(tài)。最后利用式（4）可以將四元數(shù)更新為歐拉角數(shù)據(jù)，給LADRC 控制算法作為姿態(tài)的前饋輸入。

3.4 LADRC 控制算法

LADRC 控制算法包含三個主要部分：跟蹤微分器（TD）、線性誤差補償控制器（LESF）、線性擴展觀測器（LESO）[9]，算法框圖如圖7 所示。TD 的作用是為控制輸入提供一個緩沖的過程，提取出包含有不確定干擾或噪聲的輸入信號以及它的微分信號。這個環(huán)節(jié)的主要作用是解決傳統(tǒng)的比例積分微分控制器中的超調性和快速性之間的固有矛盾。其數(shù)學表達形式[10]如下：

圖7 LADRC 控制算法框架圖

式中：v為控制信號的輸入；v1為TD 輸出的控制輸入跟蹤值；v2為輸入信號的微分；r為跟蹤速度因子，用于調控v1對輸入的跟蹤速度；h為濾波因子，用于濾除輸入信號的噪聲；fhan(·)是最速控制綜合函數(shù)[11]。

LESF 線性誤差補償控制器在本質上是一個PD 控制器，其根據(jù)當前整個系統(tǒng)的運行狀態(tài)、監(jiān)控得到的輸出與實際輸出之間的差距，以及估計得到的外部干擾來對系統(tǒng)的輸出進行補償控制。其數(shù)學表達形式如下：

式中：e1為由LESO 估計的系統(tǒng)實際狀態(tài)與TD 處理過的系統(tǒng)信號之間得到的誤差；e2則是系統(tǒng)的微分誤差；b1與b2相當于普通PD 控制器中的比例誤差增益與微分誤差增益；b0為系統(tǒng)系數(shù)。根據(jù)文獻[2]，通過帶寬法可以確定b1=w2c，b2=2wc。其中wc為控制器參數(shù)。

線性擴展觀測器LESO 是整個LADRC 算法的核心部分，相比于傳統(tǒng)的PID 控制，其不僅可以觀測系統(tǒng)實際輸出的狀態(tài)，還將系統(tǒng)受到的總擾動納入估計。LESD 線性擴展觀測器的數(shù)學表達形式如下：

4 實驗與結果分析

在STM32 的MCU 平臺上，設計一種基于LADRC 控制算法與FOC 電機控制算法驅動的云臺系統(tǒng)。為了驗證所設計系統(tǒng)的響應速度與穩(wěn)態(tài)誤差，制作并搭建了如圖8 所示的三軸增穩(wěn)云臺試驗平臺。

圖8 三軸增穩(wěn)云臺試驗平臺

開機后對電機進行校準，完成后默認云臺三軸姿態(tài)穩(wěn)定在零角度。為驗證系統(tǒng)的控制效果，以Pitch 軸為例進行實驗，通過串口向云臺發(fā)送命令，使其俯仰角向上抬30°。

通過對LADRC 的TD 組件r參數(shù)進行調整，可以控制輸入控制器的目標值對控制命令的跟蹤速度。為了保證攝像頭運動的穩(wěn)定性，不斷調整r參數(shù)，最終將數(shù)據(jù)確定在r=1。

圖9 所示為在不同俯仰角下，由控制器LADRC 模塊TD 組件輸出v1與v2對控制命令輸入處理后的結果，其中v1為控制信號跟蹤值，v2為控制信號跟蹤值的微分。由圖9 可知，經(jīng)過TD 微分控制器處理后，階躍控制信號變得平緩，減少了系統(tǒng)的超調，并濾除了控制信號的噪聲。

圖9 Pitch 軸控制輸入與控制器TD 組件輸出v1與v2

實際工況下，環(huán)境中的各種因素都會對云臺系統(tǒng)的姿態(tài)角產(chǎn)生干擾。

圖10 所示為云臺Pitch 軸對v1的跟蹤效果，可知俯仰角在0.3 s 內(nèi)達到設定值，基本沒有超調，且沒有發(fā)生明顯振蕩，基本達到了設計目標。為了驗證所設計的云臺系統(tǒng)的抗干擾性，將云臺放置于實驗室的風洞進行測試，云臺風洞試驗圖如圖11 所示。

圖10 Pitch 軸角度與控制跟蹤效果

圖11 云臺風洞試驗圖

實驗從無風狀態(tài)開始，在第3 s 設置風洞的風速為7.5 m/s，隨后在第6 s 將風速提升至17.5 m/s，模擬八級風的實際環(huán)境。實驗過程中增穩(wěn)云臺各軸角度變化如圖12~圖14 所示。

圖12 Pitch 軸角度在風場中的角度變化

圖13 Yaw 軸角度在風場中的角度變化

由圖12~圖14 可以看到，在八級風17.5 m/s 的風速下，云臺的三軸角度發(fā)生了較為明顯的震動。由于三軸受風面積影響不同，以及三軸負載不同，導致其在風場下的角度變化情況不同，但基本維持在±0.15°以內(nèi)，滿足設計要求。

5 結 論

本文設計了一種將LADRC 控制算法作為控制器的增穩(wěn)云臺，完成了FOC 電機驅動算法的實現(xiàn)；將其作為云臺的機動執(zhí)行機構，從IMU 傳感器獲取角速度數(shù)據(jù)，并通過Mahoney 互補濾波算法進行姿態(tài)解算，獲得云臺姿態(tài)歐拉角度。姿態(tài)數(shù)據(jù)通過LADRC 控制算法處理后，將控制命令發(fā)送到三個角度控制電機。通過搭建實驗平臺對控制效果進行實驗，驗證了該系統(tǒng)的響應時間在0.3 s 以內(nèi)，且超調在1%以內(nèi)，具有較好的控制效果。