基于ROS 的机器人自主探索导航与地图构建研究

邵连奇，张媛媛，袁 田，淡睿哲，马帅辉，顾伟宏

（东北林业大学，哈尔滨 150000）

ROS（Robot Operating System，下文简称“ROS”）全称为机器人操作系统，是适用于机器人开源的一种元操作系统，是管理机器人硬件的一个操作系统架构。其为操作系统提供了应有的服务，包括硬件抽象、底层设备控制、常用函数的实现和进程间消息传递，以及包管理。ROS 也提供了用于获取、编译、编写和跨计算机运行代码所常用的机器人开发通用工具和开源库函数。

ROS 系统的起源是2007 年斯坦福大学人工智能实验室与Wilow Garage 公司的一个机器人合作项目——斯坦福人工智能机器人项目（the STAIR project）。2008 年之后由Willow Garagre 来进行推动。ROS现己成为国内外大学进行机器人实验研发的首选平台。据调查可知，实现无人驾驶技术的最关键问题之一就是地图构建。地图构建的精准度，又受制于传感器所获取信息的时效性及准确性等。在ROS 的基础上，利用摄像头和激光雷达对周围的环境的进行探测。相比于摄像头传感器，雷达的检测范围更大、检测角度更广。利用雷达检测这一优点可以快速对障碍物进行避障，进而提高了机器人运行速度上限。但随着运动速度的提高，单一传感器检测易产生误判，因此加入摄像头进行识别辅助判断，通过对多传感器融合使用以及对数据的融合处理，降低误判的程度。

1 机器人系统设计框架

1.1 硬件系统设计

机器人的整体结构与硬件组成如图1 所示，该机器人主要由激光雷达、视觉模块、控制模块和移动平台4 个部分组成。其中移动平台由常规智能车底盘进行改造，选用航模电池进行供电。激光雷达采用三角测距原理，通过脉冲激光获取目标物的标点数据，该机器人采用的LS01X 型激光雷达测距半径可达16 m。视觉模块选取工业摄像头，利用CCD 感光芯片，对外界环境进行探测与感知。控制系统的核心为MiniPC，承载Ubuntu 20.04。移动平台在无刷电机、舵机、轮式编码器及散热模块与减震模块的协同下，可以在复杂的环境中进行快速行驶。

图1 机器人的整体结构与硬件组成

1.2 控制系统设计

机器人的自主导航以及地图构建需要利用激光雷达、摄像头、IMU（惯性传感器）和编码器4 种传感器所采集的信息，通过MiniPC 将这些数据进行融合处理；并且借助ROS 丰富的开源社区资源与rviz 软件平台实现自主探索导航与二维可视化建图功能。

2 机器人移动平台的SLAM 系统搭建

2.1 SLAM 系统概述

SLAM 是一种解决移动机器人同步定位与导航的技术，机器人通过其所搭载的传感器对外界环境信息进行获取，从而进行增量式地图构建，进而利用所获取的环境信息对自身位姿进行获取与调整。SLAM 主要分为4 个方面：激光SLAM、视觉SLAM、多传感器融合SLAM 和深度学习SLAM，其中多传感器融合SLAM相较于其他SLAM 定位精度更高，拥有更好的鲁棒性。

2.2 视觉系统模型

OpenCV 是一个开源的BSD 许可库，由一系列的C 函数和少量C++类构成，可以实现计算机视觉和图像处理方面的很多算法，具有强大的图像和矩阵计算能力。图像处理与分析函数、轮廓、几何学处理函数、目标跟踪与追踪函数及颜色识别函数是OpenCV 常用的几种函数，合理的应用OPenCV 可以有效提高机器人的自主导航与避障能力。

3 机器人运动情况下的定位与对未知环境的地图构建

3.1 卡尔曼滤波算法研究

卡尔曼滤波算法（Kalman Filter）通过系统的观测数据及系统状态对全局最优状态进行推算。通过上一状态和下一状态来预测当前状态，公式如下

