基于STM32 兩軸深水觀測云臺控制系統設計

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

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

0 引言

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

國內外云臺多為單自由度或固定式，且常見的云臺微控制器多為單核或雙核，數據傳輸速度和存儲量不高[3]。2014 年，中國海洋石油總公司設計了一款移動式云臺，只適用于淺海水下場景的實時監控[4]。2016 年，殷莉甜針對水下云臺結構的密封性和圖像檢測要求，設計了一款應用于水下焊接機器人的雙目視覺系統，但其檢測精度不高[5-6]。基于上述相關研究，本文設計了基于STM32 兩軸深水觀測云臺結構及其控制系統，不但能實現回轉和俯仰兩自由度的運動，而且降低了水下機器人系統的開發成本，是水下機器人智能化研究的關鍵技術。

1 兩自由度云臺結構方案設計

圖1 所示為兩自由度深水觀測云臺的整體結構圖，其主要由云臺主體、數字攝像頭、模擬攝像頭以及深水LED 燈組成。由于水下機器人內部空間狹小，對云臺結構進行了小型化設計，并采用連接板對攝像頭和燈進行固定。

圖1 兩自由度云臺整體結構圖

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

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

2 兩自由度云臺系統總體設計

2.1 云臺控制系統總體方案設計

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

表1 兩自由云臺控制系統組成

2.2 云臺控制系統硬件設計

2.2.1 云臺測控系統組成

云臺控制系統硬件主要由云臺通信主控板和云臺攝像頭與LED 燈的控制板兩部分構成。采用RS485 主從通信方式，對控制電機轉向的控制命令以及大量數據進行解碼、存儲和調用。主控板控制系統主要由MCU、A4988 電機驅動模塊、IO 接線端子、RS485 通信接口與電源電路等部分組成，如圖3 所示。

圖3 云臺測控系統組成圖

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

2.2.2 云臺電機控制原理

采用28 行星減速、兩相混合式步進電機使二自由度云臺能夠實現回轉180°、俯仰±50°大范圍水下環境的監控。電機的供電電壓為24 V，扭矩為0.18 Nm。電機控制模塊電路如圖4 所示。

圖4 電機控制模塊電路圖

設定電機轉一圈，需要400 個脈沖。電機轉速和脈沖頻率成正比。通過控制1 s 內發送的脈沖個數和脈沖頻率，調節電機轉速。電機驅動芯片選用A4988，其模塊電路如圖5 所示。

圖5 A4988 模塊電路圖

步進電機是將電脈沖信號轉變為角位移或線位移的開環控制元件。步進電機的脈沖數和脈沖頻率分別決定了其轉動的速度和總角度位移，所有形式的運動方式改變均可以在很少的脈沖數量下完成。云臺電機控制如圖6 所示，通過改變相應IO 口電平即可控制電機的轉向。使用角度傳感器記錄電機轉的圈數，計數隨角度傳感器的轉向而增加或減少。隨著電機軸的旋轉給出一個精準的角度信號。電位器測一圈，確定云臺的當前位置。

圖6 云臺電機控制圖

2.2.3 云臺通信方案設計

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

圖7 云臺通信方案示意圖

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

兩自由度深水觀測云臺搭載的水下攝像頭與深水LED 燈組成了水下機器人視覺系統，深水LED 燈輔助攝像頭拍攝、對深海光線不足的環境進行補光，是水下機器人進行深海環境圖像觀測與采集的重要依據。云臺選用兩個高清模擬攝像頭和數字攝像頭。模擬攝像頭固定安裝在機械臂正下方，是導航和發現目標的依據。

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

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

2.3 云臺位置PID 控制算法

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

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

圖9 PID 控制原理圖

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

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

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

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

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

圖10 云臺PID 控制流程圖

3 云臺控制系統軟件設計

在Windows 操作系統上使用Visual Studio 軟件，選用面向對象的C# 語言對上位機控制界面進行開發[13]。云臺上位機控制界面主要有串口通信、云臺姿態控制、視頻監控、深水LED 燈調光等功能。云臺下位機調試軟件Keil5的主要作用是：對控制器的通信端口進行初始化、配置定時器，對控制命令和數據進行解析、數模轉換，使用Modbus 實現數據的雙向傳輸，實現各個子系統功能。

3.1 控制系統和DMA 數據采集

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

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

云臺控制系統對云臺實際回轉和俯仰電機位置進行采集，根據設定值和實際值之差，執行運動控制中斷程序內調用的增量PID 控制算法，控制發送步進電機運動控制脈沖的快慢。通過步進電機控制程序，輸出修正后的步進電機驅動信號，調整云臺位置，實現云臺位置的精確調控。云臺與上位機采用RS485 通信、DMA 中斷進行數據采集，收到的數據存入緩沖區，主程序按照標志位處理。根據標志位控制電機位置主線程如圖11 所示，定時器中斷流程圖如圖12 所示。

圖11 根據標志位控制電機位置主線程

圖12 定時器中斷流程圖

3.2 Modbus 通信協議

Modbus 是一種串行通信協議[17]，現在是工業電子設備之間常用的連接方式，支持RS232 和RS485 電氣接口。常用的Modbus 串行傳輸模式有ASCII 和RTU兩種[18]。根據云臺和上位機的通信數據量，云臺控制系統采用Modbus RTU 模式，采用二進制表示數據，數據以幀的方式進行傳輸。串口通信參數設置為：1 位起始位、8 位數據位、1 位停止位，無校驗，數據傳輸的波特率為38 400。

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

4 系統通信測試與實驗分析

4.1 通信測試和結果

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

圖13 云臺調試實物圖

Modbus 數據幀包含的內容有：地址域+功能碼+數據+差錯校驗。

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

4.2 海洋實驗結果

圖14 所示為云臺搭載在水下機器人上。在水下機器人進行作業時，地面工控機通過電纜將控制命令發送到機器人電子倉內控制板，再轉發到云臺控制板。圖15所示云臺控制系統完全滿足水下機器人對云臺性能的要求，實現水下工況的實時監測和圖片采集。

圖14 水下機器人裝置

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

5 結論

本文研制了一種兩自由度深水觀測云臺結構及其控制系統，通過集攝像頭和深水LED 燈于一體，為機器人在100 m 水深作業提供實時性的監控平臺。

設計了基于FreeRTOS的云臺控制系統，采用DMA 中斷和USART 通信，實現實時多任務操作的同時，便于代碼模塊化編寫，系統響應快。采用STM32 控制板結合PID 算法，應用RS485 通信和Modbus 通信協議，實現回轉和俯仰步進電機的位置控制。通過通信測試和海洋實驗，證明了本文所設計的基于STM32 兩軸深水觀測云臺控制系統運行平穩，能夠實現對深水高質量圖片和視頻的實時采集。

相比國內外高昂的云臺裝置，本文所設計的云臺結構和控制系統均為自主研發，開發成本低，具有很強的實用價值和廣闊的市場。