基于ROS的全向移动机器人导航系统设计与仿真

Design and Simulation of Omnidirectional Mobile Robot Navigation System Based on ROS

摘要: 动态环境复杂多变，机器人如何在动态环境下实现自主移动是一个难题。本文以麦克纳姆轮底盘为对象，基于机器人操作系统(ROS)，设计并实现了开放性好和代码复用率高的全向移动自主导航系统。首先，先对麦轮底盘进行URDF建模，运动学分析，根据得到速度关系重写新的底盘ROS节点；其次结合MOVE_BASE框架，搭建自主导航系统，利用SLAM技术构建二维栅格地图，结合AMCL和路径规划算法的融合导航算法开展自主导航测试。实验结果表明，该方法可以实现机器人的自主移动和避障要求，提升路径规划效果，验证了机器人自主导航的可行性。

Abstract: Dynamic environment is complex and changeable, so it is a difficult problem for robot to realize autonomous movement in dynamic environment. Based on the Robot Operating System (ROS), this paper designs and implements an omnidirectional mobile autonomous navigation system with good openness and high code reuse rate based on the Mecanum wheel chassis. Firstly, URDF modeling and kinematics analysis were carried out for wheat wheel chassis, and new chassis ROS nodes were rewritten. Secondly, the autonomous navigation system was built by combining MOVE_BASE framework, 2D raster map was constructed by SLAM technology, and autonomous navigation test was carried out by combining AMCL and path planning algorithm. Experimental results show that this method can achieve the requirements of autonomous robot movement and obstacle avoidance, improve the effect of path planning, and verify the feasibility of autonomous robot navigation.

1. 引言

近年来，随着智能制造、网络信息和高集成度芯片等新兴技术的飞速发展，移动机器人在智能仓储、智能工厂和物流运输等行业内得到了广泛的应用 [1]。但在非线性结构环境下，机器人容易受到环境、随机障碍物等外界因素等影响，从而导致自主导航失效。而机器人在进入没有先验信息的未知环境下，需要解决三个核心问题才能实现自主导航，即地图建模、自我定位和路径规划 [2]。针对前两个问题，同步定位与建图(SLAM)技术应运而生，通过机器人自身携带的传感器，感知周围环境、获取深度信息以实现对位置环境地图的重建。目前根据后端优化主要分为基于滤波的激光SLAM和基于图优化的视觉SLAM。视觉SLAM通常采用单目相机、双目相机和深度相机传感器感知环境，代表算法有VINS MONO、ORB_SLAM系列；激光SLAM则采用毫米级激光或激光雷达；代表算法有EKF-SLAM、Gmapping。

路径规划作为自主导航的核心，分为全局路径规划和局部路径规划。常用的全局路径规划算法有A*算法、蚁群算法以及RRT算法，针对原始算法存在的不足，国内外学者也做了大量的研究。Koenig S [3] 等人提出了一种即实时自适应A*算法，可以加快重复A*搜索速度；李静 [4] 等人提出了一种改进蚁群算法，通过概率选择函数，栅格法地图建模和改进信息素等方式提高了路径的全局规划能力和搜索能力；Gammell J D [5] 等人提出了Informed RRT*算法，在保证搜索路径质量的前提下缩短了RRT原始算法的路径搜索时间。

基于此，本文基于机器人操作系统(Robot Operating System, ROS)设计了一个由麦克纳姆轮驱动的全向移动导航系统。首先在GAZEBO中建立URDF机器人模型和地图环境，针对传统IMU存在的严重误差累积问题，融合IMU和AMCL信息增强定位；其次结合MOVE_BASE导航框架，在GAZEBO以及RVIZ中进行实验测试，使用基于ROS的A*导航算法和TEB规矩优化算法实现自主移动及动态避障。

2. 机器人建模与底盘节点

URDF全称(United Robotics Description Format)统一机器人描述格式，是一个XML语法框架下用来描述机器人的语言格式 [6]。为了更为直观地进行仿真和观察机器人的运行状态，使用URDF文件对机器人进行建模。通过定义机器人的关节、自由度，世界坐标、惯性矩阵、碰撞属性、质量等属性来尽可能地模拟真实机器人。

1) 麦轮底盘设计

编写底盘的URDF文件，使其能够接收由导航节点发布的cmd_vel指令、发布里程计数据。首先在SOLIDWORKS中创建机器人模型，并计算惯性参数和碰撞属性；其次在URDF文件中为车体框架和麦轮添加惯性参数和碰撞属性，对麦轮在额外添加传动装置和减速比；最后通过控制器插件(ROS_Control)配置机器人的移动模式，完成底盘设计。URDF结构图如图1所示，其中base_footprint为车体中心投影点，起定位作用，base_link表示车体部件，link1~link4表示麦轮部件，laser表示激光雷达，蓝色字体部分表示各关节状态，joint1~joint4为转动关节，其余为固定关节。

