☰
曼哈顿距离在机器人路径规划中的原理与实战详解
2026/10/19 5:55:34 网站建设 项目流程

前两天在调我们一台小型自动清扫机器人的任务调度时,又看到有同事对着坐标嘀咕:机器人停在(3,5),任务点在(7,2),两点间直线距离明明是5,为什么程序最后算出的是7?这个问题背后,正是曼哈顿距离(也叫 L1 距离、城市街区距离)。很多刚接触路径规划的同学,第一次看到 |x1-x2|+|y1-y2| 这个公式都会觉得太简单,可真正用起来才发现,它的适用场景、计算细节和常见坑,比公式本身要复杂得多。这篇文章我就结合机器人坐标的例子,从原理讲到代码,再讲到我实际调试时踩过的那些雷。

1. 曼哈顿距离到底解决机器人什么问题?先看清它的使用场景

1.1 为什么叫“曼哈顿”,而不是“两点之间线段最短”

曼哈顿距离这个名字来自城市街区的道路结构。在网格状路网里,你想从街口 A 走到街口 B,没法直接穿过一栋栋建筑,只能沿着东西方向和南北方向的道路走。一个方向走 x 格,另一个方向走 y 格,总步数就是 |x1-x2|+|y1-y2|。

把这个模型搬进机器人地图里就很直观了。当机器人只能在网格地图上做“前后左右”四方向移动时,它走过的路径只能和坐标轴平行。从 (3,5) 到 (7,2):

  • x 方向需要从 3 走到 7,共 4 格;
  • y 方向需要从 5 走到 2,共 3 格;
  • 总步数就是 4 + 3 = 7。

如果机器人能斜着走,比如无人机,那走直线距离才是最短;可对我们这种在货架间穿行的清扫机器人,斜线往往意味着撞到货架、越过障碍,根本走不了。所以路径规划里算代价时,曼哈顿距离比欧几里得距离更贴近真实运动成本。

1.2 直线距离会低估机器人的实际“腿脚成本”

欧几里得距离的公式是 sqrt((x1-x2)^2 + (y1-y2)^2),代入(3,5)和(7,2)会得到 5。单纯看数值,5 确实比 7 小,如果你拿 5 当作两点的通行代价去调度机器人,就会出现一个很微妙的偏差:机器人明明要拐弯绕路,程序却以为它几分钟就能到位,结果任务超时、电量预估偏小,甚至后续任务排队全乱掉。

这不是代码 bug,而是“距离模型”选错了。调度系统关心的是机器人实际执行任务的开销,不是人类在地图上用尺子量的直线长度。问自己一个问题:机器人到达目标点的方式,是可斜穿还是必须沿网格走?如果能斜穿,用欧氏;如果只能上下左右,曼哈顿才是正确的那把尺子。这也是我强烈建议在写任何坐标距离函数之前,先在地图上搞清楚“移动约束条件”的原因。

2. 从 (3,5) 到 (7,2):把曼哈顿公式拆成两步减法

2.1 先各算各轴,再合起来,顺序不重要

很多初学者第一次写曼哈顿距离会纠结:到底应该用 x1-x2 还是 x2-x1?答案是随便,因为公式外面有绝对值。我们以机器人坐标 (3,5)、任务点坐标 (7,2) 为例:

  • x 轴差值:7 - 3 = 4,取绝对值后还是 4;
  • y 轴差值:2 - 5 = -3,取绝对值后是 3;
  • 总距离:4 + 3 = 7。

就算你把坐标反过来写,用 (7,2) 作为起点,(3,5) 作为终点,得到的是 |3-7| + |5-2| = 4 + 3 = 7,一模一样。这是个很重要的性质:曼哈顿距离是“对称”的,它不关心谁先谁后,只关心两个坐标点位置差的绝对值之和。

实际写代码时,你只要保证“两个点来自同一个坐标系”,谁在前谁在后完全无所谓。不过我发现很多新手会在这里做个多余操作:先判断两个数谁大谁小,再相减。这没有必要,abs() 函数已经把大小方向都处理好了,多写if判断只是给代码增加无谓分支。

2.2 为什么一定要绝对值?负距离没有意义

再往深一层想,|x1-x2| 这个绝对值,本质上是把“坐标差值”转换成了“距离代价”。坐标差可以是负数,代表机器人需要往反方向走;但走的步数不可能是负数。想象一下,机器人从 y=5 走到 y=2,y 方向差是 -3,意思是从正方向往负方向移动 3 格;如果丢掉绝对值,把 -3 和 4 相加,得到 1,这个数字既不是直线距离,也不是实际路径长度,只是纯粹的数学残渣。

