融合激光SLAM 实现平衡车智能导航

权钰涵，张啸，刘冬，3，罗睿，3，贺云，3

（1.沈阳航空航天大学 自动化学院，辽宁 沈阳 110136；2.中国科学院沈阳自动化研究所，辽宁 沈阳 110016；3.中国科学院机器人与智能制造创新研究院，辽宁 沈阳 110169）

0 引言

随着科技的发展，交通工具的变化日新月异。新型的交通工具给人们带来了许多方便，但是同样也伴随着出现了许多棘手的问题，主要是操作的复杂性、体积的大小、安全性及续航时间，这几点因素也是人们选择时所关注的，其中安全性和便捷性是尤为重要的。

在校园内、小区内或者非机动车行驶道路上，平衡车被越来越多使用，但是对于初学者并不友好，容易摔倒和发生碰撞，尤其是在人员密集的地方，容易发生撞人事件。从专业的角度上来说，除了人为驾驶技术外，还因为平衡车平衡稳定性比较差和不具有自主导航避障功能。

本文研究的是一种具有智能导航和建图作用的两轮直立平衡车，由以下四部分构成：

(1)传感器系统：编码器、单线激光雷达、MPU6050(加速度传感器+陀螺仪传感器)、蓝牙HC-06。

(2)控制系统：树莓派。

(3)驱动系统：Arduino UNO R3 主板、TB6612FNG 电机驱动模块、电源。

(4)执行机构：主体使用亚克力板拼装，由两个直流电机带动差速车轮实现机器人行走。其中驱动系统和执行机构构成机器人系统。

具体实现是“PC+嵌入式”，可以使用嵌入式系统树莓派充当机器人本体的控制系统，而PC 则实现远程监控，通过前者实现数据采集与直接的底盘控制，而后者则远程实现图形显示以及功能运算。其中建图与导航采用的是激光SLAM—2D 单线束激光雷达，具有环境感知和定位功能。

采用的算法有PID 控制算法(控制平衡)、卡尔曼滤波算法(数据融合)、Cartographer算法(构建地图)、map_server 算法(读取和保存地图)、Navigation 导航算法(定位AMCL、路径规划move_base、teb_local_planner 局部路径规划算法)、ros_arduino_bridge。

本文的重点研究目标是导航，主要是建图优化和Teb 路径规划算法优化，以及多传感器数据融合所选取的滤波算法。最终的效果是在实现平衡直立的基础上，可以进行前进、后退、转弯、爬坡、原地转圈和货物托运等功能，在这一切可以稳定实现的前提下，进行导航(建图、地图服务、定位和路径规划)和SLAM 建图。在指定目标点后，进行路径规划(全局、局部)，然后根据选择最优路径，途中会绕开障碍物，直至到达目标点。还可以用手机APP 通过蓝牙模块控制机器人的运动状态，从而实现键盘控制SLAM 建图。

通过具体实验数据，在算法方面，相比于互补滤波算法，采用卡尔曼滤波算法获得的融合数据要好许多，最终使平衡车达到平衡的状态，相比于采用一二阶互补滤波要更加稳定。同时就调研结果而言，目前国外做的智能导航平衡车较多，但国内很少，国内比较多的是单独的平衡车，不具有导航功能。主要原因是过去几年雷达成本比较高，不像现在雷达成本越来越低；而且开发难度比较大，要兼顾平衡性和导航性能，同时得借助ROS 开发，所需的知识储备比较大。另外在国内平衡车的用途主要是民用，在非机动车道上行驶，不易控制，安全系数低，但在加上建图与导航功能之后，安全性大大提高，可以减少交通事故，有利于它的普及，除此之外，它还可以用于工厂内货物托运和巡逻，或者勘探一些人为无法接触的区域和事物等。

1 基本原理

1.1 系统总体设计

