（7-4-2）D* Lite 算法：智能机器人路径导航系统（2）

7.4.3  实现D* Lite路径规划算法

在本项目中，文件dstar.h和dstar.cpp共同实现了D* Lite路径规划算法。其中dstar.h 提供了路径规划算法的接口声明，而 dstar.cpp 则实现了这些接口，完成了D* Lite路径规划算法的具体功能。

1. 路径规划算法声明

文件dstar.h定义了一个名为 DStar_Lite 的类，该类继承自 Environment，该类继承自 Environment，用于执行路径规划。文件dstar.h包含了路径规划算法所需的参数和方法的声明，以及用于环境建模和初始化的信息。

#ifndef DSTAR_H
#define DSTAR_H

#include "dstar_env.h"
#include <gl/freeglut.h>
#include <iostream>
#include <queue>
#include <tuple>
#include <string>

using namespace std;

typedef tuple<float, float, pair<int,int>> Tuple;

class DStar_Lite : Environment {
public:

	DStar_Lite(int nR, int nC, int startRow, int startCol, int termRow, int termCol) : Environment(nC, nR) {
		nRow = nR;
		nCol = nC;
		startCoord.first = startRow;
		startCoord.second = startCol;
		terminalCoord.first = termRow;
		terminalCoord.second = termCol;

		if (startRow > nRow - 1 || startCol > nCol - 1 || termRow > nRow - 1 || termCol > nCol - 1 || nR < 1 || nC < 1) {
			throw std::invalid_argument("Invalid selection of start (or terminal) coordinates!");
		}
		if (startRow == termRow && startCol == termCol) {
			throw std::invalid_argument("Start position and terminal position are identical!");
		}

		k_m = 0;
		SWEEPING_RANGE = 5;
		stuck_flag_count = 0;
		MAX_RETRY = 20; //maximum number of retry when stuck before throwing an exception 

		PathFinder(0, 0);
	};
	
private:

	int nRow, nCol;
	pair<int, int> startCoord;
	pair<int, int> terminalCoord;
	
	//priority_queue<Tuple, vector<Tuple>, std::greater<Tuple>> openList;
	vector<Tuple> openList;
	float k_m;
	int SWEEPING_RANGE;
	int stuck_flag_count;
	int MAX_RETRY;

	void initDStarLite();
	float heuristics(pair<int, int> targetCoord, pair<int, int> currentCoord);
	pair<float, float> calculateKey(pair<int, int> targetCoord, pair<int, int> currentCoord);
	bool topKeyComparison(pair<float, float> currKey, bool popOut);
	void computeShortestPath(pair<int, int> currentCoord);
	void updateGrid(pair<int, int> targetCoord, pair<int, int> currentCoord);
	pair<int, int> nextInShortestPath(pair<int, int> currentCoord);
	bool ScanArea(pair<int, int> currentCoord, int scan_range);
	pair<int, int> MotionAndScan(pair<int, int> currentCoord, int scan_range);
	void PathFinder(int argc, char** argv);
};

#endif // !DSTAR_H

在上述代码中，DStar_Lite(int nR, int nC, int startRow, int startCol, int termRow, int termCol) : Environment(nC, nR)是 DStar_Lite 类的构造函数，用于初始化一个 D* Lite 路径规划器的实例。构造函数DStar_Lite()接受如下所示的参数。

1. nR：网格环境的行数。
2. nC：网格环境的列数。
3. startRow：起点的行坐标。
4. startCol：起点的列坐标。
5. termRow：终点的行坐标。
6. termCol：终点的列坐标。
7. 构造函数的初始化列表部分：Environment(nC, nR) 调用了 Environment 类的构造函数，并将 nC 和 nR 作为参数传递给它，以初始化基类 Environment。这样做的目的是为了在 DStar_Lite 类中利用基类 Environment 中定义的网格环境信息来执行路径规划。

2. 路径规划算法实现

