4. 单腿摆动控制：PD控制器原理、单腿轨迹规划、仿真中实现单腿摆动

好，我们进入第四讲。前面几章我们把四足机器人的运动学、动力学基础打了一遍，也聊了聊仿真环境怎么搭。今天要干一件很具体的事——让一条腿动起来，而且是按照我们规划的轨迹，优雅地摆动。

你可能会想，一条腿摆动有什么难的？给个位置指令不就行了？嗯，没那么简单。我刚开始做这个的时候，也以为直接给关节角度目标值就行，结果腿抖得像筛糠一样。后来才明白，控制一条腿摆动，本质上是在跟重力、惯性、摩擦力做对抗。你需要一个靠谱的控制器，和一个合理的轨迹。

4.1 PD控制器原理：为什么它是最佳起点？

PD控制器，比例-微分控制，说白了就是“看偏差，看偏差变化率”。公式很简单：

τ = Kp * (q_des - q) + Kd * (dq_des - dq)

其中 τ 是输出力矩，q_des 是目标位置，q 是当前位置，Kp 是比例增益，Kd 是微分增益。

我个人习惯把 Kp 理解为“弹簧刚度”，Kd 理解为“阻尼系数”。你想想看，一个弹簧系统，刚度越大，拉回平衡位置的力量越大；阻尼越大，震荡衰减得越快。PD控制器本质上就是在模拟一个虚拟的弹簧-阻尼系统。

我在项目中遇到过一个问题：Kp 调得太大，腿在落地瞬间会弹跳；Kd 调得太大，腿响应变慢，跟不上轨迹。后来我总结了一个经验——先调 Kp 让系统有足够的刚度，再调 Kd 消除震荡，最后微调。

参数	作用	调大后的效果	调小后的效果
Kp（比例增益）	位置误差的纠正力度	响应快，但可能震荡	响应慢，跟踪误差大
Kd（微分增益）	速度误差的阻尼作用	震荡衰减快，但噪声放大	震荡衰减慢，系统易抖动

4.2 单腿轨迹规划：从A点到B点的艺术

有了控制器，我们还得告诉腿“往哪摆”。轨迹规划就是干这个的。一条完整的摆动腿轨迹，通常分为两个阶段：

• 抬腿阶段：从地面抬起到最高点
• 落地阶段：从最高点落回地面

最简单的轨迹是直线插值，但直线插值在起点和终点速度不连续，会导致冲击。我建议用五次多项式或者贝塞尔曲线，保证位置、速度、加速度都连续。

这里给一个常用的五次多项式轨迹公式：

s(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5

其中 s(t) 是归一化位置（0到1），t 是归一化时间（0到1）。通过边界条件（起点位置、速度、加速度，终点位置、速度、加速度）可以解出六个系数。

我个人喜欢用贝塞尔曲线，因为它直观，控制点可以直接调整轨迹形状。比如，你可以把控制点放在地面以上一定高度，这样腿就会自然形成一个弧线，避免拖地。

下面是一个典型的摆动腿轨迹参数：

参数	值	说明
摆动周期	0.2 ~ 0.4 s	取决于步态频率
抬腿高度	0.05 ~ 0.15 m	根据地形和机身高度调整
步长	0.1 ~ 0.3 m	前后方向移动距离
轨迹类型	五次多项式 / 贝塞尔	保证速度连续

4.3 仿真中实现单腿摆动：从理论到代码

好，理论讲完了，我们动手。在仿真中实现单腿摆动，我一般分三步走：

1. 建立运动学模型：把关节角度映射到足端位置
2. 规划足端轨迹：在笛卡尔空间生成目标位置序列
3. 逆运动学求解：把足端位置反算回关节角度
4. PD控制器跟踪：让关节实际角度跟随目标角度

下面是一个简化版的仿真代码框架（Python伪代码）：

# 1. 定义机器人参数
L1 = 0.2  # 大腿长度
L2 = 0.2  # 小腿长度

# 2. 定义轨迹规划函数
def swing_trajectory(t, T_swing, step_length, step_height):
    # 归一化时间
    s = t / T_swing
    # 五次多项式插值
    x = step_length * (10*s^3 - 15*s^4 + 6*s^5 - 0.5)
    z = step_height * (64*s^3 - 192*s^4 + 192*s^5 - 64*s^6)
    return x, z

# 3. 逆运动学求解
def inverse_kinematics(x, z):
    # 几何法求解关节角度
    cos_q2 = (x^2 + z^2 - L1^2 - L2^2) / (2*L1*L2)
    q2 = -acos(cos_q2)  # 膝关节角度
    q1 = atan2(z, x) - atan2(L2*sin(q2), L1 + L2*cos(q2))
    return q1, q2

# 4. PD控制器
def pd_controller(q_des, q_cur, dq_des, dq_cur, Kp, Kd):
    tau = Kp * (q_des - q_cur) + Kd * (dq_des - dq_cur)
    return tau

# 5. 主循环
for t in range(0, T_swing, dt):
    # 规划足端轨迹
    x_des, z_des = swing_trajectory(t, T_swing, 0.2, 0.1)
    # 逆运动学求解关节角度
    q1_des, q2_des = inverse_kinematics(x_des, z_des)
    # 计算控制力矩
    tau1 = pd_controller(q1_des, q1_cur, 0, dq1_cur, Kp=100, Kd=10)
    tau2 = pd_controller(q2_des, q2_cur, 0, dq2_cur, Kp=100, Kd=10)
    # 应用力矩到仿真模型
    apply_torque(tau1, tau2)

下面这张图展示了单腿摆动控制的核心流程，从轨迹规划到关节控制：

从图中可以看到，整个控制流程是一个闭环。轨迹规划生成足端位置，逆运动学算出关节角度，PD控制器根据偏差输出力矩，仿真模型响应后反馈状态。这个闭环的更新频率我建议至少 1kHz，才能保证摆动轨迹的平滑性。