一种基于ROS的自主导航机器人控制系统的智能小车

摘要

本发明公开了一种基于ROS的自主导航机器人控制系统的智能小车，包括，在小车上搭载激光雷达、摄像头和重力加速度传感器，利用所述激光雷达获取所述小车自身当前位置、距离信息及环境信息；基于机器人操作系统创建栅格地图，利用SLAM算法和深度相机视觉融合构建三维点云地图；所述小车利用所述机器人操作系统对所述距离信息及RGB‑D的深度摄像头的视觉里程计进行环境感知，生成所述三维点云地图；所述机器人操作系统根据GPS在路径构成回路时对所述三维点云地图进行矫正，计算所述小车路径以躲避障碍。本发明极大程度上增强了机器人的运动能力和鲁棒性，在里程计和编码器数据实现了更加精确的定位，使得其在定位上更加准确。

权利要求

1.一种基于ROS的自主导航机器人控制系统的智能小车，其特征在于：包括，在小车上搭载激光雷达、摄像头和重力加速度传感器，利用所述激光雷达获取所述小车自身当前位置、距离信息及环境信息；

基于机器人操作系统创建栅格地图，利用SLAM算法和深度相机视觉融合构建三维点云地图；

所述小车利用所述机器人操作系统对所述距离信息及RGB‑D的深度摄像头的视觉里程计进行环境感知，生成所述三维点云地图；

所述机器人操作系统根据GPS在路径构成回路时对所述三维点云地图进行矫正，计算所述小车路径以躲避障碍；

构建地图包括，

对获取的RGB‑D图像、雷达数据、里程计节点进行同步化处理，得到传感器数据；

所述传感器数据传输至短期记忆模块内，形成回环和近距离探测以完成图像优化，得到全局地图组长；

分别划分所述全局地图组长，依次得到地图数据、地图图像、TF坐标转换、基于八叉树三维地图、所述三维点云地图和所述栅格地图；

所述机器人操作系统搭载全局规划器、2TEB局部规划器和车模控制算法；

所述全局规划器包括，迪杰斯特拉算法和A*star算法；

所述车模控制算法包括，舵机控制算法和电机控制算法；

所述迪杰斯特拉算法包括，

以起始点为中心向外层扩展，直到扩展到终点为止；

利用所述迪杰斯特拉算法计算地图中的最短路径时，从顶点开始计算；

引进集合S和集合U，所述集合S记录已经求出的所述最短路径的顶点以及相应的所述最短路径的长度；

所述集合U则记录还未求出所述最短路径的顶点以及所述顶点到所述起始点的距离；

初始时，所述集合S中只有起点，所述集合U中是除了起点以外的顶点，且所述集合U中顶点的路径是所述起点到所述顶点的路径；

再从所述集合U中找出路径最短的顶点，将其加入所述集合S中；

更新所述集合U中的顶点和顶点对应的路径；

再从所述集合U中找出路径最短的顶点，并将其加入所述集合S中；

更新所述集合U中的顶点和顶点对应的路径；

重复迭代，直到遍历完所有的顶点，找到最短路径；

所述A*star算法包括，简化搜索区域，将地图简化成栅格状，定义一个方格的中心为一个节点；

从起点A的周围开始，检查其相邻的方格，向四周找到目标后，从众多可能的路径选出最短最合适的路径；

定义节点n为综合优先级，当选择下一个要遍历的节点时，选取综合优先级最高的节点；

每次从优先队列中选取优先级最高的节点作为下一个待遍历的节点；

若始终小于等于节点n到终点的代价，则一定能够找到最短的路径；

舵机控制算法包括PID控制器，所述PID控制器是一种线性控制器，根据给定值r(t)与实际输出值c(t)构成偏差：e(t)＝c(t)‑r(t)，将偏差的比例(P)、积分(I)和微分(D)通过线性组合构成控制量，对对受控象进行控制；

控制规律为：

传递函数为：

其中，Kp是比例系数，取Td＝Ti，Ki＝Kp÷Td积分系数，Kd＝Kp×Td微分系数，Td微分时间常数，Ti为积分时间常数。

2.根据权利要求1所述的基于ROS的自主导航机器人控制系统的智能小车，其特征在于：还包括，

所述机器人操作系统中的上位机接收下位机发送的数据，计算位姿匹配并匹配地图中的坐标；

