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

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

0 引 言

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

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

1 無人系統(tǒng)增穩(wěn)云臺結(jié)構(gòu)與總體設(shè)計

1.1 增穩(wěn)云臺結(jié)構(gòu)模型簡化

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

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

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

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

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

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

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

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

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

2.1 核心控制器硬件電路設(shè)計

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

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

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

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

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

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

2.2 FOC 電機驅(qū)動硬件設(shè)計

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

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

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

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

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

3.1 FOC 電機控制算法

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

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

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

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

通過采集三相電機的相電流后，結(jié)合從磁編碼器得到的電機轉(zhuǎn)子位置信息進(jìn)行一次Clark 變換和一次Park變換，可以得到在同步旋轉(zhuǎn)坐標(biāo)系下的q軸電流iq和d軸電流id。電磁轉(zhuǎn)矩的大小與iq的數(shù)值成正比，因此可以通過閉環(huán)控制iq的值來控制電機的電磁輸出轉(zhuǎn)矩[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 計算后得到目標(biāo)值Vq、Vd，結(jié)合轉(zhuǎn)子當(dāng)前的轉(zhuǎn)子角度對Vq、Vd進(jìn)行反Park 變換后，得到靜止坐標(biāo)系下的Vα和Vβ。

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

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

3.2 FreeRTOS 任務(wù)調(diào)度

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

表1 中列出了設(shè)計中的6 個線程以及各個線程負(fù)責(zé)的任務(wù)。

表1 線程及其任務(wù)

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

本文設(shè)計中使用的MPU6000 六軸姿態(tài)傳感器內(nèi)自帶的DMP（Digital Motion Processor）功能模塊。通過對MPU6000 的DMP 模塊進(jìn)行配置便可以直接讀出姿態(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]進(jìn)行姿態(tài)解算的方法獲取歐拉角數(shù)據(jù)。

3.3.1 建立坐標(biāo)系

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

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

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

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

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

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

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

對其進(jìn)行求導(dǎo)得：

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

到此便獲得了四元數(shù)與機體三軸角速度的關(guān)系，在周期性獲取角速度數(shù)據(jù)并利用重力加速度對其進(jìn)行糾正后，通過式（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)的比例積分微分控制器中的超調(diào)性和快速性之間的固有矛盾。其數(shù)學(xué)表達(dá)形式[10]如下：

圖7 LADRC 控制算法框架圖

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

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

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

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

4 實驗與結(jié)果分析

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

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

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

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

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

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

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

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

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

圖11 云臺風(fēng)洞試驗圖

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

圖12 Pitch 軸角度在風(fēng)場中的角度變化

圖13 Yaw 軸角度在風(fēng)場中的角度變化

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

5 結(jié) 論

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