企业官网建设流程全解析

1. 这不是普通路径规划：Faster Planner 里的 Kinodynamic Astar 到底在解决什么问题？

你可能已经用过 A*、Dijkstra 或 RRT 这类经典路径规划算法，也见过 ROS 里 move_base 的全局+局部分层架构。但当你把机器人放到真实场景里——比如一个带差速底盘的 AGV 要在狭窄仓库通道中避开突然出现的叉车，或者一个四轮转向的无人配送车要在 30cm 侧方余量下完成 90°直角转弯入库——你会发现：传统“只考虑位置”的规划器会卡住、抖动、甚至直接报错 abort。它算出来的是一条几何上最短的折线，但没考虑你的电机最大加速度是多少、转向机构响应延迟多大、轮胎有没有滑移临界点。这就是 Kinodynamic Astar（动力学约束下的 A*）存在的根本原因：它不只问“能不能走到”，更问“能不能在物理极限内、以可控方式、安全地走到”。

Faster Planner 正是这一思想的工业级落地代表。它不是学术论文里的玩具实现，而是被实际部署在数百台物流机器人、港口无人集卡和园区巡检车上的开源规划框架。它的核心关键词——Kinodynamic Astar——拆开看，“Kino”指运动学（kinematics），即位姿、速度、角速度等状态变量；“dynamic”指动力学（dynamics），即加速度、角加速度、力矩、摩擦约束等物理限制；而“Astar”则是搜索策略的骨架。三者叠加，意味着它在每一步扩展节点时，不是简单地往八个方向挪一格，而是基于车辆运动学模型（如自行车模型或单轨模型），生成一组满足最大加速度、最大转向角速度、最小转弯半径等硬约束的可行轨迹段，并用动力学可行性作为边权重的一部分参与启发式评估。

我第一次在客户现场调试 Faster Planner 时，就遇到个典型问题：AGV 在窄道掉头时反复振荡，ROS 的 rviz 显示路径看起来很平滑，但底层电机电流曲线像心电图。后来发现，原生的 TEB 局部规划器只做速度重规划，不校验加速度连续性；而 Faster Planner 的 Kinodynamic Astar 在全局阶段就强制要求轨迹二阶导数（加速度）有界且连续，直接从源头掐断了“指令突变→电机过载→打滑→重规划→再突变”的死循环。这背后不是数学炫技，而是对真实执行器物理特性的敬畏。所以如果你正在做轮式/履带式移动机器人、无人机轨迹生成、或任何需要“可执行性保障”的自主系统，Faster Planner + Kinodynamic Astar 就不是可选项，而是必选项——它解决的从来不是“怎么找路”，而是“怎么让机器真正稳稳地走完这条路”。

2. 为什么非得是 Kinodynamic Astar？Faster Planner 的设计哲学与技术取舍

很多人看到 Faster Planner 的 GitHub README 里写着“基于 Kinodynamic Astar”，第一反应是：“哦，又一个 A* 的变种”。但真正深入代码和实车验证后你会发现，这个“变种”背后藏着一套严密的工程权衡体系。它既不是纯采样法（如 RRT*）那种“靠运气覆盖状态空间”的随机性，也不是纯优化法（如 CHOMP、STOMP）那种“每次都要解非线性规划”的高计算开销。它的选择逻辑非常务实：在保证动力学可行性的前提下，把搜索复杂度控制在嵌入式平台可承受范围内。

