《基于Cortex-A53平台的激光雷达SLAM实现》由会员分享，可在线阅读，更多相关《基于Cortex-A53平台的激光雷达SLAM实现（8页珍藏版）》请在金锄头文库上搜索。
1、基于Cortex-A53平台的激光雷达SLAM实现自主移动机器人引是近几年的研究热点，要实现机器人的自主移动，关键是要实现SLAMS (Simultaneous Localization and Mapping ),也就是同时定位与地图构建。在移动机器人上实现SLAM目前有两种主流的方法,-是基于相机的SLAM ;二是基于激光雷达 的SLAMO在SLAM的实现中最常用的硬件处理器平台是Intel x86平台,如TurtleBot移动机器人上的 硬件处理器平台就是一台笔记本。在移动机器人上实现SLAM的硬件成本比较高,这是自主移动机器人难 以进入服务市场的主要原因之一。为了解决这个问题，本文在基
2、于Cortex-A53的处理器平台上配以激光 雷达实现了室内环境下的SLAM。1关于用低成本实现SLAM的思考在移动机器人中用嵌入式处理器实现SLAM是一种趋势，一是由于嵌入式处理器平台对工业现场环 境有较好的适应性，二是成本相对较低。但是嵌入式处理器相对于SLAM问题来说,其性能不够高,所以 在这种相对低性能的处理器上实现SLAM要解决的主要问题就是要降低计算量并且需要对算法进行优化, 使得所实现的SLAM能满足实际使用要求。基于激光雷达的SLAM相对于基于相机的SLAM来说,它的 算法相对简单,计算量相对较小,对处理器的性能要求相对较低,所以才可以在性能较低的ARM平台上 实现,所以选择用
3、激光雷达作为SLAM的测量输入。2扩展卡尔曼滤波与粒子滤波2.1扩展卡尔曼滤波与粒子滤波目前实现SLAM的两种最主要的方法是扩展卡尔曼滤波和粒子滤波。基于扩展卡尔曼滤波(EKF)的 SLAM算法对非线性的运动模型和观测模型采用线性化来解决,其线性化的方法是在目标点附近做泰勒 展开并去除其高阶部分，如果模型的非线性程度很大，就很容易使算法产生较大误差，因此非线性就成了 这种SLAM算法中的一个很严重的问题,且EKF还是基于高斯假设的，运动模型和观测模型中的噪声都 要满足高斯分布，这样使得算法的使用受到限制。粒子滤波中用一定大小的采样样本的频率分布来表示概 率分布，当样本数量趋于无穷时，粒子集的概
4、率密度函数可以近似于任意形式的概率密度分布，因此粒子 滤波可以处理任意形式的状态空间模型,而不局限于高斯噪声模型。在粒子滤波中把从后验分布中获得的采样称作粒子粒子是对真实世界中待确定的状态的一种假设。采样后的粒子集St表示为：o . (1)(2)(3)ov)，叫，旺其中，每一个粒子1 WkWN)都是F时刻的一个具体 状态M表示/时刻粒子集中的第斤个粒子，如果对应一 个机器人的状态，就表示机器人的位置和方向八 是粒 子集中的粒子总数。把运动模型/3:|:;心)应用到t-时刻的后验 粒子：；上，得到t时刻的先验粒子的概率密度，均 表示从一1时刻到F时刻作用的运动控制。根据一阶马 尔科夫模型时刻粒子
5、的先验状态只取决于1 时刻的后验状态文：;和当前的运动控制4。把观测模型P(zt |.r；ir)应用到/时刻的先验粒子 上，通过计算粒子得到观测值3的概率大小就可以得到 这个粒子的权重大小。根据粒子的权重在当前粒子集中 进行重采样。每个粒子被采样的概率与其权重成正比，重 采样后的f时刻后验粒子的概率密度为 一定数量服从此分布的粒子组成的粒子集就可以反映 状态的概率密度分布P(xtzt)o粒子滤波的算法描述如下：(1 )基于初始概率密度函数P(M),初始概率密度分 布会在下一小节进行说明，随机生成艸个粒子，记作WkWg 由于此时没有观测值可用，所以认为此 时得到的粒子即为初始时刻后验分布中的粒子
6、是合理 的，所有粒子的权重均为1AV；(2) 对每个粒子，根据运动模型执行状态更新获得时(k)刻的先验粒子 (IWZrWN)；(3) 执行测量更新，把测量值应用到粒子上。以观测 值为条件计算每个粒子得到此观测值的概率密度 P(zt |x；ir)，从而得到每个粒子的权重;(4) 把粒子的权重归一化：仏)心=(5) 基于.重采样后就得到了一组服从P(如|為)概率 密度分布的粒子，粒子集的统计学特性就反映了状态的 概率密度函数的统计学特性，主要关注其均值ET和和协 方差Covx；b均用算数平均来近似：N昭占 (3)(6) 在进行下一次的状态更新与测量更新步骤之前，(&)给重采样后的所有粒子重新设置权
7、重卩=1 /A ,重复执2.2基于粒子滤波的SLAM的实现步骤SLAM是这样的一个过程：当机器人处于一个未知环境中且自身位置未知时，移动机器人在向前运动 过程中可以构建环境地图，并同时利用构建的地图来估计自身的位置。本文中的SLAM是基于粒子滤波算 法来实现的。在一个陌生的环境中，机器人位置的初始信度未知，所以认为机器人可能位于当前环境中的任何一个 位置。从表示当前环境的状态空间的均匀分布中随机采样N个粒子来表示机器人的初始信度分布。本文中 用于地图构建的SLAM算法描述如下：(1) 粒子的初始化：从均匀分布中采样R个粒子获得 初始粒子集： , 1/N,其中1/.V表示每个粒子的初始 权重；(
8、2) 对这川个粒子执行以下操作：状态更新二：；均，机器人做了实际的运动以后，粒子按照里程计获取到的运动信息执行运动更 新，得到下一时刻的粒子先验姿态；在(时刻雷达有了新的观测数据，然后计算每个 粒子的匹配得分，这个得分是根据激光雷达的测量模型得到的。在雷达测量过程中岀现的噪声被模型化为高斯模型，用表示一次扫描中每条光线的终点与地图 中最近的障碍物的儿何距离，然后传感器测量的概率就由一个均值为0的高斯分布描述：阿(C 二 exp-(Mu )2/2(r2|,其中/表示激光雷达一次扫描测量乙中 的第/条光线，每个粒子的匹配得分5“吹就是所有光线 的测量槪率之和 score =，Z-i);()(3)
9、(sc ore New , pose IN ew ) = o pt i m ize (x z )，最优 化 每个粒 子。把粒子向前后左右各个方向旋转、移动，并求出每次 姿态变换后的粒子得分，寻求最高得分se ore N ew，及其对 应的姿态卩oseNew ,这样做是为了加快定位过程的收敛；设置一个阈值mini mu in Sc-ore ,如果上一步求得的最 高得分seoreNew大于这个阈值，则认为移动后的姿态 poseNew是有效的，然后把当前姿态poseNew作为最优粒子姿态best Pose ,当前最高得分sc or eNew作为该粒子的 最优得分liestSeore 0否则认为粒子移
10、动后并没有达到想 要的效果，然后把移动前的粒子姿态作为最优粒子 姿态bestPose，其对应的得分作为最优得分lie st Score ；(3)计算权重权重与粒子匹配得分成正比，所以 什)有ivt =a bestScore , a足一个比例系数；(4权重归一化，得到匚二 一，归一化以后的权Z X-1二 (k) 重满足工/竹=1(5)根据最优粒子姿态及激光雷达的测量来更新地图 7r?x=updateMap (bestPoseh_i),其 中 表示 f - 1 时刻的地图，77?1表示t时刻的地图；(6 在粒子滤波过程中，过度的重采样可能会导致权 重高的粒子被去除，这样就导致粒子贫瘠，粒子集不能
11、有效描述状态的概率分布。描述状态概率分布的粒子数 口有限，如果不实施重采样，不重要的粒子就会一直在粒子集中，这样就会浪费计算资源。所以有必要评估当 前粒子集是否能很好地估计状态的概率分布。m.Neff二N21/Z表示粒子集中的有效粒子数，m_Neff用来评心|z估粒子集对状态的概率分布的近似程度，当m Neff小 于一定比例的粒T总数m_TrzhDld时，可以认为粒子集 不能很好地反映状态的概率分布了，这时就实施重采样= resample (.i r ),在进行卜一次的状态更新与测量更新(町 步骤之前，把重采样后的所有粒子重新设置权重匚二1/N,重复执行步骤(2)-(6)；如果m Neff大于
12、一定比例的粒子总数m Ti林hold 9则不实施重米样9 X? =x 9重复执行步骤(2)(6)。3在Cortex-A53平台上实现SLAM的方法整个系统的构建过程如下：软件平台中使用的操作系统是支持ARM架构处理器的Ubuntu matel6.04 LTS系统；使用的ROS(Robot Operating System)机器人操作系统是kinetic版本；进行网络配置，把处理器平台上的WiFi模块配置成Access Point(AP)模式，让上位机可以接入 AP与Cortex-A53处理器平台通信,这样就可以在个人计算机上通过远程登录来操控机器人；(4) 把上位机配置成时间服务器，让Cort
13、ex-A53平台每次上电后能与上位机的时间保持同步；(5) 构建地图使用了粒子滤波方法。在未知环境中是通过粒子滤波来实现机器人定位的，并在定位的 基础上根据激光雷达的观测数据来构建地图；(6) 在已知地图的基础上定位是通过粒子滤波实现的，机器人可以根据当前的激光雷达扫描数据和里 程计数据实现对自身的定位；(7) 有了环境地图，并且机器人可以对自身定位，再通过路径规划就可以实现机器人导航。全局路径 规划使用Dijkstra算法，在成本地图上寻找从起点到终点的最低成本路径。局部路径规划的算法思想为： 在机器人可以运动的速度和旋转角度范围内对速度和角度进行离散采样；根据机器人的当前状态对每一个 采样
14、速度进行向前的运动模拟，根据模拟过程中机器人与障碍物的接近程度、与目标的接近程度以及与全 局路径的接近程度等来给每条模拟轨迹打分，并放弃不合理的路线；最后选择得分最高的路线并给底座发 送相应的速度指令来让机器人运动。此种算法在局部路径规划过程中会在整个向前运动模拟过程中持续对 速度采样；(8) 对程序进行优化，减小Cortex-A53处理器平台的计算压力。系统构建完成后就可以在上位机上实时观察构建的地图，并可以在上位机上用软件来给机器人指定目 标,实现机器人自主导航。移动底座和激光雷达与Cortex-A53平台都是通过串口来通信的。在基于Cortex-A53处理器的平台 上处理激光雷达的扫描数
15、据以及底座中采集的里程计数据，结合激光雷达的数据和里程计数据实现机器人 的定位,并在定位的基础上利用雷达扫描数据实现二维地图构建。在对应的人机接口上可以通过软件在扫 描地图的基础上给机器人指定目的地来进行导航。系统的架构如图1所示。4算法优化4.1构图算法的优化Cortex-A53处理器平台结合激光雷达实现地图构建。图2为未优化的地图。从前文可知，地图构建是通过匹配得分最高的最优粒子结合激光雷达的扫描数据来实现的。构图算法 的优化方法为：通过给扫描匹配得分设定一个阈值minimumscore来提高所得地图的精度。这个阈值表示在粒子滤波过程中进行扫描匹配时要求的最小匹配得分,这个值设置的越大，就表示对 扫描匹配的要求越高。如果阈值设置过高，很容易导致匹配失败，机器人会选择使用里程计的数据，这样 就会使得对机器人姿态估计的准确度降低，从而导致构图精度降低；如果阈值设置过低，扫描匹配很容易 成功，会导致地图中出现大量噪声。设置合理的阈值可以加快机器人定位过程的收敛，得到精度更高的地 图。本次实验过程中测得的阈值范围约为60-80 ,阈值范围取决于激光雷达的测量范围、角度分辨率、测 量精度以及环境特征。图3为优化后的地图。4.2路径规划算法的优化在系统构建过程中发现在Cortex-A53处理器平台上使用的局部路径规划算法
