汪名飞，丑武胜，樊嘉欣

1 引言

能源紧缺逐渐成为人类文明发展过程中的重要担忧问题之一，可再生能源是解决该问题的一个重要方向，核能发电作为能源升级的重要发展方向逐渐得到关注[1]。在核电站的日常维护以及故障救援过程中，由于核辐射对人体的危害，机器人成为代替人类进行危险环境下作业的工具之一。2011年，日本福岛核电站受地震发生事故后，机器人是首先进入核电站并进行辐射监测、执行相应的操作任务的[2]。

核环境下机器人完成任务需要通过远程遥操作实现，远程控制端发送指令给机器人，机器人反馈执行状态和环境信息。机器人执行任务的前提是定位与导航，机器人知道“在哪、去哪”和“怎么去”，才能去指定的位置完成相应的作业任务。机器人定位是根据传感器感知环境信息来确定自身在环境地图中的相对位置，分为绝对定位、相对定位和混合定位三种方式[3-4]。基于多传感器的混合定位方式结合了绝对定位和相对定位的优势，可以得到更精确可靠的定位。

设计的机器人系统是针对核聚变场合内巡检救援任务，不仅可以执行日常的环境监测、维护、巡检任务，也可以完成故障清除及救援任务。首先介绍了机器人系统的整体设计；其次，根据机器人的全向运动特性提出了一种机器人电磁巡线方法，介绍其原理及实现方案；然后，采用激光雷达和里程计、IMU（Inertial Measurement Unit）结合的多传感器融合混合定位方式，介绍了其基于粒子滤波的FastSLAM方法定位在ROS（Robot Operating System）上的实现过程；最后，进行了电磁导航巡线实验，并同时进行定位，证明了定位与电磁导航方法的可靠性和准确性。

2 机器人整体设计

2.1 系统概述

图1 多功能巡检救援机器人Fig.1 Multifunctional Inspection and Rescue Robot

多功能巡检与救援机器人系统原理样机，如图1所示。全向移动平台作为整个机器人系统的载体，以四个Mecanum轮实现平台的全向运动，具有负载能力强、运动灵活性高的特点[5]。多自由度作业机械臂是机器人的执行机构，末端设计为快换机构，可更换不同的机械手完成多种作业任务。巡检机械臂末端安装监视摄像头，主要功能为巡检监视环境状况和提供作业机械臂的全局作业视角。机器人四周布置了8个超声波传感器避免碰撞障碍物，底部安装四个电磁传感器用于导航巡线，前方装有激光雷达用于探测环境用于构建地图和定位。机器人系统的规格参数，如表1所示。

表1 机器人系统规格参数Tab.1 System Specification of the Robot

2.2 控制系统设计

图2 机器人控制系统框图Fig.2 Diagram of Robot Control System

机器人是用于核环境下的远程巡检与救援作业，多功能巡检与救援机器人整体系统控制框架，如图2所示。机器人通过控制台远程操作，控制台包括手持遥控器可、上位机、可穿戴式力反馈主手。机器人通过TCP/IP无线网络与控制台上位机通信，采用无线数传设备接收手动控制器的控制信息，并以无线图传设备及无线网络回传图像。

为满足机器人系统多种不同规格电源要求的设备，采用逆变器将锂电池48V直流转为220V交流强电，用直流稳压模块将其转为所需直流电压12V及24V。工控机作为机器人的主控制器，能够采集系统所以传感器信息及各模块设备状态信息，通过无线路由网络与工作台上位机通信，反馈机器人各模块状态信息并接收上位机控制指令，然后决策处理下发指令给各模块控制器。

3 电磁导航算法设计

相比于非全向移动方式，全向运动方式没有转弯半径，在狭小空间下具备优势[6]。为此，根据机器人独特的行走机构设计了直角转向的自动导航路径，机器人转向时没有转弯半径和姿态改变。机器人底部按“十”字布置四个电磁传感器分别检测水平方向与垂直方向导线，如图3所示。

图3 机器人与直导线相对位置状态图Fig.3 State Diagram of Relative Pose Between Robot and Straight Wire

机器人在直导线上行驶需要两个对角电磁传感器的数据反馈，当机器人与直线的状态如图3所示时，根据前后两电磁传感器得到与导线的距离d1，d2。由此可以计算机器人中心相对于导线的距离和姿态角分别为：

通过两个电磁传感器的数据即可得到机器人与导线的相对位置，以此为反馈采用PID控制方法控制机器人沿导线运动。

为了使机器人连续地沿着导线自主行驶，需要检测运动过程中的转角类型。我们采用状态机的表示方法来描述机器人的工作状态，分别有“向左”、“向右”、“向前”、“向后”、“待转”、“转向”、“停止”状态，各个状态切换的主要触发条件为转角的检测。

机器人在前进过程中遇到一个向右的转角，如图4所示。在转角之前，机器人处于“向前”的状态，机器人的自动行驶方法为上述走直线的控制算法。当机器人靠近转角时，由于交变电流在转角处产生的电磁信号会发生畸变，导致前传感器的检测值不能反映真实距离，故机器人走直线自动控制算法此时不可用。所以，机器人靠近转角并且右传感器检测到前方导线的距离小于一定值（前传感器未到电磁信号畸变处）时，状态切换为“待转”，该状态下机器人的短距离直线行走是靠后传感器来保证。当右传感器距离小于阈值（接近0）时，机器人开始右转，机器人运动状态切换到“转向”，同理，当后传感器检测到与导线距离大于阈值时，机器人运动状态切换为“向右”。