Figure 1. URDF structure diagram

图1. URDF结构图

2) 底盘节点设计

当前普通差速轮式机器人在狭窄空间环境或者复杂结构环境中表现不佳，往往出现移动困难、转向失败等问题。其原因为常见的差速轮子只有X方向的线速度和Z轴的角速度，无法进行全向移动，导致在狭窄空间中卡死。而麦克纳姆轮由于包含三个自由度(X、Y方向上的线速度以及Z轴的角速度)，转向时可以实现零转弯半径的旋转运动，适用性更广，可在多场景领域内应用。为此需要重新编写新的ROS麦轮底盘节点，更新Y方向上的线速度，底盘与麦轮的速度关系可由运动学模型分析得到。

ROS节点可以简单的理解为一个可执行文件，所有节点受RosMaster管理，节点之间通过话题通信、节点内部信息通过服务通信传输信息数据。对于麦轮底盘，其节点主要包含以下功能：① 在回调函数中添加麦轮底盘与轮子之间的速度关系公式，等待速度指令订阅消息、接收cmd_vel话题数据；② 发布里程计信息，将小车的位姿信息传递给ROS管理器。ROS节点可以通过订阅由导航节点发送的cmd_vel话题数据，控制小车行驶速度，然后根据底盘轮子直径和转速预估底盘的位置。

图2为底盘节点设计流程图，首先初始化底盘节点，创建节点句柄，根据句柄添加实例化对象，根据麦轮运动学模型在回调函数内添加麦轮与底盘之间的速度关系；其次底盘节点发布速度消息、订阅速度话题驱使小车运动；最后初始化里程计信息，根据行走的姿态数据计算里程计信息，在TF坐标变换后更新信息并发布给RosMaster。

Figure 2. Chassis node design process

图2. 底盘节点设计流程

3. 麦轮底盘运动学模型

和普通四轮小车不同的是，麦克纳姆轮在接收导航节点发送的cmd_vel速度控制指令时，需要额外增加一个Y方向上的速度分量。因此有必要先对麦轮进行运动学分析，以获取各个坐标方向上的分量速度，为机器人的全向移动提供理论依据。图3为麦轮安装结构，轮子布局方式采用“O”型安装方式 [7]，其中，辊子的角度为45˚， $\omega$ 表示底盘的旋转速度， $v_x$， $v_y$ 分别为底盘在X、Y轴上的线速度； $l_1$， $l_2$ 分别为轮子中心到底盘重点之间的距离； ${\omega }_1$， ${\omega }_2$， ${\omega }_3$， ${\omega }_4$ 分别表示麦轮的旋转速度。

Figure 3. Installation position of Mecanum Wheel

图3. 麦轮安装示意图

以1号轮为例，1号轮相对于地面的速度为：

$v_1=\left[\begin{array}{cc}0& \sin \alpha \\ r& \cos \alpha \end{array}\right]\left[\begin{array}{c}{\omega }_1\\ v_{1r}\end{array}\right]$ (式3-1)

1号轮相对于车体中心的速度为：

$v_1=\left[\begin{array}{ccc}1& 0& -l_2\\ 0& 1& l_1\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]$ (式3-2)

联立(式3-1)和(式3-2)，

$\left[\begin{array}{cc}0& \sin \alpha \\ r& \cos \alpha \end{array}\right]\left[\begin{array}{c}{\omega }_1\\ v_{1r}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -l_2\\ 0& 1& l_1\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]$ (式3-3)

可得：

${\omega }_1=\frac{1}{r}\left[\begin{array}{ccc}-\frac{1}{\tan \alpha }& 1& l_1+\frac{l_2}{\tan \alpha }\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]$ (式3-4)

同理，对其他轮有：

$\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\\ {\omega }_4\end{array}\right]=\frac{1}{r}\left[\begin{array}{ccc}-\frac{1}{\tan \alpha }& 1& l_1+\frac{l_2}{\tan \alpha }\\ \frac{1}{\tan \alpha }& 1& -l_1-\frac{l_2}{\tan \alpha }\\ -\frac{1}{\tan \alpha }& 1& -l_1-\frac{l_2}{\tan \alpha }\\ \frac{1}{\tan \alpha }& 1& l_1+\frac{l_2}{\tan \alpha }\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]$ (式3-5)

由于辊子的偏置角为45˚，所以化简整理得麦轮的逆运动学模型：

