Webots中基于Torque Control的直流电机控制器异常问题排查

问题详情 #

我正在Webots中开发扭矩控制器，基于直流电机动力学方程和PID控制器来模拟伺服电机的精准动作。机器人在所有关节采用Position Control模式时运行正常，但切换到Torque Control模式后出现异常（机器人"爆炸"）。直流伺服电机的所有参数均通过Simulink的Parameter Estimation Tool获取。

观察到的核心现象：仿真步骤间关节角速度过高，导致计算出的力矩值异常巨大，而Position Control模式下不会出现这种情况。

代码实现 #

KP = 0.5
KD = 0#0.00019425
KI = 0#0.013507
KM = 0.0020
RA = 1.45
LA = 0.00019652

def computeTorques():
    global joint, joint_angular_velocity, joint_sensor, joint_angular_position
    global prev_joint_angular_position, joint_torque, error, delta_error, acum_error
    global prev_error, Ia, enable_torque_control
    print("New Simulation Step")
    if enable_torque_control == 0:
       return
    MAX_TORQUE = 3.43  # Limit torque to the motor's max capacity
    DAMPING_FACTOR = 0  # Optional damping
    ALPHA = 0  # Low-pass filter for velocity

    for i in range(len(joint)):
        # Get position and compute velocity with smoothing
        joint_angular_position[i] = joint_sensor[i].getValue()
        print("Angular Position %d: %f" % (i, joint_angular_position[i]))
        print("Previous Angular Position %d: %f" % (i,prev_joint_angular_position[i]))
        raw_velocity = (joint_angular_position[i] - prev_joint_angular_position[i]) / (timestep * 1e-3)
        joint_angular_velocity[i] = ALPHA * joint_angular_velocity[i] + (1 - ALPHA) * raw_velocity
        prev_joint_angular_position[i] = joint_angular_position[i]

    # PID control
    error = target_joint - joint_angular_position
    delta_error = (error - prev_error) / (timestep * 1e-3)
    acum_error += error * timestep * 1e-3
    prev_error = error

    # Voltage and current calculations
    VA = KP * error + KD * delta_error + KI * acum_error
    Eb = joint_angular_velocity * 373 * KM
    Ia = (VA - Eb) / RA

    # Torque calculation
    joint_torque = Ia * KM * 373
    joint_torque = np.clip(joint_torque - DAMPING_FACTOR * joint_angular_velocity, -MAX_TORQUE, MAX_TORQUE)


    print("VA", VA)
    print("Eb", Eb)
    
    print("Omega", joint_angular_velocity)  
    #print("dI_dt", dI_dt)
    #print("Ia", Ia)
    print("joint_torque", joint_torque)
    # Apply torque to joints
    for i in range(len(joint)):
        joint[i].setTorque(joint_torque[i])
        #print("Torque %d: %f" %(i,joint[i].getTorqueFeedback()))

内容的提问来源于stack exchange，提问作者JOAQUÍN PIÑEIRO