文件dstar.cpp实现了D* Lite路径规划算法的关键部分，包括初始化、计算启发式函数、计算关键值、更新网格状态、计算最短路径等功能。文件dstar.cpp的具体实现流程如下所示。

（1）函数DStar_Lite::initDStarLite() 用于初始化D* Lite算法，主要执行了如下所示的操作：

1. 将终点的右手边界设置为0。
2. 计算起点和终点之间的关键值。
3. 将终点添加到优先队列中，并根据关键值排序。
4. 调用 computeShortestPath() 函数计算最短路径。

#include "dstar.h"

void DStar_Lite::initDStarLite() {
	
	cells[terminalCoord.first][terminalCoord.second].rhs = 0;
	pair<float, float> keys = calculateKey(terminalCoord, startCoord);
	openList.push_back({ keys.first, keys.second, terminalCoord });
	sort(openList.begin(), openList.end());
	computeShortestPath(startCoord);
}

（2）函数DStar_Lite::heuristics(pair<int, int> targetCoord, pair<int, int> currentCoord)用于计算启发式（heuristic）代价，即从当前坐标到目标坐标的估计距离。在这个函数中，使用欧几里得距离公式计算两个坐标之间的距离，即直线距离。

float DStar_Lite::heuristics(pair<int, int> targetCoord, pair<int, int> currentCoord) {
  return sqrtf(powf(targetCoord.first - currentCoord.first, 2) + powf(targetCoord.second - 
  currentCoord.second, 2));

}

（3）函数DStar_Lite::calculateKey(pair<int, int> targetCoord, pair<int, int> currentCoord) 用于计算 D* Lite 算法中的键（key）值，这个键值用于确定节点在优先队列中的顺序。根据 D* Lite 算法的定义，键值包括两部分：第一部分是从起点到目标节点的路径代价加上启发式代价；第二部分是起点到目标节点的路径代价。

pair<float, float> DStar_Lite::calculateKey(pair<int, int> targetCoord, pair<int, int> currentCoord) {
	pair<float, float> keys;
	keys.first = min(cells[targetCoord.first][targetCoord.second].g, cells[targetCoord.first][targetCoord.second].rhs) + heuristics(targetCoord, currentCoord) + k_m;
	keys.second = min(cells[targetCoord.first][targetCoord.second].g, cells[targetCoord.first][targetCoord.second].rhs);
	return keys;
}

（4）函数DStar_Lite::topKeyComparison(pair<float, float> currKey, bool popOut) 用于比较给定的键值 currKey 与优先队列中队首节点的键值的大小关系。根据 D* Lite 算法的定义，键值由两部分组成，该函数首先比较两个键值的第一个部分，如果相等则比较第二个部分。函数中的 popOut 参数指示是否需要弹出队首节点。

bool DStar_Lite::topKeyComparison(pair<float, float> currKey, bool popOut) {
	float K1, K2;
	if (!openList.empty()) {
		const Tuple& topKey = openList[0];
		K1 = get<0>(topKey);
		K2 = get<1>(topKey);
		pair<int, int> coords = get<2>(topKey);
		if (popOut) {
			openList.erase(openList.begin());
		}

	}
	else {
		K1 = K2 = FLT_MAX;
	}



	if (K1 < currKey.first) {
		return true;
	}
	else if (K1 > currKey.first) {
		return false;
	}
	else {
		if (K2 < currKey.second) {
			return true;
		}
	}

	return false;

}

（5）函数DStar_Lite::computeShortestPath(pair<int, int> currentCoord) 用于计算从当前坐标到目标坐标的最短路径。它通过不断更新节点的成本和启发值来实现路径的计算，直到满足终止条件为止。

