4. 最近邻搜索：KD-Tree原理、构建与查询、在路径规划中的应用
好，咱们进入采样规划里一个绕不开的核心问题——最近邻搜索。
你想想看，不管是RRT还是PRM，每一步都在干同一件事：找一个离我最近的节点。RRT要找个最近的树节点去生长，PRM要找个最近的邻居来连边。如果每次都用暴力遍历，那复杂度就是O(n)，节点一多，几千几万个，直接卡死。
所以，我们需要一个数据结构，能快速回答：“给定一个点，谁离我最近？”
KD-Tree就是干这个的。我个人习惯叫它“空间二分树”，说白了就是把空间一层层切下去，切到每个小区域里只装几个点为止。
4.1 为什么不用暴力搜索？
我刚开始做路径规划时，也偷懒过。节点才几百个，暴力搜索完全够用。但后来场景复杂了，地图大了，节点数轻松破万。有一次跑RRT*，每次扩展都要找最近邻，暴力搜索让整个程序像蜗牛爬。我盯着终端等了快一分钟，才意识到——不优化不行了。
暴力搜索的时间复杂度是O(n)，而KD-Tree可以把平均查询降到O(log n)。在路径规划里，这个差距就是“能不能实时”的分水岭。
4.2 KD-Tree原理：空间二分法
KD-Tree的原理其实不复杂。你想象一下，我们有一堆二维点，散落在平面上。怎么快速找到某个点的邻居？
思路是：把空间切成两半，再切，再切，直到每个小区域里只有几个点。
具体怎么切？
- 第一层：按x坐标排序，取中位数，画一条竖线，把点分成左右两堆。
- 第二层：左右两堆各自按y坐标排序，取中位数，画横线，再分。
- 第三层：再按x坐标……如此交替下去。
为什么交替？因为这样能保证每个维度都被考虑到，空间被均匀分割。如果一直按x切，那y方向的信息就浪费了。
4.3 构建KD-Tree：递归切分
构建过程就是递归。我给你看一段伪代码，这是我常用的写法：
function build_kdtree(points, depth):
if points 为空:
return None
// 根据深度决定按哪个维度切
axis = depth % k // k是维度数，2D时k=2
// 按axis排序，取中位数
sort(points, by=axis)
median_index = len(points) // 2
median_point = points[median_index]
// 递归构建左右子树
node = new Node()
node.point = median_point
node.left = build_kdtree(points[0:median_index], depth+1)
node.right = build_kdtree(points[median_index+1:], depth+1)
return node嗯，这里要注意：取中位数是关键。如果每次取中位数，树就是平衡的，查询效率最高。如果随便取一个点，树可能歪歪扭扭，最坏情况退化成链表，那就白费功夫了。
4.4 查询最近邻：剪枝的艺术
查询是KD-Tree的精髓。给定一个目标点，怎么找到最近的？
步骤是这样的：
- 从根节点开始，根据目标点的坐标，决定往左子树还是右子树走（比如当前层按x切，目标点x小于节点x，就走左边）。
- 一直走到叶子节点，记录当前最近距离。
- 然后回溯。回溯时检查：另一侧子树有没有可能包含更近的点？
- 怎么判断？看目标点到分割超平面的距离。如果这个距离小于当前最近距离，那另一侧可能有更近的点，必须进去搜。否则，直接剪掉。
说白了，就是用空间分割来排除不可能的区域。这个剪枝操作，让平均复杂度降到了O(log n)。
伪代码长这样：
function nearest_search(node, target, best):
if node is None:
return best
// 先更新当前节点
dist = distance(node.point, target)
if dist < best.dist:
best.point = node.point
best.dist = dist
// 决定先搜哪一侧
axis = node.axis
if target[axis] < node.point[axis]:
first = node.left
second = node.right
else:
first = node.right
second = node.left
// 先搜优先侧
nearest_search(first, target, best)
// 检查是否需要搜另一侧
if abs(target[axis] - node.point[axis]) < best.dist:
nearest_search(second, target, best)
return best|target[axis] - node.point[axis]| < best.dist。如果这个条件不成立，另一侧的所有点都不可能更近，直接跳过。这就是KD-Tree快的原因。
4.5 在路径规划中的应用
KD-Tree在路径规划里，主要用在两个地方：
| 算法 | 用途 | 说明 |
|---|---|---|
| RRT / RRT* | 找最近树节点 | 每次扩展时，从随机点找最近的树节点，然后朝它生长。RRT*还需要找半径内的所有邻居，KD-Tree也能支持范围查询。 |
| PRM | 找近邻节点连边 | 采样完所有点后，每个点要找到k个最近邻居来连边。KD-Tree可以快速完成这个任务。 |
| Informed RRT* | 椭球内采样 | 在椭球内采样时，需要快速判断点是否在椭球内，以及找椭球内的节点。KD-Tree可以加速这个判断。 |
我记得有一次做RRT*的优化，节点数到了5000，每次扩展都要找最近邻和半径邻居。用暴力搜索，一次扩展要几十毫秒，整个规划下来要几十秒。换成KD-Tree后，一次扩展降到几毫秒，总时间不到1秒。这个差距，你感受一下。
4.6 注意事项与扩展
KD-Tree不是万能的。有几个坑，我踩过，你注意一下：
- 高维诅咒： 维度超过10，KD-Tree的性能会急剧下降，甚至不如暴力搜索。路径规划里通常只有2D或3D，所以问题不大。但如果你做机械臂的C空间规划，维度可能很高，那时候要考虑其他方法，比如哈希网格或VP-Tree。
- 动态更新： RRT是动态添加节点的。标准的KD-Tree不支持动态插入（会破坏平衡）。我的做法是：批量构建。比如每插入100个节点，重新构建一次KD-Tree。或者用更高级的“增量式KD-Tree”，但实现起来复杂一些。
- 内存占用： KD-Tree每个节点只存一个点，内存开销不大。但如果你存了所有节点，还要存树结构，注意不要重复存储。
好了，KD-Tree就讲到这里。下一节我们聊聊更高级的搜索策略——双向RRT和RRT*的优化技巧。到时候你会发现，KD-Tree是这些高级算法的基础设施，没有它，很多优化都跑不起来。