海岸帶履帶機(jī)器人控制系統(tǒng)的研究與設(shè)計(jì)

吳凱，周悅，郭威，李廣偉，孫洪鳴

（1.上海海洋大學(xué)工程學(xué)院，上海 201306；2.中國(guó)科學(xué)院深?？茖W(xué)與工程研究所，三亞 572000；3.中國(guó)科學(xué)院大學(xué),北京 100049)

0 引言

我國(guó)海岸帶領(lǐng)域面積寬廣、資源豐富，是經(jīng)濟(jì)發(fā)展的“黃金地帶”[1]。近年來(lái)，各國(guó)對(duì)海岸帶開(kāi)發(fā)力度逐漸加大，但由于海岸帶環(huán)境復(fù)雜，人工勘測(cè)難度大，作業(yè)成本高等問(wèn)題。因此，作為重要載體和技術(shù)手段的海岸帶勘測(cè)裝備的研發(fā)尤為重要。

為滿足海岸帶大范圍、無(wú)人化移動(dòng)考察作業(yè)需求，在海岸帶履帶機(jī)器人載體平臺(tái)基礎(chǔ)上，基于LabVIEW開(kāi)發(fā)上位機(jī)人機(jī)交互終端，使用ARMCortex-M4內(nèi)核的STM32F407芯片作為控制核心，軟件上基于FreeRTOS實(shí)時(shí)操作系統(tǒng)開(kāi)發(fā)，設(shè)計(jì)了一套結(jié)構(gòu)簡(jiǎn)單、通信可靠、實(shí)時(shí)性強(qiáng)的履帶機(jī)器人控制系統(tǒng)，從而實(shí)現(xiàn)其近程端、遠(yuǎn)程端的有效運(yùn)動(dòng)控制、數(shù)據(jù)獲取、存儲(chǔ)、顯示等功能。

1 海岸帶履帶機(jī)器人結(jié)構(gòu)及組成

海岸帶履帶機(jī)器人載體平臺(tái)采用克里斯蒂全獨(dú)立懸掛設(shè)計(jì)，車體兩側(cè)使用內(nèi)嵌凱夫拉纖維橡膠一體成型履帶韌性強(qiáng)、耐磨性好。承重輪主要用于載體支撐及履帶規(guī)正，以保證其運(yùn)動(dòng)的平穩(wěn)性。誘導(dǎo)輪通過(guò)調(diào)節(jié)履帶的張緊度以適應(yīng)不同路況的越障需求。托鏈輪用于托起履帶上部部分重力，避免履帶下垂。載體平臺(tái)的驅(qū)動(dòng)力是由兩臺(tái)直流無(wú)刷電機(jī)經(jīng)減速器將動(dòng)力傳遞給驅(qū)動(dòng)輪。同時(shí)，合理的底盤(pán)大小保證了運(yùn)動(dòng)控制系統(tǒng)執(zhí)行器和搭載的各類傳感器有充裕的空間[2-3]。海岸帶履帶機(jī)器人載體平臺(tái)及主要參數(shù)如圖1、表1所示。

圖1 海岸帶履帶機(jī)器人載體平臺(tái)

表1 載體平臺(tái)主要參數(shù)

2 控制系統(tǒng)設(shè)計(jì)

海岸帶履帶機(jī)器人作為一種新型的海岸帶探測(cè)裝備，其控制系統(tǒng)設(shè)計(jì)主要為實(shí)現(xiàn)遠(yuǎn)程模式下履帶機(jī)器人的運(yùn)動(dòng)控制、定點(diǎn)精細(xì)化勘測(cè)以及應(yīng)對(duì)突發(fā)狀況下履帶機(jī)器人的回收控制。這要求控制系統(tǒng)及設(shè)備對(duì)采集的數(shù)據(jù)進(jìn)行快速處理并上報(bào)至上位機(jī)終端；要求上位機(jī)有效的對(duì)下位機(jī)進(jìn)行控制以及對(duì)下位機(jī)上傳的各種數(shù)據(jù)進(jìn)行記錄、存儲(chǔ)和顯示；要求各個(gè)傳感器依據(jù)某一標(biāo)準(zhǔn)協(xié)議及接口與主控制器進(jìn)行通信。

