企业官网建设流程全解析

1. 先说结论：二维DWA直接搬上无人机，坠机概率是100%

做无人机自主避障的人，大概率都翻过这样一篇文章或者代码仓库：地面移动机器人用DWA（动态窗口法）在二维平面里跑得风生水起，转弯、绕障、找目标一气呵成。然后很多人第一反应是——这玩意儿不是正好可以给无人机用吗？把激光雷达或者深度相机的数据喂进去，DWA输出角速度和线速度，无人机跟着跑不就行了？

我这么说吧，如果你真的直接把二维DWA的输出接到无人机底层控制器上，悬停状态下还可能凑合，只要一前飞，大概率是要出事的。我踩过这个坑，当时用一款开源四旋翼仿真环境测试，DWA给的水平线速度指令直接让无人机姿态大幅度倾斜，一个绕障动作还没做完，偏航角就和期望方向偏了快180度。

问题不出在DWA本身，而出在无人机的运动学模型和二维DWA假设的运动模型根本对不上。

二维DWA默认机器人是一个"全驱动或差速驱动的平面刚体"，小车的横滚、俯仰是天然稳定的，它只需要考虑平面内的前后移动和旋转。但无人机是一个典型的三维欠驱动系统，横滚和俯仰本身就是它产生水平加速度的唯二手段。你在平面里给一个前向速度指令，无人机为了执行这个指令必须先压坡度，这个坡度带来的高度损失又和DWA预测的轨迹完全不匹配。也就是说，DWA在二维里预测的路径，在无人机真实执行时根本不是那个样子。

所以这篇文章要聊的核心，就是怎么把DWA改造成适合无人机动态环境避障的版本。我会结合MATLAB代码实现来讲，不整虚的，全部是我实际跑通过的思路和参数，附带我在仿真和实飞中遇到的一系列问题。

2. DWA算法拆开看：采样、预测、打分三步棋

要改造DWA，先得把它吃透。DWA的精髓其实就三个字：采样、预测、打分。

2.1 采样：在速度空间里画一个"安全窗口"

DWA的全称是Dynamic Window Approach，翻译过来就是动态窗口法。所谓"动态窗口"，是指在每一个控制周期里，机器人能实际达到的速度集合不是无限的，而是受到三个条件的约束：

• 自身速度极限：电机能输出多大的最大线速度和角速度。
• 电机加减速能力：在下一个控制周期内，当前速度加减速后能达到的速度范围。比如当前线速度是1m/s，最大加速度是0.5m/s²，采样周期是0.1s，那下一个周期的线速度范围就是0.95到1.05m/s。这个约束非常关键，它保证了规划出来的轨迹是"当前状态下机器人真的能执行出来的"，而不是纸上谈兵。
• 制动距离约束：在碰到障碍物之前能停下来。这里需要计算以当前速度行驶，在最大减速度下需要多远才能刹停，如果这个距离超过到最近障碍物的距离，那这个速度就得剔除。

这三层约束取交集，得到的就是一个"动态窗口"。窗口实际上代表的是机器人此时此地的可控空间，把不可控的危险速度排除掉，剩下的是速度和安全性都有保障的选项集。

2.2 预测：每个速度对对应一条"未来轨迹"

有了候选速度集合之后，第二步是预测轨迹。在简单场景里，可以将每一组线速度和角速度代入运动学模型，模拟一小段时间内的运动。比如模拟3秒钟，每0.1秒算一个点，把30个点串起来，就是这条速度指令对应的未来轨迹。

这里的运动学模型有很多种，最简单的是：

x(t+dt) = x(t) + v * cos(theta(t)) * dt y(t+dt) = y(t) + v * sin(theta(t)) * dt theta(t+dt) = theta(t) + w * dt

这是地面移动机器人的模型。对无人机来说，模型要复杂非常多，后面专门讲。

2.3 打分：在轨迹里挑"最优"而不是"最快"

每一条轨迹都算出来之后，就需要一个评估函数来判断哪条轨迹最合适。经典DWA的评估函数由三部分组成：

• 方位角代价（目标方向偏差）：这条轨迹的终点方向与目标方向的夹角度量，夹角越小分数越高。
• 距离代价（障碍物距离）：轨迹经过的路径与最近障碍物的距离度量，距离越大分数越高。
• 速度代价（前进速度影响）：线速度越大的轨迹分数越高，避免机器人因为过度保守而停滞不前。

