基于ROS的阿克曼机器人室内导航实现

盖军雄，雷晓春,2，江泽涛,2

(1.桂林电子科技大学 计算机与信息安全学院, 桂林 541004;2.桂林电子科技大学 广西图像图形与智能处理重点实验室, 桂林 541004)

0 引言

随着人工智能技术和芯片性能的发展，出现了具备各种功能的机器人，在众多室内移动机器人中，以两轮差速结构机器人最为常见。但相较于两轮差速机器人，基于阿克曼结构的机器人室内室外场景均适宜，实际应用场景更加广泛。具有阿克曼结构的移动机器人也叫Car-Like机器人，即为像车一样的机器人，因为与真实汽车外形相似，运动结构相似，可以应用于物流配送、农耕、教育等领域，目前在无人物流配送领域应用常见。为探索阿克曼结构机器人在室内的应用场景，本文提出了一个基于ROS[1]系统、具有阿克曼转向结构的室内移动机器人(下文简称阿克曼机器人)，利用SLAM技术、传感器融合技术、互补滤波算法、时间弹性带(Timed Elastic Band，TEB)算法、局部路径规划算法等技术实现了阿克曼机器人在室内环境的定位和导航。

1 阿克曼机器人控制系统

1.1 阿克曼机器人系统架构

机器人搭载有激光雷达、摄像头、IMU(惯性测量单元)、带有编码器的电机，嵌入式计算硬件等设备，阿克曼机器人硬件架构如图1所示。嵌入式计算硬件(RK3399)作为机器人大脑，运行ROS系统。系统中激光雷达、IMU、带编码器的电机是主要的感知硬件，其中激光雷达负责阿克曼机器人对环境的感知，IMU负责阿克曼机器人自身的位姿变化的感知，带编码器电机作为机器人的里程计通过编码器记录机器人的里程信息，三者结合在阿克曼机器人的建图、定位和导航上起到重要作用。

图1 阿克曼室内移动机器人硬件架构

1.2 阿克曼结构运动学原理

对机器人运动的精准控制基于对机器人建立的数学化模型。模型精确越高，对机器人运动的控制效果和精度就会越好。

当前，室内机器人以两轮差速结构机器人最为常见，这种结构通常具有一个万向轮作为前轮，两个后轮通过电机驱动作为动力轮，通过控制两个后轮不同的转速控制机器人移动。本系统的阿克曼结构和两轮差速结构的区别主要在于，两轮差速结构的前轮为万向轮或全向轮，而阿克曼结构的前轮与路上常见的汽车结构相同，是两个普通的单向轮。其运动包含两大核心部件：转动结构，控制前轮转向；差速器，驱动后轮以及控制后轮的差速运动。阿克曼结构在直线行驶时，四个车轮的轴线互相平行。转弯时，前轮转动的角度是不同的，要使阿克曼机器人实现纯滚动运动，必须保证机器人的四个轮子运动方向的法线相交于一点，该点为机器人的转向中心点，如图2点O，由于机器人前轮的转动角度是有限的，因此阿克曼模型存在最小转弯半径这一运动约束。阿克曼机器人的运动模型描述在图2中用实线表示。后轴中心C设为阿克曼机器人的基坐标系，阿克曼机器人前后轴的轴距为L，两后轮的轮距为K。O点为阿克曼机器人的瞬时旋转中心，R为瞬时旋转半径，左前轮的瞬时转向角设为α，右前轮的瞬时转向角设为β。阿克曼结构的核心公式是：

图2 阿克曼运动模型与单车模型

(1)

四轮的阿克曼运动模型可以等效简化为图2中虚线所示的单车模型，这是因为两种模型在运动机理方面可以视为一致，通过分析单车模型也可以更方便地研究对阿克曼机器人的控制。图中θ为单车模型前轮的转向角，可以视为整个阿克曼机器人的转动角度，同样的，阿克曼机器人此刻的瞬时旋转半径为R，前后轮间的轴距为L，O为阿克曼机器人的旋转中心，在这个瞬时状态下转弯半径与转向角有以下关系：

(2)

