張申毅，樊紹勝，程嘉翊，黃 輝

(1.長(zhǎng)沙理工大學(xué)電氣與信息工程學(xué)院，湖南長(zhǎng)沙 410114；2.湖南省中南橋梁安裝工程有限公司，湖南懷化 418000)

0 引言

城市發(fā)展面臨地下供電、通信、給排水、供氣等管道交錯(cuò)復(fù)雜、維護(hù)不便等困難。 地下綜合管廊集成了電力、通信、燃?xì)?、給排水等各種工程管道。地下綜合管廊運(yùn)維壓力大，地下綜合管廊空間狹長(zhǎng)、管線密布，傳統(tǒng)的人工檢查效率低、無(wú)法發(fā)現(xiàn)各種細(xì)微故障；綜合管廊往往溫度較高、濕度較大，有時(shí)還會(huì)積聚毒害氣體，作業(yè)環(huán)境比較惡劣；除此之外，人工巡檢效率較低，巡檢間隔較長(zhǎng)，難以獲知地下管線和相關(guān)設(shè)備的運(yùn)行狀況。

鑒于地下綜合管廊人工巡檢的缺陷與不足，使用軌道式機(jī)器人代替人工進(jìn)行日常檢查成為研究熱點(diǎn)。因此，本文設(shè)計(jì)了一種基于STM32的軌道式巡檢機(jī)器人控制系統(tǒng)，該控制系統(tǒng)具有安全性能好、定位精度高、響應(yīng)速度快等優(yōu)點(diǎn)。

1 機(jī)器人控制系統(tǒng)總體設(shè)計(jì)

軌道式巡檢機(jī)器人的結(jié)構(gòu)示意圖如圖1和圖2所示：機(jī)器人主要由3部分組成： 頂部的行走部分，中部的控制部分和底部的工作部分。

圖1 軌道式巡檢機(jī)器人側(cè)面示意圖

圖2 軌道式巡檢機(jī)器人正面示意圖

軌道式巡檢機(jī)器人的控制系統(tǒng)總體架構(gòu)如圖3所示。該系統(tǒng)包括STM32微處理器及其最小系統(tǒng)、電源、CAN總線通信、光耦隔離、AD采樣、485總線通信以及以太網(wǎng)通信等模塊。主控模塊主要完成的功能為：與上位機(jī)監(jiān)控界面進(jìn)行信息交互，接收、解析上位機(jī)監(jiān)控系統(tǒng)發(fā)出的指令并分發(fā)至對(duì)應(yīng)的任務(wù)線程執(zhí)行相應(yīng)操作；收集并整理各傳感器數(shù)據(jù)、電機(jī)運(yùn)行數(shù)據(jù)和機(jī)器人位置數(shù)據(jù)并實(shí)時(shí)發(fā)送至上位機(jī)。感知單元主要包括溫度傳感器、可燃性氣體模組、超聲波雷達(dá)和電流傳感器組成；定位模塊主要包括500線3通道增量式里程計(jì)、磁性開(kāi)關(guān)、RFID收發(fā)器。電源模塊包括48 V鋰電池、24 V/12 V/5 V 3個(gè)電壓等級(jí)的直流降壓模塊、無(wú)線充電模塊接收端。

圖3 軌道式機(jī)器人總體框圖

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

軌道式巡檢機(jī)器人控制系統(tǒng)硬件部分主要分為微處理器以及信號(hào)調(diào)理電路設(shè)計(jì)[1]。

2.1 微處理器

微處理器采用STM32F407ZGT6單片機(jī)[2]。該型微處理器具有32位高性能ARM Cortex-M4處理器和強(qiáng)大的時(shí)鐘系統(tǒng)，其頻率可達(dá)168 MHz；擁有豐富的I/O口資源。在本設(shè)計(jì)中主要用到的I/O口有：(1)5個(gè)AD采樣端口，分別對(duì)機(jī)器人前/后超聲波雷達(dá)、鋰電池電壓、充放電回路電流進(jìn)行采樣與處理；(2)8個(gè)邏輯輸出端口，主要控制行走電機(jī)、關(guān)節(jié)電機(jī)與前/后LED探照燈、無(wú)線充電等設(shè)備固態(tài)繼電器的通斷；(3)2個(gè)485總線通信接口，分別與RFID收發(fā)器和氣體傳感器模組通信；(4)1個(gè)CAN總線通信接口，向行走電機(jī)、二自由度機(jī)械臂關(guān)節(jié)電機(jī)進(jìn)行通信。

