TTYW算法：高效实时路径规划新算法

项目背景

在自主导航领域，路径规划始终是核心问题之一。传统的全局路径规划算法（如经典A*、Dijkstra、启发式搜索等）通常假设环境是静态且已知的。然而在实际应用中，环境往往复杂多变：地图尺度大、障碍物分布不均匀，既包含大范围稀疏区域也包含局部高密度区域，并且环境中还存在动态移动的障碍物（如行人、车辆等），甚至需要多机器人同时在共享空间内协同作业。面对这样的情况，单一分辨率的静态规划方法难以兼顾规划效率和避障鲁棒性。

主播整了个“多分辨率 + 动态避障 + 多机器人”相结合的路径规划方案：

• 多分辨率环境表示：针对大规模地图，将环境划分为不同分辨率层次，在空旷区域使用低分辨率以加速寻路，在复杂区域使用高分辨率保障精细避障。这种分层地图可显著减少寻路节点数量，提高全局规划效率。

• 动态障碍避让：机器人工作环境常存在移动障碍，如其它车辆或行人。规划算法需要具备动态障碍预测和重规划能力，及时更新路径避免碰撞，保证导航的安全性和实时性。

• 多机器人协同：在仓储物流、车队编队等场景下，多台机器人共享道路资源。需要一个调度策略为每个机器人规划无冲突路径，并在任务层面进行协调，防止机器人间相撞或堵塞，提高整体任务效率。

主播在整一种面向实战的路径规划算法“TTYw”，融合多分辨率地图结构、双向A*高效搜索、KDTree加速碰撞检测、动态障碍预测重规划以及多机器人任务调度等技术，提供从算法原理到ROS集成的完整实现方案。

算法设计原理

多分辨率地图结构

现实环境中的障碍物分布具有不均匀特性：有些区域障碍密集，需要精细规划；有些区域空旷宽阔，不需要过高精度。这就催生了多分辨率地图的想法。在本算法中，我们将环境表示为两级分辨率：

• 高分辨率网格地图（细网格）：精细表示局部环境，每个栅格代表较小面积，能准确刻画狭窄通道、小障碍物等细节。用于机器人局部避障和路径精细调整。

• 低分辨率网格地图（粗网格）：通过对细网格降采样获得，每个栅格覆盖更大区域。粗网格用于全局规划中的快速粗略路径搜索，帮助机器人在大范围内规划一条初始路径骨架。

这种多层次地图结构结合了全局与局部视野：规划时先在粗网格上以较小计算代价找到一条接近最优的全局路径，然后再映射回细网格进行局部优化。由于粗网格抽象掉了细节障碍，搜索空间大幅减小，提高了全局规划速度；细网格确保路径的可行性和安全避障。实践中，有研究也建议“全局路径规划使用低分辨率地图，局部避障使用高分辨率地图”，以在保证安全的同时兼顾计算效率。

具体实现上，可以采用栅格金字塔或四叉树/八叉树的数据结构。比如，以2的幂为倍率将细网格聚合生成粗网格：如果细网格分辨率为0.1m，粗网格可以是0.5m或1m，每个粗栅格标记为障碍只要其覆盖范围内存在任何细栅格障碍。这样，可在粗图上运行快速搜索，再将结果路径投射回细图做精化处理。

优化的双向 A*（TTYw） 搜索策略

算法 TTYw 的核心路径搜索采用了一种优化的双向 A策略。传统 A 算法从起点出发单向搜索，遇到远距离大地图时，搜索节点过多、效率下降。而双向 A* 则同时从起点和终点两个方向展开搜索，当两侧搜索前沿在中间相遇时即找到路径。

双向搜索能够显著减少搜索节点数量。其基本原理是：从起点和终点各自维护一个开启列表（OPEN集）和关闭列表（CLOSED集），交替从两端各取出代价最小的节点扩展。每次扩展邻居节点时，检查该节点是否已被对向搜索访问过。如果是，则说明两端搜索路径相连，找到了通路。在路径相交点汇合后，可回溯得到完整路径。

为进一步优化效率，TTYw 算法在双向 A* 中引入以下策略：

• 一致启发函数：保证起点方向和终点方向采用一致的启发式估计（例如都使用欧几里得距离或曼哈顿距离）。启发函数的一致性确保算法仍然最优（或可控次优）并减少不必要扩展。

• 搜索空间限制：在粗分辨率地图上优先找到大概路径后，可将细分辨率搜索限制在该路径一定带宽内，以减少精细搜索范围。

• 预检剪枝：在扩展新节点前，先估计该节点与另一搜索前沿的最短可能连通距离，如果这个理论下界已经大于当前已知最短路径，则可放弃扩展（类似于双向A*的适时终止准则）。

采用双向A后，路径规划在大规模环境中的效率大大提高。特别是在起点和终点相距很远时，同时从两端搜索能够将复杂度降低很多，极大加速寻路过程。当然，由于双向搜索需要维护两套OPEN/CLOSED列表，实际实现需注意内存和逻辑的同步。在算法实现部分，主播会详细展示 TTYw 双向A 的伪代码与实际代码。

KDTree 静态障碍碰撞检测

在路径规划过程中和结果路径的优化过程中，经常需要频繁进行碰撞检查（collision check），即判断一条直线路径或某个节点是否与障碍物发生碰撞。若使用简单遍历网格的方法检查直线路径是否可通行，在高分辨率地图上代价很高。为提升效率，我们引入KD 树（KDTree）数据结构对静态障碍物进行索引，加速碰撞检测。