在ROS系统中对阿克曼机器人进行实际的运动控制时，控制消息主要包含机器人X、Y、Z三轴的角度和线速度，由于机器人主要运动在2D平面，因此只需要考虑Z轴方向的转角和X轴方向的线速度，对应真实车辆的转向和油门。

对于阿克曼机器人的转向控制，接收到系统上层发送的转向信息是机器人应具有的瞬时转向角度。机器人自身轴距L已知。应用公式(2)，便可以得到车辆在此刻的转弯半径R，如图2。再次应用公式(2)，可以得到左前轮的转向角关系式为：

(3)

右前轮的转向角关系式为：

(4)

对公式(3)、公式(4)进行逆解，得到左右前轮的转向角。

阿克曼机器人的转向结构使用了如图2所示的梯形机械结构，这样便无需考虑对每个前轮的单独控制。本系统使用一个舵机对机器人进行转向控制实验，根据左右前轮的转向角度、机器人运动方向与舵机的转动角度，反复实验后得到了舵机转动角度与机器人运动方向的映射关系，这样使用一个舵机控制梯形机械结构的转动便可以实现阿克曼机器人的转向控制。由于前轮转向的机械结构转动角度是有限制的，通过将左(右)轮转动到最大角度，通过公式(3)(公式(4))便可以得到最小的转弯半径这一限制参数，方便后续控制算法使用。

对于机器人的速度控制，设小车的前进速度为V，左后轮速度为VL，右后轮速度为VR，由角速度的一致性可得：

(5)

结合公式(2)和角度信息得到转弯半径R，各动力轮需要提供的速度关系如下：

(6)

(7)

得到左右后轮速度信息后，结合小车传动比和车轮半径便能得到每个驱动轮具体的转速，这样就实现了在转弯时模拟差速器的设计和实现(当然也可通过机械结构实现差速器)，在实际机器人运动时会有车轮形变和车轮打滑的可能性，因此后轮速度要根据真实情况进行微调。

2 基于传感器融合的位姿估计、定位与地图构建

2.1 机器人传感器坐标变换

系统硬件搭建完成后，首先根据机器人各个感知模块相对于系统质心的真实物理位置对系统进行传感器的坐标系变换。

由于感知模块一旦经过安装便不会移动，因此使用静态坐标变换便可以满足系统对坐标变换的需求，将影响系统感知的IMU和激光雷达坐标系变换到机器人的BASE坐标系，当机器人移动时便可以通过激光雷达的点云信息和IMU信息获得机器人BASE坐标系在世界坐标系中的位置和位姿。通过使用RVIZ对实现的坐标变换进行可视化，如图3所示，BASE表示机器人基坐标系，IMU表示IMU的坐标系，LIDAR表示激光雷达坐标系。

图3 系统主要感知模块坐标系

2.2 误差控制与传感器融合位姿估计

通过电机编码器进行里程计计算[2]的流程为：已知控制参数转向角、速度，结合公式(2)(8)逆推可以获得机器人的转向角速度。

(8)

结合机器人运动的速度信息，根据公式(9)对机器人短时间的运动迭代积分进行位姿估计。公式(8)(9)中，θt为机器人此刻的转向角，ω为机器人的角速度，V表示机器人的速度，dt表示很短的时间段。由于机器人运动过程中不仅可能存在车轮打滑等物理情况产生误差，而且在迭代积分计算里程计信息的过程中存在累计误差，因此实验时机器人使用传统方法并不能很好地进行位姿的估计。

(9)

由于车载的IMU可以提供角度、角速度等信息，针对以上问题，考虑将里程计信息和IMU数据进行融合处理以获得更加精确的位姿估计。

数据融合前的准备过程中，在单独测试IMU数据时，发现所使用的IMU存在零点漂移的现象，而且通过串口获得的角度数据为X、Y、Z轴的欧拉角，而需要提供给ROS的IMU角度数据为四元数。同样为了位姿估计的精度，首先对IMU的欧拉角数据进行四元数的转换，之后使用互补滤波器对IMU的四元数数据进行滤波处理，最后通过ROS话题将IMU数据发送到话题池。滤波效果如图4所示。

图4 互补滤波前后偏航角对比

