基于ROS系統(tǒng)的變電站智能巡檢機(jī)器人建圖與控制

劉培焱，劉豐碩，沈 瑤，孫 敏

（西安交通大學(xué) 電工電子教學(xué)實(shí)驗(yàn)中心，陜西 西安 710049）

0 引 言

電力系統(tǒng)變電站是電力傳輸與分配的樞紐，變電站設(shè)備的可靠運(yùn)行，直接影響到輸變電站生產(chǎn)安全?，F(xiàn)在電力系統(tǒng)的規(guī)模及巡檢環(huán)境復(fù)雜性不斷增加，巡檢任務(wù)強(qiáng)度也不斷上升，巡檢內(nèi)容變得更加復(fù)雜，信息化對(duì)變電站巡檢工作的數(shù)據(jù)保存以及實(shí)時(shí)評(píng)估等方面的要求也越來越高[1]。隨著計(jì)算機(jī)技術(shù)、信息通信技術(shù)及人工智能技術(shù)的不斷發(fā)展，機(jī)器人智能化巡檢系統(tǒng)逐漸進(jìn)入人們的視野，許多變電站逐步引入智能機(jī)器人代替人工巡檢。智能巡檢機(jī)器人能夠?qū)崿F(xiàn)全天候不間斷巡檢，一方面有效提高巡檢質(zhì)量、降低人工勞動(dòng)強(qiáng)度和安全風(fēng)險(xiǎn)[2]，另一方面提高了變電站工作的自動(dòng)化程度，減少了電力系統(tǒng)現(xiàn)場(chǎng)運(yùn)維人員的投入。

隨著技術(shù)的不斷發(fā)展，智能監(jiān)測(cè)系統(tǒng)不斷更新，功能不斷完善，其必在不久的將來完全取代傳統(tǒng)的巡檢方式，在巡檢工作中發(fā)揮巨大的作用。提升智能巡檢系統(tǒng)的巡檢效率和完善后臺(tái)程序是未來智能巡檢系統(tǒng)明確的發(fā)展方向[3]。此外，變電站智能巡檢機(jī)器人契合“信息化、數(shù)字化、自動(dòng)化、互動(dòng)化”為特征的智能電網(wǎng)建設(shè)的深入，實(shí)現(xiàn)“互聯(lián)網(wǎng)+”信息化模式下的智能巡檢將推動(dòng)“智能電網(wǎng)”建設(shè)。

本文以樹莓派4B 為上位機(jī)，STM32F103RCT6 為主控芯片，設(shè)計(jì)了一款用于變電站智能巡檢的兩輪差速機(jī)器人。為了實(shí)現(xiàn)智能巡檢，機(jī)器人應(yīng)能夠自主運(yùn)行并規(guī)劃路徑，本文重點(diǎn)研究了智能機(jī)器人主體搭建、速度閉環(huán)控制及ROS系統(tǒng)中的建圖和導(dǎo)航功能，為實(shí)現(xiàn)機(jī)器人智能巡檢奠定基礎(chǔ)。

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

在機(jī)器人硬件設(shè)計(jì)方案中采用分立模塊進(jìn)行設(shè)計(jì)，圖1所示為控制系統(tǒng)硬件設(shè)計(jì)框圖，主要模塊包括：

圖1 控制電路硬件設(shè)計(jì)框圖

（1）樹莓派4B，安裝Ubuntu 20.04 系統(tǒng)，并在Ubuntu系統(tǒng)上安裝ROS Noetic 系統(tǒng)，用以實(shí)現(xiàn)地圖構(gòu)建和導(dǎo)航。

（2）激光雷達(dá)，與樹莓派配合實(shí)現(xiàn)地圖構(gòu)建。

（3）主控板STM32F103RCT6，用于控制機(jī)器人運(yùn)動(dòng)，獲取各種傳感器信息，如航向角MPU6050、藍(lán)牙和編碼器信號(hào)。

（4）帶編碼器電機(jī)和電機(jī)驅(qū)動(dòng)，電機(jī)驅(qū)動(dòng)A4950 接收主控板STM32 的PWM 信號(hào)，驅(qū)動(dòng)電機(jī)旋轉(zhuǎn)，同時(shí)電機(jī)輸出正交編碼信號(hào)，用于測(cè)速。

