图搜索与 A*Graph Search & A*

仓库里的搬运机器人要从货架 A 开到打包台，扫地机器人要绕过沙发回到充电座，自动驾驶汽车要从车道开进一个车位，机械臂要从一个姿势摆到另一个姿势而不碰到桌子。它们共同的问题是运动规划：在构型空间的自由部分 \(\mathcal C_{\text{free}}\) 里，找一条从起点到终点的路径，最好还是最短、最省时或最安全的那条。

最直接的办法是把连续的空间离散化：把地图切成格子（每个空闲格是一个节点，相邻格之间连一条边，边上写着走过去的代价），问题就变成了在一张图上找最短路。6.12 节已经用值迭代在格子世界里算出过“每个格子到终点的最优代价” \(V\)，那是对所有起点一次性算完。可是机器人通常只关心一个起点：此刻它在哪。这时没必要把整张表算出来，只要从起点出发，按某种聪明的顺序往外探，探到终点就停。这种顺序的选择，就是 BFS、Dijkstra 与 A* 的区别。

A* 是 1968 年为 SRI 的移动机器人 Shakey 发明的，五十多年后仍然是 ROS 导航栈、游戏寻路和自动驾驶泊车规划的核心。本节不仅讲它为什么对、为什么快，也讲在真实机器人上用它时要面对的几件事：启发式怎么选、能不能为了速度牺牲一点最优性、栅格路径为什么歪歪扭扭、汽车不能原地转向怎么办、地图边走边变时怎么不必从头再来。

A warehouse robot must drive from shelf A to the packing station, a vacuum robot must get round the sofa back to its dock, a self-driving car must get from the lane into a parking space, an arm must swing from one posture to another without hitting the table. Their shared problem is motion planning: in the free part \(\mathcal C_{\text{free}}\) of the configuration space, find a path from start to goal, preferably the shortest, fastest or safest one.

The most direct approach is to discretize the continuous space: cut the map into cells (every free cell a node, an edge between neighbouring cells labelled with the cost of moving across), and the problem becomes a shortest path in a graph. Lesson 6.12 already computed, by value iteration on a grid world, "the optimal cost from every cell to the goal" \(V\), for all starts at once. But a robot usually cares about one start: where it is right now. Then there is no need for the whole table: start from the robot, probe outward in some clever order, and stop when the goal is reached. The choice of that order is what separates BFS, Dijkstra and A*.

A* was invented in 1968 for SRI's mobile robot Shakey, and more than fifty years later it is still at the heart of the ROS navigation stack, game path-finding and parking planners for self-driving cars. This lesson covers why it is correct and why it is fast, and also what you face when using it on a real robot: how to choose the heuristic, whether to trade a little optimality for speed, why grid paths zig-zag, what to do about a car that cannot turn on the spot, and how to avoid starting over when the map changes as you drive.

看下面这张小路网：10 个路口，路的长度就是两点间的直线距离（虚线那条是土路，同样长度要付 1.5 倍代价）。从 S 到 G 怎么走最短？

Dijkstra 算法维护两个集合。开放集（open list，也叫“前沿”）放的是“已经找到一条路能到、但还不确定是不是最短”的节点，每个节点记着目前已知的从起点出发的最小代价 \(\hl{g}{g(n)}\)；关闭集（closed list）放的是“最短代价已经确定”的节点。每一步：从开放集里取出 \(g\) 最小的节点 \(u\)，把它移进关闭集；然后看它的每个邻居 \(v\)，如果“经过 \(u\) 到 \(v\)”比 \(v\) 现有的记录更便宜，就更新 \(g(v)\) 并记下 \(v\) 的父节点是 \(u\)（这一步叫松弛）。终点被取出时，沿父节点倒推回去就是最短路。

为什么取出的那一刻 \(g(u)\) 就是最短的？因为边的代价都不为负，开放集里别的节点的 \(g\) 都不比它小，任何经过它们再绕到 \(u\) 的路只会更长。所以 Dijkstra 是按“离起点的距离”从近到远、一圈圈地把节点定下来：这正是 6.12 节值迭代的“水波”，只是从起点出发、并且每个节点只处理一次。

A* 只改了一处：取节点时不看 \(g(n)\)，而看 \(f(n)=g(n)+\hl{h}{h(n)}\)，其中 \(h(n)\) 是对“从 \(n\) 到终点还要多少”的估计，这里取到 G 的直线距离。远离目标的节点 \(h\) 大，就被往后排。下面一步步跑一遍，对比两者取出了多少个节点。

Look at the small road network below: 10 junctions, each road as long as the straight line between its ends (the dashed one is a dirt road, where the same length costs 1.5 times as much). What is the shortest way from S to G?