依據(jù)控制功能及要求，最終將控制系統(tǒng)劃分為五大單元：

1）控制單元：用于近程遙控端、上位機(jī)遠(yuǎn)程端對(duì)履帶機(jī)器人實(shí)時(shí)監(jiān)視與運(yùn)動(dòng)控制；

2）通信單元：用于主控制器與上位機(jī)終端之間的4G網(wǎng)絡(luò)通信，以及主控制器與GPS、位姿傳感器、電機(jī)控制器的串行通信；

3）數(shù)據(jù)采集單元：包括用于采集作業(yè)環(huán)境的圖像信息以及自身的位置和姿態(tài)信息；

4）電源單元：為各類傳感器、控制器、電機(jī)等用電模塊供電；

5）電機(jī)控制單元：用于控制兩臺(tái)直流無(wú)刷電機(jī)運(yùn)動(dòng)。

2.1 系統(tǒng)硬件設(shè)計(jì)

海岸帶履帶機(jī)器人控制系統(tǒng)硬件設(shè)計(jì)采用模塊化設(shè)計(jì)思想[4,5]，依據(jù)控制系統(tǒng)5大功能，設(shè)計(jì)控制單元、數(shù)據(jù)采集單元、通信單元、電機(jī)控制單元、電源單元的硬件電路，這樣既便于硬件安裝、調(diào)試及維護(hù)，又可為后續(xù)海岸帶履帶機(jī)器人系統(tǒng)功能的升級(jí)優(yōu)化奠定基礎(chǔ)?？刂葡到y(tǒng)硬件架構(gòu)如圖2所示。

圖2 控制系統(tǒng)硬件架構(gòu)圖

為了滿足以上控制應(yīng)用需求，主控制器選用高性能、低功耗、實(shí)時(shí)響應(yīng)ARMCortex內(nèi)核的STM32F407ZGT6作為控制核心[6,7]，該芯片擁有高速數(shù)據(jù)緩存、168MHz高速運(yùn)行頻率、自帶FPU與DSP指令集、豐富的外設(shè)接口等特點(diǎn)。各項(xiàng)參數(shù)指標(biāo)與實(shí)踐表明，STM32F407ZGT6芯片是一款功能強(qiáng)大且適用于海岸帶履帶機(jī)器人主控制器的芯片。

采集電路主要包括采集位置、位姿、圖像等數(shù)據(jù)信息電路。由于控制系統(tǒng)對(duì)位置、位姿數(shù)據(jù)采集精度要求較高，而GPS信號(hào)經(jīng)過(guò)對(duì)流層時(shí)受到水汽分布不均的影響，在短距離設(shè)置基站時(shí)可以通過(guò)使用差分方法消除這一誤差，但中長(zhǎng)距離、單點(diǎn)定位時(shí)我國(guó)設(shè)置參考基站并不能滿足有要求精度的模型。經(jīng)過(guò)試驗(yàn)，本文選用體積小、性能優(yōu)異的ATK-S1216F8-BD GPS/北斗雙模定位芯片。位姿數(shù)據(jù)采集選用Bosch公司BMX055自帶數(shù)字運(yùn)動(dòng)處理器及硬件加速引擎的MPU6050芯片，該芯片內(nèi)部具有3軸陀螺儀和3軸加速度計(jì)。通過(guò)試驗(yàn)，該芯片能夠滿足采集的速度與精度要求，適用于該控制系統(tǒng)。由于海岸帶履帶機(jī)器人需要在夜間作業(yè)，環(huán)境信息的圖像采集也存在數(shù)據(jù)信息量較大、MCU處理資源不足問(wèn)題，為減輕MCU負(fù)擔(dān)提高其實(shí)時(shí)處理的能力，本文選用HIKVISION 1920*1080px分辨率、內(nèi)置鋰電池、微光全彩夜視、超長(zhǎng)待機(jī)的4G高清攝像頭。通過(guò)驗(yàn)證，該4G高清攝像頭是對(duì)海岸帶履帶機(jī)器人無(wú)線圖像采集的一種有效解決方案。

