改进A星算法：剔除冗余节点与光滑转折点-编程实验室

改进A星算法剔除冗余节点，光滑转折点对比优化前后路径。

在路径规划领域，A星算法无疑是一颗耀眼的明星。然而，原始的A星算法生成的路径可能存在冗余节点，并且转折点不够光滑，影响了路径的实用性和美观性。今天咱们就来聊聊如何对A星算法进行改进，解决这些小烦恼。

剔除冗余节点

冗余节点是什么

冗余节点就是那些对整体路径走向没有实质性影响的节点。比如在一条笔直的路径上，中间有若干个挨得很近且几乎在同一直线上的节点，它们对于从起点到终点的路径描述并没有提供更多有效信息。

代码实现思路

假设我们已经通过A星算法得到了一条路径path，这是一个节点列表。我们可以遍历这个列表，对于每一个节点path[i]，检查它是否可以被跳过，即判断path[i-1]，path[i]和path[i+1]是否大致在同一条直线上。

def remove_redundant_nodes(path): new_path = [path[0]] for i in range(1, len(path) - 1): p1 = path[i - 1] p2 = path[i] p3 = path[i + 1] # 这里简单通过斜率判断是否在同一直线，实际应用可能需要更精确方法 if (p3[1] - p1[1]) * (p2[0] - p1[0])!= (p2[1] - p1[1]) * (p3[0] - p1[0]): new_path.append(p2) new_path.append(path[-1]) return new_path

代码分析

首先，我们创建一个新的路径列表new_path，并将原路径的起点加入其中。
然后，从原路径的第二个节点开始遍历，一直到倒数第二个节点。对于每个节点p2，我们取出它前一个节点p1和后一个节点p3。
通过比较斜率来判断这三个节点是否大致在同一条直线上。如果不在同一条直线上，说明p2是有意义的，将其加入新路径；如果在同一条直线上，说明p2可能是冗余的，跳过它。
最后，将原路径的终点加入新路径，并返回这个剔除冗余节点后的新路径。

光滑转折点

为什么要光滑转折点

在实际应用中，过于尖锐的转折点可能会导致机器人、车辆等在路径执行过程中遇到困难，比如难以快速平稳地转弯。

代码实现思路

我们可以使用一些曲线拟合的方法来光滑转折点。这里简单介绍一种基于贝塞尔曲线的方法。假设我们有三个相邻的节点p1，p2，p3，我们可以生成一条贝塞尔曲线来连接p1和p3，并使用曲线上的点来替代p2。

import numpy as np import matplotlib.pyplot as plt def bezier_curve(points, num=20): t = np.linspace(0, 1, num) curve = np.zeros((num, 2)) for i in range(num): curve[i] = (1 - t[i]) ** 2 * points[0] + 2 * (1 - t[i]) * t[i] * points[1] + t[i] ** 2 * points[2] return curve def smooth_turns(path): new_path = [] for i in range(len(path) - 2): p1 = np.array(path[i]) p2 = np.array(path[i + 1]) p3 = np.array(path[i + 2]) curve = bezier_curve([p1, p2, p3]) new_path.extend(curve[1:-1].tolist()) new_path.append(path[-1]) return new_path

代码分析

bezier_curve函数实现了贝塞尔曲线的生成。它接受三个点points，并生成num个点组成的贝塞尔曲线。通过np.linspace生成从0到1的num个均匀分布的参数t。
然后根据贝塞尔曲线的公式(1 - t)^2p1 + 2(1 - t)tp2 + t^2 * p3计算曲线上每个点的坐标。
smooth_turns函数遍历路径，对于每三个相邻的节点，生成贝塞尔曲线，并将曲线上除起点和终点外的点加入新路径，最后加入原路径的终点。

对比优化前后路径

直观对比

为了更直观地看到优化前后路径的差异，我们可以通过绘图来展示。假设我们在一个二维平面上进行路径规划，起点和终点固定，通过A星算法得到原始路径，然后对其进行上述的冗余节点剔除和转折点光滑处理，得到优化后的路径。

# 假设path是原始路径，path1是剔除冗余节点后的路径，path2是光滑处理后的路径 plt.plot([p[0] for p in path], [p[1] for p in path], label='Original Path', marker='o') plt.plot([p[0] for p in path1], [p[1] for p in path1], label='Path after removing redundant nodes', marker='s') plt.plot([p[0] for p in path2], [p[1] for p in path2], label='Path after smoothing turns', marker='^') plt.legend() plt.show()