Dijkstra's algorithm keeps two sets. The open list (also called the frontier) holds nodes that "we have found some way to reach, but are not yet sure it is the shortest"; each remembers the smallest known cost from the start, \(\hl{g}{g(n)}\). The closed list holds nodes whose shortest cost is settled. Every step: take the node \(u\) with the smallest \(g\) out of the open list and move it to the closed list; then for each neighbour \(v\), if "reaching \(v\) through \(u\)" is cheaper than \(v\)'s current record, update \(g(v)\) and note that \(v\)'s parent is \(u\) (this is called relaxing the edge). When the goal is taken out, following the parents back gives the shortest path.

Why is \(g(u)\) already the shortest at the moment \(u\) is taken out? Because no edge cost is negative: every other open node has a \(g\) at least as large, and any route that goes through one of them and then on to \(u\) can only be longer. So Dijkstra settles nodes ring by ring, in order of distance from the start: this is exactly the "ripple" of value iteration in Lesson 6.12, but starting from the start and processing each node only once.

A* changes one thing: it picks nodes not by \(g(n)\) but by \(f(n)=g(n)+\hl{h}{h(n)}\), where \(h(n)\) estimates "how much more from \(n\) to the goal", here the straight-line distance to G. Nodes far from the goal have a large \(h\) and are pushed back in the queue. Step through both below and compare how many nodes each takes out.

现在换成机器人最常用的图：占据栅格。每个空闲格是一个节点；“4 连通”只能上下左右走（每步代价 1），“8 连通”还能走斜线（代价 \(\sqrt2\)），但斜走时不能切过墙格的角（斜向两侧的两个直邻格都必须空闲）。走进一个格子要付该格的价格：普通格 1，绿色泥地 4（和 6.12 节一样）。墙不能走。

栅格上 \(h\) 有几种常见选择，都按“最便宜的格子价格 1”来估计剩余距离，设 \(\Delta x,\Delta y\) 是到终点相差的格数：曼哈顿距离 \(\Delta x+\Delta y\)（4 连通空地图上恰好等于真实代价）；八方向距离（octile）\(\max(\Delta x,\Delta y)+(\sqrt2-1)\min(\Delta x,\Delta y)\)（8 连通空地图上恰好等于真实代价）；欧氏距离 \(\sqrt{\Delta x^2+\Delta y^2}\)（两种连通都不会高估，但比前两者更“松”）。另有两个变体：加权 A* 用 \(g+w\,h\)，\(w>1\) 时更“急于”冲向终点；贪心最佳优先只看 \(h\)，完全不管已经走了多远。

Now switch to the graph robots use most: the occupancy grid. Every free cell is a node; "4-connected" moves only up, down, left and right (cost 1 per step), "8-connected" also moves diagonally (cost \(\sqrt2\)), but a diagonal step may not cut the corner of a wall cell (both cells beside the diagonal must be free). Entering a cell costs that cell's price: 1 for an ordinary cell, 4 for green mud (as in Lesson 6.12). Walls cannot be entered.

A grid offers several common choices of \(h\), all estimating the remaining distance at the cheapest price of 1 per cell, with \(\Delta x,\Delta y\) the number of cells to the goal in each direction: the Manhattan distance \(\Delta x+\Delta y\) (exactly the true cost on an empty 4-connected map); the octile distance \(\max(\Delta x,\Delta y)+(\sqrt2-1)\min(\Delta x,\Delta y)\) (exactly the true cost on an empty 8-connected map); the Euclidean distance \(\sqrt{\Delta x^2+\Delta y^2}\) (never an overestimate for either connectivity, but looser than the other two). Two variants: weighted A* uses \(g+w\,h\), which for \(w>1\) is more "eager" to rush for the goal; greedy best-first looks only at \(h\) and ignores how far it has already come.

每个格子都有一个真实的“到终点的最小代价” \(\hl{hstar}{h^*(n)}\)：它就是 6.12 节的价值函数 \(V(n)\)，可以从终点反向跑一遍 Dijkstra 算出来。启发式 \(h\) 是它的廉价替身，好坏可以用比值 \(h/h^*\) 衡量：处处等于 1 时 A* 只展开最短路上的格子；处处等于 0（即 \(h\equiv0\)）时 A* 退化成 Dijkstra。只要 \(h\le h^*\)（称为可容许，admissible），A* 就保证最优；一旦某处 \(h>h^*\)，就可能错过最短路。