通信電路設(shè)計(jì)包括4G無(wú)線通信、RS232通信、IIC總線通信電路[8]。其中，4G無(wú)線通信電路負(fù)責(zé)將圖像信息上傳至上位機(jī)以及主控制器與上位機(jī)終端的數(shù)據(jù)交互；IIC和RS232通信電路分別負(fù)責(zé)將采集的位姿數(shù)據(jù)和GPS數(shù)據(jù)經(jīng)主控制器打包上傳至上位機(jī)。使用3種通信協(xié)同合作使得該控制系統(tǒng)信號(hào)傳輸速率快、通信穩(wěn)定可靠。

履帶機(jī)器人運(yùn)動(dòng)依靠?jī)膳_(tái)直流無(wú)刷電機(jī)差速旋轉(zhuǎn)實(shí)現(xiàn)，因此電機(jī)控制器需要同時(shí)滿足控制兩臺(tái)直流無(wú)刷電機(jī)運(yùn)動(dòng)，同時(shí)采集轉(zhuǎn)子位置反饋的霍爾信號(hào)及增量式編碼器信號(hào)以實(shí)現(xiàn)電機(jī)速度閉環(huán)控制。本文選用了DC48DPW50BL-PC2智能型雙直流無(wú)刷電機(jī)控制器，該控制器可以同時(shí)控制兩臺(tái)直流無(wú)刷電機(jī)，支持模擬量、脈沖量以及數(shù)字量輸入。

考慮到海岸帶履帶機(jī)器人需要執(zhí)行遠(yuǎn)程情況下的運(yùn)動(dòng)控制、定點(diǎn)精細(xì)化勘測(cè)，以及應(yīng)對(duì)突發(fā)狀況下近程端履帶機(jī)器人的回收，將系統(tǒng)控制模式設(shè)計(jì)為遠(yuǎn)程端控制和近程端控制兩種方式。如圖2所示，遠(yuǎn)程端控制模式采用數(shù)字量輸入控制方式，當(dāng)主控制器接收到上位機(jī)終端指令時(shí)，經(jīng)過(guò)SP3232（TTL電平轉(zhuǎn)RS232電平模塊）將解析的數(shù)據(jù)下達(dá)至電機(jī)控制器。而近程端遙控模式采用RC航模遙控控制方式即航模發(fā)射器發(fā)出脈寬信號(hào)，RC航模Radio接收機(jī)將接收的脈寬信號(hào)傳至電機(jī)控制器模擬量輸入端，設(shè)定接收1.0ms脈寬信號(hào)時(shí)對(duì)應(yīng)于操縱桿的最小速度，2.0ms脈寬時(shí)對(duì)應(yīng)于操縱桿的最大速度，通過(guò)調(diào)節(jié)1.0ms～2.0ms脈寬實(shí)現(xiàn)電機(jī)的速度控制[9,10]。

根據(jù)硬件電路連接需求，將主控制器I/O引腳進(jìn)行分配，引腳分配如表2所示。

表2 主控制器I/O引腳分配表

2.2 下位機(jī)軟件設(shè)計(jì)

下位機(jī)基于FreeRTOS嵌入式實(shí)時(shí)操作系統(tǒng)進(jìn)行設(shè)計(jì)。FreeRTOS是一個(gè)可裁剪、輕量級(jí)、調(diào)度策略靈活的實(shí)時(shí)嵌入式操作系統(tǒng)內(nèi)核，具有任務(wù)通知、消息隊(duì)列、信號(hào)量、時(shí)間片調(diào)度等功能[11]。依據(jù)控制需求設(shè)計(jì)4個(gè)應(yīng)用任務(wù)：初始化任務(wù)、數(shù)據(jù)采集任務(wù)、電機(jī)控制任務(wù)和通信任務(wù)，利用其多任務(wù)（scheduler）管理機(jī)制協(xié)調(diào)各個(gè)工作任務(wù)，可以提高系統(tǒng)的實(shí)時(shí)性?；贔reeRTOS操作系統(tǒng)的軟件結(jié)構(gòu)如圖3所示。

