具有升降云臺(tái)的軌道式巡檢機(jī)器人控制系統(tǒng)設(shè)計(jì)

范亞南，蘇冠明，王增光，張風(fēng)奇

(中國(guó)航空工業(yè)集團(tuán)公司洛陽(yáng)電光設(shè)備研究所，河南洛陽(yáng) 471009)

0 引言

海上升壓站室內(nèi)電氣設(shè)備(如變壓器、開(kāi)關(guān)柜、二次設(shè)備) 的穩(wěn)定運(yùn)行是保證海上風(fēng)電正常運(yùn)行的關(guān)鍵因素。目前人工巡檢的方式，存在效率低、巡檢間隔長(zhǎng)、容易漏檢等問(wèn)題，難以快速獲知海上設(shè)備的運(yùn)行狀態(tài)。除此之外，海上升壓站室內(nèi)設(shè)備的人工巡檢不能做到隨時(shí)隨地，運(yùn)維人員出海成本高、存在安全風(fēng)險(xiǎn)[1]。

鑒于海上升壓站人工巡檢的不足，使用軌道式機(jī)器人定時(shí)定點(diǎn)進(jìn)行對(duì)海上設(shè)備進(jìn)行日常檢查成為了研究熱點(diǎn)。針對(duì)傳統(tǒng)的軌道機(jī)器人不具備云臺(tái)升降功能，不能滿足對(duì)電控柜上下所有設(shè)備全面識(shí)別的問(wèn)題，本文設(shè)計(jì)了一種具有升降云臺(tái)的軌道式巡檢機(jī)器人控制系統(tǒng)，該控制系統(tǒng)具有可靠性高、安全性好、功能全面、定位精度高等優(yōu)點(diǎn)[2]。

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

軌道式機(jī)器人是巡檢系統(tǒng)的核心部分，通過(guò)配備可見(jiàn)光攝像機(jī)、熱成像儀、溫濕度傳感器、局放檢測(cè)儀等設(shè)備，對(duì)配電室的電氣設(shè)備、異常發(fā)熱、放電檢測(cè)等設(shè)備狀態(tài)進(jìn)行監(jiān)控，并可接受來(lái)自監(jiān)控后臺(tái)下發(fā)的巡檢任務(wù)執(zhí)行定時(shí)定點(diǎn)巡檢，上報(bào)告警提示，提高巡檢效率，節(jié)省成本[3]。 圖1為掛軌型巡檢機(jī)器人本體結(jié)構(gòu)。

圖1 掛軌型機(jī)器人結(jié)構(gòu)組成示意圖

軌道式巡檢機(jī)器人的控制系統(tǒng)可分為行走升降控制分系統(tǒng)(簡(jiǎn)稱WLCU)和云臺(tái)傳感控制分系統(tǒng)(簡(jiǎn)稱PSCU)，分別由2塊STM32F407微處理器及其最小系統(tǒng)、電機(jī)驅(qū)動(dòng)電路、A/D采樣、邏輯門電路以及以太網(wǎng)、RS232、RS485、SPI等通信電路組成[4]。機(jī)器人控制系統(tǒng)主要完成的功能為：接收上位機(jī)指令，并控制相應(yīng)模塊完成動(dòng)作響應(yīng)；解析編碼器信號(hào)完成電機(jī)的運(yùn)動(dòng)控制；采集各傳感器數(shù)據(jù)及系統(tǒng)運(yùn)行狀態(tài)數(shù)據(jù)并實(shí)時(shí)發(fā)送至上層用戶。其中傳感器主要包括：溫濕度傳感器、激光測(cè)距、局放檢測(cè)、電流檢測(cè)、軌壓檢測(cè)等；定位組件主要包括1024線增量編碼器、磁性開(kāi)關(guān)、條碼掃描儀、光電開(kāi)關(guān)等。供電模組包括24 V鋰電池12 V/5 V/3.3 V 3個(gè)等級(jí)的降壓模塊。

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

