基于蚁群算法的路径规划

REF：移动机器人 SLAM 建图与路径规划导航的研究应用

1. 基础概念

• Cartographer SLAM算法框架

f84165fba134cd6517789f6011f0478a.png

• 路径规划算法：目的是在满足约束条件的情况下找到从初始状态到目标状态的路径。

c93fb96fe500a56688220431e994a4d9.png

• IMU：用来测量加速度和角速度，采集到的原始数据都存在较大的误差，经过标定和滤波之后的数据，就可以用来进行姿态融合，求出传感器的运动状态

1a0f33f59e95981ac3d35ed2422aa63a.png

• 激光雷达：工作原理是发射探测信号并接收物体反射回来的信号，将这些返回信号与原始发射信号进行对比处理，从而能够提取出物体的多种信息，包括物体的距离、方位、高度、速度、姿态以及形状等参数。

630c10d9b963f1c98a488327783141c2.png

• ICP 算法：求前后两帧雷达数据对应机器人位置的转移关系𝑇，用位置的转移关系𝑇近似表示其当前的运动𝑉，然后利用这个运动信息𝑉对当前的雷达数据做补偿。

0d2608fc4994b1a7d91a24300de00b6a.png

• 结合里程计与 ICP：先利用轮式里程计对雷达畸变做初步校正，然后将校正后的雷达数据放入 ICP 算法求位置转移，利用得到的位置转移信息反过来修正里程计的误差，形成一个迭代闭环，通过不断的迭代，实现对雷达运动的畸变校正。

b6fd42748df7b537b53faf837b701f16.png

• 波束模型： 通过发射激光束并测量它们反射回来的时间来确定物体的距离，对于单线激光雷达，会在特定的角度发出激光束测量到值𝑧𝑘𝑖 ，当激光雷达完成一轮完整的旋转扫描后，它会收集到一串观测值𝑧𝑘，包括(𝑧𝑘1, 𝑧𝑘2, ⋯ , 𝑧𝑘𝑖, ⋯ , 𝑧𝑘𝑛)，观测𝑧𝑘的不确定性是每个单独激光束测量值𝑧𝑘𝑖 不确定性的累积结果，这可以表示为式中的乘积形式

51b419b04a5f6d5453516cb135b2a546.png

6bfcfcdddb552e84e5766cf9a0cafd21.png

其中：

• hit：由于自身的构造、计算精准度、分辨率等因素而引入误差，遵循高斯分布

83924cbef1378141100f536b713bc649.png

• short：表示环境中的动态障碍物，截断激光束导致地图中已知障碍物的测量值小于其实际距离，指数分布

00ebf99eef23f3e925d929112c7ab054.png

• max：表示导致激光雷达的测量失败的特定障碍物，如黑色吸光、透明或具有镜面反射的物体，倒数分布

a440cb5b0c65e799c7548ab5e9a61dcc.png

• rand：那些难以具体建模的随机误差，均匀分布

2858b65cbbe2b1ec39028c1fd83dfde0.png

2. SLAM建模

• Gmapping 算法：

de4e45cbf8c09b80148d059a31cd89ef.png

• 粗略匹配改进：使用主成分分析（PCA）将原始特征投影到主成分上，减小信息损失，通过计算协方差矩阵获得特征值和特征向量，并构建转换矩阵，将数据的维度映射到与主成分数量相同的维度上，实现点云的粗略配准。
• 改进 ICP 精确配准：考虑点到线段几何关系和权重因子，通过引入动态权重和归一化距离度量，旨在出现噪声和异常点的情况下提高点云配准在的鲁棒性和准确性

3. 路径规划

ae011d08068cde3e6bb42ec32211bd82.png

1. 传统蚁群算法：模仿蚂蚁在寻找食物的过程中，会在路径上留下信息素，后续的蚂蚁会根据信息素的浓度来决定选择哪条路径，这种正反馈机制有助于蚁群逐渐找到最短的食物路径，假设共有𝐴只蚂蚁，蚂蚁𝑎(𝑎 = 1,2, ⋯ , 𝐴)受到信息素浓度影响选择下一个访问的路径，传统蚁群算法转移概率（在时间𝑡，蚂蚁k从当前节点𝑖转移到节点𝑗的概率）如下：

e28bf1456d5cfad57f68bd7d034764ce.png

为了确保信息素启发因子能够有效地影响路径选择，在每次迭代完成后对信息素进行更新。设定𝜌表示信息素挥发系数，𝜌的取值范围为（0,1），信息素更新公式如下

9ca593db3a949a13d722245eaac20906.png

• ∆𝜏𝑖𝑗(𝑡)：𝑛只蚂蚁在一次迭代后，在路径节点< 𝑖,𝑗 >上留下的信息素增量和
• ∆𝜏𝑖𝑗𝑎(𝑡)：第𝑎只蚂蚁在路径< 𝑖,𝑗 >上留下的信息素量
• 𝑄：一个常量，为信息素强度系数
• 𝐿𝑎：第𝑎只蚂蚁本次迭代所走过的路径长度
• 𝜏：指在时间𝑡从节点𝑖转移到节点𝑗路径上的信息素浓度
• 𝛼：用来调节信息素影响大小的参数
• 𝛽：期望启发信息在路径选择中的重要性因子
• 𝑎𝑙𝑙𝑜𝑤𝑒𝑑_k：所有蚂蚁𝑘尚未访问栅格的一个集合
• 𝜂：一个启发式信息，它与节点𝑖到节点𝑗之间的距离成反比，𝜂=1/d_ij，d表示i、j间的欧式距离

1. 人工势场改进启发信息函数：当前节点到目标点的距离越大，受到的人工势场引力势能值就越高，通过增强搜索过程中的目的性，从而提高算法的搜索效率
2. 优化信息素自适应能力：在原有信息素更新的基础上，引入上一代最优路径和最差路径上所有蚂蚁携带的信息素常量总和的差值，以此来调整信息素的更新，从而提高算法的全局搜索能力，自适应的调节信息素
3. 三角剪枝法优化转弯点：将原始路径中相邻的转弯点进行连接，如果连接后的路径不与障碍物相交，则去除两点之间多余的转折点和冗余线段，并对路径进行优化更新。