动态窗口法求路径规划的算法结构

1. 初始化：

定义起点和终点，设置障碍物，设定起点距离为0，其他点的距离为无穷大，将起点加入开放列表（open list）中。

2. 寻找最佳路径：

从开放列表中选取距离起点最近的点作为当前点，将其从开放列表中移除并加入封闭列表（closed list）中。对当前点的邻居进行遍历，如果邻居不在封闭列表中且不是障碍物，则计算它到起点的距离（即g值），如果比之前的距离更短，则更新邻居的距离和父节点，并将邻居加入开放列表中。

3. 判断终点是否可达：

如果终点在封闭列表中，则说明找到了一条通往终点的路径，否则如果开放列表为空，则说明没有通往终点的路径。

4. 输出路径：

从终点开始，沿着每个点的父节点一直回溯到起点，即可得到最佳路径。