巡檢機(jī)器人具有1.0 m云臺(tái)升降功能，為了盡量減少機(jī)器人內(nèi)部線纜，本文設(shè)計(jì)整個(gè)控制系統(tǒng)按照電氣的布局，分為位于上部行走部分的WLCU系統(tǒng)和位于云臺(tái)內(nèi)部的PSCU系統(tǒng)，如圖2、圖3所示。2個(gè)分系統(tǒng)架構(gòu)基本一致，主要分為微處理器、電機(jī)驅(qū)動(dòng)電路、信號(hào)處理電路等[5]。

圖2 機(jī)器人行走升降控制分系統(tǒng)框圖

圖3 機(jī)器人云臺(tái)傳感分系統(tǒng)框圖

2.1 微處理器

控制系統(tǒng)采用STM32F407VET6微處理器，具有32位高性能ARM Cortex-M4處理器和高達(dá)168 MHz的時(shí)鐘系統(tǒng)，并擁有豐富的I/O資源。本設(shè)計(jì)中主要使用I/O有：高級(jí)定時(shí)器TIM1和TIM8提供的PWM互補(bǔ)輸出用于控制H橋驅(qū)動(dòng)電機(jī)；多個(gè)TIMx提供的編碼器輸入接口，完成電機(jī)的位置檢測(cè)和速度檢測(cè);由8路ADC構(gòu)成的電機(jī)電流、軌壓檢測(cè)、電池電量等模擬信號(hào)采集電路；RS232和RS485通信接口，分別與掃碼儀和局放檢測(cè)傳感器通信；SPI接口通信的激光測(cè)距模組；多個(gè)邏輯輸入輸出端口，主要控制狀態(tài)燈帶、LED照明繼電器、多個(gè)光電到位開(kāi)關(guān)、磁性零位開(kāi)關(guān)等；由RMII接口提供的100兆以太網(wǎng)通信接口。

2.2 電機(jī)驅(qū)動(dòng)電路

掛軌機(jī)器人共包含6個(gè)電機(jī)驅(qū)動(dòng)電路，在WLCU分系統(tǒng)中包含行走和升降電機(jī)驅(qū)動(dòng)，在PSCU分系統(tǒng)中包含三軸云臺(tái)和局放伸縮共4軸電機(jī)驅(qū)動(dòng)。

2.2.1 行走升降電機(jī)驅(qū)動(dòng)電路

本文采用VNH5019A-E全橋驅(qū)動(dòng)芯片，實(shí)現(xiàn)對(duì)機(jī)器人行走和升降控制[6]。該芯片具有高電壓大電流輸出能力和良好的散熱性能，是驅(qū)動(dòng)10 A左右的直流電機(jī)或電磁閥的首選，該H橋芯片使用簡(jiǎn)單，其INA和INB引腳可以直接與MCU相連，用于決定電機(jī)的轉(zhuǎn)動(dòng)方向和剎車動(dòng)作。

VNH5019A-E兼容CMOS電平輸入，實(shí)際應(yīng)用時(shí)在PWM輸入前使用SN74LVC1G08正與門G1增加了對(duì)PWM的使能控制和波形整定的功能，通過(guò)改變PWM占空比調(diào)節(jié)電機(jī)轉(zhuǎn)速。圖4中M表示直流電機(jī)，R13是用于檢測(cè)電樞電流的毫歐電阻，Q1為NMOS，用于電壓反向保護(hù)。本文只列了主要的引腳功能，在設(shè)計(jì)過(guò)程中，需要對(duì)各引腳做深入分析。

圖4 行走/升降電機(jī)驅(qū)動(dòng)電路

2.2.2 云臺(tái)電機(jī)驅(qū)動(dòng)電路

云臺(tái)電機(jī)驅(qū)動(dòng)電路由4個(gè)直流電機(jī)驅(qū)動(dòng)芯片DRV8842及其外圍電路組成，主要完成三軸云臺(tái)方位俯仰控制和局放傳感器伸縮控制[7]。

