基于無劍SoC開源平臺(tái)的二自由度云臺(tái)控制系統(tǒng)

李偉博，李珈毅，張志明，翁錦煜，許春權(quán)

(同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 200092)

0 引言

多自由度云臺(tái)控制器需要能實(shí)時(shí)調(diào)整負(fù)載的運(yùn)動(dòng)姿態(tài)，其設(shè)計(jì)與開發(fā)是一個(gè)典型的機(jī)電一體化應(yīng)用問題，涉及到嵌入式系統(tǒng)、計(jì)算機(jī)控制、運(yùn)動(dòng)軌跡規(guī)劃和功率控制變換等多學(xué)科理論知識(shí)和綜合實(shí)踐技能，已在監(jiān)控、安防、測(cè)控、測(cè)繪、機(jī)器視覺等多種實(shí)際應(yīng)用場(chǎng)景下得到應(yīng)用[1-4]。以二自由度運(yùn)動(dòng)控制設(shè)計(jì)為例，其主要由核心微控制器、控制執(zhí)行單元、外圍輔助電路等組成，實(shí)現(xiàn)控制水平和俯仰的角度變化運(yùn)動(dòng)的主要功能，并保證其位置和速度的偏差始終在一定精度范圍之內(nèi)。目前云臺(tái)控制模塊大多采用較成熟的ARM架構(gòu)MCU作為核心處理器[4-6]，但近年來隨著開源指令集RISC-V受到重視[7-8]，針對(duì)不同應(yīng)用可靈活修改指令集和芯片架構(gòu)設(shè)計(jì)，使得其在對(duì)于成本較敏感的AIoT物聯(lián)網(wǎng)應(yīng)用方向更具優(yōu)勢(shì)。本文基于RISC-V架構(gòu)下的無劍SoC開源平臺(tái)，完成相關(guān)核心功能模塊的軟硬件設(shè)計(jì)和控制算法編程，經(jīng)過系統(tǒng)聯(lián)調(diào)實(shí)現(xiàn)二自由度云臺(tái)的精確運(yùn)動(dòng)控制。

1 無劍SoC開源平臺(tái)

低功耗微控制器采用無劍100開源SoC，基于開源的RISC-V CPU架構(gòu)，采用兩級(jí)極簡(jiǎn)流水線，適用于對(duì)功耗和成本敏感的IoT應(yīng)用，提供集芯片架構(gòu)、基礎(chǔ)軟件、算法與開發(fā)工具于一體的整體解決方案[9-10]。無劍通用設(shè)計(jì)MCU具有32個(gè)GIPO口、12個(gè)PWM輸入輸出口、3個(gè)可復(fù)用的串口/SPI/IIC等的外設(shè)資源，基于FPGA通過EDA工具進(jìn)行前端仿真和片上測(cè)試，足夠滿足項(xiàng)目需求。這使得本研究能夠快速開展運(yùn)動(dòng)控制算法與軟硬件驅(qū)動(dòng)程序等設(shè)計(jì)工作。

2 系統(tǒng)構(gòu)建與實(shí)現(xiàn)

二自由度云臺(tái)控制器由硬件系統(tǒng)和軟件系統(tǒng)組成。硬件架構(gòu)框圖如圖1所示，主要包括核心控制器、人機(jī)交互接口、云臺(tái)運(yùn)動(dòng)控制單元、電源模塊等。整個(gè)項(xiàng)目基于無劍100開源平臺(tái)[9-10]，在此基礎(chǔ)上進(jìn)行Verilog和嵌入式C語言編程實(shí)現(xiàn)軟件系統(tǒng)功能。采用57/42步進(jìn)電機(jī)控制水平和俯仰2個(gè)方向的轉(zhuǎn)動(dòng)運(yùn)動(dòng)；利用按鍵和OLED模塊進(jìn)行人機(jī)交互操作；選擇藍(lán)牙模塊用于遠(yuǎn)程無線通信；IMU模塊用于角度自校準(zhǔn)。構(gòu)建與實(shí)現(xiàn)集成一體化的云臺(tái)控制系統(tǒng)，在系統(tǒng)姿態(tài)發(fā)生偏移時(shí)能夠及時(shí)檢測(cè)并控制，實(shí)現(xiàn)精確快速監(jiān)視或者移動(dòng)場(chǎng)景下的增穩(wěn)控制。