判断其是否发布目标点，若是，则计算规划所述小车路径，刷新局部代价地图并发布底盘控制指令；

若否，则直接结束。

3.根据权利要求2所述的基于ROS的自主导航机器人控制系统的智能小车，其特征在于：还包括，

所述下位机初始化数据信息，启动定时中断，设置为20ms；

所述上位机接收所述下位机发送的小车速度、小车舵机角度数据，获取MPU6050、编码器数据；

执行运动控制。

说明书

一种基于ROS的自主导航机器人控制系统的智能小车

技术领域

[0001] 本发明涉及机器人软件开发的技术领域，尤其涉及一种基于ROS的自主导航机器人控制系统的智能小车。

背景技术

[0002] 对于现今快速发展的科技，在导航技术这一块，智能小车类型的导航发展不够完善，对于导航过程中对环境的要求，还比较严苛，发明该智能化的自主导航的系统，对于在
无地图的导航、基于已知地图的导航、基于增量式地图的导航这三类地图中可以做的很好
的识别定位和探索。

[0003] 以往系列导航小车的路径规划算法较为落后，对于在稍恶劣的环境条件下，就无法计算出最好的路径，不能很好的完成导航定位等任务，基于上述问题，需要一种搭载更为
先进的控制系统的智能小车去解决上述的问题。

发明内容

[0004] 本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部
分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

[0005] 鉴于上述现有存在的问题，提出了本发明。

[0006] 因此，本发明提供了一种基于ROS的自主导航机器人控制系统的智能小车，能够解决在稍恶劣的环境条件下，导航小车就无法计算出最好的路径，不能很好的完成导航定位
等任务的问题。

[0007] 为解决上述技术问题，本发明提供如下技术方案：一种基于ROS的自主导航机器人控制系统的智能小车包括，在小车上搭载激光雷达、摄像头和重力加速度传感器，利用所述
激光雷达获取所述小车自身当前位置、距离信息及环境信息；基于机器人操作系统创建栅
格地图，利用SLAM算法和深度相机视觉融合构建三维点云地图；所述小车利用所述机器人
操作系统对所述距离信息及RGB‑D的深度摄像头的视觉里程计进行环境感知，生成所述三
维点云地图；所述机器人操作系统根据GPS在路径构成回路时对所述三维点云地图进行矫
正，计算所述小车路径以躲避障碍。

[0008] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：还包括，所述机器人操作系统中的上位机接收下位机发送的数据，计算位姿匹配
并匹配地图中的坐标；判断其是否发布目标点，若是，则计算规划所述小车路径，刷新局部
代价地图并发布底盘控制指令；若否，则直接结束。

[0009] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：还包括，所述下位机初始化数据信息，启动定时中断，设置为20ms；接收所述上位
机速度、舵机角度数据，获取MPU6050、编码器数据；执行运动控制，向所述上位机发送所述
速度、所述舵机角度数据。

[0010] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：构建地图包括，对获取的RGB‑D图像、雷达数据、里程计节点进行同步化处理，得到
传感器数据；所述传感器数据传输至短期记忆模块内，形成回环和近距离探测以完成图像
优化，得到全局地图组长；分别划分所述全局地图组长，依次得到地图数据、地图图像、TF坐
标转换、基于八叉树三维地图、所述三维点云地图和所述栅格地图。

[0011] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：所述机器人操作系统搭载全局规划器、2TEB局部规划器和车模控制算法；所述全
局规划器包括，迪杰斯特拉算法和A*star算法。

[0012] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：所述车模控制算法包括，舵机控制算法和电机控制算法。

[0013] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：所述迪杰斯特拉算法包括，以起始点为中心向外层扩展，直到扩展到终点为止；利
用所述迪杰斯特拉算法计算地图中的最短路径时，从顶点开始计算；引进集合S和集合U，所
述集合S记录已经求出的所述最短路径的顶点以及相应的所述最短路径的长度；所述集合U
则记录还未求出所述最短路径的顶点以及所述顶点到所述起始点的距离；初始时，所述集
合S中只有起点，所述集合U中是除了起点以外的顶点，且所述集合U中顶点的路径是所述起
点到所述顶点的路径；再从所述集合U中找出路径最短的顶点，将其加入所述集合S中；更新
所述集合U中的顶点和顶点对应的路径；再从所述集合U中找出路径最短的顶点，并将其加
入所述集合S中；更新所述集合U中的顶点和顶点对应的路径；重复迭代，直到遍历完所有的
顶点，找到最短路径。