先说为什么不用纯优化方法。我拿一台搭载 Jetson Orin NX 的物流机器人做过对比测试：用 IPOPT 求解一条 5 米长、含 3 个障碍物的避障轨迹，平均耗时 860ms，且对初始猜测极其敏感——如果起点速度设为 0.5m/s，而实际传感器反馈是 0.3m/s，优化器大概率收敛到不可行解，导致紧急制动。而 Faster Planner 的 Kinodynamic Astar 在同等条件下，搜索时间稳定在 42~68ms（实测中位数 53ms），且只要输入状态合法，输出轨迹必然满足 $|a| \leq a_{max}$、$|\dot{\theta}| \leq \omega_{max}$、$|v| \leq v_{max}$ 等硬约束。这种确定性来自它的状态离散化设计：它把连续的状态空间 $(x,y,\theta,v,\omega)$ 投影到一个五维网格上，每个维度按物理意义做非均匀量化——比如角度 $\theta$ 每 15° 一个桶（共 24 桶），而线速度 $v$ 则按 0.1m/s 步长从 -1.0 到 +2.0 量化（共 31 桶）。这样整个状态空间约 24×24×24×31×21 ≈ 890 万节点，远小于全精度浮点表示的无限空间，又比传统 A* 的二维栅格精细得多。

再看为什么不用纯采样法。RRT* 理论上能渐进最优，但实际部署时有个致命缺陷：它无法保证任意时刻的轨迹都满足加速度约束。RRT 的扩展步长是固定距离，而真实车辆在高速下转向半径大、低速下转向半径小，固定步长会导致低速区轨迹过密、高速区轨迹过疏。更麻烦的是，RRT 生成的路径是分段线性，必须额外接一个轨迹优化模块来插值平滑并施加动力学约束——这等于把问题拆成两段，中间接口极易出错。Faster Planner 的 Kinodynamic Astar 则把运动学模型直接编进状态转移函数里：给定当前状态 $(x,y,\theta,v,\omega)$ 和控制输入 $(a,\alpha)$（加速度、角加速度），它用四阶龙格-库塔法积分 0.1 秒，得到下一状态，并检查该过程是否超出轮胎附着极限（通过 Pacejka 模型简化版估算侧向力）。这个“前向仿真+约束校验”的闭环，确保了每一个被加入 open set 的节点，都是物理世界里真实可达到的。

最后说说它对启发式函数的精妙处理。传统 A* 的启发式 $h(n)$ 是欧氏距离，但在五维状态空间里，单纯用 $(x,y)$ 距离会严重误导搜索——比如两个节点 $(x_1,y_1,\theta_1,v_1,\omega_1)$ 和 $(x_2,y_2,\theta_2,v_2,\omega_2)$，即使位置很近，若朝向差 180°、速度符号相反，实际调整代价可能远超 10 米直线距离。Faster Planner 改用Kinodynamic Heuristic：先计算目标朝向与当前朝向的最小旋转角 $\Delta\theta$，再根据当前角速度 $\omega$ 和最大角加速度 $\alpha_{max}$，估算转向所需时间 $t_\theta = \frac{|\Delta\theta|}{\sqrt{2\alpha_{max}|\omega|}}$（这里用了匀变速公式反推）；同理计算线速度调整时间 $t_v$；最后用 $\sqrt{(x_2-x_1)^2+(y_2-y_1)^2} / v_{max} + t_\theta + t_v$ 作为启发式。这个公式把运动学耦合关系显式编码进来，实测使搜索节点数降低 67%，且首次找到的路径就是动力学可行解的概率从 31% 提升到 92%。

提示：Faster Planner 的 Kinodynamic Astar 不是“为了用 A* 而用 A*”，它是把 A* 当作一个可证明完备性的搜索骨架，把运动学模型当作状态转移规则，把动力学约束当作边有效性判据，三者缺一不可。放弃其中任何一环，都会退化成“纸上谈兵”的规划器。

3. 核心细节拆解：状态空间构建、运动学模型与约束注入机制

要真正吃透 Faster Planner 的 Kinodynamic Astar，必须沉到三个核心细节里：状态空间如何定义与量化、运动学模型如何驱动状态转移、动力学约束如何实时注入搜索过程。这三个环节环环相扣，任何一个参数设错，都会导致规划失败或轨迹发散。下面我结合实车调试中的具体案例，逐层展开。

3.1 状态空间的五维量化：不是越细越好，而是恰到好处

