护士助手机器人多传感器数据融合定位导航系统研究

陈再胜，王颜霞

（商丘工学院机械工程学院，河南 商丘 476000）

随着传感器技术和计算机技术的不断发展，传统的单一传感器信息采集和多传感器单独信息采集已不能满足现阶段高精度、高稳定性的机器人市场需求，尤其是在医院室内移动的护士助手机器人[1]。采用计算机算法将多种传感器数据进行融合获取机器人位姿，并实现定位导航，经过多学者[2-3]研究，能相对提高精度，并不断应用于移动机器人、智能汽车、自动驾驶等领域。

本文在前人[4-5]的基础上，根据项目条件，研究了一种多传感器数据融合定位导航系统，主要应用于室内移动的护士助手机器人，其承载能力30kg，最大移动速度υmax为0.6m/s，可实现自主导航、避障、充电和物品运送等[6]。该定位导航系统采用轮式里程计和惯性测量单元IMU 通过EKF 算法实现机器人较为粗糙的实时位姿和速度，采用激光雷达通过ROS 服务包构建环境地图，采用单目相机校准机器人实时位姿，通过自适应蒙特卡洛定位（AMCL）算法实现机器人精准定位及导航。本定位导航系统为护士助手机器人等室内环境下移动的机器人实现高精度定位导航提供了一种技术方案。

1 定位导航系统设计

本护士助手机器人以ROS 开源系统为基础，树莓派为上位机，向下位机分发节点命令，并接收下位机获取的位姿信息，Arduino 控制板控制底层控制系统，惯性测量单元IMU 和轮式里程计作为内部传感器，激光雷达和单目相机作为外部传感器，配合上位机完成机器人SLAM 和路径规划系统的实现。护士助手机器人定位系统主要包括以下几个方面。

1.1 底盘控制系统

该控制系统主要用于控制机器人的运动速度。首先，接收ROS速度命令，驱动电机按初始速度旋转；其次，控制器接收ROS 速度或角度命令，控制电机基本运动。同时，在该ROS 功能包中读取电机的运行速度，将其转换为行驶里程数据，并反馈到树莓派上位机。

1.2 SLAM（即时定位和地图构建）系统

本系统主要利用ROS 框架中的SLAM 功能包实现。首先，激光雷达通过360°扫描可检测到周围环境障碍物的距离信息；其次，树莓派通过该功能包将接收到的里程计数据、惯性传感器数据和雷达数据进行融合处理，从而创建机器人实时位姿和室内环境图。

1.3 路径规划系统

首先，操作者通过可视化界面规定机器人目标点，输入至树莓派控制中心；其次，机器人根据给定的目标点，在SLAM 全局地图中，通过A*算法进行全局路径的规划，计算出机器人到目标点的最优路线；对于障碍物，通过局部路径规划算法和模糊控制理论选取最优避障路径，避免与动态出现的障碍物发生碰撞，最终使机器人到达目标点。

2 SLAM 系统实现

2.1 多传感器数据融合即时定位

本护士助手机器人通过AMCL 算法获得机器人的实时位置和姿态，并将单目相机获得的初始位姿与之加权平均获得机器人即时位姿。

AMCL 算法是一种具有较高鲁棒性的护士助手机器人定位算法，其核心算法是基于概率模型的粒子滤波算法。首先，底盘控制系统中的轮式里程计作为内部传感器，通过航迹推算法基于光电编码器的脉冲变化量推算出机器人的位姿、角/线速度实时变化数据，惯性测量单元IMU 采用惯性传感器MPU6050 测算出机器人的角/线速度实时变化数据，两种数据通过EKF 算法推算出机器人实时位置的坐标和姿态数据，并与机器人基坐标系进行坐标变换后得到机器人即时位姿数据A；激光雷达作为外部传感器，通过slam_gmapping地图包构建全局地图，获得机器人周围障碍物的距离数据B；单目相机作为外部传感器，获得室内环境初始距离图像和机器人运行过程中的实时距离图像，经过图像处理和位姿估计后得到机器人位姿数据信息C；数据A、数据B 和数据C 通过AMCL 算法创建概率模型，并经测算后得到机器人的即时位姿，同时与单目相机获得的位姿数据进行加权平均，最后获得较为准确的机器人即时位姿信息。具体数据融合流程如图1 所示。