最终分数是这三项的加权和。这里特别要注意的是，权重极其敏感。你给距离代价权重加得过大，无人机会显得畏畏缩缩，一个障碍物都要绕上半天；速度权重过大，又容易无视安全距离，擦着障碍物飞过去。

3. 无人机三维空间改造：从速度窗口到运动学约束

3.1 无人机到底是个什么运动学模型

要改造DWA，必须先回答一个问题：无人机的状态空间是什么，控制输入是什么。

通常我们在做无人机路径规划时，习惯用位置、速度、姿态角来描述状态。但对DWA这种基于速度采样的方法来说，直接在位置空间采样是不现实的——位置空间太大，采样量爆炸。合理的做法是在速度空间采样，然后通过无人机运动学模型推算轨迹。

常用的无人机简化运动学模型有两种：

第一种是质点模型，把无人机当成一个三维空间中的质点，控制输入是三轴速度vx, vy, vz。这个模型在室内GPS拒止环境里配合光流或UWB定位比较常见，模型简单，DWA改造也容易，只需要把二维的x,y扩展成x,y,z即可。

第二种是包含姿态的刚体模型，状态包括位置、速度、姿态角，控制输入是期望加速度或者期望姿态角。这种情况下DWA采样的速度会经过一个内环姿态控制器转换成油门和姿态指令，模型复杂一些，但更接近真实无人机。

我建议你在MATLAB仿真阶段直接用第二种模型，因为DWA规划出来的轨迹要经过内环控制器的跟踪，如果你直接拿质点模型来设计，后面接真机时会有很大的模型失配。

3.2 二维DWA和三维DWA的本质区别

二维DWA采样的是一维线速度v和一维角速度w，轨迹方程就是平面上的圆弧运动。三维DWA有两条路线：

**路线一：降维法。**把无人机控制分成水平面和高度面，水平面用DWA做2D避障，高度方向用独立的控制律（比如定高或者高度阈值避让）。这种方案在低空巡检、农业植保场景中很实用，因为无人机大部分时间是近地面水平飞行，高度方向的变化是缓慢的，并且可以单独控制。我的第一版代码就是用的这个方案，逻辑简单，出问题也好排查。

**路线二：全三维法。**采样的速度向量直接从(x方向线速度, y方向线速度, z方向线速度, 偏航角速度)四维空间里取，然后按四维运动学模型预测轨迹。全三维法的窗口计算要考虑三维的加减速能力和三维空间的障碍物距离，计算量明显增加，但能处理复杂三维环境，比如楼层间穿梭、陡坡爬升避障。

3.3 无人机DWA的约束源不止"运动学"

这可能是大家最容易忽略的点。无人机不仅有运动学约束，还有动力学约束。

举个例子，你采样了一个很大的水平线速度vx，在二维DWA里只要机器人能加速到这个速度就没问题。但无人机要实现这个vx，必须先产生一个横滚角，横滚角的变化率又受角速度限制，而横滚角本身还受限于结构最大倾角（一般消费级无人机最大横滚角在30到35度左右）。所以，你实际上是在一个多环约束下工作：速度窗口 → 加速度窗口 → 姿态可用范围窗口。

因此，在MATLAB代码里设计采样窗口时，不能只约束加速度，还要加一个**"姿态可行性前验"**。简单说就是：采样出来的水平线速度若超过当前姿态角能产生的最大水平加速度所能达到的速度，直接剔除。这一步做好之后，你的轨迹优化结果才不会出现那种"画得出来但飞不出来"的情况。

4. MATLAB代码骨架：状态表示、采样空间与碰撞检测

4.1 环境与状态设计

我先说明一下我这套代码的仿真场景设定：无人机机载传感器是我的自定义模型，有一个深度传感器或者激光雷达，能获取半径5米内的障碍物点云数据。环境是一个30m×30m×10m的三维空间，分布着若干圆柱和方形障碍物。目标点是一个固定坐标。

状态结构体我用一个struct来管理，里面包括：

state.pos = [x, y, z]; % 三维位置 state.vel = [vx, vy, vz]; % 三维速度 state.yaw = psi; % 偏航角 state.pitch = theta; % 俯仰角 state.roll = phi; % 横滚角

控制指令我设计成：[期望vx, 期望vy, 期望vz, 期望偏航角速度]。这四个量经过一个简化的内环控制器后，生成下一时刻的状态增量。我的做法是先用低通滤波模拟内环延迟，再把生成的状态增量累加到state上。