2.2 AD采樣電路

由于微處理器I/O口只允許輸入最高3.3 V電壓，故需要對(duì)機(jī)器人各傳感器輸出信號(hào)進(jìn)行分壓與隔離采樣。圖4為0～5 V隔離采樣電路，該電路由分壓電路與電壓跟隨器組成[3]，為保證在輸入較低電壓的同時(shí)電壓跟隨電路也能準(zhǔn)確跟隨，同時(shí)考慮簡(jiǎn)化電路設(shè)計(jì)而盡量避免采用雙電源供電，故選取AD806搭建電壓跟隨電路，該芯片采用5～24 V單電源供電，能夠軌至軌輸出，具有低噪聲，高共模抑制比等優(yōu)點(diǎn)。

圖4 電壓隔離采樣電路

R801與R802構(gòu)成分壓電路，取R801=10 kΩ、R802=15 kΩ，分壓公式為

(1)

經(jīng)分壓電路后，0～5 V輸入信號(hào)將線性轉(zhuǎn)換為0～3 V。分壓電阻均采用高精密低溫漂貼片電阻以確保電壓采樣的精確度。電壓跟隨器對(duì)分壓后的信號(hào)進(jìn)行隔離，其中D801為3.3 V齊納二極管，用于過(guò)壓保護(hù)，防止因AD采樣電路輸入電壓過(guò)大或因干擾產(chǎn)生的過(guò)電壓導(dǎo)致微處理器損壞。光耦隔離電路如圖5所示。

圖5 光耦隔離電路

2.3 RS485通信電路

軌道式巡檢機(jī)器人配置2路RS485與氣體傳感器模組和RFID收發(fā)器進(jìn)行通信，RS485通信電路如圖6所示。本設(shè)計(jì)中采用致RS3485HT模塊，該模塊具有自動(dòng)收發(fā)數(shù)據(jù)功能，集成電源隔離、電氣隔離于一體，隔離耐壓達(dá)2 500 VDC，包含發(fā)送引腳及接收引腳，無(wú)需控制引腳即可實(shí)現(xiàn)自動(dòng)換向功能。 為了防止485總線出現(xiàn)瞬時(shí)過(guò)壓，485模塊A、B和RGND引腳均增加了TVS管D301、D302和D303進(jìn)行過(guò)壓保護(hù)。

圖6 RS485通信電路

3 系統(tǒng)軟件設(shè)計(jì)

軌道式巡檢機(jī)器人控制系統(tǒng)軟件包括下位機(jī)底層控制軟件和上位機(jī)監(jiān)控軟件設(shè)計(jì)兩部分[4]。

3.1 下位機(jī)底層控制程序軟件設(shè)計(jì)

3.1.1 控制程序軟件設(shè)計(jì)

FreeRTOS是一個(gè)可裁剪、可剝奪型的多任務(wù)內(nèi)核。FreeRTOS提供實(shí)時(shí)操作系統(tǒng)所需的所有功能，包括資源管理，同步，任務(wù)通信等[5]。在STM32F407ZGT6主控制板中搭載FreeRTOS實(shí)時(shí)操作系統(tǒng)可以基本上滿(mǎn)足系統(tǒng)需要，根據(jù)軌道式巡檢機(jī)器人各個(gè)模塊的作業(yè)任務(wù)的不同可相應(yīng)地在主控程序中創(chuàng)建多個(gè)線程分別進(jìn)行處理，合理配置各個(gè)線程的優(yōu)先級(jí)順序。

機(jī)器人主控軟件設(shè)計(jì)包括一個(gè)主線程及實(shí)時(shí)操作系統(tǒng)所創(chuàng)建的通信、感知與定位3個(gè)功能子線程。主控制板程序流程如圖7所示。

圖7 主程序流程圖

