基于麦克纳姆轮的智能小车SLAM导航研究

吴 迪，杜 峰，蔡一杰，史星彦，胡乐媛

（天津职业技术师范大学，天津300222）

0 引言

同步定位与建图（Simultaneous Localization And Mapping，SLAM）技术作为智能化研究的重要一环，目前已广泛应用于物流车、仓储机器人等领域，而麦克纳姆轮作为一种全向移动的运动方式，其转向范围较小，在一些特定的复杂环境下比普通的运动模式更为简单、便捷，如Airtrax开发的Sidewinder系列叉车、Vetex开发的RoboMate全向移动机器人平台、Kuka公司的YouBot等产品，可在狭窄、紧凑的区域内运动。当前对麦克纳姆轮的相关研究已逐渐成熟，Sarmento L等[1]设计的基于麦克纳姆轮的全向移动机器人实现了基于无线的远程遥控；Sang Won Yoon等[2]针对麦克纳姆轮在运动过程中的滑移问题，提出多传感器数据进行卡尔曼滤波融合，消除了传感器和滑移的噪声影响，实现了麦克纳姆轮移动机器人的精确定位。本文以麦克纳姆轮智能小车为研究对象，在ROS（Robot Operating System）系统下研究关于SLAM自主导航与避障，并验证其可行性。

1 系统框架

本文在基于麦克纳姆轮的智能小车上，搭载ROS操作系统，通过传感器探索未知区域收集环境信息，实现实时同步定位与建图（SLAM），并通过所建地图规划路径，实现自主导航避障。

1.1 硬件架构

系统硬件包括麦克纳姆轮底盘小车，激光雷达、里程计等传感器，麦克纳姆轮底盘小车集成了ROS系统，激光雷达、IMU（Inertial Measurement Unit）、里程计等传感器搭载在小车上，其硬件架构如图1所示。

图1 硬件架构

智能小车装载树莓派（内置ROS系统），通过WIFI与PC相连，借用ROS的分布式通信可将主机转移至PC端，实现远程操控。激光雷达收集点云信息，通过USB信号传输给树莓派，树莓派对信息进行相关算法处理后生成控制指令，传输给驱动板控制车轮转速，驱使小车运动，同样IMU、里程计的速度信号可反馈至树莓派。

1.2 软件架构

智能小车装载树莓派，以ROS系统为软件平台，连接上层环境感知与下层运动控制[3]，ROS是一款机器人操作系统，集成了大量的库、工具、协议等，提供了一系列可供传感器数据传递、可视的通用类功能包，基于TCP/IP的通信方式，实现软硬件的实时通信。软件层架构如图2所示。

图2 软件架构

ROS模块化的通信机制将整体程序分为三部分，感知层、决策层与下层控制。感知层为各传感器的数据输入，导入决策层后通过相关算法分析，生成控制指令传输给下层控制，控制小车运动[4]。

2 运动学模型

2.1 麦克纳姆轮简介

麦克纳姆轮是一种可全向移动的控制方式，由轮毂和辊子组成，轮毂轴与辊子转轴空间上呈45°角，如图3所示，麦轮在运动过程中，辊子与地面摩擦产生摩擦力，摩擦力的方向与辊子轴的方向相反。

图3 麦克纳姆轮

麦轮四个一组应用，两个左旋轮，两个右旋轮，左右轮呈手性对称，但不同安装方式其受力不同，运动效果也不同。本文安装方式如图4所示。

图4 麦轮安装图

2.2 运动学模型

同理可得出各个轮子的vix，viy：

图5 运动分析图

可以得到麦克纳姆轮智能小车正运动学方程[5]：逆运动学方程[6]：

因此，若给定小车一个速度状态，可通过式（4）控制小车轮子转速进而控制智能小车运动。

3 SLAM：同步定位与建图

SLAM即同步定位与建图，是指通过对陌生区域的探索，实时构建出此环境的完全地图。在陌生区域内，要构建地图就需要自身准确的定位信息，而要掌握自身位姿的信息也需要对比地图分布，因此，在SLAM问题中，主要处理的是地图信息及本身的位姿信息，且定位与建图同步进行。SLAM模型如图6所示。

图6 SLAM模型