所有距离度量都必须满足一条基本性质:非负性。曼哈顿距离通过绝对值保证了这一点。这也是我排查问题时最爱用的一个信号:如果程序算出来的“曼哈顿距离”是负数,说明不是公式写错,就是拿到的坐标数据本身有问题,比如传感器传回了一个没初始化过的坐标占位值。负数距离几乎一定会让后续路径规划逻辑出现异常。

3. 直接上代码:从裸公式到函数封装再到批量计算

3.1 先用一行代码确认结果,跑通“最小可用版本”

标题说了直接上代码,那就先来最少的。Python 里几个内置函数就够用:

x1, y1 = 3, 5 x2, y2 = 7, 2 distance = abs(x1 - x2) + abs(y1 - y2) print(distance) # 7

这段代码没什么好解释的,abs() 取绝对值,加号连接 x 轴和 y 轴的结果。放到任何支持绝对值和四则运算的语言里,思路都一样:C、C++ 用 std::abs,Java 用 Math.abs,JavaScript 用 Math.abs,Go 用 math.Abs。

不过只写这么一段,还谈不上“工程代码”。实际项目里,坐标点通常来自传感器、地图解析器或任务下发模块,直接裸写着两个变量是很脆弱的。所以我的习惯是,先把坐标打包成元组或结构体,再写一个专门的距离函数。

3.2 封装成函数,让调用方不用记公式

我给团队里的项目封装过一个非常简单的 Python 函数:

def manhattan_distance(point_a, point_b): """计算二维平面上的曼哈顿距离。 point_a, point_b 格式: (x, y) """ x1, y1 = point_a x2, y2 = point_b return abs(x1 - x2) + abs(y1 - y2) robot_pos = (3, 5) target_pos = (7, 2) print(manhattan_distance(robot_pos, target_pos)) # 输出 7

这个封装的收益在于:调用方不需要知道曼哈顿公式是绝对值相加,只要传入两个同格式的坐标点,就能得到距离。后续如果地图从二维网格升到三维(比如楼层导航),你只需要改函数内部,所有调用点的行为都会同步更新。

我再补充一个细节:函数里用了解包x1, y1 = point_a,这要求外边传入的 point_a 必须严格是长度为 2 的可迭代对象。如果你传入的坐标是列表 [3, 5],一样可以解包;但如果哪天传入一个字符串 "35",代码就会在你完全没意料到的地方报错。所以正式项目里最好再加一个类型检查或在文档字符串里写明坐标格式,否则别人接手你的代码时很容易用错。

3.3 批量计算:面对一堆任务点的时候别写循环嵌套

真实调度场景里,机器人和任务点往往不止一组。比如扫地机器人需要知道它和当前楼层 20 个待清扫区域之间的距离,这时候你当然可以一遍遍调用上面的函数,但更推荐用列表推导式或循环一次性生成“距离清单”:

robot = (3, 5) tasks = [ (7, 2), (1, 4), (6, 8), ] for index, task in enumerate(tasks): dist = manhattan_distance(robot, task) print(f"任务点 {index + 1}: 坐标 {task}, 曼哈顿距离 = {dist}")

这段代码跑出来的结果会是:

任务点 1: 坐标 (7, 2), 曼哈顿距离 = 7 任务点 2: 坐标 (1, 4), 曼哈顿距离 = 3 任务点 3: 坐标 (6, 8), 曼哈顿距离 = 6

批量计算的价值不只是省几行代码,而是让任务分配逻辑可以直接基于这个距离清单排序:“先安排最近的任务点,再安排远的”。我项目里就是这么做的,先把所有任务点按曼哈顿距离从小到大排序,再配合机器人的电量阈值做过滤,调度问题就被拆解成了一个排序问题。

3.4 扩展到三维甚至更高维:机器人在楼层之间移动

如果机器人还能在楼层之间坐电梯移动,比如从 1 层的某点跑到 3 层的某点,就需要计算三维曼哈顿距离。公式是:

distance_3d = abs(x1 - x2) + abs(y1 - y2) + abs(z1 - z2)

更高维也可以写成一个支持任意维度数量的版本:

def manhattan_distance_md(point_a, point_b): return sum(abs(a - b) for a, b in zip(point_a, point_b)) # 二维 print(manhattan_distance_md((3, 5), (7, 2))) # 7 # 三维 print(manhattan_distance_md((3, 5, 1), (7, 2, 3))) # 4 + 3 + 2 = 9