機(jī)器人程序啟動(dòng)后，先初始化各模塊，然后創(chuàng)建3個(gè)子線程分別為負(fù)責(zé)TCP通信、機(jī)器人感知和機(jī)器人定位。TCP通信子線程與上位機(jī)建立并保持通信，接收并解析上位機(jī)指令，再加載相應(yīng)的功能子線程；感知子線程采集監(jiān)測(cè)各傳感器數(shù)據(jù)，并在給定周期內(nèi)向上位機(jī)上傳數(shù)據(jù)包；定位子線程通過(guò)CAN總線向行走電機(jī)，并通過(guò)RFID收發(fā)器與磁性開(kāi)關(guān)對(duì)機(jī)器人進(jìn)行精確定位，到達(dá)指定檢測(cè)點(diǎn)后向機(jī)械臂關(guān)節(jié)電機(jī)下達(dá)角度運(yùn)動(dòng)命令對(duì)周?chē)芫€或設(shè)備進(jìn)行拍照、錄像、數(shù)據(jù)采集等操作。

3.1.2 機(jī)器人巡檢程序軟件設(shè)計(jì)

機(jī)器人在開(kāi)機(jī)之后即開(kāi)始對(duì)運(yùn)動(dòng)模塊、感知模塊與定位模塊等進(jìn)行初始化操作。在未接收到上位機(jī)下達(dá)的指令時(shí)，機(jī)器人處于休眠狀態(tài)，只保持TCP通信子線程處于工作狀態(tài)。當(dāng)接收到巡檢指令后，機(jī)器人喚醒感知與定位子程序并開(kāi)始巡檢作業(yè)。機(jī)器人按照給定的目標(biāo)點(diǎn)行進(jìn)，行進(jìn)過(guò)程中若感知到周?chē)霈F(xiàn)異常參數(shù)時(shí)，則立刻上傳異常參數(shù)與當(dāng)前所在位置。若上位機(jī)下達(dá)繼續(xù)巡檢指令，則機(jī)器人繼續(xù)向目標(biāo)巡檢點(diǎn)行進(jìn)，達(dá)到巡檢點(diǎn)后再次上傳各參數(shù)與圖片。機(jī)器人巡檢程序流程如圖8所示。

圖8 機(jī)器人巡檢程序流程圖

3.2 上位機(jī)監(jiān)控軟件

軌道式巡檢機(jī)器人的上位機(jī)測(cè)控軟件在Visual Studio 2017軟件中使用QT開(kāi)發(fā)框架編寫(xiě)，并根據(jù)巡檢作業(yè)要求制定上位機(jī)軟件界面。上位機(jī)監(jiān)控軟件界面如圖9所示：上位機(jī)窗口最右側(cè)為機(jī)器人操作區(qū)域，從上到下分別為啟?？刂茀^(qū)(啟動(dòng)、停止、重啟、充電)、行走電機(jī)控制區(qū)(前進(jìn)、后退、停止)、機(jī)械臂控制區(qū)(旋轉(zhuǎn)、復(fù)位)、雙目云臺(tái)控制區(qū)(復(fù)位、拍照、錄視頻)；界面中間為機(jī)器人實(shí)時(shí)位置、實(shí)時(shí)速度顯示區(qū)與雙目云臺(tái)視頻顯示區(qū)；上位機(jī)窗口最左側(cè)分別為機(jī)器人狀態(tài)顯示區(qū)與定位設(shè)定區(qū)。

圖9 主機(jī)監(jiān)控軟件界面圖

4 精準(zhǔn)定位控制器設(shè)計(jì)

傳統(tǒng)的軌道式巡檢機(jī)器人在定位方式上采用單獨(dú)里程計(jì)或配合RFID收發(fā)器進(jìn)行定位。采用單獨(dú)里程計(jì)進(jìn)行定位時(shí)，由于增量式里程計(jì)存在累積誤差，隨著機(jī)器人的不斷行進(jìn)必然會(huì)導(dǎo)致定位誤差不斷累積，因此該定位方法精度較低；采用RFID收發(fā)器的定位方式時(shí)，雖然RFID定位準(zhǔn)確度較高但由于機(jī)器人巡檢距離較長(zhǎng)導(dǎo)致RFID標(biāo)簽的布置成本較高。綜合考慮后，從里程計(jì)累積誤差的校正以及模糊控制器的設(shè)計(jì)2個(gè)角度提高機(jī)器人的定位精度與響應(yīng)速度。

