基于ROS的轮式机器人建模与路径规划仿真

李宏达，范继祥

(哈尔滨师范大学)

0 引言

随着近几年机器人行业的快速发展[1]，自主移动机器人成为了机器人行业发展和投资的一个热点，也是机器人技术发展的必然趋势[2-4].自主移动机器人[5]能够通过传感器感知周围陌生环境和估计自身所处位置的状态，在一个有障碍物的陌生环境中实现一个由出发点驶向目标点的自主移动，从而实现人们的作业需求[6].相比较其他类型机器人，自主移动机器人具有小巧灵活、操作简单等优点，可以广泛的应用于军事、医疗和其他日常生产生活中[3-4].新冠疫情期间，基于自主移动机器人改进的消毒机器人和转运机器人在防疫过程中发挥了巨大的作用.

目前主流的SLAM(Simultaneous localization and mapping)算法[7-9]已经与具有高扫描频率的测距传感器相结合，通过传感器数据与扫描地图互相验证确定机器人的位姿.在室内环境下基于RBPF(Rao-Blackwellized Particle Filter)的粒子滤波算法已经优化成ROS(Robot Operating System)中的一个功能包gmapping[10]，该功能包可将传感器采集到的信息整合分析，从而生成二维栅格地图.该文使用ROS[11]开源系统作为机器人开发平台.利用底层设备控制、进程之间消息传递、包管理等功能对双轮差速机器人及其周围空间环境进行建模和仿真，对机器人的路径规划、自主导航与避障系统进行研究.

1 机器人系统模型

1.1 坐标系建立

为了描述机器人在空间中的位置和姿态，建立了如图1所示坐标系模型.XOY坐标系为惯性坐标系，XRORYR为机器人坐标系，XR方向为机器人前进方向，因传感器安装在机器人本体上，所以二者共用一个坐标系.

图1 坐标系模型

机器人的位姿用三维向量(X,Y,θ)T表示，该向量包括了机器人在平面环境中的空间位置(X,Y)T和姿态角θ.姿态角θ为机器人坐标系的YR轴与惯性坐标系Y轴 (或XR与X轴)之间的夹角，用来描述机器人的运动方向，具体定义为：以Y轴(或X轴)的方向为0°，逆时针方向为正，姿态角θ的范围为-180°～ +180°.

1.2 机器人里程计运动模型

机器人在定位、地图构建和移动过程中都需要采集里程计信息[12]，里程计信息为机器人在每2个时间间隔内位置和姿态的变化量.该文所设计的双轮差速机器人里程计运动模型如图2所示.

图2 机器人里程计运动模型

在惯性坐标系中，机器人从A点到B点做圆弧运动，转动角度为dθ.相对于以A点为原点的机器人坐标系，B点的位置变化为：

(1)

式中，R为机器人做圆弧运动所对应圆的半径，与左右轮子的角速度有关.根据当前时刻两个电机的瞬时角速度可求解得到(dx,dy,dθ)T，进而可推出机器人的运动模型：

(2)

1.3 机器人激光雷达观测模型

在SLAM过程中，需要激光雷达传输回来的2个数据：路标点P到雷达的距离r和路标点P与当前雷达基准方向夹角φ，如图3所示.

图3 机器人激光雷达观测模型

当前路标点P坐标为(px,py)，根据当前时刻由里程计运动模型计算得到机器人位姿[xtytθt,]T，求得雷达观测模型：

(3)

式中，vt为观测时的噪声.

2 机器人SLAM与导航

未知环境中的机器人自主导航已经成为了机器人发展过程中一项极为关键的技术[13].SLAM问题描述为机器人在一个陌生环境的未知位置开始移动，在移动的过程中根据传感器传回的信息进行自身位置估计，同时构造增量式地图.

2.1 SLAM方法架构

SLAM使用卡尔曼滤波器对采集到的环境数据和测得的机器人运动模型进行状态估计，进而得到地图数据和机器人姿态，其方法架构如图4所示.

图4 SLAM方法架构

首先，通过激光雷达和摄像头等传感器对周围未知环境进行数据采集并提取环境特征；然后，利用里程计、IMU惯性单元等对机器人本体信息进行感知得到机器人运动模型；进一步，使用扩展卡尔曼滤波器对得到的传感器观测值和机器人运动模型进行处理，得到地图数据和机器人姿态估计；最后，将得到的估计数据与实际的地图信息和机器人姿态进行校验，验证结论数值的准确性.

2.2 SLAM功能包与导航框架及算法

2.2.1 gmapping功能包

gmapping功能包为目前在ROS中最成熟也是应用最多的一种SLAM功能包.gmapping功能包总体框架如图5所示.

图5 gmapping功能包总体框架

将激光雷达测得的深度信息、IMU信息、里程计信息输入到功能包中，使用Rao- Blackwellized粒子滤波算法来创建二维栅格地图，输出nav_msgs/OccupancyGrid地图话题.

2.2.2 基于move_base的导航框架

move_base功能包主要完成全局路径规划和本地实时规划[14]2个工作.全局路径规划主要通过Dijkstra或A*算法来进行全局最优路径规划.本地实时规划主要完成的工作为规划机器人每个周期内的线速度、角速度，帮助机器人行驶路径尽量符合全局最优路径；通过传感器信息判断障碍物并进行躲避.move_base导航框架如图6所示.

