面向小型無人機的兩軸穩(wěn)像云臺控制系統(tǒng)

陳彤陽，王向軍

(1.天津大學(xué)精密測試技術(shù)及儀器國家重點實驗室，天津 300072；2.天津大學(xué)微光機電系統(tǒng)技術(shù)教育部重點實驗室，天津 300072)

0 引言

隨著計算機技術(shù)、機電伺服技術(shù)、計算機視覺技術(shù)的快速發(fā)展，通過穩(wěn)像云臺布置視覺檢測、跟蹤裝置，并掛載在無人機上對陸地和空中目標(biāo)進(jìn)行識別跟蹤的無人機系統(tǒng)得到了廣泛應(yīng)用，并且廣泛應(yīng)用在自主避障、道路巡檢等場合。其中，穩(wěn)像云臺系統(tǒng)作為搭載光電載荷的平臺，是無人機的一個重要組成部分。穩(wěn)定云臺控制系統(tǒng)由伺服控制器、云臺框架、傳感器敏感單元以及載荷等組成，在工作時，需要實時地采集載荷的運動角速度及角位置信息，并通過伺服控制器調(diào)整載荷姿態(tài)，形成閉環(huán)控制系統(tǒng)，從而隔離載荷擾動，穩(wěn)定視軸[1-3]。在云臺控制研究中，常需要對被控對象進(jìn)行各種關(guān)鍵信息采集[4]，傳統(tǒng)單控制器的云臺，需要同時進(jìn)行傳感器數(shù)據(jù)采集、PID控制算法、指令接收與發(fā)送等步驟，會面臨高頻率中斷響應(yīng)的問題，而DSP+FPGA的解決方案雖然彌補了單控制器的缺陷，但是價格高且系統(tǒng)復(fù)雜[5]。

本文為了保證控制的實時性及準(zhǔn)確性，并基于小型化考慮，提出了由C8051F380微處理器及Atmega328微處理器作為核心單元的系統(tǒng)實現(xiàn)方案，系統(tǒng)使用適用于嵌入式系統(tǒng)的四元數(shù)法完成云臺姿態(tài)解算，并通過模糊控制方法優(yōu)化的PID控制器控制系統(tǒng)，避免了傳統(tǒng)PID控制器參數(shù)固定的缺點，同時，使用三階Hermite插值曲線實現(xiàn)變速掃描的任務(wù)。本文設(shè)計的云臺控制系統(tǒng)主要實現(xiàn)兩大功能：控制載荷繞俯仰軸和航向軸運動，并可控制轉(zhuǎn)動的角度；濾除載荷的抖動，使載荷的視軸穩(wěn)定在設(shè)置的角度上，得到清晰的圖像。

1 云臺穩(wěn)像機理

1.1 云臺姿態(tài)解算方法

云臺穩(wěn)像系統(tǒng)的姿態(tài)解算方法使用四元數(shù)法進(jìn)行解算，相較于方向余弦法和歐拉角法，四元數(shù)法具有更小的計算量，且避免了解算過程中的死鎖現(xiàn)象[6]。四元數(shù)法在表示坐標(biāo)系旋轉(zhuǎn)時，每一次的旋轉(zhuǎn)都可以看作是初始坐標(biāo)系繞某向量旋轉(zhuǎn)一定的角度。四元數(shù)描述了云臺框架的定點轉(zhuǎn)動，不關(guān)心中間過程，只等效為一次性旋轉(zhuǎn)結(jié)果，包含了等效旋轉(zhuǎn)的全部信息。

(1)

將一般的歐拉角姿態(tài)轉(zhuǎn)換矩陣[7]與四元數(shù)表示的姿態(tài)轉(zhuǎn)換矩陣進(jìn)行比較，因為在同一個姿態(tài)下，2種表示方法的姿態(tài)轉(zhuǎn)換矩陣相等，比較各項系數(shù)可以得到求解3個姿態(tài)角的表達(dá)式，本系統(tǒng)為兩軸云臺，所以只關(guān)注俯仰角α、航向角φ，求解過程如下[8]：

(2)

在姿態(tài)角計算過程中，需要實時更新四元數(shù)矩陣，結(jié)合云臺穩(wěn)像系統(tǒng)工作時的應(yīng)用場景，需要姿態(tài)解算方法兼顧實時性與高精度的要求。本文首先通過慣性傳感器經(jīng)過低通濾波獲取三軸角速度，然后使用一階龍格庫塔法更新四元數(shù)，在采樣周期Δt內(nèi)，可得四元數(shù)更新方程：

(3)

1.2 云臺穩(wěn)像控制方法