圖3 履帶機(jī)器人控制器軟件結(jié)構(gòu)圖

1）系統(tǒng)初始化

初始化任務(wù)主要完成系統(tǒng)的初始化工作。初始化主要包括系統(tǒng)時(shí)鐘、串口、IIC總線、GPIO引腳、定時(shí)器、創(chuàng)建應(yīng)用任務(wù)后掛起自身任務(wù)等。

2）數(shù)據(jù)采集任

數(shù)據(jù)采集任務(wù)主要包括對(duì)GPS數(shù)據(jù)、MPU6050位姿數(shù)據(jù)以及電機(jī)控制器數(shù)據(jù)的采集。其中，GPS數(shù)據(jù)通過(guò)ATKS1216F8-BD模塊進(jìn)行采集，采集的數(shù)據(jù)包括北京時(shí)間、經(jīng)度、緯度、海拔高度以及運(yùn)動(dòng)速度。位姿數(shù)據(jù)通過(guò)MPU6050芯片采集，采集的數(shù)據(jù)包括俯仰角、橫滾角、航向角。而電機(jī)控制器數(shù)據(jù)采集包括左右電機(jī)速度，電壓和電流等。采集任務(wù)流程如圖4所示。

圖4 采集任務(wù)流程圖

3）電機(jī)控制任務(wù)

電機(jī)控制任務(wù)通過(guò)上位機(jī)下達(dá)控制指令實(shí)現(xiàn)。上位機(jī)訪問(wèn)電機(jī)控制器時(shí)，雙方采用應(yīng)答機(jī)制進(jìn)行交互，對(duì)于有要求應(yīng)答的查詢，例如讀取速度、電流等，回復(fù)查詢被認(rèn)為是命令的響應(yīng)。對(duì)于不需要回復(fù)的命令，例如速度設(shè)置，控制器應(yīng)答數(shù)據(jù)幀將在每個(gè)回車符之后以“+”作為命令響應(yīng)結(jié)束。當(dāng)命令或查詢已被接收，但不能被識(shí)別或因?yàn)槟撤N原因不能被接收時(shí)，控制器應(yīng)答數(shù)據(jù)幀將在回車符后以“-”提示錯(cuò)誤。電機(jī)控制任務(wù)流程如圖5所示。

圖5 電機(jī)控制任務(wù)流程圖

4）通信任務(wù)

通信任務(wù)包括RS232通信，4G通信和IIC通信3種方式。其中，4G DTU模塊使用RS232轉(zhuǎn)4G雙向透?jìng)鳎瑢?shí)現(xiàn)上位機(jī)終端與下位機(jī)4G收發(fā)模塊之間的遠(yuǎn)程通信。RS232通信任務(wù)運(yùn)行時(shí)，將等待USART中斷回調(diào)函數(shù)通知，當(dāng)上位機(jī)終端下達(dá)控制指令時(shí)，會(huì)觸發(fā)USART接收中斷，在中斷回調(diào)函數(shù)中通知RS232通信任務(wù)。如果RS232通信任務(wù)未就緒或未運(yùn)行則立即退出中斷，進(jìn)行任務(wù)調(diào)度使RS232通信任務(wù)就緒獲得主控制器控制權(quán)，然后主控制器根據(jù)通信協(xié)議解析收到的指令幀從而控制各個(gè)設(shè)備。IIC通信任務(wù)負(fù)責(zé)將位姿信息傳至主控制器，根據(jù)任務(wù)要求，設(shè)置1s定時(shí)采集一次位姿數(shù)據(jù)，然后主控制器打包數(shù)據(jù)將其放入發(fā)送隊(duì)列中，等待USART發(fā)送函數(shù)將隊(duì)列中的數(shù)據(jù)依次發(fā)送至上位機(jī)終端。

軟件控制協(xié)議幀采用ASCII編碼格式，幀長(zhǎng)度規(guī)定為變長(zhǎng)，以/r/n為結(jié)束符，通信協(xié)議編碼如表3所示。

表3 通信協(xié)議編碼表

