基于STM32 兩軸深水觀測云臺控制系統(tǒng)設(shè)計

任福深 ，王 茜 ，劉 均 ，張 園

（1.東北石油大學(xué) 機械科學(xué)與工程學(xué)院，黑龍江 大慶 163318；2.陜西安然能源科技有限公司，陜西 西安 710000）

0 引言

智能視頻監(jiān)控技術(shù)結(jié)合圖像、視頻處理等技術(shù)，是實現(xiàn)運動目標(biāo)自動檢測和跟蹤的關(guān)鍵技術(shù)[1-2]。水下機器人是海洋能源開采的重要裝備，常代替人工在危險環(huán)境和人類視覺受阻的環(huán)境工作，需要借助云臺實時監(jiān)測水下環(huán)境、保證其安全高效的作業(yè)。

國內(nèi)外云臺多為單自由度或固定式，且常見的云臺微控制器多為單核或雙核，數(shù)據(jù)傳輸速度和存儲量不高[3]。2014 年，中國海洋石油總公司設(shè)計了一款移動式云臺，只適用于淺海水下場景的實時監(jiān)控[4]。2016 年，殷莉甜針對水下云臺結(jié)構(gòu)的密封性和圖像檢測要求，設(shè)計了一款應(yīng)用于水下焊接機器人的雙目視覺系統(tǒng)，但其檢測精度不高[5-6]?；谏鲜鱿嚓P(guān)研究，本文設(shè)計了基于STM32 兩軸深水觀測云臺結(jié)構(gòu)及其控制系統(tǒng)，不但能實現(xiàn)回轉(zhuǎn)和俯仰兩自由度的運動，而且降低了水下機器人系統(tǒng)的開發(fā)成本，是水下機器人智能化研究的關(guān)鍵技術(shù)。

1 兩自由度云臺結(jié)構(gòu)方案設(shè)計

圖1 所示為兩自由度深水觀測云臺的整體結(jié)構(gòu)圖，其主要由云臺主體、數(shù)字?jǐn)z像頭、模擬攝像頭以及深水LED 燈組成。由于水下機器人內(nèi)部空間狹小，對云臺結(jié)構(gòu)進(jìn)行了小型化設(shè)計，并采用連接板對攝像頭和燈進(jìn)行固定。

圖1 兩自由度云臺整體結(jié)構(gòu)圖

云臺電子倉和攝像機與燈分別采用水密電纜進(jìn)行電氣連接。云臺結(jié)構(gòu)采用兩級O 形圈進(jìn)行密封，整體密封性好，可以承受1.5 MPa的深水壓力。如圖2 所示，采用步進(jìn)電機和角度傳感器控制云臺回轉(zhuǎn)和俯仰兩個自由度的運動，通過搭載數(shù)字?jǐn)z像頭、模擬攝像頭和兩個深水LED 燈，水下機器人可以對不同環(huán)境的深水工況進(jìn)行實時監(jiān)控和視頻圖像采集。

圖2 兩自由度云臺電機布置

2 兩自由度云臺系統(tǒng)總體設(shè)計

2.1 云臺控制系統(tǒng)總體方案設(shè)計

兩自由度深水觀測云臺控制系統(tǒng)組成關(guān)系如表1所示，包括云臺機械結(jié)構(gòu)、通信和視頻采集、上位機控制等6 個子系統(tǒng)。深水觀測云臺通過連接板和固定板，實現(xiàn)了水下高清攝像機和深水LED 燈隨步進(jìn)電機在俯仰和回轉(zhuǎn)兩個方向的同步運動。通信系統(tǒng)實現(xiàn)云臺上位機和下位機的數(shù)據(jù)交換。上位機控制系統(tǒng)是操作員實時獲取水下工況和機器人云臺運動參數(shù)，并即時調(diào)控的平臺。調(diào)光系統(tǒng)通過PWM 脈寬調(diào)制、調(diào)節(jié)深水LED 燈的亮度，以適應(yīng)缺少光源的深水工況，改善拍攝環(huán)境，為采集水下高清圖像和視頻提供保障。

表1 兩自由云臺控制系統(tǒng)組成

2.2 云臺控制系統(tǒng)硬件設(shè)計

2.2.1 云臺測控系統(tǒng)組成

云臺控制系統(tǒng)硬件主要由云臺通信主控板和云臺攝像頭與LED 燈的控制板兩部分構(gòu)成。采用RS485 主從通信方式，對控制電機轉(zhuǎn)向的控制命令以及大量數(shù)據(jù)進(jìn)行解碼、存儲和調(diào)用。主控板控制系統(tǒng)主要由MCU、A4988 電機驅(qū)動模塊、IO 接線端子、RS485 通信接口與電源電路等部分組成，如圖3 所示。

