使用隐式PD控制+重力补偿控制iiwa机械臂时出现漂移

问题背景 #

控制方案为组合使用Drake最新版隐式PD控制器和基于InverseDynamics的重力补偿。当将机械臂期望位置固定为q0并运行仿真时，机械臂无法保持在q0，会漂移至另一平衡位置；漂移量随PD反馈增益减小而增大，且重力设为0时漂移现象消失。可通过以下代码复现该现象：

import numpy as np
import matplotlib.pyplot as plt

from pydrake.all import (
    DiagramBuilder,
    MultibodyPlant,
    AddMultibodyPlant,
    ProcessModelDirectives,
    MultibodyPlantConfig,
    Parser,
    LoadModelDirectivesFromString,
    InverseDynamics,
    JointActuatorIndex,
    PdControllerGains,
    VisualizationConfig,
    ApplyVisualizationConfig,
    StartMeshcat,
    Simulator,
    LogVectorOutput,
)


directives = """
directives:
    - add_model:
        name: iiwa
        file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf
        default_joint_positions:
            iiwa_joint_1: [ 0 ]
            iiwa_joint_2: [ 0 ]
            iiwa_joint_3: [ 0 ]
            iiwa_joint_4: [ -1.7 ]
            iiwa_joint_5: [ 0 ]
            iiwa_joint_6: [ 1.0 ]
            iiwa_joint_7: [ 0 ]
    - add_weld:
        parent: world
        child: iiwa::iiwa_link_0
"""

meshcat = StartMeshcat()

#%%
builder = DiagramBuilder()

plant_config = MultibodyPlantConfig()
plant_config.discrete_contact_solver = "sap"
plant_config.time_step = 1e-3
plant, scene_graph = AddMultibodyPlant(config=plant_config, builder=builder)

model_directives = LoadModelDirectivesFromString(
    directives
)
parser = Parser(plant)
added_models = ProcessModelDirectives(
    directives=model_directives,
    parser=parser,
)
iiwa_model = added_models[0].model_instance

# Implicit PD
Kp = np.ones(7) * 10
Kd = np.ones(7) * 5

actuator_indices = []
for i in range(plant.num_actuators()):
    actuator_index = JointActuatorIndex(i)
    if plant.get_joint_actuator(actuator_index).model_instance() == iiwa_model:
        actuator_indices.append(actuator_index)

for actuator_index, Kp, Kd in zip(actuator_indices, Kp, Kd):
    plant.get_joint_actuator(actuator_index).set_controller_gains(
        PdControllerGains(p=Kp, d=Kd)
    )

plant.Finalize()

# Gravity compensation
inverse_dynamics_controller = builder.AddSystem(
    InverseDynamics(
        plant,
        InverseDynamics.InverseDynamicsMode.kGravityCompensation,
    )
)
builder.Connect(
    plant.get_state_output_port(iiwa_model),
    inverse_dynamics_controller.get_input_port_estimated_state(),
)

builder.Connect(
    inverse_dynamics_controller.get_output_port_generalized_force(),
    plant.get_actuation_input_port(iiwa_model)
)

state_log_sink = LogVectorOutput(
    src=plant.get_state_output_port(iiwa_model),
    builder=builder
)

ApplyVisualizationConfig(VisualizationConfig(), builder, meshcat=meshcat)

diagram = builder.Build()


#%%
q0 = np.array([0, 0, 0, -1.7, 0, 1.0, 0])
v0 = np.zeros(7)
x0 = np.hstack([q0, v0])

simulator = Simulator(diagram)
context = simulator.get_mutable_context()
context_plant = plant.GetMyContextFromRoot(context)

plant.SetPositions(context_plant, q0)
plant.SetVelocities(context_plant, v0)
plant.get_desired_state_input_port(iiwa_model).FixValue(context_plant, x0)


meshcat.StartRecording()
simulator.AdvanceTo(5.0)
meshcat.StopRecording()
meshcat.PublishRecording()


#%%
state_log = state_log_sink.FindLog(context)

t = state_log.sample_times()
x_log = state_log.data().T

#%%
figure = plt.figure()
plt.xlabel("t [s]")
plt.ylabel("joint angles [rad]")
for i in range(7):
    plt.plot(t, x_log[:, i], label=f"q_{i}")

plt.legend(loc='upper right')
plt.show()

原因分析 #

问题根源是力矩叠加导致的重力过补偿：

• 隐式PD控制器输出力矩：$\tau_{pd} = K_p(q_{des} - q) + K_d(v_{des} - v)$，作用是通过反馈误差产生力矩，抵消重力并跟踪期望位置
• InverseDynamics重力补偿模式输出力矩：$\tau_{gravity}$，即抵消当前关节位置下重力所需的力矩
• 总力矩为$\tau_{total} = \tau_{pd} + \tau_{gravity}$，系统平衡时（$\ddot{q}=\dot{q}=0$），动力学方程要求$\tau_{total} = -\tau_{gravity}(q)$
• 代入总力矩表达式得：$K_p(q_{des} - q) + \tau_{gravity}(q) = -\tau_{gravity}(q)$，整理后平衡位置$q$满足$K_p(q_{des} - q) = -2\tau_{gravity}(q)$，显然$q \neq q_{des}$（除非$\tau_{gravity}(q_{des})=0$），因此机械臂会漂移到该平衡位置。增益$K_p$越小，$q$与$q_{des}$的偏差越大；重力为0时$\tau_{gravity}=0$，平衡位置回到$q_{des}$，漂移消失。

解决方案 #

提供两种可行修正方案：

方案1：仅使用隐式PD控制器 #

移除额外的重力补偿模块，让PD控制器通过反馈增益抵消重力。此时平衡时$K_p(q_{des} - q) = -\tau_{gravity}(q)$，稳态误差随$K_p$增大而减小，可通过调高增益满足精度要求。

修改方式：删除代码中所有重力补偿相关的模块创建与连接代码。

方案2：使用InverseDynamics位置跟踪模式 #

用InverseDynamics的kPositionTracking模式替代分开的隐式PD和重力补偿，该模式会自动整合PD反馈与重力补偿，输出正确的跟踪力矩$\tau = K_p(q_{des} - q) + K_d(v_{des} - v) + \tau_{gravity}(q,v)$，平衡时$q=q_{des}$，刚好抵消重力，无漂移。

修改后的核心代码：

# 移除原隐式PD增益设置代码
# for actuator_index, Kp, Kd in zip(actuator_indices, Kp, Kd):
#     plant.get_joint_actuator(actuator_index).set_controller_gains(
#         PdControllerGains(p=Kp, d=Kd)
#     )

plant.Finalize()

# 替换为InverseDynamics位置跟踪模式
Kp = np.ones(7) * 10
Kd = np.ones(7) * 5

inverse_dynamics_controller = builder.AddSystem(
    InverseDynamics(
        plant,
        InverseDynamics.InverseDynamicsMode.kPositionTracking,
    )
)
# 设置期望状态
inverse_dynamics_controller.get_input_port_desired_state().FixValue(
    inverse_dynamics_controller.GetMyContextFromRoot(context),
    x0
)
# 设置PD增益
inverse_dynamics_controller.set_pd_gains(Kp, Kd)

builder.Connect(
    plant.get_state_output_port(iiwa_model),
    inverse_dynamics_controller.get_input_port_estimated_state(),
)
builder.Connect(
    inverse_dynamics_controller.get_output_port_generalized_force(),
    plant.get_actuation_input_port(iiwa_model)
)

内容的提问来源于stack exchange，提问作者Pang