具体做法是：将所有静态障碍物栅格的坐标提取出来，建立一个KDTree。KDTree是一种对空间中的点进行分区的数据结构，支持高效的最近邻查询。构建完成后，我们可以快速查询“离某个坐标最近的障碍物距离”。利用这一点，可以进行高效的路径段碰撞检测：

• 节点有效性检查：在A*搜索扩展节点时，可用KDTree查询该节点与最近障碍的距离，若距离小于机器人半径（或安全间隔），则该节点视为无效（碰撞）。

• 直线路径通畅检查：在路径平滑/剪枝时，需要判断两路径点之间的直线能否不碰撞直达。我们可以采样直线上若干点，用KDTree查询最近障碍距离，如果整条线段所有采样点都距障碍大于安全距离，则认为该直线路径可行，否则中途有障碍阻挡。相比逐格检查，KDTree方法能亚毫米级精度地检测距离且速度极快（$O(\log N)$时间查询）。

需要注意，为了使用KDTree，高效地表示障碍，可以将地图中障碍物栅格的中心坐标收集为点集（例如 (x,y) 坐标对），KDTree基于这些点构建。如果地图很大障碍很多，构建KDTree本身也有开销，但对于静态障碍地图，这个构建只需一次，可以在启动时完成，后续查询开销很小。在路径规划系统中，KDTree通常用在局部路径优化阶段和机器人运动仿真阶段，用于实时计算最近障碍距离，让机器人可以感知安全余量。例如，当机器人沿规划路径前进时，可以不断查询当前机器人位置与最近障碍物距离，若低于阈值则需要减速或重新规划。

动态障碍预测与路径重规划机制

现实环境中的动态障碍（如行人、其他机器人）会随时间移动，对机器人路径造成潜在威胁。为此，TTYw 算法集成了动态障碍预测和路径重规划机制，使机器人能够边走边躲避移动障碍。

动态障碍预测：首先，我们需要获取动态障碍物的运动信息。常见方式包括：通过传感器检测（激光雷达识别移动物体、摄像头视觉检测行人）或者由系统提前提供轨迹预报（例如通过V2X获取车辆路径）。无论来源如何，我们将动态障碍表示为一系列随时间变化的位置 $(x(t), y(t))$。可以使用简单的运动模型预测短时间内的位置，比如匀速模型：根据当前速度预估未来几秒的位置。

路径重规划：机器人在执行规划路径时，采用循环监测-重规划策略：

1. 监测路径安全：机器人周期性地（例如10Hz）检查前方路径上是否出现预测的障碍冲突。具体方法是：取机器人当前位置沿规划路径向前的一段距离，查看动态障碍未来轨迹是否会出现在这段路径范围内（在相同期望时间内）。如果预测到在机器人到达某路径点时，会有障碍物也到达该点附近，则视为即将发生碰撞。

2. 触发重规划：一旦检测到前方路径在未来某时刻将被动态障碍占据，立即触发全局路径规划器重新规划。重规划时，将即将发生碰撞的动态障碍位置视同静态障碍，暂时避开它们计算新路径。由于动态障碍在移动，新路径应尽量绕开其未来位置。在实现上，可将动态障碍在未来几秒的预测位置加入地图障碍列表，让A*避开这些区域。

3. 路径平滑衔接：重规划得到新路径后，需要平滑地衔接当前机器人位置，以避免频繁剧烈转向。通常可采取路径拼接策略：保留旧路径上已经安全的部分，与新路径余下部分相接，然后再进行一次平滑优化。

4. 重复循环：机器人沿新路径继续前进，同时持续监测动态障碍。如果障碍运动导致新的冲突，再次重规划。整个过程持续进行，形成闭环反馈。

But，过于频繁的重规划并不可取l。如果每检测到一点小风险就立刻重规划，可能导致机器人来回摇摆不定，反而降低效率。因此需要设计重规划判定条件，例如只有当动态障碍距离本机器人小于一定阈值且明确会阻挡去路时才重规划，否则采用低速等待等策略。调度上，可以给每次重规划设置冷却时间，避免连续多次规划造成计算浪费。

在多机器人场景下，其他机器人本身也是动态障碍的一种。可以通过发布和共享位置/速度信息，让每个机器人都将他人视作动态障碍来避让。此外更高层还可采用优先级调度或时间窗分配方法，使机器人错开经过狭窄区域的时间，实现团队协调。

路径平滑与剪枝（Douglas-Peucker + 曲线平滑）

A算法找到的初始路径通常由栅格中心点串成，路径会呈现“折线”状：在网格约束下拐点较多，路径不够平滑。同时，A搜索得到的路径往往不是几何上最简路径，包含许多不必要的中间点（例如在开阔区域仍然保持锯齿形前进）。为提升路径质量，我们对初始路径进行剪枝与平滑处理：

• 路径剪枝（简化）：采用知名的Ramer–Douglas–Peucker算法（简称Douglas-Peucker算法）对路径进行简化。该算法的思路是：给定一条折线（由一系列点构成），它会递归地移除那些与其邻接线段近似共线的点，仅保留能造成路径偏离的关键转折点。具体过程：连接路径起终点成为一条直线，找到路径中偏离这条直线最大距离的点，如果该距离大于阈值ε，则保留该点（认为这一点是必要拐点）并以此点将路径分段，两段各自递归简化；若最大偏离≤ε，则说明中间所有点都在一条近似直线上，可全部删除。通过此算法，我们能够删除路径中冗余的节点，大幅减少路径拐弯次数，得到一条近似最简曲线。阈值ε可以根据地图分辨率和机器人尺寸调整——ε过大会导致路径贴近障碍拐弯处的关键点被删掉，ε过小则简化效果不明显。实践证明，该算法对栅格路径的节点数压缩效果非常明显。