圖3 云臺測控系統(tǒng)組成圖

云臺通信主控板采用STM32F103C8T6 芯片，它是STM32系列中具有廣泛應(yīng)用的常見型號，使用了高性能的ARM Cortex-M3內(nèi)核[7-8]。

2.2.2 云臺電機控制原理

采用28 行星減速、兩相混合式步進(jìn)電機使二自由度云臺能夠?qū)崿F(xiàn)回轉(zhuǎn)180°、俯仰±50°大范圍水下環(huán)境的監(jiān)控。電機的供電電壓為24 V，扭矩為0.18 Nm。電機控制模塊電路如圖4 所示。

圖4 電機控制模塊電路圖

設(shè)定電機轉(zhuǎn)一圈，需要400 個脈沖。電機轉(zhuǎn)速和脈沖頻率成正比。通過控制1 s 內(nèi)發(fā)送的脈沖個數(shù)和脈沖頻率，調(diào)節(jié)電機轉(zhuǎn)速。電機驅(qū)動芯片選用A4988，其模塊電路如圖5 所示。

圖5 A4988 模塊電路圖

步進(jìn)電機是將電脈沖信號轉(zhuǎn)變?yōu)榻俏灰苹蚓€位移的開環(huán)控制元件。步進(jìn)電機的脈沖數(shù)和脈沖頻率分別決定了其轉(zhuǎn)動的速度和總角度位移，所有形式的運動方式改變均可以在很少的脈沖數(shù)量下完成。云臺電機控制如圖6 所示，通過改變相應(yīng)IO 口電平即可控制電機的轉(zhuǎn)向。使用角度傳感器記錄電機轉(zhuǎn)的圈數(shù)，計數(shù)隨角度傳感器的轉(zhuǎn)向而增加或減少。隨著電機軸的旋轉(zhuǎn)給出一個精準(zhǔn)的角度信號。電位器測一圈，確定云臺的當(dāng)前位置。

圖6 云臺電機控制圖

2.2.3 云臺通信方案設(shè)計

水下裝置對通信系統(tǒng)的可靠性、實時性以及穩(wěn)定性具有很高的要求[9]。云臺通信方案如圖7 所示，采用電力載波（電纜）的方式實現(xiàn)上位機控制命令和下位機的控制模塊進(jìn)行通信。通過RS485 通信電路控制云臺、攝像頭和燈等外部設(shè)備。下位機接收到上位機指令后，獲取并實時調(diào)控云臺當(dāng)前姿態(tài)位置和轉(zhuǎn)動速度。

圖7 云臺通信方案示意圖

2.2.4 云臺攝像頭和燈的控制原理

兩自由度深水觀測云臺搭載的水下攝像頭與深水LED 燈組成了水下機器人視覺系統(tǒng)，深水LED 燈輔助攝像頭拍攝、對深海光線不足的環(huán)境進(jìn)行補光，是水下機器人進(jìn)行深海環(huán)境圖像觀測與采集的重要依據(jù)。云臺選用兩個高清模擬攝像頭和數(shù)字?jǐn)z像頭。模擬攝像頭固定安裝在機械臂正下方，是導(dǎo)航和發(fā)現(xiàn)目標(biāo)的依據(jù)。

攝像頭和燈的控制原理如圖8 所示。鏡頭通過電荷耦合器件（CCD）將深水圖像傳遞給數(shù)據(jù)處理器（DSP），之后再發(fā)送給攝像頭和燈的主控制器（MCU），通過以太網(wǎng)傳遞到地面上位機，上位機的視頻采集卡對傳輸過來的視頻進(jìn)行儲存。數(shù)字?jǐn)z像頭采集畫面清晰度高；模擬攝像頭通過兩根視頻線將模擬信號直接傳輸?shù)降孛婺M顯示器上，數(shù)據(jù)傳輸速度快，便于對深水工況進(jìn)行實時監(jiān)控，獲取高質(zhì)量水下圖像。

圖8 云臺攝像頭和燈的控制原理圖

2.3 云臺位置PID 控制算法

云臺的運動特性[10]對拍攝圖像的清晰度有關(guān)鍵影響，在選取云臺的控制方式之前，需要對云臺的位置，回轉(zhuǎn)速度v1、俯仰速度v2以及云臺俯仰方向的最大傾角αmax等參數(shù)進(jìn)行標(biāo)定。

PID 控制[11]是一種線性控制方法，其控制原理框圖如圖9 所示。

圖9 PID 控制原理圖

PID 控制結(jié)合了比例控制、積分控制和微分控制，其表達(dá)式如式（1）所示：