不过我要提醒一句:不要盲目把“越高维越好”当成真理。在三维物理空间里,如果机器人不是被限制在网格上移动,那么曼哈顿距离会严重高估实际飞行或行驶距离。三维曼哈顿只适合“楼道+电梯”“货架层+垂直升降机”这类严格分轴运动的场景。用到三维时,一定要先确认 z 轴是否真的有对应的物理移动成本,否则算出来的数字会失真。

4. 距离算对不等于规划做好:顺序、坐标系与边界三种坑

4.1 距离值相同,实际路线却可能完全不同

这是我踩过比较有意思的一个坑。曼哈顿距离只告诉你“至少需要 7 步”,但并不会告诉你“具体怎么走这 7 步”。从 (3,5) 到 (7,2),总共有 35 种不同的“先右再下”组合顺序,下面只是其中三条:

  • 路径一:先向右走 4 格到 (7,5),再向下走 3 格到 (7,2),长度是 4+3=7;
  • 路径二:先向下走 3 格到 (3,2),再向右走 4 格到 (7,2),长度也是 3+4=7;
  • 路径三:向右 2 格、向下 1 格、再向右 2 格、再向下 2 格,每一步都算上,长度依然是 7。

同样是 7,这三条路径经过的网格完全不一样。如果地图上有障碍物,某条长度最短的路可能会被墙挡住,机器人就得绕更远;但距离公式本身不会告诉你任何障碍信息。所以我后来在项目里都会强调:曼哈顿距离适合作为“代价估算”或“启发式”,而不能替代真正的寻路算法。它只是一个下界或参考值,实际的可行路径还需要靠 A*、Dijkstra 这类算法去栅格地图里搜索。

4.2 坐标系方向反了、单位不一致,距离照样算错

绝对值能解决“谁减谁”的问题,但解决不了“坐标系不对齐”的问题。举个例子,在屏幕或图像坐标里,y 轴通常向下为正;在我们常用的笛卡尔平面直角坐标系里,y 轴向上为正。同一个点在不同坐标系里坐标值完全不同。

如果你把机器人坐标用 A 地图读出来,任务点坐标用 B 地图读出来,哪怕两边都是“正坐标”,得到的曼哈顿距离也可能跟真实运动距离毫无关系。更隐蔽的问题还有单位:一格到底代表 1 米还是 1 厘米?地图缩放倍数不同,坐标数值相差很大,但距离公式永远只会机械地做绝对值相加。

我在实际项目里习惯写一个简单自检函数:把机器人的当前位置和任务点坐标从同一个地图接口读取,打印出原始坐标和差值,人工看一眼方向是否合理。比如任务点在机器人的右下方,那期望看到 dx 为正、dy 为负;如果打印出来 dy 也是正,说明两个点的坐标系来源很可能混了。距离数值会骗人,但坐标差的方向不会。

4.3 大数值坐标、浮点误差和“零距离”的特殊情况

还有一个容易忽略的问题:坐标值很大时,整数溢出。在 C++ 里,如果坐标是 32 位 int,两个大数相减后绝对值理论上可能超过 2^31-1,比如一个坐标在(2_000_000_000, 0),另一个在(-2_000_000_000, 0),差值绝对就是 4_000_000_000,直接用 int 存就会溢出。解决方案很简单:计算差值时用 long long、double 或任意精度整数。

如果坐标是浮点数,曼哈顿距离也可能有浮点误差,虽然绝对值相加只涉及加减法,误差很小,但累计多次距离后还是可能漂移。我建议在比较两个距离是否相等时,别用==,而是用一个小阈值,比如abs(d1 - d2) < 0.001。

最后还有一种特殊情况:两点坐标完全相同。此时曼哈顿距离是 0,代码没问题,但业务逻辑要小心,比如机器人和目标点重合,还继续下发“前往该任务点”的指令,就属于逻辑冗余了。我在调度代码里碰到 dist == 0 时会直接跳过任务,省电也省时间。

5. 用一个完整示例验证:从起点到多个任务点的距离清单

5.1 手算一遍,建立对数字的直觉

纸上谈兵再多,不如拿个真实小例子验证。假设我们机器人起点是 (3,5),要评估三个任务点,我通常会先手算一张表,再去对程序输出:

任务点x 轴差值y 轴差值曼哈顿距离
(7,2)7-3=42-5=-3,绝对值 34+3=7
(1,4)1-3=-2,绝对值 24-5=-1,绝对值 12+1=3
(6,8)6-3=38-5=33+3=6

手算表的价值在于:你可以快速判断哪个任务点离起点最近。这里明显是 (1,4) 距离最近,只有 3。但要注意,最近的未必是“最优完成顺序”,因为实际路径还要考虑障碍物、机器人朝向、任务优先级。距离排序只是一个很好用的初筛维度。

5.2 用代码输出同样的表,比对两边结果