DRV8842是全橋式驅(qū)動(dòng)芯片，集成了邏輯控制電路、驅(qū)動(dòng)電路以及相應(yīng)的MOS驅(qū)動(dòng)，并自帶了過(guò)流保護(hù)、熱關(guān)斷、欠壓保護(hù)等保護(hù)功能。如圖5所示，通過(guò)PWM信號(hào)控制DRV8842的IN1和 IN2引腳來(lái)決定電機(jī)的運(yùn)轉(zhuǎn)方向和速度。該芯片可通過(guò)ISEN引腳檢測(cè)電機(jī)電流，實(shí)現(xiàn)電流斬波功能。當(dāng)電機(jī)電流達(dá)到設(shè)置值時(shí)，H橋斷開(kāi)，直到下一個(gè)PWM周期，斬波電流計(jì)算公式為：Ichop=VRef/(5·R18)，檢流電阻R18為50 mΩ。圖5為某一軸的電機(jī)驅(qū)動(dòng)電路。4路電機(jī)驅(qū)動(dòng)原理相同。

圖5 云臺(tái)電機(jī)驅(qū)動(dòng)電路

2.3 編碼器信號(hào)處理電路

編碼器信號(hào)處理電路如圖6所示，利用AM26LS32AC將正交增量式編碼器的差分信號(hào)轉(zhuǎn)換為單端信號(hào)，并通過(guò)SN74LVC2G14斯密特觸發(fā)器進(jìn)行脈沖信號(hào)整形后，輸入到STM32F407的TIM編碼器解析管腳上，并通過(guò)API讀取編碼器數(shù)據(jù)，進(jìn)而實(shí)現(xiàn)電機(jī)位置和速度的解算。

圖6 編碼器信號(hào)處理電路

2.4 電流采樣電路

電流采樣電路可獲取電樞電流方向和大小等信息，實(shí)現(xiàn)機(jī)器人各關(guān)節(jié)的過(guò)流保護(hù)功能。在電機(jī)回路中加入檢流電阻來(lái)對(duì)電樞電流進(jìn)行檢測(cè)。在5 A峰值電流時(shí)，檢流電阻兩端壓差僅0.1 V，遠(yuǎn)小于STM32的ADC滿量程3.3 V，需要將其進(jìn)行放大整形處理。采用傳統(tǒng)的多級(jí)分立運(yùn)放電路體積較大，且易受噪聲干擾，因此本文選用集成芯片INA240進(jìn)行電流采樣。

INA240是電壓輸出、電流檢測(cè)放大器，具有PWM抑制功能，對(duì)于PWM電調(diào)的瞬變(ΔV/Δt)具有較好抑制，非常適用于對(duì)電樞電流的精確測(cè)量?；贗NA226的電路原理圖如圖7所示。

圖7 電樞電流采樣電路

2.5 通信電路

通信電路包括以太網(wǎng)、RS232、RS4845、SPI等通信電路。

(1)以太網(wǎng)電路選用LAN8742A PHY收發(fā)器，具有10/100M自適應(yīng)、休眠、低功耗等功能，通過(guò)RMII接口連接到STM32F407處理器，實(shí)現(xiàn)外部的TCP/UDP通信。

(2)RS232通信電路，選用MAX3232ESE串口芯片。

(3)RS485通信電路選用低功耗半雙工的SN75HVD08通信芯片，傳輸速率高達(dá)200 Kbps，適合于低功耗或噪聲容限高的場(chǎng)合。

為避免半雙工RS485同時(shí)收發(fā)數(shù)據(jù)時(shí)相互干擾的情況，本文通過(guò)三極管D1電壓轉(zhuǎn)換將輸出引腳信號(hào)作為使能信號(hào)控制半雙工工作狀態(tài)，從而解決了收發(fā)互相干擾，其電路如圖8所示。

圖8 RS485通信電路

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

掛軌式巡檢機(jī)器人軟件包括機(jī)器人底層控制軟件、任務(wù)層主控軟件、上層任務(wù)監(jiān)控軟件等。

