无人机路径规划的 动态窗口法 (DWA)

Drone DWA

本项目实现了无人机路径规划的 动态窗口法 (DWA)。以下是一些关键资源：

• GitHub 仓库 - 完整的源代码和文档。
• 源代码 - 在仓库中浏览源代码。
• README 文件 - 文档和设置说明。

理论

动态窗口法（DWA）在三维路径规划中的改进与实现

---

引言

动态窗口法（Dynamic Window Approach, DWA）是一种经典的机器人运动规划算法，广泛应用于实时避障与目标导航问题。其核心思想是在动态约束下，对速度空间内的可能值进行采样，通过轨迹预测和评价选择最优运动指令。然而，传统DWA算法主要面向二维平面场景，对于三维复杂环境（如无人机路径规划）存在一定的局限性。为此，本文基于DWA的核心思想，结合速度、加速度限制以及障碍物限制，设计了一种改进的三维动态窗口法，并通过优化轨迹评价函数实现高效、安全的路径规划。

---

DWA算法的核心步骤

动态窗口法的基本步骤包括动态窗口定义、轨迹预测、轨迹评价和最优轨迹选择。这些步骤为无人机的三维路径规划提供了系统化的解决方案。

---

1. 定义动态窗口

动态窗口是速度、加速度和障碍物限制下的速度取值范围。具体定义如下：

1. 速度限制
   无人机的速度范围由物理性能决定：
   $$v_x,v_y,v_z\in [v_{\text{min}},v_{\text{max}}]$$

2. 加速度限制
   动态窗口进一步受加速度约束，定义为：
   $$v_x,v_y,v_z\in [v_{\text{current}}-a_{\text{max}}\cdot \Delta T,v_{\text{current}}+a_{\text{max}}\cdot \Delta T]$$

3. 障碍物限制
   为保证安全性，动态窗口中的速度组合需满足障碍物安全距离的限制：
   $$d_{\text{min}}(v_x,v_y,v_z)\ge d_{\text{safe}}$$

---

2. 轨迹预测

根据动态窗口内的速度组合，采用无人机的运动学模型预测其未来轨迹。轨迹的计算公式如下：
$$\begin{array}{rl}{x}_{k+1}& =x_k+v_k\cdot \Delta T,\\ y_{k+1}& =y_k+v_k\cdot \Delta T,\\ z_{k+1}& =z_k+v_z\cdot \Delta T.\end{array}$$

---

3. 轨迹评价

轨迹评价是DWA算法的核心环节，衡量每条轨迹的优劣。本文改进后的评价函数综合考虑了目标距离、障碍物距离和时间效率，具体定义如下：
$$G(\text{trajectory})=\alpha \cdot \text{goal\_score}+\beta \cdot \text{obstacle\_score}+\gamma \cdot \text{speed\_score}$$
其中，$\alpha ,\beta ,\gamma$ 为评价权重，分别反映目标距离、障碍物安全性和速度偏好的重要性。

以下详细介绍三个评分的计算方法。

---

目标得分（Goal Score）

目标得分衡量轨迹终点与目标点的距离，同时引入理论最优路径以提高评价的物理意义。

1. 理论最优路径的最远点
   根据无人机的最大速度和时间步长，计算无人机在目标方向上理论上能够达到的最远点：
   $$\text{reachable\_point}=\text{start\_point}+{\vec{d}}_{\text{goal}}\cdot (\text{robot.v\_max}\cdot \text{robot.dt})$$
   其中：

   • $\text{start\_point}$ 为轨迹的起点；
   • ${\vec{d}}_{\text{goal}}$ 是起点到目标点的单位方向向量。

2. 轨迹终点与目标点的距离
   计算轨迹终点与目标点的欧几里得距离：
   $$\text{goal\_distance}=\Vert \text{trajectory}[-1]-\text{goal}\Vert$$

3. 最小目标距离
   理论最优路径的最小目标距离为最远点与目标点的距离：
   $$\text{min\_goal\_distance}=\Vert \text{reachable\_point}-\text{goal}\Vert$$