圖1 硬件架構(gòu)系統(tǒng)框圖

2.1 基于無劍SoC開源平臺(tái)的核心控制器設(shè)計(jì)

云臺(tái)控制系統(tǒng)中的核心控制器通過EDA工具進(jìn)行前端仿真通過后在FPGA上進(jìn)行測(cè)試。首先在Xilinx Vivado 2018.3 HLX中編譯官方原版的SoC硬件Verilog描述源碼，綜合生成比特流(bitstream)文件，再下載到XC7A200T系列FPGA中，生成RISC-V架構(gòu)的MCU[11-12]；之后使用配套的SDK以及圖形化開發(fā)套件C-Sky Develop Kit(CDK)進(jìn)行基于此MCU內(nèi)核的開發(fā)[9-12]，提供UART、SPI、I2C、Timer、PWM等多種IP以及驅(qū)動(dòng)；在此基礎(chǔ)上完成包括云臺(tái)控制算法和驅(qū)動(dòng)程序在內(nèi)的核心模塊功能開發(fā)。

Xilinx XC7A200T3B開發(fā)板的資源消耗實(shí)際測(cè)試結(jié)果如表1所示，總體資源使用率少于25%。與此同時(shí)，動(dòng)態(tài)與靜態(tài)功率消耗估計(jì)分別在0.03 W和0.14 W左右，總功率低于0.2 W。由此可見，無劍100作為一款低功耗SoC，展示出優(yōu)秀的功率特性以及低資源消耗的特性。

表1 無劍100的資源消耗統(tǒng)計(jì)

項(xiàng)目選擇主動(dòng)串行(AS)配置模式，比特流文件燒入板載的Flash中，F(xiàn)PGA在每次上電后作為主控制器，從FLASH中讀取數(shù)據(jù)，進(jìn)行配置；初始化完成后，進(jìn)入用戶模式，開始正常工作。

2.2 功能化外設(shè)模塊設(shè)計(jì)

功能化外設(shè)模塊包括直流電源模塊、驅(qū)動(dòng)模塊、人機(jī)交互模塊和角度采集模塊，分別用來為核心控制器和電機(jī)驅(qū)動(dòng)供電、控制云臺(tái)水平豎直2個(gè)方向的運(yùn)動(dòng)、驅(qū)動(dòng)藍(lán)牙等外設(shè)進(jìn)行人機(jī)交互和實(shí)時(shí)進(jìn)行角度反饋校準(zhǔn)，提高系統(tǒng)的抗干擾性。

2.2.1 電源供應(yīng)

電源模塊分為核心電路供電和驅(qū)動(dòng)電路供電兩部分。無劍FPGA核心板和外設(shè)模塊供電需要直流+5 V/+3.3 V電源；云臺(tái)驅(qū)動(dòng)電路中，步進(jìn)電機(jī)驅(qū)動(dòng)器和步進(jìn)電機(jī)的供電需要直流+12 V電源。步進(jìn)驅(qū)動(dòng)器和電機(jī)在實(shí)際運(yùn)行時(shí)，最大功率工作狀態(tài)下，檢測(cè)其總電流在2 A以內(nèi)；直流12 V電源經(jīng)LM1085后級(jí)線性降壓穩(wěn)壓電路得到+5 V/+3.3 V供電，其中+5 V最大輸出電流可達(dá)3 A，滿足無劍開發(fā)板工作電流需求，+3.3 V電源最大輸出電流1 A，滿足外設(shè)模塊的工作電流要求。室內(nèi)實(shí)驗(yàn)時(shí)輸入電源共用1個(gè)AC220V轉(zhuǎn)DC+12 V/8 A的電源適配器，室外實(shí)驗(yàn)或者沒有交流供電插座情況下由22 400 mA·h/12.6 V的串聯(lián)鋰電池組供電。