加权 A* 故意把 \(h\) 乘上 \(\hl{w}{w}>1\)，让它在很多格子上高估。好消息是代价有上界：只要原来的 \(h\) 满足一致性（第 5 节），找到的路径代价不超过 \(w\) 倍最优。下图左边按 \(w\,h/h^*\) 给每个格子着色，右边扫一遍 \(w\) 从 1 到 5，画出展开数和代价比。

Every cell has a true "minimum cost to the goal" \(\hl{hstar}{h^*(n)}\): it is the value function \(V(n)\) of Lesson 6.12, computable by running Dijkstra backward from the goal. The heuristic \(h\) is its cheap stand-in, and the ratio \(h/h^*\) measures how good it is: equal to 1 everywhere, A* expands only the cells on the shortest path; equal to 0 everywhere (\(h\equiv0\)), A* degenerates into Dijkstra. As long as \(h\le h^*\) (called admissible), A* is guaranteed optimal; as soon as \(h>h^*\) somewhere, it may miss the shortest path.

Weighted A* deliberately multiplies \(h\) by \(\hl{w}{w}>1\), so it overestimates in many cells. The good news is that the cost has a bound: as long as the original \(h\) is consistent (Section 5), the path found costs at most \(w\) times the optimum. On the left below, each cell is coloured by \(w\,h/h^*\); on the right, \(w\) is swept from 1 to 5 and the number of expansions and the cost ratio are plotted.

几点观察。(1) 在 U 形陷阱里，\(h=0\)、欧氏、八方向距离分别展开 295、132、114 个格子：\(h\) 越接近 \(h^*\)，展开越少。但 U 形开口里所有格子的 \(h\) 都远小于 \(h^*\)（直线距离穿墙而过），A* 也只好把陷阱“填满”才绕出来，这是所有只看几何距离的启发式的通病。(2) 加权 A* 的代价比往往远低于上界 \(w\)：上界是最坏情况的保证，不是典型值。(3) 曼哈顿距离在 8 连通栅格上不可容许，但大多数起点下它仍然碰巧给出最短路；“不可容许”说的是失去保证，不是每次都错。

Some observations. (1) In the U-trap, \(h=0\), Euclidean and octile expand 295, 132 and 114 cells: the closer \(h\) is to \(h^*\), the fewer expansions. But every cell inside the U has \(h\) far below \(h^*\) (the straight line goes through the wall), so A* too must "fill" the trap before it escapes; every heuristic based only on geometric distance suffers from this. (2) The cost ratio of weighted A* is usually far below the bound \(w\): the bound is a worst-case guarantee, not a typical value. (3) Manhattan is inadmissible on an 8-connected grid, yet from most starts it still happens to return the shortest path; "inadmissible" means losing the guarantee, not being wrong every time.

栅格上的“最短路”只是在允许的移动方向里最短。4 连通只能走 0°、90°……，一条 45° 方向的直线要走成阶梯，长度是 \(\hl{c4}{\Delta x+\Delta y}\)，比直线长 \(\sqrt2-1\approx41\%\)。8 连通加上了斜线，长度变成 \(\hl{c8}{\max+(\sqrt2-1)\min}\)，最坏出现在 22.5° 方向，长 8.2%。16 连通再加上“马步” \((2,1)\) 一类的移动，最坏只长 2.7%。更麻烦的是形状：同样代价的阶梯路径有很多条，哪条被选中取决于平局怎么打破，结果往往是不自然的折线，贴着墙角拐弯。

任意角度（any-angle）规划不再把路径限制在栅格边上。Theta*（Nash 等，2007）在展开节点 \(u\)、更新它的邻居 \(v\) 时，检查 \(v\) 和 \(u\) 的父节点之间有没有直线视线，有就让 \(v\) 直接连到那个父节点上（跳过 \(u\)），于是路径的拐点只出现在障碍的角上。它不保证是真正的欧氏最短路，但通常非常接近。

A "shortest path" on a grid is only the shortest among the allowed move directions. 4-connected moves go only at 0°, 90°, …, so a straight line at 45° becomes a staircase of length \(\hl{c4}{\Delta x+\Delta y}\), \(\sqrt2-1\approx41\%\) longer than the line. 8-connected adds diagonals and the length becomes \(\hl{c8}{\max+(\sqrt2-1)\min}\); the worst case is the 22.5° direction, 8.2% longer. 16-connected adds "knight" moves such as \((2,1)\), and the worst case is only 2.7%. Worse is the shape: many staircase paths have exactly the same cost, which one wins depends on how ties are broken, and the result is often an unnatural polyline that turns while hugging wall corners.