云臺穩(wěn)像系統(tǒng)采用二級模糊控制器，模糊PID控制器的結(jié)構(gòu)如圖1所示。其中，r(t)為目標(biāo)角度，y(t)為實際輸出角度，e為角度偏差值，ec為偏差變化率。每次調(diào)節(jié)前，將不同的e和ec輸入到模糊推理器中，根據(jù)變化實時調(diào)整PID 3個參數(shù)的補償量ΔKp、ΔKi、ΔKd，從而滿足不同的偏差e及偏差變化率ec對PID控制器參數(shù)的需求，使被控對象有更好的動態(tài)性能[9]。

圖1 模糊PID控制器結(jié)構(gòu)圖

模糊PID控制器的設(shè)計分為輸入輸出模糊化、確定隸屬度函數(shù)、建立控制規(guī)則、解模糊、模糊推理。

首先進(jìn)行模糊化，將e、ec、ΔKp、ΔKi、ΔKd的變化范圍定義為模糊集上的論域，模糊子集為{NB,NM,NS,ZO,PS,PM,PB}，集合中的各元素按照次序分別表示負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大。為了減小隨機測量噪聲對模糊控制器的影響，選擇三角型隸屬度函數(shù)作為模糊PID控制器的隸屬度控制規(guī)則?？紤]到控制目標(biāo)為電機位置信號，偏差e和偏差變化率ec的論域取為[-3,3]，根據(jù)經(jīng)驗設(shè)定輸出變量ΔKp的論域為[-0.3,0.3]，ΔKi的論域為[-0.06,0.06]，ΔKd的論域為[-3,3]。

Kp、Ki、Kd的整定原則為：

(1)當(dāng)偏差e較大時，Kp取較大值，Ki取較小值，增加系統(tǒng)的響應(yīng)速度；

(2)當(dāng)偏差e較小時，Kp取較小值，Ki取較大值，減小系統(tǒng)的穩(wěn)態(tài)誤差；

(3)當(dāng)偏差變化率ec較大時，Kd取較大值，實現(xiàn)減小ec值的目的[10]。

根據(jù)上述PID控制理論，制訂的模糊PID規(guī)則控制表如表1所示。

表1 Kp、Ki、Kd模糊控制規(guī)則表

使用Mamdani法進(jìn)行模糊推理，對模糊輸出量使用面積重心法進(jìn)行解模糊，就可以得到確切的輸出值，從而得到PID實時控制參數(shù)的實時補償公式：

(4)

式中：Kp0、Ki0、Kd0為控制器參數(shù)初始值，采用試湊法確定。

1.3 云臺掃描控制方法

在控制云臺運動時，有時需要云臺做定速掃描運動，可以由從控制器控制脈寬占比的改變來控制運動速率。分段降速曲線是一種常用的控制方法，它根據(jù)測量值s的改變，從而逐漸降低被控量vi的值，直到vi為零，如式(5)所示：

(5)

分段降速曲線子區(qū)間的端點處不光滑，為了提高控制曲線平滑性，使用三階Hermite插值對控制點(si,vi)擬合，三階Hermite插值算法由分段三次曲線連接而成，在連接點處具有一階連續(xù)相等的導(dǎo)數(shù)，從而可以確保在連接處的光滑性。

三階Hermite插值多項式H3(x)可以表達(dá)為[11]

(6)

云臺在做定速掃描運動時，為了控制云臺的運動速率，根據(jù)磁編碼器反饋數(shù)據(jù)，通過脈寬調(diào)制的方式將每一刻的控制量傳遞給主控制器，主控制器融合陀螺數(shù)據(jù)計算得到相應(yīng)的偏差量送入PID控制器。由于主控制器收到的脈寬調(diào)制信號在一定范圍內(nèi)是恒定的，從而可以控制云臺做定速運動。設(shè)計距離大于80°時速率為最大速度vx，在20°～60°時速率為0.4vx，可得在降速點處速度為f(x)·vx，f(x)的計算表達(dá)式為

(7)

2 系統(tǒng)總體架構(gòu)設(shè)計

2.1 系統(tǒng)總體框架