• 路径平滑：剪枝后的路径拐点少了，但仍是折线。为了生成平滑连续的轨迹，适合机器人（尤其是非全向移动底盘）的运动学约束，我们需要将折线路径进一步平滑成曲线路径。经典方法包括多段 Bézier 曲线拟合、B样条曲线（B-spline）平滑、或者简单的角度钝化处理。在本项目中，我们采用一种简易但有效的平滑策略：

  1. 逐段平滑转角：针对路径中相邻的三个点(p_i-1, p_i, p_i+1)，如果转角过于锐利，则在p_i附近插入若干插值点，使得路径在该段逐渐拐弯而非突然转向。插值点可通过在两线段之间做圆弧拟合或使用多次Catmull-Rom样条插值获得。

  2. 邻点迭代调整：采取一种迭代的方法，多次微调路径点使之更平滑。比如著名的算法是用每个点朝其前后邻点移动一点点，实现曲线平滑，但要锁定起终点不变。每次迭代让路径变得更平顺，但要确保不能碰撞障碍。我们可以在调整后再次用KDTree检查平滑曲线是否偏离安全通道，若有则缩小调整幅度。

  3. 速度曲线平滑：虽然几何路径平滑了，还可以考虑速度规划，使机器人逐渐加减速，而非在每个拐点急停。由于本文聚焦几何路径，在此不展开，但在实际部署中，可结合轨迹规划算法（如梯形速度曲线、时间弹性带TEB算法等）进一步优化运动平滑性。

经过剪枝和平滑后，最终输出的路径节点更少、曲率连续，可显著减少机器人转向和停启次数，提高行驶效率和舒适度。

系统架构分层与模块划分

为了实现上述算法并部署在实际机器人系统中，我们采用分层解耦的系统架构，将功能划分为若干模块，彼此通过明确的接口交互。整体架构如下：

• 环境建模层：负责地图构建与表示，包括静态地图的加载、多分辨率栅格的生成以及障碍物数据结构准备（如KDTree构建）。该层向上提供地图查询接口，使规划器能高效获取障碍信息。

• 路径规划层：核心算法层，实现 TTYw 路径搜索、路径优化等功能。接收环境地图、起终点等输入，输出规划的最优/次优路径。内部可细分为全局规划器（粗分辨率A*）和局部规划器（细分辨率调整）两个子模块协作。

• 动态障碍处理层：专门处理环境中移动目标的监测和预测。通过订阅传感器或外部系统的数据，实时更新动态障碍列表，并提供碰撞预测服务。如果预测到冲突，通知路径规划层进行重规划。本层也负责将其它机器人位置纳入考虑，实现机器人的互相避让。

• 多机器人调度层：当存在多个机器人时，由此模块根据任务需求进行分配调度。它会管理每个机器人的任务队列，依次调用路径规划层为其规划路径。同时协调多个机器人之间的优先级，在必要时插入等待或重新规划避免路线冲突。

• 执行与控制层：负责按照规划路径驱动机器人运动。包括将路径转换为速度指令、发送给机器人底盘执行，并根据反馈进行位置跟踪和误差校正。如果在执行中收到新的路径（重规划结果），平滑地过渡跟随新路径。

• 可视化与监控层：提供系统状态的可视化和人机交互接口。例如使用 RViz 显示地图、规划路径、机器人和障碍物位置等；提供调试信息的日志输出；运行时监控各模块状态等。开发阶段主要使用 RViz 进行路径和环境的可视化，在部署阶段也可对接上层调度系统或UI。

以上架构中，各模块通常对应独立的 ROS Node，实现松耦合设计：地图由map_server节点发布，规划器作为planner节点服务，其它如调度器、监测器各司其职。这种设计便于分工协作和功能扩展。例如需要替换路径规划算法，只需更换planner节点实现，不影响其他部分。下面主播具体介绍多机器人调度和动态避障数据接入的流程。

多机器人任务调度与轨迹模拟流程

当多台机器人在同一环境中工作时，我们需要为它们规划各自的路径并避免相互冲突。多机器人任务调度模块的作用是分配任务并协调路径规划顺序，确保机器人路线在空间和时间上都尽可能冲突最少。以下是一个典型的多机器人路径规划调度流程：

1. 任务分配：假设有若干机器人和若干目标任务（例如运输订单）。首先通过任务分配算法（如拍卖算法、Hungarian算法等）决定由哪台机器人执行哪个任务，并确定各机器人的起点和终点。任务调度不在本文重点范围，这里假定已分配好起终点对。

2. 顺序规划：调度器按照一定顺序调用路径规划模块为每个机器人规划路径。常见策略有：

   • 串行规划：一次仅规划一台机器人（例如按任务优先级排序）。先为Robot1规划路径，然后将Robot1的路径锁定，再规划Robot2，在规划Robot2时将Robot1的路径视作动态障碍（或预留出来时间窗），以避免Robot2穿过Robot1相同轨迹。在此策略下，后规划的机器人会避开先规划机器人的路线。

   • 并行规划：同时考虑所有机器人，使用联合状态空间搜索或多智能体规划算法（如CBS, ECBS等）。该方法能找到整体最优解但实现复杂、计算量大。工程上多采用前一种分步协调的方法。