使用开源的robot_pose_ekf[3]功能包，本系统使用此功能包利用扩展卡尔曼滤波器将IMU数据与里程计数据进行融合。首先，通过订阅不同话题的方式获取传感器信号，在初始阶段检查带有时间戳的传感器信息是否有效，若有效则与初始状态下(静止)的参考基准坐标系进行转换并存储，直到所有传感器的信息可用。之后，针对每个可用的数据更新扩展卡尔曼滤波器。由于两组数据的参考坐标系是不同的，且随着时间的漂移都可能有不同程度的漂移现象，所以此功能包使用两组数据的相对位姿差异更新滤波器。位姿估计流程如图5所示。

图5 优化后的位姿估计过程

2.3 基于Gmapping[4]算法的地图构建

使用Gmapping算法进行建图需要提供机器人基坐标与传感器之间的坐标变换(TF)信息、里程计信息和激光雷达的点云信息，上文叙述的准备使机器人已经具有了地图构建的条件，ROS系统导出的机器人TF树结构如图6所示。

图6 本系统实现的阿克曼机器人的TF树

2.4 使用自适应蒙特卡洛定位(AMCL)[5]方法进行定位

基于蒙特卡洛思想，将粒子滤波应用于机器人的定位问题上，称为蒙特卡洛定位(MCL)，该方法在初始位置对机器人进行初始化，之后根据地图结合激光雷达的数据进行若干次迭代粒子权重计算—重采样—状态转移这三个步骤后，粒子高度分布在可靠性高的区域，这样就完成了机器人定位。自适应蒙特卡洛定位(AMCL)在蒙特卡洛定位方法的基础上进行了优化：一是在机器人定位基本完成后，减少了粒子数量；二是机器人绑架问题，即当粒子的平均分数突然降低时(这意味着正确的粒子在某次迭代中被抛弃了)再重新撒一些粒子。AMCL在较大范围中的局部定位工作良好，且这一2D概率定位系统是ROS导航模块中唯一指定的定位系统，因此本系统使用了自适应蒙特卡洛定位(AMCL)方法进行阿克曼机器人的定位算法。算法需要输入激光雷达数据，里程计数据以及经过融合后机器人在地图中估计的位姿。系统使用经过融合后的里程计信息能进行更加精确的定位。本系统定位功能包的工作流程如图7。

图7 本系统中AMCL功能包的工作流程

3 基于TEB(Timed Elastic Band)[6]算法的局部路径规划与移动机器人导航

3.1 使用TEB算法进行局部路径规划[7]

ROS的导航功能包集[8]默认使用的局部路径规划器使用的是DWA(dynamic window approach)算法，此算法采用动态窗口法，在速度空间中采样多组速度，推算其运动轨迹，然后通过评价函数对这些轨迹进行评价，选择最优解。由于阿克曼转向结构转向角度较小，且存在最小转向半径的限制，对于DWA算法规划的局部路径阿克曼机器人可能无法进行有效的轨迹跟踪，因此本系统使用TEB算法作为阿克曼机器人的局部路径规划器。

“TEB”算法的全称是Time Elastic Band(时间弹性带)[9]，该方法首先规划了一条连接起始点、目标点的全局路径作为初始轨迹，在其中插入N个橡皮筋形状的控制点，所以“TEB”被表述为一个多目标优化问题，另外为了显示机器人运动轨迹的运动学信息，点与点之间定义了运动时间，所以这些控制点就是一些规划出的带有时间信息的离散位姿，算法通过使用“外力”使局部的路径发生“形变”，“形变”的“外力”就是所有约束的条件，这些约束主要有以下几点。

(1)机器人本身的运动学约束[10]：这些约束条件包括局限的控制量、速度、转向角、最小转弯半径等，依据这些约束算法将若干弧组成平滑的运动轨迹。

(2)最快路径约束[11]：目标函数使机器人路径上的各个位姿点在时间上均匀分撒，而并非一味追求最短路径。

(3)路径跟踪和避障[12]：此约束将全局路径规划器规划的全局路径、地图发现的障碍物(静态障碍物)、激光雷达发现的障碍物(动态障碍物)考虑在内。全局路径施力将局部路径拉向全局路径，障碍物施力使局部路径远离障碍物。

