PD控制器控制机器人无法抵达期望位置的技术排查请求

我正在用PD（比例-微分）控制器控制机器人，但机器人位置始终达不到期望位置。我需要观测期望位置（q_des）与实际位置（q）的偏差，最终误差定义为最终时刻q_des和机器人实际位置的范数。

采用的PD控制律为：

tau = Kp * (q_des - q) - Kd * dq
其中q_des是期望关节位置，q是实际位置，dq是实际关节速度，Kp、Kd是待设置的增益。

仿真时通过调整时间步长dt评估精度，位置和速度更新公式为：

q_next = q + dt * dq
dq_next = dq + dt * ddq

以下是Python仿真代码：

from adam.casadi.computations import KinDynComputations
from adam.geometry import utils
import numpy as np
import casadi as cs
from math import sqrt

urdf_path =  "/Users/tommasoandina/Desktop/doosan-robot2-master/dsr_description2/urdf/h2515.blue.urdf" 
# 关节列表
joints_name_list = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
# 指定根连杆
root_link = 'base'

kinDyn = KinDynComputations(urdf_path, joints_name_list, root_link)
num_dof = kinDyn.NDoF

H = cs.SX.sym('H', 4, 4)
# 关节角度
s = cs.SX.sym('s', num_dof)
# 基座速度
v_b = cs.SX.sym('v_b', 6)
# 关节速度
s_dot = cs.SX.sym('s_dot', num_dof)
# 基座加速度
v_b_dot = cs.SX.sym('v_b_dot', 6)
# 关节加速度
s_ddot = cs.SX.sym('s_ddot', num_dof)

# 初始化动力学计算函数
mass_matrix_fun = kinDyn.mass_matrix_fun()
coriolis_term_fun = kinDyn.coriolis_term_fun()
gravity_term_fun = kinDyn.gravity_term_fun()
bias_force_fun = kinDyn.bias_force_fun()
Jacobian_fun = kinDyn.jacobian_fun("link6")

class Controller:
    def __init__(self, kp, kd, dt, q_des):
        self.q_previous = 0.0
        self.kp = kp
        self.kd = kd
        self.dt = dt
        self.q_des = q_des
        self.first_iter = True

    def control(self, q, dq):
        if self.first_iter:
            self.q_previous = q
            self.first_iter = False

        self.q_previous = q
        return self.kp * (self.q_des - q) - self.kd * dq


class Simulator:
    def __init__(self, q, dt, dq, ddq):
        self.q = q
        self.dt = dt
        self.dq = dq
        self.ddq = ddq

    def simulate_q(self, tau, h2):
        dq = self.simulate_dq(tau, h2)
        self.q += self.dt * dq
        return self.q
    
    def simulate_dq(self, tau, h2):
        self.ddq = cs.inv(M2) @ (tau - h2)
        self.dq += self.dt * self.ddq
        return self.dq
    
    def simulate_ddq(self, M2, tau, h2):
        self.ddq = cs.inv(M2) @ (tau - h2)
        return self.ddq


# 随机初始化参数
q_des = (np.random.rand(num_dof) - 0.5) * 5
xyz = (np.random.rand(3) - 0.5) * 5
rpy = (np.random.rand(3) - 0.5) * 5
H_b = utils.H_from_Pos_RPY(xyz, rpy)
v_b = (np.random.rand(6) - 0.5) * 5
s = (np.random.rand(len(joints_name_list)) - 0.5) * 5
s_dot = (np.random.rand(len(joints_name_list)) - 0.5) * 5


M = kinDyn.mass_matrix_fun()
M2 = cs.DM(M(H_b, s))
M2 = M2[:6, :6]

h = kinDyn.bias_force_fun()
h2 = cs.DM(h(H_b, s, v_b, s_dot))
h2 = h2[:6]


q_0 = np.zeros(num_dof)
kp = 0.1 
kd = sqrt(kp)
dt = 1.0 / 16.0 * 1e-3
total_time = 2.0 * 1e-3

dq = np.zeros(num_dof)
ddq = np.zeros(num_dof)


N = int(total_time / dt)

ctrl = Controller(kp, kd, dt, q_des)
simu = Simulator(q_0, dt, dq, ddq)

for i in range(N):
    tau = ctrl.control(simu.q, simu.dq)
    simu.simulate_q(tau, h2)
    simu.simulate_ddq(M2, tau, h2)

q_des_np = cs.DM(q_des).full().flatten()
simu_q_np = cs.DM(simu.q).full().flatten()

# 计算无穷范数误差
errore_medio_infinito = np.max(np.abs(q_des_np - simu_q_np))

print(q_des_np)
print(simu_q_np)

print("无穷范数误差:", errore_medio_infinito)

错误原因分析 #

• 动力学参数未实时更新：代码中M2和h2仅在初始化时计算一次，但机器人运动时关节角度、速度会变化，质量矩阵和偏置力（科氏力、重力等）会随关节状态改变，固定参数无法反映真实动力学，导致加速度计算错误。
• 仿真逻辑顺序错误：循环中先更新位置再计算加速度，且后续计算的加速度未被使用。正确顺序应为：先计算当前加速度，再更新速度，最后更新位置，当前顺序会导致位置更新依赖旧的动力学数据。
• 仿真时间过短：total_time = 2.0 * 1e-3（仅2毫秒），PD控制器需要时间让机器人从全零初始位置收敛到目标，这么短的时间内机器人根本来不及移动到期望位置。
• 增益设置过小：kp = 0.1的增益值太小，比例项驱动力不足以让机器人快速向目标移动，通常工业机器人PD增益会取100-1000级别，过小增益会导致收敛极慢。
• 基座状态设置不合理：初始化给基座设置了随机位置和速度，但仿真中基座应固定（v_b设为零，H_b设为单位矩阵），额外的基座运动会干扰关节位置跟踪。

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