3. 冲突检测与调整：无论使用哪种策略，得到初始路径方案后，调度器需要检查机器人路径之间是否存在冲突。例如两机器人是否会同时到达某狭窄区域或相撞。如果检测到冲突，有两种应对：

   • 时间调度：让其中一个机器人等待或减速，错开通过该区域的时间。例如Robot1先通过，Robot2在安全距离外等候，待路径清空再通过。这种等待可以通过在Robot2路径中插入停留片段实现。

   • 路径重规划：令其中一个机器人重新规划绕开冲突区域。重规划时可以将另一机器人当前及预期占据的位置当作动态障碍处理。重规划后需再次验证全局无冲突。

4. 轨迹模拟与执行：当得到无冲突的路径集后，调度器下发路径给各机器人并启动执行。在执行过程中，调度器仍监控各机器人状态：

   • 如果某机器人因意外（避障停车、滑移等）延误了，使得后续机器人的预定错峰计划被打乱，调度器应及时调整。例如两机器人原本错开通过门口，但一个延迟导致可能同时到门口，则需要重新下发等待或重规划指令。

   • 调度器可设一个监督线程（Agent Supervisor），不断比较各机器人实时位置和路径计划，预测是否将接近（如接近阈值距离则标记为“危险接近”状态）。一旦进入危险接近状态且时间步差Δsteps超出阈值，就触发局部等待或重规划。

5. 迭代优化：上述过程并非一次性完成。随着新任务加入或环境变化，调度需要不断迭代。可引入滚动时间窗，每隔几秒重新评估任务队列和路径，确保多机器人系统保持全局最优或次优的状态。

下面以两个机器人为例给出一个简化的调度伪代码流程：

robots = [R1, R2]  # 两台机器人
for robot in robots:
    path = plan_path(robot.start, robot.goal)  # 调用路径规划模块
    robot.path = path

# 简单冲突检查与时间调度
t = 0
while not all robots at goal:
    for each robot:
        pos = robot.position_at_time(t)
        for each other_robot:
            other_pos = other_robot.position_at_time(t)
            if distance(pos, other_pos) < safety_distance:
                # 冲突：保持优先级高的机器人移动，低优先级机器人等待
                if robot.priority < other_robot.priority:
                    robot.freeze_at_time(t)  # 插入等待
                else:
                    other_robot.freeze_at_time(t)
    t += Δt  # 时间步推进

以上伪代码中，我们假定了一个position_at_time(t)函数可获取机器人在路径上某时刻的位置，以及freeze_at_time(t)用于在时间轴上插入等待。这种基于时间步的模拟方法可以帮助我们验证多机器人路径是否会冲突，并通过调整时间协调来避免碰撞。当然，更复杂的方案还可以调整路径顺序或动态重规划解决冲突。在实际实现中，我们可能并不会以显式的时间步模拟整个过程，而是通过事件驱动的方式检测并处理冲突，但本质思想类似。

(主播小提示：在仿真中，可使用不同颜色在RViz中绘制多个机器人的路径，并通过Marker显示机器人实时位置，以直观验证调度效果。)

动态障碍轨迹数据接入方式

实现动态避障，首先要获取动态障碍物的数据。常见的轨迹数据接入方式包括：

• 离线CSV文件：在开发和测试阶段，我们可以使用预先录制的障碍物轨迹数据文件（例如CSV格式）。文件中包含时间戳和障碍物位置、速度等信息。我们的程序可以定时读取CSV的下一行数据，模拟障碍物移动。这种方式简单直观，适合重现特定Scenario进行算法验证。

• 激光雷达（LiDAR）检测：实际机器人多配备2D或3D激光雷达，可通过聚类算法实时识别移动物体的位置。我们可以订阅雷达点云数据（如ROS话题 /scan 或 /pointcloud），然后对点云进行聚类跟踪，输出动态障碍物的中心位置和速度。每个障碍可用唯一ID标识，跟踪其运动轨迹。

• 视觉识别：使用摄像头图像配合目标检测/跟踪模型（如 YOLO、OpenPose 等）识别出人、车等动态物体，并通过深度估计或多传感器融合得到其在地图坐标系下的位置。同样需要对识别到的对象进行ID跟踪，以提供连续轨迹。

• 外部系统：在智能交通或工业调度场景，可能有独立的定位系统或V2X系统提供移动物体的位置。比如工厂中的AGV管理系统可直接提供每台车辆的坐标和速度，我们的规划器可以通过订阅相应的topic或API获得这些数据。

无论数据来源如何，动态障碍处理模块需要将离散的检测数据转化为可用于规划的轨迹预测。实现上，可对收到的历史位置应用运动模型预测未来位置。例如，最近几次位置推算出速度，然后预测。当然也可以使用更复杂的卡尔曼滤波等估计更准确的未来位置。

数据接入时还需注意：坐标转换和时间同步问题。雷达或视觉检测的障碍位置通常需要从传感器坐标系转换到全局地图坐标系（利用TF坐标变换）。同时，机器人规划需要预测未来几秒，因此传感器数据的时间戳和预测步长要与规划周期协调。例如，若规划周期为1秒，传感器10Hz，那么可能需要使用最近10帧障碍位置做外推。

在ROS1框架下，实现动态障碍数据接入的典型方法是：创建一个订阅节点，订阅诸如/dynamic_obstacles的自定义消息或多个传感器话题，在回调中融合提取出障碍物列表（带ID、位置、速度），更新全局的动态障碍列表。路径规划器每次规划时从该列表读取最新障碍预测，用于避障。由于ROS是异步的，确保在规划时拿到相对新的障碍信息非常重要，因此可通过消息过滤或者共享内存的方式让规划线程及时获得感知结果。