$\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\\ {\omega }_4\end{array}\right]=\frac{1}{r}\left[\begin{array}{ccc}-1& 1& l_1+l_2\\ 1& 1& -l_1-l_2\\ -1& 1& -l_1-l_2\\ 1& 1& l_1+l_2\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]$ (式3-6)

通过反解逆运动模型(式3-6)得到底盘的正运动学模型：

$\left[\begin{array}{c}{v}_x\\ v_y\\ \omega \end{array}\right]=\frac{4}{r}\left[\begin{array}{cccc}-1& 1& -1& 1\\ 1& 1& 1& 1\\ \frac{1}{l_2+l_1}& -\frac{1}{l_2+l_1}& -\frac{1}{l_2+l_1}& \frac{1}{l_2+l_1}\end{array}\right]\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\\ {\omega }_4\end{array}\right]$ (式3-7)

4. 导航系统设计

机器人的导航设计可分为3个部分，地图构建、自身定位和导航规划，三者之间彼此联系。在开始导航之前，首先需要通过SLAM技术得到全局地图信息；其次，基于IMU的轮式定位在长时间的运行下会不可避免地产生严重的累计误差，因此为了提高定位效果，在导航中融合AMCL算法增强定位的鲁棒性；最后则是利用导航算法实现全局地图的自主行走和避障。对于全局地图规划，采用A*算法寻找最优路径；利用基于TEB算法的局部路径规划器进行实时调整和避障。

4.1. 路径规划算法

1) A*算法

A*算法综合了Dijkstra算法和贪心算法均倾向于取最近节点信息的优点，因此即可以在两点之间找到最短路径，同时也能像贪心优先搜索算法一样，搜索最贴近目标点附近的节点信息，进而逐渐逼近目标点。基本原理为：A*算法最重要的是代价函数F(n)的设置，通过定义代价函数得到最小的值，进而确定最短的路径。代价函数由两部分组成(G(n)、H(n))，分别表示状态点距离初始状态的实际代价以及距离目标状态的估计代价，通常以F(n)=G(n)+H(n)表示。

2) TEB算法

TEB算法(Timed Elastic Band)针对A*算法生成的初始轨迹进行后续修正，从而得到平滑优化后路径。具体原理为：在相同的指定时间区间内以固定频率插入多个状态点，将连续时间内规划出的路径近似的认为是一条可以形变的时间弹性带，通过对弹性带实施约束，受力发生形变，这种形变就是它内部的优化算法。通过对其优化，便能找到满足各种约束的最优可行路径。

4.2. 地图建模与定位

1) Gmapping地图建模

Gmapping是基于滤波SLAM框架的开源SLAM算法，可以实时构建环境地图且构建后的地图精度较高，在ROS中通过输入里程计数据和激光雷达数据输出二维栅格地图。图4为实现的原理框架：

Figure 4. Gmapping diagram building process

图4. Gmapping建图流程

Gmapping通过运动模型给出预测后，以该预测为初值进行一次扫描匹配。找到一个使当前观测最贴合地图的位姿(在高斯分布内表现一个尖峰区域)，之后根据该区域所代表的高斯分布的均值和方差进行计算，得到真实分布的最佳近似。同时，新的粒子继续在高斯分布中重采样得到。

2) AMCL融合定位

由于IMU在长时间运行下会发生严重的累计误差，此外，为了减少机器人在运动过程中发生的打滑现象，采用AMCL (自适应蒙特卡洛定位)融合IMU信息对机器人进行增强定位。通过对传感器数据和从地图反馈回来的预估传感器数据相比较，判断出机器人的真实位姿。动态环境下，AMCL的每个粒子都代表机器人位姿的估计，通过粒子滤波器跟踪机器人位姿，对实际感知状态和粒子群进行价值评估，使其不断趋近于机器人在地图中的真实位置。基于ROS实现的TF坐标变换对比如图5所示：

Figure 5. Comparison of TF coordinate trees

图5. TF坐标树对比图

4.3. 导航规划

MOVE_BASE是一个开源的导航框架，通过编写文件后缀名.yaml将文件内部信息导入给路径规划层，路径规划层又分为全局路径规划和局部路径规划。在己知环境地图下，全局路径规划为移动机器人计算出一条静态路径，可以快速找到移动机器人位置与目的地之间路径最优解，但是，当已构建环境地图中未知障碍物时，全局路径规划算法比较容易发生导航错误。因此需要结合局部路径规划实现避障。局部路径规划算法在沿着静态路径运动过程中，动态地对机器人进行运动控制，实现在导航过程中机器人的最优运动。MOVE_BASE工作流程如图6所示：

Figure 6. MOVE_BASE working flow chart

