Theta* 路径规划：让栅格路径不再只走 45° 折线
Theta* 路径规划：让栅格路径不再只走 45° 折线
关键词｜Any-Angle｜视线检测｜Bresenham｜父节点重连
前言
普通 A* 在 8 邻域地图里通常会得到一条“折线味”很重的路径。原因很简单：每一步只能走水平、垂直或 45° 对角。但真实机器人并不会被栅格方向限制。如果两个较远节点之间没有障碍，理论上完全可以直接走直线。Theta* 就是在 A* 的基础上加了一条很关键的规则：
只要看得见，就允许跨过中间节点直接连。
原理讲解
1. 普通 A* 的父子关系
A* 扩展当前节点 s 的邻居 s' 时，默认：
parent(s') = s
路径天然一格一格连接。
2. Theta* 会尝试“隔代连接”
假设：
parent(s) = p
Theta* 会检查：
p 能不能直接看到 s'
如果视线无障碍，并且
那么直接：
parent(s') = p
也就是说，s' 不再经过 s。
3. 为什么能得到任意角路径
一旦允许跨栅格直接连接，路径段的方向就不再局限于：
0°
45°
90°
...
只要直线穿过的自由空间没有障碍，就可以形成任意角度。这就是 any-angle path planning。
4. line-of-sight 检查
核心问题变成：
两个栅格节点之间的直线是否碰障碍？
常见方法是 Bresenham。它不需要连续几何库，而是在栅格上近似遍历两点连线经过的格子。如果沿线出现障碍，就认为不可直连。
5. 为什么启发函数通常用欧氏距离
Theta* 最终允许直线段，所以欧氏距离非常自然：
它直接对应“当前点到 goal 的直线下界”。
6. Theta* 与“A* 后处理平滑”并不是一回事
常见做法是：
先用 A* 找路径
↓
再检查哪些拐点可以删除
↓
做直线 shortcut
这种方法当然有效，但搜索阶段的代价仍然建立在原始栅格边上。Theta* 则是在搜索时就允许父节点跨越多个栅格。换句话说，父子结构本身就是 any-angle 的。这会影响：
-
每个节点的
g； -
后续节点的父节点；
-
最终搜索方向。
所以 Theta* 不只是“自动平滑 A*”，它在搜索过程中就改变了图的隐式连接关系。
7. UpdateVertex 的两种候选路径
对邻居 ，实际上比较的是两条路：
路径 1：正常经过当前节点
路径 2：直接从当前节点的父节点过来
如果父节点与 之间视线无障碍，而且：
$$g_2就采用路径 2。这就是 Theta* 一次“跨代连接”的完整数学含义。
8. LOS 检查是路径质量和计算量之间的交换
Theta* 的路径通常更自然，但并非毫无代价。A* 扩展邻居时只需：
查栅格 + 算代价
Theta* 还要沿一条较长直线检查多个栅格。因此地图非常大时，LOS 检查次数会成为明显开销。这也是 Lazy Theta* 出现的直接动机。
代码详解
1. 外层仍然是 A*
f = OPEN(:,3) + OPEN(:,4);
[~, index] = min(f);
Theta* 并没有丢掉 A*。
2. 真正变化在 update_vertex
普通 A*：
parent(child) = current;
Theta*：
if parent(current) 与 child 可直连
尝试让 child 直接继承 parent(current)
end
这一步会自动减少很多不必要拐点。
3. 视线函数的语义要看清
有些实现把函数命名为：
line_of_sight(...)
但返回 true 反而表示“有障碍”。读代码时不要只凭函数名判断，最好直接看最终逻辑：
if ~line_of_sight(...)
到底表示“可以连”还是“不可以连”。
4. Bresenham 检测的意义
相比按很小步长采样连续坐标，Bresenham 更适合栅格地图：
-
不依赖浮点采样步长
-
与障碍栅格天然一致
-
不容易漏掉细小障碍格
代价是每次跨节点连线都要检查若干格子，因此单个节点扩展比 A* 更贵。
5. 调试残留要及时清理
工程源码里偶尔会出现：
if cur_node(1)==17 && cur_node(2)==26
cur_node(1);
end
这种不改变变量的代码往往只是调试残留。二次开发时建议及时删掉，避免后面自己都忘了它为什么存在。
9. 最值得看的不是“是否成功”，而是拐点数量
同一张地图跑 A* 和 Theta* 后，可以统计：
size(path,1)
或者进一步计算方向变化次数。Theta* 的典型特征不是一定让路径长度大幅下降，而是经常能减少不必要拐点，让路径更接近自由空间中的直线连接。
完整 MATLAB 实现：theta_star.m
function [path, goal_reached, cost, EXPAND] = theta_star(map, start, goal)
% 节点格式：[x, y, g, h, px, py]。
% Theta* 在 A* 基础上尝试让邻居直接连接当前节点的父节点，
% 若两点之间视线无障碍且代价更低，就跳过当前节点形成任意角连接。
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)
% 与 A* 相同，优先展开 f=g+h 最小的候选节点
f = OPEN(:, 3) + OPEN(:, 4);
[~, index] = min(f);
cur_node = OPEN(index, :);
OPEN(index, :) = [];
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
% 先按普通 8 邻域连接构造候选节点
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(map, 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)
% 在给定列范围内查找与 node 相同的记录
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(map, node_p, node_c)
% 若父节点到候选节点之间无碰撞，则比较跨代直连是否更省代价
if ~ line_of_sight(map, 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
end
function flag = line_of_sight(map, node1, node2)
% 基于 Bresenham 思路检查两节点连线。
% 注意：本实现中 flag=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)
% 按父节点链恢复搜索得到的路径
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
总结与思考
Theta* 很像是在 A* 上加了一层“几何直觉”。A* 只知道图上的相邻边。Theta* 会额外问：
这两个点虽然不是直接邻居，但中间如果没有障碍，为什么不能直连？
这一步非常符合真实机器人运动的直觉。所以我更愿意把 Theta* 看成一种“搜索 + 路径简化一体化”的方法，而不是单纯的 A* 后处理。
如果只是在 A* 结束后做一次直线剪枝，也能让路径变平滑；但 Theta* 是在搜索过程中就改变父子关系，两者还是有本质区别。如果后续还要接局部规划器或轨迹跟踪器，Theta* 产生的低拐点路径通常也更友好。不过真正上机器人之前，仍然要考虑机器人尺寸、曲率约束和动态可行性。“路径更直”只是第一步，不等于“轨迹一定可执行”。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献5条内容
所有评论(0)