其中，數(shù)據(jù)幀的nn 和mm 分別表示通道1 和通道2從-1000r/min到1000r/min的轉(zhuǎn)速給定值，cc表示電機(jī)通道值，aa表示設(shè)備的on/off。

3 上位機(jī)設(shè)計(jì)

上位機(jī)選用NI公司LabVIEW集成開(kāi)發(fā)環(huán)境開(kāi)發(fā)，能顯著降低控制系統(tǒng)開(kāi)發(fā)周期及維護(hù)成本[12]。為了調(diào)試時(shí)方便讀取通信的數(shù)據(jù)幀以及運(yùn)動(dòng)控制時(shí)提高操作的便捷性，上位機(jī)設(shè)計(jì)了程序調(diào)試和運(yùn)動(dòng)控制兩個(gè)窗口。

在程序調(diào)試窗口中，為了方便串口配置及通信，使用VISA函數(shù)設(shè)計(jì)了支持自動(dòng)識(shí)別串口，可以設(shè)置常用9600、115200以及自定義波特率、校驗(yàn)位、數(shù)據(jù)位和停止位，并且數(shù)據(jù)幀發(fā)送形式設(shè)計(jì)了可以支持ASCII碼或十六進(jìn)制的可選按鈕。調(diào)試時(shí)通過(guò)串口將指令幀從數(shù)據(jù)發(fā)送區(qū)下達(dá)至海岸帶履帶機(jī)器人主控制器，同時(shí)機(jī)器人的運(yùn)動(dòng)狀態(tài)、各個(gè)傳感器數(shù)據(jù)、電機(jī)的運(yùn)行狀態(tài)也將以數(shù)據(jù)幀形式響應(yīng)在程序調(diào)試窗口數(shù)據(jù)顯示區(qū)。程序調(diào)試窗口設(shè)計(jì)主要包括：串口設(shè)置區(qū)、數(shù)據(jù)顯示區(qū)、數(shù)據(jù)發(fā)送區(qū)等。

而運(yùn)動(dòng)控制窗口為了易于實(shí)現(xiàn)海岸帶履帶機(jī)器人基本運(yùn)動(dòng)控制，行駛控制區(qū)設(shè)計(jì)了4個(gè)代表履帶機(jī)器人前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)的運(yùn)動(dòng)控制按鈕，操作員通過(guò)4個(gè)控制按鈕可以實(shí)時(shí)調(diào)整履帶機(jī)器人的運(yùn)行方向及姿態(tài)?？紤]實(shí)驗(yàn)后方便對(duì)履帶機(jī)器人控制系統(tǒng)性能進(jìn)行分析，設(shè)計(jì)傳感器數(shù)據(jù)區(qū)用于記錄履帶機(jī)器人每個(gè)時(shí)刻的狀態(tài)及各個(gè)傳感器數(shù)據(jù)，同時(shí)GPS、位姿傳感器采集的數(shù)據(jù)信息將在軌跡顯示區(qū)、位姿顯示區(qū)中動(dòng)態(tài)顯示。運(yùn)動(dòng)控制窗口的設(shè)計(jì)包括：串口設(shè)置區(qū)、行駛控制區(qū)、傳感器數(shù)據(jù)區(qū)、軌跡顯示區(qū)、位姿顯示區(qū)等。上位機(jī)監(jiān)控終端架構(gòu)如圖6所示。

圖6 上位機(jī)監(jiān)控終端架構(gòu)

4 實(shí)驗(yàn)驗(yàn)證及分析

為驗(yàn)證控制系統(tǒng)有效性，本文首先對(duì)控制系統(tǒng)的各個(gè)功能模塊進(jìn)行調(diào)試，其次正確連接控制器、各類傳感器以及上位機(jī)硬件電路進(jìn)行整體測(cè)試，然后對(duì)上位機(jī)和下位機(jī)進(jìn)行系統(tǒng)聯(lián)調(diào)，最后將海岸帶履帶機(jī)器人置于海岸灘涂進(jìn)行實(shí)驗(yàn)測(cè)試，實(shí)驗(yàn)如圖7所示。