[0014] 作为本发明所述的基于ROS的自主导航机器人控制系统的智能小车的一种优选方案，其中：所述A*star算法包括，简化搜索区域，将地图简化成栅格状，定义一个方格的中心
为一个节点；从起点A的周围开始，检查其相邻的方格，向四周找到目标后，从众多可能的路
径选出最短最合适的路径；定义节点n为综合优先级，当选择下一个要遍历的节点时，选取
综合优先级最高的节点；每次从优先队列中选取优先级最高的节点作为下一个待遍历的节
点；若始终小于等于节点n到终点的代价，则一定能够找到最短的路径。

[0015] 本发明的有益效果：本发明采用了TEB算法，使其在计算路径规划时得出更优的路径方案，控制方面采用了速度前馈控制KPID，速度控制上更加灵敏，而在转向控制上同时采
用了舵机转向和差速转向，极大程度上增强了机器人的运动能力和鲁棒性；在里程计和编
码器数据实现了更加精确的定位，使得其在定位上更加准确；建图方面采用RTAB‑MAP算法，
使用更低的成本设备实现了三维点云地图的建立，并且具有较高的精度，相对于传统的设
备建图，其成本更低；在优化上面，该产品实现了前端后端优化，更加完善，还增设了回环检
测和自动校正两个功能，具有一定的自我检测和识别运动能力。

附图说明

[0016] 为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本
领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它
的附图。其中：

[0017] 图1为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的流程示意图；

[0018] 图2为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的上位机流程示意图；

[0019] 图3为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的下位机流程示意图；

[0020] 图4为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的建图流程示意图；

[0021] 图5为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的舵机控制示意图；

[0022] 图6为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的电机控制示意图；

[0023] 图7为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的第一个走廊建图测试示意图；

[0024] 图8为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的第二个走廊建图测试示意图；

[0025] 图9为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的第三个走廊建图测试示意图；

[0026] 图10为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的室外实际场景示意图；

[0027] 图11为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的第一个室外环境建图测试示意图；

[0028] 图12为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的第二个室外环境建图测试示意图；

[0029] 图13为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的局部规划器示意图；

[0030] 图14为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的又一种局部规划器示意图；

[0031] 图15为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的导航与避障功能测试示意图；

[0032] 图16为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的导航路径规划示意图；

[0033] 图17为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的硬件模块框架示意图；

[0034] 图18为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的电源模块框架示意图；

[0035] 图19为本发明一个实施例所述的基于ROS的自主导航机器人控制系统的智能小车的电机驱动模块框架示意图。

具体实施方式

[0036] 为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而
不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下
所获得的所有其他实施例，都应当属于本发明的保护的范围。

[0037] 在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的
情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

[0038] 其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指
同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

[0039] 本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本
发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。

[0040] 同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而
不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此
不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解
为指示或暗示相对重要性。

[0041] 本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，
也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人
员而言，可以具体情况理解上述术语在本发明中的具体含义。

[0042] 实施例1

[0043] 参照图1 图16，为本发明的第一个实施例，提供了一种基于ROS的自主导航机器人~
控制系统的智能小车，包括：

[0044] S1：在小车上搭载激光雷达、摄像头和重力加速度传感器，利用激光雷达获取小车自身当前位置、距离信息及环境信息。

[0045] S2：基于机器人操作系统创建栅格地图，利用SLAM算法和深度相机视觉融合构建三维点云地图。

[0046] S3：小车利用机器人操作系统对距离信息及RGB‑D的深度摄像头的视觉里程计进行环境感知，生成三维点云地图。

[0047] S4：机器人操作系统根据GPS在路径构成回路时对三维点云地图进行矫正，计算小车路径以躲避障碍。

[0048] 参照图2，机器人操作系统中的上位机接收下位机发送的数据，计算位姿匹配并匹配地图中的坐标；判断其是否发布目标点，若是，则计算规划小车路径，刷新局部代价地图
并发布底盘控制指令；若否，则直接结束。

[0049] 参照图3，下位机初始化数据信息，启动定时中断，设置为20ms；接收上位机速度、舵机角度数据，获取MPU6050、编码器数据；执行运动控制，向上位机发送速度、舵机角度数
据。

