一、工作背景与阶段目标阶段一已完成新底盘、Jetson Orin NX、供电链路、UART 通信、Python 3 兼容及 /cmd_vel 底层运动控制验证建立了“Jetson ROS—base_control—串口—底盘控制板—四轮执行”的基础控制链路。阶段二在此基础上进一步向自主导航闭环推进核心任务不再是单纯验证底盘能否响应速度命令而是实现从环境感知、定位建图、轨迹规划、轨迹跟踪到安全到达目标点的完整系统联调。本阶段以 Livox MID360、FAST-LIO2 和 EGO-Planner 为核心针对 EGO-Planner 原生面向四旋翼三维运动的特点完成全向移动底盘的 2.5D 规划适配、轨迹输出桥接、实际状态重规划、末端到点控制和航向角平滑控制。通过多轮问题定位和实机测试最终实现连续点击多个目标点后小车能够稳定规划、行驶、调整航向并安全到达。1. 阶段二主要目标完成 MID360 与 FAST-LIO2 在 Jetson 平台上的稳定运行实现实时定位、点云注册与地图构建将 FAST-LIO2 的 /Odometry 与 /cloud_registered 接入 EGO-Planner统一 world、camera_init 等坐标关系建立 EGO-Planner /planning/pos_cmd 到全向底盘 /cmd_vel 的轨迹跟踪桥接解决原始 EGO-Planner 三维规划逻辑在地面小车上的 Z 高度、地图边界与地面体素问题优化新目标、碰撞、安全重规划和终点到达逻辑使规划更贴合 FAST-LIO 实际状态加入运动方向航向角控制并完成连续多目标实机验证。二、系统总体架构与软件环境1. 硬件组成模块型号/组成作用上位计算平台Jetson Orin NX运行 ROS、FAST-LIO2、EGO-Planner、轨迹跟踪及底盘控制节点激光雷达Livox MID360提供点云与 IMU 数据用于实时定位和环境建图移动平台冰达全向移动底盘接收 /cmd_vel实现 X/Y 平移和航向旋转底盘控制链路base_control UART /dev/ttyTHS0将 ROS Twist 指令转换为底盘协议数据供电24 V 电池 12 V DC-DC底盘与 Jetson 独立供电降低反向供电风险2. 软件组成软件/功能包主要功能Ubuntu 20.04 ROS Noetic系统运行环境与 ROS1 通信框架Livox ROS Driver发布 MID360 点云和 IMU 数据FAST-LIO2激光惯性里程计、点云配准与实时地图构建EGO-Planner局部轨迹生成、B-spline 优化、碰撞检测与重规划ego_ugv_bridge / ego_ugv_tracker将 PositionCommand 转换为适合全向底盘的 Twist 控制命令base_control底盘串口协议收发和 /cmd_vel 执行RViz地图、占据体素、轨迹和目标点交互可视化3. 最终数据流MID360 点云 IMU↓FAST-LIO2/Odometry /cloud_registered↓EGO-Planner地图构建、轨迹规划与安全重规划↓/planning/pos_cmd↓UGV TrackerXY 轨迹跟踪 航向控制 FINAL_APPROACH↓/cmd_vel↓base_control → UART → 全向底盘三、FAST-LIO2 与 MID360 定位建图链路搭建1. MID360 数据链路MID360 通过以太网与 Jetson 建立通信Livox 驱动正常启动后发布激光点云和 IMU 数据。FAST-LIO2 主要使用 /livox/lidar 与 /livox/imu 进行状态估计并输出 /Odometry、/cloud_registered、/Laser_map 和 /path 等话题。实机测试确认点云能够在 RViz 中连续显示雷达数据频率和 IMU 数据均满足后续定位建图要求。核心话题/livox/lidar/livox/imu/Odometry/cloud_registered/Laser_map/path2. FAST-LIO2 实机验证FAST-LIO2 在 Jetson 上完成部署后通过静止初始化、手动推动底盘和连续运动测试验证定位建图稳定性。/Odometry 能够持续输出小车的实际位置与姿态/cloud_registered 输出注册到世界坐标系的点云可直接作为 EGO-Planner GridMap 的环境输入。本阶段后续所有轨迹起点、目标距离、末端到达判断和航向控制均以 FAST-LIO2 的实际 Odometry 为关键反馈从而避免单纯依赖理论轨迹状态造成的累计偏差。四、EGO-Planner 与 FAST-LIO2 接口适配1. 输入输出关系EGO-Planner 原生面向无人机局部三维轨迹规划其主要输入包括里程计和环境点云输出为 quadrotor_msgs/PositionCommand。该消息包含 position、velocity、acceleration、yaw、yaw_dot 等完整轨迹状态。对于全向底盘不能直接使用无人机控制器需要增加 UGV Tracker将世界坐标系下的轨迹速度与位置误差转换为 geometry_msgs/Twist。接口话题用途FAST-LIO2 → EGO/Odometry提供实际位置、姿态和状态反馈FAST-LIO2 → EGO/cloud_registered作为 GridMap 环境点云输入RViz → EGO目标点 Path/Goal触发新的导航任务EGO → Tracker/planning/pos_cmd提供期望位置、速度与轨迹状态Tracker → 底盘/cmd_vel输出全向底盘线速度和角速度控制命令2. 坐标系统一FAST-LIO2 的里程计通常以 camera_init 为参考坐标系EGO-Planner 规划地图采用 world 坐标系。为保证点云、Odometry、规划轨迹和 RViz 可视化在同一参考框架下工作本阶段统一 world 与 FAST-LIO2 世界坐标关系并在运行时检查 TF避免因隐藏的 Z 偏置或旋转关系造成规划位置与实际位置不一致。3. 关闭默认自动航点并采用 RViz 交互目标原有 EGO 示例包含默认航点或预设轨迹逻辑不适合实机小车自由导航测试。本阶段关闭自动航点使系统进入 WAIT_TARGET 状态后等待用户在 RViz 中点击目标点。每次目标点击被定义为新的导航任务并从当前实际 Odometry 状态启动新轨迹规划。五、UGV 专用 2.5D 轨迹规划模式改造1. 原始三维规划在地面小车上的问题EGO-Planner 原本允许 X、Y、Z 三个方向自由优化。用于地面小车后FAST-LIO2 的 body 原点高度、实际地面高度和 GridMap 的 ground_height 不完全一致导致起点和目标 Z 可能落在地图下边界附近甚至地图外。进一步检查发现getInflateOccupancy() 对地图外点返回 -1而部分碰撞判断直接按布尔值使用该返回值非零值会被视为占用从而出现“起点在障碍物”“First 3 control points in obstacles”以及抬起小车后突然能够规划等现象。同时当地图 Z 范围向下扩展后原始 /cloud_registered 中的地面点会进入占据地图。由于 EGO 原生为三维规划器规划器可能将低层地面体素视为障碍并向 Z 方向绕行形成 RViz 中“轨迹向空中拱起”的现象。小车实际仍可沿 XY 投影运动但这种三维自由度对于 UGV 属于无效自由度。2. 固定规划高度与 2.5D 适配为快速获得稳定的地面导航基线本阶段将 UGV 模式规划高度固定为 planning_z 0.10 m。规划起点的 X、Y 来自 FAST-LIO2 当前真实 Odometry目标 X、Y 来自 RViz 点击位置而规划用 Z 统一固定为 0.10 m。FAST-LIO2 的真实 Z 不被修改仅在 EGO 的 UGV 规划层中隔离 Z 漂移和地面高度问题。UGV 规划起点start (odom_x, odom_y, 0.10)UGV 规划目标goal (goal_x, goal_y, 0.10)该策略可定义为“固定规划高度的 2.5D EGO-Planner 适配”定位与建图仍保持三维EGO 在 UGV 模式下主要完成平面运动规划Z 作为固定规划层。这样既保留 EGO 的轨迹优化和障碍约束能力又避免小车无意义地在 Z 方向进行轨迹优化。3. RViz 可视化检查图 1 EGO 占据地图与轨迹俯视/斜视检查图 2 侧视图中规划轨迹与低层占据体素关系说明上述图像用于记录问题定位过程。最终 UGV 稳定基线采用固定 planning_z 与窄 Z 规划层避免继续把三维高度自由度作为小车导航的主要调参方向。六、EGO-Planner 到全向底盘的轨迹跟踪桥接1. UGV Tracker 设计新增 ego_ugv_tracker.py 作为 EGO-Planner 与底盘控制之间的桥接层。Tracker 订阅 /planning/pos_cmd 和 /Odometry依据 EGO 轨迹速度、FAST-LIO 实际位置和实际 yaw 生成 /cmd_vel。该结构将“规划器”与“执行器”解耦使 EGO 核心仍保持轨迹规划职责而底盘专用坐标变换、速度限幅、末端到达与航向控制集中在 Tracker 中实现。2. 世界坐标系到车体坐标系转换EGO PositionCommand 中的 velocity.x、velocity.y 按世界坐标系解释。Tracker 使用 FAST-LIO 当前四元数计算实际 yaw再将世界系速度旋转到小车 body 坐标系。对于全向底盘该方式允许小车在保持任意车头方向时执行 X/Y 平移同时为后续单独控制 yaw 留出独立通道。velocity_world EGO 速度前馈 位置修正[ vx_body ] [ cos(yaw) sin(yaw)] [ vx_world ][ vy_body ] [-sin(yaw) cos(yaw)] [ vy_world ]3. 速度标定与动力学参数匹配为避免规划速度与底盘实际执行能力不匹配本阶段在满载地面条件下进行低速速度标定。测试结果表明底盘实际速度与命令速度接近无需采用 1.52.0 倍的经验补偿系数。命令速度实测平均速度跟随比例结论0.030 m/s约 0.029 m/s约 96.6%低速可稳定执行0.050 m/s约 0.048 m/s约 96.9%跟随良好0.080 m/s约 0.079 m/s约 98.4%接近命令值当前 UGV 稳定测试基线将 EGO 最大速度设置为约 0.08 m/s、最大加速度设置为约 0.15 m/s²使规划速度、Tracker 输出能力和底盘实际执行能力基本一致。该参数用于安全验证和算法链路调通后续若提高平台速度应重新进行动力学标定。七、底盘长时间运行异常与安全控制链路修正1. 问题现象阶段一的短时 /cmd_vel 测试中底盘能够正常运动并在停止发布命令后停车。但在阶段二将 EGO、Tracker 与 base_control 长时间同时运行后出现更复杂的通信行为短时间控制正常运行一段时间后新速度命令可能无法正常执行关闭 Tracker 后底盘偶尔仍保持上一速度严重时需要断电重启底盘才能恢复。2. 原因分析检查 base_control.py 后发现节点除接收 /cmd_vel 外还周期性发送里程计查询和电池查询。底盘 MCU 的失联保护依据“是否长时间未收到任何有效协议数据”而不是单独判断速度命令是否超时。因此即使 /cmd_vel 已停止只要 odom/battery 查询仍持续发送通信链路仍被认为存活底盘可能继续保持最后一次速度状态。3. 当前稳定化处理为优先完成 EGO 导航链路验证本阶段采用临时工程稳定方案禁用周期 odom 与 battery 查询定时器保留通信接收处理运行时主要由 Tracker 发送速度控制帧。该配置经过 120 s、183 s 等多轮持续控制测试可完成前进、斜向、旋转、反向及零速停止未再出现之前的持续保持速度问题。说明该处理属于当前 UGV 实验基线。后续若长期工程化使用应进一步在 base_control 中加入串口互斥、写超时、速度 watchdog、shutdown 连续零速帧及有限握手机制而不是永久依赖关闭状态查询。八、基于实际状态的重新规划机制改造1. 新目标从真实 Odometry 开始规划原 EGO 在部分重规划场景中会沿用旧轨迹的理论状态。对于实际底盘理论轨迹与真实运动之间可能存在偏差尤其在连续点击多个目标点时新轨迹若从旧理论位置生成会出现轨迹起点与小车当前位置不一致。为此本阶段将人工点击的新目标统一视为新的导航任务进入 GEN_NEW_TRAJ并以最新 FAST-LIO Odometry 作为实际 XY 起点。RViz 新目标↓NEW_GOAL_ODOM↓GEN_NEW_TRAJ↓start XY 当前 FAST-LIO 实际位置2. 轨迹结束后的实际目标检查增加 ACTUAL_GOAL_CHECK 逻辑。当 B-spline 轨迹时间结束时不再仅根据理论轨迹状态判断任务完成而是计算当前 FAST-LIO 实际位置与最终目标之间的 XY 距离。若仍未进入末端接近范围则从实际位置重新生成轨迹若已进入末端范围则交由 FINAL_APPROACH 完成最后一段精确到达。3. 实际偏离和停滞触发为减少无意义的周期重规划UGV 模式关闭基于理论轨迹时间阈值的普通重规划改为更接近实机状态的触发逻辑实际车辆持续偏离当前 B-spline 超过阈值时触发 ACTUAL_DEVIATION长时间实际位移不足时触发 ACTUAL_STUCK确认未来轨迹发生占用碰撞时触发 SAFETY 重规划。该策略符合“车辆真正走歪、停滞或有碰撞风险时再重规划”的原则。九、安全碰撞检测与实际位置重规划1. 原始 SAFETY 频繁触发问题早期测试中单次 occupancy hit 即可能触发 SAFETY 重规划。由于占据地图存在噪声、边界体素和低层地面体素单次命中容易造成频繁轨迹替换若重规划继续从旧理论轨迹状态开始还可能出现轨迹折返或起点跳变。2. 三次连续命中确认UGV 模式加入安全去抖机制规划轨迹采样点连续三次检测到占用后才确认轨迹碰撞。日志以 1/3、2/3、3/3 形式记录既保留了安全敏感性又降低单次瞬态噪声造成的误触发。UGV_SAFETY_DEBOUNCE 1/3UGV_SAFETY_DEBOUNCE 2/3UGV_SAFETY_CONFIRMED 3 consecutive↓SAFETY_ACTUAL_ODOM3. SAFETY_ACTUAL_ODOM确认碰撞后不再调用基于旧理论轨迹状态的 planFromCurrentTraj()而是将 have_target_ 保持为真并切换到 GEN_NEW_TRAJ使新的安全轨迹从 FAST-LIO 当前真实位置重新规划。该修改显著提高了重规划轨迹与实际车辆状态的一致性。十、末端精确到达机制 FINAL_APPROACH1. EGO 终点附近的短轨迹死区EGO-Planner 在局部目标距离过近时会拒绝继续生成过短的 B-spline 轨迹。实机表现为小车已经接近目标但 EGO 不再持续生成新的 PositionCommand导致车辆可能停在目标附近而无法稳定进入到达容差。2.FINAL_APPROACH 状态为跨越该终端死区Tracker 增加 FINAL_APPROACH。正常 EGO 轨迹跟踪阶段持续执行 /planning/pos_cmd当实际位置进入约 0.22 m 的终点接近范围后Tracker 不再依赖新的 PositionCommand而是直接根据“最终目标 XY - FAST-LIO 当前实际 XY”计算低速 P 位置闭环。距离目标 0.22 m正常 EGO 轨迹跟踪↓距离目标 ≤ 0.22 m进入 FINAL_APPROACH↓FAST-LIO 实际位置 目标 XY → 低速 P 闭环↓进入到达容差ARRIVED → 发布零速度并锁定停车当前末端接近参数基线为 final_approach_kp≈0.25、最大末端速度约 0.03 m/s。该机制已多次实机验证可在 EGO 停止生成短轨迹后继续完成最后几十厘米的精确靠近。十一、运动方向航向角控制1. 初始航向控制问题第一版航向控制直接使用 EGO 当前 XY 轨迹速度方向作为 desired_yaw。虽然没有使用 cmd.yaw但在低速、重规划或轨迹切线突变时atan2(vy,vx) 得到的候选方向仍可能突然变化造成车头大幅旋转甚至出现接近 ±180° 的方向跳变。2. 平滑航向参考机制最终采用“候选运动方向 航向参考限速”的平滑控制方式。EGO 当前世界系 XY 运动方向只作为 candidate_yaw真正用于闭环的 yaw_reference 以有限变化速率逐步接近 candidate_yaw。每次新 trajectory_id 到来时重置航向参考从 FAST-LIO 当前实际 yaw 重新建立平滑过渡避免新轨迹继承旧轨迹航向。candidate_yaw atan2(vy_world, vx_world)↓yaw_reference 按最大变化速率平滑更新↓yaw_error wrap(yaw_reference - actual_yaw)↓死区 P 控制 角速度限幅↓/cmd_vel.angular.z3. 当前稳定航向参数参数当前值作用enable_yaw_controltrue启用正常轨迹阶段航向控制kp_yaw0.4航向误差比例增益yaw_deadband_deg10.0°小角度死区降低来回抖动yaw_update_min_speed0.03 m/s低于该速度不更新航向参考min_angular_speed0.15 rad/s克服底盘低速旋转死区max_angular_speed0.20 rad/s限制最大旋转速度yaw_reference_slew_rate_deg10.0°/s限制期望航向变化速率yaw_control_min_goal_distance0.35 m接近终点后提前停止航向调整4. 实机验证结果完成航向控制后连续选取 5 个不同方向的目标点进行实机测试小车均能正常生成轨迹、平移行驶、缓慢调整车头方向、进入末端接近并安全停车。测试过程中未再出现此前的航向突变、异常原地大角度旋转或因航向控制导致的导航失败。测试
（二）冰达小车改造——fastlio2+ego planner自主导航
发布时间：2026/7/23 13:45:54
一、工作背景与阶段目标阶段一已完成新底盘、Jetson Orin NX、供电链路、UART 通信、Python 3 兼容及 /cmd_vel 底层运动控制验证建立了“Jetson ROS—base_control—串口—底盘控制板—四轮执行”的基础控制链路。阶段二在此基础上进一步向自主导航闭环推进核心任务不再是单纯验证底盘能否响应速度命令而是实现从环境感知、定位建图、轨迹规划、轨迹跟踪到安全到达目标点的完整系统联调。本阶段以 Livox MID360、FAST-LIO2 和 EGO-Planner 为核心针对 EGO-Planner 原生面向四旋翼三维运动的特点完成全向移动底盘的 2.5D 规划适配、轨迹输出桥接、实际状态重规划、末端到点控制和航向角平滑控制。通过多轮问题定位和实机测试最终实现连续点击多个目标点后小车能够稳定规划、行驶、调整航向并安全到达。1. 阶段二主要目标完成 MID360 与 FAST-LIO2 在 Jetson 平台上的稳定运行实现实时定位、点云注册与地图构建将 FAST-LIO2 的 /Odometry 与 /cloud_registered 接入 EGO-Planner统一 world、camera_init 等坐标关系建立 EGO-Planner /planning/pos_cmd 到全向底盘 /cmd_vel 的轨迹跟踪桥接解决原始 EGO-Planner 三维规划逻辑在地面小车上的 Z 高度、地图边界与地面体素问题优化新目标、碰撞、安全重规划和终点到达逻辑使规划更贴合 FAST-LIO 实际状态加入运动方向航向角控制并完成连续多目标实机验证。二、系统总体架构与软件环境1. 硬件组成模块型号/组成作用上位计算平台Jetson Orin NX运行 ROS、FAST-LIO2、EGO-Planner、轨迹跟踪及底盘控制节点激光雷达Livox MID360提供点云与 IMU 数据用于实时定位和环境建图移动平台冰达全向移动底盘接收 /cmd_vel实现 X/Y 平移和航向旋转底盘控制链路base_control UART /dev/ttyTHS0将 ROS Twist 指令转换为底盘协议数据供电24 V 电池 12 V DC-DC底盘与 Jetson 独立供电降低反向供电风险2. 软件组成软件/功能包主要功能Ubuntu 20.04 ROS Noetic系统运行环境与 ROS1 通信框架Livox ROS Driver发布 MID360 点云和 IMU 数据FAST-LIO2激光惯性里程计、点云配准与实时地图构建EGO-Planner局部轨迹生成、B-spline 优化、碰撞检测与重规划ego_ugv_bridge / ego_ugv_tracker将 PositionCommand 转换为适合全向底盘的 Twist 控制命令base_control底盘串口协议收发和 /cmd_vel 执行RViz地图、占据体素、轨迹和目标点交互可视化3. 最终数据流MID360 点云 IMU↓FAST-LIO2/Odometry /cloud_registered↓EGO-Planner地图构建、轨迹规划与安全重规划↓/planning/pos_cmd↓UGV TrackerXY 轨迹跟踪 航向控制 FINAL_APPROACH↓/cmd_vel↓base_control → UART → 全向底盘三、FAST-LIO2 与 MID360 定位建图链路搭建1. MID360 数据链路MID360 通过以太网与 Jetson 建立通信Livox 驱动正常启动后发布激光点云和 IMU 数据。FAST-LIO2 主要使用 /livox/lidar 与 /livox/imu 进行状态估计并输出 /Odometry、/cloud_registered、/Laser_map 和 /path 等话题。实机测试确认点云能够在 RViz 中连续显示雷达数据频率和 IMU 数据均满足后续定位建图要求。核心话题/livox/lidar/livox/imu/Odometry/cloud_registered/Laser_map/path2. FAST-LIO2 实机验证FAST-LIO2 在 Jetson 上完成部署后通过静止初始化、手动推动底盘和连续运动测试验证定位建图稳定性。/Odometry 能够持续输出小车的实际位置与姿态/cloud_registered 输出注册到世界坐标系的点云可直接作为 EGO-Planner GridMap 的环境输入。本阶段后续所有轨迹起点、目标距离、末端到达判断和航向控制均以 FAST-LIO2 的实际 Odometry 为关键反馈从而避免单纯依赖理论轨迹状态造成的累计偏差。四、EGO-Planner 与 FAST-LIO2 接口适配1. 输入输出关系EGO-Planner 原生面向无人机局部三维轨迹规划其主要输入包括里程计和环境点云输出为 quadrotor_msgs/PositionCommand。该消息包含 position、velocity、acceleration、yaw、yaw_dot 等完整轨迹状态。对于全向底盘不能直接使用无人机控制器需要增加 UGV Tracker将世界坐标系下的轨迹速度与位置误差转换为 geometry_msgs/Twist。接口话题用途FAST-LIO2 → EGO/Odometry提供实际位置、姿态和状态反馈FAST-LIO2 → EGO/cloud_registered作为 GridMap 环境点云输入RViz → EGO目标点 Path/Goal触发新的导航任务EGO → Tracker/planning/pos_cmd提供期望位置、速度与轨迹状态Tracker → 底盘/cmd_vel输出全向底盘线速度和角速度控制命令2. 坐标系统一FAST-LIO2 的里程计通常以 camera_init 为参考坐标系EGO-Planner 规划地图采用 world 坐标系。为保证点云、Odometry、规划轨迹和 RViz 可视化在同一参考框架下工作本阶段统一 world 与 FAST-LIO2 世界坐标关系并在运行时检查 TF避免因隐藏的 Z 偏置或旋转关系造成规划位置与实际位置不一致。3. 关闭默认自动航点并采用 RViz 交互目标原有 EGO 示例包含默认航点或预设轨迹逻辑不适合实机小车自由导航测试。本阶段关闭自动航点使系统进入 WAIT_TARGET 状态后等待用户在 RViz 中点击目标点。每次目标点击被定义为新的导航任务并从当前实际 Odometry 状态启动新轨迹规划。五、UGV 专用 2.5D 轨迹规划模式改造1. 原始三维规划在地面小车上的问题EGO-Planner 原本允许 X、Y、Z 三个方向自由优化。用于地面小车后FAST-LIO2 的 body 原点高度、实际地面高度和 GridMap 的 ground_height 不完全一致导致起点和目标 Z 可能落在地图下边界附近甚至地图外。进一步检查发现getInflateOccupancy() 对地图外点返回 -1而部分碰撞判断直接按布尔值使用该返回值非零值会被视为占用从而出现“起点在障碍物”“First 3 control points in obstacles”以及抬起小车后突然能够规划等现象。同时当地图 Z 范围向下扩展后原始 /cloud_registered 中的地面点会进入占据地图。由于 EGO 原生为三维规划器规划器可能将低层地面体素视为障碍并向 Z 方向绕行形成 RViz 中“轨迹向空中拱起”的现象。小车实际仍可沿 XY 投影运动但这种三维自由度对于 UGV 属于无效自由度。2. 固定规划高度与 2.5D 适配为快速获得稳定的地面导航基线本阶段将 UGV 模式规划高度固定为 planning_z 0.10 m。规划起点的 X、Y 来自 FAST-LIO2 当前真实 Odometry目标 X、Y 来自 RViz 点击位置而规划用 Z 统一固定为 0.10 m。FAST-LIO2 的真实 Z 不被修改仅在 EGO 的 UGV 规划层中隔离 Z 漂移和地面高度问题。UGV 规划起点start (odom_x, odom_y, 0.10)UGV 规划目标goal (goal_x, goal_y, 0.10)该策略可定义为“固定规划高度的 2.5D EGO-Planner 适配”定位与建图仍保持三维EGO 在 UGV 模式下主要完成平面运动规划Z 作为固定规划层。这样既保留 EGO 的轨迹优化和障碍约束能力又避免小车无意义地在 Z 方向进行轨迹优化。3. RViz 可视化检查图 1 EGO 占据地图与轨迹俯视/斜视检查图 2 侧视图中规划轨迹与低层占据体素关系说明上述图像用于记录问题定位过程。最终 UGV 稳定基线采用固定 planning_z 与窄 Z 规划层避免继续把三维高度自由度作为小车导航的主要调参方向。六、EGO-Planner 到全向底盘的轨迹跟踪桥接1. UGV Tracker 设计新增 ego_ugv_tracker.py 作为 EGO-Planner 与底盘控制之间的桥接层。Tracker 订阅 /planning/pos_cmd 和 /Odometry依据 EGO 轨迹速度、FAST-LIO 实际位置和实际 yaw 生成 /cmd_vel。该结构将“规划器”与“执行器”解耦使 EGO 核心仍保持轨迹规划职责而底盘专用坐标变换、速度限幅、末端到达与航向控制集中在 Tracker 中实现。2. 世界坐标系到车体坐标系转换EGO PositionCommand 中的 velocity.x、velocity.y 按世界坐标系解释。Tracker 使用 FAST-LIO 当前四元数计算实际 yaw再将世界系速度旋转到小车 body 坐标系。对于全向底盘该方式允许小车在保持任意车头方向时执行 X/Y 平移同时为后续单独控制 yaw 留出独立通道。velocity_world EGO 速度前馈 位置修正[ vx_body ] [ cos(yaw) sin(yaw)] [ vx_world ][ vy_body ] [-sin(yaw) cos(yaw)] [ vy_world ]3. 速度标定与动力学参数匹配为避免规划速度与底盘实际执行能力不匹配本阶段在满载地面条件下进行低速速度标定。测试结果表明底盘实际速度与命令速度接近无需采用 1.52.0 倍的经验补偿系数。命令速度实测平均速度跟随比例结论0.030 m/s约 0.029 m/s约 96.6%低速可稳定执行0.050 m/s约 0.048 m/s约 96.9%跟随良好0.080 m/s约 0.079 m/s约 98.4%接近命令值当前 UGV 稳定测试基线将 EGO 最大速度设置为约 0.08 m/s、最大加速度设置为约 0.15 m/s²使规划速度、Tracker 输出能力和底盘实际执行能力基本一致。该参数用于安全验证和算法链路调通后续若提高平台速度应重新进行动力学标定。七、底盘长时间运行异常与安全控制链路修正1. 问题现象阶段一的短时 /cmd_vel 测试中底盘能够正常运动并在停止发布命令后停车。但在阶段二将 EGO、Tracker 与 base_control 长时间同时运行后出现更复杂的通信行为短时间控制正常运行一段时间后新速度命令可能无法正常执行关闭 Tracker 后底盘偶尔仍保持上一速度严重时需要断电重启底盘才能恢复。2. 原因分析检查 base_control.py 后发现节点除接收 /cmd_vel 外还周期性发送里程计查询和电池查询。底盘 MCU 的失联保护依据“是否长时间未收到任何有效协议数据”而不是单独判断速度命令是否超时。因此即使 /cmd_vel 已停止只要 odom/battery 查询仍持续发送通信链路仍被认为存活底盘可能继续保持最后一次速度状态。3. 当前稳定化处理为优先完成 EGO 导航链路验证本阶段采用临时工程稳定方案禁用周期 odom 与 battery 查询定时器保留通信接收处理运行时主要由 Tracker 发送速度控制帧。该配置经过 120 s、183 s 等多轮持续控制测试可完成前进、斜向、旋转、反向及零速停止未再出现之前的持续保持速度问题。说明该处理属于当前 UGV 实验基线。后续若长期工程化使用应进一步在 base_control 中加入串口互斥、写超时、速度 watchdog、shutdown 连续零速帧及有限握手机制而不是永久依赖关闭状态查询。八、基于实际状态的重新规划机制改造1. 新目标从真实 Odometry 开始规划原 EGO 在部分重规划场景中会沿用旧轨迹的理论状态。对于实际底盘理论轨迹与真实运动之间可能存在偏差尤其在连续点击多个目标点时新轨迹若从旧理论位置生成会出现轨迹起点与小车当前位置不一致。为此本阶段将人工点击的新目标统一视为新的导航任务进入 GEN_NEW_TRAJ并以最新 FAST-LIO Odometry 作为实际 XY 起点。RViz 新目标↓NEW_GOAL_ODOM↓GEN_NEW_TRAJ↓start XY 当前 FAST-LIO 实际位置2. 轨迹结束后的实际目标检查增加 ACTUAL_GOAL_CHECK 逻辑。当 B-spline 轨迹时间结束时不再仅根据理论轨迹状态判断任务完成而是计算当前 FAST-LIO 实际位置与最终目标之间的 XY 距离。若仍未进入末端接近范围则从实际位置重新生成轨迹若已进入末端范围则交由 FINAL_APPROACH 完成最后一段精确到达。3. 实际偏离和停滞触发为减少无意义的周期重规划UGV 模式关闭基于理论轨迹时间阈值的普通重规划改为更接近实机状态的触发逻辑实际车辆持续偏离当前 B-spline 超过阈值时触发 ACTUAL_DEVIATION长时间实际位移不足时触发 ACTUAL_STUCK确认未来轨迹发生占用碰撞时触发 SAFETY 重规划。该策略符合“车辆真正走歪、停滞或有碰撞风险时再重规划”的原则。九、安全碰撞检测与实际位置重规划1. 原始 SAFETY 频繁触发问题早期测试中单次 occupancy hit 即可能触发 SAFETY 重规划。由于占据地图存在噪声、边界体素和低层地面体素单次命中容易造成频繁轨迹替换若重规划继续从旧理论轨迹状态开始还可能出现轨迹折返或起点跳变。2. 三次连续命中确认UGV 模式加入安全去抖机制规划轨迹采样点连续三次检测到占用后才确认轨迹碰撞。日志以 1/3、2/3、3/3 形式记录既保留了安全敏感性又降低单次瞬态噪声造成的误触发。UGV_SAFETY_DEBOUNCE 1/3UGV_SAFETY_DEBOUNCE 2/3UGV_SAFETY_CONFIRMED 3 consecutive↓SAFETY_ACTUAL_ODOM3. SAFETY_ACTUAL_ODOM确认碰撞后不再调用基于旧理论轨迹状态的 planFromCurrentTraj()而是将 have_target_ 保持为真并切换到 GEN_NEW_TRAJ使新的安全轨迹从 FAST-LIO 当前真实位置重新规划。该修改显著提高了重规划轨迹与实际车辆状态的一致性。十、末端精确到达机制 FINAL_APPROACH1. EGO 终点附近的短轨迹死区EGO-Planner 在局部目标距离过近时会拒绝继续生成过短的 B-spline 轨迹。实机表现为小车已经接近目标但 EGO 不再持续生成新的 PositionCommand导致车辆可能停在目标附近而无法稳定进入到达容差。2.FINAL_APPROACH 状态为跨越该终端死区Tracker 增加 FINAL_APPROACH。正常 EGO 轨迹跟踪阶段持续执行 /planning/pos_cmd当实际位置进入约 0.22 m 的终点接近范围后Tracker 不再依赖新的 PositionCommand而是直接根据“最终目标 XY - FAST-LIO 当前实际 XY”计算低速 P 位置闭环。距离目标 0.22 m正常 EGO 轨迹跟踪↓距离目标 ≤ 0.22 m进入 FINAL_APPROACH↓FAST-LIO 实际位置 目标 XY → 低速 P 闭环↓进入到达容差ARRIVED → 发布零速度并锁定停车当前末端接近参数基线为 final_approach_kp≈0.25、最大末端速度约 0.03 m/s。该机制已多次实机验证可在 EGO 停止生成短轨迹后继续完成最后几十厘米的精确靠近。十一、运动方向航向角控制1. 初始航向控制问题第一版航向控制直接使用 EGO 当前 XY 轨迹速度方向作为 desired_yaw。虽然没有使用 cmd.yaw但在低速、重规划或轨迹切线突变时atan2(vy,vx) 得到的候选方向仍可能突然变化造成车头大幅旋转甚至出现接近 ±180° 的方向跳变。2. 平滑航向参考机制最终采用“候选运动方向 航向参考限速”的平滑控制方式。EGO 当前世界系 XY 运动方向只作为 candidate_yaw真正用于闭环的 yaw_reference 以有限变化速率逐步接近 candidate_yaw。每次新 trajectory_id 到来时重置航向参考从 FAST-LIO 当前实际 yaw 重新建立平滑过渡避免新轨迹继承旧轨迹航向。candidate_yaw atan2(vy_world, vx_world)↓yaw_reference 按最大变化速率平滑更新↓yaw_error wrap(yaw_reference - actual_yaw)↓死区 P 控制 角速度限幅↓/cmd_vel.angular.z3. 当前稳定航向参数参数当前值作用enable_yaw_controltrue启用正常轨迹阶段航向控制kp_yaw0.4航向误差比例增益yaw_deadband_deg10.0°小角度死区降低来回抖动yaw_update_min_speed0.03 m/s低于该速度不更新航向参考min_angular_speed0.15 rad/s克服底盘低速旋转死区max_angular_speed0.20 rad/s限制最大旋转速度yaw_reference_slew_rate_deg10.0°/s限制期望航向变化速率yaw_control_min_goal_distance0.35 m接近终点后提前停止航向调整4. 实机验证结果完成航向控制后连续选取 5 个不同方向的目标点进行实机测试小车均能正常生成轨迹、平移行驶、缓慢调整车头方向、进入末端接近并安全停车。测试过程中未再出现此前的航向突变、异常原地大角度旋转或因航向控制导致的导航失败。测试
![[Android] 可视化音乐制作 -短视频超火的音乐视频制作工具](http://pic.xiahunao.cn/yaotu/[Android] 可视化音乐制作 -短视频超火的音乐视频制作工具)
