2. 欧几里得距离代价：二维与三维空间中的欧氏距离计算，及其在网格地图与连续空间中的应用
大家好，欢迎来到路径规划代价函数设计的第二讲。
今天咱们聊聊最基础、也最常用的代价函数——欧几里得距离。说白了，就是咱们初中就学过的「两点间直线距离」。但在机器人路径规划里，这个简单的公式背后，藏着不少门道。
2.1 欧氏距离的数学本质
先回顾一下公式。二维空间里，点 A(x₁, y₁) 到点 B(x₂, y₂) 的欧氏距离是：
d = √[(x₂ - x₁)² + (y₂ - y₁)²]三维空间里，再加一个 z 轴：
d = √[(x₂ - x₁)² + (y₂ - y₁)² + (z₂ - z₁)²]嗯，就这么简单。但为什么它这么重要？
因为欧氏距离反映的是「理想世界」中的最短路径。没有障碍物，没有地形限制，纯粹的空间几何关系。我经常跟团队说，欧氏距离是代价函数的「锚点」——所有其他代价，都是在这个基础上做加减法。
核心认知：欧氏距离代价代表的是「理论最优值」。任何实际路径的代价，都不可能低于这个值。它是路径规划的下界。
2.2 二维空间中的欧氏距离计算
二维场景最常见，比如扫地机器人、仓储AGV。代码实现很简单：
# 二维欧氏距离
import math
def euclidean_2d(x1, y1, x2, y2):
dx = x2 - x1
dy = y2 - y1
return math.sqrt(dx*dx + dy*dy)
# 或者用 numpy
import numpy as np
def euclidean_2d_np(p1, p2):
return np.linalg.norm(np.array(p1) - np.array(p2))我个人习惯用 numpy 版本，代码更简洁。但要注意，在嵌入式系统或实时性要求高的场景，math.sqrt 的开销不可忽视。我曾经在一个项目中，因为频繁调用 sqrt，导致规划周期从 10ms 飙升到 50ms。后来改用平方距离比较，性能直接翻倍。
小技巧：如果只是比较距离大小，不需要开平方。直接比较平方距离即可。比如判断哪个点更近，用 dx² + dy² 就够了。
2.3 三维空间中的欧氏距离计算
三维场景多用于无人机、机械臂、水下机器人。公式只是多了一个维度：
def euclidean_3d(x1, y1, z1, x2, y2, z2):
dx = x2 - x1
dy = y2 - y1
dz = z2 - z1
return math.sqrt(dx*dx + dy*dy + dz*dz)你想想看，三维空间里欧氏距离的意义是什么？它代表无人机从 A 点飞到 B 点的「直线航程」。但实际中，无人机有最大俯仰角限制，不能直接走直线。这时候欧氏距离就变成了一个「理想参考值」。
我记得有一次做无人机巡检项目，客户要求路径必须是最短。我直接用欧氏距离做代价，结果规划出来的路径无人机根本飞不了——因为要垂直爬升。后来我加了高度变化惩罚项，才解决问题。这个后面章节会细讲。
2.4 网格地图中的欧氏距离
网格地图是路径规划的老朋友了。A* 算法、Dijkstra 算法都在网格上跑。但网格地图里用欧氏距离，有个坑。
先看代码：
# 网格地图中，计算两个格子中心的欧氏距离
def grid_euclidean(cell1, cell2, resolution=1.0):
# cell1, cell2 是格子坐标 (row, col)
x1 = cell1[1] * resolution # 列 -> x
y1 = cell1[0] * resolution # 行 -> y
x2 = cell2[1] * resolution
y2 = cell2[0] * resolution
return math.sqrt((x2-x1)**2 + (y2-y1)**2)这里要注意 resolution 参数。如果网格分辨率是 0.1 米，那格子坐标差 1 代表实际 0.1 米。不乘分辨率，算出来的距离就是错的。
避坑指南：我曾经在调试 A* 算法时，发现规划的路径总是偏长。查了半天，发现是网格距离计算时忘了乘分辨率。结果启发式函数比实际代价小太多，导致算法退化成 Dijkstra。嗯，这种低级错误，犯过一次就记住了。
网格地图中，欧氏距离通常用作 A* 的启发式函数。它比曼哈顿距离更准确，但计算开销也更大。对于 8 连通网格（可以斜着走），欧氏距离是「可采纳的」——也就是说，它永远不会高估实际代价。这是 A* 算法最优性的保证。
2.5 连续空间中的欧氏距离
连续空间里，欧氏距离就是「直线距离」。但实际路径规划中，我们很少直接用欧氏距离作为最终代价。为什么？
因为连续空间里，机器人有运动学约束。差速轮不能横着走，阿克曼底盘有最小转弯半径，无人机不能急转弯。欧氏距离只关心「位置」，不关心「姿态」和「运动能力」。
那欧氏距离在连续空间里有什么用？
- 作为启发式函数：在 RRT*、PRM 等采样算法中，欧氏距离用来估计「从当前点到目标点还有多远」。
- 作为代价下界：任何路径的代价，都不会小于起点到终点的欧氏距离。这个性质可以用来剪枝。
- 作为局部规划器的参考：在 DWA、TEB 等局部规划器中，欧氏距离可以作为一个子目标评价项。
举个例子，在 RRT* 中，我们经常这样用：
# RRT* 中，用欧氏距离作为代价函数
def cost_from_start(node):
# node 包含位置信息 (x, y)
dx = node.x - start.x
dy = node.y - start.y
return math.sqrt(dx*dx + dy*dy)
# 或者更精确地，累加路径上每段的欧氏距离
def path_cost(path):
total = 0.0
for i in range(len(path)-1):
total += euclidean_2d(path[i].x, path[i].y,
path[i+1].x, path[i+1].y)
return total这里有个细节：累加路径上每段的欧氏距离，得到的是「路径长度」。而直接用起点到终点的欧氏距离，得到的是「直线距离」。两者之差，就是路径的「曲折程度」。这个差值越大，说明路径越绕。
2.6 欧氏距离的局限性
说了这么多优点，也得聊聊它的不足。
| 场景 | 欧氏距离的问题 | 替代方案 |
|---|---|---|
| 有障碍物 | 无视障碍，可能穿过墙 | Dijkstra、A* 的累积代价 |
| 非欧几何 | 在曲面或流形上不准确 | 测地距离 |
| 运动学约束 | 不考虑转弯半径、加速度 | Dubins曲线、Reeds-Shepp曲线 |
| 各向异性 | 假设各方向代价相同 | 加权欧氏距离 |
你看，欧氏距离虽然简单，但用不好就是坑。我见过不少新手，直接用欧氏距离做全局代价，结果规划出来的路径要么穿墙，要么机器人根本走不了。
我的建议：欧氏距离最适合做「启发式」或「参考值」，不要直接用它做最终代价。把它和其他代价（障碍物代价、平滑度代价、时间代价）加权组合，才是工程实践中的正确姿势。
2.7 实战经验总结
最后，分享几个我在项目中积累的经验：
- 平方距离优先：能不用 sqrt 就不用。比较距离、排序、剪枝，平方距离完全够用。
- 注意坐标系：像素坐标、世界坐标、网格坐标，转换清楚再算距离。我见过有人把像素距离当米来用，结果路径规划出来全是错的。
- 归一化处理：如果欧氏距离要和其他代价加权，最好先归一化到 [0,1] 区间。不然量纲不同，权重很难调。
- 动态分辨率：在多层规划中，不同层的分辨率不同。计算距离时，要统一到同一坐标系下。
嗯，欧氏距离这部分就讲到这里。它虽然基础，但确实是路径规划代价函数的基石。下一章，我们会聊曼哈顿距离和切比雪夫距离——这两种距离在网格地图中特别有用，尤其是当你需要考虑「只能横平竖直走」的场景时。
记住一句话：欧氏距离是理想，实际路径是妥协。理解了这一点，你就算入门了。