由图6可以看出，将导航定位信息、激光雷达信息、里程计数据信息进行坐标变换得到机器人的位置姿态输入到框架中.全局权值地图节点读取地图信息节点输出的nav_msgs地图话题进行全局规划，局部权值地图根据激光雷达/Kinect节点传入的sensor_msgs话题实时更新地图信息并进行局部规划，躲避新出现在地图中的障碍物.机器人底盘控制节点接收cmd_vel(速度控制指令)信息实现机器人的运动控制.

2.2.3 A*路径规划算法

A*路径规划算法[15]是一种静态路网中求解最短路径最有效的直接搜索方法，也是许多其他问题的常用启发式算法.公式表示为：

图6 move_base导航框架

f(n)=g(n)+h(n)

(4)

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

以d(n)表示状态n到目标状态的距离，那么h(n)的选取有如下3种情况：

(1)如果h(n)

(2)如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行， 此时的搜索效率是最高的.

(3)如果h(n)>d(n)，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解.

3 机器人及空间环境建模仿真

3.1 机器人建模

该文使用Rviz+Arbotix来对机器人进行整体建模.在Rviz中，URDF文件使用可扩展标记语言XML[16]对机器人的刚体外观、坐标位置、关节类型等方面进行描述，并在界面中呈现出来；同时还可以通过图形化的方式实时显示传感器数据、机器人运动状态、周围环境的变化等信息.ArbotiX所配套的ROS功能包提供了一个差速控制器，通过接收速度控制指令更新机器人的joint状态，从而实现机器人在rviz中的运动.该文所建立的URDF模型整体结构如图7所示.

图7 URDF模型整体结构图

x,y,z)坐标和(r,p,y)坐标可以获得各个节点在模型中的位置关系.

机器人模型在Rviz中可视化显示如图8所示.

图8 机器人仿真模型

3.2 机器人运动环境仿真

机器人运动环境的建立使用Gazebo三维动态物理仿真器，其具备强大的物理引擎、高质量的图形渲染、方便的编程和图形接口，能够准确高效地仿真在复杂的室内外环境下机器人群体.机器人的传感器信息也可以通过插件的形式加入到仿真环境中并进行可视化的显示，为程序和用户提供交互接口，其典型应用场景包括：测试机器人算法、现实情境下的回溯测试等.

在Gazebo仿真器中设计如图9所示机器人运动环境，机器人所处环境为一个长方形的封闭空间，内部分割成三个小房间，机器人可以通过各个房间之间的小门在该环境中自由移动.

图9 机器人运动环境仿真图

控制机器人的移动对机器人运动环境进行扫描并将传感器采集到的深度信息和里程计信息输入到gmapping功能包中，建立栅格地图过程如图10所示.

图10 SLAM仿真过程及结果

由图10可以看出，机器人首先扫描房间①的地图(如图10a所示)，然后扫描房间②的地图(如图10b所示)，其次扫描房间③的地图(如图10c所示)，最后生成gmapping SLAM地图(如图10d所示).结果表明，激光雷达能够成功扫描各个房间的边界，并成功构建地图.

4 仿真过程及结果分析

该文机器人路径规划分为2种情况进行仿真：第1种为无临时障碍物的路径规划仿真，第2种为有临时障碍物的路径规划仿真.

对于无临时障碍物的情况，其运动环境如图9所示.图11为此种情况下的机器人路径规划仿真图，图中A点为出发点，箭头尾部B点为目标点，箭头方向为机器人最终姿态方向.

图11 无临时障碍物的机器人路径规划仿真图

由图11可以看出，机器人从房间③的A点出发(如图11a所示)，沿着规划好的路径移动，经由房间③和房间②之间的门到达房间②的目标点B(如图11b所示)，到达目标点B后机器人保持设定的姿态(如图11c所示).

对于存在临时障碍物的情况，设置新的出发点C和目标点D，D点的箭头方向为机器人最终姿态方向，在机器人运动过程中放置临时障碍物M，其运动环境如图12所示.

图12 存在障碍物时机器人运动环境仿真图

图13为有临时障碍物的机器人路径规划仿真图，由图可知，机器人从房间②的C点出发(如图13a所示)，沿着规划好的路径移动(如图13b所示).到达E点时探测到前进路线上放置的临时障碍物M(如图13c所示)，机器人对运动路径进行了重新规划，并沿着新的路径运动.当运动到障碍物M的左下角时，探测到障碍物的下侧面，再次重新进行路径规划并沿着新路径运动(如图13d所示)，当机器人运动到障碍物的右下角时，探测到前进方向上无其他障碍物(如图13e所示)，重新规划最短路径到达目标点D，并保持设定姿态(如图13f所示).

图13 有临时障碍物的机器人路径规划仿真图

仿真结果表明：机器人可以在无临时障碍物出现的情况下进行自主规划路径与导航；在行进过程中遇到临时障碍物，能够及时发现障碍物并重新规划路径进行规避，实现机器人的路径规划、自主导航与避障的功能.

5 结束语

该文搭建了基于ROS的导航仿真平台，在Rviz中建立了双轮差速机器人模型，并在Gazebo中搭建了机器人运动环境.机器人可以在未知的空间环境中通过传感器进行SLAM地图构建进而生成二维栅格地图，并且在Rviz中实时观看地图建立过程.应用A*导航算法，针对所构建的地图该机器人能够成功实现自主规划最优路径及导航功能；若在行进过程中遇到临时出现的障碍物，能够通过激光雷达传感器及时检测到障碍物，并更新前进路径，避开障碍物，进而成功到达目标点，实现机器人的路径规划、自主导航与避障功能.对于后期搭建机器人硬件平台并在真实环境中进行验证具有一定的指导意义.