我平时会写一段验证脚本,把表里的每个字段都打印出来,而不是只打印最终距离。打印差值能让你一眼看到符号是不是正常:

robot = (3, 5) tasks = [(7, 2), (1, 4), (6, 8)] print("起点:", robot) print("任务点 | dx | dy | 距离") for task in tasks: dx = task[0] - robot[0] dy = task[1] - robot[1] dist = abs(dx) + abs(dy) print(f"{task} | {dx} | {dy} | {dist}")

输出:

起点: (3, 5) 任务点 | dx | dy | 距离 (7, 2) | 4 | -3 | 7 (1, 4) | -2 | -1 | 3 (6, 8) | 3 | 3 | 6

对表里每个点,dx、dy 的符号代表了机器人该往哪两个方向走。比如 (1,4) 的 dx 是 -2,说明任务点在机器人左边;dy 是 -1,说明任务点在机器人下方或 y 轴负方向。看到这种钉子符号,你才能确认坐标读进来之后没有发生轴翻转。

5.3 验证时最有用的调试方法:打印原始输入和中间差值

很多调试不顺的时候,问题根本不在曼哈顿计算公式,而在数据源头。传感器或地图接口偶尔会返回 null、NaN 或者颠倒好的坐标,而公式照样能算出一个数字。比如 y 坐标被传成 null,在某些语言里会被强制转成 0,距离结果看起来“合理”,实际上完全错误。

所以我调试时有一个死习惯:任何距离计算前,先把两个点的原始坐标打出来,再看 dx、dy,然后才看最终距离。不要怕日志刷屏,这种信息在排查“机器人为什么突然把自己规划到地图外面”的时候,能救你一命。加日志的成本极低,而跑一遍完整路径模拟的时间成本高得多。

6. 实战中什么时候选曼哈顿,什么时候换成欧氏或切比雪夫

6.1 四方向网格、轴对齐运动:曼哈顿是默认选择

如果地图被抽象成方格,机器人只能上下左右移动,那么曼哈顿距离几乎就是“最准确的最短路径下界”。在 A* 寻路算法里,它之所以广受欢迎,是因为它满足“可采纳性”:任何情况下估算值都不会大于真实最短路径长度,从而保证 A* 找到的最终路径是最优的。

这类场景我见过很多:仓储机器人沿货架通道移动、扫地机器人沿瓷砖缝走、电路板布线避开斜线等。曼哈顿距离在这些领域表现极稳,代码又轻,性能开销只有几次减法和绝对值,比开平方快得多。在几百个任务点的实时调度里,这种性能差距是能感知到的。

6.2 八方向移动、斜穿无阻挡区域:曼哈顿会高估距离

一旦机器人允许斜向移动,比如八方向网格,或者开阔平地上可以自由转弯,曼哈顿距离就会高估真实距离。此时欧几里得距离更接近实际,但如果你只想在网格里快速估算,也可以使用切比雪夫距离,或者给曼哈顿公式加一个系数:

def octile_distance(point_a, point_b, diagonal_cost=1.414): dx = abs(point_a[0] - point_b[0]) dy = abs(point_a[1] - point_b[1]) return max(dx, dy) + (diagonal_cost - 1) * min(dx, dy)

但这个函数已经算不上“纯曼哈顿距离”了,而是混合距离。如果你用 A* 做八方向寻路,启发式也必须跟着换,否则估算法过高,算法可能会绕过真正最优路径。我见过有人做了八方向地图却继续用纯曼哈顿,结果规划出来的路径总是多绕一步,调了一整天才发现是启发式函数和移动成本不匹配。

6.3 机器人的朝向、加速度也会影响距离选择

我最后一次想聊的实战坑是:机器人运动模型不只看“能不能斜走”,还要看“转个弯要花多少代价”。差速驱动机器人可以原地转方向,转向代价相对小;车式机器人则需要更大的转弯半径,曼哈顿距离和实际路径就差得更多。此时单纯的曼哈顿距离只适合做粗略估价,还需要额外考虑转向惩罚。

我在做清扫机器人调度时,最终采用的做法是两层距离并用:第一层用曼哈顿距离快速筛选出候选任务点,把范围从 30 个缩到 5 个;第二层再对这 5 个候选做带障碍和转向代价的路径搜索,找出真正可行的执行顺序。这样既保住了曼哈顿距离的性能优势,又避免了它“不考虑障碍”的天然短板。如果你也在做类似调度,不妨试一试这种“粗筛 + 精排”的组合思路。

需要专业的网站建设服务?

联系我们获取免费的网站建设咨询和方案报价,让我们帮助您实现业务目标

立即咨询