图6. MOVE_BASE工作流程图

首先通过开源SLAM算法Gmapping进行地图构建，将所建立好的地图上传到ROS中的参数服务器以供map_server节点调用。MOVE_BASE的路径规划层根据地图服务信息进行路径规划，结合定位算法向底盘节点发送速度指令(cmd_vel)驱使电机运动，实现机器人的行走，部分关键参数配置表如表1所示：

说明如下：

1) 全局地图参数：设定地图坐标系/map和基坐标系/base_footprint，以供接收地图信息，为了更好地接收地图信息，将发布频率和更新频率设置为1。

2) 局部地图参数：设置了地图坐标系、基坐标系以及局部地图尺寸大小。

3) 通用地图参数：设置机器人的外形尺寸，模拟机器人的障碍边界，源传感器的坐标系；为了保护机器人安全，设置冗余尺寸及膨胀半径。

4) 本地规划器：设置了机器人的速度和加速度，当机器人卡出时的退出速度，麦轮在y方向上的速度等。

Table 1. Key parameter configuration table

表1. 部分关键参数配置表

5. 实验结果分析

仿真环境为ROS-Noetic版本，操作系统为Ubuntu 20.04，仿真器选用Gazebo物理仿真平台。在GAZEBO中建立模拟仿真环境，地图环境尺寸为：(长10 m，宽8 m)，如图8(a)。实验目标为地图建模过程中的性能分析，定位效果分析以及自主避障测试；观察所建地图是否有缺失、定位效果是否失效以及是否能够完成自主避障；实验开始前需要配置好机器人的具体属性参数。

5.1. 地图建模与定位结果分析

地图建模过程如下：

(a) (b) (c)

Figure 7. Gmapping diagram building process. (a) Start of map construction; (b) Mapping process; (c) End of map establishment

图7. Gmapping建图过程。(a) 开始建图；(b) 建图中；(c) 建图结束

首先打开Gazebo仿真环境，并导入机器人模型，小车初始出现的位置在地图的左下角；其次启动Gmapping节点，在RVIZ中点击2D_GOAL按钮指定目标点，小车向目标点前进并感知到更多的地图信息，直至完成地图的构建；最后使用map_server地图服务器工具保存创建好的地图。图7为地图建模的过程，可以发现，在指定目标点后，小车会自动前往所选目的地，感知周围环境信息及地图建模。

Gazebo物理仿真环境及建立完成后的地图如下，地图的分辨率大小为0.02：

(a) (b)

Figure 8. Comparison diagram after drawing construction. (a) Physical simulation environment; (b) The completed two-dimensional map

图8. 建图后的对比图。(a) 物理仿真环境；(b) 建立完成后的二维地图

图8建图后的对比图，图8(a)为gazebo物理仿真环境，图8(b)为经Gmapping节点启动后建立的地图。可以发现，所得的地图与物理仿真下的环境相差无几。

5.2. 导航结果分析

为了测试在动态环境或未知环境下的导航效果，开展机器人的自主移动和避障测试。

(a) (b)

Figure 9. Experimental comparison of obstacle avoidance. (a) Obstacle avoidance simulation in Gazebo physical simulation environment; (b) Obstacle avoidance simulation of raster map under Rivz

图9. 避障实验对比。(a) Gazebo物理仿真环境下的避障模拟；(b) Rivz下的栅格地图避障模拟

图9为避障实验对比，图9(a)为Gazebo物理仿真环境下的避障模拟，图9(b)为Rivz下的栅格地图避障模拟。首先，在地图中随机选择目标点，可以发现，全局路径规划算法在机器人的基点到目标点之间规划出了一条绿色的路径，并且机器人逐渐向目标点前进；当向GAZEBO中添加障碍物后，DWA局部路径规划算法驱使小车移动绕开了障碍物，并且在RVIZ测试平台中逐渐补全地图信息，所建地图和GAZEBO中模拟环境中的地图差别不大，实现了良好的导航和避障效果。

6. 结语及展望

本文首先对麦轮进行了运动学分析，重新设计了ROS底盘节点。结合ROS开源平台，编写了机器人模型，通过传感器感知里程计信息和深度信息建立了栅格地图，结合MOVE_BASE导航框架规划路径和动态避障，设计并实现了移动机器人自主导航功能。试验结果表明，该机器人可以有效地满足自主导航需求，能够完成实时避障，在未知环境下实现自主移动有积极意义，为之后程序移植到本体样机上提供了技术支撑。

基金项目

重庆科技学院研究生科技创新计划项目，“基于视觉引导的搬运机器人的路径规划研究”(YKJCX2020313)。

NOTES

^*通讯作者。