系統(tǒng)總體結(jié)構(gòu)框圖如圖2所示，整個系統(tǒng)包括傳感器敏感單元、微控制器處理單元以及電機傳動單元。微控制器處理單元為了減少高頻率中斷響應(yīng)、降低控制復(fù)雜度，采取雙控制器協(xié)同控制方法，其中C8051F380微控制器作為主控制器，負(fù)責(zé)采集慣導(dǎo)傳感器數(shù)據(jù)，經(jīng)過姿態(tài)解算后輸入帶輸出約束的模糊PID控制算法中，將計算結(jié)果轉(zhuǎn)換為控制信號，輸出到電機傳動單元中，控制電機濾除云臺擾動，穩(wěn)定云臺。ATmega328微控制器作為從控制器，分擔(dān)主控制器的角度傳感器數(shù)據(jù)采集、云臺運動角度控制、上位機指令接收及解釋等任務(wù)，在控制云臺作定速掃描運動時，使用三階Hermite插值的降速曲線控制云臺繞俯仰軸和航向軸的運動速率及運動方向。

圖2 穩(wěn)像云臺控制系統(tǒng)結(jié)構(gòu)框圖

2.2 系統(tǒng)控制流程

系統(tǒng)軟件包括主控制器軟件與從控制器軟件2部分，軟件流程圖如圖3所示。

(a)主控制器 (b)從控制器圖3 主控制器和從控制器軟件流程圖

主控制器在上電后進(jìn)行函數(shù)初始化及電機初始化，電機初始化位置以編碼器零位位置為基準(zhǔn)，因此在初始化階段，程序需驅(qū)動電機做掃描運動實現(xiàn)零位尋找功能。接著進(jìn)入循環(huán)，讀取陀螺數(shù)據(jù)解算出云臺角度，并接收從控制器的角度偏移量，兩者經(jīng)數(shù)據(jù)融合后共同作為偏差量輸入模糊PID控制器中，計算出電機控制量并產(chǎn)生PWM脈寬調(diào)制信號驅(qū)動電機動作。而從控制器上電后進(jìn)行系統(tǒng)初始化，接著進(jìn)入循環(huán)，等待上位機指令，在收到上位機指令后，觸發(fā)串口處理中斷，在需要調(diào)整角度時，通過三階Hermite插值降速曲線，計算角度偏移量并通過脈寬調(diào)制信號發(fā)送給主控制器，并不斷更新編碼器角度數(shù)據(jù)，實現(xiàn)云臺運動的精確控制。

3 系統(tǒng)硬件電路設(shè)計

3.1 系統(tǒng)硬件電路總體設(shè)計

控制器硬件電路由C8051F380微處理器及ATmega328微處理器，以及外圍電路組成，外圍電路包括傳感器采集電路、電源轉(zhuǎn)換電路、電機驅(qū)動電路、通訊電路、下載調(diào)試電路、晶振電路以及復(fù)位電路等，穩(wěn)像云臺控制系統(tǒng)硬件結(jié)構(gòu)如圖4所示。

圖4 穩(wěn)像云臺控制系統(tǒng)硬件結(jié)構(gòu)

3.2 主、從控制器

主控芯片選擇C8051F380微控制器，它具有高達(dá)48 MIPS流水線結(jié)構(gòu)的內(nèi)核，內(nèi)置一個可編程的高頻振蕩器，可作為系統(tǒng)時鐘源，最高頻率達(dá)48 MHz，晶振誤差小于0.25%，可以保證云臺運動的實時控制；同時，C8051F380微控制器具有高速SMBus接口用以慣導(dǎo)傳感器數(shù)據(jù)的采集，內(nèi)置16位可編程計數(shù)器陣列可產(chǎn)生多路脈寬調(diào)制信號，滿足航向軸及俯仰軸的三相無刷直流電機的驅(qū)動需求。

從控芯片選擇ATmega328微處理器，它在20 MHz工作頻率下指令執(zhí)行速度可達(dá)20 MIPS，可以保證傳感器信號采集的實時性；該處理器內(nèi)置一個USART接口，用于程序下載以及上位機通信，一個I2C接口實現(xiàn)兩軸角度傳感器數(shù)據(jù)采集，滿足設(shè)計需求。同時具有低功耗、小型化的優(yōu)點，適合作為輔助控制器。

3.3 傳感器采集電路

角度傳感器選用AS5048磁編碼器，相較于光電編碼器，它有著體積小、穩(wěn)定性高的優(yōu)點[12]。AS5048的測角范圍可以達(dá)到360°，具有14位的高分辨率輸出，最大系統(tǒng)精度達(dá)0.05°，同時因為磁體的零位不需要機械校準(zhǔn)，使用I2C命令即可完成零位編程。

將其分別安裝在俯仰電機、航向電機處，測量兩框架的角偏移量，電路如圖5所示，其中Q1、Q2為N型MOS管，R1～R4為4.7 kΩ，C1為0.1 μF。

圖5 磁編碼器數(shù)據(jù)采集電路