RViz 可视化与 ROS1 集成步骤

将算法从理论落地，需要与ROS1系统和RViz可视化工具集成。下面以ROS1为基础，概述集成的主要步骤和注意事项：

ROS节点架构设计

按照前文架构，我们在ROS中实现以下节点（仅列出与路径规划直接相关的部分）：

• map_server节点：使用ROS自带的map_server加载静态地图（通常是yaml+pgm文件）。该节点发布/map主题（消息类型nav_msgs/OccupancyGrid）。我们的路径规划器可从此获得静态栅格地图数据。

• planner节点：即我们实现的TTYw路径规划器核心。它需要订阅地图和障碍物信息，并提供服务或话题用于接受规划请求、输出规划路径。

  • 订阅：/map (静态地图), /dynamic_obstacles (动态障碍，自定义消息列表) 等。

  • 提供服务：可以使用ROS服务/plan_path（srv类型可定义为起点、终点输入，返回路径）或 订阅一个/goal话题（如geometry_msgs/PoseStamped）来触发规划。

  • 发布：/plan（规划的全局路径，类型nav_msgs/Path），/trajectory（如果考虑时间的轨迹，可以发布自定义带时间戳的路径消息）等。

• scheduler节点（多机器人时）：负责接受多个目标任务，并调用planner服务多次。可以发布多条路径，例如/robot_1/plan, /robot_2/plan分别对应每个机器人路径。或者该节点本身整合多个机器人的路径，然后发布一个可视化Marker数组展示。

• agent节点（每个机器人一个）：这个节点负责机器人本体控制，订阅自己的路径（如/robot_i/plan），将路径点转化为速度指令给底盘。对于仿真，可以用turtlebot等机器人的导航包直接发送路径跟随；对于实际，可借助move_base接口执行。

以上节点通过ROS通信协作。例如，当我们在RViz中点击一个2D Nav Goal，RViz会发布目标位姿到某个话题，我们的planner节点接收后开始规划，然后发布Path；RViz订阅Path后即可显示路径曲线。

nav_msgs/Path 路径发布

ROS中nav_msgs/Path是用于表示路径的标准消息类型，其内容是一个带有头（包含坐标系frame_id等信息）和一组位姿序列的结构。每个位姿为geometry_msgs/PoseStamped类型。为了在RViz中正确显示，我们需要设置：

• frame_id：Path消息中的header.frame_id应该设置为地图坐标系，例如"map"，这样RViz会将路径点解释为地图坐标下的位置。若frame不匹配（比如用了base_link），路径可能无法显示或位置错误。

• poses列表：将规划得到的路径节点按顺序填入poses数组。每个PoseStamped需要包含位姿，位置(x,y)直接取路径点坐标，orientation可根据路径段方向计算，也可统一给定默认朝向（例如朝向下一点的航向角）。如果不需要机器人朝向展示，orientation可以不关注，RViz在Path中只绘制位置点和相连的线，不绘制朝向。

路径发布的实现很简单，在planner节点中，例如使用Python的rospy：

path_msg = Path()
path_msg.header.frame_id = "map"
path_msg.header.stamp = rospy.Time.now()
for (x, y) in path_points:  # 我们的规划结果点列
    pose = PoseStamped()
    pose.pose.position.x = x
    pose.pose.position.y = y
    pose.pose.position.z = 0.0
    pose.pose.orientation.w = 1.0  # 朝向这里简单处理为无旋转
    path_msg.poses.append(pose)
path_publisher.publish(path_msg)

发布后，在RViz中添加一个Path显示，选定相应的话题和正确的全局坐标系，就能看到路径曲线。同时可以在RViz中添加Map显示（OccupancyGrid），RobotModel显示（机器人位置，需要运行AMCL或真值位置播报TF）等，以观察路径相对于环境和机器人的关系。

地图和传感器对接建议

集成过程中，有几条实践建议：

• 地图获取：如果地图较大，可以考虑在planner启动时只订阅一次OccupancyGrid，或直接从文件加载为数组，避免每次规划都从话题取数据。将地图存入规划器的内部数据结构（比如numpy数组、KDTree）以供快速查询。

• 动态障碍获取：建议使用单独线程持续订阅障碍物消息，并维护全局动态障碍列表。规划时加锁读取该列表，结合当前时间预测障碍位置。在多机器人情况下，这个障碍列表也应包含其它机器人的当前位置（可由各机器人发布自己的Pose到一个集合topic，供规划器获取）。

• 参数配置：通过ROS参数服务器设置一些可调参数，例如resolution_coarse粗略网格分辨率、replan_period重规划周期、safe_distance安全距离等。这样方便在launch文件或命令行调整，无需修改代码。

• 调试可视化：充分利用RViz的Marker机制发布调试信息。例如发布动态障碍的Marker（移动的立方体/球体）、发布规划网格的探索情况（比如将A*搜索过的节点用Marker表示），这些都有助于理解算法行为。尽管正式部署时不需要，但在开发阶段非常有价值。

全部关键模块的完整 Python 实现代码

下面给出TTYw多分辨率路径规划算法的关键模块完整实现代码（Python版）。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import threading
import numpy as np
import math
import heapq
from scipy.spatial import KDTree
from nav_msgs.msg import OccupancyGrid, Path
from geometry_msgs.msg import PoseStamped
import time

