四、PID + 前馈复合控制
def control_loop(theta_desired, theta_actual, dt):
# 计算误差
e = theta_desired - theta_actual
# PID计算
integral += e * dt
derivative = (e - prev_e) / dt
tau_pid = Kp * e + Ki * integral + Kd * derivative
# 前馈计算（需已知theta_desired的导数）
theta_dot_desired = ... # 通过差分或滤波器获取
theta_ddot_desired = ...
tau_ff = J * theta_ddot_desired + B * theta_dot_desired
# 总控制量
tau_total = tau_pid + tau_ff
prev_e = e
return tau_total
| 
楼主