4. 归一化目标得分
   根据实际目标距离与理论最优距离计算得分：
   $$\text{goal\_score}=\frac{\text{min\_goal\_distance}}{\text{goal\_distance}+ϵ}$$

---

障碍物得分（Obstacle Score）

障碍物得分反映轨迹点与障碍物之间的距离，定义为：

1. 遍历轨迹点和障碍物
   对轨迹中每个点，计算其到所有障碍物的最小距离：
   $$d_t^i=\{\begin{array}{ll}\Vert {\mathbf{p}}_t-{\mathbf{o}}_i\Vert -r,& \text{球形障碍物}\\ \max \left(0,\Vert {\mathbf{p}}_t^{xy}-{\mathbf{o}}_i^{xy}\Vert -r\right),& \text{圆柱形障碍物（满足高度条件）}\\ \infty ,& \text{未知类型或超出高度范围}\end{array}$$

2. 根据距离定义得分
   根据最小距离与安全距离的关系，障碍物得分定义为：
   $$\text{obstacle\_score}=\{\begin{array}{ll}\text{min\_obstacle\_distance},& \text{若}\text{min\_obstacle\_distance}<0\\ \frac{\text{min\_obstacle\_distance}}{\text{safe\_distance}},& \text{若}0\le \text{min\_obstacle\_distance}<\text{safe\_distance}\\ 1.0,& \text{若}\text{min\_obstacle\_distance}\ge \text{safe\_distance}\end{array}$$

---

速度得分（Speed Score）

速度得分衡量轨迹的时间效率，定义如下：

1. 计算平均速度
   平均速度为轨迹总距离与总时间的比值：
   $$\text{average\_speed}=\frac{\text{total\_distance}}{\text{total\_time}}$$

2. 归一化速度得分
   将平均速度裁剪到无人机的速度范围 $[\text{v\_min},\text{v\_max}]$，并归一化：
   $$\text{speed\_score}=\frac{\text{average\_speed}-\text{v\_min}}{\text{v\_max}-\text{v\_min}}$$

---

4. 选择最优轨迹

通过计算所有轨迹的综合得分，选择得分最高的轨迹作为下一步的运动指令。

---

改进优势

1. 目标导向性增强
   引入理论最优路径，使轨迹评价更符合物理实际，激励无人机选择最短时间路径。

2. 障碍物规避优化
   精确考虑障碍物的三维结构和安全距离要求，有效提高避障能力。

3. 时间效率提升
   通过速度评分综合考虑轨迹的时间效率，确保无人机的运动规划高效可靠。

---

总结

本文提出了一种改进的动态窗口法算法，适用于三维复杂环境中的无人机实时路径规划。该算法通过目标得分、障碍物得分和速度得分的综合评价，显著提升了路径规划的可靠性和安全性。未来，可结合机器学习技术进一步优化轨迹评价函数，提高算法的鲁棒性和自适应能力，为无人机和机器人路径规划提供更广泛的应用支持。

一般障碍物的距离公式推导

在三维路径规划中，无人机可能面临多种类型的障碍物，如球形、圆柱形、或其他复杂形状。为了统一对这些障碍物的距离计算，这里推导一种通用形式的障碍物距离公式，将特定类型的障碍物转化为统一的数学表达。

---

1. 定义与假设

假设：

• 无人机当前轨迹点 ${\mathbf{p}}_t=(x_t,y_t,z_t)$。
• 障碍物的中心或基点为 ${\mathbf{o}}_i=(x_i,y_i,z_i)$。
• 障碍物的形状由其类型（如球体、圆柱体等）和参数（如半径 $r$ 和高度 $h$）描述。

目标：

• 计算任意轨迹点 ${\mathbf{p}}_t$ 到障碍物的最小安全距离 $d_t^i$。

---

2. 一般公式定义

设障碍物的表面满足隐函数 $f({\mathbf{p}}_t,{\mathbf{o}}_i)=0$，则轨迹点 ${\mathbf{p}}_t$ 到障碍物的距离 $d_t^i$ 定义为：
$$d_t^i=\underset{\mathbf{q}\in S}{\min }\Vert {\mathbf{p}}_t-\mathbf{q}\Vert$$
其中：

• $S$ 是障碍物的表面，由 $f(\mathbf{q},{\mathbf{o}}_i)=0$ 描述；
• $\mathbf{q}$ 是障碍物表面上的点。

该公式的几何意义是轨迹点 ${\mathbf{p}}_t$ 到障碍物表面 $S$ 的最小距离。

---

3. 特定类型障碍物的距离公式

(1) 球形障碍物

球形障碍物的中心为 ${\mathbf{o}}_i$，半径为 $r$。其表面方程为：
$$f(\mathbf{q},{\mathbf{o}}_i)=\Vert \mathbf{q}-{\mathbf{o}}_i\Vert -r=0$$
因此，轨迹点 ${\mathbf{p}}_t$ 到球形障碍物的最小距离为：
$$d_t^i=\Vert {\mathbf{p}}_t-{\mathbf{o}}_i\Vert -r$$

---

(2) 圆柱形障碍物

圆柱形障碍物以 ${\mathbf{o}}_i=(x_i,y_i,z_i)$ 为底面中心，半径为 $r$，高度为 $h$。其表面包括以下部分：

1. 侧面：满足水平距离为 $r$：
   $$f_{\text{side}}(\mathbf{q},{\mathbf{o}}_i)=\Vert {\mathbf{q}}^{xy}-{\mathbf{o}}_i^{xy}\Vert -r=0$$
   其中 ${\mathbf{q}}^{xy}=(x_q,y_q)$ 表示点在水平面的投影。

2. 底面和顶面：满足高度范围约束 $z_i\le z_q\le z_i+h$。

轨迹点 ${\mathbf{p}}_t$ 到圆柱形障碍物的最小距离分以下几种情况：

• 若点在圆柱高度范围内，则距离为：
  $$d_t^i=\Vert {\mathbf{p}}_t^{xy}-{\mathbf{o}}_i^{xy}\Vert -r$$
• 若点高于或低于圆柱范围，则需计算到顶面或底面的最短距离：
  $$d_t^i=\sqrt{(\Vert {\mathbf{p}}_t^{xy}-{\mathbf{o}}_i^{xy}\Vert -r{)}^2+\Delta z^2}$$
  其中 $\Delta z=\min (|z_t-z_i|,|z_t-(z_i+h)|)$。

---

(3) 其他复杂障碍物

对于复杂障碍物，其表面通常由隐式函数 $f(\mathbf{q},{\mathbf{o}}_i)=0$ 描述。轨迹点到障碍物的距离可以通过最小化求解问题计算：
$$d_t^i=\underset{\mathbf{q}\in S}{\min }\Vert {\mathbf{p}}_t-\mathbf{q}\Vert$$
这里可以采用数值优化方法（如梯度下降法或基于凸优化的方法）来求解。

---

4. 通用距离公式

将以上特例整合，通用障碍物距离公式可表达为：
$$d_t^i=\{\begin{array}{ll}\Vert {\mathbf{p}}_t-{\mathbf{o}}_i\Vert -r,& \text{球形障碍物}\\ \max \left(0,\Vert {\mathbf{p}}_t^{xy}-{\mathbf{o}}_i^{xy}\Vert -r\right)+\Delta z,& \text{圆柱形障碍物}\\ {\min }_{\mathbf{q}\in S}\Vert {\mathbf{p}}_t-\mathbf{q}\Vert ,& \text{其他障碍物}\end{array}$$
其中，$\Delta z$ 表示轨迹点到圆柱体底面或顶面的高度差，具体为：
$$\Delta z=\{\begin{array}{ll}0,& z_i\le z_t\le z_i+h\\ \min (|z_t-z_i|,|z_t-(z_i+h)|),& \text{否则}\end{array}$$