2.2.2 云臺(tái)驅(qū)動(dòng)

云臺(tái)開環(huán)驅(qū)動(dòng)控制框圖如圖2所示，硬件設(shè)計(jì)選擇一臺(tái)57步進(jìn)電機(jī)和一臺(tái)42步進(jìn)電機(jī)分別提供水平和俯仰方向上的360°的自由旋轉(zhuǎn)運(yùn)動(dòng)，由無劍MCU提供驅(qū)動(dòng)控制信號(hào)。無劍100的I/O口連接到DM542步進(jìn)驅(qū)動(dòng)器，分別實(shí)現(xiàn)2個(gè)旋轉(zhuǎn)方向的自由度運(yùn)動(dòng)姿態(tài)控制，具有易開發(fā)、精度高、延遲低、低速帶載能力強(qiáng)等優(yōu)點(diǎn)。XC7A200T3B開發(fā)板與步進(jìn)電機(jī)驅(qū)動(dòng)器的連接方式為共陽極連接，使用6 400細(xì)分，即步距角為360°/6 400=0.056 25°，再加上防丟步控制算法，空載時(shí)的電機(jī)角度控制精度在水平和垂直方向?qū)崪y(cè)最好效果可至0.1°以內(nèi)。

圖2 云臺(tái)步進(jìn)電機(jī)開環(huán)控制

2.2.3 外設(shè)模塊

外設(shè)模塊包括功能按鍵、用于顯示控制器內(nèi)部運(yùn)行狀態(tài)的OLED顯示模塊以及用于無線通訊的藍(lán)牙模塊。按鍵使用GPIO進(jìn)行讀取；OLED使用SPI協(xié)議進(jìn)行指令控制和數(shù)據(jù)讀寫；藍(lán)牙模塊通過串口轉(zhuǎn)換后進(jìn)行透?jìng)髂Ｊ酵ㄐ?，?shí)現(xiàn)數(shù)據(jù)的無線發(fā)送和接收。無劍100的UART0/UART1通過中斷觸發(fā)接收方式從FIFO中進(jìn)行讀取數(shù)據(jù)，分別獲取藍(lán)牙通信數(shù)據(jù)和IMU姿態(tài)數(shù)據(jù)。IMU九軸傳感器[13]以10 Hz的速率發(fā)送包含校驗(yàn)位和歐拉角數(shù)據(jù)的數(shù)據(jù)幀。在云臺(tái)鎖定模式下，無劍100實(shí)時(shí)檢測(cè)角度的變化，計(jì)算歐拉角姿態(tài)并做出相應(yīng)的反應(yīng)，產(chǎn)生對(duì)應(yīng)的PWM脈沖信號(hào)，向電機(jī)發(fā)送動(dòng)作指令，以完成經(jīng)典PID負(fù)反饋控制[14]，實(shí)現(xiàn)角度偏差校正。

2.3 SoC平臺(tái)下的控制器軟件設(shè)計(jì)

SoC平臺(tái)下的控制器軟件設(shè)計(jì)采用Verilog和嵌入式C語言編程實(shí)現(xiàn)，完成云臺(tái)控制算法和驅(qū)動(dòng)程序在內(nèi)的核心模塊功能開發(fā)，以及按鍵、IMU、藍(lán)牙和OLED等主要外設(shè)的數(shù)據(jù)傳輸和處理程序。

2.3.1 系統(tǒng)程序運(yùn)行流程