图1 多传感器融合流程图

2.2 SLAM 算法实时地图构建

本护士助手机器人选择粒子滤波的SLAM方法（Fast-SLAM）完成地图构建。Fast-SLAM 算法利用扩展卡尔曼滤波算法（EKF）进行状态估计，利用粒子滤波算法（PF）进行机器人的位姿估计。若已知机器人的路径，且各个地标点的位置估计是相互独立的，则机器人路径信息和环境路标信息估计模型为：

FastSLAM 算法的基本步骤可以分为以下几步：

Step 4.计算重要性权值：为新粒子计算其重要性权值ωk。

Step 5.重采样：用同样数目的粒子将原来N 个粒子替换，并且对每个粒子按照其对应的重要性权值ωk进行采样。具体流程如图2 所示。

图2 Fast-SLAM 基本流程

3 系统测试与分析

3.1 地图构建

由于医院无法实地检测，为测试本系统的构图性能，选择在学校教室和教室走廊两种环境下，模拟医院长走廊和多房间的环境，进行实时构建地图。在教室环境下构建的实时地图，地图构建基本符合实际场景。在教室环境下，机器人地图构建系统跟随机器人不断变化，采用绿色边线表示障碍物，此种障碍物具有两种特点：一是高度超过了激光雷达扫描的最高高度；二是竖直方向上为平面，即可以遮挡住激光雷达在竖直方向上的扫描光线。而当遇到障碍物竖直方向上不为平面时，激光雷达的扫描光线可以通过该障碍物，扫描到其他位置，中部分地图边线明显，而其他地图边线不明显，甚至有射线射出的情况。同时，我们也检测了在某些带有玻璃门的教室环境下地图构建情况，当机器人经过玻璃门时，激光雷达的射线可以穿透玻璃门，导致环境地图构建失败，地图边线不存在或者不突出。

机器人在行走过程中在教室长走廊环境下构建的实时地图，地图边线明显的部分即为墙体，而地图边线不明显，有射线射出的部分表示教室门是开着的，同时，如果是带有条状玻璃的教室门或者通体玻璃门，情况也类似。

该测试结果，一方面证明了本文提出的多传感器数据融合方法地图构建的有效性，同时也反映出激光雷达在地图构建方面的缺陷，因此可以考虑增加视觉系统，提高地图构建的可靠性；另一方面也说明在障碍越明显的地方，机器人系统构建的地图越清晰。这里的障碍物的明显性，比如墙体相对于桌子障碍的明显性就更突出，激光雷达扫描射线的反射率就越高。

3.2 实时定位

为测试本系统的实时定位功能，选择重复到点定位精度作为测试指标，实验过程中先记录一下机器人到指定位置的坐标值，作为初始位置，并记作0 点，然后让机器人根据规划路线离开初始位置0 点，之后再回到初始位置0 点，重复10 次，分别记录各次机器人的x，y，θ方向的误差，并计算10 次的平均误差，作为精度值，最终测试结果见表1。需要指出的是，该机器人在该规划路线行走的过程中有2 张带有靠背的椅子和1 张竖直方向为平面的桌子作为障碍物，且椅子和桌子的最高高度均超过机器人的最高高度。由表1可见，本定位导航系统精度较高，效果较理想。并且在机器人按照规划路线运行的过程中可以很好地检测到障碍物，并绕过障碍物继续行进，直至到达测试终点，说明该机器人多传感器数据融合定位导航系统实时定位功能具有较为可靠的稳定性，能实现较为有效的路径规划和自主导航。

表1 本系统定位精度

4 结语

本文介绍了一种应用于室内移动护士助手机器人的多传感器数据融合定位导航系统，采用里程计和惯性测量单元IMU 作为内部传感器，激光雷达和单目相机作为外部传感器，内部传感器通过EKF 算法进行机器人的即时位姿估计，激光雷达测算出机器人环境距离，并构建环境地图，单目相机拍摄的图像经过数据处理得到机器人的实时位姿，最后通过AMCL 算法进行机器人实时定位。经过系统测试，该系统具有较为良好的定位精度，以及较为有效的地图构建系统，在室内移动机器人定位导航技术方面有一定的借鉴价值。