范亞南，蘇冠明，王增光，張風奇

(中國航空工業(yè)集團公司洛陽電光設備研究所，河南洛陽 471009)

0 引言

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

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

1 機器人控制系統(tǒng)總體設計

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

圖1 掛軌型機器人結構組成示意圖

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

2 系統(tǒng)硬件設計

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

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

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

2.1 微處理器

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

2.2 電機驅動電路

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

2.2.1 行走升降電機驅動電路

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

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

圖4 行走/升降電機驅動電路

2.2.2 云臺電機驅動電路

云臺電機驅動電路由4個直流電機驅動芯片DRV8842及其外圍電路組成，主要完成三軸云臺方位俯仰控制和局放傳感器伸縮控制[7]。

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

圖5 云臺電機驅動電路

2.3 編碼器信號處理電路

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

圖6 編碼器信號處理電路

2.4 電流采樣電路

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

INA240是電壓輸出、電流檢測放大器，具有PWM抑制功能，對于PWM電調的瞬變(ΔV/Δt)具有較好抑制，非常適用于對電樞電流的精確測量。基于INA226的電路原理圖如圖7所示。

圖7 電樞電流采樣電路

2.5 通信電路

通信電路包括以太網、RS232、RS4845、SPI等通信電路。

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

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

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

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

圖8 RS485通信電路

3 控制系統(tǒng)軟件設計

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

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

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

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

圖9 WLCU程序流程圖

圖10 PSCU程序流程圖

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

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

3.2 電機驅動調試軟件設計

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

3.3 功能調試軟件設計

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

圖12 掛軌機器人功能調試軟件

4 機器人各自由度的精確定位控制器設計

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

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

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

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

速度-位置雙閉環(huán)控制系統(tǒng)如圖13所示，外環(huán)由目標位置與機器人位置偏差構成的反饋控制器，其輸出為參考速度。內環(huán)為電機速度閉環(huán)控制，實現(xiàn)對外環(huán)控制器輸出的參考轉速的跟蹤控制。

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

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

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

4.2 分段PID控制器設計

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

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

5 機器人樣機試驗結果

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

圖15 掛軌型機器人運行現(xiàn)場圖

6 結束語

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