Lazy Theta*：把昂贵的视线检测留到"真正需要"时再做
关键词｜Lazy Evaluation｜Any-Angle｜延迟碰撞检测
前言
Theta* 能产生漂亮的任意角路径，但它有一个明显代价：经常做 line-of-sight 检查。问题是，OPEN 中很多候选节点最后并不会真的成为最终搜索主干。如果每生成一个候选就立刻做视线检测，其中很大一部分计算最后是白费的。Lazy Theta* 的改进非常"程序员"：
先假设这条边能走，等节点真的要被使用时再检查。
原理讲解
1. Theta* 是"提前验证"
生成邻居时：
父节点能直接看到邻居吗？能，才改父节点。
2. Lazy Theta* 是"延迟验证"
生成邻居时先乐观地认为：
应该能看见直接更新候选父节点。等该节点真正从 OPEN 中弹出时，再检查父边是否合法。
3. 如果父边不合法怎么办
发现有障碍时，不是整棵树推倒重来，而是从当前节点已经关闭的邻居里重新找一个代价最小的合法父节点。可以写成：
这一步就是"修复"。
4. 为什么可能更快
假设 OPEN 里有 1000 个候选节点。Theta* 可能对这些候选做大量视线检查。Lazy Theta* 只对真正被弹出来处理的节点做验证。如果最终只有其中 300 个真的被展开，那么就省掉了很多 LOS 计算。
5. 用"快递验货"理解 Lazy Theta*
可以把父节点直连想象成一次发货。Theta* 的做法是：
每下一单，先立刻验货；确认没问题才把订单留下。
Lazy Theta* 则是：
先把可能可行的订单记下来，等真正轮到使用时再验货。
如果很多候选订单最后根本不会被采用，那么后者显然可以少做很多无意义检查。这就是 lazy evaluation 的直觉。
6. SetVertex 修复到底在做什么
假设当前节点 s 从 OPEN 中弹出，结果发现：
parent(s) → s这条直线穿过障碍。此时 s 的父节点承诺失效。算法会在 CLOSED 邻居中寻找新的候选父节点 ：
然后重新设置：
这一步使得延迟验证失败后仍能恢复一条合法连接。
7. Theta* 和 Lazy Theta* 应该怎样选择
如果地图障碍非常密集，很多"乐观直连"最终都会失败，Lazy Theta* 可能经常触发修复。如果地图比较开阔，大量直连都能成立，那么延迟检查通常更划算。所以两者并不是绝对谁更好，而是在：
提前检查成本
vs
失败后修复成本之间做不同取舍。
代码详解
1. 生成节点时不马上检查
update_vertex 只比较代价：
candidate_cost = parent_g + distance;更短就先改父节点。
2. 弹出节点后再验证
主循环中会找到当前节点记录的父节点，然后做：
line_of_sight(parent, current)如果边无效：
cur_node(3) = inf;接着重新寻找父节点。
3. 修复为什么只看 CLOSED
CLOSED 中的节点已经有相对稳定的代价，因此从它们中找替代父节点更可靠。这其实和很多增量搜索、动态规划中的"只使用已经确认状态"思想很相似。
4. Lazy 不等于"少算一切"
Lazy Theta* 不是免费的。它少做了提前 LOS，但增加了：
-
父边失效时的修复
-
邻居重新评估
因此是否更快，和地图结构、候选节点数量有关。不过在很多场景里，LOS 是比较昂贵的操作，延迟检查通常值得。
8. 如果想验证 Lazy 是否真的省计算
最有价值的实验不是只看最终路径，而是在 line_of_sight 函数中增加一个计数器。分别统计：
Theta* 的 LOS 调用次数
Lazy Theta* 的 LOS 调用次数在路径质量接近的情况下，LOS 次数差异才真正说明 Lazy 策略是否发挥作用。
完整 MATLAB 实现：lazy_theta_star.m
function [path, goal_reached, cost, EXPAND] = lazy_theta_star(map, start, goal)
% 与 Theta* 不同，Lazy Theta* 不在生成邻居时立即验证父边；
% 它先乐观地继承父节点，等候选节点真正弹出 OPEN 后再做视线检查，
% 若父边无效，再从 CLOSED 邻居中寻找替代父节点。
OPEN = [];
CLOSED = [];
EXPAND = [];
cost = 0;
goal_reached = false;
motion = [-1, -1, sqrt(2); ...
0, -1, 1; ...
1, -1, sqrt(2); ...
-1, 0, 1; ...
1, 0, 1; ...
-1, 1, sqrt(2); ...
0, 1, 1; ...
1, 1, sqrt(2)];
motion_num = size(motion, 1);
node_s = [start, 0, h(start, goal), start];
OPEN = [OPEN; node_s];
while ~isempty(OPEN)
% 按 f=g+h 选择下一候选节点
f = OPEN(:, 3) + OPEN(:, 4);
[~, index] = min(f);
cur_node = OPEN(index, :);
OPEN(index, :) = [];
% 节点真正被取出时才验证其父节点连线
p_index = loc_list(cur_node(5: 6), CLOSED, [1, 2]);
if p_index
node_p = CLOSED(p_index, :);
if line_of_sight(map, node_p, cur_node)
% 父边失效后，从 CLOSED 中的相邻节点重新选择更优父节点
cur_node(3) = inf;
for i = 1:motion_num
node_n_x = cur_node(1) + motion(i, 1);
node_n_y = cur_node(2) + motion(i, 2);
np_index = loc_list([node_n_x, node_n_y], CLOSED, [1, 2]);
if np_index
node_n_p = CLOSED(np_index, :);
if cur_node(3) > node_n_p(3) + dist(node_n_p(1: 2), cur_node(1: 2)')
cur_node(3) = node_n_p(3) + dist(node_n_p(1: 2), cur_node(1: 2)');
cur_node(5) = node_n_x;
cur_node(6) = node_n_y;
end
end
end
end
end
if loc_list(cur_node, CLOSED, [1, 2])
continue
end
if ~loc_list(cur_node, EXPAND, [1, 2])
EXPAND = [EXPAND; cur_node(1:2)];
end
if cur_node(1) == goal(1) && cur_node(2) == goal(2)
CLOSED = [cur_node; CLOSED];
goal_reached = true;
cost = cur_node(3);
break
end
% 原实现中的调试残留，对搜索结果没有实际影响
if (cur_node(1) ==17) &&(cur_node(2) == 26)
cur_node(1);
end
for i = 1:motion_num
node_n = [
cur_node(1) + motion(i, 1), ...
cur_node(2) + motion(i, 2), ...
cur_node(3) + motion(i, 3), ...
0, ...
cur_node(1), cur_node(2)];
node_n(4) = h(node_n(1:2), goal);
if loc_list(node_n, CLOSED, [1, 2])
continue
end
if map(node_n(1), node_n(2)) == 2
continue
end
% 邻居生成时只做代价上的乐观父节点更新，不在这里检查视线
p_index = loc_list(cur_node(5: 6), CLOSED, [1, 2]);
if p_index
node_p = CLOSED(p_index, :);
else
node_p = 0;
end
if node_p ~= 0
node_n = update_vertex(node_p, node_n);
end
OPEN = [OPEN; node_n];
end
CLOSED = [cur_node; CLOSED];
end
path = extract_path(CLOSED, start);
end
%%
function h_val = h(node, goal)
% 欧氏距离启发函数
h_val = dist(node(1: 2), goal');
end
function index = loc_list(node, list, range)
% 按指定字段查找节点
num = size(list);
index = 0;
if ~num(1)
return
else
for i = 1:num(1)
if isequal(node(range), list(i, range))
index = i;
return
end
end
end
end
function node_c = update_vertex(node_p, node_c)
% 暂不做视线验证，仅比较继承父节点后的候选代价
if node_p(3) + dist(node_c(1: 2), node_p(1: 2)') <= node_c(3)
node_c(3) = node_p(3) + dist(node_c(1: 2), node_p(1: 2)');
node_c(5: 6) = node_p(1: 2);
end
end
function flag = line_of_sight(map, node1, node2)
% Bresenham 风格连线检查；本实现 true 表示碰撞，false 表示可直连
if (map(node1(1), node1(2)) == 2) || (map(node2(1), node2(2)) == 2)
flag = true;
return
end
x1 = node1(1); y1 = node1(2);
x2 = node2(1); y2 = node2(2);
d_x = abs(x2 - x1);
d_y = abs(y2 - y1);
if (x2 - x1) == 0
s_x = 0;
else
s_x = (x2 - x1) / d_x;
end
if (y2 - y1) == 0
s_y = 0;
else
s_y = (y2 - y1) / d_y;
end
x = x1; y = y1; e = 0;
if d_x > d_y
tao = (d_y - d_x) / 2;
while x ~= x2
if e > tao
x = x + s_x;
e = e - d_y;
elseif e < tao
y = y + s_y;
e = e + d_x;
else
x = x + s_x;
y = y + s_y;
e = e + d_x - d_y;
end
if map(x, y) == 2
flag = true;
return;
end
end
else
tao = (d_x - d_y) / 2;
while y ~= y2
if e > tao
y = y + s_y;
e = e - d_x;
elseif e < tao
x = x + s_x;
e = e + d_y;
else
x = x + s_x;
y = y + s_y;
e = e + d_y - d_x;
end
if map(x, y) == 2
flag = true;
return;
end
end
end
flag = false;
end
function path = extract_path(close, start)
% 根据 CLOSED 中记录的父节点逐级恢复路径
path = [];
closeNum = size(close, 1);
index = 1;
while 1
path = [path; close(index, 1:2)];
if isequal(close(index, 1:2), start)
break
end
for i = 1:closeNum
if isequal(close(i, 1:2), close(index, 5:6))
index = i;
break
end
end
end
end总结与思考
Lazy Theta* 最有意思的地方，不只是路径规划本身，而是一种很通用的工程思想：
昂贵的事情，不要太早做。
在算法设计里，这叫 lazy evaluation。类似思路还会出现在：
-
Lazy PRM
-
Lazy collision checking
-
延迟约束验证
-
按需计算代价
所以这篇文章真正值得带走的，是"什么时候检查"也可以成为算法优化的一部分。从更广的算法设计角度看，Lazy Theta* 的价值甚至超过路径规划本身：它告诉我们，验证时机也可以成为优化变量。很多计算不是不能做，而是没有必要现在就做。这个思路在更复杂的规划与优化问题里非常常见。