智能巡檢機(jī)器人系統(tǒng)所包含的硬件模塊如圖2所示，分別是機(jī)器人車體、車輪、帶編碼器電機(jī)及驅(qū)動(dòng)、STM32 單片機(jī)、安裝Ubuntu 20.04+ROS Noetic 系統(tǒng)的樹莓派和各種傳感器。

圖2 系統(tǒng)硬件模塊

各個(gè)模塊的具體參數(shù)和功能如下：

（1）上位機(jī)。本項(xiàng)目選用樹莓派4B 作為上位機(jī)。樹莓派（Raspberry Pi）是一款基于ARM 的微型計(jì)算機(jī)主板，以SD/MicroSD 卡為內(nèi)存硬盤，主板周圍有4 個(gè)USB 接口和1 個(gè)10/100 以太網(wǎng)接口，具備PC 的基本功能。在本項(xiàng)目中，樹莓派4B安裝Ubuntu 20.04和ROS Noetic系統(tǒng)，用于建圖和導(dǎo)航。

（2）單片機(jī)控制板。STM32F1 系列微控制器是當(dāng)前使用最為廣泛的單片機(jī)，具有豐富的接口資源?；赟TM32CubeMX[4]的圖形化開發(fā)環(huán)境為學(xué)習(xí)者提供了極大的便利，提高了開發(fā)效率。本項(xiàng)目使用的主控芯片是STM32F103RCT6，優(yōu)點(diǎn)是輕便、簡(jiǎn)單，包含基本電源模塊和USB 接口，芯片的所有引腳均引出，易于開發(fā)。

（3）車體選擇。本項(xiàng)目選擇金屬結(jié)構(gòu)的兩輪差速機(jī)器人，該機(jī)器人共有3 個(gè)輪，前端一個(gè)全向輪，后端兩個(gè)驅(qū)動(dòng)輪，通過兩個(gè)驅(qū)動(dòng)輪的差速配合可以實(shí)現(xiàn)前進(jìn)、后退、轉(zhuǎn)彎等運(yùn)動(dòng)。電機(jī)為帶霍爾編碼器的直流減速電機(jī)。編碼器能夠?qū)㈦姍C(jī)的轉(zhuǎn)動(dòng)信息轉(zhuǎn)換為脈沖信號(hào)，采用正交編碼，當(dāng)電機(jī)旋轉(zhuǎn)時(shí)，編碼器輸出兩路相位相差為90°的脈沖信號(hào)，通過檢測(cè)兩路脈沖信號(hào)的相位關(guān)系可以確定電機(jī)的旋轉(zhuǎn)方向，可用于測(cè)速。

（4）藍(lán)牙模塊。藍(lán)牙技術(shù)是取代數(shù)據(jù)電纜的短距離無線通信技術(shù)[5]。藍(lán)牙模塊是指集成藍(lán)牙功能的基本電路集合，用于無線網(wǎng)絡(luò)通信。本項(xiàng)目使用藍(lán)牙HC-05 模塊與單片機(jī)串口實(shí)現(xiàn)數(shù)據(jù)交互，在智能手機(jī)端安裝SPP 藍(lán)牙助手，實(shí)現(xiàn)機(jī)器人遠(yuǎn)程控制。

（5）MPU6050 模塊。MPU6050 內(nèi)部整合了3 軸陀螺儀和3 軸加速度傳感器，利用自帶的數(shù)字運(yùn)動(dòng)處理器（Digital Motion Processor,DMP）硬件加速引擎，通過I2C 接口，向單片機(jī)輸出完整的9 軸融合演算數(shù)據(jù)。本項(xiàng)目中用于為ROS機(jī)器人的建圖導(dǎo)航提供航向角數(shù)據(jù)。

（6）激光雷達(dá)。激光雷達(dá)是以發(fā)射激光束探測(cè)目標(biāo)的位置、速度等特征量的雷達(dá)系統(tǒng)。本項(xiàng)目所使用的激光雷達(dá)型號(hào)為思嵐A1，在ROS 系統(tǒng)中運(yùn)行Rviz 可視化界面即可得到激光雷達(dá)建立的點(diǎn)云模型，配合機(jī)器人的運(yùn)動(dòng)數(shù)據(jù)以及ROS建圖功能包可以實(shí)時(shí)構(gòu)建地圖信息。

各硬件模塊相互聯(lián)系和配合，構(gòu)成如圖3所示兩輪差速智能巡檢機(jī)器人的整體結(jié)構(gòu)。

圖3 兩輪差速機(jī)器人車體

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

2.1 機(jī)器人運(yùn)動(dòng)控制

