Go2控制——关节控制让狗站起来

/* MD / 富文本中的 .toc（含博客园搬家等嵌套结构）；.toc-box 在侧栏，不受影响 */
#content_views .toc,
/* 编辑器常在目录前后插入空 p（:empty 仍占 20px），一并去掉避免顶空隙 */
#content_views.markdown_views > p:empty:has(+ .toc),
#content_views.markdown_views > .toc + p:empty,
/* 富文本旧版目录标记 */
#content_views.htmledit_views #main-toc,
#content_views.htmledit_views #hr-toc,
#content_views.htmledit_views p[id*=\”-toc\”] {
display: none !important;
}
/* 目录去掉后，紧跟的首个标题不再多出一块上边距 */
#content_views.markdown_views > .toc + h1,
#content_views.markdown_views > .toc + h2,
#content_views.markdown_views > .toc + h3,
#content_views.markdown_views > .toc + h4,
#content_views.markdown_views > .toc + p:empty + h1,
#content_views.markdown_views > .toc + p:empty + h2,
#content_views.markdown_views > .toc + p:empty + h3,
#content_views.markdown_views > .toc + p:empty + h4 {
margin-top: 0 !important;
}

Go2控制——关节控制让狗站起来

先看一下我们直接加载默认的Go2模型后的效果，以下是go2.xml中定义的初始状态，为站立状态，但是电机给了一点点力矩，这里为什么要给初始力矩不得而知，可能是为了抵消部分重力，总之狗会在重力的作用下趴下，因为我们并没有给一个可以位置站立的合理力矩，这就是这一篇我们要解决的，通过控制各个关节电机，根据目标角度实时计算关机所需的力矩，最终让关节保持在预设的角度：

<keyframe>
<key name="home" qpos="0 0 0.27 1 0 0 0 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"
ctrl="0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8 0 0.9 -1.8"/>
</keyframe>

这里有两个重要参数：

• qpos：定义机器人初始位置和姿态，包括机身位置、姿态以及各个关节的初始角度。
• ctrl：定义各个执行器的初始控制输入，具体作用取决于执行器的配置，这里的控制器是，对应力矩。

可以看到，Go2 的初始关节角度已经被设置为一个站立姿态，同时执行器也设置了对应的初始控制输入。

但是，这并不意味着机器人就能一直保持站立。

当仿真开始后，重力会持续作用于机器人。如果执行器提供的控制力矩不足以抵消重力以及其他关节之间的动力学作用，机器人就会逐渐失去原来的姿态，最终趴在地面上。

也就是说，设置一个初始关节角度，只是让机器人从指定姿态开始运动，并不能保证机器人持续保持这个姿态。

这一篇我们就来解决这个问题。

我们将通过 PD 控制器，实时计算每个关节所需要的控制力矩，让关节能够跟踪指定的目标角度，并在受到重力和外部扰动时尽可能保持目标位置。

先从最基础的关节位置控制开始。

1.PID控制原理

PID 是一种经典的闭环反馈控制算法，在工业控制、电机控制和机器人控制中都有广泛应用。

PID 分别代表：

• P（Proportional）：比例控制
• I（Integral）：积分控制
• D（Derivative）：微分控制

它的基本思想是：不断比较系统的目标值与实际值，根据两者之间的误差计算控制量，再通过控制量调整系统状态，使实际值逐渐接近目标值。对于 Go2 的关节控制而言：

• 目标值：关节期望转动到的角度。
• 实际值：关节当前的角度。
• 控制量：施加给关节电机的控制力矩。

1.1 比例控制（P）

比例控制根据当前误差的大小，直接计算对应的控制量。假设我们希望某个关节转动到目标角度 (q_d)，当前实际角度为 (q)，那么位置误差为：

$e=q_d-q$

比例控制的输出为：

${\tau }_P=K_p(q_d-q)$

其中：

• (q_d)：目标关节角度。
• (q)：当前关节角度。
• (K_p)：比例增益。
• (\\tau_P)：比例控制产生的力矩。

可以看到，关节距离目标角度越远，产生的控制力矩就越大；越接近目标角度，控制力矩就越小。例如，目标角度为 0.9 rad，当前角度为 0.5 rad，那么控制器就会产生一个正向力矩，让关节向目标角度运动。如果当前角度超过了目标角度，误差就会变成负值，控制器也会输出反方向的力矩。因此，P 控制能够让关节向目标位置靠近。不过，仅靠 P 控制通常会产生振荡，而且在存在重力等持续外力的情况下，可能无法准确保持目标位置。

