黃海潤(rùn)，施振華，蘇成悅，何家俊，麥偉圖

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

0 引 言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 FOC 電機(jī)驅(qū)動(dòng)硬件設(shè)計(jì)

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

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

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

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

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

3.1 FOC 電機(jī)控制算法

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

圖6 FOC 電機(jī)控制算法框架圖

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

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

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

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

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

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

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

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

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

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

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

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

DMP 模塊減少了MCU 的計(jì)算資源，從而能夠直接獲取到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 互補(bǔ)濾波算法[7]進(jìn)行姿態(tài)解算的方法獲取歐拉角數(shù)據(jù)。

3.3.1 建立坐標(biāo)系

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

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

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

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

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

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

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

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

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

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

3.4 LADRC 控制算法

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

圖7 LADRC 控制算法框架圖

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

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

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

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

4 實(shí)驗(yàn)與結(jié)果分析

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

圖8 三軸增穩(wěn)云臺(tái)試驗(yàn)平臺(tái)

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

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

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

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

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

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

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

圖11 云臺(tái)風(fēng)洞試驗(yàn)圖

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

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

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

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

5 結(jié) 論

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