Faster Planner 的状态向量定义为 $s = [x, y, \theta, v, \omega]$，其中 $(x,y)$ 是笛卡尔坐标（单位：米），$\theta$ 是航向角（单位：弧度），$v$ 是线速度（单位：m/s），$\omega$ 是角速度（单位：rad/s）。关键在于，这五个维度不是等间隔采样的。比如 $\theta$ 维度，如果按 0.01745 rad（1°）量化，360° 就要 360 个桶；但实际中，AGV 的转向电机分辨率只有 0.5°，且控制器采样周期 50ms，过细的量化只会徒增内存占用，不提升精度。我们最终采用15° 量化步长（即 $\pi/12$ rad），共 24 个桶，覆盖 $[-\pi, \pi)$ 区间。这个选择的依据是：实测发现，当 $\theta$ 误差超过 10° 时，PID 控制器就开始明显超调；而 15° 量化能保证任意真实朝向都能被最近的桶覆盖，且相邻桶间插值误差 < 0.02 rad（约 1.15°），完全在控制容差内。

$v$ 维度更值得细说。很多用户直接套用文档里的 [-2.0, 2.0] m/s 范围，结果在低速段（0~0.3m/s）轨迹抖动严重。问题出在量化粒度上：若用 0.1m/s 步长，0~0.3m/s 只有 4 个桶（0.0, 0.1, 0.2, 0.3），而车辆在 0.15m/s 时，状态会被强制映射到 0.1 或 0.2，造成指令跳变。我们的解决方案是分段非均匀量化：在 [-0.3, 0.3] 区间用 0.05m/s 步长（共 13 个桶），在 [-2.0, -0.3) 和 (0.3, 2.0] 区间用 0.2m/s 步长（各 9 个桶）。这样总桶数从 41 降到 31，但低速区分辨率提升 4 倍。实测显示，AGV 在 0.1~0.2m/s 区间运行时，速度曲线标准差从 0.08m/s 降至 0.012m/s。

$\omega$ 维度则与转向机构强相关。我们的四轮转向车最大角速度为 ±1.2 rad/s（约 ±69°/s），但电机在 ±0.3 rad/s 以下响应非线性明显。因此，$\omega$ 量化范围设为 [-1.2, 1.2]，步长 0.15 rad/s，共 17 个桶，并在软件里对 [-0.3, 0.3] 区间的桶增加“迟滞补偿”——即当目标 $\omega$ 落在此区间时，优先选择绝对值最小的非零桶（±0.15），避免零速抖动。这个细节在官方文档里没提，却是实车零故障运行的关键。

3.2 运动学模型：从自行车模型到实际控制链路的映射

Faster Planner 默认采用改进型自行车模型（Modified Bicycle Model），其状态转移方程为：

$$ \begin{cases} \dot{x} = v \cos\theta \ \dot{y} = v \sin\theta \ \dot{\theta} = \frac{v}{L} \tan\delta \ \dot{v} = a \ \dot{\omega} = \alpha \end{cases} $$

其中 $L$ 是轴距（单位：米），$\delta$ 是前轮转向角（单位：弧度），$a$ 是线加速度，$\alpha$ 是角加速度。注意，这里 $\delta$ 并非直接控制输入，而是由 $\omega$ 和 $v$ 通过运动学关系反解：$\delta = \arctan(\frac{L \omega}{v})$（当 $v \neq 0$）。这个设计巧妙地把转向角 $\delta$ 从状态中剥离，避免了 $\delta$ 与 $\omega$ 的冗余耦合，同时保证了模型与实际控制链路的一致性——因为底层驱动器接收的正是 $v$ 和 $\omega$ 指令，而非 $\delta$。

但真实世界没这么理想。我们在港口无人集卡上测试时发现，当 $v$ 接近 0 时，$\delta$ 计算会因除零产生 NaN，导致轨迹中断。解决方案是在状态转移函数里加入速度阈值保护：当 $|v| < 0.05$m/s 时，强制设 $\delta = \text{clip}(\omega \cdot L \cdot 0.8, -0.4, 0.4)$（单位：rad），其中 0.8 是经验缩放系数，±0.4rad（约 ±23°）是转向机构机械限位。这个修正让车辆在原地转向时，轨迹平滑度提升 3 倍（用 jerk 指标衡量）。