其基本控制思路為：當(dāng)設(shè)定位置和目標(biāo)實際位置差值較大時，采用合適的比例系數(shù)，使云臺快速移動到指定位置，對目標(biāo)進(jìn)行跟蹤和圖像視頻采集，以達(dá)到良好的監(jiān)控效果。

位置式PID 算法和增量式PID 算法應(yīng)用廣泛，適用于不同的控制場景[12]。位置式PID 算法，計算時對e（t）進(jìn)行累加。本文選用增量式PID 控制每次輸出較上次輸出的增量，不會出現(xiàn)誤差累積，其動態(tài)方程如式（2）和式（3）所示：

式中，e（k）為本次偏差，e（k-1）為上一次的偏差，e（k-2）為上上次的偏差；KP為比例系數(shù)；TI為積分時間常數(shù)；TD為微分時間常數(shù)。

操作員通過地面上位機發(fā)送運動控制指令，設(shè)置云臺期望的運動目標(biāo)位置。根據(jù)當(dāng)前位置和期望位置的誤差，由運動控制中斷程序進(jìn)行步進(jìn)電機運動控制脈沖的計算和產(chǎn)生，采用角度傳感器對云臺轉(zhuǎn)動的角位移進(jìn)行計數(shù)，并轉(zhuǎn)換為閉環(huán)坐標(biāo)值，保存在寄存器中。在運動控制中斷程序內(nèi)調(diào)用增量PID 控制算法模塊，通過改變定時器中速度系數(shù)，從而控制發(fā)步進(jìn)電機運動控制脈沖的快慢。當(dāng)目標(biāo)位置和實際位置相距較遠(yuǎn)時電機快速運動，相距較近時運動慢一些。云臺增量式PID 控制流程圖如10 所示。

圖10 云臺PID 控制流程圖

3 云臺控制系統(tǒng)軟件設(shè)計

在Windows 操作系統(tǒng)上使用Visual Studio 軟件，選用面向?qū)ο蟮腃# 語言對上位機控制界面進(jìn)行開發(fā)[13]。云臺上位機控制界面主要有串口通信、云臺姿態(tài)控制、視頻監(jiān)控、深水LED 燈調(diào)光等功能。云臺下位機調(diào)試軟件Keil5的主要作用是：對控制器的通信端口進(jìn)行初始化、配置定時器，對控制命令和數(shù)據(jù)進(jìn)行解析、數(shù)模轉(zhuǎn)換，使用Modbus 實現(xiàn)數(shù)據(jù)的雙向傳輸，實現(xiàn)各個子系統(tǒng)功能。

3.1 控制系統(tǒng)和DMA 數(shù)據(jù)采集

FreeRTOS 是源碼公開、可在ARM 等多款處理器上移植的實時操作系統(tǒng)[14]。本文的云臺控制系統(tǒng)選用STM32F103 控制芯片。在FreeRTOS 操作系統(tǒng)上設(shè)置不同優(yōu)先級的多任務(wù)，優(yōu)先級高的任務(wù)先運行。在實現(xiàn)高效利用CPU 資源的同時，保證了系統(tǒng)的實時性和可靠性[15]。

DMA（Direct Memory Access）技術(shù)用來提供在外設(shè)和存儲器之間、存儲器與存儲器之間的高速數(shù)據(jù)傳輸。在這種方式下，不需要用到CPU，可以直接讀寫系統(tǒng)存儲器[16]。

云臺控制系統(tǒng)對云臺實際回轉(zhuǎn)和俯仰電機位置進(jìn)行采集，根據(jù)設(shè)定值和實際值之差，執(zhí)行運動控制中斷程序內(nèi)調(diào)用的增量PID 控制算法，控制發(fā)送步進(jìn)電機運動控制脈沖的快慢。通過步進(jìn)電機控制程序，輸出修正后的步進(jìn)電機驅(qū)動信號，調(diào)整云臺位置，實現(xiàn)云臺位置的精確調(diào)控。云臺與上位機采用RS485 通信、DMA 中斷進(jìn)行數(shù)據(jù)采集，收到的數(shù)據(jù)存入緩沖區(qū)，主程序按照標(biāo)志位處理。根據(jù)標(biāo)志位控制電機位置主線程如圖11 所示，定時器中斷流程圖如圖12 所示。

圖11 根據(jù)標(biāo)志位控制電機位置主線程

圖12 定時器中斷流程圖

3.2 Modbus 通信協(xié)議