4.2 核心函数：采样窗口计算

这个函数是DWA的心脏。我直接贴代码骨架，大家理解思路就好：

function speeds = calc_dynamic_window(state, params) % 根据当前速度与加速度限制，生成候选速度集合 % 无人机在三维空间中的速度窗口需要同时约束线性加速度和偏航角速度 % 第一层：物理极限 Vs = [ params.max_vx, -params.max_vx; params.max_vy, -params.max_vy; params.max_vz, -params.max_vz; params.max_yaw_rate, -params.max_yaw_rate; ]; % 第二层：动态窗口（考虑加减速能力） Vd = [ state.vel(1) + params.ax_max * params.dt, state.vel(1) - params.ax_max * params.dt; state.vel(2) + params.ay_max * params.dt, state.vel(2) - params.ay_max * params.dt; state.vel(3) + params.az_max * params.dt, state.vel(3) - params.az_max * params.dt; state.yaw_rate + params.alpha_max * params.dt, state.yaw_rate - params.alpha_max * params.dt; ]; % 第三层：姿态可行性窗口（无人机特有） % 由最大横滚角和俯仰角反推水平加速度上限 max_h_acc = params.g * tan(params.max_roll); Vp = [ min(state.vel(1) + max_h_acc * params.dt, Vs(1, 1)), ... max(state.vel(1) - max_h_acc * params.dt, Vs(1, 2)); ]; % 取交集 V = [ max(Vs(1, 2), max(Vd(1, 2), Vp(1, 2))), min(Vs(1, 1), min(Vd(1, 1), Vp(1, 1))); max(Vs(2, 2), Vd(2, 2)), min(Vs(2, 1), Vd(2, 1)); max(Vs(3, 2), Vd(3, 2)), min(Vs(3, 1), Vd(3, 1)); max(Vs(4, 2), Vd(4, 2)), min(Vs(4, 1), Vd(4, 1)); ]; % 然后在这个窗口内按分辨率采样 speeds = sample_speeds(V, params.sample_resolution); end

4.3 轨迹预测：不能再用圆弧近似了

二维DWA的轨迹可以用圆弧近似，很优雅。但三维DWA如果用圆弧近似，那就忽略了无人机的爬升、下压、横滚等维度。所以我的代码里直接用运动学模型一步步积分：

function traj = predict_trajectory(state, v_cmd, yaw_rate_cmd, params) % 输入：当前状态、线速度指令、偏航角速度指令 % 输出：一条3秒预测轨迹，逐点存储 traj = zeros(params.predict_steps, 3); temp_state = state; for i = 1:params.predict_steps % 模拟内环延迟与姿态响应 actual_acc = lsim(params.inner_loop_sys, [v_cmd(1), v_cmd(2), v_cmd(3)], 1); vx_new = temp_state.vel(1) + actual_acc(1) * params.dt; vy_new = temp_state.vel(2) + actual_acc(2) * params.dt; vz_new = temp_state.vel(3) + actual_acc(3) * params.dt; % 偏航角更新 yaw_new = temp_state.yaw + yaw_rate_cmd * params.dt; % 位置更新 temp_state.pos(1) = temp_state.pos(1) + vx_new * params.dt; temp_state.pos(2) = temp_state.pos(2) + vy_new * params.dt; temp_state.pos(3) = temp_state.pos(3) + vz_new * params.dt; temp_state.vel = [vx_new, vy_new, vz_new]; temp_state.yaw = yaw_new; traj(i, :) = temp_state.pos; end end

这里我用了lsim来模拟内环控制器对速度指令的跟踪效果，实际项目中可以换成你自己的飞控内环模型。这一步是二维DWA没有的，却是无人机DWA必不可少的。

4.4 碰撞检测：不只是算距离

碰撞检测的思路是：把预测轨迹上的每个点与障碍物点云做距离检验。但这里有个细节，无人机有物理尺寸，不能当质点处理。所以距离阈值必须加上无人机半径。比如，我的无人机轴距是0.35m，我设置的min_clearance是0.8m，包含了旋翼半径和安全余量。

function collision_flag = check_collision(traj, obstacle_map, min_clearance) collision_flag = false; for i = 1:size(traj, 1) dist = compute_min_distance(traj(i, :), obstacle_map); if dist < min_clearance collision_flag = true; return; end end end