[0050] 参照图4，构建地图包括，对获取的RGB‑D图像、雷达数据、里程计节点进行同步化处理，得到传感器数据；传感器数据传输至短期记忆模块内，形成回环和近距离探测以完成
图像优化，得到全局地图组长；分别划分全局地图组长，依次得到地图数据、地图图像、TF坐
标转换、基于八叉树三维地图、三维点云地图和栅格地图。

[0051] 参照图5和图6，机器人操作系统搭载全局规划器、2TEB局部规划器和车模控制算法；全局规划器包括，迪杰斯特拉算法和A*star算法；车模控制算法包括，舵机控制算法和
电机控制算法；迪杰斯特拉算法包括，以起始点为中心向外层扩展，直到扩展到终点为止；
利用迪杰斯特拉算法计算地图中的最短路径时，从顶点开始计算；引进集合S和集合U，集合
S记录已经求出的最短路径的顶点以及相应的最短路径的长度；集合U则记录还未求出最短
路径的顶点以及顶点到起始点的距离；初始时，集合S中只有起点，集合U中是除了起点以外
的顶点，且集合U中顶点的路径是起点到顶点的路径；再从集合U中找出路径最短的顶点，将
其加入集合S中；更新集合U中的顶点和顶点对应的路径；再从集合U中找出路径最短的顶
点，并将其加入集合S中；更新集合U中的顶点和顶点对应的路径；重复迭代，直到遍历完所
有的顶点，找到最短路径；A*star算法包括，简化搜索区域，将地图简化成栅格状，定义一个
方格的中心为一个节点；从起点A的周围开始，检查其相邻的方格，向四周找到目标后，从众
多可能的路径选出最短最合适的路径；定义节点n为综合优先级，当选择下一个要遍历的节
点时，选取综合优先级最高的节点；每次从优先队列中选取优先级最高的节点作为下一个
待遍历的节点；若始终小于等于节点n到终点的代价，则一定能够找到最短的路径。

[0052] 表1：下位机发送的串口数据通信包。

[0053]

[0054] 不难理解的是，RTAB‑Map是一个大规模且长期在线的激光与视觉SLMA的开源库，RTAB‑Map最初是一种基于特征的回环检测方法，且具有内存管理的功能，应用于各类机器
人和移动平台上实现同步定位和建图，即SLMA，RTAB‑Map通过限制地图的大小使得回环检
测始终在固定的时间限制内处理，从而满足长期和大规模环境在线建图要求，RTAB‑Map已
经成为一个跨平台的独立C++库和一个ROS包，非常方便使用。

[0055] TEB全称Time Elastic Band（时间弹性带）Local Planner，该方法针对全局路径规划器生成的初始轨迹进行后续修正(modification)，从而优化机器人的运动轨迹，属于
局部路径规划；对于二维路径，将其看作Eletic Band（橡皮筋）一端连接起点，一端连接终
点，这个路径可以像橡皮筋一样弹性变化，路径变形的条件就是将所有的约束条件视作为
外力对橡皮筋的作用，起点、终点由全局规划器指定，中间插入N个控制橡皮筋形状的控制
点，为了显示轨迹的运动学信息，在点与点之间定义运动时间Time；此时，就可以理解TEB算
法就是Time+Eletic Band=TEB。

[0056] 通俗的解释，TEB生成的局部轨迹由一系列带有时间信息的离散位姿(pose)组成，g2o算法优化的目标即这些离散的位姿，使最终由这些离散位姿组成的轨迹能达到时间最
短、距离最短、远离障碍物等目标，同时限制速度与加速度使轨迹满足机器人的运动学；局
部路径规划器需要实现对于全局路径的跟踪同时也要避开障碍物，这两个需求本质上是一
类问题，在路径上找到障碍物或者全局路径点，计算二者之间的距离，定义一个基于距离的
势场，跟踪路径随着距离的增大而增大，避障则是随着距离增大而减小。