圖7 海岸帶機(jī)器人實(shí)驗(yàn)

實(shí)驗(yàn)時(shí)首先對(duì)GPS、位姿傳感器進(jìn)行調(diào)試，通過(guò)將GPS和位姿任務(wù)程序下載至主控制器，正確連接硬件電路、設(shè)置通信參數(shù)，用開(kāi)發(fā)的上位機(jī)程序調(diào)試窗口進(jìn)行數(shù)據(jù)讀取，讀取結(jié)果如圖8所示。由程序調(diào)試窗口數(shù)據(jù)接收區(qū)可以看到GPS和位姿傳感器數(shù)據(jù)每間隔1s上發(fā)一次。GPS數(shù)據(jù)的授時(shí)時(shí)間、經(jīng)度、緯度、海拔高度與GPS校準(zhǔn)儀測(cè)量的數(shù)據(jù)基本一致。位姿傳感器的航向角、俯仰角、橫滾角數(shù)據(jù)也與位姿標(biāo)定傳感器測(cè)量的數(shù)據(jù)相同。

圖8 程序調(diào)試界面

最后，將海岸帶履帶機(jī)器人置于灘涂進(jìn)行實(shí)驗(yàn)，通過(guò)上位機(jī)終端的運(yùn)動(dòng)控制窗口下達(dá)前進(jìn)運(yùn)動(dòng)指令，履帶機(jī)器人在實(shí)驗(yàn)灘涂從起始點(diǎn)以0.5m/s速度平穩(wěn)出發(fā)，實(shí)驗(yàn)員通過(guò)視頻圖像及上位機(jī)終端界面操作4個(gè)運(yùn)動(dòng)控制按鈕，使履帶機(jī)器人按照作業(yè)路徑安全平穩(wěn)行駛，此間控制系統(tǒng)通信穩(wěn)定，各個(gè)傳感器數(shù)據(jù)采集及上位機(jī)顯示正常，履帶機(jī)器人實(shí)驗(yàn)運(yùn)動(dòng)軌跡曲線實(shí)時(shí)顯示于上位機(jī)運(yùn)動(dòng)控制窗口地圖中，如圖9所示。傳感器數(shù)據(jù)區(qū)記錄其運(yùn)動(dòng)軌跡經(jīng)緯度坐標(biāo)點(diǎn)，使用Matlab繪制坐標(biāo)曲線與上位機(jī)地圖軌跡相同，如圖10所示。

圖9 運(yùn)動(dòng)控制界面

圖10 經(jīng)、緯度坐標(biāo)曲線圖。

5 結(jié)語(yǔ)

本文針對(duì)海岸帶大范圍、無(wú)人化勘測(cè)作業(yè)需求，對(duì)海岸帶履帶機(jī)器人控制系統(tǒng)進(jìn)行設(shè)計(jì)、實(shí)現(xiàn)與實(shí)驗(yàn)。下位機(jī)基于ARM Cortex-M4內(nèi)核STM32F407芯片，圍繞功能需求進(jìn)行了系統(tǒng)的硬件與軟件設(shè)計(jì)。其中，軟件部分采用FreeRTOS嵌入式實(shí)時(shí)操作系統(tǒng)進(jìn)行模塊化編程，提高了MCU資源的利用率及事件的實(shí)時(shí)響應(yīng)速度。同時(shí)采用模塊化設(shè)計(jì)思想，根據(jù)不同勘測(cè)需求搭載不同傳感器，克服了單一固定數(shù)據(jù)采集的難題，也便于后期系統(tǒng)的維護(hù)以及功能的升級(jí)。上位機(jī)采用LabVIEW集成開(kāi)發(fā)環(huán)境進(jìn)行監(jiān)控終端的設(shè)計(jì)，縮短了控制系統(tǒng)程序調(diào)試周期，提高了開(kāi)發(fā)效率。實(shí)驗(yàn)測(cè)試結(jié)果表明，設(shè)計(jì)的控制系統(tǒng)通信可靠、實(shí)時(shí)性強(qiáng)、工作性能穩(wěn)定、上位機(jī)終端控制效果理想。