系統(tǒng)程序運(yùn)行流程如圖3所示。首先連接電源，按下RE_PROG鍵后，F(xiàn)PGA主動(dòng)讀取快閃存儲(chǔ)器(FLASH)中的數(shù)據(jù)進(jìn)行配置，等待LED D3亮起即為配置成功，再連接USB線通過電腦端的CDK軟件環(huán)境進(jìn)行調(diào)試。主函數(shù)初始化通用輸入/輸出接口(GPIO)和異步串行傳輸(UART)后，設(shè)置串口UART0和UART1通過中斷觸發(fā)讀取先入先出(FIFO)的方式，按鍵通過外部中斷觸發(fā)讀取，OLED通過GPIO模擬串行外設(shè)接口(SPI)協(xié)議進(jìn)行控制顯示。串口UART0接收到遠(yuǎn)程藍(lán)牙指令后，首先解析指令翻譯成電機(jī)片選序號(hào)及其旋轉(zhuǎn)方向和脈沖數(shù)量，之后在主循環(huán)中進(jìn)行電機(jī)控制，使用GPIO產(chǎn)生固定數(shù)量的脈沖寬度調(diào)制信號(hào)(PWM)和方向控制信號(hào)輸入到步進(jìn)驅(qū)動(dòng)器中，達(dá)到運(yùn)動(dòng)控制目的。OLED用于顯示電機(jī)當(dāng)前的位置參數(shù)，提示用戶驗(yàn)證指令解析是否正確和記錄顯示云臺(tái)姿態(tài)參數(shù)。在解析并執(zhí)行完用戶指令后電機(jī)保持不動(dòng)，串口UART1以100 Hz的頻率接收來自IMU的歐拉角姿態(tài)數(shù)據(jù)，并計(jì)算判斷出當(dāng)前電機(jī)是否有位姿的偏移誤差，同時(shí)進(jìn)行實(shí)時(shí)調(diào)整，保持云臺(tái)穩(wěn)定。

圖3 系統(tǒng)程序流程圖

2.3.2 電機(jī)控制和狀態(tài)自校準(zhǔn)鎖定

驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)的實(shí)現(xiàn)過程需要適應(yīng)硬件組成，軟件控制上使用S曲線算法[15-16]防止步進(jìn)電機(jī)運(yùn)動(dòng)過程中出現(xiàn)丟步，增大誤差，同時(shí)在進(jìn)入角度鎖定狀態(tài)后實(shí)現(xiàn)自校準(zhǔn)反饋控制算法。

步進(jìn)電機(jī)的驅(qū)動(dòng)信號(hào)可以分為脈沖信號(hào)和方向控制信號(hào)。選用GPIO口電平反轉(zhuǎn)和適當(dāng)延時(shí)產(chǎn)生驅(qū)動(dòng)步進(jìn)電機(jī)需要的PWM脈沖信號(hào)波形，其優(yōu)點(diǎn)在于脈沖數(shù)容易控制、頻率與占空比可及時(shí)調(diào)節(jié)。無劍100 SoC主頻可達(dá)20 MHz，GPIO的電平變換可以依賴高速總線實(shí)現(xiàn)快速反轉(zhuǎn)的功能以滿足步進(jìn)驅(qū)動(dòng)所需頻率，實(shí)測(cè)最高可達(dá)50 kHz。方向控制信號(hào)則使用單個(gè)GPIO口引腳產(chǎn)生，輸出高電平可使得電機(jī)向預(yù)設(shè)正方向旋轉(zhuǎn)，電平反轉(zhuǎn)為低電平后即可反方向旋轉(zhuǎn)。

步進(jìn)電機(jī)按步長(zhǎng)方式進(jìn)行運(yùn)動(dòng)，每一步對(duì)應(yīng)一個(gè)脈沖。根據(jù)實(shí)際應(yīng)用調(diào)節(jié)頻率即可滿足不同運(yùn)行速度的需要，本設(shè)計(jì)中細(xì)分?jǐn)?shù)取6 400，理論精度為0.056 25°，即輸入一個(gè)脈沖轉(zhuǎn)動(dòng)的角度。輸入的PWM頻率在恒速階段為f=20 kHz，步進(jìn)電機(jī)運(yùn)行速度約為3.125 rps。

