狭长巷道场景的规划导航方案

REF：地下矿山极简 SLAM 无人驾驶系统设计与应用研究

1. 系统架构

系统整体划分为系统管理层、远程操作层、边缘计算层与底层接口层，分别承担不同功能：

1. 管理层：负责全局任务配置与策略控制，提供调度分发与运行模式管理；
2. 远程操作层：支持实时监控、故障诊断与人工干预接口，同时集成自动避障与安全检测机制；
3. 边缘计算层：定位融合、环境感知、路径规划、轨迹预测及控制决策等核心算法模块；
4. 底层接口层：采集激光雷达、UWB、IMU 等传感器数据获取，并通过线控接口驱动执行机构执行。

a9c64cf39db85a9bb72759656c659d91.png

2. 导航算法

受限空间中路线使用 UWB 预先标记出轨迹点坐标（即预先标记的 UWB 轨迹点坐标），所有轨迹点均与系统实际部署的 UWB 基准点进行位置绑定，并在系统初始启动阶段同步作为导航初始化与实时位置校准的基准基点，构成整套导航系统的路径框架基础。通过UWB实现高精度路径规划与方向确定，并通过边界修正与控制策略动态调整车辆姿态，保障无人驾驶装备的路径可行性与运行安全：

1. 位置感知融合：UWB 模块提供稀疏但高精度的绝对定位信息，用于构建“极简轨迹点”路径骨架；激光雷达实时获取设备周围环境轮廓信息，执行局部地图校正与障碍物检测
2. 极坐标路径解算：将导航过程转化为极坐标系下的路径跟踪问题，通过方向角判别与安全边界判断实现目标点的方向解算与动态路径生成
3. 数据存储与响应优化：仅保留关键轨迹点与必要地图要素，简化地图建构与内存开销，结合轻量级运行架构，实现毫秒级路径更新与控制指令输出

3. UWB-激光雷达数据融合

在移动机器人配置 UWB 标识卡与 2D 激光雷达，依托多源数据融合架构增强定位鲁棒性与导航稳定性。其中，UWB 模块提供全局参考坐标定位数据，激光雷达则构建环境特征拓扑及边界约束模型。在融合策略上，采用扩展卡尔曼滤波器（EKF）对来自 UWB 测距与激光扫描的位置估计进行状态更新，基本流程包括：

1. UWB 测距输入：作为绝对定位基准，提供稳定的参考位置；
2. 激光雷达边界提取：实现与地图或局部特征的匹配，强化方向修正；
3. 卡尔曼滤波更新：引入协方差预测与观测残差修正，实现状态量平滑更新。

4. 局部路径规划

基于巷道“中间线”导向控制策略，结合极坐标路径方向与设备中心轴线之间的偏差信息，计算当前设备中心线与巷道中线形成的偏移角度作为矫正量反馈，形成装备控制输出的角度和角速度，装备动态调整行驶角度和速度，系统可实时修正设备路径偏差，动态适应巷道曲率变化，确保运行过程中的方向稳定性与边界安全性。

1. 角度控制输出模型：结合极坐标解算角和安全边界修正角，通过如下公式计算最终控制输出的角度

，其中θ\thetaθ表示控制输出的角度，k表示安全边界修正因子， δ\deltaδ表示安全边界修正角和极坐标解算角之间的安全边界修正角度差

1. 角速度条件模型：为进一步增强转向过程中的动态响应性，系统采用一阶动态响应模型对角速度进行调节

其中K表示比例控制增益，ω表示角速度

2695a327ee74af3ca383278b8396fe75.png

5. 自适应控制

实现系统的实时响应控制，采用典型 PID 控制器对方向偏差进行闭环调节，将受限空间无人驾驶控制输出，转化为 UWB 引导下的行进方向输出、转向角度与角速度输出，极大简化了装备无人驾驶控制过程，从而提高操作的简便性和执行效率。

1. 转向角度计算：Δθ=θ目标−θ当前
2. 角速度输出：比例-微分控制器 (PID 控制器) 计算角速度为：

1. 自适应PD参数：参数的值自适应实时更新，基于系统性能指标误差 e(t) 及其变化率自动修正控制参数，从而增强控制响应的灵活性与稳态精度

1. 角速度平滑限制：防止角速度的急剧变化对机械系统造成损害，引入一个限制函数来平滑角速度为：