Cortex-A53嵌入式处理器平台上实现激光雷达SLAM的方法

自主移动机器人]是近几年的研究热点，要实现机器人的自主移动，关键是要实现SLAM[4-7]（Simultaneous Localization and Mapping），也就是同时定位与地图构建。

在移动机器人上实现SLAM目前有两种主流的方法，一是基于相机的SLAM；二是基于激光雷达[8]的SLAM。在SLAM的实现中最常用的硬件处理器平台是Intel x86平台，如TurtleBot移动机器人上的硬件处理器平台就是一台笔记本。在移动机器人上实现SLAM的硬件成本比较高，这是自主移动机器人难以进入服务市场的主要原因之一。为了解决这个问题，本文在基于Cortex-A53的处理器平台上配以激光雷达实现了室内环境下的SLAM。

1关于用低成本实现SLAM的思考

在移动机器人中用嵌入式处理器实现SLAM是一种趋势，一是由于嵌入式处理器平台对工业现场环境有较好的适应性，二是成本相对较低。但是嵌入式处理器相对于SLAM问题来说，其性能不够高，所以在这种相对低性能的处理器上实现SLAM要解决的主要问题就是要降低计算量，并且需要对算法进行优化，使得所实现的SLAM能满足实际使用要求。基于激光雷达的SLAM相对于基于相机的SLAM来说，它的算法相对简单，计算量相对较小，对处理器的性能要求相对较低，所以才可以在性能较低的ARM平台上实现，所以选择用激光雷达作为SLAM的测量输入。

2扩展卡尔曼滤波与粒子滤波

2.1 扩展卡尔曼滤波与粒子滤波

目前实现SLAM的两种最主要的方法是扩展卡尔曼滤波和粒子滤波。基于扩展卡尔曼滤波(EKF)的SLAM算法[8]对非线性的运动模型和观测模型采用线性化来解决，其线性化的方法是在目标点附近做泰勒展开并去除其高阶部分，如果模型的非线性程度很大，就很容易使算法产生较大误差，因此非线性就成了这种SLAM算法中的一个很严重的问题[9]，且EKF还是基于高斯假设的，运动模型和观测模型中的噪声都要满足高斯分布，这样使得算法的使用受到限制。粒子滤波中用一定大小的采样样本的频率分布来表示概率分布，当样本数量趋于无穷时，粒子集的概率密度函数可以近似于任意形式的概率密度分布，因此粒子滤波可以处理任意形式的状态空间模型，而不局限于高斯噪声模型。

在粒子滤波中，把从后验分布中获得的采样称作粒子，粒子是对真实世界中待确定的状态的一种假设。采样后的粒子集St表示为：

2.2 基于粒子滤波的SLAM的实现步骤

SLAM是这样的一个过程：当机器人处于一个未知环境中且自身位置未知时，移动机器人在向前运动过程中可以构建环境地图，并同时利用构建的地图来估计自身的位置。本文中的SLAM是基于粒子滤波算法来实现的。

在一个陌生的环境中，机器人位置的初始信度未知，所以认为机器人可能位于当前环境中的任何一个位置。从表示当前环境的状态空间的均匀分布中随机采样N个粒子来表示机器人的初始信度分布。本文中用于地图构建的SLAM算法描述如下：

3在Cortex-A53平台上实现SLAM的方法

整个系统的构建过程如下：

(1)软件平台中使用的操作系统是支持ARM架构处理器的Ubuntu mate16.04 LTS系统；

(2)使用的ROS(Robot Operating System)机器人操作系统是kinetic版本；

(3)进行网络配置，把处理器平台上的WiFi模块配置成Access Point(AP)模式，让上位机可以接入AP与Cortex-A53处理器平台通信，这样就可以在个人计算机上通过远程登录来操控机器人；

(4)把上位机配置成时间服务器，让Cortex-A53平台每次上电后能与上位机的时间保持同步；