步進(jìn)電機(jī)把脈沖信號(hào)轉(zhuǎn)換為角位移，具有控制精度高、控制簡(jiǎn)單等特點(diǎn)，即使是開環(huán)控制也能獲得較高的控制精度。但由于固有機(jī)電結(jié)構(gòu)原因，步進(jìn)電機(jī)啟動(dòng)時(shí)需要的加速電流較大，線性度變差，會(huì)出現(xiàn)啟動(dòng)慢、丟步的現(xiàn)象；同理，在停止時(shí)也會(huì)出現(xiàn)過沖的現(xiàn)象，從而影響到步進(jìn)電機(jī)的控制精度，所以需要對(duì)步進(jìn)電機(jī)啟動(dòng)和停止時(shí)的加速度和速度作合理的規(guī)劃[15-16]。綜合考慮后選擇使用S曲線加減速控制算法，在從啟動(dòng)到停止整個(gè)運(yùn)動(dòng)過程中電機(jī)的加速度可分為5段，在整個(gè)運(yùn)動(dòng)控制周期內(nèi)獲得較平穩(wěn)且收斂較快的電機(jī)加速度和速度變化，并基于實(shí)際使用環(huán)境對(duì)S曲線的基本公式y(tǒng)=1/(1+e-x)進(jìn)行合理的改進(jìn)，得到整個(gè)加速、勻速和減速階段的頻率、周期、速度、加速度的計(jì)算公式。其中在加速階段和減速階段分別使用頻率計(jì)算式(1)和式(2)：

(1)

(2)

式中：fmin=50 Hz；fmax=20 000 Hz；i為不同時(shí)段的頻率節(jié)點(diǎn)的序號(hào)，每一階段i均從0開始，加速和減速階段i的范圍為0～1 000。

勻速階段的頻率f為定值20 000 Hz，頻率節(jié)點(diǎn)序號(hào)i為0～10。3個(gè)階段中，每個(gè)節(jié)點(diǎn)所對(duì)應(yīng)的周期和速度的計(jì)算式分別見式(3)和式(4)：

ti=1/fi

(3)

式中：fi為頻率，Hz；ti為時(shí)間，s。

vi=fi×360/6400

(4)

式中vi為速度，(°)/s。

加減速階段的加速度a的差分計(jì)算式如式(5)所示：

ai=(vi-vi-1)/ti

(5)

式中ai為加速度，(°)/s2。

勻速階段的加速度a的數(shù)值等于0。

3 實(shí)驗(yàn)測(cè)試與結(jié)果分析

搭建二自由度云臺(tái)實(shí)驗(yàn)平臺(tái)，設(shè)計(jì)的原型系統(tǒng)實(shí)物照片如圖4所示，系統(tǒng)硬件結(jié)構(gòu)包括無劍100開源RISCV開發(fā)板FMX7AR3B、供電模塊、雙軸步進(jìn)電機(jī)及其驅(qū)動(dòng)器、九軸慣性測(cè)量單元(IMU)、OLED顯示屏、藍(lán)牙單元通信模塊等。系統(tǒng)使用DC+12 V輸入，可直接使用電池供電。整個(gè)系統(tǒng)實(shí)測(cè)空載運(yùn)行電流不超過1 A，總體功率不高，適合移動(dòng)使用，可以應(yīng)用到不同負(fù)載的應(yīng)用場(chǎng)景，如負(fù)載較小的監(jiān)控安防云臺(tái)、視覺定位的底層驅(qū)動(dòng)系統(tǒng)、負(fù)載較大的機(jī)器人頭部傳感器模塊等，還能達(dá)到較高的精確度和響應(yīng)速度。

圖4 原型系統(tǒng)實(shí)物照片

3.1 基于步進(jìn)電機(jī)的多自由度運(yùn)動(dòng)控制

