动态、静态障碍物局部路径规划（matlab） 自动驾驶 阿克曼转向系统 考虑车辆的运动学、几何学约束 DWA算法一般用于局部路径规划，该算法在速度空间内采样线速度和角速度,并根据车辆的运动学模型预测其下一时间间隔的轨迹。 对待评价轨迹进行评分,从而获得更加安全、平滑的最优局部路径。 本代码可实时展示DWA算法规划过程中车辆备选轨迹的曲线、运动轨迹等，具有较好的可学性，移植性。 代码清楚简洁，方便更改使用！可在此基础上进行算法的优化。
最近在倒腾阿克曼转向小车的路径规划，发现DWA算法这玩意儿在动态环境下确实有点东西。今天咱们就结合Matlab撸个带运动学约束的DWA实现，既能实时看到备选轨迹，又能自己动手魔改算法参数。
先说说阿克曼这货的特性：转弯半径和速度强相关，前轮转向角存在物理限制。假设咱们的小车最大转向角30度，轴距1.2米，那最小转弯半径大概是2.05米。这些参数得在代码里卡死：
car.max_steer = deg2rad(30); % 最大转向角 car.wheelbase = 1.2; % 轴距 car.min_turn_radius = car.wheelbase / tan(car.max_steer); % 动态约束DWA的核心是速度采样与轨迹预测。这里有个坑——阿克曼的运动学模型不能用简单的差分模型。咱们得用自行车模型来近似：
function state = motion_model(state, v, delta, dt) % 自行车模型运动预测 delta = constrain(delta, -car.max_steer, car.max_steer); beta = atan2(tan(delta)/2, 1); % 简化阿克曼转角计算 state(3) = state(3) + v * tan(delta) / car.wheelbase * dt; % 航向角 state(1) = state(1) + v * cos(state(3) + beta) * dt; % X坐标 state(2) = state(2) + v * sin(state(3) + beta) * dt; % Y坐标 end速度采样别傻乎乎地全空间搜索，要根据当前速度和加速度约束生成候选集。比如线速度每次变化不超过0.2m/s，角速度基于最大转向角计算：
v_samples = linspace(... max(0, current_v - 0.2),... min(4.0, current_v + 0.2), 5); % 线速度采样 w_samples = current_v * tan(linspace(-car.max_steer, car.max_steer, 7))... / car.wheelbase; % 角速度与转向角关联轨迹评分是算法的灵魂。这里展示个三要素打分函数——安全距离、速度偏好、轨迹平滑度：
function score = evaluate_trajectory(traj, obstacles) % 障碍物安全距离（30%权重） min_dist = min(vecnorm(traj(:,1:2) - obstacles, 2, 2)); safety = 1 / (1 + exp(-(min_dist - 1.0))); % 安全距离软阈值 % 速度偏好（40%权重） speed_bias = traj(end,3) / 4.0; % 当前最大速度4m/s % 轨迹曲率（30%权重） curvature = mean(abs(diff(traj(:,4)))); % 航向角变化率 smoothness = exp(-curvature * 5); score = 0.3*safety + 0.4*speed_bias + 0.3*smoothness; end实时可视化这块是教学重点。咱们用Matlab的动画功能显示备选轨迹和最优路径：
function update_plot(ax, trajs, best_traj) % 清空并重绘 cla(ax); hold(ax, 'on'); % 绘制所有备选轨迹 cellfun(@(t) plot(ax, t(:,1), t(:,2), 'Color',[0.8 0.8 1]), trajs); % 高亮最优路径 plot(ax, best_traj(:,1), best_traj(:,2), 'b', 'LineWidth',2); % 绘制车身轮廓 draw_car(ax, best_traj(end,1:3)); % 自定义函数 axis equal end实测发现当障碍物突然切入时（比如从右侧窜出来的自行车），算法能在0.5秒内生成避让路径。不过现版本对急转弯工况的轨迹平滑处理还不够，可以在评分函数里加入航向与目标点方向的一致性项来优化。
完整代码把上述模块串起来不到200行，修改障碍物位置、车辆参数都很方便。想要提升实时性的话，可以把速度采样从均匀采样改为动态分辨率采样——在紧急制动时加大低速区间的采样密度。
