（6-3）D*算法：基于D*算法的动态路径规划可视化系统

6.3  基于D*算法的动态路径规划可视化系统

本项目实现了基于D*算法的动态路径规划系统，具备动态环境适应性、高效路径规划、用户友好界面和开放可扩展性等功能。

实例6-2：动态路径规划系统（codes/6/Dynamic-A-D-algorithm/）

6.3.1  项目介绍

本“动态路径规划系统”是一款基于D*算法的路径规划系统，旨在为用户提供灵活、高效的路径规划解决方案。该系统以实时动态环境变化为特色，能够即时响应环境的变化，实现路径的动态规划和实时修正。无论是机器人导航、自动驾驶、无人机飞行还是其他需要路径规划的场景，该系统都能提供可靠的路径规划支持。

1. 主要特点

1. 动态环境适应性：系统能够动态响应环境变化，及时更新路径规划结果，保证路径的有效性和安全性。
2. 高效路径规划：基于D*算法的路径规划算法，具有较高的路径规划效率和优化能力，能够在复杂环境中快速生成最优路径。
3. 开放性与可扩展性：系统提供灵活的接口和扩展能力，可以方便地与其他系统集成，满足不同领域的路径规划需求。

2. 应用领域

1. 机器人导航：用于机器人在复杂环境中的路径规划，实现自主导航和避障功能。
2. 自动驾驶：用于自动驾驶汽车、无人车等车辆的路径规划和导航，保证行驶安全和效率。
3. 无人机飞行：用于无人机在空中的路径规划和航行，实现任务执行和地理勘测等功能。
4. 物流配送：用于物流配送系统中的路径规划和货物运输，提高配送效率和服务质量。

本“动态路径规划系统”将为用户提供可靠、高效的路径规划解决方案，助力各种应用场景下的智能化和自动化发展。

6.3.2  D*算法功能的声明

在本项目中，文件myDstar.h的主要功能是声明类D_star及其成员函数和成员变量，定义了类D_star的接口和结构，但没有具体的实现。这样的设计使得类的声明与实现分离，可以提高代码的模块化和可维护性，同时也方便其他文件包含并使用该类。

#include<iostream>
#include<fstream>
#include<tuple>
#include<map>
#include<math.h>
#include<string>
#include<vector>
using namespace std;

class D_star
{
public:
	explicit D_star();
	map<tuple<int,int,int>, double> Alldirec;   // 27个方向，获得子节点
	map<tuple<int, int, int>, tuple<int, int, int>> b;  // 记录反向指针，即从一个节点指向父节点，D*是反着指的
	map<tuple<int, int, int>, double> OPEN; // 开节点集
	map <tuple<int, int, int>, double> h;   // 路径耗散记录表
	map <tuple<int, int, int>, string> tag; // 标志：Open Closed New
	vector<tuple<int, int, int>> path;      // 记录规划路径的容器
	int count;                              // 记录迭代次数

	tuple<int, int, int> start;             // 起始点位置
	tuple<int, int, int> goal;              // 终止点位置

	double cost(tuple<int, int, int>&, tuple<int, int, int>&); // 欧式距离计算函数（碰撞返回一个 大数）
	void check_state(tuple<int, int, int>&);                   // 添加信息
	double get_kmin();                                         // 获取开节点集最小值
	tuple<tuple<int, int, int>,double> min_state();            // 获取开节点集最小值并弹出元素
	void insert(tuple<int, int, int>&,double&);                // 插入开节点集并更新h表
	double process_state();                                    // D*核心算法 详见原论文
	vector<tuple<int, int, int>> children(tuple<int, int, int>&);    // 获取子节点
	void modify_cost(tuple<int, int, int>&);                         // 更新动态环境下节点信息
	void modify(tuple<int, int, int>&);                              // 同上，配合使用
	void get_path();                                                 // 获取规划路径
	void run();                                                      // 主函数 
	void save_path(string);                                          // 保存路径到csv

	/*运动障碍球信息*/
	double obs_r;
	tuple<int, int, int> obs_pos;
};

在上述代码中声明了如下所示的成员变量：

1. Alldirec：用于存储所有方向的信息，用于获取子节点。
2. b：记录反向指针，即从一个节点指向父节点。
3. OPEN：开节点集，存储待扩展的节点。
4. h：路径耗散记录表。
5. tag：标志节点的状态，包括 Open、Closed、N