根據(jù)各模塊的特點(diǎn)，先列出各硬件模塊所需要的資源，見表1所列。

表1 單片機(jī)硬件資源需求

根據(jù)硬件模塊資源需求及主控芯片各引腳的功能，對(duì)STM32CubeMX 中主控芯片的管腳進(jìn)行分配。在分配好管腳之后，就可以開始對(duì)各模塊進(jìn)行測(cè)試。

機(jī)器人運(yùn)動(dòng)的軟件控制框圖如圖4所示。上電啟動(dòng)后，先對(duì)定時(shí)器和串口進(jìn)行初始化配置，如啟動(dòng)定時(shí)器TIM6開始定時(shí)，開啟UART4 接收手機(jī)藍(lán)牙信號(hào)。當(dāng)手機(jī)藍(lán)牙發(fā)送機(jī)器人運(yùn)動(dòng)命令后，機(jī)器人開始運(yùn)動(dòng)，同時(shí)在TIM6的定時(shí)中斷服務(wù)函數(shù)中讀取編碼器數(shù)據(jù)，進(jìn)行速度PID控制。

圖4 機(jī)器人系統(tǒng)軟件控制框圖

2.2 速度PID 控制

在自動(dòng)控制系統(tǒng)中，PID 控制器是最為廣泛應(yīng)用的一種控制方法，它具有原理簡(jiǎn)單、參數(shù)整定簡(jiǎn)便和易于實(shí)現(xiàn)的優(yōu)點(diǎn)。PID 算法采用閉環(huán)控制，通過比例、積分和微分控制有效地糾正被控制對(duì)象的偏差，從而使其達(dá)到一個(gè)穩(wěn)定的狀態(tài)。

本系統(tǒng)的控制目標(biāo)是控制機(jī)器人兩個(gè)電機(jī)的速度一致，以便構(gòu)建地圖。由于電機(jī)轉(zhuǎn)速與電機(jī)外加電壓（即PWM 波占空比）的大小成正比，這就構(gòu)成了PID 調(diào)節(jié)的基礎(chǔ)。速度閉環(huán)控制的方法是，通過編碼器檢測(cè)單位時(shí)間內(nèi)的脈沖數(shù)來測(cè)量電機(jī)的轉(zhuǎn)速，并與設(shè)定的轉(zhuǎn)速目標(biāo)值對(duì)比，得到控制偏差；通過對(duì)控制偏差進(jìn)行比例、積分和微分控制，得到PWM 波脈寬的增量，根據(jù)PWM 波脈寬的增量重新計(jì)算并更新PWM，實(shí)現(xiàn)機(jī)器人速度閉環(huán)控制的目的。

模擬PID 控制器的數(shù)學(xué)模型可以用式（1）的微分方程表示：

式中：u(t)是t時(shí)刻調(diào)節(jié)器的輸出；Kp是比例系數(shù)；e(t)是調(diào)節(jié)器的輸入；Ti是積分時(shí)間常數(shù)；Td是微分時(shí)間常數(shù)。

由于單片機(jī)只能識(shí)別數(shù)字量，不能對(duì)連續(xù)的控制量直接進(jìn)行計(jì)算，而且得到的速度值也為離散值，故必須對(duì)式（1）的PID 算式進(jìn)行離散化處理。采用數(shù)字式的差分方程來代替連續(xù)系統(tǒng)的微分方程，得到增量式離散PID 公式：

式中：Kp、Ki和Kd分別稱為比例、積分和微分系數(shù)；e(k)為本次偏差；e(k-1)為上一次偏差；e(k-2)為上上次的偏差[6-8]。

要對(duì)速度實(shí)現(xiàn)PID 閉環(huán)控制算法，首先要測(cè)量速度，在本項(xiàng)目中根據(jù)測(cè)量單位時(shí)間內(nèi)編碼器輸出的脈沖信號(hào)個(gè)數(shù)來計(jì)算速度。主控芯片的定時(shí)器自帶編碼器接口模式，極大地簡(jiǎn)化了速度測(cè)量過程。由表1可知，選擇定時(shí)器TIM2 和TIM3 為編碼器接口模式，分別測(cè)量?jī)蓚€(gè)車輪的轉(zhuǎn)速。定時(shí)器TIM6 每隔10 ms 產(chǎn)生定時(shí)中斷，在中斷函數(shù)中，首先獲取當(dāng)前編碼器的數(shù)值，根據(jù)式（2）對(duì)當(dāng)前獲取的編碼器速度值與設(shè)定的目標(biāo)值的偏差進(jìn)行PID 運(yùn)算，得到每個(gè)輪子的PWM 脈寬增量，在定時(shí)器TIM6 的定時(shí)中斷函數(shù)中更新每個(gè)輪子的PWM 脈寬，實(shí)現(xiàn)實(shí)時(shí)調(diào)節(jié)速度。

