基于Hector SLAM算法的全场景自动驾驶护士床设计

郭凯诚

近几年来，随着科技水平的提高，互联网和医疗器械行业之间的联系越来越紧密，各种智能医疗设备如雨后春笋般涌现到市场中来，这些设备极大的提高了医护工作人员的工作效率，并且在很大程度上减轻了疾病给患者带来的痛苦。

在医院中有很多病人由于其自身疾病或是自身肢体残疾的原因，没有办法进行自主移动。这些病人在住院期间的生活起居，往往都需要医护工作人员或者是病人家属进行照料。由于无法完成正常人最基本的行走动作，他们在想去某个目的地的时候，往往需要先呼叫医护人员帮助完成。而在医院资源比较紧张的今天，医护人员往往没有办法满足所有病人的需求，病人此时只能等待他人的帮助。

由此可见，能够直接帮助病患进行移动的自动驾驶医疗设备势在必行。本设计针对上述问题，采用SLAM 和树莓派技术开发了一种多功能智能护理床,可以实时定位构建地图、路径规划及导航。主控模块选用树莓派3B,在树莓派上运行基于ROS 系统的SLAM 技术进行实时地图建模,整个机器人控制系统被整合到ROS 系统中,具备结构简洁、一体化程度较高的特点。有助于帮助患者解决生活中的移动问题，使得患者有更大的活动空间。

1 设计原理及实现

1.1 系统总体设计

本设计是在树莓派Ubuntu16.05 的基础上搭建了ROS 系统。在ROS 的基础框架上,通过选配符合功能要求的功能包,快速实现机器人的控制系统开发。主要分为三大模块，分别是下位机系统设计、ROS 系统和上位机软件设计。下位机系统设计主要包括图像采集模块、STM32 控制模块和激光雷达扫描模块；ROS 系统主要是针对各个节点进行设计，主要有视频传输节点、运动控制节点、路径规划节点、激光雷达通信节点、SLAM 节点等多个松耦合的节点,每个节点都是1 个ROS 进程；上位机软件设计i 主要是设计人机交互界面，方便操作者的使用。整个系统设计框图如图1 所示。

1.2 系统构建及算法设计

1.2.1 ROS 系统构建

本系统包括远程操控端和车体端两个系统部分，操控部分包括摇杆操控、键盘或者屏幕的鼠标实现对机器人的远程操控。车载计算机是核心，负责机器人系统运算和各部分的通信。通过对远程遥控端和车载树莓派核心控制系统建立联系，一般来说，远程控制端主要是通过在Linux 系统下对车载计算机进行SSH 访问达到远程控制的目的。

ROS 系统主要是使机器人各个传感器之间相互协调配合工作，实现系统的目标功能。本系统中，通过相机和激光雷达来实现对外界环境的感知。相机负责进行图像采集，通过Hector SLAM 实现机器人的位姿预测，激光雷达可以实时探测周围的障碍物，在形式的过程中完成避障。

除此之外，本系统中为了对机器人进行速度的校准和调节，加入了三轴加速度传感器模块，可以对机器人的行驶速度进行精确的测量。

上层配置文件包括远程操控端和车体端两个部分。车体端配置文件是gammbot-bringup 包中的gammab0t.1aunch，此文件主要由3 个部分的内容组成：系统参数设置、系统启动节点和启动其他模块或服务的launch 启动文件。系统参数设置中可以设置是 否可以启动或停止某些传感器(如深度相机)的启动、端口参数设置以及是否仿真运行等。

ROS 实现移动机器人运动控制的核心是roscontrol 设计与配置，通过自定义，可以实现对不同硬件接口协议的耦合，满足不同机器人硬件配置的要求。本文设计了移动机器人控制架构，基于ros-control 控制机制，配置差速运动控制器完成对移动机器人的运动控制。控制器设计部分可以通过修改配置文件中的参数。ros-control 的运动控制机制设计实现步骤包括4 个部分：上层配置文件、配置服务器、硬件接口层设计和硬件执行层设计。

1.2.2 Hector SLAM 系统构建

Hector SLAM 核心过程图2 所示，SLAM 系统主要有3 个步骤：

第一步为预处理。对通过激光雷达获得的数据进行过滤处理，然后通过摄像头提取环境特征。

第二步为匹配。在激光雷达不断采集周围环境的过程中，地图上的每一个位置都会经过激光雷达信号的处理，对应每一个点云数据。之后又通过ICP 点云匹配算法来实现实际环境和点云数据的对应。

第三步为地图融合。在实际地图环境和采集到的点云数据进行一一匹配之后，我们可以将激光雷达采集到的新数据逐步拼接到地图中来，实现地图的更新。

1.2.3 图像预处理

本系统通过摄像头采集环境数据，在视频采集过程中可能会受到各种环境因素及硬件条件的影响，如光照强度不均匀、硬件功能不稳定等因素会导致噪声产生，从而影响后期图像特征的提取与图像识别的结果。另外，摄像头获取的视频帧是RGB图像，为了减少计算量，实现实时处理，在后期关键特征的提取采用灰色图像。本文图像预处理主要包括图像的灰度转换和二值化处理、图像边缘检测。

图像的灰度转换和二值化处理：灰度变换可以简化信息，灰度化以后就只剩下灰度值。进行图像的灰度转换可以有效的突出图像中的目标特征，同时对于一些无关紧要的特征值可以对其进行抑制。进行灰度转换之前的图像灰度值和转换之后的灰度值之间有一定的转换关系，我们一般用灰度变换函数T(D)来表示这个关系。其中D=f(x,y)表示的是原图像中的灰度值，D'=g(x,y)则表示灰度转换之后的灰度值。其满足关系

