一、自动驾驶路径重规划的常见痛点
咱们开自动驾驶车的都遇到过这种情况：本来规划好的路线走得好好的，突然路边窜出个外卖车、或者前方有人临时变道，原来的路线就撞了。这时候车得赶紧重新找一条能走的路，这个过程就是路径重规划。
以前很多车的重规划逻辑挺简单的，就是绕开障碍物直接找新路线，但经常出问题：比如绕的时候不小心占了非机动车道、或者和对向车抢路，违反交规；还有的绕得太急，方向盘猛打，车里人晃得难受。说白了就是以前的重规划太“笨”，只想着绕开障碍物，忘了还有一堆限制条件。
二、动态障碍物下的路径重规划需求
动态障碍物就是会动的障碍物，比如刚才说的外卖车、变道的私家车、甚至横穿马路的行人。这些障碍物的位置、速度一直在变，所以重规划的路线不能只考虑当前的情况，还得预判一会儿障碍物的位置，不然刚规划好路线，障碍物又动了，又得重新来，来回折腾。
而且重规划还得满足一堆“硬要求”：比如不能压双黄线、不能超速、得和其他车保持安全距离、转弯角度不能太大（不然车翻了）、得尽量沿着原来的路线走（不能随便绕远）。这些要求合起来，就是我们说的“约束”。
三、基于约束满足的再搜索方案核心思路
这个方案的核心就是：先把所有的限制条件都列出来，然后再找新路线的时候，不仅要绕开障碍物，还得满足所有限制条件。简单说就是“带着镣铐跳舞”，不能为了绕开障碍物就违反规矩。
举个例子，原来的路线是从A点到B点，中间C点突然有个外卖车。我们先列约束：1. 不能压双黄线；2. 和外卖车的距离不能小于1米；3. 转弯角度不能超过30度；4. 速度不能超过60公里/小时。然后再找新路线，从A出发，绕开C点的外卖车，同时满足这四个约束，最后到B点。
3.1 约束的分类
约束可以分成两类： 第一类是“硬约束”，就是必须满足的，违反了就会出事或者违法。比如不能压双黄线、不能撞车、不能超速。 第二类是“软约束”，就是尽量满足的，实在满足不了可以稍微妥协。比如尽量沿着原来的路线走、尽量少变道、尽量保持速度平稳。
3.2 再搜索的基本步骤
再搜索的步骤其实很清晰： 第一步，预判障碍物的未来位置。比如外卖车现在的速度是30公里/小时，方向是向东，那1秒后它的位置大概是在现在位置东边8米的地方。 第二步，列出所有的硬约束和软约束。 第三步，在原来的路线附近找所有可能的新路线，一条条检查是否满足所有约束。 第四步，从满足约束的路线里，选一条最优的，比如最接近原来路线的、最平稳的。
四、完整示例（技术栈：Python）
接下来我们用Python写一个简化的示例，模拟动态障碍物下的路径重规划。这个示例会包含预判障碍物位置、检查约束、找新路线的整个过程。
首先，我们先定义几个基础的类，用来表示位置、障碍物、路线这些东西：
import math
# 位置类，用来表示一个点的坐标
class Position:
def __init__(self, x, y):
self.x = x # 横向坐标，单位：米
self.y = y # 纵向坐标，单位：米
# 计算两个位置之间的距离
def distance_to(self, other):
return math.sqrt((self.x - other.x)**2 + (self.y - other.y)**2)
# 动态障碍物类
class DynamicObstacle:
def __init__(self, current_pos, speed, direction):
self.current_pos = current_pos # 当前位置
self.speed = speed # 速度，单位：米/秒
self.direction = direction # 方向，角度，0度为向东，逆时针增加
# 预判未来某一时刻的位置
def predict_position(self, time):
# 把方向转成弧度
rad = math.radians(self.direction)
# 计算未来位置的坐标
new_x = self.current_pos.x + self.speed * time * math.cos(rad)
new_y = self.current_pos.y + self.speed * time * math.sin(rad)
return Position(new_x, new_y)
# 路线类
class Route:
def __init__(self, points):
self.points = points # 路线上的一系列位置点
# 计算路线的总长度
def total_length(self):
length = 0
for i in range(len(self.points) - 1):
length += self.points[i].distance_to(self.points[i+1])
return length
接下来，我们定义约束检查函数，用来判断一条路线是否满足所有约束：
# 定义硬约束的阈值
MIN_SAFE_DISTANCE = 1.0 # 和障碍物的最小安全距离，单位：米
MAX_TURN_ANGLE = 30 # 最大转弯角度，单位：度
MAX_SPEED = 16.67 # 最大速度，单位：米/秒（约60公里/小时）
# 检查路线是否满足所有硬约束
def check_constraints(route, obstacle, time):
# 约束1：和障碍物的安全距离，检查路线上所有点和障碍物未来位置的距离
obs_future_pos = obstacle.predict_position(time)
for point in route.points:
if point.distance_to(obs_future_pos) < MIN_SAFE_DISTANCE:
return False, "违反安全距离约束"
# 约束2：最大转弯角度，检查路线上相邻三个点的转弯角度
for i in range(len(route.points) - 2):
p1 = route.points[i]
p2 = route.points[i+1]
p3 = route.points[i+2]
# 计算向量p1p2和p2p3的夹角
v1_x = p2.x - p1.x
v1_y = p2.y - p1.y
v2_x = p3.x - p2.x
v2_y = p3.y - p2.y
# 计算向量的点积
dot_product = v1_x * v2_x + v1_y * v2_y
# 计算向量的模长
mod_v1 = math.sqrt(v1_x**2 + v1_y**2)
mod_v2 = math.sqrt(v2_x**2 + v2_y**2)
# 计算夹角的余弦值
cos_angle = dot_product / (mod_v1 * mod_v2)
# 把余弦值转成角度，注意要限制范围避免计算错误
cos_angle = max(-1.0, min(1.0, cos_angle))
angle = math.degrees(math.acos(cos_angle))
if angle > MAX_TURN_ANGLE:
return False, "违反最大转弯角度约束"
# 约束3：最大速度，假设路线上的点间隔是0.1秒，计算每个间隔的速度
for i in range(len(route.points) - 1):
distance = route.points[i].distance_to(route.points[i+1])
speed = distance / 0.1 # 时间间隔是0.1秒
if speed > MAX_SPEED:
return False, "违反最大速度约束"
return True, "满足所有约束"
最后，我们模拟一个场景，测试重规划的过程：
# 场景模拟：
# 原来的路线：从起点(0,0)到终点(10,0)，路线上的点是(0,0), (2,0), (4,0), (6,0), (8,0), (10,0)
original_route = Route([
Position(0, 0),
Position(2, 0),
Position(4, 0),
Position(6, 0),
Position(8, 0),
Position(10, 0)
])
# 动态障碍物：当前位置(5,0)，速度10米/秒（约36公里/小时），方向0度（向东）
obstacle = DynamicObstacle(Position(5, 0), 10, 0)
# 重规划时间：1秒后
replan_time = 1
# 生成候选路线：原来的路线（不绕）、绕到北边1米的路线、绕到北边2米的路线
candidate_routes = [
# 候选路线1：原来的路线
Route([
Position(0, 0),
Position(2, 0),
Position(4, 0),
Position(6, 0),
Position(8, 0),
Position(10, 0)
]),
# 候选路线2：绕到北边1米
Route([
Position(0, 0),
Position(2, 0),
Position(4, 1),
Position(6, 1),
Position(8, 0),
Position(10, 0)
]),
# 候选路线3：绕到北边2米
Route([
Position(0, 0),
Position(2, 0),
Position(4, 2),
Position(6, 2),
Position(8, 0),
Position(10, 0)
])
]
# 检查每个候选路线是否满足约束，并选择最优路线
valid_routes = []
for route in candidate_routes:
is_valid, reason = check_constraints(route, obstacle, replan_time)
print(f"候选路线检查结果：{reason}")
if is_valid:
valid_routes.append(route)
# 从有效路线中选最优的（这里选长度最短的，也就是最接近原路线的）
if valid_routes:
# 按路线长度排序，选最短的
valid_routes.sort(key=lambda r: r.total_length())
best_route = valid_routes[0]
print(f"最优路线总长度：{best_route.total_length()}米")
else:
print("没有找到满足约束的路线")
运行这个代码的结果会是：候选路线1（原路线）因为和障碍物未来位置的距离太近，违反安全约束；候选路线2和3满足所有约束，其中候选路线2更短，所以被选为最优路线。
五、技术优缺点分析
5.1 优点
第一个优点是安全性高。因为所有的约束都提前列出来，重规划的时候必须满足，不会出现违反交规或者撞车的情况。比如刚才的示例，原路线直接被排除，不会让车撞上去。
第二个优点是路线更合理。不仅绕开障碍物，还会考虑转弯角度、速度这些，车里的人坐起来更舒服，不会晃得难受。
第三个优点是鲁棒性强。不管障碍物怎么动，只要约束不变，重规划的逻辑就不会乱，不会一会儿左一会儿右来回变路线。
5.2 缺点
第一个缺点是计算量可能比较大。如果约束很多，候选路线也很多，检查每条路线的约束会花不少时间。比如城市里的复杂路况，有好几个动态障碍物，还有各种交通标志，计算起来就会很慢。
第二个缺点是约束的定义比较难。约束太松，就起不到作用；约束太严，就找不到路线。比如安全距离设太小，还是会撞；设太大，就会绕太远。
第三个缺点是对障碍物的预判要求高。如果预判的障碍物位置不准，那重规划的路线还是会有问题。比如外卖车突然减速，预判的位置就错了，路线就不安全了。
六、应用场景
这个方案最适合城市道路的自动驾驶，比如网约车、无人配送车。城市里路况复杂，动态障碍物多，交通规则也多，需要重规划的时候既安全又合理。
还有停车场的自动驾驶泊车。停车场里有其他车、行人，还有柱子这些障碍物，重规划的时候要满足不能撞车、不能压车位线这些约束。
另外，还有园区里的无人观光车。园区里有游客、其他车辆，重规划的时候要保证乘客的安全和舒适。
七、注意事项
第一，约束要动态调整。比如在高速上，安全距离要设大一点；在小区里，速度限制要低一点。不能所有场景都用一样的约束。
第二，障碍物预判要实时更新。不能只预判一次，要每隔一段时间就重新预判障碍物的位置，随时调整路线。
第三，要留有余量。比如安全距离可以比实际需要的大一点，转弯角度可以比最大允许的小一点，这样就算有一点误差，也不会出问题。
第四，要和其他系统配合。比如和导航系统配合，尽量沿着原来的路线走；和车辆控制系统配合，保证路线能被车辆正常执行。
八、文章总结
自动驾驶路径重规划是自动驾驶里非常关键的一环，以前的简单重规划逻辑已经满足不了复杂的路况。基于约束满足的再搜索方案，通过提前定义各种约束，再从候选路线里选满足约束的最优路线，解决了重规划时只绕障碍物、不考虑其他限制的问题。
这个方案的核心是“约束”和“再搜索”，约束保证了路线的安全和合法，再搜索保证了路线的最优。通过示例我们可以看到，这个方案的逻辑并不复杂，只要把约束定义清楚，就能找到合理的新路线。
当然，这个方案也有缺点，比如计算量大、约束定义难这些，未来可以通过优化算法、用更准确的障碍物预判方法来改进。总的来说，基于约束满足的再搜索方案，是动态障碍物下自动驾驶路径重规划的一个实用、有效的方法。
评论
围绕“自动驾驶路径重规划回溯：动态障碍物下基于约束满足的再搜索方案”参与讨论