4.1 速度-位置雙模式匹配運(yùn)動(dòng)控制方法

本文提出了一種速度-位置雙模式匹配運(yùn)動(dòng)控制方法用于校正里程計(jì)的累積誤差。速度-位置雙模式匹配運(yùn)動(dòng)控制方法示意圖如圖10所示。

圖10 速度-位置雙模式匹配運(yùn)動(dòng)控制方法示意圖

在軌道式機(jī)器人懸掛的工字軌道上每間隔1 m打入1顆定位螺釘，機(jī)器人搭載的磁性開(kāi)關(guān)每靠近定位螺釘則產(chǎn)生一次觸發(fā)信號(hào)，微處理器通過(guò)磁性開(kāi)關(guān)的觸發(fā)信號(hào)對(duì)里程計(jì)數(shù)據(jù)進(jìn)行清零以消除累積誤差。在管廊的重要巡檢點(diǎn)布置RFID標(biāo)簽以提高機(jī)器人定位系統(tǒng)的可靠性。

當(dāng)機(jī)器人獲知上位機(jī)控制系統(tǒng)下達(dá)的目標(biāo)行駛里程后即對(duì)目標(biāo)里程進(jìn)行速度-位置模式匹配：將目標(biāo)里程分為速度模式下行進(jìn)與位置模式下行進(jìn)2個(gè)區(qū)間，并依靠磁性開(kāi)關(guān)的觸發(fā)信號(hào)判斷速度-位置行進(jìn)區(qū)間的切換點(diǎn)。當(dāng)機(jī)器人行走至給定目標(biāo)點(diǎn)前一個(gè)螺釘時(shí)，微處理器通過(guò)向行走電機(jī)發(fā)送停止指令再切換為位置模式下行走剩余里程。速度-位置雙模式匹配運(yùn)動(dòng)控制方法流程圖如圖11所示。

圖11 速度-位置雙模式匹配運(yùn)動(dòng)控制方法流程圖

4.2 模糊控制器設(shè)計(jì)

機(jī)器人定位精度除了與增量式里程計(jì)的累積誤差有關(guān)外，還與當(dāng)前行進(jìn)速度密切關(guān)聯(lián)。進(jìn)行精確定位時(shí)若行進(jìn)速度過(guò)快，由于機(jī)械結(jié)構(gòu)及慣性作用導(dǎo)致機(jī)器人在停止過(guò)程中出現(xiàn)打滑；若行進(jìn)速度過(guò)慢，機(jī)器人行進(jìn)至目標(biāo)點(diǎn)所需時(shí)間過(guò)長(zhǎng)，實(shí)用性無(wú)法滿(mǎn)足。因此，設(shè)計(jì)單輸入單輸出形式的模糊控制器實(shí)現(xiàn)行進(jìn)速度跟隨當(dāng)前位置距離目標(biāo)點(diǎn)的誤差進(jìn)行相應(yīng)調(diào)整。

4.2.1 輸入輸出變量的確定

設(shè)機(jī)器人當(dāng)前位置與目標(biāo)位置的里程誤差為，作為模糊控制器輸入量[6]。里程誤差e的基本域是[-20 m,20 m],模糊域選擇為[-3，3]。取機(jī)器人在速度-位置雙模式匹配運(yùn)動(dòng)控制過(guò)程中速度模式區(qū)間下的行進(jìn)速度為v，作為模糊控制器輸出量。取速度v的基本論域?yàn)閇-80 cm/s,80 cm/s],模糊論域選擇為[-6,6]。

取輸入量e的模糊子集為{負(fù)遠(yuǎn)程，負(fù)中程，負(fù)近程，零，正近程，正中程，正遠(yuǎn)程}，用符號(hào)簡(jiǎn)記為{NF，NM，NN，ZO，PN，PM，PF}。取輸出變量γ的模糊子集為{負(fù)快速，負(fù)中速，負(fù)慢速，零，正慢速，正中速，正快速}，用符號(hào)簡(jiǎn)記為{NQ，NM，NS，ZO，PS，PM，PQ}。為簡(jiǎn)化計(jì)算，輸入量e與輸出量v均采用三角形隸屬度函數(shù)[7]。各變量的隸屬度函數(shù)如圖12以及圖13所示。