图4 机器人转向示意图Fig.4 Robot Turning Diagram

机器人经过转角是“直线—转角—直线”的过程，由此机器人在行驶中通过传感器数据来触发状态机的切换，并以直线状态下PID自动控制方法为基础，实现自主巡线导航，导航程序的状态机图,如图5所示。机器人启动时要求在直线处，而且在自主导航过程中遇到特殊情况会机器人会切换到“停止”状态，如超声波传感器检测到障碍物、电磁传感器返回值异常等，以确保机器人及作业环境安全。

图5 自主导航状态机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。

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

根据以上两式就可根据电机编码器值计算速度进而计算里程计。

图6 机器人运动分析建模Fig.6 Diagram for Movement Analysis and Modelling

4.2 IMU与里程计EKF融合

由里程计模型可以推出的状态转移方程的位置预测（Xk+1=f（Xk，Uk）为：

对其进行线性化推出EKF（Extend Kalman Filter）状态转移方程的雅克比增益矩阵为：

式中：Qk+1—假设里程计运动模型高斯噪声为qk+1的系统协方差矩阵。

由于机器人系统是在平面上运动，IMU模块输出为三个轴的姿态欧拉角及加速度，则机器人的姿态转角就是IMU输出的偏航角，机器人位置变化量可以将IMU在运动平面内两个轴加速度二次积分得到，故观测模型方程为：

其中Rk+1假设里观测模型高斯噪声为rk+1的系统协方差矩阵。

4.3 基于ROS的FastSLAM定位实现

机器人系统采用里程计、IMU和激光雷达多传感器融合的混合定位方式，首先通过EKF融合IMU及里程计信息，可以一定程度上减小里程计的累积误差，可以得到里程计测量u1：t-1，激光雷达传感器数据作为观测量z1：t。机器人的定位的核心是估计地图m和机器人运动轨迹x1：t，采用RBPF（Rao-Blackwellised Particle Fliter）方法[7]对机器人建图定位做如下分解：

这种分解将定位和建图分开，即先通过运动模型与观测量估计机器人的轨迹，再通过给定轨迹和观测量计算地图，这种采用RBPF方法粒子滤波解决SLAM问题方法称为FastSALM方法[8]。

采用了这种定位方法，根据IMU矫正的里程计信息和激光雷达观测量，基于ROS实现的FastSLAM开源框架[9]实现了机器人的定位。计算图级是ROS处理数据一种点对点的网络形式，程序运行时的所有进程及数据处理都会通过一种点对点的网络形式处理[10]，机器人定位系统ROS的节点消息关系图，如图7所示。图中主要节点的功能说明，如表2所示。

图7 机器人系统节点关系图Fig.7 Relation Graph of Robot System Nodes

表2 机器人系统节点功能说明Tab.2 Function Declaration of Robot System Nodes

5 导航定位实验

实验导航规划路线图，如图8（a）所示。其中，一条线为地下电磁导引线的指示线。机器人导航定位实验的另一视角场景图，通过手控控制机器人在该场景下通过传感器采集环境数据，离线生成地图，如图8（b）所示。

图8 导航与定位实验现场Fig.8 Experiment Site for Navigation and Localization

启动机器人自主巡线导航及定位程序，机器人会沿着导引线平移运动而不改变姿态，机器人导航过程中机器人的巡线情况，如图9所示。该过程中机器人与导引线偏差情况，如图10所示。导航偏差在2cm以内，转角换向时由于机器人的运动惯性会有偏差的增大。

图9 巡线结果Fig.9 Line-Tracking Result

图10 导航偏差Fig.10 Navigation Deviation

图11 机器人定位效果图Fig.11 Robot Localization Result

ROS中rviz显示的机器人导航过程中的运动轨迹，绿色箭头为代表机器人的位姿状态，如图11所示。箭头方向始终没有改变代表机器人在自主巡线过程中没有改变姿态，且地图中轨迹曲线与实际环境中规划的导航路线一致。建立的地图黑色为障碍物，白色为可通行区域，灰色为不确定区域，彩色的点集轮廓为激光雷达扫描的点，与所建地图轮廓重合代表定位非常准确。机器人在6个转角处实际坐标和定位坐标，计算的平均定位误差为5.5cm，如表3所示。

实验结果表明导航巡线及定位情况良好，验证了机器人导航定位算法的可行性和FastSLAM定位方法的准确性。

表3 定位误差计算Tab.3 Calculation of Localization Error

6 结论

设计了一种核环境下巡检救援的机器人系统，详细介绍了其系统构成、控制方式。提出了一种适用于全向机器人的电磁导航方法，导航偏差小于2cm。采用IMU修正里程计，在ROS上实现了FastSLAM定位方法，定位平均误差为5.5cm。研制的机器人具有良好导航定位功能，但由于机械臂作业精度要求更高，后续工作将针对精细作业任务展开研究。