[0057] 在轨迹优化过程中，TEB拥有多种优化目标，包括但不限于：整体路径长度、轨迹运行时间、与障碍物的距离、通过中间路径点以及机器人动力学、运动学以及几何约束的符合
性；TEB明确考虑了运动状态下时空方面的动态约束，如机器人的速度和加速度是有限制
的，TEB被表述为一个多目标优化问题，大多数目标都是局部的，只与一小部分参数相关，因
为只依赖于几个连续的机器人状态，这种局部结构产生了一个稀疏的系统矩阵，使得它可
以使用快速高效的优化技术，例如使用开源框架“g2o”来解决“TEB”问题。

[0058] 需要指出的是g2o优化的结果并非一定满足约束，即实际都是软约束条件，若参数设置不合理或环境过于苛刻，teb都有可能失败，规划出非常奇怪的轨迹，所以在teb算法中
包含有冲突检测的部分，在生成轨迹之后逐点判断轨迹上的点是否与障碍物冲突，此过程
考虑机器人的实际轮廓。

[0059] 舵机部分的控制采用传统的PID控制，由上位机发送机器人当前方向与路线的偏差角度至单片机中，由单片机进行舵机角度的控制并直接输出占空比控制舵机，从而控制
车身不断跟算路径规划。

[0060] PID 控制器是一种线性控制器，它根据给定值 与实际输出值 构成偏差：，将偏差的比例（P）、积分（I）和微分（D）通过线性组合构成控制量，对对受
控象进行控制。

[0061] 其控制规律为：

[0062]

[0063] 传递函数为：

[0064]

[0065] 其中， 是比例系数， 积分系数， 微分系数， 微分时间常数，将积分系数设置成 0。

[0066] 电机部分的控制采用传统的 PID 控制和前馈控制方法，为了尽可能让机器人速度响应及时，传统的PID控制，因为积分项需要时间去积分，虽然控制精度很高，但无法提高
机器人的速度响应时间，而且收敛速度往往不理想；积分项的物理意义在于平衡阻力，由于
该机器人需要更加快的响应速度，因此系统需要更高的收敛速度，所以使用前馈控制来代
替积分项平衡阻力；因此需要对系统建立一个模型，控制精度取决于模型精度，虽然精度肯
定不如 PID 控制那么好，但是收敛速度非常快。

[0067] 优选的，为了更好地对本发明方法中采用的技术效果加以验证说明，本实施例选择以传统的技术方案与本发明方法进行对比测试，以科学论证的手段对比试验结果，验证
本发明方法所具有的真实效果。

[0068] 传统的技术方案：相较于SLAM技术，传统的技术方案在多传感器融合、优化数据关联与回环检测、与前端异构处理器集成、提升鲁棒性和重定位精度方面都存在一定的不足，
而且Sensor精度不高、计算量大、Sensor在使用场景上不具有普适性问题。

[0069] 由于传感器种类不同和安装方式不同，SLAM在实现方式以及构建难度上会有所差异，目前而言，激光SLAM技术更加成熟，对于构建地图精度更高，一般误差在3cm，更适合机
器人的导航，激光雷达（激光SLAM技术）的诞生使得测量更加快速更加精准，信息获取也更
为丰富。

[0070] 本实施例还需要说明的是，主要测试的内容有建图功能和导航避障功能，在仿真平台中，搭建一个赛道进行建图，在导航过程中加入锥桶进行避障测试，观察机器人是否能
顺利避开障碍物，经测试仿真实现了建图、导航和避障功能，实物测试，分别测试了室内封
闭狭窄空间、室内大场景和室外环境测试，进行建图和导航测试，测试结果较好得实现了建
图和导航功能，具有较好的鲁棒性，在测试中通过计算前方路径的曲率数据作为机器人的
转向前瞻，在控制算法中为了让舵机能够更加实时地打角，因此会提供一定地前瞻性，用来
克服舵机的机械转向慢的问题。

[0071] 具体测试过程如下：

[0072] （1）室内狭小封闭环境建图功能测试

[0073] 在室内用挡板搭建一个长方形封闭空间，将小车放入其中，运行建图程序，在RVIZ显示器上封闭环境轮廓被描绘出来，随着机器人不断运动，蓝色线条为机器人运动轨迹线，
周围地图被不断完善最终形成封闭地图。

[0074] （2）室内大场景环境测试

