039、YOLOv8改进实战：LSK大核选择注意力机制原理与C2f_LSK模块代码实现
2026/7/23 14:34:19
一、工作背景与阶段目标
阶段一已完成新底盘、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 Tracker：XY 轨迹跟踪 + 航向控制 + 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
/path
2. 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.5～2.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/3
UGV_SAFETY_DEBOUNCE 2/3
UGV_SAFETY_CONFIRMED 3 consecutive
↓
SAFETY_ACTUAL_ODOM
3. SAFETY_ACTUAL_ODOM
确认碰撞后，不再调用基于旧理论轨迹状态的 planFromCurrentTraj()，而是将 have_target_ 保持为真并切换到 GEN_NEW_TRAJ，使新的安全轨迹从 FAST-LIO 当前真实位置重新规划。该修改显著提高了重规划轨迹与实际车辆状态的一致性。
十、末端精确到达机制 FINAL_APPROACH
1. 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.z
3. 当前稳定航向参数
参数 | 当前值 | 作用 |
enable_yaw_control | true | 启用正常轨迹阶段航向控制 |
kp_yaw | 0.4 | 航向误差比例增益 |
yaw_deadband_deg | 10.0° | 小角度死区，降低来回抖动 |
yaw_update_min_speed | 0.03 m/s | 低于该速度不更新航向参考 |
min_angular_speed | 0.15 rad/s | 克服底盘低速旋转死区 |
max_angular_speed | 0.20 rad/s | 限制最大旋转速度 |
yaw_reference_slew_rate_deg | 10.0°/s | 限制期望航向变化速率 |
yaw_control_min_goal_distance | 0.35 m | 接近终点后提前停止航向调整 |
4. 实机验证结果
完成航向控制后连续选取 5 个不同方向的目标点进行实机测试，小车均能正常生成轨迹、平移行驶、缓慢调整车头方向、进入末端接近并安全停车。测试过程中未再出现此前的航向突变、异常原地大角度旋转或因航向控制导致的导航失败。
测试