汪名飛，丑武勝，樊嘉欣

1 引言

能源緊缺逐漸成為人類文明發(fā)展過程中的重要擔憂問題之一，可再生能源是解決該問題的一個重要方向，核能發(fā)電作為能源升級的重要發(fā)展方向逐漸得到關(guān)注[1]。在核電站的日常維護以及故障救援過程中，由于核輻射對人體的危害，機器人成為代替人類進行危險環(huán)境下作業(yè)的工具之一。2011年，日本福島核電站受地震發(fā)生事故后，機器人是首先進入核電站并進行輻射監(jiān)測、執(zhí)行相應的操作任務的[2]。

核環(huán)境下機器人完成任務需要通過遠程遙操作實現(xiàn)，遠程控制端發(fā)送指令給機器人，機器人反饋執(zhí)行狀態(tài)和環(huán)境信息。機器人執(zhí)行任務的前提是定位與導航，機器人知道“在哪、去哪”和“怎么去”，才能去指定的位置完成相應的作業(yè)任務。機器人定位是根據(jù)傳感器感知環(huán)境信息來確定自身在環(huán)境地圖中的相對位置，分為絕對定位、相對定位和混合定位三種方式[3-4]?；诙鄠鞲衅鞯幕旌隙ㄎ环绞浇Y(jié)合了絕對定位和相對定位的優(yōu)勢，可以得到更精確可靠的定位。

設計的機器人系統(tǒng)是針對核聚變場合內(nèi)巡檢救援任務，不僅可以執(zhí)行日常的環(huán)境監(jiān)測、維護、巡檢任務，也可以完成故障清除及救援任務。首先介紹了機器人系統(tǒng)的整體設計；其次，根據(jù)機器人的全向運動特性提出了一種機器人電磁巡線方法，介紹其原理及實現(xiàn)方案；然后，采用激光雷達和里程計、IMU（Inertial Measurement Unit）結(jié)合的多傳感器融合混合定位方式，介紹了其基于粒子濾波的FastSLAM方法定位在ROS（Robot Operating System）上的實現(xiàn)過程；最后，進行了電磁導航巡線實驗，并同時進行定位，證明了定位與電磁導航方法的可靠性和準確性。

2 機器人整體設計

2.1 系統(tǒng)概述

圖1 多功能巡檢救援機器人Fig.1 Multifunctional Inspection and Rescue Robot

多功能巡檢與救援機器人系統(tǒng)原理樣機，如圖1所示。全向移動平臺作為整個機器人系統(tǒng)的載體，以四個Mecanum輪實現(xiàn)平臺的全向運動，具有負載能力強、運動靈活性高的特點[5]。多自由度作業(yè)機械臂是機器人的執(zhí)行機構(gòu)，末端設計為快換機構(gòu)，可更換不同的機械手完成多種作業(yè)任務。巡檢機械臂末端安裝監(jiān)視攝像頭，主要功能為巡檢監(jiān)視環(huán)境狀況和提供作業(yè)機械臂的全局作業(yè)視角。機器人四周布置了8個超聲波傳感器避免碰撞障礙物，底部安裝四個電磁傳感器用于導航巡線，前方裝有激光雷達用于探測環(huán)境用于構(gòu)建地圖和定位。機器人系統(tǒng)的規(guī)格參數(shù)，如表1所示。

表1 機器人系統(tǒng)規(guī)格參數(shù)Tab.1 System Specification of the Robot

2.2 控制系統(tǒng)設計

圖2 機器人控制系統(tǒng)框圖Fig.2 Diagram of Robot Control System

機器人是用于核環(huán)境下的遠程巡檢與救援作業(yè)，多功能巡檢與救援機器人整體系統(tǒng)控制框架，如圖2所示。機器人通過控制臺遠程操作，控制臺包括手持遙控器可、上位機、可穿戴式力反饋主手。機器人通過TCP/IP無線網(wǎng)絡與控制臺上位機通信，采用無線數(shù)傳設備接收手動控制器的控制信息，并以無線圖傳設備及無線網(wǎng)絡回傳圖像。

為滿足機器人系統(tǒng)多種不同規(guī)格電源要求的設備，采用逆變器將鋰電池48V直流轉(zhuǎn)為220V交流強電，用直流穩(wěn)壓模塊將其轉(zhuǎn)為所需直流電壓12V及24V。工控機作為機器人的主控制器，能夠采集系統(tǒng)所以傳感器信息及各模塊設備狀態(tài)信息，通過無線路由網(wǎng)絡與工作臺上位機通信，反饋機器人各模塊狀態(tài)信息并接收上位機控制指令，然后決策處理下發(fā)指令給各模塊控制器。

3 電磁導航算法設計