就整个车体而言，主要由三部分组成：车体结构、硬件部分及软件部分。车体结构主要是指车模，本车模主要由两个直流驱动电机、两个独立车轮及固定底盘构成[1]。硬件部分包括主控模块树莓派、激光雷达传感器、驱动芯片Arduino 主板、六轴传感器MPU6050、电机驱动芯片TB6612FNG、HC-06 蓝牙传感器和AB 相双向霍尔磁敏编码器等[2]。软件部分主要由手机APP 和主控程序构成，主要是借助Linux 系统在ROS 操作系统上开发的，用的开发软件有Vscode、Arduino IDE、MATLAB。系统整体控制框图如图1 所示。

图1 系统整体控制框图

1.2 实验平台

本系统主要有车模底盘、电源电路、电机驱动电路、MPU6050 六轴传感器模块、电机测速模块电路、HC-06蓝牙模块电路、单线激光雷达传感器模块电路、Arduino驱动模块电路和树莓派主控电路。由于雷达模块和树莓派电路设计简单，只需要连接端口就行，因此在绘制电路图和制作PCB 的时候不需要考虑了[3]。具体如图2所示。

图2 硬件电路图及PCB

MPU6050 模块使用的工作电压为3.3 V，通过IIC 接口与MCU 通信，该芯片在内部整合了一套DMP 数据处理器，将四元数通过硬件解算出来，解决了姿态解算困难的问题[6]，消除了陀螺仪和加速率传感器之间的耦合，从而减小了给定的干扰和自身漂移带来的影响，同时DMP 引擎还可以减少对复杂的融合信息、感测器同步化以及位置传感器等方面的干扰[7]。所研究的两轮自平衡车采用的是集成编码器——AB 相增量式霍尔(磁)编码器，本身具有上拉整形和不侧漏的功能，且波形稳定[8]，相比于光电编码器更加稳定，对于平衡车的速度控制更加优越，有利于实现自平衡，并且作为里程计编码器，在建图和定位时可以大大减小位姿误差。同时，为了便于控制导航平衡车运动，采用了HC-06 蓝牙模块，然后通过客户端蓝牙APP 直接控制车模的基本动作。通过SPI 或IIC 通信方式实现蓝牙与MCU 之间数据通信[8]，基本功能传输流程如图3 所示。

图3 蓝牙通信基本框图

采用的雷达是思岚的Mapper M2M1，如图4 所示，自带定位功能，有效测距是40 m，频率是9 200 Hz，可进行每秒 10 次以上的地图数据融合和最大10 万平米地图数据绘制。激光雷达每秒进行9 200 次的测距动作[9]。

图4 SLAMTEC Mapper 框图

为了方便控制，在树莓派上也安装ROS1，通过电脑进行远程监控，实时观测建图导航信息，用键盘控制车模建图。考虑到平衡车的特殊性，搭建车模的硬件需要注意其物理特性，主要是陀螺仪和雷达的安装位置，为了车模平衡更加稳定和避免SLAM 的建图发生畸变，所以车模的重心不能太高，经过测试和调节，最终的安装结构如图5 所示。

图5 车模及导航示意图

2 软件设计及算法优化

在程序设计上，涉及多传感器数据的融合以及不同系统上程序的移植，具体软件设计流程如图6 所示。

图6 程序流程图

智能导航平衡车中，软件的主要功能如下：

(1)所涉及传感器信号的采集与处理；

(2)电机驱动模块的PWM 输出；

(3)小车基本动作的实现，主要是平衡直立控制、速度控制和方向控制；

(4)SLAM 建图与导航，确定初始位姿，设置目标点，绕开障碍物，规划最优全局(局部)路径；

(5)小车运行流程控制，包括各模块程序初始化、车模的启动与结束，以及车模状态监控；

(6)通过上位机和PC 端实时监测车模的状态以及在线调节相关参数。

2.1 卡尔曼滤波器设计