void DStar_Lite::computeShortestPath(pair<int, int> currentCoord) {

	while (cells[currentCoord.first][currentCoord.second].rhs != cells[currentCoord.first][currentCoord.second].g || topKeyComparison(calculateKey(currentCoord, currentCoord), false)) {


		const Tuple& topKey = openList[0];
		float K1 = get<0>(topKey);
		float K2 = get<1>(topKey);
		pair<int, int> U = get<2>(topKey);
		//cout << "[" << U.first << "," << U.second << "]: " << K1 << ", " << K2 << endl;
		if (topKeyComparison(calculateKey(U, currentCoord), true)) {
			pair<float, float> keys = calculateKey(U, currentCoord);
			openList.push_back({ keys.first, keys.second, U });
			sort(openList.begin(), openList.end());
		}
		else if (cells[U.first][U.second].g > cells[U.first][U.second].rhs) {
			cells[U.first][U.second].g = cells[U.first][U.second].rhs;
			for (auto it = cells[U.first][U.second].preds.begin(); it != cells[U.first][U.second].preds.end(); ++it) {
				updateGrid(it->first, currentCoord);
			}
		}
		else {
			cells[U.first][U.second].g = FLT_MAX;
			updateGrid(U, currentCoord);
			for (auto it = cells[U.first][U.second].preds.begin(); it != cells[U.first][U.second].preds.end(); ++it) {
				updateGrid(it->first, currentCoord);
			}
		}
	}

}

（6）函数DStar_Lite::updateGrid(pair<int, int> targetCoord, pair<int, int> currentCoord)用于更新网格中目标坐标的信息，包括计算目标坐标的成本、更新网格状态和调整优先级队列中的元素。

void DStar_Lite::updateGrid(pair<int, int> targetCoord, pair<int, int> currentCoord) {
	if (targetCoord != terminalCoord) {
		float min_rhs = FLT_MAX;
		for (auto it = cells[targetCoord.first][targetCoord.second].succs.begin(); it != cells[targetCoord.first][targetCoord.second].succs.end(); ++it) {
			min_rhs = min(min_rhs, cells[it->first.first][it->first.second].g + it->second);
		}

		cells[targetCoord.first][targetCoord.second].rhs = min_rhs;
	}

	vector<pair<int, int>> gridInOpenList;
	for (int i = 0; i < openList.size(); ++i) {
		const Tuple& vals = openList[i];
		if (get<2>(vals) == targetCoord) {
			gridInOpenList.push_back(targetCoord);
		}
	}

	if (gridInOpenList.size() > 0) {
		if (gridInOpenList.size() > 1) {
			throw std::invalid_argument("More than one targetCoord in the queue!");
		}
		for (int i = 0; i < openList.size(); ++i) {
			const Tuple& vals = openList[i];
			if (get<2>(vals) == gridInOpenList[0]) {
				openList.erase(openList.begin() + i);
			}
		}
	}

	if (cells[targetCoord.first][targetCoord.second].rhs != cells[targetCoord.first][targetCoord.second].g) {
		pair<float, float> keys = calculateKey(targetCoord, currentCoord);
		openList.push_back({ keys.first,keys.second,targetCoord });
		sort(openList.begin(), openList.end());
	}

}

（7）函数DStar_Lite::nextInShortestPath(pair<int, int> currentCoord) 用于找到当前坐标在最短路径中的下一个坐标，并返回该坐标。

pair<int, int> DStar_Lite::nextInShortestPath(pair<int, int> currentCoord) {
	float min_rhs = FLT_MAX;
	pair<int, int> s_next(-1, -1);
	if (cells[currentCoord.first][currentCoord.second].rhs == FLT_MAX) {
		cout << "You are stuck at the moment, Let's explore some more to see if there's a new opening" << endl;
		stuck_flag_count++;
		if (stuck_flag_count == MAX_RETRY) {
			cout << "It seems there's no way out" << endl;
			throw;
		}
		else {
			return currentCoord;
		}
	}
	else {
		for (auto it = cells[currentCoord.first][currentCoord.second].succs.begin(); it != cells[currentCoord.first][currentCoord.second].succs.end(); ++it) {
			float child_cost = cells[it->first.first][it->first.second].g + it->second;

			if (child_cost < min_rhs) {
				min_rhs = child_cost;
				s_next = it->first;
			}
		}
		if (s_next.first >= 0 && s_next.second >= 0) {

			return s_next;
		}
		throw std::invalid_argument("Could not find child for transition!");

	}

}

