建图及路径的优化处理方法

REF：基于激光 SLAM 的移动机器人自主导航方法研究

1. 常用导航方式概述

移动机器人的导航方式有很多，包括GPS导航、惯性导航、超声波导航、视觉导航、激光导航等，实际应用中需根据机器人不同的工作环境与导航要求进行选择。

• GPS 导航：通过无线电发射信号，应用于室外具有大范围定位需求的机器人，但GPS 系统确定位置受环境因素影响较大，易造成定位失败，因此多与惯导等传感器联合使用
• 磁导航：主要有电磁导航和磁带、磁钉导航， 电磁导航需要预先在地下铺设导线，铺设和维护成本较高、路径难以更改；磁带容易损坏，机器人只能沿着固定轨迹行走，无法实现智能避让或通过控制系统实时更改任务； 单磁钉导航容易受到干扰，双磁钉定位模型补偿惯性导航角度误差，在磁干扰区域则可通过视觉定位进行补偿
• 惯性导航：不依赖任何外部信息，不受气候条件、外部信息、电磁干扰的影响，能够在全天候全时间工作在任何环境下，随着时间的积累，误差将不断增大
• 路标导航：使用具有固定位置的特殊标志，如二维码、反光板等，可分为两大类：人工路标导航是事先在机器人运行环境内采用反光材料打印制作颜色鲜明图案作为标记，通过外界光照被摄像机或激光雷达捕捉；自然路标导航是通过对周围自然环境进行特征识别完成导航
• 超声波导航：容易受天气、周围环境以及被测物体表面形状等外界因素的影响
• 传统视觉导航：预先在行进路径上铺设导航标识带、二维码或者各种定位符等轨迹标志
• 基于深度强化学习的人工智能体视觉导航：是人工智能和机器人领域的一个新的研究热点，将深度强化学习的决策过程融入到视觉导航中，导航方式无需复杂设置，但可靠性不高。
• 激光导航：分为基于反射物和基于外界自然环境的激光导航，基于反射物主要是利用激光测距原理，反射物(如反光板)预先被设置在环境中，通过三边定位与三角定位等实现定位，这种方式灵活性较差，适用于机械作业的工厂车间。基于外界自然环境激光导航主要为 SLAM 激光导航，可使机器人在没有获得外界环境的先验知识下利用自身传感器实现定位与建图。

636213e071dc04dac27d1294a71fff4c.png

2. 系统架构

机器人软件系统架构图

2f7f89ee0fca077b3799e4c6bae3de09.png

机器人软件系统数据流图

db0345ca441ebff11544cebb797cfa32.png

机器人硬件数据流图

b9307741f5decc24c09918e341b448c3.png

3. SLAM建图优化

图优化的 SLAM包括前后端，前端本质上为数据关联问题，接收传感器采集到的障碍物点云信息，通过相近两帧点云关系估计机器人的位姿，可将相近激光帧进行匹配以增量式的建立地图；后端接收不同时刻机器人的观测数据，利用回环检测得到的相应约束关系对前端累积误差进行优化。

367f9c0100a80cf2832548783ad256a2.png

• 前端优化直线特征提取：在原始空间中属于同一条直线上的点转换到霍夫空间后会交于一点，此点在霍夫空间的值即表示了原始空间中包含较多点的某条直线斜率与截距。因此霍夫变换是一种利用投票算法来检测具有特定形状对象的过程，该过程在参数空间中通过计算累计结果的局部最大值，得到一个满足特定形状的集合作为霍夫变换结果。

fa2f1237338a7d95a9718d2c68e5d3db.png

• 前端基于法向回归的最小二乘直线拟合：通过霍夫检测可以提取同一条直线上的点，但为提高检测效率，在霍夫变换时对角度与长度进行了离散，故检测的直线不可作为点云特征线段，需要对属于同一直线的点进行拟合。

3c00c175941980faed6058be3966e388.png

• 前端基于线段特征的帧间匹配：相邻两帧点云中即使同一条特征线段也可能存在长度不同的情况，传统利用线段长度、中点位置等作为度量标准的方法并不适用于特征匹配，相较于使用单一的直线段特征，线段之间的夹角则具有平移、旋转和缩放不变等优良性质，在利用霍夫变换提取直线特征后求取了线段的斜率等参数，因此使用线段之间的长度、角度等进行特征匹配。

f665748f18fa39b39d09e5aa8b62a417.png

• 后端自适应LM 算法的信赖域：传统LM 算法的步长无法自动调节，为避免增量步长过长或过短，通过目标函数差作为调节因子，设置动态调节步长。
• 后端优化异常值处理：引入Huber核函数，在误差较大时，把原先平方误差项替换为增长较慢的函数，同时保证函数整体光滑的性质，会使结果更加鲁棒。

250db0c64c2fed4e428f3e18ceee73b6.png

4. A-star全局路径优化

• 动态邻域优化：8领域改为动态5领域

631eb7f6e88eee8d6dcea6d620cd83af.png

• 启发函数优化：使用曼哈顿距离与欧氏距离的组合作为启发函数核心，进行指数衰减加权后，同时增大反向惩罚力度。

6f360cf0c2143d000fde81bd188885ba.png

7be2347f3e8875ea489679e8f0cb48aa.png

4a1aee067b562a73af18ffe368eebfc1.png

d25847695e7a92b09f3ebab614bc6931.png

4562d6875440d98123590587d240f50a.png

• 拐点数优化：在当前节点选取的扩展节点在保证代价一致的基础上，优先与当前节点的父节点扩展方向一致

dd01f320e67a13c38d7be7a3e529a031.png

5. DWA局部路径优化

默认DWA评价函数如下，主要从三方面进行优化，优化前后评价函数如下：

d98fb51727df73b6e3e82e3bd9797400.png

a39d4337495fa0b052579dd72658734a.png

• 优化方位角约束：在机器人初期距离目标点较远时，应减小方位角的评估权重，便于避障；当机器人逐渐接近目标点时，应增大方位角的评估权重，便于到达目标点。

dbaddc1a330734739573328048eebb85.png

• 增加目标点距离约束：当机器人逐渐靠近目标点时，优先选择距离目标点较近的轨迹能使机器人更为准确到达目标点，但在初始运动阶段，这一过程应被削弱。

c448fa39a82439f725d4cb4af579b004.png

• 增加速度平滑性约束：尽可能保证机器人行驶速度的连续以提高运行安全性。

a76c9eee7bebf518ace15f311c809d7b.png

6. 规划融合

轨迹规划的目的是生成含时间序列并满足一定约束条件让机器人在每一个路径节点都能平滑过渡的轨迹，其要求与必要性主要有：(1) 适合机器人自主移动；(2) 速度和加速度等动力学状态无法突变；(3) 机器人不必在拐点处加减速；(4) 节约能量。因此需要选择合适的轨迹表达方式。

• 多项式路径平滑：
• 贝塞尔轨迹规划：

0f25eb79a966d84cb4934885efdeb369.png