煤矿机器人路径规划

REF：基于 SLAM 的煤矿巡检机器人路径规划及导航研究

1. 矿用机器人

矿区环境Telerescuer巡逻机器人

defebd0be506945a681a0352e810c220.png

澳大利亚联邦科学与工业研究组织研制的煤矿burro巡逻机器人

4530a89175f7f73f32c0578b7d1ceb37.png

美国卡内基梅隆大学的机器人研究所研制的煤矿Groundhog巡检机器人

4a3d34a49cb06f0cdd7bf2aa88cb193e.png

英国诺丁汉大学研制的六足仿生巡逻监察机器人

63aa2c471ffcb3e04a6a9d0aa8fe0ba4.png

中国矿业大学机器人研究中心研制的矿山侠 KXJ-1 巡检机器人

0f19a13a74dda1e50e3398a5774d5e40.png

中国煤科机器人公司研制的胶带机巡检机器人

5657861ccc6c571f1ae36efc6e7920f2.png

山西科达自控股份有限公司研制的矿用挂轨式智能巡检机器人

9750eece8a2b2dae14f7453f046029da.png

2. SLAM建图

• 基于 ROS 系统的 SLAM 流程

29e9897b7e7ceeca90c7170d47509366.png

• 基于粒子滤波的Gmapping 算法：主干采用RBPF 重采样粒子滤波器，将机器人定位和地图构建分为两个独立流程进而分两步进行：分别使用粒子滤波器进行机器人定位，使用定位结果来进行环境构建。

21d479884ce6a966ab693197ccbd1a06.png

• 基于高斯牛顿优化的Hector 算法：一种基于 2D 激光雷达的建图与实时定位方法，建图过程的核心是扫描匹配，核心原理采用高斯牛顿优化的扫描匹配技术，通过直接对齐连续的激光扫描数据，从而估计机器人的运动，Hector 不依赖里程计信息，只需要激光雷达数据就可实现位姿估计，这使得在没有轮式编码器或里程计不可靠的情况下表现更好。

a7b98eba6e2fba718463ae027da2c8d8.png

• 基于图优化的Cartographer 算法：首先通过传感器计算出每一时刻的位姿，并建立每一位姿之间的约束关系，再进行总体优化。机器人的不同时间点的位姿被视为一个节点，而位姿之间的关系则通过边表示，这种由节点与边组成的图叫做位姿图，在完成位姿图的构建后，调整机器人的位置姿态，以尽可能符合由边所构成的约束条件，使得经过优化之后的位姿图以最小的误差满足节点之间的约束关系，优化的结果就是机器人最终的轨迹估计。

b27b287e3ac9b6ae644d5dd77b752df5.png

3. 全局路径规划

海洋捕食者算法（MPA）是一种元启发式优化算法，设计灵感来源于海洋生态系统中捕食者与猎物的动态交互机制，算法通过模拟海洋生物的群体行为：捕食者个体通过分阶段策略动态调整自身位置以优化搜索目标，具体包括探索、追踪与围捕三个阶段，每个阶段对应不同的数学运算规则以实现全局探索与局部开发的平衡；与此同时，算法引入猎物的逃逸行为模型，通过随机扰动机制提升种群多样性，从而增强算法跳出局部最优的能力；算法结合布朗运动和莱维飞行两种关键的随机游走策略，分别用于不同阶段的全局探索和局部开发。算法通过模拟海洋生态系统中捕食者与猎物的互动行为，将优化过程划分为三个阶段：

• 高速探索阶段：目的是快速覆盖解空间，避免陷入局部最优。在此阶段，捕食者以高速移动，模拟鲨鱼在资源丰富 区域的大范围搜索行为；猎物移动速度较慢，但随机性较强，模拟逃逸模式，其位置更新基于布朗运动，随机性强，适合全局探索。
• 平衡阶段：目的是平衡探索与开发，逐步收敛至潜在最优区域。此阶段，捕食者与猎物速度一致模拟生态平衡状态，部分个体转向局部开发。捕食者组采用莱维飞行策略，利用精英信息进行局部开发；猎物组采用布朗运动策略，继续进行全局探索。通过混合，布朗运动维持多样性，莱维飞行增强局部搜索。
• 低速开发阶段：目的是在最优解附近精细搜索，提升收敛精度。在此阶段，捕食者低速移动，模拟在猎物稀少时对局部区域的精细搜索；猎物加速逃离，但捕食者通过莱维飞行的短步长扰动保持局部搜索能力。此时更新公式以莱维飞行为主，结合精英引导，短步长扰动在精英附近密集采样，偶尔的长步长跳出局部最优，防止收敛。