图6中，灰色三角形与实线表示小车t（t=1，2，）时刻的实际运动位姿，表示小车t时刻的位姿状态，白色三角形与虚线表示对小车t时刻估计的位姿信息，方形代表路标信息。小车运动使得自身位姿发生变化，里程计对位姿信息进行更新，但在实际运行过程中，因为环境因素，运动模型误差，信息延迟等因素，对小车的位姿估计往往与实际值产生较大误差，并且误差伴随着系统不断运算而累积叠加，这也就需要智能小车在运动过程中通过传感器的观测信息（激光、IMU等）不断矫正误差，利用激光、IMU信息自定位[7]。总的来说，SLAM问题实质是针对位姿信息“估计-观测-校正-估计”不断循环的过程。

3.1 基于图优化的SLAM理论

SLAM算法大体分为两类：基于滤波的SLAM算法与基于图优化的SLAM算法。基于滤波的SLAM算法原理是根据传感数据信息去估计机器人当前时刻的位姿信息，其问题在于，若位姿估计错误会使得之后的计算无法修正，特别是大型复杂环境下此缺点尤为突出。

与基于滤波的方法不同，基于图优化的SLAM不仅仅是对小车当前位姿的估计，而是对整体运动轨迹和地图的完整估计。将SLAM问题描述成图结构，图由节点与边组成，其节点（Node）表示小车位姿信息，而位姿之间的空间约束则构成边（Edge）[8]。图优化SLAM可分解成两部分：（1）构建图，小车位姿为节点，位姿约束关系为边，称为前端（Front-end）；（2）优化图，即调整小车位姿节点以满足边的约束，称为后端（Back-end）。本文采用基于图优化的Cartographer-SLAM算法。

3.2 Cartographer算法架构

Cartographer算法[9]是基于图优化理论的SLAM框架，分为局部SLAM（也称前端）和全局SLAM（也称后端），其建图步骤如图7所示，Cartographer框架如图8所示。

[28]Eli Pariser, The Filter Bubble, London: Penguin Books Ltd (UK), 2012.

图7 建图步骤

图8 Cartographer算法框架

（1）局部SLAM

由IMU和里程计通过互补滤波的方式预测初始的位姿，建立匹配机制，即通过非线性最小二乘进行优化求解得到最优匹配，算法中通过调用Ceres库[10]执行。激光扫描后，得出的位姿可看作一个节点，且点云信息会经由体素滤波器去除冗余的点云信息，然后与IMU、里程计预测的位姿子图进行匹配Scan to map[10]，优化位姿的同时不断插入数据帧使得子图更新，当子图不再更新即认为子图创建完成。接着去创建下一个子图，得出的最优位姿用作下一次的初始位姿，循环过程。

（2）回环检测

Cartographer通过创建大量的子图来实现场景建图，子图在短时间内的准确度是可靠的，但长时间会存在累积误差，为了消除累积误差，需要通过回环检测来构建回环，最终是为了优化所有子图的位姿，检测回环的方法为分支定界法[10]，将检测的节点所携带的点云与当时的子图进行匹配，可得到回环约束位姿。

（3）全局SLAM

根据局部SLAM过程，节点与节点之间存在边的约束，节点与子图之间可看作局部约束，但对于回环约束，有可能节点与子图不一定在同一轨迹中，即同一节点可能与多个子图存在匹配关系，这也就是全局约束，针对全局的约束问题进行优化即为全局SLAM的过程。

4 实验

实验设备为PIBOT智能小车，装载激光雷达思岚A2，内置编码里程计、IMU，环境为室内实验室，如图 9（a）所示。

图9 实验设备与环境

通过PC端与小车WIFI相连，控制小车移动探索区域建图，遍历实验室环境（图9（b）），通过Cartographer算法构建出的地图效果如图9（c）所示，从图9（b）和图9（c）中可以看出，所构建的地图能够反映出实验室的环境特征，其中桌面下端是连通的，符合环境特征。

智能小车导航，全局路径规划采用A*算法[11]，局部路径规划采用DWA算法[12]。根据构建的地图给定机器人目标点与起始点，如图10（a）所示，机器人按照规划的路径自主导航。在全局的路径上放置障碍物，如图 10（b）所示，其路径随之改变图 10（c），机器人成功避开障碍物到达目标终点。

图10 SLAM建图与导航图

5 结束语

麦克纳姆轮全向移动智能小车搭载ROS系统，基于Cartographer算法下能完成室内环境下的同步定位与地图构建，且能实现自主导航与实时避障功能，对全向移动的定位导航具有良好的应用价值。