云臺(tái)采用步進(jìn)電機(jī)控制各自由度運(yùn)動(dòng)，適用于負(fù)載需求變化較大的場(chǎng)合。此外，步進(jìn)電機(jī)在步進(jìn)驅(qū)動(dòng)器的配合下可以達(dá)到較高精確度，且其每一執(zhí)行步的誤差不會(huì)積累；啟動(dòng)和停止響應(yīng)快，不需要太復(fù)雜的控制算法。S曲線算法控制下的水平軸運(yùn)動(dòng)工況-時(shí)間曲線如圖5所示，其中圖5(a)給出角位移變化效果圖，圖5(b)給出運(yùn)動(dòng)速度變化曲線圖，圖5(c)給出運(yùn)動(dòng)加速度變化曲線圖。

(a)水平軸角位移變化曲線圖

示例中，水平方向步進(jìn)電機(jī)從起始位置逆時(shí)針旋轉(zhuǎn)0.31·360°，加減速階段角位移和速度隨時(shí)間變化的S理論仿真曲線(虛線表示)和實(shí)際測(cè)試效果曲線(實(shí)線表示)如圖5所示，角位移、速度和加速度的實(shí)測(cè)數(shù)據(jù)曲線和理論曲線相契合，加減速階段的對(duì)稱性也可反映出步進(jìn)電機(jī)累積誤差很小，具有較好的控制精度。

3.2 自適應(yīng)反饋控制鎖定

在用戶發(fā)送完動(dòng)作指令后，云臺(tái)系統(tǒng)接收指令并進(jìn)行轉(zhuǎn)動(dòng)到目標(biāo)位置，之后進(jìn)入鎖定狀態(tài)。在未接收到用戶移動(dòng)指令時(shí)，如果系統(tǒng)外界環(huán)境發(fā)生三維空間內(nèi)位置上的偏移，導(dǎo)致整個(gè)系統(tǒng)的基準(zhǔn)面發(fā)生改變，云臺(tái)的角度也會(huì)隨之改變，從而無法達(dá)到目標(biāo)位置。采用IMU獲取系統(tǒng)基準(zhǔn)面的三維空間位置信息，據(jù)此判斷外界環(huán)境是否發(fā)生改變，當(dāng)檢測(cè)到變化后，啟動(dòng)經(jīng)典PID負(fù)反饋控制[14]算法，保持云臺(tái)的角度維持在用戶所設(shè)定的目標(biāo)角度，達(dá)到自適應(yīng)反饋控制鎖定功能。

3.3 系統(tǒng)資源的極小化運(yùn)用

使用盡量少的接口、簡(jiǎn)潔高效的程序?qū)崿F(xiàn)最核心實(shí)用的功能，在整個(gè)設(shè)計(jì)中僅使用到與步進(jìn)電機(jī)控制相關(guān)的3個(gè)通用輸入/輸出接口以及2個(gè)定時(shí)器中斷與1個(gè)異步串行傳輸通訊接口。從而可以極大地節(jié)省FPGA的資源，為更多外設(shè)應(yīng)用分配出充足的開發(fā)空間，在后續(xù)處理過程中加入圖像采集和視覺處理功能等，進(jìn)一步優(yōu)化控制算法。

4 結(jié)束語

使用開源無劍100 SoC作為主控核心，系統(tǒng)快速靈活編程，協(xié)同控制雙軸運(yùn)動(dòng)。原型系統(tǒng)實(shí)際場(chǎng)景測(cè)試結(jié)果表明，可精簡(jiǎn)系統(tǒng)設(shè)計(jì)，實(shí)現(xiàn)較高精度和較快響應(yīng)速度，以及基于IMU姿態(tài)的閉環(huán)反饋控制等。項(xiàng)目實(shí)踐可進(jìn)一步深入了解RISC-V架構(gòu)，快速集成、快速驗(yàn)證，減少基礎(chǔ)模塊開發(fā)成本，實(shí)現(xiàn)精確快速監(jiān)視、物體捕捉或者用于移動(dòng)場(chǎng)景下的增穩(wěn)控制，適合在AIoT環(huán)境下的實(shí)際應(yīng)用。