机器人自主导航仿真

成家骏

（山西农业大学软件学院 山西省晋中市 030800）

1 引言

移动机器人的研究始于60年代末，斯坦福研究院（SRI）的Nils Nilssen 和Charles Rosen 等人，在1966年至1972年中制造出了取名为Shakey 的自主移动的机器人[3]，时至今日，世界机器人发展迅猛犹如几十年前计算机行业，移动机器已经融入到我们生活的各行各业，使我们的生活更便捷，使我们的生产更高效。同时伴随着5G 技术的落地并运用覆盖，带宽增加推动新产业升级，加速技术融合起到了决定性的作用，各式各样以计算机技术的发展为基础的机器人如雨后春笋般涌现。机器人能自适应和感知周围环境，在实际运动过程中由配备传感器采集一系列环境信息，并用处理后的数据进行分析调整姿态绘制出地图的机器人控制方式的广泛应用，可以尽可能地保证了机器人在复杂和不确定环境中作业的稳定性。本文主要针对在ROS 环境下自主导航的仿真，相较鹿霖,谢树新等人和曹正万,平雪良,陈盛龙蒋毅等人[4-5]，采用solidworks 建立三维模型再解析URDF 导入ROS 的方法完成机械臂建模，本文主要使用Linux 下手工编码的方式完成移动机器人建模，减少工具熟悉和学习的时间，但也对编写代码能力和对机器人关节关系理解要求较高。

2 ROS（Robot Operating System 机器人操作系统）

2.1 ROS综述

ROS 是2010年Willow Garage 公司发布的专为机器人软件开发所设计出来的一套开源操作系统。最早是基于ubuntu 系统设计，支持c++、java、python 等编程语言，极大地提高了代码的复用，加速了开发速度，减少了开发周期[6]。同时ROS 也是一种分布式松耦合框架，不同的执行单元被分散设计，形成独立的“黑盒”软件包，可以根据项目的实际需要进行组合，实现相应的实验需要。

ROS 把单个软件包称为节点，节点间通过发布和订阅主题的传递方式，发出节点将消息发布到指定节点，订阅节点以订阅目标主题的方式来获取数据信息[7]。

2.2 URDF(统一机器人描述格式 Unified Robot Description Format)

图1：在Gazebo 环境下的xacro 模型

图2：RViz 环境下的Astar 算法仿真

图3：RViz+Gazebo 自主导航仿真

表1：URDF 主要标签表

表2：Xacro 宏定义表

URDF 一种符合XML 规范，描述机器人结构格式，主要用于RViz 和Gazebo 中进行建图和物理仿真。其主要标 签和标签主要属性如表 1，＜?mxl＞标签声明版本信息，＜robot＞标签来命名机器人名称，其下通常包含＜link＞和＜joint＞标签来描述机器人的部件和关节，＜link＞内部有部件名属性，其下通常用＜visual＞描述部件形体和材质，＜collision＞用来配置碰撞矩阵相关参数，＜joint＞标签name 属性来给标签命名，type 定义两个部件间的连接方式，其下包含＜parent＞和＜child＞描述父子节点部件，＜origin＞描述关节量和旋转量，＜axis＞描述关节的旋转方向。

2.3 RViz和Gazebo综述

2.3.1 RViz

RViz 是ROS 官方提供的一款三维坐标可视化工具，可以将激光雷达、深度摄像头、里程计等传感器发布的数据以及机器人仿真模型和tf 坐标进行处理并可视化显示，并且RViz 拥有扩展机制，可以使用插件来丰富本身的功能。

2.3.2 Gazebo

Gazebo 是一款物理仿真软件，由ROS 功能包提供对Gazebo外部接口，由于Gazebo 为物理仿真环境，需要设置碰撞矩阵、刚体质量等参数使得代码维护扩充变得困难，为此通常用xacro 宏定义文件替代urdf 文件提高了代码的可读性、复用性和可扩展性。并且Gazebo 内部包含丰富的模型库和绘图调整工具，使得开发人员可以根据自身机器人设计情况进行事先模拟出工作环境，并可以将模拟搭建出的仿真环境进行保存成.world 文件进行重复调用。Gazebo 仿真环境在进行传感器选型和车体设计（如图1）方面，不但节约了开发成本降低资源浪费，也缩短了开发周期提高了研发效率，但如果在高要求高精度环境中作业的机器人也会因为xacro 建模语言忽略细节的大体表示而引起不可避免的误差，从而影响到测量精度和作业展开。

2.3.3 Xacro 格式

当机器人在Gazebo 物理仿真环境中做仿真时，所需要的物理参数更加繁多，URDF 模型代码冗长，重复过多，修改参数不便，没有参数运算功能的问题，既不方便阅读，也不便于二次开发的缺点显露出来，Xacro 在urdf 的基础上加入宏定义标签如表2，＜robot＞标签下通常先引用＜xacro:property＞标签进行参数声明，其内部name 和value 属性分别写出参数名和参数值，＜xacro:macro＞标签通常封装机器人部件的惯性矩阵等运算代码和部件节点的组合封装，将代码模块化，并用name 属性描述封装内容方便调用，最后使用base_footprin 虚拟部件来定义机器人坐标位置，并用＜gazebo＞标签调用插件发布或订阅节点信息。达到参数和代码分离的目的。

3 激光SLAM建图及实现