(5)构建地图使用了粒子滤波方法。在未知环境中是通过粒子滤波来实现机器人定位的，并在定位的基础上根据激光雷达的观测数据来构建地图；

(6)在已知地图的基础上定位是通过粒子滤波实现的，机器人可以根据当前的激光雷达扫描数据和里程计数据实现对自身的定位；

(7)有了环境地图，并且机器人可以对自身定位，再通过路径规划就可以实现机器人导航。全局路径规划使用Dijkstra算法，在成本地图上寻找从起点到终点的最低成本路径。局部路径规划的算法思想为：在机器人可以运动的速度和旋转角度范围内对速度和角度进行离散采样；根据机器人的当前状态对每一个采样速度进行向前的运动模拟，根据模拟过程中机器人与障碍物的接近程度、与目标的接近程度以及与全局路径的接近程度等来给每条模拟轨迹打分，并放弃不合理的路线；最后选择得分最高的路线并给底座发送相应的速度指令来让机器人运动。此种算法在局部路径规划过程中会在整个向前运动模拟过程中持续对速度采样；

(8)对程序进行优化，减小Cortex-A53处理器平台的计算压力。

系统构建完成后就可以在上位机上实时观察构建的地图，并可以在上位机上用软件来给机器人指定目标，实现机器人自主导航。

移动底座和激光雷达与Cortex-A53平台都是通过串口来通信的。在基于Cortex-A53处理器的平台上处理激光雷达的扫描数据以及底座中采集的里程计数据，结合激光雷达的数据和里程计数据实现机器人的定位，并在定位的基础上利用雷达扫描数据实现二维地图构建。在对应的人机接口上可以通过软件在扫描地图的基础上给机器人指定目的地来进行导航。

系统的架构如图1所示。

4算法优化

4.1 构图算法的优化

Cortex-A53处理器平台结合激光雷达实现地图构建。图2为未优化的地图。

从前文可知，地图构建是通过匹配得分最高的最优粒子结合激光雷达的扫描数据来实现的。构图算法的优化方法为：通过给扫描匹配得分设定一个阈值minimumScore来提高所得地图的精度。

这个阈值表示在粒子滤波过程中进行扫描匹配时要求的最小匹配得分，这个值设置的越大，就表示对扫描匹配的要求越高。如果阈值设置过高，很容易导致匹配失败，机器人会选择使用里程计的数据，这样就会使得对机器人姿态估计的准确度降低，从而导致构图精度降低；如果阈值设置过低，扫描匹配很容易成功，会导致地图中出现大量噪声。设置合理的阈值可以加快机器人定位过程的收敛，得到精度更高的地图。本次实验过程中测得的阈值范围约为60~80，阈值范围取决于激光雷达的测量范围、角度分辨率、测量精度以及环境特征。图3为优化后的地图。

4.2 路径规划算法的优化

在系统构建过程中发现在Cortex-A53处理器平台上使用的局部路径规划算法无法实现局部路径规划，因为这种算法的计算量太大，在路径规划过程中处理器CPU占用率达到100%，但路径规划依然无法执行，此平台无法满足该算法的要求。

使用改进后的局部路径规划算法机器人可以正常执行导航功能。改进后的路径规划算法只对一步向前运动模拟进行速度采样。这就使得速度采样的样本空间大大减小，因而改进后的局部路径规划算法更高效，对计算能力的要求会相对较低。但是当对机器人的加速度限制较小时，改进后的算法性能可能会不如改进前的算法性能好。不过在室内环境下，机器人运动的加速度会相对比较小，所以改进后的算法适合在室内条件下进行局部路径规划。

5总结

本文提出了在Cortex-A53嵌入式处理器平台上实现激光雷达SLAM的方法，实现了地图构建和地图优化，以及移动机器人室内环境下的导航、实时避障等功能。实践证明，该方法效果较好，对处理器性能要求较低。后续的研究将集中在如何使用消费级的激光雷达来实现SLAM上，从而更有效地降低移动机器人实现SLAM的成本。