基于三次样条插值的双足机器人多步态平滑切换仿真

项目概述

本教程在 ROS 2 Humble 中搭建一个平面双足机器人：躯干 + 双腿，每条腿 3 个关节（髋、膝、踝），共 6 个主动关节。 机器人依次经历 站立 → 行走 → 快步 → 跑步 → 行走 → 站立 六段步态，每段之间用三次 Hermite 样条完成平滑切换。

重点不是单一步态的步态生成，而是多步态切换的连续性：切换瞬间如果直接跳变关节目标，关节角速度/角加速度会出现尖峰； 三次样条同时匹配切换起点与终点的位置和速度，使关节运动在 C1 意义下连续。项目还运行一组“直接切换”对照实验，用实测数据量化平滑效果。

系统结构

controller_node (200 Hz)
   │  /joint_cmd (期望角度 + 角速度)
   ▼
sim_node (500 Hz, 子步进 PD 伺服)
   │  /joint_states (实际角度)
   │  /biped_metrics (质心、ZMP、裕度、双脚位置/接触)
   ▼
logger_node (100 Hz 固定网格) → trajectory.csv

• 控制器只使用自身规划，不读取任何外部定位；
• 仿真节点用带速度前馈的 PD 伺服跟踪期望角，关节限位做饱和保护；
• 记录节点以自身单调时钟按 100 Hz 固定网格打时间戳（time = 起始时刻 + 序号/rate），CSV 相邻行严格等间隔，不再使用控制器消息中四舍五入的 elapsed；
• 接触状态由控制器按规划相位给出，仿真节点用期望关节角 qdes 的 FK 构造支撑多边形；ZMP 由期望质心轨迹的平滑二阶差分计算，实际质心与跟踪误差单独统计。

步态定义

单腿相位 p 在 [0, 1) 内循环，左右腿相差 0.5。支撑相占比大于 0.5 时存在双支撑（行走/快步），小于 0.5 时存在腾空（跑步）。

摆动脚相对髋部的位置：

支撑相：x_rel = A - 2A·p/s        （A = s·step）
摆动相：u = (p - s)/(1 - s)
        x_rel = -A + 2A·(3u² - 2u³)
        z_rel = clearance·16u²(1-u)²

摆动相的水平与竖直曲线在两端速度均为 0，减少落足冲击。

三次 Hermite 样条切换

设切换时刻为 t0，过渡时长 T = 0.6 s。记录当前 6 个关节角 q0 与角速度 dq0， 并预测目标步态在 t1 = t0 + T 时的角度 q1 与角速度 dq1。过渡期内：

u  = (t - t0) / T
h00 = 2u³ - 3u² + 1      h10 = u³ - 2u² + u
h01 = -2u³ + 3u²        h11 = u³ - u²
q(t) = h00·q0 + h10·T·dq0 + h01·q1 + h11·T·dq1

该式为标准三次 Hermite 插值：端点位置精确，端点速度精确，因而位置与速度连续（C1）。 控制器同时使用该式的解析导数作为 /joint_cmd 的速度前馈，切换首拍不会把速度发布成 0。 切换期间前进速度也从旧步态速度平滑过渡到新步态速度（smoothstep），避免基座速度突变。

本教程中的“三次样条”指分段三次 Hermite 插值；自然三次样条（C2 连续、端点二阶导为零）并未在主流程中使用。

运行方法

mkdir -p ~/biped_ws/src
ln -s /path/to/biped_cubic_spline_gait_switch ~/biped_ws/src/
cd ~/biped_ws
colcon build --packages-select biped_cubic_spline_gait_switch --symlink-install
source install/setup.bash
cd src/humanoid/biped_cubic_spline_gait_switch
python3 main.py

main.py 会依次运行 spline 与 direct 两组 34 s 实验，并调用 tools/analyze.py 生成指标、图表与 GIF。

实验结果

34 s 调度（站立 3 s → 行走 6 s → 快步 6 s → 跑步 6 s → 行走 6 s → 站立 7 s），100 Hz 记录，共 3401 条严格等间隔采样。对照组在相同参数下把切换方式改为直接跳变。

切换发生在 3 / 9 / 15 / 21 / 27 s。pytest 对 5 次切换逐次断言首拍速度与上一拍相同（使用规划器解析导数）；CSV 中 0.530 rad/s 的最大表观左右差出现在 21.006 s（跑步→行走），是因为 100 Hz 记录点在切换后 6 ms、过渡已开始演化，并非真实跳变。最大相邻采样 qddes 差 0.550 rad/s 出现在 21.126 s，属于 21.0 s 的切换窗口（跑步→行走）；直接切换同窗口为 149.811 rad/s。

jerk/角加速度是差分指标，会随分析网格变化：Hermite 只保证 C1，过渡起止点的加速度仍有跳变，因此 jerk 同样不收敛。下表把同一份 100 Hz 记录插值到更细网格，仅用于说明分辨率敏感性；主结论采用切换时刻 qddes 左右极限差。

接触状态按规划相位统计，walk / fast_walk 腾空为 0、run 段为 0.255（理论 0.24）、整体 0.045；步数 50（排除切换边界）；ZMP 裕度由 qdes 的 FK 支撑多边形得到，为规划几何量；行走/跑步阶段为负是因为本仿真为开环轨迹跟踪、未加入平衡控制器。

关节轨迹

上图为 6 个关节的期望角（实线）与实际角（虚线），橙色区间为 0.6 s 切换过渡。切换期间角度、角速度均连续。

质心与 ZMP

切换平滑性对比

三次样条切换与直接切换在同一调度、同一 PD 参数下对比：切换时刻命令速度左右极限差与相邻采样差均显著下降；jerk 与角加速度作为同网格参考，并附分辨率敏感性表。

加加速度

行走动画

局限性

• 采用平面（矢状面）运动学，未建模侧向平衡、脚掌打滑与接触力；
• 浮动基座位置由步态规划给定，不是完整浮动基动力学积分；
• ZMP 由期望质心轨迹计算，支撑多边形由期望关节角 qdes 的 FK 与规划接触相位得到；二者均为规划几何量，不代表真实接触力；
• PD 伺服参数为等效惯量模型，不等同于真实电机/减速器参数。

复现说明

results/run_spline 与 results/run_direct 由 main.py 在本机实际运行产生，均包含 100 Hz、3401 条等间隔采样的 trajectory.csv；run_spline 还包含分析脚本生成的 metrics.json、metrics.md、图表与 GIF。运行输出已通过 .gitignore 排除、不入库，文档所需图表保存在 docs/img/。测试通过 python3 -m pytest -q test 验证三次 Hermite 插值与解析导数、运动学，以及完整 34 s 调度下全部 5 次切换的逐窗口速度连续性。