# ==== 多分辨率地图类 ====
class MultiResGridMap(object):
    def __init__(self):
        self.ready = False
        self.map_lock = threading.Lock()
        self.grid = None
        self.coarse_grid = None
        self.obst_kdtree = None
        self.origin = (0,0)
        self.resolution = 0.05
        self.coarse_factor = 4

    def update_map(self, occ_grid, coarse_factor):
        with self.map_lock:
            self.coarse_factor = coarse_factor
            self.width = occ_grid.info.width
            self.height = occ_grid.info.height
            self.resolution = occ_grid.info.resolution
            self.origin = (occ_grid.info.origin.position.x, occ_grid.info.origin.position.y)
            arr = np.array(occ_grid.data).reshape((self.height, self.width))
            self.grid = (arr > 50).astype(np.uint8)
            ch = int(math.ceil(self.height / float(coarse_factor)))
            cw = int(math.ceil(self.width / float(coarse_factor)))
            coarse_grid = np.zeros((ch, cw), dtype=np.uint8)
            for i in range(ch):
                for j in range(cw):
                    sy = i*coarse_factor
                    sx = j*coarse_factor
                    sub = self.grid[sy:sy+coarse_factor, sx:sx+coarse_factor]
                    if np.any(sub==1): coarse_grid[i,j]=1
            self.coarse_grid = coarse_grid
            self.coarse_height = ch
            self.coarse_width = cw
            obst_points = np.argwhere(self.grid==1)
            pts = []
            for (y,x) in obst_points:
                wx = self.origin[0] + (x+0.5)*self.resolution
                wy = self.origin[1] + (y+0.5)*self.resolution
                pts.append((wx,wy))
            if pts:
                self.obst_kdtree = KDTree(pts)
            else:
                self.obst_kdtree = None
            self.ready = True

    def is_free(self, x_idx, y_idx, coarse=False):
        if coarse:
            if 0<=y_idx<self.coarse_height and 0<=x_idx<self.coarse_width:
                return self.coarse_grid[y_idx, x_idx]==0
            else: return False
        else:
            if 0<=y_idx<self.height and 0<=x_idx<self.width:
                return self.grid[y_idx, x_idx]==0
            else: return False

    def world_to_grid(self, wx, wy, coarse=False):
        dx = wx-self.origin[0]
        dy = wy-self.origin[1]
        if coarse:
            cx = int(math.floor(dx/(self.resolution*self.coarse_factor)))
            cy = int(math.floor(dy/(self.resolution*self.coarse_factor)))
            return (cx, cy)
        else:
            gx = int(math.floor(dx/self.resolution))
            gy = int(math.floor(dy/self.resolution))
            return (gx, gy)

    def grid_to_world(self, x_idx, y_idx):
        wx = self.origin[0] + (x_idx+0.5)*self.resolution
        wy = self.origin[1] + (y_idx+0.5)*self.resolution
        return (wx, wy)

# ==== 动态障碍物管理 ====
class DynamicObstacleManager(object):
    def __init__(self):
        self.obs_lock = threading.Lock()
        self.obs_trajs = {}  # id -> list of (x, y)
        self.obs_times = {}  # id -> 起始时间戳
        self.obs_vel = {}    # id -> 每step前进的轨迹点数（或速度）

    def set_obstacle_trajectory(self, obs_id, traj, velocity=1.0):
        with self.obs_lock:
            self.obs_trajs[obs_id] = traj
            self.obs_times[obs_id] = time.time()
            self.obs_vel[obs_id] = velocity

    def get_positions(self, dt=0):
        now = time.time()
        positions = []
        with self.obs_lock:
            for obs_id in self.obs_trajs:
                traj = self.obs_trajs[obs_id]
                v = self.obs_vel.get(obs_id,1.0)
                t0 = self.obs_times[obs_id]
                idx = int((now-t0+dt)*v)%len(traj)
                positions.append(traj[idx])
        return positions

    def check_path_collision(self, path, safe_dist=0.4):
        for step, pt in enumerate(path):
            obs_pts = self.get_positions(dt=step*0.1)
            for o in obs_pts:
                if np.hypot(pt[0]-o[0], pt[1]-o[1]) < safe_dist:
                    return True
        return False