[0075] 参照图7 图9，该设计除了需要应对室内封闭狭小环境，同时应该对校园环境中常~
见的场景有一定适应能力，比如走廊，走廊环境比较特殊，一般长度较长，颜色单一，基本上
为纯白色，这样的场景特征点单一，连续几帧差别不大，对于算法的考验较大，所以在测试
中，在实验楼D的一楼走廊进行了测试，左侧为机器人第一视角，右侧为RVIZ显示的地图，可
见左侧反光较为严重，环境亮度很高，伴随机器人的运动，地图不断扩大，直到周围的已知
区域逐渐完善接近真实环境，地图无明显扭曲，边际完整，其中底层黑色点云为墙体，这是
根据激光雷达的扫描数据和深度相机的数据生成的，地面以上的彩色点云部分为深度相机
所拍摄的数据，经过机器人自身的移动性能，弥补了激光雷达和深度相机的机动性，从而完
成对于室内环境的三维地图构建。

[0076] 综上，机器人在室内环境中能够较好的完成地图构建并基本吻合实际环境；

[0077] （3）室外环境测试

[0078] 参照图10 图12，机器人在校园环境中运行难免会遇到室外情况，为了试验机器人~
在室外的适应能力和建图能力故在室外进行测试，室外环境，主要困难在于草丛、灌木丛等
半稠密介质，不同于普通墙壁和挡板，激光有可能穿透介质进行建图，从而认为这一块没有
物体是可通行区域，但是融合了深度摄像头后，对于这种半稠密介质有了更好的适应性，机
器人在构建地图的时候对于灌木丛并没有投射，而是还原了实际情况，这是普通2D雷达所
不具有的优势。

[0079] 对于大场景的建图，往往需要多次的回环检测从而对地图不断矫正，来达到对于地图的真实还原效果，机器人在对于室外环境也有很好的建图效果，基本上还原了实际场
景，地图质量较好可以用于导航。

[0080] （4）导航与避障功能测试

[0081] 参照图13 图16，当对环境建图完成之后需要在地图上进行路径规划以及实现实~
时避障的功能，为局部规划器对周围环境的匹配效果。

[0082] 当环境中放入障碍物比如锥桶，局部规划器会实时对周围的环境实时扫描，红色圈中的粉色点，此为局部规划器扫描到的障碍物，局部规划器会根据激光雷达扫描到的信
息对栅格地图的膨胀层进行修改，因此，在机器人运动过程中，通过激光雷达和深度相机的
实时扫描，更新地图的膨胀层和障碍层，对局部路径进行修改，则能够实现机器人的实时避
障。

[0083] 导航开始前，使用rviz自带的2D nav goal工具在地图上标定目标点，其本质是发布了一个goal_话题，供导航节点订阅，当标定完目标点之后，则会在地图中生成规划好的
路径，此时机器人会根据规划的路径响应控制器的速度和舵机输出。

[0084] 当在环境中放置障碍物，局部路径规划期则会规划出一条绕过障碍的路径，路径规划器避开了障碍物的膨胀层，规划了一条明显可见的弯曲的路径。

[0085] 经过机器人在室内环境下的测试，能够得出结论，机器人能够在室内环境中进行路径规划、避障，并且实际路径能够一定程度上贴合规划的路径。

[0086] （5）测试分析

[0087] 利用虚拟机ssh远程控制可连接小车主机的IP地址，通过命令框运行机器人的导航节点，便可在虚拟机端利用rviz远程调出已经建立好的地图，并能监测到机器人在地图
中的位置、姿态以及周边环境，通过rviz的目标点标定，即可在地图中标出机器人需要到达
的目标点，等待目标点话题发布过后则会激活小车的移动程序，在行驶过程中小车会在遵
循局部规划期的路线的基础上，跟据周边环境进行障碍物的避让。

[0088] 在已经建立好的地图中通过rviz进行目标点标定后，便可以看见路径规划算法所规划出的黑色路径，这是利用全局规划器进行整个地图的粗略规划，机器人附近的彩色点
云会不断匹配周围场景的特征点，与地图匹配起来，在小范围内用于实时校准自己的定位，
地图中机器人前方的三个圆形点即为机器人的转向前瞻，在控制算法中为了让舵机能够更
加实时地打角，因此会提供一定地前瞻性，用来克服舵机的机械转向慢的问题。

[0089] 因此合理地设置好前瞻距离之后，也可以利用话题的形式发布出来，显示在rviz中，基于此，为本实施例所搭建的系统展示出来的定位导航效果。

[0090] 实施例2