在灰度图像的二值化处理过程中，图像中像素点的灰度值一般设置为0 或255。二值化处理是为了通过对图像的再处理，以此得到图像的目标信息，获取图像的目标轮廓。

图像边缘检测：在本设计中对环境特征的提取需要基于边缘和角点等特征完成，因此需要对图像进行边缘检测。边缘在图像处理中有重要的作用，一般来说灰度均匀程度不同的图像区域的边界就是图像边缘，沿着边缘法线方向的灰度变化比较剧烈，在这里我们采用了算法canny 对其进行边缘检测，该算法包含了多个阶段的算法。通过一阶差分偏导计算出梯度值以及方向，一阶差分是通过sobel 算子得到，将得到的梯度值不是极值的点均进行置0操作，并测定其阈值范围，超出阈值范围的点属于边缘。

1.2.4 视觉跟踪算法

图2 SLAM原理图

卡尔曼滤波主要是通过系统的输入输出来对数据进行观测，是对系统状态进行最优估计的一种算法。所谓滤波，是对在所观测的数据中来自系统或者是外界各种因素的干扰进行剔除。之所以采用卡尔曼滤波算法进行目标位置的估计，是因为在进行目标位置的估计、速度的测量估计以及加速的测量估计过程中会产生噪声。我们可以通过卡尔曼滤波算法，在采集到目标动态信息的前提下，得到目标位置的最优估计。

在我们进行机器人位置姿态估计的过程中，首先我们会采集机器人在各个时刻的位置状态，记作x1,x2...xk，下标k代表离散时间。用两个方程来描述状态估计问题：

其中，f为运动方程、u为输入、w为输入噪声、g为观测方程、y为观测数据、n为观测噪声。

根据高斯性质，得如下方程：

其中Qk和Rk表示两个噪声项的协方差矩阵,、C分别为转移矩阵和观测矩阵。通过计算可知卡尔曼滤波器构成的最优无偏估计可以通过以下两个步骤完成：

1）对下一个位置姿态进行预测,

2）对目标位置进行实时更新,其概率的分布如公式6 所示，其中K表示卡尔曼增益。

1.3 上位机软件设计

图3 上位机软件设计框图

上位机软件的开发使用了QT 平台进行人机交互界面的设计，通过与下位机的通信和ROS 系统的连接，实现病床所在区域地图的构建、移动控制等功能。本设计设计了一种简明易操作的人机交互界面，实现在上位机对整个系统的精确控制。界面中包含了所需要的参数设置，如平台位置读取、移动速度、移动模式等。其设计框图如图3 所示。

2 机械结构设计

自动驾驶护理床作为一种服务机器人，机械结构设计应满足人机工程学，运动实现上应满足设计原则为：

1）自动驾驶护理床应满足医疗器械基本要求，安全平稳。

2）自动驾驶护理床速度不应过高，保持安全范围。

3）自动驾驶护理床要实现前进、后退、转向、变速等功能。

4）自动驾驶护理床在检测到路况信息时需将信息传达给病人。

本文设计的自动驾驶护理床整体结构如图4 所示，自动驾驶护理床的驱动部分是由两个直流减速电机驱动承重轮来实现的，采用了双轮差速驱动的方式，可实现动态避障效果。装置采用塔式结构，将各硬件放置在不同高度位置，有利于功能的实现以及保持装置移动过程中的平稳性。各硬件空间位置关系如图4 所示。

图4 中树莓派处理器为整个装置的核心部位，用于处理和发送指令数据，STM32 单片机将信号发送至电机，实现装置的运动，激光雷达是用来检测障碍物并实现避障功能的传感器，摄像头为采集周围环境，并将信息指令发送给树莓派处理器，显示屏可显示工作时各部件的信息以及调试装置。

图4 自动驾驶护理床设计整体结构图

3 测试结果与分析

在机器软硬件系统开发完毕后,对其进行了实际测试,搭建了如图5 的测试场地。在实验中，主要通过对机器人进行地图绘制、自动导航以及实时避障的实验。

首先是对地图进行绘制，选取特定的区域使机器人在该区域范围内活动，通过激光雷达采集的数据，实现对该区域地图的绘制。在得到的地图中进行自动导航测试，并在其形式路径中摆放障碍物，测试其是否可以准确到达目的地并实现实时避障。

经过实验发现，机器人可以在所建地图中进行自动导航行驶，并在遇到障碍物的时候会及时躲避，实现了避障功能。实验结果表明，本系统的软硬件开发成功，并且到达了目标设计需求。

图5 系统测试图

4 总结与展望

本系统设计了一种基于HectorSLAM 的全场景自动驾驶床系统总体框架，以机载核心控制器树莓派位核心控制器，通过上位机人机交互界面的直接控制，并实现上下位机之间的通信，将各个传感器的数据进行交互和远程传输，采用卡尔曼滤波算法对机器人的目标位置姿态进行估计，并基于HectorSLAM 算法对行驶环境进行机器地图的建立。经过实验分析可知，在机器人建立好地图之后，通过设定目标地点以及布置障碍物，实现了机器人自动导航行驶，并在行驶的过程中实现避障功能，达到了预期设计目标。

但是在本系统在使用过程中，功耗加大，不利于长时间使用，如何提高系统的续航是一个需要解决的问题；本系统所使用的多个传感器价格比较昂贵，整个系统的成本较高，降低成本提高其接受性也需亟待解决；此外当前系统没有涉及系统的安全保护电路，后续应当添加系统保护电路。

总体来说，本设计实现了基本功能，在医疗器械与互联网的大背景下，越来越多的智能医疗设备被发明，相信基于HectorSLAM 算法的自动驾驶床的应用必定拥有更加广阔的市场。