在PID 算法中，調(diào)節(jié)Kp、Ki和Kd系數(shù)是非常關(guān)鍵的。在本系統(tǒng)中，通過多次實(shí)際測(cè)試，最終選擇Kp=8、Ki=1 和Kd=0。表2所示為對(duì)PID 算法的測(cè)試情況，由表2可知，本文所用PID 算法可以實(shí)現(xiàn)對(duì)差速機(jī)器人的速度控制。

表2 速度PID 算法測(cè)試

2.3 基于ROS 的地圖構(gòu)建與導(dǎo)航

ROS（Robot Operating System）是一個(gè)用于編寫機(jī)器人軟件的靈活框架，它集成了大量的工具、庫、協(xié)議，提供了類似操作系統(tǒng)所提供的功能，可以極大簡(jiǎn)化繁雜多樣的機(jī)器人平臺(tái)下的復(fù)雜任務(wù)創(chuàng)建與穩(wěn)定行為控制[9]。在ROS 系統(tǒng)中，用戶可以將實(shí)現(xiàn)特定功能的程序和啟動(dòng)文件放入同一文件夾中，交由ROS 系統(tǒng)檢查編譯，使用時(shí)通過相應(yīng)的ROS 命令啟動(dòng)文件夾中的程序，即可實(shí)現(xiàn)對(duì)應(yīng)的功能。該文件夾則稱為一個(gè)功能包。

為實(shí)現(xiàn)智能巡檢機(jī)器人的地圖構(gòu)建與導(dǎo)航功能，本文基于ROS 系統(tǒng)，編寫了多個(gè)功能包。巡檢小車的功能實(shí)現(xiàn)過程如下：

（1）串口通信。為了實(shí)現(xiàn)智能機(jī)器人的地圖構(gòu)建和SLAM 導(dǎo)航功能，ROS 系統(tǒng)需要不斷與STM32 單片機(jī)之間通信，在ROS 系統(tǒng)中編寫串口通信的功能包serial_demo，通過串口線連接樹莓派和STM32 進(jìn)行通信。機(jī)器人每隔10 ms 將運(yùn)動(dòng)信息例如左右輪速度、航向角等發(fā)送給ROS 系統(tǒng)，同時(shí)ROS 也可以給機(jī)器人發(fā)送運(yùn)行指令。

（2）地圖構(gòu)建。為了實(shí)現(xiàn)智能巡檢，首先需要構(gòu)建一張巡檢區(qū)域的地圖，為機(jī)器人路徑規(guī)劃做準(zhǔn)備。構(gòu)建地圖需要機(jī)器人獲取自身所處的環(huán)境信息。本項(xiàng)目采用激光雷達(dá)作為傳感器，通過rplidar_ros 功能包控制激光雷達(dá)工作。在該功能包的launch 文件中設(shè)置激光雷達(dá)的掃描參數(shù)和掃描范圍。啟動(dòng)launch 文件后，激光雷達(dá)開始對(duì)環(huán)境中的障礙物進(jìn)行掃描，同時(shí)發(fā)布掃描到的環(huán)境信息。

在構(gòu)建地圖時(shí)，可以運(yùn)用ROS 系統(tǒng)中的開源建圖功能包hector 和gmapping。為滿足工程需求，保證導(dǎo)航精度，本項(xiàng)目采用gmapping 功能包進(jìn)行地圖構(gòu)建。使用gmapping功能包時(shí)，首先需要獲取機(jī)器人的里程計(jì)信息。本文通過serial_demo 功能包中的C++節(jié)點(diǎn)文件serial_demo.cpp 接收STM32 發(fā)送的運(yùn)動(dòng)信息，并通過C++節(jié)點(diǎn)文件odom_pub.cpp 對(duì)運(yùn)動(dòng)信息進(jìn)行整合，從而將其轉(zhuǎn)換為里程計(jì)信息并發(fā)布，使里程計(jì)信息可以被gmapping 功能包識(shí)別運(yùn)用。

