1. 双足机器人运动学建模与步态参数定义
任务目标
为双足机器人建立简化的二维运动学模型，定义步态周期、步长、步频等基本参数，并实现正运动学计算，为后续步态控制提供参数化基础。
模型定义
采用五连杆模型描述机器人单腿支撑相：躯干(hip)连接左右腿，每条腿由髋关节、膝关节和踝关节组成，踝关节固定于地面时构成开链结构。模型参数如下：
- 连杆长度：大腿
L1=0.4m，小腿L2=0.4m，躯干高度L0=0.6m - 关节角：髋关节角
q_hip，膝关节角q_knee（均为相对于竖直向下方向的夹角，屈曲为正） - 步态参数：步态周期
T（完成一个完整步态循环所需时间），步长S（单步髋关节水平位移），步频cadence = 2/T（每分钟步数，双足各迈一步计为两步）
正运动学实现
以下Python代码实现支撑腿的正运动学计算，输入髋关节和膝关节角度，输出踝关节在髋关节坐标系下的位置，并定义步态参数数据结构：
```python import numpy as np from dataclasses import dataclass # 连杆长度 (m) L1 = 0.4 # 大腿 L2 = 0.4 # 小腿 @dataclass class GaitParams: """步态基本参数定义""" T: float = 1.2 # 步态周期 (s) S: float = 0.5 # 步长 (m) step_height: float = 0.1 # 迈步高度 (m) @property def cadence(self) -> float: """步频：每分钟步数（双腿各迈一步计为2步）""" return 2.0 * 60.0 / self.T def forward_kinematics(q_hip: float, q_knee: float) -> tuple: """ 支撑腿正运动学：计算踝关节在髋关节坐标系下的位置。 髋关节为原点，y轴竖直向下，x轴指向行进方向。 参数: q_hip: 髋关节角 (rad)，相对竖直向下方向，屈曲为正 q_knee: 膝关节角 (rad)，相对大腿延长线方向，屈曲为正 返回: (x_ankle, y_ankle): 踝关节位置 (m) """ # 髋关节到膝关节 x_knee = L1 * np.sin(q_hip) y_knee = L1 * np.cos(q_hip) # 膝关节到踝关节（膝关节角叠加在髋关节角上） q_total = q_hip + q_knee x_ankle = x_knee + L2 * np.sin(q_total) y_ankle = y_knee + L2 * np.cos(q_total) return (x_ankle, y_ankle) def compute_swing_foot_trajectory(params: GaitParams, num_points: int = 50) -> np.ndarray: """ 计算摆动腿踝关节在髋关节坐标系下的期望轨迹（简化正弦轨迹）。 用于验证步态参数与运动学模型的一致性。 返回: trajectory: shape (num_points, 2)，每行为 (x, y)，y轴向下为正 """ t = np.linspace(0, params.T, num_points) # 归一化相位 [0, 1] phase = t / params.T # x方向：从后方 -S/2 到前方 +S/2（髋关节坐标系） x = -params.S / 2 + params.S * phase # y方向：抬腿高度按正弦变化，起点和终点落地 y = params.step_height * np.sin(np.pi * phase) return np.column_stack([x, y]) # 示例：验证正运动学与步态参数 if __name__ == "__main__": # 站立姿态：髋关节和膝关节均为0 x_stand, y_stand = forward_kinematics(0.0, 0.0) print(f"站立时踝关节位置: x={x_stand:.3f}m, y={y_stand:.3f}m") print(f"腿长验证: {np.sqrt(x_stand**2 + y_stand**2):.3f}m (应为{L1+L2}m)") # 典型行走姿态：髋屈曲20°，膝屈曲40° qh = np.deg2rad(20) qk = np.deg2rad(40) x_walk, y_walk = forward_kinematics(qh, qk) print(f"行走姿态踝关节位置: x={x_walk:.3f}m, y={y_walk:.3f}m") # 步态参数 gp = GaitParams(T=1.2, S=0.5) print(f"步频: {gp.cadence:.1f} 步/分钟") # 摆动腿轨迹 traj = compute_swing_foot_trajectory(gp) print(f"摆动腿轨迹点数量: {len(traj)}") print(f"轨迹x范围: [{traj[:,0].min():.3f}, {traj[:,0].max():.3f}]m (步长{S}m)") print(f"最大抬腿高度: {traj[:,1].max():.3f}m (设定{gp.step_height}m)") ```运行与验证
运行方式：将代码保存为 biped_model.py，在Python 3.8+环境中执行（需安装NumPy）：
验证方法：观察以下输出并核对：
- 站立姿态下踝关节位置应为
(0.000, 0.800)，且腿长计算值等于0.800m（即L1+L2），验证正运动学正确性 - 行走姿态下踝关节x坐标应为正（髋关节前屈使踝前移），y坐标小于0.8m（膝关节屈曲使脚上抬）
- 步频输出应为
100.0 步/分钟（T=1.2s时，2×60/1.2=100） - 摆动腿轨迹x范围应恰好覆盖
[-0.25, 0.25]m（步长0.5m），最大抬腿高度为0.1m
若上述数值与预期一致，说明运动学模型和步态参数定义正确，可作为后续步态控制的基础。
常见问题：若站立姿态验证时腿长计算值不为0.8m，请检查角度单位是否为弧度；膝关节角度的参考方向为大腿延长线，而非竖直方向，计算总角度时需将髋关节角与膝关节角相加。