1.2 积分控制（I）

积分控制关注的是误差随时间的累积。

其数学表达式为：

${\tau }_I=K_i{\int }_0^{t}e(\tau )d\tau$

其中：

• (K_i)：积分增益。
• (e(\\tau))：随时间变化的位置误差。

如果关节始终存在一个较小的误差，积分项就会不断累积，逐渐增加控制输出，直到误差得到消除。例如，关节在重力作用下始终无法准确保持目标角度，存在一定的静态误差，那么积分项就可以逐渐增加力矩，补偿这部分误差。但是，积分控制也有缺点。如果积分持续累积，而执行器又存在力矩限制，就可能产生积分饱和，导致系统响应变慢甚至振荡。

1.3 微分控制（D）

微分控制关注的是误差变化的速度。其数学表达式为：

${\tau }_D=K_d\frac{de}{dt}$

在目标角度保持不变时：

$\frac{de}{dt}=-\dot{q}$

因此：

${\tau }_D=-K_d\dot{q}$

其中：

• $K_d$：微分增益。
• $\dot{q}$：关节当前角速度。

D 控制可以理解为给关节增加阻尼。当关节快速向目标位置运动时，D 项会产生反向力矩，抑制关节运动速度，减少到达目标位置时的过冲和振荡。

1.4 PID 控制公式

将三个部分组合起来，就得到了完整的 PID 控制器：

$\tau =K_{p}e+K_i{\int }_0^{t}e(\tau )d\tau +K_d\frac{de}{dt}$

在 Go2 的关节位置控制中，我们可以根据目标角度与当前角度的误差，计算每个关节需要的控制力矩。不过，实际机器人关节控制并不一定需要使用完整的 PID。对于本篇的目标——让 Go2 的各个关节运动到指定角度并保持位置，我们首先使用 PD 控制即可：

$\boxed{\tau =K_p(q_d-q)-K_d\dot{q}}$

P 项负责让关节靠近目标角度，D 项负责抑制运动过程中的振荡，两者配合就能实现基本的关节位置控制。

1.5 为什么 Go2 关节控制不需要积分项？

前面介绍了 PID 控制器的三个组成部分，那么对于 Go2 的关节位置控制，为什么我们最终只使用 PD，而不使用完整的 PID 呢？首先，我们需要明确当前的控制目标：**让每个关节能够跟踪指定的目标角度，并在目标位置保持稳定。**在这个过程中，P 项和 D 项已经能够完成主要的控制任务：

• P 项根据目标角度与实际角度之间的误差产生控制力矩，让关节向目标位置运动。
• D 项根据关节当前的运动速度产生反向阻尼力矩，抑制关节运动过程中的振荡。

那么，I 项还有什么作用？积分项主要用于消除系统中持续存在的稳态误差。例如，一个关节在重力作用下，即使已经接近目标角度，仍然存在一定的角度偏差。由于 P 项输出的力矩与误差成正比，只要误差存在，控制器就会持续输出力矩，但可能无法完全消除这个偏差。这时，积分项就可以不断累积误差，逐渐增加控制输出，直到稳态误差得到消除。但是，对于 Go2 这样的机器人关节，我们还需要考虑一个问题：重力本身是可以通过动力学模型计算和补偿的，并不一定需要依靠积分项来抵消。

在实际机器人控制中，可以通过重力补偿，根据机器人当前姿态计算各个关节需要额外提供的力矩，从而抵消重力对关节的影响。另外，Go2 的关节执行器本身通常也具有一定的力矩控制能力，能够根据控制器输出提供相应的驱动力矩。因此，在当前这个基础的关节位置控制任务中，PD 控制通常已经足够。当然，这并不意味着积分项完全没有价值。如果系统存在持续的外部负载、模型误差、摩擦力等因素，导致 PD 控制始终存在稳态误差，那么积分项仍然可以发挥作用。但积分项也有自己的问题，例如积分饱和、响应变慢，以及在执行器力矩受到限制时产生额外的控制问题。所以，对于当前的 Go2 关节控制，我们先采用 PD 控制：

$\boxed{\tau =K_p(q_d-q)-K_d\dot{q}}$

先实现12个关节的角度跟踪和稳定保持。后续如果涉及更复杂的动力学控制，再根据实际需求决定是否引入积分项或其他补偿方法。