3.1 激光雷达传感器

激光雷达，是激光探测及测距系统的简称，其由激光器、接收器、信号处理单元、旋转机构组成的光学传感器。其工作原理是由作为辐射源的激光器发射探测信号，之后接收器将从目标物体反射回来的目标回波进行接收，最后由信号处理单元将目标回波和探测信号进行信号比对，并在此基础上由传动装置组成的旋转机构保持旋转检测。该电子设备不但可以获取目标物体的方位、距离、速度等参数，并且拥有较好的稳定性，较高的精确度，成为了被广泛用于移动机器人在二维未知环境中感知周围环境的主要传感器。

3.2 栅格地图

移动机器人进行自主导航的地图生成法有很多比如拓扑地图生成法[7]、特征点地图、直接表征法以及栅格地图生成法，拓扑地图生成法通常由后三种地图生成法通过相应算法提取得到，较为抽象；特征点地图通常是由激光雷达扫描获得数据或深度摄像头经过特殊算法处理过后的数据图像；而直接表征法是由传感器接收图像未经特征点化或栅格化处理得到的原始图像；本文主要使用通过栅格化处理的栅格地图上进行激光雷达特征点采集的房屋平面图建模，采用这种方法具有较好的空间表现，较为直观展现出房间主要形状及其比例，以及建模效果与原房屋的对比检验。

4 RViz+Gazebo联合仿真和激光雷达建图实现

4.1 Gmapping

Gmapping 是一款基于激光雷达，使用RBPF 粒子滤波算法的ROS 功能包，其通常用于二维栅格化地图，采用OpenSlam 开源算法的开源功能包。当移动机器人在一个未知的环境中移动，构建当前环境地图。地图可以用一个储存每个单元格颜色的二维矩阵表示，每个单元格的颜色只能用黑或白两种颜色。并且，Rao-Blackwellized 粒子滤波算法可以预先估计机器人的轨迹，而后再根据已知轨迹路径计算地图，解决同步定位与地图构建的问题[8]。

4.2 move_base

move_base 是一款ROS 提供的导航功能包，使得移动机器人可以实时的根据已知建图信息进行路径规划，并根据地图的完善实时更改最优路径。其实现原理是由全局路径规划Astar 算法或DijiKstra 算法进行全局方向把握，之后根据实际情况采用局部优化DWA 算法和TEB 算法进行局部优化。

Astar 算法，是采用DijiKstra 算法和贪心算法的启发式算法，其公式表示为：

f(n) 是从初始状态经由当前状态n 到目标状态的代价估计。g(n) 是在栅格空间中从初始状态到当前状态n 的实际代价。h(n) 是从当前状态n 到目标 状态的最佳路径的估计代价。

其算法实现如下：

（1）初始化open 队列和close 队列，并将起始节点加入open队列当中作为当前节点

（2）判断是否可以到达终结点，如果不能结束算法，返回false，否则进入（3）

（3）在8 方向中，选择当前节点的相邻节点中f(n)最小的节点放入open 队列中，并将open 队列根据f(n)大小进行排序，将f(n)最小的元素作为当前节点，并弹出open 队列，加入到close 队列中

（4）如果n 为终结点，则从open 队列中移除放入close 队列当中，最后对close 队列进行回溯遍历，算法结束，返回true，当给g(n)和h(n)赋予权重进行算法调整，当h(n)=0 时，f(n)=g(n)，此时Astar 变成Dijkstra 算法，这只保证路径搜索能找到最短路径。当g(n)=0 时；f(n)=h(n)，此时Astar 变为贪心算法，到达目标点的最优运行速度而不关心最短路径；故如果h(n)所预测的移动到目标所需要的代价刚好等于最短代价，则算法可以实现在非常快的情况下寻找出最优路径，而如果h(n)大于估计最短代价，则机器人从n 点运动到终结点路径会变长但运行速度更快。如图2（上）为在RViz 中使用grid_map 的Astar 算法的三维仿真，（下）命令行终端输出的运行时间、规划点数(nodes_size)、目标节点坐标（Position）、偏移角（Angle）等信息。

4.3 建图实现

通过Gazebo 构建地图仿真物理环境，RViz 订阅里程计数据、激光雷达数据、tf 坐标变换等数据，在add 选项中添加显示信息，会出现激光雷达点云信息、里程计信息等显示，最后根据建图需要，Gazebo 中的机器人模型会根据，RViz 机器人的移动而移动，RViz中的机器人模型会根据，Gazebo 的物理信息来调整位姿状态，再根据地图状态，启动导航节点进行自主导航如图3，为经过膨胀处理的RViz（左上）+Gazebo（右上）联合仿真实验，机器人模型根据/odom 息标注出机器人运行的方向，订阅的/scan 信息提取出激光雷达扫描的点云坐标显示在RViz 界面中，以及最后建图结果保存地图（右下）。

5 结语

本文基于ROS 平台搭建了Astar 算法和自主导航仿真环境，完成了自主导航方面从机器人模型，物理仿真环境搭建，激光雷达信息、里程计信息采集订阅，Astar 仿真地图搭建，返回算法参考重要数据进行比对等，相较于MATLAB 仿真，在RViz 中可以进行三维Astar 仿真用C++语言实现，但仿真代码冗余，使得算法仿真编码效率较缓慢、非机器人领域人员使用较少等问题亟待解决。