✅ 一、项目背景详细介绍

在移动机器人自主导航领域，路径规划与避障是核心任务之一。对于机器人而言，要能够在未知或半已知环境中快速、安全地规划出可行路径，需要高效的本地规划算法。动态窗口法（Dynamic Window Approach, DWA）是一种经典且实用的局部路径规划方法，广泛应用于：

• 服务机器人（扫地机器人）

• 自动驾驶小车

• 室内移动机器人

• 工业AGV

• ROS Navigation Stack（move_base）

DWA 的特点：

1. 实时性强：基于速度空间采样评估。

2. 计算代价低：适合嵌入式运行。

3. 可避免动态障碍物。

4. 效果稳定可靠，工程实用性好。

5. ROS 默认局部规划器（base_local_planner）采用 DWA。

DWA 的核心思想是：
在当前可执行的速度范围内采样一批候选速度，通过预测轨迹并打分，选择最优速度。

---

✅ 二、项目需求详细介绍

我们要实现一个独立的 C++ DWA 模块，满足：

1. 输入：

   • 机器人当前状态（位置、速度、朝向）

   • 目标点位置

   • 障碍物（点集）

   • 机器人动力学参数（最大速度、加速度）

   • 参考 dt 时间间隔

2. 输出：

   • 最优速度（v，w）

   • 预测轨迹用于调试

3. 功能需求：

   • 计算动态窗口

   • 在速度空间采样

   • 预测轨迹 (motion model)

   • 轨迹评价（距离、速度、目标方向等）

   • 返回最优控制指令

4. 主要模块：

   • 数据结构定义

   • 动态窗口计算

   • 轨迹预测函数

   • 评价函数

   • 主控制循环

5. 工程特性：

   • 代码易移植

   • 结构清晰

   • 支持调试输出

   • 支持扩展为 ROS 插件

---

✅ 三、相关技术详细介绍

3.1 动态窗口法核心公式

速度空间 V

机器人动力学给定最大线速度与角速度：

v ∈ [0, vmax] w ∈ [-wmax, wmax]

加速度约束

根据加速度限制，下一时刻能达到的速度区间：

v ∈ [v_current - a_linear * dt, v_current + a_linear * dt] w ∈ [w_current - a_angular * dt, w_current + a_angular * dt]

两者交集即为动态窗口。

---

3.2 轨迹预测

采用单段曲线（以速度积分）：

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

---

3.3 评价函数

DWA 评价函数通常由三部分组成：

1. heading_cost：轨迹终点与目标点的角度偏差

2. clearance_cost：距离障碍物的最小距离

3. velocity_cost：速度越大越好

最终得分为：

score = α*heading + β*velocity + γ*clearance

其中 α、β、γ 为人为给定参数（经验优化）。

---

✅ 四、实现思路详细介绍

1. 数据结构定义 RobotState、ObstacleList、Trajectory

2. 创建动态窗口：

   • 基于速度限制

   • 基于加速度限制

   • 取交集

3. 在窗口中采样：

   • v 采样范围 [min_v, max_v]

   • w 采样范围 [min_w, max_w]

   • 步长可调节精度/计算量

4. 对每组速度：

   • 模拟机器人未来若干个 dt

   • 生成预测轨迹

   • 计算评价函数

5. 选择得分最高的速度对 (v, w)

6. 输出控制指令

---

✅ 五、完整实现代码

/******************************************************
 * C++ 实现动态窗口法（DWA）
 * 所有代码放在一个代码块中
 * 不同模块用注释区分
 ******************************************************/

#include <iostream>
#include <vector>
#include <cmath>
#include <limits>
using namespace std;

//======================= 数据结构定义 =======================//

struct Pose {
    double x;
    double y;
    double theta;
};

struct Velocity {
    double v;   // 线速度
    double w;   // 角速度
};

struct RobotState {
    Pose pose;
    Velocity vel;
};

struct Params {
    double max_v = 1.0;            // 最大线速度
    double max_w = 1.0;            // 最大角速度
    double max_acc_v = 0.5;        // 最大线加速度
    double max_acc_w = 0.5;        // 最大角加速度
    double dt = 0.1;               // 预测步长
    double predict_time = 3.0;     // 预测总时长
    double v_step = 0.05;
    double w_step = 0.05;

    // cost 权重
    double heading_weight = 0.8;
    double clearance_weight = 0.6;
    double velocity_weight = 0.1;

    double robot_radius = 0.3;     // 碰撞半径
};

struct Trajectory {
    vector<Pose> points;
    Velocity cmd;
    double score;
};

//======================= 工具函数 =======================//

double normalizeAngle(double ang) {
    while (ang > M_PI) ang -= 2*M_PI;
    while (ang < -M_PI) ang += 2*M_PI;
    return ang;
}

// 欧式距离
double dist(double x1, double y1, double x2, double y2) {
    return sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
}

//======================= 轨迹预测 =======================//

Trajectory simulate(const RobotState& state, const Velocity& cmd, const Params& p) {
    Trajectory traj;
    traj.cmd = cmd;

    Pose cur = state.pose;

    double t = 0;
    while (t <= p.predict_time) {
        traj.points.push_back(cur);

        // 状态更新（运动模型）
        cur.x += cmd.v * cos(cur.theta) * p.dt;
        cur.y += cmd.v * sin(cur.theta) * p.dt;
        cur.theta += cmd.w * p.dt;

        t += p.dt;
    }
    return traj;
}

//======================= 动态窗口计算 =======================//