地圖構(gòu)建功能的實(shí)現(xiàn)則通過mbot_navigation 功能包完成。首先在該功能包中編寫啟動(dòng)gmapping建圖的launch文件，依次編寫調(diào)用serial_demo功能包中的C++節(jié)點(diǎn)文件serial_demo.cpp 和odom_pub.cpp、加載機(jī)器人模型參數(shù)、對(duì)地圖坐標(biāo)系和機(jī)器人坐標(biāo)系進(jìn)行TF 變換、啟動(dòng)激光雷達(dá)和加載Rviz 可視化界面的命令，最后運(yùn)行該launch 文件，即可開始構(gòu)建地圖。圖5為機(jī)器人在實(shí)驗(yàn)室環(huán)境下，實(shí)時(shí)構(gòu)建的地圖。

圖5 機(jī)器人在實(shí)驗(yàn)室構(gòu)建的地圖

（3）導(dǎo)航。在變電站中，機(jī)器人的自主導(dǎo)航功能十分重要，它可以幫助機(jī)器人自動(dòng)運(yùn)行至指定位置，并保持正確的姿態(tài)，便于進(jìn)行下一步巡檢工作。本文運(yùn)用move_base 功能包實(shí)現(xiàn)智能機(jī)器人的導(dǎo)航功能。該功能包允許用戶在rviz可視化界面中，根據(jù)所建立的地圖選擇智能機(jī)器人的目標(biāo)位置和方向。move_base功能包在運(yùn)行過程中會(huì)不斷發(fā)布Twist消息，經(jīng)過C++節(jié)點(diǎn)文件odom_pub.cpp處理后，Twist消息可以轉(zhuǎn)換為運(yùn)動(dòng)控制指令，并通過serial_demo.cpp將其發(fā)送至下位機(jī)STM32 中，從而控制機(jī)器人運(yùn)動(dòng)至指定位置并保持正確的方向。

啟動(dòng)導(dǎo)航功能時(shí)，需要在mbot_navigation 功能包的launch文件中加入運(yùn)行move_base功能包對(duì)應(yīng)的launch文件。導(dǎo)航時(shí)，在Rviz 視圖中已建立的地圖上選取目標(biāo)位置和方向，系統(tǒng)就會(huì)自動(dòng)規(guī)劃路徑，并通過串口實(shí)時(shí)交換機(jī)器人的運(yùn)動(dòng)信息和運(yùn)動(dòng)控制指令，控制智能機(jī)器人自動(dòng)導(dǎo)航至指定位置，并保持正確的位姿[10-12]，如圖6所示。

圖6 機(jī)器人自動(dòng)導(dǎo)航至指定位置

3 結(jié) 語

本項(xiàng)目是大學(xué)生創(chuàng)新訓(xùn)練項(xiàng)目的設(shè)計(jì)成果，以電力系統(tǒng)變電站智能巡檢為背景，設(shè)計(jì)了一款可在室內(nèi)進(jìn)行實(shí)時(shí)建圖與導(dǎo)航的智能機(jī)器人，主要研究成果包括：

（1）搭建了智能機(jī)器人系統(tǒng)。自行組裝兩輪差速機(jī)器人底盤，整合電源模塊、穩(wěn)壓模塊、主控板STM32 和傳感器構(gòu)建了機(jī)器人硬件平臺(tái)。

（2）速度閉環(huán)控制。采用霍爾編碼器功能測(cè)量轉(zhuǎn)速，用PID 算法實(shí)現(xiàn)速度閉環(huán)控制，保證兩輪速度一致，為構(gòu)建地圖打下基礎(chǔ)。

（3）樹莓派安裝Ubuntu+ROS 環(huán)境，配合激光雷達(dá)和SLAM 算法實(shí)現(xiàn)實(shí)時(shí)地圖構(gòu)建與導(dǎo)航。

本項(xiàng)目所設(shè)計(jì)的智能機(jī)器人，為變電站的智能巡檢工作奠定了基礎(chǔ)，在未來將繼續(xù)增加儀表識(shí)別功能，實(shí)現(xiàn)真正的智能巡檢工作。該項(xiàng)目為學(xué)生提供了一個(gè)良好的實(shí)踐鍛煉機(jī)會(huì)，激發(fā)了學(xué)生的學(xué)習(xí)興趣，增強(qiáng)了學(xué)生自信心，提高了學(xué)生解決工程問題的能力和創(chuàng)新能力。