更重要的是，Faster Planner 把轮胎动力学简化模型编进了状态转移的校验环节。它不直接计算侧向力，而是用经验公式估算滑移率：$\lambda = \frac{|v_y|}{\sqrt{v_x^2+v_y^2+0.01}}$，其中 $v_x,v_y$ 是车身坐标系下的速度分量。当 $\lambda > 0.15$（对应约 15% 滑移）时，判定该状态转移不可行，拒绝扩展。这个阈值来自我们对 Michelin X系列轮胎的实测数据——在干燥沥青路面，滑移率超过 15% 时，侧向力开始非线性衰减，车辆易失控。把这条经验线写进代码，比调用完整 Pacejka 模型节省 92% 计算量，且精度足够工程使用。

3.3 动力学约束注入：从“能算”到“真能跑”的最后一道闸门

Kinodynamic Astar 的灵魂，在于它把动力学约束不是作为后处理，而是作为搜索的“准入门槛”。Faster Planner 的约束注入发生在三个层级：

1. 状态空间边界约束：在初始化时，对每个维度设置硬限。例如，$v$ 的上限不是简单设为 2.0m/s，而是根据电池 SOC（荷电状态）动态调整：SOC > 80% 时 $v_{max}=2.0$，60%~80% 时 $v_{max}=1.7$，<60% 时 $v_{max}=1.4$。这个逻辑写在StateSpace::getMaxVelocity()函数里，避免了低电量时电机过载。

2. 状态转移约束：每次从 open set 取出节点 $s_i$，生成邻居 $s_j$ 时，必须校验：

   • 加速度 $a = (v_j - v_i)/\Delta t$ 满足 $|a| \leq a_{max}(v_i)$，其中 $a_{max}(v_i)$ 是查表函数——因为电机在不同速度下最大加速度不同（高速时受功率限制，低速时受扭矩限制）；
   • 角加速度 $\alpha = (\omega_j - \omega_i)/\Delta t$ 满足 $|\alpha| \leq \alpha_{max}$；
   • 转向角速度 $\dot{\delta} = \frac{d}{dt}\arctan(\frac{L\omega}{v})$ 的绝对值不超过转向电机最大角速度（实测为 2.5 rad/s）。

3. 轨迹段可行性约束：对每个生成的轨迹段（从 $s_i$ 到 $s_j$ 的 RK4 积分结果），进行离散点校验：在积分步长内，每隔 0.02 秒采样一次，检查：

   • 所有采样点是否在 freespace 内（调用 costmap 的getCost()）；
   • 所有采样点的侧向加速度 $a_y = v \cdot \omega$ 是否满足 $|a_y| \leq \mu g$（$\mu=0.8$ 为摩擦系数，$g=9.8$）；
   • 所有采样点的纵向加速度 $a_x$ 是否满足 $|a_x| \leq a_{max}(v)$。

这套三层约束机制，确保了 Faster Planner 输出的每一条路径，都不是“理论上可行”，而是“此刻此地，我的硬件真能跑出来”。我在某次客户验收时，曾故意把 $a_{max}$ 设高 20%，结果车辆在弯道出口急加速时后轮打滑，轨迹立刻失效——这恰恰证明了约束注入的有效性：它不是保守，而是诚实。

4. 实操全流程：从配置参数到实车部署的完整链路

把 Faster Planner 的 Kinodynamic Astar 从 GitHub clone 下来，到真正在机器人上跑出第一条动力学可行轨迹，中间隔着一堆看似琐碎却决定成败的配置项。下面我以一台标准差速底盘 AGV（轮距 0.5m，最大线速 1.5m/s，最大角速 1.2rad/s）为例，还原完整的实操链路，包括每个参数的物理含义、调试技巧和踩过的坑。

4.1 环境准备与依赖安装：别让编译器成为第一个拦路虎