Any-angle planning no longer restricts the path to grid edges. When Theta* (Nash et al., 2007) expands a node \(u\) and updates a neighbour \(v\), it checks whether there is a clear line of sight between \(v\) and \(u\)'s parent, and if so connects \(v\) directly to that parent (skipping \(u\)), so the path only bends at obstacle corners. It is not guaranteed to be the true Euclidean shortest path, but it is usually very close.

图 \(\mathcal G=(\mathcal V,\mathcal E)\)，每条边有非负代价 \(c(u,v)\ge0\)。一条路径的代价是它各边代价之和，\(C^*\) 是从起点 \(s\) 到终点 \(t\) 所有路径中的最小代价。记 \(g^*(n)\) 为从 \(s\) 到 \(n\) 的最小代价，\(\hl{hstar}{h^*(n)}\) 为从 \(n\) 到 \(t\) 的最小代价，它们各满足一个贝尔曼方程：

A graph \(\mathcal G=(\mathcal V,\mathcal E)\) with a non-negative cost \(c(u,v)\ge0\) on every edge. The cost of a path is the sum of its edge costs, and \(C^*\) is the minimum over all paths from the start \(s\) to the goal \(t\). Let \(g^*(n)\) be the minimum cost from \(s\) to \(n\) and \(\hl{hstar}{h^*(n)}\) the minimum cost from \(n\) to \(t\); each satisfies a Bellman equation:

第二个方程正是 6.12 节的 \(V\)（\(\gamma=1\) 的最短路问题）。最后一个等式说：经过最短路上任意一点 \(n\) 的 \(g^*+h^*\) 都等于 \(C^*\)，不在任何最短路上的点则严格大于 \(C^*\)。A* 的全部思想，就是用一个可计算的 \(h\) 去近似这里的 \(h^*\)。

The second equation is exactly \(V\) of Lesson 6.12 (the shortest-path problem with \(\gamma=1\)). The last identity says that \(g^*+h^*\) equals \(C^*\) at every node \(n\) on a shortest path and is strictly larger at nodes on no shortest path. The whole idea of A* is to approximate this \(h^*\) by a computable \(h\).

把起点放进开放集 \(\hl{open}{\mathcal O}\)，\(g(s)=0\)，其余 \(g=\infty\)。重复：从 \(\mathcal O\) 取出排序键最小的 \(u\)，放进关闭集 \(\hl{closed}{\mathcal K}\)；若 \(u=t\) 则沿父指针返回路径；否则对每个邻居 \(v\notin\mathcal K\)，若 \(g(u)+c(u,v)<g(v)\)，令 \(g(v)\leftarrow g(u)+c(u,v)\)、父节点 \(\pi(v)\leftarrow u\)，并把 \(v\) 以新的键放进 \(\mathcal O\)。排序键：

Put the start into the open list \(\hl{open}{\mathcal O}\) with \(g(s)=0\) and \(g=\infty\) elsewhere. Repeat: take the \(u\) with the smallest key out of \(\mathcal O\) and put it into the closed list \(\hl{closed}{\mathcal K}\); if \(u=t\), return the path by following parents; otherwise for every neighbour \(v\notin\mathcal K\), if \(g(u)+c(u,v)<g(v)\), set \(g(v)\leftarrow g(u)+c(u,v)\), the parent \(\pi(v)\leftarrow u\), and put \(v\) into \(\mathcal O\) with its new key. The keys:

开放集用二叉堆实现，复杂度为 \(O\big((|\mathcal V|+|\mathcal E|)\log|\mathcal V|\big)\)；栅格上每个节点至多 8 条边。BFS 用普通队列，\(O(|\mathcal V|+|\mathcal E|)\)，但只在所有边代价相同时最优（FIG 2 的泥地）。实现上通常不做“降低堆中键值”，而是把新键再压一次堆，取出时跳过过期的条目（惰性删除）。

With a binary heap for the open list the cost is \(O\big((|\mathcal V|+|\mathcal E|)\log|\mathcal V|\big)\); on a grid every node has at most 8 edges. BFS uses a plain queue, \(O(|\mathcal V|+|\mathcal E|)\), but is optimal only when all edge costs are equal (the mud of FIG 2). Implementations usually do not "decrease a key in the heap"; they push the node again with the new key and skip stale entries when they come out (lazy deletion).

(1) 是可容许，(2) 是一致。一致（也叫单调）是边上的三角不等式：估计值走一步最多下降这一步的代价。沿着从 \(n\) 到 \(t\) 的最短路把它加起来，就得到 \(h(n)\le h^*(n)\)：一致蕴含可容许。直线距离、八方向距离（8 连通）、曼哈顿距离（4 连通）都是一致的。

理解一致性最干净的方式是约化代价（reduced cost）。定义