经过以上目标函数的约束，再使用开源的G2O库(General Graph Optimization通用图优化法)进行优化，最终获得一条到达时间最短、距离最短、远离障碍物、尽可能跟踪全局路径的局部路径。

由于TEB算法拥有上述特点，因此该算法非常适合阿克曼转向结构。本文实现的阿克曼机器人使用开源的teb_local_planner[13]功能包作为局部路径器。

3.2 机器人导航功能控制流程示意图

全局路径规划使用ROS导航功能包集中的默认全局路径规划器，ROS导航功能包集中的move_base[8]功能包将全局路径和局部路径程序链接在一起，在地图中给定一个目标，move_base节点将尝试控制移动阿克曼机器人的基座移动到这个目标。结合以上的所有准备，配置move_base功能包整体结构如图8所示。实现了机器人的室内导航功能。

图8 机器人move_base框架实现

4 真实环境系统功能验证

4.1 实验环境与定位优化

本系统在实际环境中进行系统功能的验证，验证环境如图9所示。

图9 系统验证环境

系统完成初期，首先对建图功能进行验证，实验结果如图10 所示，由于IMU的零点漂移与积分里程计计算累计误差的影响，导致阿克曼机器人在环境中的定位与位姿估计较差，对建图效果影响巨大。对IMU互补滤波处理、使用扩展卡尔曼滤波进行传感器融合定位，定位精度有了大幅度提高，实现建图效果如图11所示。

图10 未经传感器融合的建图效果

图11 经传感器融合后的建图效果

在获得更为精确的环境地图后，通过切换融合前后的定位算法，通过手动控制阿克曼机器人从相同起点移动到相同终点，在其中设置四个观察点，使用RVIZ对阿克曼机器人的位姿进行可视化，进一步验证定位算法优化对阿克曼机器人在环境中的定位效果与对自主导航系统的影响。图12～图15为RVIZ显示的机器人运动过程的可视化结果。其中，黄色轨迹为机器人里程计位姿估计结果形成的运动轨迹;红色点状轮廓为激光雷达扫描的环境轮廓;灰色区域为构建的环境地图，也为机器人可运动区域;黄色运动轨迹末端为粒子滤波的置信程度，越密集则置信度越高，越稀疏则置信度越低。

图12 观察点1运动轨迹对比

图13 观察点2运动轨迹对比

图14 观察点3运动轨迹对比

图15 观察点4运动轨迹对比

对比实验结果可知，在IMU零点漂移与积分累计误差的影响下，机器人逐渐失去在环境中的定位，通过传感器融合的方式能减少误差，有效估计机器人位姿。

4.2 TEB算法对阿克曼机器人的局部路径规划

实验在已建好地图的环境中放置动态障碍物，观察TEB算法对有最小转弯半径限制的阿克曼机器人局部路径规划的效果。实验效果如图16～图18所示。其中，紫黑色为全局代价地图;浅蓝色为局部代价地图，机器人将尽可能避免与代价地图发生接触;红色为全局路径规划器规划出的全局路径;绿色为TEB算法规划出的局部路径;灰色为环境地图。

图16 全局代价地图与局部代价地图

图17 动态障碍下的局部路径规划

图18 静态障碍下的局部路径规划

测试结果表明,TEB算法能够考虑到阿克曼机器人转弯半径的局限性，规划出一条尽可能接近全局路径、远离障碍物，且轨迹半径不超过阿克曼机器人转弯半径的局部路径。

5 总结

本文设计了一种基于阿克曼转向结构的室内导航机器人，对阿克曼机器人的运动学模型、硬件设计、软件系统结构与功能进行了描述。经实验，本系统可以在室内未知环境中进行建图，并能依据地图和传感器实现阿克曼机器人在环境中定位与自主导航等室内导航机器人的基本功能。对于软硬件精度不足的问题使用滤波和传感器融合的方法对机器人的位姿估计、建图、定位、导航进行了优化，通过在真实环境中对系统进行优化前后对比实验，优化效果较为明显。并根据阿克曼运动结构的特点，使用TEB算法作为阿克曼机器人的局部路径规划器，通过测试，满足阿克曼机器人在自主导航状态下的运动要求。