Faster Planner 官方推荐 Ubuntu 20.04 + ROS Noetic，但我们在 Jetson Orin 上用 Ubuntu 22.04 + ROS Humble 也成功部署。关键不是 ROS 版本，而是C++ 标准和 Eigen 版本的匹配。Faster Planner 依赖 Eigen 3.3.x，而 Ubuntu 22.04 自带 Eigen 3.4.x，直接apt install会导致Eigen::Matrix构造函数签名不匹配，编译报错no matching function for call to 'Eigen::Matrix<double, 3, 1>::Matrix()'。解决方案是手动编译 Eigen 3.3.9：

wget https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz tar -xzf eigen-3.3.9.tar.gz cd eigen-3.3.9 mkdir build && cd build cmake -DCMAKE_INSTALL_PREFIX=/usr/local .. sudo make install

然后在 Faster Planner 的CMakeLists.txt中，把find_package(Eigen3 REQUIRED)改为find_package(Eigen3 3.3.9 REQUIRED)。这个坑我们花了 17 小时才定位，教训是：永远先看 CMakeLists 里的版本声明，再看系统包管理器的版本。

ROS 依赖方面，除了标准的ros-humble-navigation2，必须额外安装ros-humble-nav2-bringup和ros-humble-nav2-system-tests（后者提供了nav2_costmap_2d的完整插件，Faster Planner 的 obstacle layer 依赖它）。安装命令：

sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-nav2-system-tests

注意：不要用rosdep install自动解决依赖，它会漏掉nav2-system-tests，导致 costmap 初始化失败，报错Could not load plugin 'obstacle_layer'。

4.2 核心参数配置：每个数字背后都是实车数据

Faster Planner 的主配置文件faster_planner_params.yaml里，最关键的 7 个参数及其调试逻辑如下：

特别强调acc_lim_x和acc_lim_theta：它们不是动力学极限，而是舒适性约束。我们曾把acc_lim_x设为 1.0，车辆在启动时电流峰值达 85A（额定 60A），电机温升 15°C/分钟；降到 0.4 后，峰值电流 52A，温升 3°C/分钟，寿命提升 3 倍。这个参数的调试方法是：用示波器抓取电机驱动器的 PWM 信号，观察上升沿斜率，换算成 jerk，再反推acc_lim_x。

另一个易错点是costmap_topic。Faster Planner 默认订阅/move_base/global_costmap/costmap，但 ROS2 的 Navigation2 默认 topic 是/global_costmap/costmap_raw。必须在 launch 文件里显式 remap：

<param name="costmap_topic" value="/global_costmap/costmap_raw"/>

否则 planner 会收不到 costmap，一直报错Costmap is empty，而日志里没有任何提示——这是个静默失败，排查起来极耗时间。

4.3 轨迹生成与执行闭环：从 plan 到 ctrl 的无缝衔接

Faster Planner 输出的是nav_msgs::Path类型的路径消息，但 ROS 的 controller（如dwb_controller）需要的是geometry_msgs::Twist。中间的转换由trajectory_tracker模块完成，其核心是时间参数化（Time Parameterization）。Faster Planner 本身不生成时间戳，而是输出一系列(x,y,theta,v,omega)点，trajectory_tracker负责给每个点分配时间，并插值生成连续轨迹。

我们发现，默认的quintic_polynomial插值在高速段会产生高频振荡。解决方案是改用梯形速度规划（Trapezoidal Velocity Profile）：对每一段路径，先按最大加速度加速到 $v_{max}$，再匀速，最后按最大减速度停止。这个算法在trajectory_tracker/src/time_parameterizer.cpp里，只需把interpolation_method: "quintic"改为"trapezoidal"。

更关键的是执行器延迟补偿。我们的差速底盘从收到Twist指令到实际速度变化，有 83ms 固定延迟（测自 CAN 总线传输+电机控制器处理）。如果不补偿，planner 会基于“当前速度”预测未来状态，而实际速度滞后，导致轨迹跟踪误差累积。我们在trajectory_tracker的computeControlCmd()函数里，加入了前馈补偿：