(1) is admissibility, (2) is consistency. Consistency (also called monotonicity) is a triangle inequality on edges: the estimate drops by at most the cost of a step. Summing it along a shortest path from \(n\) to \(t\) gives \(h(n)\le h^*(n)\): consistent implies admissible. The straight-line distance, the octile distance (8-connected) and the Manhattan distance (4-connected) are all consistent.

The cleanest way to understand consistency is through reduced costs. Define

任何一条从 \(s\) 出发的路径 \(P_{s\to n}\)，其约化代价只是原代价减去常数 \(h(s)\) 再加上终点的 \(h(n)\)，所以到 \(t\) 的路径谁短谁长完全不变（\(h(t)=0\)），而 Dijkstra 在约化代价上的键 \(\tilde g(n)=g(n)+h(n)-h(s)=f(n)-h(s)\)，与 A* 的 \(f\) 只差一个常数。结论：一致启发式下的 A* 就是边代价为 \(\tilde c\ge0\) 的 Dijkstra。Dijkstra 的一切性质随之而来：节点被取出时 \(g\) 已是最优，关闭的节点不必重开，取出的 \(f\) 值单调不减，取出 \(t\) 时路径最优。只可容许、不一致的 \(h\) 也能保证最优，但需要允许重开关闭集里的节点。

A* 展开哪些节点。由于 \(f\) 单调不减，所有 \(g^*(n)+h(n)<C^*\) 的节点在 \(t\) 之前必然被展开，\(f>C^*\) 的节点永远不会。所以 \(h\) 越大（越接近 \(h^*\)），这个集合越小：若 \(h_1\ge h_2\) 且都一致，用 \(h_1\) 展开的节点（除平局外）是用 \(h_2\) 的子集。两个极端：\(h\equiv0\) 时是 Dijkstra；\(h=h^*\) 时 \(f\) 在最短路上处处等于 \(C^*\)，配合“平局取 \(g\) 大”的规则，A* 只展开最短路上的节点。这也是“学习一个启发式”有意义的原因：价值函数本身就是最好的启发式。

加权 A* 的上界。令 \(f_w=g+w\,h\)。取出 \(t\) 的那一刻，最短路上总有某个节点 \(n\) 在开放集里且 \(g(n)=g^*(n)\)，于是

For any path \(P_{s\to n}\) from \(s\), the reduced cost is the original cost minus the constant \(h(s)\) plus \(h(n)\) of its endpoint, so the ranking of paths to \(t\) is unchanged (\(h(t)=0\)), and Dijkstra's key on reduced costs, \(\tilde g(n)=g(n)+h(n)-h(s)=f(n)-h(s)\), differs from A*'s \(f\) by a constant. Conclusion: A* with a consistent heuristic is Dijkstra on edge costs \(\tilde c\ge0\). Every property of Dijkstra follows: \(g\) is optimal when a node is taken out, closed nodes never need reopening, the \(f\) values taken out never decrease, and the path is optimal when \(t\) is taken out. An admissible but inconsistent \(h\) also guarantees optimality, but closed nodes must then be allowed to reopen.

Which nodes A* expands. Because \(f\) never decreases, every node with \(g^*(n)+h(n)<C^*\) is certainly expanded before \(t\), and no node with \(f>C^*\) ever is. So the larger \(h\) (the closer to \(h^*\)), the smaller this set: if \(h_1\ge h_2\) are both consistent, the nodes expanded with \(h_1\) are (up to ties) a subset of those expanded with \(h_2\). The two extremes: \(h\equiv0\) is Dijkstra; with \(h=h^*\), \(f\) equals \(C^*\) all along the shortest path and, with ties broken toward the larger \(g\), A* expands only nodes on the shortest path. This is also why "learning a heuristic" makes sense: the value function itself is the best heuristic.

The bound of weighted A*. Let \(f_w=g+w\,h\). At the moment \(t\) is taken out, some node \(n\) of a shortest path is in the open list with \(g(n)=g^*(n)\), so

这一推导在允许重开节点时直接成立；Likhachev 等（2003）证明了 \(h\) 一致时即使不重开也成立（FIG 2、FIG 3 都不重开）。所以 \(w\) 是一个旋钮：用至多 \(w\) 倍的代价，换取通常少得多的展开。ARA*（随时修复 A*）从大 \(w\) 开始，先快速给出一条路，再逐步降低 \(w\) 并复用之前的搜索，时间允许就一直改进。

