概述

双足机器人作为机器人领域的一个重要分支，其研究具有重要的理论价值和广阔的应用前景。行走控制是双足机器人的核心技术之一，而摆动腿控制又是行走控制中的关键环节。摆动腿控制的好坏直接影响到机器人的稳定性、行走速度和适应不同地形的能力。本文将从数学原理的角度，对双足机器人摆动腿控制技术进行深入剖析。

一、摆动腿控制概述

由于等式约束，处于摆动阶段的机器人腿不施加地面接触力，因此不受基于力和力矩的MPC控制。为了在每个步态周期中控制腿和脚的位置，在笛卡尔空间中用PD位置控制器控制期望的脚轨迹。步态序列完全基于时间，步态周期目前设置为 0.3 秒。我们使用正向运动学获得当前的脚位置。

1、摆动腿状态判断

双足机器人的行走周期分为支撑相和摆动相。摆动腿控制主要负责在摆动相期间，使摆动腿按照预定轨迹运动，完成支撑腿的转换。有以下运行原则：

• 在每个步态周期中，步态发生器会判断机器人腿是处于支撑相还是摆动相。
• 如果是摆动相，则将摆动腿控制权交给摆动腿控制器。

2、控制策略

摆动腿控制策略主要包括虚拟弹簧阻尼系统和PD控制律。以下将详细介绍这两种策略的数学原理。

二、数学原理介绍

三、代码实现

1、updateCommand

这里引用rl_mpc中的腿部控制器进行讲解，updateCommand用于更新机器人腿部在模拟器中的命令。以下是代码的详细解释：

创建一个长度为12的零向量 legTorques，用于存储每个腿部的关节扭矩。这里假设机器人有4条腿，每条腿有3个关节。

legTorques = np.zeros(12, dtype=DTYPE)

循环遍历机器人的4条腿

for leg in range(4):

计算每条腿的足部力。

• forceFeedForward 是前馈力。
• kpCartesian 和 kdCartesian 分别是笛卡尔坐标下的位置和速度PD增益。
• pDes 和 vDes 分别是期望的位置和速度。
• p 和 v 是当前的位置和速度。

footForce = self.commands[leg].forceFeedForward \
            + self.commands[leg].kpCartesian @ (self.commands[leg].pDes - self.datas[leg].p) \
            + self.commands[leg].kdCartesian @ (self.commands[leg].vDes - self.datas[leg].v)

计算腿部关节扭矩。

• tauFeedForward 是前馈扭矩。
• J.T 是雅可比矩阵的转置，用于将足部力转换为关节扭矩。

legTorque = self.commands[leg].tauFeedForward + self.datas[leg].J.T @ footForce

添加关节PD控制。

• kpJoint 和 kdJoint 分别是关节位置和速度PD增益。
• qDes 和 qdDes 分别是期望的关节位置和速度。
• q 和 qd 是当前的关节位置和速度。

legTorque += self.commands[leg].kpJoint @ (self.commands[leg].qDes - self.datas[leg].q)
legTorque += self.commands[leg].kdJoint @ (self.commands[leg].qdDes - self.datas[leg].qd)

将计算得到的关节扭矩存储到 legTorques 向量中。

legTorques[leg*3:(leg+1)*3] = legTorque.flatten()

打印第一条腿的关节扭矩，用于调试。

print("leg 0 effort %.3f %.3f %.3f"%(legTorques[0], legTorques[1], legTorques[2]))

2、computeLegJacobianAndPosition

用于计算四足机器人的腿部雅可比矩阵（Jacobian Matrix）和足部位置（p）。雅可比矩阵在机器人学中是一个非常重要的概念。以下是代码的详细解释：

点击【四足机器人】MPC控制框架下的摆动腿控制查看全文。