// 获取当前指令时间戳 rclcpp::Time now = this->now(); // 预测 83ms 后的状态 double dt_compensate = 0.083; double v_pred = current_v + acc * dt_compensate; double omega_pred = current_omega + alpha * dt_compensate; // 用预测状态生成控制指令 cmd.linear.x = v_pred; cmd.angular.z = omega_pred;

这个 83ms 不是拍脑袋定的，而是用 ROS 的ros2 topic hz工具，分别测cmd_vel发布频率和底盘 odometry 回传频率，取两者时间戳差值的统计中位数。实测补偿后，轨迹跟踪 RMSE 从 0.18m 降至 0.04m。

4.4 实车部署 checklist：一份血泪总结的避坑清单

• [ ]激光雷达坐标系对齐：Faster Planner 假设/laser坐标系与/base_link重合，Z 轴向上。若你的雷达安装偏航角 5°，必须在 URDF 里修正，不能只靠 TF。否则 costmap 的障碍物位置会整体偏移，规划路径擦着墙走。
• [ ]IMU 数据质量：/imu主要用于航向角融合。若 IMU 噪声 RMS > 0.02 rad/s，会导致 $\theta$ 估计漂移，planner 反复重规划。我们加了 10Hz 低通滤波：ros2 run imu_filter_madgwick imu_filter_node --ros-args -p use_mag:=false -p gain:=0.01。
• [ ]costmap 分辨率匹配：global_costmap的resolution必须 ≤ 0.05m。若设为 0.1m，0.3m 宽的货架会被 costmap 合并成单个障碍单元，planner 误判为可通过。
• [ ]TF 时间戳同步：所有 TF（map->odom->base_link->laser）的时间戳必须严格单调递增。若 odom 话题因网络抖动延迟发布，会导致transform timeout错误。解决方案是启用tf2_ros::Buffer的setUsingDedicatedThread(true)。
• [ ]内存泄漏监控：Faster Planner 在高频重规划（>5Hz）时，std::vector动态分配可能引发碎片。我们在planner_core.cpp的plan()函数末尾，加了std::vector<State>().swap(open_set);强制释放内存，使内存占用稳定在 45MB 以内。

5. 常见问题与排查技巧实录：那些文档里不会写的实战真相

Faster Planner 的 Kinodynamic Astar 看似结构清晰，但实车调试中，90% 的问题都不在算法本身，而在环境感知、状态估计与执行器响应的耦合误差。下面是我整理的 7 个高频问题，每个都附带真实日志、定位方法和根治方案——这些内容，你在任何官方文档或 Stack Overflow 里都找不到。

5.1 问题：规划路径频繁重算，rviz 显示路径“跳舞”

现象：机器人静止时，path topic 每 2~3 秒刷新一次，路径形状随机变化，但终点不变。

日志线索：[ WARN] [1712345678.123456789] [faster_planner]: Costmap update rate too low (< 1.0 Hz)
根因分析：这不是 planner 的 bug，而是 costmap 更新频率不足。Faster Planner 每次 plan 前，会检查 costmap 的header.stamp与当前时间差，若 > 1.0s，直接返回失败并触发重试。而 costmap 更新慢，通常是因为obstacle_layer的track_unknown_space设为 true，且激光点云中存在大量inf值（对应远距离无反射），导致raytrace计算耗时飙升。

解决方案：

1. 在costmap_common_params.yaml中，添加：

obstacle_layer: track_unknown_space: false max_obstacle_height: 2.0 obstacle_range: 5.0 # 限制激光有效距离

2. 在激光驱动节点里，过滤inf点：

// 在 laser callback 中 for (auto& r : msg->ranges) { if (r > msg->range_max || r < msg->range_min) r = msg->range_max; }

实测后，costmap 更新率从 0.3Hz 提升到 8.2Hz，路径“跳舞”消失。

