陳彤陽，王向軍

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

0 引言

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

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

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

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

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

(1)

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

(2)

在姿態(tài)角計(jì)算過程中，需要實(shí)時(shí)更新四元數(shù)矩陣，結(jié)合云臺穩(wěn)像系統(tǒng)工作時(shí)的應(yīng)用場景，需要姿態(tài)解算方法兼顧實(shí)時(shí)性與高精度的要求。本文首先通過慣性傳感器經(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)為實(shí)際輸出角度，e為角度偏差值，ec為偏差變化率。每次調(diào)節(jié)前，將不同的e和ec輸入到模糊推理器中，根據(jù)變化實(shí)時(shí)調(diào)整PID 3個(gè)參數(shù)的補(bǔ)償量ΔKp、ΔKi、ΔKd，從而滿足不同的偏差e及偏差變化率ec對PID控制器參數(shù)的需求，使被控對象有更好的動(dòng)態(tài)性能[9]。

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

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

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

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

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

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

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

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

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

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

(4)

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

1.3 云臺掃描控制方法

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

(5)

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

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

(6)

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

(7)

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

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

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

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

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

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

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

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

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

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

控制器硬件電路由C8051F380微處理器及ATmega328微處理器，以及外圍電路組成，外圍電路包括傳感器采集電路、電源轉(zhuǎn)換電路、電機(jī)驅(qū)動(dòng)電路、通訊電路、下載調(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)置一個(gè)可編程的高頻振蕩器，可作為系統(tǒng)時(shí)鐘源，最高頻率達(dá)48 MHz，晶振誤差小于0.25%，可以保證云臺運(yùn)動(dòng)的實(shí)時(shí)控制；同時(shí)，C8051F380微控制器具有高速SMBus接口用以慣導(dǎo)傳感器數(shù)據(jù)的采集，內(nèi)置16位可編程計(jì)數(shù)器陣列可產(chǎn)生多路脈寬調(diào)制信號，滿足航向軸及俯仰軸的三相無刷直流電機(jī)的驅(qū)動(dòng)需求。

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

3.3 傳感器采集電路

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

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

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

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

MPU6050的可測范圍最大達(dá)到±2 000(°)/s，分辨率可達(dá)0.06(°)/s，I2C通信速率可達(dá)到400 Kbps，將其放置在內(nèi)框架或載體上，采集云臺3個(gè)軸向的角速度及加速度數(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)壓、通信及電機(jī)驅(qū)動(dòng)電路

電源轉(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芯片實(shí)現(xiàn)了USB通信，可使用上位機(jī)USB端口直接下載調(diào)試，簡化調(diào)試流程。電機(jī)驅(qū)動(dòng)電路包括三相逆變?nèi)珮螂娐?、電流采樣電路和直流母線電壓采樣電路。其中單相的驅(qū)動(dòng)電路如圖7所示，類似的，使用3個(gè)JY21L和6個(gè)N-MOS管，使OUT1～OUT3連接電機(jī)即可完成三相電機(jī)驅(qū)動(dòng)。

圖7 三相無刷電機(jī)單相驅(qū)動(dòng)電路

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

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

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

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

表2 航向軸與俯仰軸擾動(dòng)隔離度

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

(a)航向軸角度

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

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

5 結(jié)束語

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

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

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