2. 关节的PD控制

机械狗或者具身机器人的关节一般采用PMSM永磁同步电机+减速器作为关节驱动模组。电机内部的驱动器已经帮我们完成了FOC控制算法，因此这里的关节控制并不是电机的FOC控制，而是更高层次的控制，即给电机一个力矩让电机转起来，通过实时控制力矩的大小，就可以控制电机转到预定的角度，所以我们这里的关节控制实际上是给定一个目标角度/角速度，通过PD控制器实时计算控制力矩，输入给电机，让电机逐渐逼近目标角度/角速度。常见的控制策略如下：

2.1 我们的目标角度

不同关节的角度不同，最终展现出来的就是整个机械狗身体的姿态步态，因此在往上层走，关节目标角度是根据狗身体的整体姿态以及要完成的动作来决定的，甚至还需要IK（逆运动学）求解，这个不在包含本篇内。本篇我们只是手搓一组目标角度，让它能够站起来并为之站立姿势就好了，当然通过微调目标角度可以调整站立的姿态。

这里我们假设12个关节的角度如下：

2.2 向目标角度靠齐

现在我们的目标角度有了，根据PD控制器，我们还需要实时获取各个关节当前的角度和角速度，以及为每个关节电机控制器设置K_p和K_d。角度和角速度的获取我们之前已经交代过了，通过data.qpos和data.qvel获得，我们已经建立了索引映射表，通过关节名称名称创建id，然后通过id进行索引：

"""go2_control/constants.py — 关节语义映射（系列共用）"""
import mujoco

LEGS = ["FR", "FL", "RR", "RL"] # 前右、前左、后右、后左
JOINTS_PER_LEG = ["hip", "thigh", "calf"] # 每条腿的三个关节

# 语义顺序下的关节名列表，例如 ["FR_hip_joint", "FR_thigh_joint", …]
JOINT_NAMES = [f"{leg}_{j}_joint" for leg in LEGS for j in JOINTS_PER_LEG]
ACTUATOR_NAMES = [f"{leg}_{j}" for leg in LEGS for j in JOINTS_PER_LEG]

def joint_qpos_addr(model, name):
jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
return model.jnt_qposadr[jid]

def joint_qvel_addr(model, name):
jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
return model.jnt_dofadr[jid]

def build_joint_map(model):
m = {}
for name in JOINT_NAMES:
jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
m[name] = (model.jnt_qposadr[jid], model.jnt_dofadr[jid])
return m

def actuator_index(model, name):
return mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name)

def build_actuator_map(model):
return {name: actuator_index(model, name) for name in ACTUATOR_NAMES}

读取代码如下，首先遍历关节和对应执行器的名字，然后依次得到qpos/qvel/actuator的id，并存放在数组里，方便后面批量读取和控制控制，批量控制时注意关节名字和驱动器的名字要一一对应，当然一个关节一个关节控制也可以。

import common

qpos_ids = []
qvel_ids = []
actuator_ids = []

for j_name, a_name in zip(common.JOINT_NAMES, common.ACTUATOR_NAMES):
qpos_ids.append(common.joint_qpos_addr(model, j_name))
qvel_ids.append(common.joint_qvel_addr(model, j_name))
actuator_ids.append(common.actuator_index(model, a_name))

qpos_ids = np.array(qpos_ids)
qvel_ids = np.array(qvel_ids)
actuator_ids = np.array(actuator_ids)

# 读取当前12个关节角度和角速度
q = data.qpos[qpos_ids]
dq = data.qvel[qvel_ids]

下一步就是设置目标角度，这里我们给定12个关节的目标角度如下，也是定义在数组中，一共4条腿，每条腿三个关节分别设置三个目标角度，四条腿保持一致，这里是随便设置，可以自行修改目标角度，让狗的站立姿态更优雅，当然如果设置不当可能会摔倒，注意我们是要让狗静止站立，所以目标角速度都是0，实际行走时可能不是0哦：

# 每条腿：hip=0, thigh=0.5, calf=-0.9
q_des = np.tile([0.0, 0.5, –0.9], 4)

下面重点来了，我们的PD参数，我们依然选择把所有电机的K_p设置成一样，而K_d也设置成一样，只是为了偷懒，这个参数在后面通过其他办法得到。

kp = np.full(12, 30.0)
kd = np.full(12, 2.0)

完整的程序如下：

import mujoco
import mujoco.viewer
import numpy as np
import time
import common