5.2 问题：车辆能规划，但执行时总在障碍物前 0.5m 急停

现象：rviz 显示路径离障碍物有 0.8m 余量，但实际运行时，车辆在距障碍 0.5m 处触发emergency_stop。

日志线索：[ERROR] [1712345678.123456789] [dwb_controller]: Cannot find valid velocity command
根因分析：dwb_controller的max_vel_x设为 0.5m/s，而 Faster Planner 的max_vel_x是 1.2m/s。当 planner 输出 1.0m/s 的速度指令，controller 因限幅无法跟踪，持续报错，最终 fallback 到 stop。这不是 planner 问题，而是controller 与 planner 的速度域不匹配。

解决方案：

• 统一速度上限：在controller.yaml中，设max_vel_x: 1.2；
• 同时，在faster_planner_params.yaml中，设min_vel_x: -0.2（保证倒车能力）；
• 关键一步：在dwb_controller的dwb_critics中，禁用oscillationcritic（它会因速度指令抖动而惩罚），改用obstaclecritic 的scale参数强化避障权重。

提示：永远用ros2 topic echo /cmd_vel直接看 controller 输出，而不是只信 rviz 的 path 显示。path 是 planner 的“愿望”，cmd_vel才是 controller 的“行动”。

5.3 问题：原地转向时路径呈螺旋状，且越来越密

现象：目标点就在正前方 1m，但 planner 生成一条绕圈路径，圈数随时间增加。

日志线索：[DEBUG] [1712345678.123456789] [faster_planner]: State (x,y,theta)=(0.0,0.0,0.0) -> (0.0,0.0,3.14) has high theta cost
根因分析：Kinodynamic Astar 的启发式函数对大角度差敏感。当 $\theta$ 从 0 跳到 $\pi$，KinodynamicHeuristic会高估转向时间，导致搜索偏向“先平移再转向”，而平移又受障碍物阻挡，形成死循环。

解决方案：

• 在heuristic.cpp中，修改角度差计算：

double delta_theta = std::abs(angles::shortest_angular_distance(theta1, theta2)); // 原来是 std::abs(theta1 - theta2)，易受 2π 跳变影响

• 同时，在state_space.cpp的getNeighbors()函数里，为原地转向增加专用动作：当 $|v| < 0.01$ 时，允许 $\omega$ 直接跳变到目标值，跳过加速度约束（因为原地转向时，$v=0$，$a$ 无意义）。

这个修改让原地转向路径从螺旋状变为干净的圆弧，规划时间从 1200ms 降至 85ms。

5.4 问题：充电对接时，路径在充电桩前 10cm 处反复横移

现象：充电桩视觉 marker 识别正常，但 planner 生成的路径在对接点前左右摇摆，无法稳定对准。

日志线索：[WARN] [1712345678.123456789] [faster_planner]: Goal state has high curvature, skipping kinodynamic check
根因分析：Faster Planner 对 goal state 有特殊处理：若目标点曲率 > 0.5m⁻¹，会跳过动力学校验，直接用直线连接。而充电桩对接要求毫米级精度，直线路径无法满足转向角约束，导致 controller 持续修正。

解决方案：

• 在 goal callback 中，预生成一个“软目标”：以充电桩中心为圆心，半径 0.05m 的圆弧，取弧中点作为 planner 的 goal；
• 同时，在faster_planner_params.yaml中，设goal_tolerance: 0.02（2cm），yaw_goal_tolerance: 0.05（2.86°）；
• 最关键：在trajectory_tracker里，对接阶段切换为pure_pursuit控制器，用 lookahead distance = 0.15m，比默认的 0.3m 更精准。

这套组合拳让对接成功率从 63% 提升到 99.2%。

5.5 问题：多机协同时，A 机规划路径总被 B 机实时 costmap “吃掉”

现象：两台 AGV 同时运行，A 机刚规划好路径，B 机的 costmap 就把 A 的路径区域标记为 occupied，导致 A 机立即重规划。

日志线索：`[INFO] [1712345678.12345678