3.1 底層控制系統(tǒng)軟件設(shè)計(jì)

底層控制軟件主要接收上層用戶指令，并完成機(jī)器人各運(yùn)動(dòng)關(guān)節(jié)控制和傳感器信號(hào)采集。底層軟件使用STM32CubeMx工具進(jìn)行可視化管腳配置，生成基于HAL庫(kù)的底層驅(qū)動(dòng)代碼，并加入FreeRTOS系統(tǒng)，實(shí)現(xiàn)多線程資源管理、同步和線程通信。

在WLCU和PSCU中使用FreeRTOS實(shí)時(shí)操作系統(tǒng)可以滿足多電機(jī)和傳感器的控制需求，根據(jù)機(jī)器人各模塊的功能可在主控程序中創(chuàng)建響應(yīng)線程分別處理，合理配置各線程的優(yōu)先級(jí)順序和內(nèi)存大小。其中WLCU控制板，在主線程中創(chuàng)建了TCP通信、電機(jī)驅(qū)動(dòng)、行走控制、升降控制4個(gè)功能子線程，其程序流程圖如圖9所示；PSCU控制板，在主線程中創(chuàng)建了TCP通信、電機(jī)驅(qū)動(dòng)、云臺(tái)控制、局放伸縮、局放傳感5個(gè)子線程，其程序流程圖如圖10所示。

圖9 WLCU程序流程圖

圖10 PSCU程序流程圖

WLCU和PSCU程序啟動(dòng)后，先初始化各模塊，然后分別創(chuàng)建TCP 通信、電機(jī)驅(qū)動(dòng)線程，并各自創(chuàng)建機(jī)器人關(guān)節(jié)控制線程、傳感采集線程。其中，TCP線程會(huì)根據(jù)客戶端連接數(shù)量動(dòng)態(tài)創(chuàng)建通信子線程，接收并解析上位機(jī)指令，回傳傳感數(shù)據(jù)和系統(tǒng)狀態(tài)。2個(gè)控制板分別最大支持3個(gè)TCP客戶端連接。

電機(jī)驅(qū)動(dòng)線程主要運(yùn)行電機(jī)驅(qū)動(dòng)控制算法，通過(guò)解析電機(jī)速度、位置、電流信息，最終實(shí)現(xiàn)對(duì)電機(jī)的轉(zhuǎn)速閉環(huán)控制。關(guān)節(jié)控制線程包括行走、升降、云臺(tái)、伸縮線程，用于響應(yīng)用戶指令，實(shí)現(xiàn)不同關(guān)節(jié)的前/后、升/降、旋轉(zhuǎn)、伸/縮等操作。由于局放采集采用RS485通信，傳感器以狀態(tài)機(jī)模式工作，且局放圖譜數(shù)據(jù)大、時(shí)間長(zhǎng)，為避免阻塞其他任務(wù)，因此創(chuàng)建單獨(dú)的局放傳感線程用于處理局放數(shù)據(jù)。

3.2 電機(jī)驅(qū)動(dòng)調(diào)試軟件設(shè)計(jì)

電機(jī)驅(qū)動(dòng)調(diào)試軟件使用QT編寫，用于前期電機(jī)驅(qū)動(dòng)調(diào)試，具有電機(jī)驅(qū)動(dòng)參數(shù)顯示、配置、記錄等功能。軟件界面如圖11所示：界面上側(cè)從左到右依次是電機(jī)參數(shù)設(shè)定區(qū)、PID參數(shù)設(shè)定、IP端口設(shè)置、串口設(shè)置等；界面下側(cè)分別為WLCU和PSCU驅(qū)動(dòng)電機(jī)的電流、轉(zhuǎn)速、位置波形圖；通過(guò)網(wǎng)絡(luò)選擇配置，可滿足掛軌式機(jī)器人控制系統(tǒng)共6軸電機(jī)的驅(qū)動(dòng)參數(shù)配置與電機(jī)運(yùn)行調(diào)試，提高了控制系統(tǒng)軟件開(kāi)發(fā)效率。

