9.2 RRT算法的定义与实现
RRT算法通过随机采样和树结构迭代地探索搜索空间，直到找到连接起点和目标点的路径。算法不断生成新节点，并将其连接到最近邻节点，直到达到最大迭代次数或找到路径。最终，从树中提取路径作为解。
9.2.1 RRT算法的实现步骤
实现RRT算法的基本步骤如下所示。
（1）初始化：将起点添加到搜索树中作为唯一节点。
（2）循环：在搜索树中不断迭代，直到找到目标点或达到最大迭代次数。
- 采样：在自由空间中随机采样一个点作为新的探索点。
- 最近邻搜索：在搜索树中找到与新采样点最近的节点。
- 扩展：从最近邻节点向采样点之间的直线路径上进行一步扩展，并检查是否碰撞了障碍物。
- 连接：如果扩展路径是无碰撞的，则将新的节点添加到搜索树中，并将连接它的边添加到树中。
- 目标检测：检查新添加的节点是否足够接近目标点，如果是，则算法终止。
（3）路径提取：当算法终止时，从搜索树中提取从起点到目标点的路径。
9.2.2 原始的RRT算法
“原始的 RRT 算法”指的是最初提出的、最基本版本的 RRT（Rapidly-exploring Random Tree）算法。这个版本是由 Steven M. LaValle 在他的论文 "Rapidly-exploring Random Trees: A New Tool for Path Planning" 中提出的。原始的 RRT 算法是用于路径规划的一种基本方法，其核心思想是通过随机采样和树结构迭代地探索搜索空间，直到找到连接起点和目标点的路径。在算法的每次迭代中，随机生成一个点并通过扩展树来探索新的空间区域，以此逐步构建一棵树。通过这种方式，RRT 算法能够在自由空间中快速生成路径，并且在有限时间内收敛到一条可行路径。
原始的 RRT 算法包括两个关键部分：算法主体和 Extend 函数，具体说明如下所示。
1. RRT算法主体
算法主体通过初始化顶点和边集，然后在迭代过程中不断扩展树，直到达到指定的迭代次数。
2. Extend 函数
Extend 函数是主体的一部分，它负责从采样到的点开始扩展树，RRT 的扩展能够趋向于位姿空间中没有扩展到的部分。这就决定了RRT 一开始能够快速的进行扩展，而且能够形成对空间的全面覆盖。RRT 顶点是分配在位姿空间中是一致均匀的，如果路径存在，在顶点数目一定的条件下是肯定可以找到一条路径的。当然 RRT 算法也有一些缺点，它是一种纯粹的随机搜索算法对环境类型不敏感，当 C 中包含大量障碍物和狭窄通道约束时，算法的收敛速度慢，效率会大幅下降。为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率（0.0到1.0 的随机值 p）来选择生长方向是目标点还是随机点。
例如下面的例子实现了基本的RRT算法，在这个例子中，主要的RRT算法实现在 main 函数的for 循环中。在每次迭代中，都会随机采样一个点 q_rand，然后找到树中最接近该点的节点 q_nearest，并使用 extend 函数将该随机点扩展到树中。最后，将扩展后的节点加入树中。
实例9-1：使用原始的RRT算法寻找路径（codes/9/YRRT.cpp）
实例文件YRRT.cpp的具体实现代码如下所示。
#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>
using namespace std;
// 定义节点结构
struct Node {
double x, y;
Node(double x_, double y_) : x(x_), y(y_) {}
};
// 计算两个节点之间的距离
double distance(const Node& n1, const Node& n2) {
return sqrt(pow(n1.x - n2.x, 2) + pow(n1.y - n2.y, 2));
}
// 扩展函数
Node extend(const Node& nearest, const Node& rand, double max_distance) {
double d = distance(nearest, rand);
if (d <= max_distance) {
return rand;
} else {
double theta = atan2(rand.y - nearest.y, rand.x - nearest.x);
double x = nearest.x + max_distance * cos(theta);
double y = nearest.y + max_distance * sin(theta);
return Node(x, y);
}
}
// 主函数
int main() {
Node q_init(0, 0); // 起始点
Node q_goal(10, 10); // 目标点
vector<Node> V; // 存储节点的集合
V.push_back(q_init); // 将起始点加入集合
int iterations = 1000; // 迭代次数
double max_distance = 1.0; // 最大扩展距离
for (int i = 0; i < iterations; ++i) {
// 采样随机点
Node q_rand(rand() % 100, rand() % 100);
// 找到距离随机点最近的节点
Node q_nearest = V[0];
for (const auto& node : V) {
if (distance(node, q_rand) < distance(q_nearest, q_rand)) {
q_nearest = node;
}
}
// 扩展树
Node q_new = extend(q_nearest, q_rand, max_distance);
V.push_back(q_new);
}
// 输出节点集合
cout << "Nodes: ";
for (const auto& node : V) {
cout << "(" << node.x << ", " << node.y << ") ";
}
cout << endl;
return 0;
}上述代码实现了基本的RRT（Rapidly-exploring Random Tree）算法，具体实现流程如下所示。
（1）首先，定义了一个节点结构 Node，用来表示二维空间中的点，每个节点具有 x 和 y 坐标。
（2）然后，定义了计算两个节点之间距离的函数 distance，以及扩展节点的函数 extend。distance 函数使用欧几里得距离公式计算两个节点之间的距离，而 extend 函数用来在当前最近节点和随机生成的节点之间进行扩展，保证扩展距离不超过最大距离。
（3）接着，在 main 函数中，定义了起始节点 q_init 和目标节点 q_goal，并创建了一个存储节点的集合 V，将起始节点加入集合。
（4）在迭代过程中，程序将执行以下操作：
- 首先，随机生成一个节点 q_rand，其坐标位于二维空间内随机位置。
- 然后，找到当前节点集合中距离随机节点 q_rand 最近的节点 q_nearest。
- 接着，使用 extend 函数将 q_nearest 和 q_rand 之间的距离进行扩展，生成一个新的节点 q_new，并将其加入节点集合 V 中。
- 这个过程会持续进行指定的迭代次数。
（5）最后，打印输出节点集合 V 中所有节点的坐标，展示了算法在迭代过程中生成的节点的分布情况。
9.2.3 基于概率P的RRT算法
基于概率 P 的 RRT（Probabilistic Roadmap Planning）是一种路径规划算法，是 RRT 算法的一种变体。与传统的 RRT 算法不同，基于概率 P 的 RRT 引入了概率因素来指导树的生长，以提高路径搜索的效率。
在传统的 RRT 算法中，随机采样点的选择是均匀随机的，而在基于概率 P 的 RRT 中，采样点的选择是依据一定的概率分布进行的。这个概率分布通常会考虑到环境的特性，比如障碍物的分布情况、起点和终点的位置等。通过合理选择概率分布，可以使得采样点更有可能出现在搜索空间中有意义的区域，从而加速路径搜索的过程。
基于概率 P 的 RRT 算法仍然遵循 RRT 的基本思想，即通过不断生长树来探索搜索空间，并最终找到起点到目标点的可行路径。但是，通过引入概率因素，该算法能够更加智能地选择扩展方向，从而更快地收敛到解。这种方法通常可以提高算法的效率，并且在某些情况下能够更好地应对复杂的环境和任务需求。
基于概率P的RRT算法的Extend函数的实现步骤如下。
- 将当前图G的顶点集合V和边集合E复制到新的集合V′和E′，以便在函数中对它们进行修改，而不影响原始图G。
- 选择一个目标点q，这个目标点可以根据一定的概率P来决定，通常是根据算法的策略和目标点的位置来选择。这个目标点可能是目标点qgoal，也可能是随机点qrand，具体取决于算法的设计。
- 找到当前树G中与目标点q最近的节点qnearest。
- 使用Steer函数在节点qnearest和目标点q之间进行一个局部扩展，得到一个新的节点qnew。
- 如果从节点qnearest到节点qnew的路径是障碍物避开的，则执行以下步骤：
- 将新节点qnew添加到V′中。
- 将从qnearest到qnew的边添加到E′中。
- 返回更新后的图G′，其中G′=(V′,E′)。这个图包含了原始图G的所有顶点和边，以及新添加的节点和边。
- 上述Extend函数的目的是根据当前树G的状态和目标点qgoal的位置，尝试向目标点扩展树，以便更好地覆盖搜索空间并尽快找到一条可行路径。
由此可见，基于概率 P 的 RRT算法的主要改变就是在于 Extend 函数时，增加了一个ChoseTarget 函数。下面是一个基于概率P的RRT算法的完整例子，包括了节点结构的定义、距离计算函数、扩展函数以及主函数实现。
实例9-2：使用基于概率P的RRT算法（codes/9/PRRT.cpp）
实例文件PRRT.cpp的具体实现代码如下所示。
#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <ctime>
using namespace std;
// 定义节点结构
struct Node {
double x, y;
Node(double x_, double y_) : x(x_), y(y_) {}
};
// 计算两个节点之间的距离
double distance(const Node& n1, const Node& n2) {
return sqrt(pow(n1.x - n2.x, 2) + pow(n1.y - n2.y, 2));
}
// ChoseTarget函数，根据概率P选择目标点
Node ChoseTarget(const Node& qrand, const Node& qgoal, double P) {
if (rand() / double(RAND_MAX) < P) {
return qgoal;
} else {
return qrand;
}
}
// 找到距离目标点最近的节点
Node Nearest(const vector<Node>& V, const Node& q) {
Node nearest = V[0];
double min_dist = distance(nearest, q);
for (const auto& node : V) {
double dist = distance(node, q);
if (dist < min_dist) {
nearest = node;
min_dist = dist;
}
}
return nearest;
}
// 在节点qnearest和目标点q之间进行局部扩展，得到新节点
Node Steer(const Node& qnearest, const Node& q) {
double theta = atan2(q.y - qnearest.y, q.x - qnearest.x);
double x = qnearest.x + cos(theta);
double y = qnearest.y + sin(theta);
return Node(x, y);
}
// 判断从节点qnearest到节点qnew的路径是否避开障碍物
bool ObstacleFree(const Node& qnearest, const Node& qnew) {
// 在这里假设路径是始终可行的
return true;
}
// Extend函数，实现基于概率P的RRT算法的扩展操作
vector<Node> Extend(const vector<Node>& V, const Node& qrand, const Node& qgoal, double P) {
vector<Node> V_prime = V; // 复制当前节点集合到新节点集合
Node q = ChoseTarget(qrand, qgoal, P); // 根据概率P选择目标点
Node qnearest = Nearest(V_prime, q); // 找到距离目标点最近的节点
Node qnew = Steer(qnearest, q); // 在节点qnearest和目标点q之间进行局部扩展，得到新节点
if (ObstacleFree(qnearest, qnew)) { // 如果从节点qnearest到节点qnew的路径是障碍物避开的
V_prime.push_back(qnew); // 将新节点qnew添加到节点集合V'中
}
return V_prime; // 返回更新后的节点集合V'
}
// 主函数
int main() {
srand(time(NULL)); // 初始化随机种子
Node q_init(0, 0); // 起始点
Node q_goal(10, 10); // 目标点
vector<Node> V; // 存储节点的集合
V.push_back(q_init); // 将起始点加入集合
int iterations = 1000; // 迭代次数
double P = 0.9; // 概率P
for (int i = 0; i < iterations; ++i) {
// 采样随机点
Node q_rand(rand() % 100, rand() % 100);
// 扩展树
V = Extend(V, q_rand, q_goal, P);
}
// 输出节点集合
cout << "Nodes: ";
for (const auto& node : V) {
cout << "(" << node.x << ", " << node.y << ") ";
}
cout << endl;
return 0;
}上面的代码使用了基于概率 P 的 RRT（Rapidly-exploring Random Tree）算法的一种简化版本。在上述代码中，函数 Extend 实现了 RRT 算法的扩展操作。在每次迭代中，算法首先随机采样一个点 q_rand，然后根据概率 P 选择目标点 q_goal 或者随机点 q_rand。接着，算法找到距离目标点最近的节点 qnearest，并在节点 qnearest 和目标点之间进行局部扩展，得到新节点 qnew。最后，如果从节点 qnearest 到节点 qnew 的路径是避开障碍物的，就将节点 qnew 添加到节点集合中，从而完成了一次扩展操作。
9.2.4 RRT_Connect 算法
RRT-Connect 算法是 RRT（Rapidly-exploring Random Tree）算法的一种变体，用于解决路径规划问题。与标准的 RRT 算法不同，RRT-Connect 算法旨在通过两个相互连接的 RRT 树来更快地找到起始点和目标点之间的路径。
上述的RRT 每次搜索都只有从初始状态点生长的快速扩展随机树来搜索整个状态空间，如果从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。为此，在 2000 年由 LaValle 教授和日本东京大学的 Kuffner 教授联合提出了基于双向扩展平衡的连结型双树RRT 算法，即RRT_Connect 算法。
RRT-Connect 算法的基本思想是从起始点和目标点分别开始，在两个树之间进行交替的扩展，直到两个树连接在一起或达到最大迭代次数。算法会在两个树中选择一个目标点，然后利用 RRT 的基本扩展过程来不断扩展树，直到找到连接起始点和目标点的路径为止。
RRT-Connect 算法的优点如下所示：
- 较快地找到路径：通过同时从起始点和目标点开始扩展树，可以更快地找到连接两点的路径。
- 避免局部最优：由于使用了两个相互连接的树，可以避免陷入局部最优解。
RRT-Connect 算法在机器人路径规划和运动规划等领域得到了广泛的应用，特别是在需要快速找到路径且避免局部最优的情况下。
RRT-Connect 算法与原始 RRT 相比，在目标点区域建立第二棵树进行扩展。每一次迭代中，开始步骤与原始的 RRT 算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点后，以这个新的目标点作为第二棵树扩展的方向。
例如下面的实例演示了实现 RRT_Connect 算法的过程，首先定义了一个简单的二维空间，包括起始点和目标点。然后使用 RRT_Connect 算法寻找连接起始点和目标点的路径。如果找到路径，则输出路径上的节点；如果未找到路径，则输出“Failed to find a path!”。
实例9-3：使用RRT_Connect 算法寻找路径（codes/9/RRT_Connect.cpp）
实例文件RRT_Connect.cpp的具体实现代码如下所示。
#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <ctime>
using namespace std;
// 定义节点结构
struct Node {
double x, y;
Node(double x_, double y_) : x(x_), y(y_) {}
};
// 计算两个节点之间的距离
double distance(const Node& n1, const Node& n2) {
return sqrt(pow(n1.x - n2.x, 2) + pow(n1.y - n2.y, 2));
}
// 采样随机点
Node SampleRandomPoint() {
return Node(rand() % 100, rand() % 100);
}
// 扩展树
Node Extend(vector<Node>& tree, const Node& q_new, double step_size) {
Node q_nearest = tree[0];
double min_dist = distance(q_new, q_nearest);
for (const auto& node : tree) {
double dist = distance(node, q_new);
if (dist < min_dist) {
q_nearest = node;
min_dist = dist;
}
}
double theta = atan2(q_new.y - q_nearest.y, q_new.x - q_nearest.x);
double x = q_nearest.x + step_size * cos(theta);
double y = q_nearest.y + step_size * sin(theta);
Node q_new_extended(x, y);
tree.push_back(q_new_extended);
return q_new_extended;
}
// 检查是否连接两棵树
bool IsConnected(const Node& q1, const Node& q2, double threshold) {
return distance(q1, q2) < threshold;
}
// RRT_Connect 算法
vector<Node> RRT_Connect(const Node& start, const Node& goal, int max_iter, double step_size, double threshold) {
vector<Node> tree_start, tree_goal;
tree_start.push_back(start);
tree_goal.push_back(goal);
srand(time(NULL));
for (int iter = 0; iter < max_iter; ++iter) {
// 从起始树扩展
Node q_rand = SampleRandomPoint();
Node q_extended_start = Extend(tree_start, q_rand, step_size);
// 从目标树扩展
Node q_extended_goal = Extend(tree_goal, q_extended_start, step_size);
// 检查是否连接
if (IsConnected(q_extended_start, q_extended_goal, threshold)) {
// 如果连接，构建路径并返回
vector<Node> path;
path.push_back(start);
for (const auto& node : tree_start) {
path.push_back(node);
}
path.push_back(q_extended_start);
for (int i = tree_goal.size() - 1; i >= 0; --i) {
path.push_back(tree_goal[i]);
}
path.push_back(goal);
return path;
}
}
// 如果达到最大迭代次数仍未找到路径，则返回空路径
return vector<Node>();
}
int main() {
Node start(0, 0); // 起始点
Node goal(10, 10); // 目标点
int max_iter = 1000; // 最大迭代次数
double step_size = 1.0; // 扩展步长
double threshold = 1.0; // 连接阈值
vector<Node> path = RRT_Connect(start, goal, max_iter, step_size, threshold);
if (path.empty()) {
cout << "Failed to find a path!" << endl;
} else {
cout << "Found a path:" << endl;
for (const auto& node : path) {
cout << "(" << node.x << ", " << node.y << ") ";
}
cout << endl;
}
return 0;
}对上述代码的实现流程如下所示：
- 首先，定义了一个 Node 结构体，用于表示二维空间中的节点，其中包含节点的 x 和 y 坐标。
- 然后，实现了distance 函数，用于计算两个节点之间的欧氏距离。
- 接着，实现了SampleRandomPoint 函数，用于在给定的空间内随机采样一个点作为新节点。
- 然后，实现了 Extend 函数，用于在树中扩展一个新节点，该函数会找到距离新节点最近的节点，并沿着连接这两个节点的方向进行扩展。
- 接着，实现了IsConnected 函数，用于检查两个节点之间的距离是否小于给定的阈值，从而判断是否连接。
- 接着，实现了核心函数RRT_Connect，该函数接收起始点、目标点、最大迭代次数、扩展步长和连接阈值作为输入参数。在每次迭代中，该函数会从起始树和目标树分别扩展节点，并检查是否两棵树已经连接。如果连接，则构建路径并返回；如果达到最大迭代次数仍未找到路径，则返回空路径。
- 最后，在主函数main中，定义了起始点、目标点、最大迭代次数、扩展步长和连接阈值，并调用 RRT_Connect 函数来执行路径规划。根据返回的路径结果，输出相应的信息，如果找到了路径，则输出路径上的节点坐标；如果未找到路径，则输出失败信息。
执行后会输出：
Found a path:
(0, 0) (0, 0) (0.666795, 0.745241) (1.63821, 0.982624) (2.15109, 1.84108) (2.56541, 2.75122) (3.42105, 3.26879) (4.23841, 3.84491) (4.5559, 4.79317) (4.5559, 4.79317) (5.0172, 5.11175) (5.84004, 5.68003) (6.49759, 6.43344) (7.19464, 7.15046) (7.91953, 7.83933) (8.61269, 8.56011) (9.28992, 9.29588) (10, 10) (10, 10)9.2.5 RRT*算法
RRT*（Rapidly-exploring Random Tree Star）算法是 RRT（Rapidly-exploring Random Tree）算法的改进版本，旨在提高路径规划的效率和路径质量。RRT* 算法最早是由 Steven M. LaValle 和 James J. Kuffner Jr在2000年提出的。
RRT* 算法主要通过优化现有路径树的方式来改进 RRT 算法，而不是像 RRT 那样简单地在空间中不断生成随机样本和扩展树。其基本思想是通过引入代价函数来评估树中的各个节点，并在树的生长过程中不断优化树结构，以使生成的路径更加高效和优化。
RRT* 算法的主要特点和优势包括：
- 最优性保证：RRT* 算法可以保证找到的路径是最优或接近最优的，特别是在迭代次数趋于无穷大的极限情况下。
- 连续优化：算法通过对树中节点的代价进行优化，使得树的结构在不断扩展的过程中能够更好地反映问题的最优解。
- 自适应性：RRT* 算法能够根据问题的具体情况和要求，自适应地调整代价函数和优化策略，以获得更好的路径质量和性能。
总的来说，RRT* 算法是一种强大的路径规划算法，适用于需要高效、优化和最优路径的问题领域，如机器人运动规划、自动驾驶车辆路径规划等。
2010 年，MIT 的 Sertac 和 Emilio 提出了完整的RRT*算法。这个算法与上面的算法相比，不但可以找到可行解，还可以找到一条相对次优的算法。例如下面的实例实现了RRT*算法，通过随机扩展树、优化树结构等步骤，用于寻找从起始点到目标点的最优路径。
实例9-4：使用RRT*算法寻找路径（codes/9/RRTStar.cpp）
实例文件RRTStar.cpp的具体实现代码如下所示。
#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <limits>
#include <algorithm>
using namespace std;
struct Node {
double x, y;
Node* parent;
vector<Node*> children;
double cost;
Node(double x_, double y_) : x(x_), y(y_), parent(nullptr), cost(0.0) {}
};
double distance(const Node& n1, const Node& n2) {
return sqrt(pow(n1.x - n2.x, 2) + pow(n1.y - n2.y, 2));
}
Node* nearestNode(const vector<Node*>& nodes, const Node& target) {
double minDist = numeric_limits<double>::max();
Node* nearest = nullptr;
for (Node* node : nodes) {
double dist = distance(*node, target);
if (dist < minDist) {
minDist = dist;
nearest = node;
}
}
return nearest;
}
bool obstacleFree(const Node& from, const Node& to) {
// Assuming the path between two nodes is always obstacle-free
return true;
}
vector<Node*> nearNodes(const vector<Node*>& nodes, const Node& target, double gamma, double eta) {
vector<Node*> near;
for (Node* node : nodes) {
double dist = distance(*node, target);
if (dist < gamma * eta) {
near.push_back(node);
}
}
return near;
}
Node* generateRandomNode() {
return new Node(rand() % 100, rand() % 100);
}
Node* steer(const Node& from, const Node& to, double step_size) {
double theta = atan2(to.y - from.y, to.x - from.x);
double newX = from.x + step_size * cos(theta);
double newY = from.y + step_size * sin(theta);
return new Node(newX, newY);
}
void rrtStar(Node* start, Node* goal, int max_iter, double step_size, double gamma, double eta) {
vector<Node*> nodes;
nodes.push_back(start);
srand(time(nullptr));
for (int iter = 0; iter < max_iter; ++iter) {
Node* q_rand = generateRandomNode();
Node* q_nearest = nearestNode(nodes, *q_rand);
Node* q_new = steer(*q_nearest, *q_rand, step_size);
if (obstacleFree(*q_nearest, *q_new)) {
vector<Node*> near_nodes = nearNodes(nodes, *q_new, gamma, eta);
Node* q_min = q_nearest;
double c_min = q_nearest->cost + distance(*q_nearest, *q_new);
for (Node* near_node : near_nodes) {
if (obstacleFree(*near_node, *q_new) && near_node->cost + distance(*near_node, *q_new) < c_min) {
q_min = near_node;
c_min = near_node->cost + distance(*near_node, *q_new);
}
}
q_new->cost = c_min;
q_new->parent = q_min;
q_min->children.push_back(q_new);
nodes.push_back(q_new);
for (Node* near_node : near_nodes) {
if (obstacleFree(*q_new, *near_node) && q_new->cost + distance(*q_new, *near_node) < near_node->cost) {
near_node->parent = q_new;
near_node->cost = q_new->cost + distance(*q_new, *near_node);
}
}
}
}
}
void printPath(Node* goal) {
vector<Node*> path;
while (goal) {
path.push_back(goal);
goal = goal->parent;
}
reverse(path.begin(), path.end());
cout << "Path: ";
for (const auto& node : path) {
cout << "(" << node->x << ", " << node->y << ") ";
}
cout << endl;
}
int main() {
Node* start = new Node(0, 0);
Node* goal = new Node(10, 10);
int max_iter = 1000;
double step_size = 1.0;
double gamma = 1.0;
double eta = 0.5;
rrtStar(start, goal, max_iter, step_size, gamma, eta);
printPath(goal);
return 0;
}上述代码的实现流程如下所示：
- 首先，定义了节点结构 Node，包含坐标、父节点指针、子节点集合和代价等信息。
- 然后，实现了一系列辅助函数，包括计算节点间距离、查找最近节点、判断路径是否自由、寻找邻近节点等。
- 接着，通过 rrtStar 函数实现了 RRT* 算法的核心逻辑。在每次迭代中，随机生成一个节点，寻找最近节点并通过 steer 函数生成新节点，然后检查路径是否自由。如果路径自由，更新节点的代价和父节点，并进行树结构的优化。
- 最后，通过 printPath 函数打印输出从目标节点回溯到起始节点的最优路径。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