# ==== 路径规划器 ====
class PathPlanner(object):
    def __init__(self, grid_map):
        self.map = grid_map
        self.dirs = [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(1,-1),(-1,1),(1,1)]
        self.move_cost = lambda dx,dy: math.hypot(dx,dy)

    def heuristic(self, x0,y0,x1,y1):
        return math.hypot(x1-x0,y1-y0)

    def plan(self, start_w, goal_w):
        with self.map.map_lock:
            sx,sy = self.map.world_to_grid(start_w[0],start_w[1],False)
            gx,gy = self.map.world_to_grid(goal_w[0],goal_w[1],False)
            csx,csy = self.map.world_to_grid(start_w[0],start_w[1],True)
            cgx,cgy = self.map.world_to_grid(goal_w[0],goal_w[1],True)
            if not self.map.is_free(sx,sy) or not self.map.is_free(gx,gy):
                rospy.logwarn("起点/终点在障碍上")
                return []
            open_set=[]
            heapq.heappush(open_set,(0+self.heuristic(csx,csy,cgx,cgy),(csx,csy)))
            came_from={}
            g_cost={(csx,csy):0}
            came_from[(csx,csy)] = None
            goal_found = False
            meet_cx,meet_cy = None,None
            while open_set:
                f,(cx,cy)=heapq.heappop(open_set)
                if (cx,cy)==(cgx,cgy):
                    goal_found=True; meet_cx,meet_cy=cx,cy; break
                for dcx,dcy in self.dirs:
                    n_cx=cx+dcx; n_cy=cy+dcy
                    if not self.map.is_free(n_cx,n_cy,coarse=True): continue
                    new_cost = g_cost[(cx,cy)]+self.move_cost(dcx,dcy)
                    if (n_cx,n_cy) not in g_cost or new_cost<g_cost[(n_cx,n_cy)]:
                        g_cost[(n_cx,n_cy)] = new_cost
                        came_from[(n_cx,n_cy)] = (cx,cy)
                        f_cost = new_cost + self.heuristic(n_cx,n_cy,cgx,cgy)
                        heapq.heappush(open_set,(f_cost,(n_cx,n_cy)))
            if not goal_found: return []
            coarse_path_cells = []
            node = (meet_cx,meet_cy)
            while node: coarse_path_cells.append(node); node = came_from[node]
            coarse_path_cells.reverse()
            fine_path=[]; prev_coarse_idx=coarse_path_cells[0]
            for coarse_idx in coarse_path_cells:
                if coarse_idx==prev_coarse_idx: local_start=(sx,sy); prev_coarse_idx=coarse_idx; continue
                gx_local,gy_local=None,None
                cx,cy=coarse_idx
                coarse_cx_world=self.map.origin[0]+(cx+0.5)*(self.map.resolution*self.map.coarse_factor)
                coarse_cy_world=self.map.origin[1]+(cy+0.5)*(self.map.resolution*self.map.coarse_factor)
                min_dist=float('inf'); target_cell=None
                start_x=cx*self.map.coarse_factor
                start_y=cy*self.map.coarse_factor
                for dy in range(self.map.coarse_factor):
                    for dx in range(self.map.coarse_factor):
                        x_idx=start_x+dx; y_idx=start_y+dy
                        if not self.map.is_free(x_idx,y_idx): continue
                        wx,wy = self.map.grid_to_world(x_idx,y_idx)
                        d = math.hypot(wx-coarse_cx_world,wy-coarse_cy_world)
                        if d<min_dist: min_dist=d; target_cell=(x_idx,y_idx)
                if target_cell is None: target_cell=(gx,gy)
                gx_local,gy_local = target_cell
                path_segment = self._a_star_grid(local_start,(gx_local,gy_local))
                if not path_segment: continue
                if fine_path: fine_path.extend(path_segment[1:])
                else: fine_path.extend(path_segment)
                local_start=(gx_local,gy_local); prev_coarse_idx=coarse_idx
            final_goal_idx=(gx,gy)
            if fine_path and fine_path[-1]!=final_goal_idx:
                last_seg = self._a_star_grid(fine_path[-1],final_goal_idx)
                if last_seg: fine_path.extend(last_seg[1:])
            elif not fine_path: fine_path = self._a_star_grid((sx,sy),(gx,gy))
            if not fine_path: return []
            world_path = [self.map.grid_to_world(x,y) for (x,y) in fine_path]
            world_path = self._optimize_path(world_path)
            return world_path

    def _a_star_grid(self,start_idx,goal_idx):
        sx,sy=start_idx; gx,gy=goal_idx
        open_set=[]; heapq.heappush(open_set,(0+self.heuristic(sx,sy,gx,gy),(sx,sy)))
        came_from={(sx,sy):None}; g_cost={(sx,sy):0}; visited=set()
        while open_set:
            f,(x,y)=heapq.heappop(open_set)
            if (x,y)==(gx,gy):
                path=[]; node=(x,y)
                while node is not None: path.append(node); node=came_from[node]
                path.reverse(); return path
            visited.add((x,y))
            for dx,dy in self.dirs:
                nx,ny=x+dx,y+dy
                if not self.map.is_free(nx,ny): continue
                new_cost=g_cost[(x,y)]+self.move_cost(dx,dy)
                if (nx,ny) in visited and new_cost>=g_cost.get((nx,ny),float('inf')): continue
                if new_cost<g_cost.get((nx,ny),float('inf')):
                    came_from[(nx,ny)]=(x,y); g_cost[(nx,ny)]=new_cost
                    f_cost=new_cost+self.heuristic(nx,ny,gx,gy)
                    heapq.heappush(open_set,(f_cost,(nx,ny)))
        return None

    def _optimize_path(self,path):
        if len(path)<3: return path
        def douglas_peucker(points,eps):
            dmax=0.0; index=0; line_start=np.array(points[0]); line_end=np.array(points[-1])
            line_vec=line_end-line_start; line_len_sq=np.dot(line_vec,line_vec)
            for i in range(1,len(points)-1):
                p=np.array(points[i])
                if line_len_sq==0: d=np.linalg.norm(p-line_start)
                else:
                    t=max(0,min(1,np.dot(p-line_start,line_vec)/line_len_sq))
                    proj=line_start+t*line_vec; d=np.linalg.norm(p-proj)
                if d>dmax: index=i; dmax=d
            if dmax>eps:
                left=douglas_peucker(points[:index+1],eps)
                right=douglas_peucker(points[index:],eps)
                return left[:-1]+right
            else:
                return [points[0],points[-1]]
        eps=1.5*self.map.resolution
        simplified_path=douglas_peucker(path,eps)
        if len(simplified_path)<3: return simplified_path
        smooth_path=[simplified_path[0]]
        for i in range(1,len(simplified_path)-1):
            p_prev=np.array(simplified_path[i-1]); p_curr=np.array(simplified_path[i]); p_next=np.array(simplified_path[i+1])
            v1=p_curr-p_prev; v2=p_next-p_curr
            cos_angle=np.dot(v1,v2)/(np.linalg.norm(v1)*np.linalg.norm(v2)+1e-6)
            if cos_angle<0.99:
                new_p1=p_curr+0.33*v2
                new_p2=p_curr+0.66*v2
                safe=True
                if self.map.obst_kdtree:
                    dist1,_=self.map.obst_kdtree.query(tuple(new_p1))
                    dist2,_=self.map.obst_kdtree.query(tuple(new_p2))
                    if dist1<self.map.resolution*2 or dist2<self.map.resolution*2:
                        safe=False
                if safe: smooth_path.append(tuple(new_p1)); smooth_path.append(tuple(new_p2))
                else: smooth_path.append(tuple(p_curr))
            else:
                smooth_path.append(tuple(p_curr))
        smooth_path.append(simplified_path[-1])
        return smooth_path