The derivation holds directly when nodes may be reopened; Likhachev et al. (2003) showed it also holds without reopening when \(h\) is consistent (FIG 2 and FIG 3 do not reopen). So \(w\) is a dial: accept at most \(w\) times the cost in exchange for usually far fewer expansions. ARA* (Anytime Repairing A*) starts with a large \(w\) to return a path quickly, then lowers \(w\) step by step while reusing the previous search, improving the path for as long as time allows.

9.1 节的运动学自行车模型：后轴中心 \((x,y)\)、航向 \(\theta\)，

The kinematic bicycle model of Lesson 9.1: rear-axle centre \((x,y)\), heading \(\theta\),

在 \((x,y)\) 栅格上，8 连通的“边”要求原地转向，车做不到。办法是把图的节点换成 \((x,y,\theta)\)，边换成运动基元：在曲率约束下实际能开出来的短轨迹（直行、左转弧、右转弧，前进或倒车）。

• 状态格（state lattice，Pivtoraiko 与 Kelly）：航向只取有限几个值（FIG 5 用 8 个，每 45° 一个），每个基元都精心设计成从一个格点出发、恰好落在另一个格点上（FIG 5 的转弯基元是半径 \(R=2+2\sqrt2\approx4.83\) m 的 45° 圆弧加 0.83 m 直线，正好从 \((0,0,0°)\) 到 \((4,2,45°)\)）。它仍是一张普通的图，A* 的最优性在这张图上成立；代价是表达能力受基元集合限制。
• Hybrid A*（Dolgov 等）：节点里存连续的位姿，基元直接从这个连续位姿积分出去，只用 \((x,y,\theta)\) 的栅格单元来去重：同一个单元里只保留代价最小的那个连续状态。这样每条边都严格可行，但同一单元里被丢掉的状态可能正是通往终点所需要的，所以它既不完备也不最优，只是“分辨率够细时很好用”。真实实现在接近终点时还会尝试一条 Reeds–Shepp 曲线直接解析地连到目标位姿（解析扩展），FIG 5 为简洁省略了这一步，改为允许到达目标附近 0.8 m、航向差 0.25 rad（约 14°）以内。

启发式取两个估计的较大者（Dolgov 等称两者都可容许）：考虑运动学、忽略障碍（无障碍时的 Reeds–Shepp 最短长度，可以离线制表），以及考虑障碍、忽略运动学（在二维栅格上从终点跑一次 Dijkstra）。前者在开阔处管用（车需要掉头时直线距离严重低估，9.1 节已经指出），后者把搜索从死胡同里拉出来。FIG 5 只实现了后者与直线距离的较大者。注意二维栅格距离可能比连续空间的真实距离略长（FIG 4：最多 8%），严格说并不可容许；FIG 5 因此把它减去 1.5 m 的余量再用，实践中这点误差通常被接受。

On an \((x,y)\) grid, the 8-connected "edges" demand turning on the spot, which a car cannot do. The cure is to make the graph's nodes \((x,y,\theta)\) and its edges motion primitives: short trajectories the car can actually drive under the curvature limit (straight, left arc, right arc, forward or reverse).