相比于非全向移動方式，全向運動方式?jīng)]有轉(zhuǎn)彎半徑，在狹小空間下具備優(yōu)勢[6]。為此，根據(jù)機器人獨特的行走機構(gòu)設計了直角轉(zhuǎn)向的自動導航路徑，機器人轉(zhuǎn)向時沒有轉(zhuǎn)彎半徑和姿態(tài)改變。機器人底部按“十”字布置四個電磁傳感器分別檢測水平方向與垂直方向?qū)Ь€，如圖3所示。

圖3 機器人與直導線相對位置狀態(tài)圖Fig.3 State Diagram of Relative Pose Between Robot and Straight Wire

機器人在直導線上行駛需要兩個對角電磁傳感器的數(shù)據(jù)反饋，當機器人與直線的狀態(tài)如圖3所示時，根據(jù)前后兩電磁傳感器得到與導線的距離d1，d2。由此可以計算機器人中心相對于導線的距離和姿態(tài)角分別為：

通過兩個電磁傳感器的數(shù)據(jù)即可得到機器人與導線的相對位置，以此為反饋采用PID控制方法控制機器人沿導線運動。

為了使機器人連續(xù)地沿著導線自主行駛，需要檢測運動過程中的轉(zhuǎn)角類型。我們采用狀態(tài)機的表示方法來描述機器人的工作狀態(tài)，分別有“向左”、“向右”、“向前”、“向后”、“待轉(zhuǎn)”、“轉(zhuǎn)向”、“停止”狀態(tài)，各個狀態(tài)切換的主要觸發(fā)條件為轉(zhuǎn)角的檢測。

機器人在前進過程中遇到一個向右的轉(zhuǎn)角，如圖4所示。在轉(zhuǎn)角之前，機器人處于“向前”的狀態(tài)，機器人的自動行駛方法為上述走直線的控制算法。當機器人靠近轉(zhuǎn)角時，由于交變電流在轉(zhuǎn)角處產(chǎn)生的電磁信號會發(fā)生畸變，導致前傳感器的檢測值不能反映真實距離，故機器人走直線自動控制算法此時不可用。所以，機器人靠近轉(zhuǎn)角并且右傳感器檢測到前方導線的距離小于一定值（前傳感器未到電磁信號畸變處）時，狀態(tài)切換為“待轉(zhuǎn)”，該狀態(tài)下機器人的短距離直線行走是靠后傳感器來保證。當右傳感器距離小于閾值（接近0）時，機器人開始右轉(zhuǎn)，機器人運動狀態(tài)切換到“轉(zhuǎn)向”，同理，當后傳感器檢測到與導線距離大于閾值時，機器人運動狀態(tài)切換為“向右”。

圖4 機器人轉(zhuǎn)向示意圖Fig.4 Robot Turning Diagram

機器人經(jīng)過轉(zhuǎn)角是“直線—轉(zhuǎn)角—直線”的過程，由此機器人在行駛中通過傳感器數(shù)據(jù)來觸發(fā)狀態(tài)機的切換，并以直線狀態(tài)下PID自動控制方法為基礎，實現(xiàn)自主巡線導航，導航程序的狀態(tài)機圖,如圖5所示。機器人啟動時要求在直線處，而且在自主導航過程中遇到特殊情況會機器人會切換到“停止”狀態(tài)，如超聲波傳感器檢測到障礙物、電磁傳感器返回值異常等，以確保機器人及作業(yè)環(huán)境安全。

圖5 自主導航狀態(tài)機Fig.5 State Machine in Autonomous Navigation

4 基于多傳感器的機器人定位

4.1 里程計模型

建立機器人坐標系，如圖6所示。圖中：Or—機器人幾何中心；Or-XrYr—機器人坐標系；vx、vy、ωz—機器人的速度；ωi（i=1，2，3，4）—四個Mecanum輪的角速度；α—Mecanum主輪與輥子的軸線夾角；r—輪子半徑；e、d—機器人幾何中心到Mecanum輪中心坐標系方向的距離。對機器人進行運動學建模分析可得到運動學正解為：

式中：D=ecotα+d。

由此可以得到在全局坐標系下的里程計模型：

根據(jù)以上兩式就可根據(jù)電機編碼器值計算速度進而計算里程計。

圖6 機器人運動分析建模Fig.6 Diagram for Movement Analysis and Modelling

4.2 IMU與里程計EKF融合

由里程計模型可以推出的狀態(tài)轉(zhuǎn)移方程的位置預測（Xk+1=f（Xk，Uk）為：

對其進行線性化推出EKF（Extend Kalman Filter）狀態(tài)轉(zhuǎn)移方程的雅克比增益矩陣為：