（8）函数DStar_Lite::ScanArea(pair<int, int> currentCoord, int scan_range) 用于扫描当前坐标周围指定范围内的区域，检测是否存在障碍物或者障碍物被清除，并更新环境状态。

bool DStar_Lite::ScanArea(pair<int, int> currentCoord, int scan_range) {

	int range_checked = 0;
	unordered_map<pair<int, int>, float, hash_pair> states_to_update;
	if (scan_range >= 1) {
		for (auto it = cells[currentCoord.first][currentCoord.second].succs.begin(); it != cells[currentCoord.first][currentCoord.second].succs.end(); ++it) {
			states_to_update[it->first] = cells[it->first.first][it->first.second].status;
		}
		range_checked = 1;
	}

	while (range_checked < scan_range) {
		unordered_map<pair<int, int>, float, hash_pair> new_set;
		for (auto it = states_to_update.begin(); it != states_to_update.end(); ++it) {
			new_set[it->first] = it->second;
			for (auto it2 = cells[it->first.first][it->first.second].succs.begin(); it2 != cells[it->first.first][it->first.second].succs.end(); ++it2) {
				if (new_set.find(it2->first) == new_set.end()) {
					new_set[it2->first] = cells[it2->first.first][it2->first.second].status;
				}
			}
		}
		range_checked += 1;
		states_to_update = new_set;
	}

	bool new_obstacle = false;
	for (auto it = states_to_update.begin(); it != states_to_update.end(); ++it) {
		if (it->second < 0) {
			//found a cell with obstacle
			for (auto it2 = cells[it->first.first][it->first.second].succs.begin(); it2 != cells[it->first.first][it->first.second].succs.end(); ++it2) {

				if (it2->second != FLT_MAX) {
					//first time to observe this obstacle that wasn't there before
					cells[it->first.first][it->first.second].status = -2;
					cells[it2->first.first][it2->first.second].succs[it->first] = FLT_MAX;

					cells[it->first.first][it->first.second].succs[it2->first] = FLT_MAX;
					updateGrid(it->first, currentCoord);
					new_obstacle = true;
				}
			}
		}
		else if(it->second==1) {
			//detected an obstacle clearance
			for (auto it2 = cells[it->first.first][it->first.second].succs.begin(); it2 != cells[it->first.first][it->first.second].succs.end(); ++it2) {
				if (it2->second == FLT_MAX) {
					//This is the previous obstacle cell that is cleared within the sweeping range
					cells[it->first.first][it->first.second].status = 0;
					cells[it2->first.first][it2->first.second].succs[it->first] = heuristics(it->first,it2->first);

					cells[it->first.first][it->first.second].succs[it2->first] = heuristics(it->first, it2->first);
					updateGrid(it->first, currentCoord);
				}
			}
		}
	}
	return new_obstacle;
}

（9）函数DStar_Lite::MotionAndScan(pair<int, int> currentCoord, int scan_range) 用于执行移动并扫描操作，根据当前坐标和扫描范围，进行路径规划和环境扫描，检测障碍物并更新路径。

pair<int, int> DStar_Lite::MotionAndScan(pair<int, int> currentCoord, int scan_range) {
	if (currentCoord == terminalCoord) {
		return terminalCoord;
	}

	pair<int, int> s_new = nextInShortestPath(currentCoord);
	if (cells[s_new.first][s_new.second].status == -1) {
		//Just ran into new obstacle
		s_new = currentCoord;
	}

	bool result = ScanArea(s_new, scan_range);
	if (result) {
		k_m += heuristics(currentCoord, s_new);
		computeShortestPath(currentCoord);
	}

	return s_new;
}

未完待续