卡尔曼滤波方程是一种综合分析利用线性系统的状态方程,通过观察到的系统在其输入和给定时间内的各种状态，对系统在其输出和给定时间内的状态信号进行了最优估计的一种算法。由于这些观察到的数据中涉及系统运行过程中的各种噪声和其他干扰等因素，因此这种最优估计方程可以被简单地看作是一种滤波。主要指的是预测和滤波两个过程，可设离散差分和观测方程为[10]：

其中，x(k)是n维状态向量；u(k)是m维控制向量；z(k)是l 维状态观测向量；Φ(k+1,k)是n×n的转移矩 阵；G(k+1,k)是n×m维矩阵；Γ(k+1,k)是n×p的噪声驱动矩阵；H(k)是l×n的量测矩阵，v(k)为量测噪声序列。

MPU6050 六轴传感器的输出方式为惯性传感器的模拟量，利用Arduino UNO 自带的ADC 进行数模转换，从而获得传感器的输出电压大小，而且要对加速度和陀螺仪传感器的输出进行变换，使卡尔曼滤波器接收到的是角度和角速度变化量[11]，其转换公式如式(3)所示：

式中,W为陀螺仪测得的角速度，Vout、Voffset和Vsen分别为陀螺仪的输出电压、静态输出电压和灵敏度，K表示放大电路的放大系数[11]。

检测角度和加速计输出电压转换公式如下：

其中，A为加速计检测到的重力加速度，Uout为加速计的输出电压，Uoffset为加速计的平衡状态输出电压，Usen为加速计灵敏度[11]。

事实上，车模在高速旋转运动时本身就可能会自动产生一定程度晃动的旋转时间和运动加速度，这在研究过程中就是自身的干扰信号，它会叠加在输出信号上，无法精确地反馈车模的倾斜角度，具体如图7 所示。

图7 小车运动引起的的加速度信号波动

2.2 PID 控制算法

前面已经说到车模的控制任务可以分为三部分，第一部分是控制小车平衡，第二部分是控制小车速度，第三部分是控制小车方向，通过一系列的分析之后，得出车模在直立行走过程中的直立、速度及方向控制都使用了PD 控制，通过对这三种算法的输出结果叠加来完成电机加速的控制，最终使小车达到平衡状态，故而PD 调节要非常平滑缓慢，不能出现跳跃性调节。

在小车运动能过程中，由于车轮与地面之间存在静摩擦，电机电枢得电，此时电机转子并没有发生相对转动，因此要根据实际情况添加一个死区常量用来提高系统的净稳定性，如图8 所示。

图8 电机死区补偿

2.3 Cartographyer 算法地图构建

Cartographyer 算法是由Google 所发布的一种基于地图优化技术的SLAM 算法,它的结构主要由Local SLAM 和Global SLAM 两个部分所组成。Local SLAM是用里程计和IMU 数据通过轨迹计算,得出小车的原始位姿估计值ξ=(ξx,ξy,ξθ)，ξx,ξy表示位置，ξθ表示姿态角，作为初始值将于雷达数据进行匹配，同时对位姿估计器的值进行更新，在雷达的一帧帧数据经过运动滤波后，再通过叠加，形成子图submap[13]。而在Global SLAM 部分，进行回环检测和后端优化，减小地图累计误差，最终形成一张完整可用于导航的地图。

在智能导航平衡车中，采用Cartographyer 可以实现大场景高精度(5 cm)的2D 栅格地图。在前端匹配环节，每得到一次雷达 scan 数据后，便与时间戳邻近的submap 进行扫描匹配，插入到匹配度高的最优位置上，直至最后没有新的scan 插入，此时一个submap 便构成，以此类推到最后一个submap[13]。由一个起点和数个终点构成一个激光点云图，其中每一个scan 为激光点云图，将点云集合用H表示，其表达形式为H={hk}k=1,2,..,k，hk∈R2。

当获得新的scan时，要插入到submap中，通过转换系数Tξ进行坐标转换，转换公式如下:

在scan 插入submap 之前，需要利用ceres 库的扫描匹配器对每个状态的位姿进行优化，将求解scan 位姿问题转化为求解非线性最小二乘方程的解[14]。随着生成的submap 数目越多，扫描匹配过程中的累计误差也在逐渐增大，因此需要采取稀疏姿态调整方法优化scan 与submap 的位姿，也就是闭 环检测，将匹配程度好的位姿加入到优化当中[16]。扫描匹配用的方法是深度优先的分支定界加速算法，每当新的scan 出现时，在附近窗口搜索最优匹配帧，若此帧符合要求，便形成一个闭环，这个过程中建立了一个搜索窗口，把整个解空间当作一棵树来表示，其根节点代表搜索窗口，树中的每一个节点的子节点都是对该节点所代表的搜索空间的一个划分，每个叶子节点对应一个解，整个搜索过程的基本思想就是不断地分割搜索空间，称为分支，再为新产生的分支叶子节点给定一个适当的上界，称为定界，若子节点中有定界超过了最优解的值，将对该节点进行舍弃[17]。

运用此方法能够高效地确定激光点云与子图的匹配度，并估计出车模的位姿。在了解了建图原理之后，配置revo_lds.lua 和demo_revo_lds.launch 文件，配置前需要通过rqt 看tf tree，然后在launch 文件里修改lua 文件的名字和topic，lua 文件需要配置的是：map_frame=“map”(地图坐标系)；tracking_frame=“base_footprint”(将所有传感器数据转换到该坐标系下)；published_frame=“odom”(tf: map-＞odom)，其他一些参数根据实际调试和硬件属性配置。

对于建图场景比较复杂的，进行必要的特征点提取，只保留一部分有效雷达数据，去除无效点，具体效果如图9 所示。

图9 有效特征点提取前后对比图

2.4 Teb 局部路径规划算法

本车模作为实验模型，核心目的是平衡车的导航技术验证，为此在室内和走廊进行模拟验证。此过程中，活动的实验人员等同于动态障碍物，桌椅等物品就是静态障碍物，同时也设置了倾斜角度为15°～30°的斜坡，负载为10 kg，路面相对平坦一点，在此约束场景下，进行车模测试。

就导航模块而言，关键技术有5 点,分别是全局地图、自身定位、路径规划、运动控制和环境感知，主要介绍一下自身定位和路径规划。由于Cartographyer 本身就有重定位功能，并且选用的雷达传感器Mapper 也有定位功能，因此此处不再运用AMCL 定位。至于路径规划，通俗来说，就是从一个点到另一个指定目标点的过程，在这个过程中，导航平衡车根据目标位置计算最优路线，途中会遇到静态和动态的障碍物，然后绕开障碍物，重新规划路线，直至到达发布的目标位姿。规划过程中，有全局路径规划和局部路径规划，使用Dijkstra 或A*算法进行全局路径规划，在本地路径规划上，move_base 默认调用的是Dwa，经过与Teb 局部路径规划对比，选用后者，主要是因为它的最优目标是规划最优时间，Teb 算法将避障硬约束为软约束，以寻找代价最小的轨迹解决方案。

由于实际环境是变化的，障碍物的出现是随机的，因而对于导航而言静态地图并不适合。为了在导航过程中实时获取障碍物数据，在静态地图的基础上添加膨胀区数据。主要是由全局代价地图和本地代价地图构成，前者用于全局路径规划，后者用于本地路径规划。需要配置的yaml 文件有四个，分别是全局和本地路径规划调用的通用参数、全局代价参数设置、本地代价参数设置和基本的局部规划器参数配置。其中膨胀半径inflation_radius 和代价比例系数cost_scaling_factor 给全局和本地各单独设置，以便于远离障碍物，避免进入膨胀区域而陷入假死状态，Teb 中的是否考虑动态障碍物设为true，动态障碍物膨胀距离设为0.45 m，动态障碍物权重设为10 以及膨胀带权重设为0.2，以上是车模实际测试参数，个别与仿真不一致。