pair<Velocity, Velocity> calcDynamicWindow(const RobotState& state, const Params& p) {
    double min_v = max(0.0, state.vel.v - p.max_acc_v * p.dt);
    double max_v = min(p.max_v, state.vel.v + p.max_acc_v * p.dt);

    double min_w = max(-p.max_w, state.vel.w - p.max_acc_w * p.dt);
    double max_w = min(p.max_w, state.vel.w + p.max_acc_w * p.dt);

    return {{min_v, min_w}, {max_v, max_w}};
}

//======================= 轨迹评价函数 =======================//

double calcHeadingCost(const Trajectory& traj, const Pose& goal) {
    Pose last = traj.points.back();
    double angle_to_goal = atan2(goal.y - last.y, goal.x - last.x);
    double diff = fabs(normalizeAngle(angle_to_goal - last.theta));
    return M_PI - diff;  // 越接近目标方向得分越高
}

double calcClearanceCost(const Trajectory& traj, const vector<Pose>& obstacles, const Params& p) {
    double min_dist = numeric_limits<double>::max();
    for (auto& point : traj.points) {
        for (auto& obs : obstacles) {
            double d = dist(point.x, point.y, obs.x, obs.y);
            if (d < min_dist) {
                min_dist = d;
            }
        }
    }
    return min_dist; // 越远越安全
}

double calcVelocityCost(const Velocity& cmd) {
    return cmd.v; // 越快越好
}

//======================= 综合评价 =======================//

double calcScore(const Trajectory& traj, const Pose& goal, const vector<Pose>& obstacles, const Params& p) {
    double h = calcHeadingCost(traj, goal);
    double c = calcClearanceCost(traj, obstacles, p);
    double v = calcVelocityCost(traj.cmd);

    return p.heading_weight*h + p.clearance_weight*c + p.velocity_weight*v;
}

//======================= 主 DWA 规划器 =======================//

Velocity dwaPlan(const RobotState& state, const Pose& goal,
                 const vector<Pose>& obstacles, const Params& p)
{
    auto window = calcDynamicWindow(state, p);
    double bestScore = -1;
    Velocity bestCmd = {0, 0};

    for (double v = window.first.v; v <= window.second.v; v += p.v_step) {
        for (double w = window.first.w; w <= window.second.w; w += p.w_step) {

            Velocity cmd{v, w};
            Trajectory traj = simulate(state, cmd, p);

            double score = calcScore(traj, goal, obstacles, p);

            if (score > bestScore) {
                bestScore = score;
                bestCmd = cmd;
            }
        }
    }
    return bestCmd;
}

//======================= 主程序示例 =======================//

int main() {
    RobotState state;
    state.pose = {0, 0, 0};
    state.vel = {0, 0};

    Pose goal = {5, 5, 0};

    vector<Pose> obstacles = {
        {2, 2, 0}, {3, 3, 0}, {4, 2, 0}
    };

    Params params;

    for (int i = 0; i < 30; i++) {
        Velocity cmd = dwaPlan(state, goal, obstacles, params);

        // 更新机器人状态（仅演示）
        state.pose.x += cmd.v*cos(state.pose.theta)*params.dt;
        state.pose.y += cmd.v*sin(state.pose.theta)*params.dt;
        state.pose.theta += cmd.w*params.dt;
        state.vel = cmd;

        cout << "Step " << i << ": v=" << cmd.v << " w=" << cmd.w
             << " pos=(" << state.pose.x << "," << state.pose.y << ")" << endl;
    }

    return 0;
}

✅ 六、代码详细解读

✅ calcDynamicWindow

根据速度与加速度限制，计算可采样的速度区间。

✅ simulate

基于运动学模型，预测未来一段时间的轨迹点。

✅ calcHeadingCost

计算终点指向目标点方向的偏差。

✅ calcClearanceCost

计算轨迹上与所有障碍物的最小距离，反映安全性。

✅ calcVelocityCost

鼓励机器人保持较高速度。

✅ calcScore

综合评分函数，将 heading、clearance、velocity 按权重加权。

✅ dvaPlan

遍历动态窗口内所有速度组合，模拟、打分，并取最优。

✅ main

模拟机器人运动回合，打印每步控制结果。

---

✅ 七、项目详细总结

本项目对 DWA 实现进行了完整的工程级讲解，包含：

• 理论背景

• 速度空间约束

• 动态窗口推导

• 可行轨迹预测

• 综合评价算法

• 完整可运行 C++ 实现

实现特点：

1. 高模块化

2. 可移植性强

3. 方便扩展到 ROS 实际机器人

4. 逻辑结构清晰

5. 参数可调节优化性能

---

✅ 八、项目常见问题及解答

❓ 1. DWA 是否可以保证全局最优？

不能。
DWA 是局部规划器，优化范围有限，可能陷入局部最优。

---

❓ 2. 障碍物太近时容易出现震荡怎么办？

可能原因：

• clearance 权重太高

• 采样步长太大

• predict_time 太短

优化方式：

• 减小 clearance 权重

• 调整 predict_time

• 适当减小步长

---

❓ 3. 机器人为何无法靠近目标？

可能原因：

• heading_weight 设置过低

• max_v 太小导致无法推进

---

❓ 4. 为什么会出现急停？

可能原因：

• 轨迹靠近障碍物评分为负

• 加速度限制导致无法快速调整速度

解决方法：

• 放宽 clearance 权重

• 调整 robot_radius

---

✅ 九、扩展方向与性能优化

✅ 1. 扩展速度采样方式

加入随机采样、双向采样，提高探索能力。

✅ 2. 轨迹预测可加入动力学加速度模型

更真实地模拟惯性系统。

✅ 3. 可加入障碍物膨胀

提升安全性。

✅ 4. 添加目标速度控制

使机器人接近目标时减速。

✅ 5. 使用 KDTree 加速最近距离搜索

减少障碍物遍历时间，提升性能。

✅ 6. 多线程优化

将不同速度采样并行计算评分。