式中：Qk+1—假設里程計運動模型高斯噪聲為qk+1的系統(tǒng)協(xié)方差矩陣。

由于機器人系統(tǒng)是在平面上運動，IMU模塊輸出為三個軸的姿態(tài)歐拉角及加速度，則機器人的姿態(tài)轉(zhuǎn)角就是IMU輸出的偏航角，機器人位置變化量可以將IMU在運動平面內(nèi)兩個軸加速度二次積分得到，故觀測模型方程為：

其中Rk+1假設里觀測模型高斯噪聲為rk+1的系統(tǒng)協(xié)方差矩陣。

4.3 基于ROS的FastSLAM定位實現(xiàn)

機器人系統(tǒng)采用里程計、IMU和激光雷達多傳感器融合的混合定位方式，首先通過EKF融合IMU及里程計信息，可以一定程度上減小里程計的累積誤差，可以得到里程計測量u1：t-1，激光雷達傳感器數(shù)據(jù)作為觀測量z1：t。機器人的定位的核心是估計地圖m和機器人運動軌跡x1：t，采用RBPF（Rao-Blackwellised Particle Fliter）方法[7]對機器人建圖定位做如下分解：

這種分解將定位和建圖分開，即先通過運動模型與觀測量估計機器人的軌跡，再通過給定軌跡和觀測量計算地圖，這種采用RBPF方法粒子濾波解決SLAM問題方法稱為FastSALM方法[8]。

采用了這種定位方法，根據(jù)IMU矯正的里程計信息和激光雷達觀測量，基于ROS實現(xiàn)的FastSLAM開源框架[9]實現(xiàn)了機器人的定位。計算圖級是ROS處理數(shù)據(jù)一種點對點的網(wǎng)絡形式，程序運行時的所有進程及數(shù)據(jù)處理都會通過一種點對點的網(wǎng)絡形式處理[10]，機器人定位系統(tǒng)ROS的節(jié)點消息關(guān)系圖，如圖7所示。圖中主要節(jié)點的功能說明，如表2所示。

圖7 機器人系統(tǒng)節(jié)點關(guān)系圖Fig.7 Relation Graph of Robot System Nodes

表2 機器人系統(tǒng)節(jié)點功能說明Tab.2 Function Declaration of Robot System Nodes

5 導航定位實驗

實驗導航規(guī)劃路線圖，如圖8（a）所示。其中，一條線為地下電磁導引線的指示線。機器人導航定位實驗的另一視角場景圖，通過手控控制機器人在該場景下通過傳感器采集環(huán)境數(shù)據(jù)，離線生成地圖，如圖8（b）所示。

圖8 導航與定位實驗現(xiàn)場Fig.8 Experiment Site for Navigation and Localization

啟動機器人自主巡線導航及定位程序，機器人會沿著導引線平移運動而不改變姿態(tài)，機器人導航過程中機器人的巡線情況，如圖9所示。該過程中機器人與導引線偏差情況，如圖10所示。導航偏差在2cm以內(nèi)，轉(zhuǎn)角換向時由于機器人的運動慣性會有偏差的增大。

圖9 巡線結(jié)果Fig.9 Line-Tracking Result

圖10 導航偏差Fig.10 Navigation Deviation

圖11 機器人定位效果圖Fig.11 Robot Localization Result

ROS中rviz顯示的機器人導航過程中的運動軌跡，綠色箭頭為代表機器人的位姿狀態(tài)，如圖11所示。箭頭方向始終沒有改變代表機器人在自主巡線過程中沒有改變姿態(tài)，且地圖中軌跡曲線與實際環(huán)境中規(guī)劃的導航路線一致。建立的地圖黑色為障礙物，白色為可通行區(qū)域，灰色為不確定區(qū)域，彩色的點集輪廓為激光雷達掃描的點，與所建地圖輪廓重合代表定位非常準確。機器人在6個轉(zhuǎn)角處實際坐標和定位坐標，計算的平均定位誤差為5.5cm，如表3所示。

實驗結(jié)果表明導航巡線及定位情況良好，驗證了機器人導航定位算法的可行性和FastSLAM定位方法的準確性。

表3 定位誤差計算Tab.3 Calculation of Localization Error

6 結(jié)論

設計了一種核環(huán)境下巡檢救援的機器人系統(tǒng)，詳細介紹了其系統(tǒng)構(gòu)成、控制方式。提出了一種適用于全向機器人的電磁導航方法，導航偏差小于2cm。采用IMU修正里程計，在ROS上實現(xiàn)了FastSLAM定位方法，定位平均誤差為5.5cm。研制的機器人具有良好導航定位功能，但由于機械臂作業(yè)精度要求更高，后續(xù)工作將針對精細作業(yè)任務展開研究。