compute_min_distance是我自己写的一个优化过的点云最近邻搜索函数，用KDTree加速，在障碍物点云超过数千个点时尤其重要。别小看这个，纯暴力循环在MATLAB里跑起来奇慢无比，几十个候选轨迹一算，一帧要卡几百毫秒，根本达不到实时。

5. 动态障碍物处理：你不能只盯着当前帧的位置

5.1 动态环境对DWA意味着什么

动态环境下，障碍物是运动的，你基于当前时刻障碍物位置计算出来的安全轨迹，几秒后可能就撞上了。很多做DWA避障的人，第一个版本都是拿当前帧的障碍物位置做碰撞检测，结果就是无人机在静态环境下表现很好，一旦有移动的车辆、行人进入，就开始各种急刹车或者绕飞失稳。

我处理动态障碍物的思路，核心是把时间轴加入碰撞检测。

具体来说，预测轨迹不再是只看空间位置，还要看时间。也就是说，预测轨迹的第i个点不仅有一个坐标，还有一个时间戳。同时，对于每个动态障碍物，我也要预测它未来一段时间的轨迹，然后拿无人机轨迹的每个点与动态障碍物轨迹的对应时间点做碰撞检测。

5.2 动态障碍物轨迹预测：等速模型就够了

你可能会说，动态障碍物运动轨迹复杂，预测不准怎么办。我的经验是，在无人机避障场景中，等速直线运动模型在绝大多数场景下已经足够。行人虽然会有加减速和转向，但一个控制周期只有几百毫秒，预测时域也就是3到5秒，在这个尺度内，等速模型的误差在可接受范围内。如果你要处理高速车辆，可以加一个简单的加速度估计，用卡尔曼滤波去跟踪障碍物的加速度。

代码里我维护了一个动态障碍物列表，每个障碍物包含位置、速度、半径和类型：

dynamic_obstacles(i).pos = [x, y, z]; dynamic_obstacles(i).vel = [vx, vy, vz]; dynamic_obstacles(i).radius = 0.5;

在碰撞检测时，将动态障碍物的位置按时间步长外推，再和无人机轨迹比较：

function collision_flag = check_dynamic_collision(traj, dynamic_obstacles, min_clearance, params) collision_flag = false; for i = 1:size(traj, 1) t = i * params.dt; for j = 1:length(dynamic_obstacles) obs_pos = dynamic_obstacles(j).pos + dynamic_obstacles(j).vel * t; dist = norm(traj(i, :) - obs_pos); if dist < (min_clearance + dynamic_obstacles(j).radius) collision_flag = true; return; end end end end

5.3 动态窗口的"时间尺度"陷阱

这里有一个非常重要的理解：动态窗口法天生就是考虑时间的，它的"动态"一词本身说的就是速度窗口随时间变化。所以处理动态障碍物时，不要额外增加一个时间维度去采样，那样计算量会爆炸。正确做法是：仍然只采样速度空间，但在轨迹评估时加入时间预测。

我在实现时，把"动态障碍物接近速度"作为评价函数的一个惩罚项。也就是说，即使当前轨迹在空间上不碰撞，但如果预测出无人机与某个动态障碍物以很快的相对速度靠近，那这条轨迹的分数也要适当降低。这个设计极大地改善了无人机在行人穿行场景中的表现。

6. 评价函数权重调优：方向、安全、速度的三角平衡

6.1 经典权重组合与无人机的差异

二维DWA经典评价函数是三项权衡。在无人机场景中，我需要引入第四个维度：高度一致性。特别是当目标点和起点在同一高度层时，无人机不应该频繁地上下起伏。所以我的评价函数设计为：

score = alpha * heading_score + beta * clearance_score + gamma * velocity_score + delta * altitude_score

这四项的含义分别是：

6.2 权重设置的实用策略：先粗调后细调

第一次调权重，我踩了不少坑。alpha权重设太大，无人机遇到障碍物会绕着大圈子非要正对目标方向；beta设太大，无人机在空旷区域也离障碍物恨不得两米远，绕路绕得离谱；gamma设太大，无人机又容易贴着障碍物飞过。

我的经验是先粗调：

• 先把alpha、beta、gamma按1:1:1初始设置跑一轮，看无人机能不能到目标。
• 再看避障过程中最短距离是多少，如果低于阈值，加大beta，每次加0.2直到最低距离满足安全要求。
• 如果绕路太明显，说明alpha权重相对过大，适当降低alpha或者提高gamma。