式中:At指当前时刻的状态转移矩阵（就是指从上一状态转变为下一状态的关系矩阵）；Bt为当前时刻的控制矩阵（就是指影响控制量的控制矩阵）；ut为当前时刻的控制量；μt-1为上一时刻最优估计值；μ¯t为当前时刻估计值。

预测均值的协方差来计算Kalman 增益为，其中协方差为

式中：Σt-1为上一时刻预测值方差矩阵；Rt指当前时刻测量值噪声矩阵。

Kalman 增益的计算公式为

式中：KT为卡尔曼增益（指的就是权值）；CT为测量方程；QT为观察量的协方差矩阵；

通过Kalman 增益来计算状态估计值

预测当前状态需要用到上一状态的协方差，所还需要计算当前状态的协方差用于下一次迭代

通过以上公式即可预测出物体的位置坐标及速度。

3.2 数据融合

robot_pose_ekf 是 ROS Navigation stack 中的一个package，通过扩展卡尔曼滤波器对IMU、里程计odom、视觉里程计vo 的数据进行融合，从而计算出机器人在平面上的真实位姿，并输出odom_combined 消息。robot_pose_ekf 只适用于平面上的轮式移动机器人，因此odom 信息中可以忽略z，pitch 与roll 分量。IMU 可以提供机器人坐标系相对于世界坐标系的姿态（RPY 角），其中Roll 和Pitch 是绝对角度，而偏航角Yaw 在IMU 中没有集成电子罗盘测量地球磁场角作为参考则是一个相对角度。IMU 姿态的协方差矩阵代表了姿态测量的不确定度。robot_pose_ekf 节点默认会从odom、IMU_data、vo 这三个topic 上订阅消息，通过remap 将其映射到新名称的topic 上。每当节点使用重映射中的原始名时，ROS 客户端库就会将其默默地替换成其对应的新名称。IMU 信息的协方差矩阵中代表机器人航向角的分量方差为10-6，而里程计信息的协方差矩阵中机器人姿态分量的协方差为103，2 个值相差很大。而在进行EKF 融合时，会更“相信”IMU 提供的姿态信息，因其方差更小。

3.3 Gmapping 建图算法

Gmapping 是一种基于Rao-Blackwellized 粒子滤波的二维激光SLAM 方法，gmapping 的运行步骤一般为：采样—计算权重—重采样—地图估计。gmapping 基于RBpf 粒子滤波算法，即将定位和建图过程分离，先进行定位再进行建图。东北林业大学为了解决RBpf 频繁重采样的缺点，gmapping 提出了选择性重采样，即减少重采样的次数，设定阈值；当粒子权重大于所设阈值才执行重采样，这样便减少了采样的次数，即减缓了粒子的退化。

利用gmapping 功能包订阅机器人的深度信息、通过利用IMU 信息和里程计信息以及卡尔曼滤波融合，进行小车的精确定位，同时进行一些必要的参数配置，例如对gmapping 功能包中的雷达最大检测距离及地图刷新频率的调试，进而更好地实现同步定位与地图构建。

编写gmapping 节点相关launch 文件，修改关键参数map_update_interval（地图刷新频率）为3，maxUrange（雷达最大检测范围）40 cm，其余参数保持不变，修改好～base_frame（string, default:"base_link"）机器人基坐标。～map_frame（string, default:"map"）地图坐标系。～odom_frame（string, default:"odom"）里程计坐标系，运行launch 文件，即可创建并输出基于概率的二维栅格地图，因为其方差更小。

4 机器人移动平台的路径规划算法研究

4.1 全局路径规划——A*算法

全局路径规划是指轮式机器人找到起点与目标点间可通行路径的过程，此过程仅包含路径的信息，同时构建一系列规划的目标，包括路径最短躲避障碍物等。本文研究是基于ROS 的研究，故选取A*算法作为全局路径规划算法。

