简介面向移动机器人路径规划研究这套MATLAB代码将改进的RRT全局路径规划与DWA局部动态窗口法结合实现全局轨迹引导下的实时避障。压缩包共14个m文件大小仅14KB主要包含主程序、动态窗口计算、轨迹生成、碰撞检测与评价函数等模块代码结构清晰便于按需调用。目前已有370人学习下载。代码基于DWA三维框架设计可加载三维RRT算法生成的全局参考轨迹当机器人感知到全局路径存在障碍物时自动计算离开原路径的切出点与返回原路径的切入点完成局部重规划并绕过障碍物后继续沿全局路径运动。全部m文件均附详细注释涵盖参数初始化、障碍物构建、安全距离判定与速度采样等关键环节适合学习路径规划算法原理、复现混合规划流程并进行二次开发。1. RRTDWA这套组合拳先想清楚全局和局部谁说了算做移动机器人的路径规划最稳的组合不是把某个算法调到万能。用改进后的RRT算法先算出一条可走的全局路径再用DWA动态窗口法在局部做重规划、实时绕开传感器新发现的障碍是室内轮式机器人上最常见的分层方案。全局层管大方向局部层管刹车和转向两者按视野与频率拆开各自都保持简单可靠。这篇笔记适合已经接触过基础RRT但嫌路径绕、或者单独写过DWA但发现它容易陷进死胡同的从业者。下面是带详细中文注释的RRT改进实现、DWA采样与评价实现以及联调时最常出现的几个排查场景。2. 改进RRT全局路径目标偏置采样与路径剪枝的落地写法2.1 基础RRT的痛点与三处改进基础RRT的逻辑是从起点开始随机长树直到某个扩展节点够到终点附近。它实现简单、概率完备但生成路径很粗糙节点多、折线密、路径贴障碍边缘走。这对上层DWA是有压力的——DWA视野有限拿到这样的全局路径容易在某个折点突然变向造成轨迹震荡甚至原地打转。所以工程里几乎不会直接拿基础RRT的原始路径给DWA用中间必须加改进和剪枝。改进集中在三个地方目标偏置采样让树尽量朝终点生长而不是满图乱钻固定步长扩展每一步长度受限碰撞检测在扩展的同时完成路径剪枝把能直连的冗余节点删掉。这三处改动代码量都很小但对路径形态的影响是质变。先看采样函数改进点在“以一定概率直接采目标点”import random import math def sample_with_bias(goal, width, height, bias0.15): 目标偏置采样每次撒点前掷一次骰子。 - 命中bias区间直接用目标点作为采样点 - 否则整张地图范围内均匀随机采样 作用让RRT树更快朝目标伸展减少空旷区域的无效生长 路径整体更直迭代次数通常能下降一半以上。 if random.random() bias: return (goal[0], goal[1]) return (random.uniform(0, width), random.uniform(0, height))bias取0.15含义是平均每7次采样约一次直接指向目标点。bias太高时树会近似贪心地向目标冲一旦中途碰壁容易僵住太低则改进效果消失。一般从0.1起步地图障碍率低时可以往0.2调。这个值不影响正确性只影响收敛速度和路径质量所以是全局层第一个值得打印出来对比的参数。有了采样点下一步从现有树里找最近节点并向采样点方向扩展一个固定步长def steer(nearest, sample_point, step_size): 从nearest出发朝sample_point方向扩展step_size长度。 如果sample_point就在步长范围内直接返回sample_point。 返回的是一棵树上新节点的坐标。 dx sample_point[0] - nearest[0] dy sample_point[1] - nearest[1] dist math.hypot(dx, dy) if dist step_size: return sample_point ratio step_size / dist return (nearest[0] dx * ratio, nearest[1] dy * ratio)这段代码没有难点但步长取值直接影响路径锯齿程度。步长太小树长得慢迭代次数必须上调步长太大新节点容易跨过障碍边缘碰撞检测会频繁拒绝新点在窄通道里连续失败。我习惯先按地图尺寸的1%~2%给初值再根据碰撞失败率微调。如果新版节点被拒绝的比例超过三成优先减小步长而不是加大迭代次数。2.2 碰撞检测与膨胀半径几何判断和性能取舍碰撞检测是RRT里调用最频繁的函数它的快慢直接决定规划耗时。常见做法是把两点之间的线段离散成等分点逐个检查是否落入障碍膨胀区。下面这个版本按点障碍列表实现适合圆形或近似圆形的障碍描述def is_collision_free(p1, p2, obstacle_list, inflation5): 线段碰撞检测把p1到p2的线段按约2像素间隔切分 检查每个中间点是否进入障碍物膨胀后的范围。 - obstacle_list: 每项为(x, y, radius) - inflation: 障碍半径额外扩大的安全距离 返回True代表两点之间可通行。 dist math.hypot(p2[0] - p1[0], p2[1] - p1[1]) steps max(1, int(dist / 2)) for i in range(steps 1): t i / steps px p1[0] (p2[0] - p1[0]) * t py p1[1] (p2[1] - p1[1]) * t for (ox, oy, r) in obstacle_list: if math.hypot(px - ox, py - oy) r inflation: return False return True离散间隔取2像素。间隔太粗会把细缝误判成可通行太细则每条线段要做几百次距离计算RRT一次规划要检测上千条树枝耗时明显放大。如果地图是栅格格式更高效的做法是提前把障碍栅格做一层膨胀再在检查时逐个查栅格值省去距离计算。用栅格地图时is_collision_free的离散点直接索引地图数组比遍历障碍列表快一个数量级。膨胀半径单独拎出来说是因为它经常被忽略。RRT的膨胀半径不应等于车体半径而应等于“车体半径 DWA安全余量 定位误差”。这层余量不加上全局路径就会贴着障碍边缘走DWA在高速度下根本来不及躲。我见过不少案例全局规划和局部参数分别调得都不错联调时反复碰撞最后发现就是全局膨胀半径少给了半个车身宽度。2.3 主循环与路径剪枝把绕路节点删掉把上面的函数串成完整主循环逻辑并不复杂。下面是带注释的RRT主函数骨架包含了采样、扩展、碰撞检测、终止判断四个环节def build_rrt_path(start, goal, obstacle_list, width, height, config): 改进RRT主流程 采样 - 找最近节点 - 扩展 - 碰撞检测 - 判断到终点 - 剪枝 config里至少包含bias, step_size, inflation, max_iter, goal_tolerance node_list [start] parent {start: None} for i in range(config.max_iter): if i % 500 0 and i 0: # 调试时打印进度确认树在正常生长 print(iter, i, nodes, len(node_list)) rand sample_with_bias(goal, width, height, config.bias) nearest nearest_node(node_list, rand) new steer(nearest, rand, config.step_size) if is_collision_free(nearest, new, obstacle_list, config.inflation): node_list.append(new) parent[new] nearest if math.hypot(new[0] - goal[0], new[1] - goal[1]) config.goal_tolerance: raw_path extract_path(parent, new) return prune_path(raw_path, obstacle_list, config) return Nonenearest_node的计算就是遍历node_list找欧氏距离最近的点数据量到几千节点时性能尚可节点过万后建议用KD树索引否则每轮采样都要全表扫描。extract_path沿着parent字典从终点回溯到起点得到原始路径。原始路径通常节点稀疏且存在绕路所以最后一步交给prune_path。路径剪枝的本质是贪婪拉直从起点开始尽量跳到更远的节点上只要两点之间没有障碍就跳过中间的节点。这样可以把几十个节点的路径压缩到个位数。实现在第2.1节里给了一个版本这里再补充一个更贴合实际调用的改进版它接受容差参数避免剪枝后折点太尖锐def prune_path(path, obstacle_list, config): 剪枝从起点开始把能直连的远节点直接相连。 最终保留的节点数通常只有原来的1/5到1/10。 if len(path) 2: return path pruned [path[0]] i 0 while i len(path) - 1: for j in range(len(path) - 1, i, -1): if is_collision_free(path[i], path[j], obstacle_list, config.inflation): pruned.append(path[j]) i j break else: i 1 if pruned[-1] ! path[-1]: pruned.append(path[-1]) return pruned这段代码的关键是内层循环从最远节点往前扫保证每次跳跃都尽量大。如果中间全被障碍挡住它会退化为原来的节点序列不会破坏路径。剪枝后路径是折线骨架真正执行要靠DWA把折线跟踪成平滑轨迹。我一般还会在剪枝后做一步角度松弛对相邻线段夹角过小的节点尝试向两边偏移一点再做碰撞检测如果偏移后路径更顺滑就替换。这一条不是必须的但在窄通道场景里对DWA的过冲问题有明显改善。RRT改进这块的参数较多我整理了一张速查表按室内小车、地图比例尺1像素≈5厘米来给初值参数初值作用调参方向目标偏置bias0.15采样倾向目标的概率障碍多下调空旷上调步长step_size10每次扩展长度窄通道变小大地图变大膨胀半径inflation5障碍安全距离小则缝变窄大则保守最大迭代max_iter5000规划超时上限复杂地图调大终点容差goal_tolerance5判定到达的距离按定位精度定两个常见误区。第一个是把步长当成“速度”来调想省迭代就调大结果碰撞检测失败率飙高反而更慢。第二个是全局膨胀半径只按车体半径给忘了给DWA留活动余量导致局部层一加速就压线。这两点在第4章的联调避坑里还会反复出现。3. DWA局部避障动态窗口里的速度采样与代价评价3.1 动态窗口到底是什么窗口DWA的核心假设是路径规划不需要遍历整个速度空间只需要考虑当前速度在当前加速度限制下、一个控制周期内能到达的速度集合再叠加“能在前方障碍前刹停”的约束后剩下的速度组合就是动态窗口。这个窗口每帧都在变所以叫动态窗口。口径先立住如果机器人当前速度是0.5 m/s线加速度是0.4 m/s²控制周期0.1 s那么下一个周期速度最多到0.54 m/s。window的名字就是指这个“当前可达速度集合”而不是全速度区间。下面参数表是一类室内轮式机器人的典型初值后面代码里的每个数字都从这儿来参数含义典型值最大线速度v_max小车能跑多快1.0 m/s最大角速度w_max小车能转多快20 deg/s线加速度a_v油门响应0.4 m/s²角加速度a_w转向响应45 deg/s²控制周期dt每次决策间隔0.1 s前向模拟时间sim_time预测多远的轨迹1.5 s结合当前速度计算动态窗口的代码如下。我加了可读性注释实际工程里还会再并上“能刹停”约束def compute_dynamic_window(v, w, config): 计算当前速度(v,w)下一个控制周期内可达的速度窗口。 返回(v_min, v_max, w_min, w_max) v_min max(config.v_min_, v - config.a_v * config.dt) v_max min(config.v_max, v config.a_v * config.dt) w_min max(config.w_min, w - config.a_w * config.dt) w_max min(config.w_max, w config.a_w * config.dt) # 能刹停约束v^2 / (2*a) 必须不大于可观测的最短障碍距离。 # 这里obstacle_dist是传感器探测到前方最近障碍的距离 # v_max超出安全值就往下压。 max_safe_v math.sqrt(2 * config.a_v * config.obstacle_dist) v_max min(v_max, max_safe_v) return (v_min, v_max, w_min, w_max)能刹停约束在实际工程里怎么取obstacle_dist是个容易被问到的点。常见做法是把激光雷达或深度相机点云投影到车体坐标系取前方扇形区域内的最近点距离也可以对局部代价地图做一条射线求交得到机器人到障碍的最近距离。这个约束是DWA区别于普通速度采样方法的关键不建议省略。省略后机器人在高速状态会直直冲向突然出现的障碍因为它没有把“停不下来”的情况排除在外。3.2 轨迹预测一组速度对应一条弧线动态窗口内每一组(v, w)都对应一条前向模拟的轨迹。模拟时长由sim_time决定太长会把远处的预测精度浪费掉太短则看不到前方急弯。下面这段是按差速小车运动学模型做的轨迹预测注释里写了每个变量def predict_trajectory(pose, v, w, dt, steps): 前向模拟一条轨迹返回[(x, y, theta), ...]序列。 pose是当前位姿(x, y, theta)steps sim_time / dt。 x, y, theta pose traj [] for _ in range(steps): x v * math.cos(theta) * dt y v * math.sin(theta) * dt theta w * dt traj.append((x, y, theta)) return traj这是自行车模型的最简版本差速小车用足够。阿克曼底盘在高速转弯时轮胎侧偏明显theta更新需要换成带滑移角的模型否则轨迹预测会偏乐观DWA以为能转过去实际转不过去。另外注意traj里存的是位姿不是单纯的点序列后面算朝向代价时要用到终点的theta。3.3 评价函数三个代价的权重怎么分配评价函数是DWA的决策核心。常用三项朝向代价heading让轨迹终点的朝向尽量对准目标方向障碍代价obstacle让轨迹尽量远离障碍速度代价velocity鼓励机器人别太慢。下面实现里把三项都做了归一化和安全判定def evaluate_trajectory(traj, v, w, goal, local_obstacles, config): 对一条轨迹打分返回(total, heading_cost, obstacle_cost, velocity_cost) 轨迹上任何一点进入safety_radius直接返回无穷大代价。 end_x, end_y, end_theta traj[-1] target_angle math.atan2(goal[1] - end_y, goal[0] - end_x) angle_diff abs(normalize_angle(target_angle - end_theta)) # 用指数把朝向误差压到[0,1)角度差大时代价趋近1 heading_cost 1.0 - math.exp(-angle_diff * config.weight_heading) # 障碍代价取轨迹与最近障碍距离的倒数越近越危险 min_dist float(inf) for (ox, oy) in local_obstacles: for (px, py, _) in traj: d math.hypot(px - ox, py - oy) if d min_dist: min_dist d if min_dist config.safety_radius: return float(inf), heading_cost, 0.0, 0.0 obstacle_cost 1.0 / min_dist # 速度代价用1 - v/v_max鼓励差不多接近最大速度 velocity_cost 1.0 - abs(v) / config.v_max total (config.alpha * heading_cost config.beta * obstacle_cost config.gamma * velocity_cost) return total, heading_cost, obstacle_cost, velocity_cost注意normalize_angle需要把角度差折叠到[-pi, pi]否则朝向代价在跨越±pi时会突变机器人会在目标正后方时出现诡异的掉头行为。这项细节是调参血泪经验里排在第一位的问题。障碍代价用倒数而不是线性值是为了让“接近障碍”的惩罚随距离急剧上升给安全半径一个明显的代价悬崖。三个权重alpha、beta、gamma的分工要说明白alpha管“朝目标去的欲望”beta管“躲障碍的保守程度”gamma管“往前走的速度”。调参时最忌讳三个一起动我一般固定gamma0.3先调beta到不碰障碍再调alpha到转弯干脆最后回头看gamma决定整体速度偏好。再看几组经验组合场景alphabetagamma效果障碍稀疏0.50.20.3走得快偶尔绕远障碍密集0.30.60.1保守速度起不来走廊直行0.40.30.3直道快拐弯慢把每轮选中的轨迹的heading/obstacle/velocity逐项打印出来是调参最有效的习惯。看到具体哪一项代价主导了决策比盲调权重快很多。很多开发者只知道“调参很玄学”其实玄学的根源在于代价没有归一化三个量纲不同的数字硬加在一起权重自然无从调起。DWA主循环把上面的函数串起来就是每帧决策的核心代码def dwa_plan(pose, v, w, goal, local_obstacles, config): DWA主循环遍历动态窗口内的(v,w)组合 模拟轨迹并打分选总代价最小的一组作为控制输出。 v_range, w_range compute_dynamic_window(v, w, config) v_min, v_max v_range w_min, w_max w_range best_cost float(inf) best_v, best_w 0.0, 0.0 # 把速度空间离散成网格。20x16起步性能允许再加密。 v_samples np.linspace(v_min, v_max, config.v_samples) w_samples np.linspace(w_min, w_max, config.w_samples) for candidate_v in v_samples: for candidate_w in w_samples: traj predict_trajectory(pose, candidate_v, candidate_w, config.dt, int(config.sim_time / config.dt)) cost evaluate_trajectory(traj, candidate_v, candidate_w, goal, local_obstacles, config) if cost[0] best_cost: best_cost cost[0] best_v, best_w candidate_v, candidate_w return best_v, best_w采样密度是另一处容易出问题的点。线速度分成20份、角速度16份起步是够的。如果机器人表现滑顺说明密度够如果行为忽左忽右先加大采样密度而不是怀疑权重。采样太粗会让真正合适的(v,w)组合根本不在网格里DWA在相邻网格之间跳来跳去表现出来就是轨迹抖动。4. RRT与DWA联调避坑4类翻车现场与排查手记4.1 现象一全局路径刚生成DWA却完全不认账现象RRT规划出一条看起来不错的全局路径但机器人一启动就朝着终点直冲完全无视路径甚至直接穿过全局路径明确绕开的区域。原因DWA只被喂了最终终点坐标没有被喂全局路径上的“局部目标点”。DWA的视野只有几米对它而言全局路径是个黑匣子它只认当前的目标点。正确做法是沿全局路径取一个前瞻点作为DWA的目标点如果取的就是终点DWA会从当前位置直接拉直线过去全局路径自然被绕开了。另一个常见原因是坐标系不统一——全局路径的坐标在MAP系DWA在ODOM系没有做位姿变换。解决加一个局部目标点选择函数沿全局路径按路程取前瞻点而不是取欧氏距离最近的点def pick_local_goal(global_path, robot_pose, lookahead1.2): 沿全局路径找距机器人路程为lookahead的点 作为DWA的局部目标点。 lookahead太小会蛇形太大则会在岔路口把DWA带偏。 dist 0.0 for i in range(len(global_path) - 1): seg math.hypot(global_path[i1][0] - global_path[i][0], global_path[i1][1] - global_path[i][1]) if dist seg lookahead: t (lookahead - dist) / seg x global_path[i][0] t * (global_path[i1][0] - global_path[i][0]) y global_path[i][1] t * (global_path[i1][1] - global_path[i][1]) return (x, y) dist seg return global_path[-1]lookahead初值取1.2米车越大、速度越快取值越大。验证方法看航向误差曲线航向误差平滑贴近零说明跟踪正常周期性摆动说明lookahead太小。如果没有航向误差曲线可以打印局部目标点的ID观察它是否随机器人移动而平滑切换。4.2 现象二窄通道口反复横跳就是过不去现象全局路径明明规划穿过了窄门机器人到了门口却左右摆动迟迟不进严重时直接掉头。这时全局路径看起来没问题局部层却在门口“晕头转向”。原因DWA的sim_time太短轨迹预测还没覆盖到门后的区域机器人到门口时“看不清”门后的路又或者动态窗口内能穿门的速度组合太少采样网格太粗恰好漏掉了那组能通过的速度。还有一层原因全局路径在门口有两个极近的折点局部目标点在两个折点之间来回切换导致DWA目标不断抖动。解决先把sim_time拉长到覆盖门后1~2米同时检查局部目标点切换的逻辑确保它按路程平滑变化而不是按欧氏距离跳动。如果这两个都做了还不行在门口减速并观察把lookahead从1.2逐步降到0.5如果能进门说明是前瞻距离问题如果所有取值都进不去说明门本身在全局层就被膨胀半径堵死了这时候改DWA没意义要回全局层调膨胀半径。4.3 现象三局部目标抖动触发RRT重规划风暴现象机器人每走一小段全局路径就重新规划一次路径形态不断变化轨迹歪歪扭扭像喝了酒一样。原因重规划触发条件太敏感。常见实现里只要局部目标点更新超过某个阈值就重新调用RRT而局部目标点在障碍边缘轻微抖动时RRT会返回一条形态差异很大的新路径形成“重规划风暴”。全局路径每次都换DWA的跟踪目标一直在变机器人自然走不稳。解决给重规划加死区。只有当新局部目标点距离当前全局路径的投影距离超过阈值比如0.3米时才触发重规划或者固定按时间周期检查每3秒一次而不是每帧触发。更稳妥的做法是重规划完成后对新旧路径做对比——新路径终点一致、总长不超过旧路径的1.2倍才切换使用任何一条不满足就沿用旧路径直到终点。这套逻辑不复杂但对稳定性的提升很直观。4.4 现象四权重怎么调都崩溃问题在归一化现象alpha/beta/gamma怎么改机器人表现都感觉是随机变化。调高beta它还是撞障碍调高gamma它又原地转圈看起来没有任何规律。原因三个代价没有归一化量纲不同。heading是弧度obstacle是1/米velocity是速度差三者直接加权相加等于在调量纲换算系数。换一张地图尺度的变化、速度范围的变化原来的权重就失效。另一个隐藏原因是采样密度太粗表现也是“调参无效”——真正合适的速度组合根本不在网格里权重怎么动都选不到它。解决先把三项代价全部归一到[0,1]再调权重。heading除以pivelocity除以v_maxobstacle用一个距离截断后做倒数归一化。完成归一化后alpha/beta/gamma的行为才可预测。再检查采样密度线速度20份、角速度16份起步调完一轮后可以降采样看性能损失。这两步做完调权重的“玄学感”会消失大半。5. 衔接与验证从仿真曲线到实车的实测技巧5.1 全局路径跟踪的平滑处理RRT剪枝后的路径是折线直接喂DWA会让机器人在拐角处走出锯齿状轨迹。我习惯把剪枝后的路径做等距插值再对相邻插值点做滑动平均把折线软化。软化后的点仍然沿原路径分布DWA跟踪的是平滑后的道路中心线转弯时不会因为折点突变而猛打方向。插值间距取0.2米左右太密浪费计算太疏拐角形变明显。叠加速度限幅根据路径曲率动态限制v_max曲率大时降速避免转弯过冲。这个逻辑放在DWA外部相当于给局部层加了一个“路况提示”。5.2 三组验证场景与曲线判读验证这套RRTDWA是否落地我建议在仿真里固定跑三组场景每次记录横向误差和控制频率。横向误差是机器人当前位置到全局路径的垂距直道上应小于一个车身宽弯道处放宽到1.5倍。场景设置通过标准空旷直道障碍物极少全局路径近似直线DWA保持接近v_max单障碍绕行中间一块圆形障碍机器人绕行横向误差不超过0.3米窄门穿行门宽略大于车宽减速通过无横跳一次性过门看曲线不能只看结果要看“过程形态”。横向误差曲线如果平滑说明前瞻点切换稳定如果出现等间隔的锯齿多半是局部目标点按欧氏距离取值导致的前瞻跳动。控制频率稳定在预期值附近说明DWA每帧计算量可控如果掉帧明显优先检查评价函数里对local_obstacles的双层循环这是最常见的性能瓶颈。5.3 从仿真到实车先加噪声再上车仿真里表现完美实车一开就抖最常见的两个原因传感器噪声让DWA看到“幽灵障碍”以及坐标系漂移让全局路径和局部代价地图错位。我吃过一次大亏当时在仿真里一切正常上车后DWA频繁急刹车排查了很久发现是点云里个别离群点被当成障碍物obstacle代价剧烈波动。后来我在仿真里给传感器加了高斯噪声和丢点率把探测距离截短到3米来模拟实车传感器极限问题在仿真阶段就暴露出来了。另一个习惯是全局规划放后台线程跑用路径版本号管理。DWA永远取“最近一次成功规划”的结果而不是在规划完成前阻塞等待。全局规划耗时不稳定但局部控制不能断用版本号切换让双层解耦体感会流畅很多。我调这套RRTDWA时踩过最深的坑就是全局规划放在主循环里同步跑RRT一卡顿整台车就停后来改后台线程一步到位。希望帮到你。本文还有配套的精品资源点击获取