# ==== 多机器人管理 ====
class RobotManager(object):
    def __init__(self):
        self.robots = {}  # name -> dict with {start, goal, curr, traj, lock}
        self.lock = threading.Lock()

    def add_robot(self, name, start, goal):
        with self.lock:
            self.robots[name] = {
                'start': start,
                'goal': goal,
                'curr': start,
                'traj': [start],
                'reached': False,
                'lock': threading.Lock()
            }

    def update_goal(self, name, new_goal):
        with self.lock:
            if name in self.robots:
                with self.robots[name]['lock']:
                    self.robots[name]['goal'] = new_goal
                    self.robots[name]['reached'] = False

    def get_all(self):
        with self.lock:
            return list(self.robots.items())

    def update_traj(self, name, new_point):
        with self.robots[name]['lock']:
            self.robots[name]['traj'].append(new_point)
            self.robots[name]['curr'] = new_point

    def set_reached(self, name):
        with self.robots[name]['lock']:
            self.robots[name]['reached'] = True

# ==== 主ROS节点 ====
class TTYwROSPlanner(object):
    def __init__(self):
        rospy.init_node('ttyw_multires_planner')
        self.map = MultiResGridMap()
        self.dyn_obs_mgr = DynamicObstacleManager()
        self.robots = RobotManager()
        self.coarse_factor = rospy.get_param("~coarse_factor", 4)
        self.safe_dist = rospy.get_param("~safe_dist", 0.4)
        robot_list = rospy.get_param("~robot_list", ['robot1'])
        for r in robot_list:
            sx = rospy.get_param(f"~{r}_start", [1.0, 1.0])
            gx = rospy.get_param(f"~{r}_goal", [8.0, 8.0])
            self.robots.add_robot(r, tuple(sx), tuple(gx))
        self.path_pubs = {}
        for r in robot_list:
            self.path_pubs[r] = rospy.Publisher(f"/{r}/ttyw_path", Path, queue_size=1)
        rospy.Subscriber("/map", OccupancyGrid, self.map_callback, queue_size=1)
        self.stop_flag = False
        self.planner_thread = threading.Thread(target=self.planning_loop)
        self.planner_thread.start()

    def map_callback(self, msg):
        self.map.update_map(msg, self.coarse_factor)

    def planning_loop(self):
        rospy.loginfo("等待地图初始化...")
        while not rospy.is_shutdown() and not self.map.ready:
            rospy.sleep(0.1)
        rospy.loginfo("地图已准备，开始主循环。")
        planner = PathPlanner(self.map)
        loop_rate = rospy.Rate(3)  # 3Hz
        # ==== 动态障碍轨迹示例 ====
        traj1 = [(3+1.5*math.sin(i*0.04), 5+0.5*math.cos(i*0.04)) for i in range(500)]
        traj2 = [(6+1.5*math.sin(i*0.06+2), 7+0.5*math.cos(i*0.05)) for i in range(500)]
        self.dyn_obs_mgr.set_obstacle_trajectory('obs1', traj1, velocity=2)
        self.dyn_obs_mgr.set_obstacle_trajectory('obs2', traj2, velocity=1)
        while not rospy.is_shutdown():
            allrobots = self.robots.get_all()
            for name, state in allrobots:
                if state['reached']:
                    continue
                curr = state['curr']
                goal = state['goal']
                path = planner.plan(curr, goal)
                if not path:
                    rospy.logwarn(f"[{name}] 规划失败，等待重试。")
                    continue
                if self.dyn_obs_mgr.check_path_collision(path, safe_dist=self.safe_dist):
                    rospy.logwarn(f"[{name}] 检测到动态障碍即将碰撞，等待或重规划。")
                    continue
                # 向前推进若干点
                next_idx = min(4, len(path)-1)
                next_pos = path[next_idx]
                self.robots.update_traj(name, next_pos)
                # 检查到达
                if np.hypot(next_pos[0]-goal[0], next_pos[1]-goal[1])<self.safe_dist:
                    self.robots.set_reached(name)
                    rospy.loginfo(f"[{name}] 已到达目标点。")
                # 发布路径到RViz
                path_msg = Path()
                path_msg.header.frame_id = "map"
                path_msg.header.stamp = rospy.Time.now()
                for (wx,wy) in path:
                    pose = PoseStamped()
                    pose.pose.position.x = wx
                    pose.pose.position.y = wy
                    pose.pose.position.z = 0.0
                    pose.pose.orientation.w = 1.0
                    path_msg.poses.append(pose)
                self.path_pubs[name].publish(path_msg)
            loop_rate.sleep()

if __name__ == "__main__":
    try:
        planner_node = TTYwROSPlanner()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass