ROS2 环境下移动机器人导航及路径规划研究

江 航，邱胜海，孙小肖，王 清，郭子鹏，程泽鹏

（南京工程学院 机械工程学院，江苏 南京 211167）

0 引 言

随着新一代信息技术的发展，移动机器人相关技术得到迅速发展。目前在制造领域，大多数物料运输搬运机器人是自动导引小车（Automated Guided Vehicle, AGV）形式，其自身安装有感应电磁与光线等信息传感器，能够沿着预定轨道自主行驶。随着物联网的普及应用及智能制造与数字孪生的推广应用，传统的AGV 小车已经逐步被更智能、更自由、更安全的AMR 移动机器人取代。

ROS 系统以其开源性、灵活性佳在机器人领域广受关注，针对AMR 移动机器人，ROS 系统提供了导航所需的SLAM建图算法与路径规划算法，能够通过算法进行灵活自主的行驶和避障，增加了其机动性与安全性。目前国内外学者对智能制造、数字孪生及ROS 的机器人应用研究理论较多，在实际应用场合缺少规范及成熟的技术，如：S.Gatesichapakorn[1]等、P. Phueakthong[2]等、F. Martín[3]等在ROS 中利用2D 激光雷达、RGB-D 相机、PCL 获得三维深度点云用于障碍物检测，将2D 激光雷达和RGB-D 相机互补，增强了移动机器人导航功能；陶飞[4]等人针对新一代信息技术发展的背景，提出数字孪生的概念、系统组成、运行机制、特点、关键技术等，探讨了CPS 与信息交互和共融理论及实现方法；施佳宏[5]等人提出面向生产线仿真的数字孪生逻辑模型构建方法，并通过生产线实例仿真验证了建模方法的有效性；曹远冲[6]等人提出了一种基于数字孪生的复杂产品离散装配车间动态调度方法，构建了数字孪生的调度框架和调度模型，构建调度原型并验证了该方法的有效性和可行性；褚俊娴[7]以石化行业某包装车间为研究对象，对运输任务与AGV 之间的匹配策略和AGV 的行走路径问题进行探讨，建立AGV调度优化模型，并结合Pareto 理论与改进粒子算法求解调度问题，同时根据车间电子地图，应用改进A*算法解决单个AGV 的行走路径问题，为车间实现物流智能化提供重要的理论依据和技术支撑；温淑慧[8]等设计了基于ROS 的移动机器人自主建图与路径规划系统，利用基于头尾双向搜索的A*算法进行全局路径规划，采用DWA 算法完成局部避障工作及自主导航任务；王东霞[9]开发了移动机器人导航系统，实现了机器人的定位、绘图、导航等功能。综上所述，国内外学者对ROS 移动机器人进行了广泛研究，包括ROS1 系统的定位与导航等，但对ROS2 系统的研究却较少。ROS2系统增加了对多机器人系统的支持，同时在ROS1 的基础上支持MCU 等嵌入式微控制器，实现了跨平台运行。因此，基于ROS2 系统，对移动机器人导航及路径规划系统进行设计与开发，并通过自研的移动机器人进行现场测试，满足了系统的功能及性能要求。

1 AMR 导航与路径规划系统总体设计

AMR 导航与路径规划系统总体框架采用2 层分布式架构，从下往上分别是硬件层和软件层，如图1 所示。

图1 AMR 系统总体框架

系统2 层构成描述如下所示：

（1）硬件层：硬件层包括2D 激光雷达、IMU 以及轮毂伺服电机三种硬件。激光雷达是如今移动机器人领域最常见、最重要的传感器之一，通过360°全方位扫描提供平面点云的信息，使用驱动程序启动雷达工作；IMU 包含众多运动传感器，主要用来测量机器人角速度和加速度，本文中的IMU主要提供机器人的角度信息；轮毂伺服电机内带有编码器，主要能够提供轮毂转速、位置等信息，控制器通过编码器传递的脉冲来确定位置。

（2）软件层：软件层包含SLAM 建图模块和Navigation2导航模块。建图时需要激光雷达信息和机器人的位姿信息，其中，机器人位姿信息通过里程计与IMU 提供的数据融合之后得到。再使用SLAM Toolbox 包实现对周围环境的二维地图创建，保存后提供给导航使用。导航时，机器人启动之后通过调用Navigation2 包加载建成二维地图，通过Rviz2 可视化界面显示二维地图，确定机器人初始位姿，给出目标位置，调用路径规划算法或自定义优化算法实现自主导航与路径规划。该层在ROS2-Galactic 开发环境中实现。

软件层与硬件层之间通过Modbus 通信协议、MQTT 物联网通信协议进行连接与信息传递。

2 多传感器数据融合与地图构建技术

AMR 机器人状态信息包括位姿、行走距离、速度、旋转角度、电量消耗等，由不同的传感器采集，通过数据采集与控制系统对各类信息进行有效的数据融合，具体描述如下：

（1）2D 激光雷达：激光雷达分为节点的发布与订阅两部分，启动后可以在Rviz2 中看到激光雷达扫描到的平面点云的信息，这些信息将会在建成的二维地图中显示出来。

（2）轮式里程计计数及计算：轮式里程计通过统计编码脉冲在单位时间内相对增加或减少的值转化而来。每0.1 s读取一次编码器脉冲值，相邻两次读数之差即为0.1 s 编码器的增量值，依次推测出机器人行走的距离和速度，又由两轮编码器数值之差可以推算出机器人旋转的角度和角速度，在此基础上对小车进行里程计发布。里程计信息包括父坐标系、子坐标系、x坐标值、y坐标值、线速度、角速度等属性值。

（3）IMU 数据：IMU 主要通过陀螺仪测算出机器人旋转的角度值，其坐标系与里程计坐标系一致。

导航地图构建主要采用软件层的SLAM 建图模块完成，其过程为先扫描实际制造车间的三维地图，再在编码器里程计中设定坐标x，y以及偏航角速度vyaw为真值，IMU 中设定偏航角yaw 为真值，将这些参数整合，最后应用SLAM Toolbox 包建图。完成的二维地图如图2（a）所示，实际车间地图如图2（b）所示。

图2 车间布局地图对照图

3 移动机器人导航算法及实现

移动机器人导航系统基于ROS2 中搭载的Navigation2包实现。Navigation2 项目包继承了ROS 中的Navigation 项目，其核心组件是ROS2 系统，主要目的是寻找一条安全的路径控制机器人从A点移动到B点。使用Navigation2 包能够实现动态路径规划、动态移动速度计算、躲避障碍物等功能。Navigation2 中提供的全局路径规划算法是传统的Dijkstra 或A*算法，可以获得最短路径，但并没有考虑能量的消耗，并不适用制造车间物流检测AMR 的路径规划。本文提出一种考虑能量消耗的改进A*算法，综合考虑AMR 与地面摩擦、载重等因素，建立能耗模型并进行优化。

3.1 传统A*算法

A*算法常用于静态环境下最优路径的求解，其根据代价函数在静态环境下选择一条从起点到终点距离最优的路径。传统A*算法的代价函数如式（1）所示：

式中：g(n)表示起点到节点n的实际距离代价；h(n)表示节点n到目标点的预估距离代价。距离代价采用欧式距离计算，如式（2）所示：

式中：(xn,yn)、(xstart,ystart)、(xgoal,ygoal)分别代表的是节点n、起点start、终点goal 的坐标。

3.2 改进的A*算法

改进的A*算法在传统A*算法基础上增加了能耗最省约束条件，需要建立移动机器人能耗方程。AMR 能耗主要包括电子设备工作消耗的能量Eequipment和运动过程中所消耗的能量Emotion，因此AMR 能耗方程可表示为：

式中：pmotion为运动过程中的功率耗损；pequipment为机器人电子设备的功率耗损。

移动机器人在运动过程中克服牵引阻力所损耗的能量为：

式中：W为运动过程的总功；Ftraction为运动过程的总牵引力；Ff、Fair、Fg为运动过程中的摩擦力、空气阻力以及重力斜面水平分力；L为移动距离；移动机器人在运动过程中始终保持匀速行驶且速度低。因此，由式（3）、式（4）可知机器人在运动过程中的总能耗Erobot：

式中：m为机器人总质量；g 为重力加速度；μ为摩擦系数；φ为坡度；s为移动距离；k为比例系数。

为了表征复杂环境下机器人的移动距离和能量消耗，Mejri 等[10]构建了基于距离的邻接矩阵AD与基于能量的邻接矩阵AE。以能量矩阵AE(i,j)为改进A*算法的搜索准则：

式中：μi,j为位置i到位置j的平均摩擦系数。考虑现实坡度对机器人移动的能耗影响，采用水平地面的邻接矩阵Aφ(i,j)，其表达式如式（7）所示：

式中：Aφ(i,j)表示位置i到位置j的坡度大小以及坡度状态，相邻位置i与j数值差的绝对值代表对应区域间的坡度，正负表示移动机器人爬坡与下坡两种状态。结合式（1）、式（6）、式（7）可得基于能耗成本的路径搜索新准则：

3.3 AMR 导航系统的实现步骤

（1）配置TF 坐标变换，实现地图到里程计、里程计到底盘坐标系和底盘坐标系到各传感器坐标系的转换。

（2）设置里程计与传感器，该任务在建图时已经配置完成，此时启动相应节点即可。

（3）设定机器人的地面投影footprint，在Nav2 导航算法包中，将params.yaml 文件中footprint 的值修改为实际机器人地面投影大小的值。

（4）应用C++语言实现改进型A*算法，并将其命名为SeaStar。Nav2 已包含多个算法，例如Dijkstra、A*及混合A*算法。测试AMR 为二轮差速机器人，为了能在ROS2 中应用SeaStar 算法，只需在Nav2 的规划服务器中添加该算法，重新编译后便可调用SeaStar 算法。

（5）实体机器人进行导航测试，启动机器人节点，即启动雷达、IMU、里程计发布节点，在nav2_car 工作空间中运行所保存的地图map.yaml。使用系统中的“2D Pose Estimate”功能，系统估算出机器人的实际位置，启动导航。使用“Nav2 Goal”功能，在地图上指定一个目标位置，AMR 便能够通过SeaStar 算法规划出一条能耗最小路径，通过计算AMR 电池能量消耗数值，与传统A*算法进行比较，改进SeaStar 算法节省约14.6%的能耗。

4 结 语

对AMR 导航系统和路径规划算法进行研究，通过参考ROS2 系统中提供的Slam Toolbox 包和Navigation2 包设计基于ROS2 的机器人导航系统。为使AMR 在运动过程中能耗最小，提出一种改进A*算法并在ROS2 中实现。对自研的AMR 进行试验，结果表明，该系统能够实现快速建图与最小能耗路径规划，为低碳制造提供了可行的途径。

注：本文通讯作者为邱胜海。