• State lattice (Pivtoraiko and Kelly): the heading takes only a few values (8 in FIG 5, one every 45°), and every primitive is designed to start on a lattice point and land exactly on another (FIG 5's turning primitive is a 45° arc of radius \(R=2+2\sqrt2\approx4.83\) m plus 0.83 m of straight line, taking \((0,0,0°)\) exactly to \((4,2,45°)\)). It is still an ordinary graph, and A*'s optimality holds on it; the price is that what it can express is limited by the primitive set.
• Hybrid A* (Dolgov et al.): nodes store a continuous pose, primitives are integrated directly from that pose, and the \((x,y,\theta)\) grid cells are used only to prune duplicates: each cell keeps only the cheapest continuous state that reached it. Every edge is then exactly drivable, but a state discarded in a cell may be just the one needed to reach the goal, so the method is neither complete nor optimal, merely "very good when the resolution is fine enough". Real implementations also try, near the goal, a Reeds–Shepp curve that connects analytically to the exact goal pose (the analytic expansion); FIG 5 omits this for brevity and instead accepts arriving within 0.8 m and 0.25 rad (about 14°) of the goal.

The heuristic is the larger of two estimates (Dolgov et al. call both admissible): with kinematics, without obstacles (the obstacle-free Reeds–Shepp length, which can be tabulated offline), and with obstacles, without kinematics (one Dijkstra run from the goal on a 2D grid). The first helps in the open (where the car must turn around and the straight-line distance badly underestimates, as Lesson 9.1 pointed out), the second pulls the search out of dead ends. FIG 5 implements only the larger of the latter and the straight-line distance. Note that a 2D grid distance can be slightly longer than the true continuous distance (FIG 4: by up to 8%), so strictly speaking it is not admissible; FIG 5 therefore subtracts a 1.5 m margin from it, and in practice this small error is usually accepted.

机器人边走边用传感器发现新障碍，每发现一次，图上就有几条边的代价变成 \(\infty\)。从头跑一遍 A* 当然可以，但大部分旧结果其实仍然有效。D* Lite（Koenig 与 Likhachev，2002）的做法：从终点向机器人方向搜索，于是 \(g(s)\) 是“从 \(s\) 到终点”的代价，机器人移动时终点不变，这些值不会作废；再给每个节点加一个一步前瞻值

The robot discovers new obstacles with its sensors as it drives, and every discovery turns the cost of a few edges to \(\infty\). Re-running A* from scratch works, but most of the old result is still valid. D* Lite (Koenig and Likhachev, 2002) searches from the goal toward the robot, so \(g(s)\) is the cost "from \(s\) to the goal"; the goal does not move when the robot does, so these values stay meaningful. It then gives every node a one-step lookahead value

\(g=\mathrm{rhs}\) 的节点叫局部一致；边代价一变，只有它两端的节点变得不一致，被放进队列。队列按两段键 \(k\) 处理不一致的节点，直到机器人所在节点一致且队列中再没有键更小的节点：效果上等价于“在新地图上重新算出了需要的那部分 \(g\)”，但只碰受影响的区域。\(h(s_{\text{start}},\cdot)\) 让修补朝机器人聚焦（像 A*），机器人每走一段，\(k_m\) 加上 \(h(s_{\text{last}},s_{\text{start}})\)，使队列里旧的键仍然是下界，不必重排整个队列。D* Lite 的路径与在当前已知地图上重新跑 A* 的代价完全相同（我们在 FIG 6 两张地图、半径 2 到 6 的每一次重规划上都核对过），省的只是计算。

Nodes with \(g=\mathrm{rhs}\) are called locally consistent; when an edge cost changes, only the nodes at its ends become inconsistent and enter the queue. The queue processes inconsistent nodes by the two-part key \(k\) until the robot's node is consistent and no smaller key remains: in effect "recomputing the needed part of \(g\) on the new map", while touching only the affected region. \(h(s_{\text{start}},\cdot)\) focuses the repair toward the robot (as in A*), and each time the robot moves, \(k_m\) grows by \(h(s_{\text{last}},s_{\text{start}})\) so that the old keys in the queue remain lower bounds without re-sorting the whole queue. D* Lite's path has exactly the same cost as re-running A* on the currently known map (we checked this at every replan of FIG 6, on both maps, for radii 2 to 6); what it saves is only computation.

下图是一辆 4.0 m × 1.8 m 的车，最小转弯半径 4.83 m，状态是后轴中心的 \((x,y,\theta)\)。车身用沿中心线的 3 个圆盘近似做碰撞检测（这是常见做法：圆盘到障碍的距离很便宜）。Hybrid A* 每次展开生成 5 种曲率 \(\times\) 前进/倒车的 1.6 m 弧，用 1 m × 1 m × 5°（72 个航向）的单元去重；代价是路程，倒车的路程乘 2，每次换挡加 3 m，打方向的变化也有小惩罚。

Below is a 4.0 m × 1.8 m car with a 4.83 m minimum turning radius, its state the rear-axle centre \((x,y,\theta)\). For collision checking the body is approximated by 3 discs along its centre line (a common trick: disc-to-obstacle distances are cheap). Each Hybrid A* expansion generates 1.6 m arcs at 5 curvatures \(\times\) forward/reverse and prunes duplicates in 1 m × 1 m × 5° cells (72 headings); the cost is distance travelled, with reverse distance counted double, 3 m added for every change of direction and a small penalty for changing the steering.

几个细节。(1) 换成“状态格”：停车场仍能停进去，但要倒车调整两次，代价约为 Hybrid A* 的两倍；死胡同里要换向 5 次，代价 66.6，是 Hybrid A* 的 33.9 的近两倍；侧方停车干脆无从下手：目标位姿吸附到最近的整数格点 \((10,1)\) 后，车身压到了场地边界，读数显示“起点或目标与障碍相撞”。原因是 8 个航向、整数格点、4 m 长的转弯基元太粗了，换成更密的航向和更短的基元才行。基元集合的设计就是状态格规划器的全部功夫。(2) Hybrid A* 的路径还要后处理：实际系统会再做平滑（Dolgov 等用了带障碍项的共轭梯度优化，与 6.13、14.3 节的轨迹优化同类），再交给 9.1 节的路径跟踪控制器。(3) 打开“显示栅格 A*”比一比：点机器人的路径更短，却是车开不出的折线。

A few details. (1) Switch to "State lattice": it still gets into the parking space, but with two reversing corrections at about twice the cost of Hybrid A*; in the dead end it needs 5 changes of direction at a cost of 66.6, nearly twice Hybrid A*'s 33.9; the parallel park cannot even start: snapped to the nearest integer lattice point \((10,1)\), the goal pose overlaps the edge of the site, and the readout reports that the start or goal pose collides. The reason is that 8 headings, integer lattice points and 4 m turning primitives are too coarse; denser headings and shorter primitives would be needed. Designing the primitive set is the whole craft of a lattice planner. (2) A Hybrid A* path still needs post-processing: real systems smooth it (Dolgov et al. used conjugate-gradient optimization with an obstacle term, the same family as the trajectory optimization of Lessons 6.13 and 14.3) before handing it to a path-tracking controller like those of Lesson 9.1. (3) Turn on "Show grid A*" and compare: the point-robot path is shorter, but it is a polyline the car cannot drive.

下图的机器人一开始不知道墙在哪，只假设未知的地方都能走（乐观假设）。它每走一格，用半径为 \(r\) 的传感器看一圈，发现新墙就重规划。同一时刻用同样的已知地图做两件事：D* Lite 增量修补，以及从机器人当前位置从头跑一次 A*，比较两者展开的节点数。

The robot below starts without knowing where the walls are and assumes every unknown cell is free (the optimistic assumption). After every step it looks around with a sensor of radius \(r\), and replans whenever it finds a new wall. At the same moments and on the same known map two things are done: D* Lite repairs incrementally, and A* runs from scratch from the robot's current cell; their expansion counts are compared.

在迷宫里（传感器半径 3），D* Lite 的累计展开只有从头 A* 的约三分之一（943 对 2767）；半径越大，一次看到的墙越多，优势越小（半径 6 时约一半）。原因是：迷宫里直线距离启发式很差，从头 A* 每次都要重新填满大片走廊，而墙总是在机器人身边被发现，D* Lite 只需修补靠近机器人的一小块。在“房间”地图上差别就小得多，甚至可能反过来（半径 3 时 326 对 300）：开阔地上 A* 本来就只展开一条窄带，D* Lite 每次修补还要付出维护 \(\mathrm{rhs}\) 和队列的开销。增量方法省多少，取决于变化发生在哪里、启发式有多好。还要注意第一次规划：迷宫里（半径 3）D* Lite 展开 350 个节点，A* 只展开 33 个。在还几乎全是未知（当作空闲）的地图上，许多格子的 \(f\) 相同；D* Lite 的第二段键取 \(g\) 小的先处理，会把这些平局的格子都处理一遍，而这里的 A* 平局时取 \(g\) 大的。D* Lite 的优势要在之后多次重规划里才体现出来。

In the maze (sensor radius 3), D* Lite's total expansions are only about a third of A*-from-scratch's (943 versus 2767); a larger radius sees more walls at once and shrinks the advantage (about half at radius 6). The reason: the straight-line heuristic is poor in a maze, so every fresh A* has to refill large stretches of corridor, while walls are always discovered next to the robot and D* Lite only repairs a small patch near it. On the "Rooms" map the difference is much smaller and can even reverse (326 versus 300 at radius 3): in open space A* expands only a narrow band anyway, and every D* Lite repair also pays for maintaining \(\mathrm{rhs}\) and its queue. How much an incremental method saves depends on where the changes happen and how good the heuristic is. Note also the first plan: in the maze (radius 3) D* Lite expands 350 nodes and A* only 33. On a map that is still almost entirely unknown (treated as free), many cells share the same \(f\); D* Lite's second key component processes the smaller \(g\) first and so works through all those ties, whereas the A* here breaks ties toward the larger \(g\). D* Lite's advantage only shows over the many replans that follow.

接下来：图搜索在二维、三维里无可替代，但到了机械臂的 6、7 维关节空间就撑不住了。14.2 节改为在 \(\mathcal C\) 里随机撒点、用碰撞检测连线（PRM、RRT），14.3 节把图搜索或采样得到的折线优化成平滑、可执行的轨迹，15.3 节讲规划器所用的代价地图从哪里来。

What comes next: graph search is irreplaceable in two and three dimensions, but it breaks down in the 6- or 7-dimensional joint space of an arm. Lesson 14.2 instead scatters random samples in \(\mathcal C\) and connects them with collision checks (PRM, RRT), Lesson 14.3 optimizes the polyline from search or sampling into a smooth, executable trajectory, and Lesson 15.3 covers where the planner's costmaps come from.