测试过程中，可实现任意角度原地转弯，但在遇到突然出现的行人时，车模在停止的过程中会出现惯性过冲的现象，易造成车模倾倒，经分析，主要问题是雷达横向检测范围太小和车模的PID 参数欠优，未能及时检测到动态障碍物，为此增大了雷达的扫描角度，由原来的-30°～30°改变为-60°～60°，PID 参数整定见表1。

表1 三个闭环控制环节PID 参数

3 分析与讨论

基于前面的理论分析，可以直观地从图10 中看出，对里程计编码器和六轴传感器采集到的数据采用卡尔曼滤波器进行数据融合，获得的效果相比于一二阶互补滤波算法要好很多，极大地减小了干扰，有利于车模平衡。具体对比如图10 所示(曲线1 指原始角度波形；曲线2 指经低阶互补滤波后的角度输出曲线；曲线3 指经卡尔曼滤波后的角度输出波形)。

图10 角度输出曲线

在车模运行过程中，经测试，当倾斜角度超过18°时(设定为15°)，车模重心偏移严重，将无法保持直立，滤波也就不再适用，这也符合实际情况，超过临界值将无法自主调整。

在对数据进行滤波融合处理之后，便是让车模直立起来，并能稳定地进行速度和方向控制，为此需要PID参数调节。为了更精准地整定，要充分理解PID 控制算法的基本原理和每个参数在整个控制系统中的作用。调试的时候直接在Linux 下面的vscode 编辑器修改参数，采用快速调节法(先调P，后调D，逐步逼近临界值，最后对临界值×0.8)，三环调节顺序是先调角度，再调速度，最后调方向。

优化前后的效果对比如图11 所示。运用Cartographer 算法构建地图时，经过后端优化后，去除了畸变叠图，大大提高了地图精度，获得的图与实际环境更加吻合、轮廓更加清晰。同时也得出结论，当建图效果不太理想并且计算量过大时，调节ceres 的匹配权重，将地图权重增大，平移旋转权重减小。

图11 优化算法前后的实际建图效果

在路径规划上，采用move_base 里面的teb_local_planner 算法，加入了全局和本地代价地图，为障碍物设置了膨胀层，在rviz 里面发布了目标点之后，能够绕开障碍物快速寻求最短路径，获得最优时间，同时加入了Cartographer 的重定位功能，使得智能导航平衡车走过的路径都在栅格地图上显示出来。值得注意的是实际测试中车模有时会陷入假死状态，根据代价比例系数越小，代价值越大，为此在全局代价参数yaml 文件中设inflation_radius=0.5、cost_scaling_factor=8.0，全局代价参数yaml 文件中设inflation_radius=0.3、cost_scaling_factor=4.0，从而解决了假死这一问题，效果如图12 所示。

图12 优化后导航示意图

4 结论

为了提高平衡车的安全性、灵活性和智能化，通过雷达传感器、AB 相增量式霍尔(磁)编码器轮式里程计和六轴传感器MPU6050 来获取车模的姿态，利用卡尔曼滤波器进行数据融合，运用了Cartographer 算法的建图和纯定位功能，加入了Teb 路径规划算法、Cartographer 和路径规划算法融合以上几种传感器的数据信息，实现了平衡车的自主建图和导航避障功能。在仿真没有任何问题的条件下，搭建了实物实验平台，在实际环境中进行了测试，测试结果达到了预期效果。对于建图过程中出现的畸变叠图情况，进行了优化，消除了叠图，采用的导航算法可以快速绕开障碍物到达目标点，完全可以实现无人驾驶。相信在不久的将来，雷达的成本会越来越低，融合SLAM 技术的智能导航平衡车将发展为新的轻盈便捷的代步工具，那时肯定会受到广大客户的青睐。