[0091] 参照图17 图19，为本发明的第二个实施例，该实施例不同于第一个实施例的是，~
提供了一种基于ROS的自主导航机器人控制系统的智能小车的硬件模块，具体包括：

[0092] 参照图17，采用STM32F103作为主控制器用于输出控制电机和电机的PWM，同时读取编码器和IMU（惯性测量单元）数据，通过串口发送给上位机Jeston nano，Jeston nano进
行主要的运算，在建图模式下通过获取激光雷达和深度摄像头的数据构建地图，在导航模
式下激光雷达和深度相机用于匹配特征点估计机器人自身位姿，计算出运动路径，并将控
制期望发送给下位机STM32。

[0093] 参照图18，12V电源输入，经过LM2596S降压为5V，5V再经过AMS1117降压为3.3V，5V稳压采用LM2596S，这是一款降压型电源管理单片集成电路的开关电压调节器，能够输出3A
的驱动电流，同时具有很好的线性和负载调节特性，3.3V稳压采用AMS1117，这是一种输出
电压为3.3V的正向低压降稳压器，工作温度范围：‑40 125°C，输入电压5V。
~

[0094] 参照图19，电机驱动采用TB6612FNG，该模块可以驱动2两个电机，STBY:接单片机IO口，置0则电机停止工作，置1则通过AIN1、AIN2、BIN1、BIN2来控制电机电机正反转，VM接
12V电源，VCC接5V电源，GND接地，TB6612FNG可供每通道输出最高1A的连续驱动电流，启动
峰值2A(连续脉冲)/3A(单脉冲)，工作温度在‑20℃～85℃，PWMA、PWMB接单片机PWM输出，
A01、A02、B01、B02为模块输出端，AIN与电机正反转关系见表2。

[0095] 表2：AIN与电机正反转关系。

[0096]

[0097] 表3：电机线序。

[0098]

[0099] 本实施例采用20KG扭矩的舵机，型号为XTARK XDS‑S20A,可控角度为180°，使用PWM（脉冲宽度调制）驱动，尺寸为40*20*37.2mm。

[0100] 具体的，本实施例还需要说明的是，本发明所提供的一种基于ROS的自主导航机器人控制系统的智能小车，其硬件模块包括，激光雷达、RGBD摄像头、IMU、编码器、上位机、下
位机、电机和舵机，激光雷达和RGBD摄像头通过USB接口连接至上位机，IMU和编码器分别基
于IIC和IO连接至下位机中，上位机和下位机之间通过串口进行连接，电机和舵机基于PWM
技术连接下位机，通过串口下发指令和计算程序至电机和舵机中实现小车的控制。

[0101] 应当认识到，本发明的实施例可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编
程技术‑包括配置有计算机程序的非暂时性计算机可读存储介质在计算机程序中实现，其
中如此配置的存储介质使得计算机以特定和预定义的方式操作——根据在具体实施例中
描述的方法和附图。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机系
统通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编
译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

[0102] 此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执
行指令的一个或多个计算机系统的控制下执行，并且可作为共同地在一个或多个处理器上
执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组
合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

[0103] 进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成
的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存
储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算
平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当
存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此
外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器
或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不
同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明
还包括计算机本身。计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输
入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出
设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器
上产生的物理和有形对象的特定视觉描绘。

[0104] 如在本申请所使用的，术语“组件”、“模块”、“系统”等等旨在指代计算机相关实体，该计算机相关实体可以是硬件、固件、硬件和软件的结合、软件或者运行中的软件。例
如，组件可以是，但不限于是：在处理器上运行的处理、处理器、对象、可执行文件、执行中的
线程、程序和/或计算机。作为示例，在计算设备上运行的应用和该计算设备都可以是组件。
一个或多个组件可以存在于执行中的过程和/或线程中，并且组件可以位于一个计算机中
以及/或者分布在两个或更多个计算机之间。此外，这些组件能够从在其上具有各种数据结
构的各种计算机可读介质中执行。这些组件可以通过诸如根据具有一个或多个数据分组
（例如，来自一个组件的数据，该组件与本地系统、分布式系统中的另一个组件进行交互和/
或以信号的方式通过诸如互联网之类的网络与其它系统进行交互）的信号，以本地和/或远
程过程的方式进行通信。

[0105] 应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术
方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发
明的权利要求范围当中。