3.3 功能調(diào)試軟件設(shè)計(jì)

上位機(jī)調(diào)試軟件使用python編寫，用于對(duì)掛軌機(jī)器人控制系統(tǒng)進(jìn)行功能測(cè)試。 “前期調(diào)試”的子界面如圖12所示，包含通信配置、行走電機(jī)、云臺(tái)、升降桿、局放控制等功能測(cè)試框。該軟件模擬上層用戶下發(fā)TCP指令給機(jī)器人，并解析和顯示機(jī)器人上報(bào)信息，可有效簡(jiǎn)化控制系統(tǒng)各硬件的功能調(diào)試工作。

圖12 掛軌機(jī)器人功能調(diào)試軟件

4 機(jī)器人各自由度的精確定位控制器設(shè)計(jì)

機(jī)器人在正常巡檢過(guò)程中，需要相機(jī)對(duì)目標(biāo)進(jìn)行準(zhǔn)確對(duì)準(zhǔn)。因此需要行走、升降、云臺(tái)的方位俯仰各軸的運(yùn)動(dòng)位置具有較高的準(zhǔn)確度。對(duì)于升降機(jī)構(gòu)、云臺(tái)機(jī)構(gòu)本文通過(guò)采用零位開(kāi)關(guān)配合光電編碼器的方式對(duì)各軸位置進(jìn)行定位，并通過(guò)PID閉環(huán)控制對(duì)升降機(jī)構(gòu)(優(yōu)于± 5mm)和3軸云臺(tái)(優(yōu)于±0.7°)進(jìn)行精確的位置控制。

對(duì)于行走的位置控制，傳統(tǒng)的軌道式巡檢機(jī)器人在定位方式上采用單獨(dú)里程計(jì)或配合RFID收發(fā)器進(jìn)行定位。單獨(dú)里程計(jì)定位時(shí)存在累積誤差，定位精度低；本文采用條形碼識(shí)別定位的方式對(duì)行走位置進(jìn)行修正，可解決累積誤差的問(wèn)題。采用條碼定位方式相對(duì)于RFID定位標(biāo)簽具有精度高、成本低等優(yōu)點(diǎn)。此外，在控制方法上，本文從對(duì)編碼器誤差的校正和控制算法兩方面提高機(jī)器人的定位精度與響應(yīng)速度。

4.1 速度-位置雙閉環(huán)的運(yùn)動(dòng)控制方法

機(jī)器人的控制系統(tǒng)包含行走、升降、左俯仰、右俯仰、方位旋轉(zhuǎn)、局放伸縮6個(gè)自由度。在對(duì)各關(guān)節(jié)進(jìn)行位置精確控制時(shí)：若速度過(guò)快、加速過(guò)猛時(shí)，會(huì)由于慣性作用和結(jié)構(gòu)原因造成結(jié)構(gòu)件過(guò)度磨損甚至損壞;若速度過(guò)慢，機(jī)器人目標(biāo)對(duì)準(zhǔn)過(guò)程過(guò)長(zhǎng)，實(shí)用性較差。因此，本文設(shè)計(jì)了速度-位置雙閉環(huán)控制器來(lái)實(shí)現(xiàn)行對(duì)各軸的調(diào)整速度跟隨位置誤差進(jìn)行相應(yīng)調(diào)整的功能。

速度-位置雙閉環(huán)控制系統(tǒng)如圖13所示，外環(huán)由目標(biāo)位置與機(jī)器人位置偏差構(gòu)成的反饋控制器，其輸出為參考速度。內(nèi)環(huán)為電機(jī)速度閉環(huán)控制，實(shí)現(xiàn)對(duì)外環(huán)控制器輸出的參考轉(zhuǎn)速的跟蹤控制。

圖13 速度-位置雙閉環(huán)的運(yùn)動(dòng)控制框圖