经过这样几轮调整，通常可以得到一组合理的权重。一个可行的初始权重组合是：alpha=0.5，beta=0.3，gamma=0.4，delta=0.2。注意这些权重最后要做归一化处理，不然不同量纲的值混在一起，权重根本没有可比性。

6.3 退化场景死循环的坑

还有一个常见问题：当无人机陷入一个U形障碍物区域时，DWA很容易出现死循环——来回徘徊，既找不到出路，又不停地变换候选轨迹。

这个问题我一开始就是靠增大采样时间来试图解决，结果发现治标不治本。后来我在评价函数中加入了一个**"历史轨迹一致性"惩罚项**：如果当前采样的轨迹和上几个周期的轨迹非常相似但发生了大角度偏转，就降低分数。这相当于给DWA增加了一个短暂记忆，让它不那么容易被局部的、短暂的路径差异牵着鼻子走。

7. 仿真到实飞的鸿沟：步长、噪声、延迟与参数自适应

7.1 仿真步长：你真的以为0.1s够用？

在MATLAB仿真中，我第一版设定的控制周期是0.1s，轨迹预测步长也是0.1s。仿真跑得很顺，但实飞时无人机晃动明显，悬停偏移和速度超调都很严重。后来一查原因，才意识到0.1s的控制周期对于无人机这种快速动态系统来说太粗糙了。

无人机内环姿态控制的典型响应时间是0.02到0.05秒，也就是说，当你给出一个速度指令，内环需要一小段时间来建立姿态、产生加速度。如果你的DWA控制周期是0.1s，那意味着在两次规划之间，无人机已经完成了好几个姿态响应周期，整个运动过程呈现明显的阶跃式变化。

所以我在改进版代码里把控制周期缩短到了0.05s，轨迹预测步长也改为0.05s。效果立竿见影，飞行轨迹平滑了很多。不过MATLAB仿真时，每控制周期需要重新采样、预测、评估，计算负担增加了一倍左右，好在现代电脑跑起来还是游刃有余的。

7.2 传感器噪声的影响比想象中更大

仿真相对于实飞，最大的区别是噪声。MATLAB仿真里距离测量是精确的，一旦加上高斯噪声和偶尔的丢包，单纯靠DWA就会开始出现不适感。

我给障碍物点云加了标准差0.05m的高斯噪声，结果发现：有时无人机明明在安全通道内，因为噪声的影响，连续几个控制周期评估出来的"最佳轨迹"开始抖来抖去，无人机就像喝醉了一样。这个问题的本质是，DWA是纯反应式方法，对输入噪声非常敏感。

解决方案是加一个轨迹平滑器：把当前选中的速度指令和一个历史速度指令做加权平均，权重动态调整。

v_final = k * v_dwa + (1-k) * v_prev;

k的取值根据当前障碍物紧逼程度动态变化：安全时k取0.6左右，紧急避障时k取0.9。这样既保证了快速响应，又避免了速度指令的剧烈抖动。

7.3 最终效果：几个必须知道的实际测试指标

我把改进后的DWA算法在MATLAB的仿真环境中做了多组测试，也给几个做无人机毕设的朋友用，他们的反馈有好有坏。好的地方是这个代码框架确实能跑通，避障动作能用；坏的反馈集中在：怎么在复杂动态环境里还是不够从容。

我把最终结果整理了一下，供大家参考：

从测试结果能够看出，DWA作为一种局部避障算法，它的上限在于视野内信息。障碍物一旦多起来，没有全局规划帮助的DWA会显得很局促。所以这套代码的最佳使用方式，是用全局规划器（比如RRT或者A的3D版本）先规划一条参考路径，DWA负责跟踪参考路径并做局部动态避障。

最后分享一个小技巧：在MATLAB里调试权重时，画一个实时的评分热力图会极大加速调参过程。把所有候选轨迹画出来，按分数用颜色标注，你一眼就能看出是哪个评价项在拖后腿。这个可视化代码我封装在了MATLAB的一个小脚本里，不需要额外装工具箱，自己用plot3就能实现。

这套代码我放到自己的项目里大概又迭代了三个版本，目前用在一台推力相对富裕的四旋翼验证机上，动态环境下悬停避障和定高绕飞都挺稳定。如果你也在做无人机DWA相关的课题，建议先跑通静态环境，再加入动态障碍物预测，最后再处理噪声和延迟。每一步都验证到位了，再往真机上搬。仿真和实飞之间的差距永远存在，但把仿真阶段能想到的问题都提前想到，你后面接实飞时能少掉很多头发。