海洋捕食者的行为模式由其内在运动策略与外部环境动力学共同驱动，生物行为与环境因素的协同作用，共同塑造了海洋捕食者群体的时空动态特征。从个体行为层面分析，捕食者通过分阶段切换的运动机制实现资源搜索，在稀疏环境中采用长距离跳跃的莱维飞行策略，而在高密度区域则切换为短程探索的布朗运动；从环境影响维度考察，涡流、洋流等海洋流体动力学特征通过改变物质输运路径间接调控捕食者分布，而鱼类聚集装置 FADs 作为人工设施则通过人为诱集效应显著影响其空间聚集模式。

• Tent 混沌映射：目的是利用混沌系统覆盖整个解空间的遍历性和看似随机但由确定性规则生成的伪随机性，替代传统的均匀随机初始化，以提升初始种群的多样性和分布均匀性，避免算法因初始解聚集而陷入局部最优。Tent 映射作为具有确定性遍历特性的离散混沌系统，可避免传统均匀分布初始化中可能出现的局部聚集现象，通过其独特的动态行为，在保持随机性的同时确保全局搜索的均匀性，从而提升优化算法的初始种群质量。
• 惯性权重调整：目标是动态平衡算法的全局探索与局部开发阶段，避免过早收敛到局部最优，同时提升收敛速度和精度，通过设计非线性递减的惯性权重策略，引入自适应惯性权重设计，惯性权重控制个体位置更新时历史位置的影响程度。
• 混合模拟退火：扰动机制与退火策略，在迭代后，解的适应度值更为理想的情况下，当前解也有一定的概率被接受，在 MPA 的每次迭代中对当前最优路径施加随机扰动，并基于退火温度决定是否接受新解。

将 Tent 混合策略、参数自适应优化和基于模拟退火策略三种改进方法按顺序引入海洋捕食者算法，在种群初始化阶段、高速探索阶段和低速开发阶段对 MPA 算法进行改进。

4. 局部路径规划

TEB 算法又称时间弹性带算法，是一种基于优化的局部路径规划方法，专为动态环境下的移动机器人设计，核心思想将机器人的运动轨迹建模为一条由连续位姿点构成的弹性带，结合时间维度对轨迹进行多目标优化，同时严格约束机器人的运动学模型及动力学可行性，核心是通过多目标优化生成满足约束条件的最优轨迹，其目标函数和约束条件共同定义了轨迹的安全性、运动学可行性和效率。

6de14103e12369f44e0b866d59a1e5c5.png

• 路径跟随与避障约束：在运行过程中需要跟随全局路径并且避开障碍物，算法通过施力使其远离障碍物，拉力使其拉向全局路径，路径跟随和障碍物约束可用惩罚函数表示。
• 速度与线速度约束：为保障机器人运动轨迹的动力学可行性，构建基于状态转移约束的优化框架，包括角速度、线速度、角加速度和线加速度，针对速度与角速度的约束，通过构建惩罚函数将运动学限制转化为优化目标的显式代价项；同时为确保轨迹的平滑性和运动可行性，需对机器人线加速度和角加速度施加约束，加速度惩罚函数通过相邻位姿间的平均速度变化率构建
• 非完整运动约束：阿克曼机器人作为典型的非完整约束系统，其运动学特性受限于车辆结构设计。在二维平面运动学模型中，该类机器人仅具备线速度和角速度两个自由度，其运动轨迹受限于瞬时航向角，仅能沿当前方向执行直线运动或以车辆轴距为半径的圆弧转弯，这种运动学约束导致转向操作必须通过连续圆弧段的平滑衔接实现，其路径规划本质上为对圆弧轨迹的分段优化问题，机器人在转向过程中需通过多段圆弧轨迹的组合实现方向调整。
• 最短时间约束：算法采用整个轨迹的最短时间来代替最短路径评价函数，其约束函数将累加路径上的所有时间间隔，通过寻找最小值来判断最优路径。

77cca8f977d7416b31955a016cd7ec6a.png

5. 融合路径规划

为提升系统在煤矿环境中的导航精度与任务适应性，对两种算法进行融合，融合算法在全局路径规划中，通过增 Tent 映射优化初始种群分布，改善传统随机初始化种群分布不均匀问题；接着调整自适应参数，在算法第一阶段和第三阶段改变惯性权重，提升算法的收敛精度；最后引入模拟退火算法，利用算法的扰动机制和退火策略，优化算法陷入局部最优解的情况；局部避障过程中 TEB 算法基于时间弹性带理论以全局路径作为参考路线，通过滚动时域控制生成平滑可执行的轨迹，在突发障碍物时实现避障。

effee14849777e4580422ba692fe3ce1.png