慣導(dǎo)傳感器選用MPU6050傳感器，它集成了三軸MEMS陀螺儀、三軸MEMS加速度計以及一個數(shù)字運動處理器DMP，它的優(yōu)點是體積小、成本低，而且相對于多個獨立的加速度計陀螺儀組合的多組件方案，有效解決了安裝陀螺儀和加速度計時軸間誤差的問題，且顯著減小傳感器占用的尺寸。

MPU6050的可測范圍最大達(dá)到±2 000(°)/s，分辨率可達(dá)0.06(°)/s，I2C通信速率可達(dá)到400 Kbps，將其放置在內(nèi)框架或載體上，采集云臺3個軸向的角速度及加速度數(shù)據(jù)。

電路如圖6所示，其中，Q1、Q2為N型MOS管，R1～R4為4.7 kΩ，R5、R7為120 Ω，R6為10 kΩ，C1為10 μF，C2、C3為0.1 μF。

圖6 MPU6050數(shù)據(jù)采集電路

3.4 穩(wěn)壓、通信及電機驅(qū)動電路

電源轉(zhuǎn)換電路選擇兩級降壓穩(wěn)壓電路，系統(tǒng)為12 V直流供電，通過LM2575穩(wěn)壓芯片與LM2014穩(wěn)壓芯片產(chǎn)生5 V及3.3 V電壓，為相應(yīng)的電路模塊供電。通訊電路從控制器部分采用MAX3490芯片將TTL電平信號轉(zhuǎn)換為差分信號，減少噪聲干擾，提高傳輸穩(wěn)定性；從控制器部分采用FT232RL芯片實現(xiàn)了USB通信，可使用上位機USB端口直接下載調(diào)試，簡化調(diào)試流程。電機驅(qū)動電路包括三相逆變?nèi)珮螂娐?、電流采樣電路和直流母線電壓采樣電路。其中單相的驅(qū)動電路如圖7所示，類似的，使用3個JY21L和6個N-MOS管，使OUT1～OUT3連接電機即可完成三相電機驅(qū)動。

圖7 三相無刷電機單相驅(qū)動電路

4 實驗結(jié)果分析

搭建由穩(wěn)像云臺、三軸定速擺臺構(gòu)成的實驗測試平臺，如圖8所示。以高精度慣性器件Ellipse2-N作為姿態(tài)參考器件，記錄擺臺在運動過程中的視軸偏差，統(tǒng)計峰峰值，并與轉(zhuǎn)臺運動角度峰值進(jìn)行除法運算，經(jīng)過多次測試，可以得到較準(zhǔn)確的隔離度指標(biāo)。

圖8 測試系統(tǒng)實物圖

將兩軸框架安裝于定速擺臺上，調(diào)節(jié)俯仰軸，使其向下偏轉(zhuǎn)5°，模擬無人機光電載荷真實情況。相繼控制擺臺的航向軸和俯仰軸在±5°范圍內(nèi)，以2 Hz的頻率進(jìn)行往復(fù)運動，將擾動過程中航向軸和俯仰軸的穩(wěn)定角度峰值記錄在表2中，航向軸與俯仰軸的擾動隔離度分別為0.55%和0.88%，兩軸的擾動隔離度均小于1%。

表2 航向軸與俯仰軸擾動隔離度

擾動過程中，采集不同時間云臺的航向軸與俯仰軸角度，如圖9所示。

(a)航向軸角度

(b)俯仰軸角度圖9 ±5°、2 Hz擾動時的角度穩(wěn)定曲線

搭載相機拍攝固定靶標(biāo)，開啟三軸擺臺的航向軸，鎖定其他兩軸，在穩(wěn)像控制系統(tǒng)開啟和關(guān)閉狀態(tài)下，以第一幀為基礎(chǔ)，分別抽取100、200、300幀圖像做差影圖，結(jié)果如圖10所示。

5 結(jié)束語

本文搭建了基于雙控制器的小型兩軸云臺穩(wěn)像系統(tǒng)，針對小型無人機飛行中振動對光電載荷視軸穩(wěn)定的影響及定速運動的需求，設(shè)計了四元數(shù)姿態(tài)解算、模糊PID控制與三階Hermite插值降速曲線的云臺控制方法。實驗結(jié)果表明，在擺臺±5°、2 Hz擾動下兩軸的穩(wěn)定隔離度小于1%。為小型無人機的光電載荷穩(wěn)定提供了一種可靠易行的方案。

(a)未開啟穩(wěn)像控制系統(tǒng)

(b)開啟穩(wěn)像控制系統(tǒng)圖10 差影圖