Modbus 是一種串行通信協(xié)議[17]，現(xiàn)在是工業(yè)電子設(shè)備之間常用的連接方式，支持RS232 和RS485 電氣接口。常用的Modbus 串行傳輸模式有ASCII 和RTU兩種[18]。根據(jù)云臺和上位機的通信數(shù)據(jù)量，云臺控制系統(tǒng)采用Modbus RTU 模式，采用二進(jìn)制表示數(shù)據(jù)，數(shù)據(jù)以幀的方式進(jìn)行傳輸。串口通信參數(shù)設(shè)置為：1 位起始位、8 位數(shù)據(jù)位、1 位停止位，無校驗，數(shù)據(jù)傳輸?shù)牟ㄌ芈蕿?8 400。

當(dāng)云臺下位機控制板收到上位機發(fā)送的控制命令時，先判斷發(fā)送的數(shù)據(jù)地址是否對應(yīng)下位機地址，若地址一致則讀取信息并對收到的數(shù)據(jù)進(jìn)行CRC 校驗。若CRC 效驗正確，則執(zhí)行相應(yīng)的任務(wù)。拷貝地址和命令、功能碼，并將執(zhí)行后的數(shù)據(jù)和CRC 校驗碼一起回送給上位機。若發(fā)送的數(shù)據(jù)地址與下位機地址不一致則返回錯誤。如果CRC 校驗出錯，則不返回任何信息。發(fā)送與接收數(shù)據(jù)都進(jìn)行CRC 校驗，確保了發(fā)送數(shù)據(jù)的準(zhǔn)確性[19]。

4 系統(tǒng)通信測試與實驗分析

4.1 通信測試和結(jié)果

云臺實物調(diào)試如圖13 所示。通過Keil5 軟件對程序編譯無誤后，采用ST LINK 仿真器將程序下載到云臺控制板中。采用RS485 一端連接電腦上位機，另一端連接云臺，使用MODBUS POLL 軟件調(diào)試電機轉(zhuǎn)向。當(dāng)上位機發(fā)送控制命令給云臺控制板時，觀察到云臺在回轉(zhuǎn)和俯仰兩自由度上，能夠根據(jù)相應(yīng)指令，做出對應(yīng)的運動。

圖13 云臺調(diào)試實物圖

Modbus 數(shù)據(jù)幀包含的內(nèi)容有：地址域+功能碼+數(shù)據(jù)+差錯校驗。

當(dāng)通過上位機給下位機發(fā)送（0x03）讀寄存器指令（30 03 00 12 00 02 60 2F）后，下位機（云臺）返回數(shù)據(jù)為（30 03 02 F1 37 C1 C6）。其 中F1 37 為返回數(shù)據(jù)，轉(zhuǎn)換為二進(jìn)制，表示水平轉(zhuǎn)速為241 r/min。通過測速儀測得水平電機的轉(zhuǎn)速為239 r/min，結(jié)果表明云臺監(jiān)控系統(tǒng)通信正常。

4.2 海洋實驗結(jié)果

圖14 所示為云臺搭載在水下機器人上。在水下機器人進(jìn)行作業(yè)時，地面工控機通過電纜將控制命令發(fā)送到機器人電子倉內(nèi)控制板，再轉(zhuǎn)發(fā)到云臺控制板。圖15所示云臺控制系統(tǒng)完全滿足水下機器人對云臺性能的要求，實現(xiàn)水下工況的實時監(jiān)測和圖片采集。

圖14 水下機器人裝置

圖15 工控機上云臺攝像頭拍攝的畫面

5 結(jié)論

本文研制了一種兩自由度深水觀測云臺結(jié)構(gòu)及其控制系統(tǒng)，通過集攝像頭和深水LED 燈于一體，為機器人在100 m 水深作業(yè)提供實時性的監(jiān)控平臺。

設(shè)計了基于FreeRTOS的云臺控制系統(tǒng)，采用DMA 中斷和USART 通信，實現(xiàn)實時多任務(wù)操作的同時，便于代碼模塊化編寫，系統(tǒng)響應(yīng)快。采用STM32 控制板結(jié)合PID 算法，應(yīng)用RS485 通信和Modbus 通信協(xié)議，實現(xiàn)回轉(zhuǎn)和俯仰步進(jìn)電機的位置控制。通過通信測試和海洋實驗，證明了本文所設(shè)計的基于STM32 兩軸深水觀測云臺控制系統(tǒng)運行平穩(wěn)，能夠?qū)崿F(xiàn)對深水高質(zhì)量圖片和視頻的實時采集。

相比國內(nèi)外高昂的云臺裝置，本文所設(shè)計的云臺結(jié)構(gòu)和控制系統(tǒng)均為自主研發(fā)，開發(fā)成本低，具有很強的實用價值和廣闊的市場。