A*算法的关键在于如何利用启发式信息决定哪个点是下一步要扩展的节点，其搜索过程实际上是被选节点扩展的过程，使用最少的资源获取最优解。机器从起始点出发，不断地寻找以最小代价通向目标点的节点并优先扩展那些可使目标函数较小的路径点，进而形成一个点集。该点集中的路径点有序连接即为所求的最优路径，因此启发函数是A*算法核心内容，启发函数越精确，找到最优解的速度就越快、越准确。

A*算法的模型为

式中：f（n）为总的搜索代价；g（n）为从起点到当前节点n 的代价和；h（n）为从当前节点n 到目标节点的最优代价的启发函数。

标准的启发函数正是曼哈顿启发函数

如果单元在地图上允许对角线移动，对角线函数便可以对其进行补充

如果单元在地图上允许向任意方向移动，这时利用欧几里得算法可以得到最短距离，但A*算法运行时间将更长

式中：xi（i=n，goal），yi（i=n，goal）和zi（i=n，goal）分别是节点n 与终点节点在三维空间中的坐标。

4.2 局部路径规划研究——TEB 局部规划控制器实现

在局部路径规划问题中，受控对象的动力学模型会被更多地考虑进来并建立局部动态地图，将自身空间参数与障碍物信息进行融合，并引入时间信息。算法不断在适量空间中，实时得出各轴的速度、角速度及加速度的信息，并将所筛选的信息发布给控制器进行使用，从而实现局部路径规划。

实现TEB 局部规划控制需要先下载安装TEB，和TEB 例程。软件包包括一个简单的测试节点（test_optim_node），其可以优化固定初始姿态与目标姿态之间的轨迹。首先在初始与目标之间配置单个轨迹（Timed-Elastic-Band）的规划，然后在独特的拓扑中激活和设置规划。使用ROS 参数服务器停用并行计划（确保运行roscore）启动test_optim_node 并结合预配置的rviz 节点进行可视化。通过运行rqt_reconfigure 来定制优化。解决本地优化方案的问题，并在独特的拓扑中实现并行规划。此扩展计划程序默认启用，需要更多计算资源。重新启动roscore 或重新激活扩展计划程序，启动test_optim_node 并结合预配置的rviz 节点进行可视化。

5 机器人移动平台导航实验研究

本设计在一定的实验环境下，采用合理的模拟方法，建立了以锥桶为障碍物围绕而成的环形通道，如图2所示。实现了在节省地方的基础上，最大化地满足研究需求。本实验是让机器人绕环形通道行驶2 圈，在第一圈时，自动识别障碍物并进行建图。第二圈时，在已构建的地图基础上，进行路径规划进而实现自动导航的功能。

图2 实验环境

首先，让机器人能够自主进行场地信息的处理。本设计采用视觉系统以及雷达系统信息相结合的方法，来完成环形通道的行驶。通过雷达对周围信息的反馈，根据雷达返回的每个点对应的距离以及角度，判断出障碍物的大致范围，进而通过视觉实现精准的定位，如图3 所示。最终，可以使机器人自主通过此环形区域。

图3 机器人在实际环境中所找到的障碍物

与此同时，本设计也利用采集到的雷达信息，结合轮式编码器及IMU 的信息进行gmapping 建图操作，从而实现机器人边行驶边进行二维栅格地图建立的过程，当机器人完成全部的建图之后，便得到一个环形的通道图形，如图4 所示。

图4 路径规划时的图像

此外，本设计利用上一步所建立的图形，进行局部规划以及利用A*算法对图形进行第二圈的导航。在实验过程中，发现本步骤十分依赖于第一步所建立的地图。然而，作者在实验过程中发现，若想建立好稳定的图形，就要保证激光雷达始终保持在一个基准平面，这样所建立的图形就能够精准地还原实际状态。我们通过对雷达等周边零件的固定，以及改造车体，使其更加不易晃动。最终，通过这些步骤，在完善设计所建立的地图后，通过局部规划以及A*算法成功地实现了第二圈闭环跑操作。