# 1. 加载模型
model = mujoco.MjModel.from_xml_path("unitree_go2/scene.xml")
data = mujoco.MjData(model)

# 2. 获取关节和执行器索引
qpos_ids = []
qvel_ids = []
actuator_ids = []

for j_name, a_name in zip(common.JOINT_NAMES, common.ACTUATOR_NAMES):
qpos_ids.append(common.joint_qpos_addr(model, j_name))
qvel_ids.append(common.joint_qvel_addr(model, j_name))
actuator_ids.append(common.actuator_index(model, a_name))

qpos_ids = np.array(qpos_ids)
qvel_ids = np.array(qvel_ids)
actuator_ids = np.array(actuator_ids)

# 3. 设置目标关节角度
# 每条腿：hip=0, thigh=0.5, calf=-0.9
q_des = np.tile([0.0, 0.5, –0.9], 4)

# 4. 设置PD参数（实验初始值，需要调试）
kp = np.full(12, 30.0)
kd = np.full(12, 2.0)

# 5. 获取每个执行器的控制范围
ctrl_min = model.actuator_ctrlrange[actuator_ids, 0]
ctrl_max = model.actuator_ctrlrange[actuator_ids, 1]

# 6. 启动仿真
step_count = 0

with mujoco.viewer.launch_passive(model, data) as viewer:

while viewer.is_running():

# 读取当前12个关节角度和角速度
q = data.qpos[qpos_ids]
dq = data.qvel[qvel_ids]

# 计算12个关节的PD控制力矩
tau = kp * (q_des – q) – kd * dq

# 力矩限幅
tau = np.clip(tau, ctrl_min, ctrl_max)

# 输出到12个motor actuator
data.ctrl[actuator_ids] = tau

# 推进仿真
mujoco.mj_step(model, data)
viewer.sync()

# 每隔100步打印一次状态
step_count += 1

if step_count % 100 == 0:
print("当前关节角度:", np.round(q, 3))
print("当前关节力矩:", np.round(tau, 3))

time.sleep(model.opt.timestep)

效果如下：

我们把各个关节的角度和角速度绘制出来看一下：

从这张图上看，总感觉有点问题，虚线是我们的目标位置，但是三个关节的角度似乎都没有靠上去，前面我们设置的kp=30,kd=2，狗下落的着陆的时候感觉有点软，刚性不够，所以关节的跟踪效果比较差，下面给一组较大的kp和kd，增加kd是为了增加点阻尼，防止过冲太狠。当然kp也不能太大，控制力超出驱动器的输入范围也不好。

# 4. 设置PD参数（实验初始值，需要调试）
kp = np.full(12, 400.0)
kd = np.full(12, 20.0)

可以看到现在比较好的贴合目标值，而且狗下落到停止，干脆利落：

大家自己尝试一下不同的PD参数，对比一下不同参数下的效果，kp越大，就像是弹簧越硬，怼上去硬邦邦的，但是追踪效果好，kd越大，就像是增加了一个阻尼，越大缓冲效果越好。

3. 小结

到这里，我们已经完成了 Go2 的基础关节位置控制。通过实时读取12个关节的角度和角速度，利用 PD 控制器计算各个关节所需的控制力矩，并将力矩输入对应的电机执行器，实现了12个关节对指定目标角度的跟踪与保持。本次实验还对比了两组不同的 PD 参数：

• 当 Kp=30、Kd=2 时，关节的位置跟踪存在比较明显的偏差，机器人落地后的稳定过程也相对柔和。
• 当 Kp=400、Kd=20 时，关节能够更好地贴近目标角度，落地后也能更快地稳定下来。

这里还有一个值得注意的地方：本次实验没有加入额外的重力补偿，仅通过 PD 控制就实现了基本的关节位置保持。 但在重力负载下，PD 控制需要通过一定的位置误差产生相应的保持力矩，因此增大 Kp 可以减小稳态位置偏差，但并不意味着重力影响已经消失。当然，目前我们只是给12个关节设置了一组相同的目标角度和 PD 参数，让 Go2 能够维持一个基本的站立姿态。至于这些目标角度应该如何确定，如何让机器人按照指定轨迹运动，以及如何协调四条腿完成行走，都还没有涉及。后续我们将继续围绕 Go2 展开，从关节位置控制逐步深入到机器人姿态控制、步态规划和运动控制，最终让这只狗真正动起来。