圖12 輸入量隸屬度函數(shù)

圖13 輸出量隸屬度函數(shù)

4.2.2 模糊控制規(guī)則的設(shè)計(jì)

模糊控制規(guī)則如表1所示。進(jìn)行精確定位時(shí)，機(jī)器人行進(jìn)速度主要取決于當(dāng)前位置到目標(biāo)位置的里程誤差[8],當(dāng)機(jī)器人遠(yuǎn)離目標(biāo)點(diǎn)時(shí)，行進(jìn)速度相應(yīng)地越大，提高響應(yīng)速度；機(jī)器人靠近目標(biāo)點(diǎn)附近時(shí)，行進(jìn)速度進(jìn)行相應(yīng)調(diào)整，以便精確定位；機(jī)器人接近目標(biāo)點(diǎn)時(shí)，行進(jìn)速度相應(yīng)較慢，防止較大速度下因慣性作用出現(xiàn)的打滑。

表1 模糊規(guī)則表

5 機(jī)器人樣機(jī)試驗(yàn)與結(jié)果分析

為了測(cè)試機(jī)器人的定位精度誤差與響應(yīng)速度，需要對(duì)機(jī)器人進(jìn)行檢測(cè)[9]。實(shí)驗(yàn)平臺(tái)為長(zhǎng)度40 m的工字軌道，在工字軌道上每間隔1 m打入1顆定位螺釘，在該平臺(tái)上對(duì)機(jī)器人進(jìn)行實(shí)驗(yàn)。

5.1 機(jī)器人樣機(jī)精確定位實(shí)驗(yàn)

在實(shí)驗(yàn)中通過(guò)向機(jī)器人給定目標(biāo)位置，再測(cè)量機(jī)器人實(shí)際行駛里程與相應(yīng)時(shí)間，精確定位實(shí)驗(yàn)結(jié)果如表2所示。由實(shí)驗(yàn)結(jié)果可知：機(jī)器人定位最大誤差不超過(guò)3 cm，定位精度高，響應(yīng)時(shí)間快，平均速度可跟隨里程變化而變化。

表2 機(jī)器人精確實(shí)驗(yàn)結(jié)果

5.2 機(jī)器人實(shí)際作業(yè)效果

在確定機(jī)器人精確定位功能滿(mǎn)足要求后，下一步對(duì)機(jī)器人進(jìn)行實(shí)際巡檢作業(yè)進(jìn)行測(cè)試，檢測(cè)機(jī)器人能否實(shí)時(shí)上傳環(huán)境參數(shù)，能否對(duì)異常參數(shù)進(jìn)行預(yù)警，并能準(zhǔn)確定位故障點(diǎn)，能否有效檢測(cè)電纜等設(shè)備過(guò)熱。實(shí)驗(yàn)結(jié)果表明，使用本控制系統(tǒng)的軌道式機(jī)器人可高效、自動(dòng)地完成地下綜合管廊的巡檢任務(wù)。實(shí)際作業(yè)效果如圖14所示。

圖14 機(jī)器人實(shí)際巡檢效果

6 結(jié)束語(yǔ)

開(kāi)發(fā)了一種基于STM32的軌道式巡檢機(jī)器人控制系統(tǒng)并對(duì)其進(jìn)行了測(cè)試。該系統(tǒng)采用STM32F407ZGT6為微處理器，內(nèi)嵌FreeRTOS操作系統(tǒng)。采用上下位機(jī)結(jié)構(gòu)并通過(guò)以太網(wǎng)通信實(shí)現(xiàn)遠(yuǎn)程無(wú)線信息交互。根據(jù)傳統(tǒng)軌道式巡檢機(jī)器人定位精度不足等缺點(diǎn)，提出了一種速度-位置雙模式匹配運(yùn)動(dòng)控制方法并設(shè)計(jì)了模糊控制器以提高機(jī)器人定位精度與響應(yīng)速度。實(shí)驗(yàn)測(cè)試表明該系統(tǒng)能夠精確、快速實(shí)現(xiàn)定位，能夠有效彌補(bǔ)人工作業(yè)的不足，提高了地下綜合管廊巡檢作業(yè)智能化程度。