作者小怪兽1. 二维障碍物环境下的移动机器人路径规划概述
在室内服务机器人、工业AGV等实际应用中，移动机器人常需要在包含桌椅、设备等障碍物的二维平面环境中自主导航。这类场景下的路径规划需要解决三个核心问题：如何高效探索未知环境、如何避开静态/动态障碍物、如何处理定位误差带来的不确定性。传统基于栅格地图的A*、Dijkstra等算法虽然能保证最优解，但在复杂环境中计算量会呈指数级增长。而基于随机采样的RRT算法则展现出独特优势——它不需要预先构建完整地图，通过树状结构的增量式扩展就能快速找到可行路径。
我在实际项目中发现，单纯的RRT算法生成的路径往往存在两个典型问题：一是路径曲折不光滑，导致机器人需要频繁启停转向；二是当定位存在误差时，规划路径可能与实际障碍物发生碰撞。这就引出了本文要探讨的两个关键技术：RRT算法的优化改进，以及卡尔曼滤波对定位不确定性的修正。通过MATLAB仿真可以看到，经过优化的RRT*算法配合卡尔曼滤波定位修正，能使路径长度缩短约25%，同时将碰撞概率降低60%以上。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. RRT算法原理与实现细节
2.1 基础RRT算法的工作流程
RRT算法的核心思想可以用"盲人摸象"来类比——机器人像盲人一样通过随机试探来逐步构建环境认知。其MATLAB实现通常包含以下关键步骤：
- 初始化树结构：以起点q_start为根节点创建空树T
- 随机采样：在自由空间内生成随机点q_rand
- 寻找最近邻：在T中找到距离q_rand最近的节点q_near
- 扩展新节点：从q_near向q_rand方向步进固定距离ε，得到新节点q_new
- 碰撞检测：检查线段q_near→q_new是否与障碍物相交
- 添加节点：若无碰撞则将q_new加入T，并记录父子关系
- 终止判断：若q_new进入目标区域则回溯生成路径
matlab复制function path = RRT(map, start, goal, max_iter)
tree.vertex(1).coord = start;
tree.vertex(1).parent = 0;
for k = 1:max_iter
q_rand = random_point(map);
[q_near, idx] = nearest_neighbor(q_rand, tree);
q_new = steer(q_near, q_rand, step_size);
if ~collision_check(q_near, q_new, map)
tree.vertex(end+1).coord = q_new;
tree.vertex(end).parent = idx;
if norm(q_new-goal) < goal_radius
path = backtrace(tree);
return;
end
end
end
end
2.2 碰撞检测的工程实现要点
在实际编码中，碰撞检测是最影响性能的环节。对于矩形障碍物，我推荐采用分离轴定理(SAT)进行快速判断。其MATLAB实现技巧包括：
- 障碍物预处理：将障碍物顶点按顺时针顺序存储，便于计算边法向量
- 投影优化：只计算线段与障碍物相邻边的投影，减少计算量
- 提前终止：发现任一分离轴立即返回无碰撞结果
matlab复制function collision = collision_check_segment(p1, p2, obstacles)
for i = 1:size(obstacles,1)
obs = reshape(obstacles(i,:),2,4)'; % 转换为4x2顶点矩阵
if sat_test(p1,p2,obs)
collision = true;
return;
end
end
collision = false;
end
实测表明，这种优化能使碰撞检测速度提升3-5倍，特别在障碍物密集环境中效果显著。
3. RRT算法的优化改进策略
3.1 RRT*的渐进最优特性
RRT*在基础RRT上增加了重布线(Rewire)步骤，通过以下两个关键改进实现路径优化：
-
近邻节点集选择：以q_new为中心，半径r内所有节点作为候选父节点
matlab复制neighbors = []; for i = 1:length(tree.vertex) if norm(tree.vertex(i).coord - q_new) < r neighbors = [neighbors i]; end end -
最优父节点选择：计算经各邻居节点到达q_new的路径成本，选择最小成本路径
matlab复制min_cost = inf; best_parent = idx; for j = neighbors cost = tree.vertex(j).cost + norm(tree.vertex(j).coord - q_new); if cost < min_cost && ~collision_check(tree.vertex(j).coord, q_new, map) min_cost = cost; best_parent = j; end end
3.2 实际应用中的参数调优经验
通过大量实验，我总结出以下参数设置经验：
-
步长ε的选择：
- 小型机器人(直径<0.5m)：建议ε=环境对角线长度的1/50
- 中型机器人(0.5-1m)：建议ε=1/30
- 过大步长会导致碰撞概率增加，过小则降低探索效率
-
重布线半径r：
- 理论值应为γ*(log(n)/n)^(1/d)，其中d为维度
- 实践中我常用r=2~3ε，平衡优化效果与计算开销
-
采样偏向：
- 以10%概率直接采样目标点，加速收敛
matlab复制if rand() < 0.1 q_rand = goal; else q_rand = random_point(map); end
4. 卡尔曼滤波处理定位不确定性
4.1 机器人运动与观测模型
考虑两轮差分驱动机器人，其状态向量为x=[px,py,θ,v,ω]^T，包含位置、朝向、线速度和角速度。运动模型采用：
code复制x_k = f(x_{k-1},u_k) + w_k
= [px + v*dt*cosθ
py + v*dt*sinθ
θ + ω*dt
v + a_v*dt
ω + a_ω*dt] + w_k
观测模型假设使用激光雷达和编码器：
code复制z_k = [px
py
θ
v] + v_k
4.2 卡尔曼滤波实现步骤
-
预测阶段：
matlab复制% 状态预测 x_pred = f(x_est, u); % 协方差预测 F = compute_jacobian_f(x_est, u); % 计算雅可比矩阵 P_pred = F * P_est * F' + Q; -
更新阶段：
matlab复制% 卡尔曼增益计算 H = [1 0 0 0 0; 0 1 0 0 0; 0 0 1 0 0; 0 0 0 1 0]; K = P_pred * H' / (H * P_pred * H' + R); % 状态更新 x_est = x_pred + K * (z - H * x_pred); % 协方差更新 P_est = (eye(5) - K * H) * P_pred;
4.3 定位误差对路径规划的影响量化
通过蒙特卡洛仿真可以量化定位误差的影响：
- 在10m×10m环境中设置5%的障碍物密度
- 分别测试定位误差σ=0cm、5cm、10cm时的规划结果
- 每组实验重复100次，统计指标：
| 误差水平 | 平均路径长度(m) | 碰撞概率(%) | 到达时间(s) |
|---|---|---|---|
| σ=0cm | 14.2 | 0 | 8.7 |
| σ=5cm | 15.8 (+11.3%) | 23 | 12.4 |
| σ=10cm | 17.5 (+23.2%) | 41 | 15.9 |
结果表明，定位误差会导致路径质量显著下降。这正是需要集成卡尔曼滤波的原因。
5. RRT与卡尔曼滤波的协同实现
5.1 系统架构设计
完整的协同系统包含三个核心模块：
- 感知层：激光雷达+IMU+编码器多传感器融合
- 估计层：卡尔曼滤波实时输出最优状态估计
- 规划层：RRT*算法基于修正后的状态进行路径规划
matlab复制while ~reach_goal
% 传感器数据读取
[z_lidar, z_encoder, z_imu] = read_sensors();
% 卡尔曼滤波更新
[x_est, P_est] = kalman_update(x_pred, P_pred, z_lidar, z_encoder);
% RRT*路径规划
if mod(step, replan_interval) == 0
path = RRT_star(map, x_est(1:2), goal);
end
% 控制指令生成
u = pure_pursuit(path, x_est);
% 状态预测
[x_pred, P_pred] = kalman_predict(x_est, P_est, u);
end
5.2 不确定性感知的路径优化
在RRT*的代价函数中引入不确定性项：
code复制cost = α*path_length + β*uncertainty
其中uncertainty通过协方差矩阵P的迹来度量：
matlab复制function u_cost = uncertainty_cost(node, P)
sigma_max = 0.5; % 最大允许不确定性
u_cost = trace(P(1:2,1:2)) / sigma_max;
end
这种优化会使机器人优先选择定位精度更高的区域通过，虽然可能略微增加路径长度，但能显著提高安全性。
6. MATLAB实现技巧与调试心得
6.1 可视化调试工具搭建
良好的可视化能极大提升开发效率。我通常创建以下图形窗口：
-
主视图：显示地图、障碍物、RRT树、路径
matlab复制figure(1); clf; plot_obstacles(map); hold on; plot_tree(tree); plot_path(path); -
卡尔曼滤波状态监控：
matlab复制figure(2); subplot(211); plot(x_est(1),x_est(2),'bo'); % 位置估计 subplot(212); plot(P_est(1,1),'r-'); % 协方差变化 -
性能统计面板：
matlab复制fprintf('Iteration: %d, Path cost: %.2f, Uncertainty: %.3f\n',... iter, path_cost, trace(P_est));
6.2 常见问题排查指南
-
路径震荡问题：
- 现象：机器人频繁改变行进方向
- 检查：增大重规划间隔replan_interval
- 调整：在代价函数中加入路径平滑项
-
定位发散问题：
- 现象：协方差矩阵P持续增大
- 检查：观测噪声矩阵R是否设置合理
- 调整：增加传感器冗余，如添加视觉里程计
-
规划超时问题：
- 现象：RRT*无法在指定时间内找到路径
- 检查：采样区域是否包含可行通道
- 调整：采用双向RRT(从起点和目标点同时生长)
通过实际项目验证，这套方法在办公环境服务机器人中实现了98%的任务完成率，平均路径规划耗时控制在200ms以内，定位误差稳定在±3cm范围内。