外環(huán)位置控制器根據(jù)機(jī)器人某關(guān)節(jié)當(dāng)前距離目標(biāo)位置的偏差設(shè)置加速、定速、減速3個(gè)過(guò)程，對(duì)各運(yùn)動(dòng)關(guān)節(jié)的速度進(jìn)行控制。以水平運(yùn)動(dòng)為例，機(jī)器人自動(dòng)巡檢運(yùn)行速度按照0.4 m/s進(jìn)行速度限幅，按照0.1 m/s2的加速度進(jìn)行速度調(diào)節(jié)。通過(guò)數(shù)學(xué)推理可知，當(dāng)距離誤差大于1.6 m時(shí)，運(yùn)行過(guò)程如圖14所示。當(dāng)距離誤差小于1.6 m時(shí)，按照前半程(deltaPostion/2)加速運(yùn)動(dòng)，后半程執(zhí)行減速過(guò)程。

圖14 位置控制器的速度輸出示意圖

4.2 分段PID控制器設(shè)計(jì)

由于機(jī)器人行走速度范圍(0～0.8 m/s)較大，對(duì)于傳統(tǒng)的PID控制器，很難同時(shí)兼顧整個(gè)速度范圍內(nèi)機(jī)器人速度控制的穩(wěn)定性。因此本文將位置型PID按照目標(biāo)速度將PID參數(shù)分段控制，從而實(shí)現(xiàn)了在全速度范圍內(nèi)對(duì)機(jī)器人速度的快速、穩(wěn)定控制[8]。

表1 分段PID參數(shù)表

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

為測(cè)試機(jī)器人功能，在某海上升壓站繼保室中，搭建了一條長(zhǎng)10 m的工字軌道，并每隔1 m設(shè)置1個(gè)定位條形碼。在機(jī)器人實(shí)際功能測(cè)試中，分別記錄機(jī)器人各關(guān)節(jié)的控制精度。實(shí)驗(yàn)結(jié)果表明，機(jī)器人行走精度優(yōu)于±10 mm，云臺(tái)各軸精度優(yōu)于±0.75°，升降精度小于±5 mm。在實(shí)際使用過(guò)程中，該機(jī)器人可自動(dòng)、高效地定時(shí)定點(diǎn)完成巡檢任務(wù)，實(shí)際作業(yè)現(xiàn)場(chǎng)如圖15所示。

圖15 掛軌型機(jī)器人運(yùn)行現(xiàn)場(chǎng)圖

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

開(kāi)發(fā)了一款具有云臺(tái)升降功能的掛軌式巡檢機(jī)器人控制系統(tǒng)。該系統(tǒng)采用模塊化設(shè)計(jì)理念，通過(guò)行走升降分系統(tǒng)和云臺(tái)傳感分系統(tǒng)完成整個(gè)機(jī)器人的6個(gè)自由度運(yùn)動(dòng)控制。各分控制系統(tǒng)使用STM32F4為微處理器，并集成電機(jī)驅(qū)動(dòng)、電流檢測(cè)、以太網(wǎng)等電路。通過(guò)FreeRTOS多線程處理，實(shí)現(xiàn)了雙閉環(huán)電機(jī)驅(qū)動(dòng)控制、傳感器采集、多連接TCPServer等功能，整個(gè)系統(tǒng)通過(guò)TCP協(xié)議實(shí)現(xiàn)數(shù)據(jù)信息交互。針對(duì)傳統(tǒng)軌道式機(jī)器人定位精度不足等缺點(diǎn)，提出了一種速度-位置雙閉環(huán)運(yùn)動(dòng)控制，并對(duì)速度環(huán)設(shè)計(jì)了分段PID控制器以提高機(jī)器人響應(yīng)速度和定位精度。實(shí)驗(yàn)測(cè)試表明該控制系統(tǒng)功能豐富、精準(zhǔn)穩(wěn)定，能夠長(zhǎng)期穩(wěn)定代替人工巡檢作業(yè)，提高了海上升壓站巡檢智能化水平，降低了運(yùn)維成本。