腿足机器人之三- 驱动器控制算法PID

PID与PD控制的基本原理

• PID

PID 控制器是一种常见的反馈控制器，其名称来源于比例（Proportional）、积分（Integral）和微分（Derivative）这三个控制作用元素。PID 控制器通过计算设定点（期望值）和系统实际输出之间的误差，并对该误差应用比例、积分和微分运算，以调整系统输入，从而使系统输出更接近设定点。

• 比例项（P）：快速响应当前误差，但可能导致稳态误差或超调。
• 积分项（I）：累积历史误差，消除稳态误差（如重力或摩擦导致的静差），但可能引入振荡或积分饱和。
• 微分项（D）：预测未来误差趋势，抑制超调并提高稳定性。

PID 控制的基本表达式为：
$$u(t)=K_{p}e(t)+K_i{\int }_0^t{e}^{\tau }d\tau +K_d\frac{de(t)}{dt}$$
其中$e(t)=r(t)-y(t)$是误差项，即设定点$r(t)$和系统输出$y(t)$的差值，$K_p$,$K_i$,$K_d$为比例、积分和微分增益（控制参数）。

• PD

PD 控制器是 PID 控制器的一个简化版本，仅包含比例和微分项。这种控制器常用于机器人关节和其他需要快速响应且对稳态误差不敏感的系统中，因为微分项能够预测系统误差的未来趋势，并立即做出调整。

PD 控制的基本表达式为：

$$u(t)=K_{p}e(t)+K_d\frac{de(t)}{dt}$$

相对PID算法，减少积分项带来的相位滞后，提高动态稳定性，但是无法消除静态差（如重力导致的关节角度偏差）。

在腿足机器人等应用中，常用伺服电机进行精确的位置、速度和扭力控制。伺服电机通常是集成的系统，包含电机本身和一个电机控制器。这些伺服控制器一般都集成了控制算法，如 PID 或 PD 控制，用户无需从头编写这些基本的控制算法。

但对于不同的运动模式，如奔跑或缓慢移动，机器人的控制需求可能会有所不同，这通常表现在对速度、加速度、稳定性等方面的不同需求。因此，PD 控制参数（$K_p$和$K_d$）需要根据不同的运动模式进行调整以适应各种动态行为。

比如 在奔跑时，系统可能需要更高的响应速度和较大的增益，以实现快速动作和高动态稳定性。此时，$K_p$（比例增益）和$K_d$微分增益）可能需要设置得相对较高。在缓慢移动时，为了保证平滑的运动和减少抖动，增益可能需要较低，特别是微分增益$K_d$以避免由噪声引起的过度响应。

通常电机控制器提供编程接口动态调整控制参数，这使得腿足机器人可以根据环境反馈或机器人状态实时调整控制参数。例如，如果感应到路面湿滑或不平，控制系统可以自动调整参数以适应这种变化，从而保持稳定和安全。

在腿足机器人中的应用场景

1. PID控制
   适用场景：

• 需要消除稳态误差的低速、高精度位置控制（如关节定点保持）。
• 存在持续外部干扰（如地面摩擦力、负载变化）时的力/力矩控制。

挑战：

• 腿足机器人的动态运动（如奔跑、跳跃）会引入高频扰动，积分项易导致振荡。
• 齿轮间隙、电机非线性（如死区）可能引发积分饱和。在PID控制中，若误差持续存在，积分项会不断累积（即使输出已达极限，如电机最大扭矩为10Nm，但控制器要求输出15Nm时，实际输出被限制在10Nm，即发生饱和。），导致系统恢复时出现严重超调或振荡。
• 腿足机器人在跳跃或者爬坡时，关节电机可能因负载过大达到扭矩上限。
• 电机转速受限于电源电压或散热能力，导致关节无法快速跟踪目标角度，导致出现速度饱和。

2. PD控制

• 适用场景：

• 高速动态运动（如四足机器人的步态控制），需快速响应且允许微小静差。
• 结合前馈补偿（如重力补偿、动力学模型）时，可弥补无积分项的不足。
• 常用于阻抗控制框架，调节关节刚度与阻尼。

波士顿动力Spot机器人采用PD+前馈控制，结合动力学模型实现稳定步态，MIT Cheetah通过PD控制调节关节力矩，实现高速奔跑。现代腿足机器人（如ANYmal、Unitree）多采用PD+模型预测控制（MPC）或强化学习优化参数，PID/PD作为底层执行器控制。

PID/PD公式在腿足机器人中的具体实现

上面的是公式是积分和微分，这里以一个例子说明是怎么从数学公式到代码的：
假设控制目标是让大腿关节跟踪目标角度${\theta }_{desired}$，实际角度为${\theta }_{actual}$,则：

• 误差信号为$e(t)={\theta }_{desired}-{\theta }_{actual}$
• 则离散化PID控制代码如下：

# 参数设定
Kp = 2.0
Ki = 0.1
Kd = 0.5
prev_error = 0
integral = 0

# 每个控制周期（如1ms）执行一次
def pid_control(theta_desired, theta_actual):
    error = theta_desired - theta_actual
    integral += error * dt  # dt为控制周期时间
    derivative = (error - prev_error) / dt
    output = Kp*error + Ki*integral + Kd*derivative
    prev_error = error
    return clamp(output, -max_torque, max_torque)  # 限幅抗饱和

实际应用中需要修正

• 重力补偿，在竖直方向的关节（如大腿），直接计算重力矩并加入前馈：

gravity_torque = m * g * l * cos(theta_actual)  # m:质量, l:力臂
output = pid_control(...) + gravity_torque

• 摩擦补偿：加入库伦摩擦模型$f_{friction}=K_c\cdot sing(\dot{\theta })$

参数整定方法

上面的例子中$K_p$等是固定的，在实际使用中需要调节以便取得最优参数：
(1) 手动试凑法
步骤：
1.先调节$K_p$至系统出现轻微振荡；
2.增加$K_d$抑制振荡；
3.若需静差消除，逐步加入 $K_i$(PID场景)。
适用性：适用于简单系统，但对腿足机器人这类非线性系统效率较低。

(2) Ziegler-Nichols法
步骤

1. 令$K_i=0,K_d=0$，逐渐增大$K_p$至临界振荡（临界增益$K_{cr}$，震荡周期$T_{cr}$）.
2. 根据经验公式设定参数：

• PID: $K_p=0.6K_{cr},K_i=2K_p/T_{cr},K_d=K_p{T}_{cr}/8$
• PD: $K_p=0.5K_{cr},K_d=K_{p}Tcr/8$
  这依赖系统线性假设，对腿足机器人可能需要二次调整

(3) 模型参考自适应控制（MRAC）

基于系统动力学模型（如拉格朗日方程），自动调整PID参数以适应负载变化或地面接触力。
常用于复杂地形下的自适应步态控制。

腿足机器人中的特殊考量

(1) 动力学耦合与非线性

• 多关节联动导致力矩耦合，需结合解耦控制或全状态反馈（如LQR）。
• 齿轮箱背隙、电机饱和等非线性问题，需加入抗饱和策略（如积分分离PID）。

(2) 力控与位置控的权衡

• 位置型PID：适用于轨迹跟踪（如关节角度控制）。
• 力控型PD：结合导纳控制，实现柔顺触地（如应对未知地形）。

(3) 实时性要求

• 高频控制周期（1kHz以上）需优化算法计算量，通常选择PD或简化PID（如去掉积分项）。

波士顿动力Spot机器人的PD+前馈控制

波士顿动力的 Spot 机器人使用的是一种结合了比例-微分控制器（PD 控制器）和前馈控制的复合控制结构，以实现高度稳定和响应灵敏的机器人行为。这种控制方法结合了反馈和预测控制的优势，优化了Spot机器人各种行走和操作任务的性能。

spot机器狗

前馈项是指基于系统动力学模型预测所需的控制输入，补偿预期扰动（如重力、惯性力）。PD反馈项则用于修正实际运动与期望轨迹的偏差，抑制振荡和外部扰动。

其控制框图如下：

期望轨迹(q_des, q_des_dot, q_des_ddot) 
      → 前馈控制器 → 控制力矩(τ_ff)
      ↓
PD控制器（误差反馈） → 控制力矩(τ_pd)
      ↓
总控制力矩 τ = τ_ff + τ_pd → 机器人动力学 → 实际轨迹(q, q_dot)

前馈力矩通过逆动力学模型计算，直接提供主要扭矩，无需等待误差累计，这涉及到动力学模型，模型计算的${\tau }_{ff}$更接近真实需求，减少反馈项压力。这里先给出来公式和简要解释，后续博客再深入解释：

动力学模型和公式

机器人的关节运动方程可以表示为：
$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)=\tau$$

• $\dot{q}$表示速度，$\ddot{q}$表示加速度
• $M(q)$:质量矩阵（惯性力）
• $C(q,\dot{q})$:科里奥利力和离心力矩阵
• $G(q)$:重力项
• $\tau$:关节力矩

总控制力矩由前馈项和PD反馈项组成：
$$\tau =\begin{array}{c}\underset{\text{前馈项}{\tau }_{ff}}{\underbrace{M(q_{des}){\ddot{q}}_{des}+C(q_{des},{\dot{q}}_{des}){\dot{q}}_{des}+G(q_{des})}}+\underset{\text{total}}{\underbrace{K_p(q_{des}-q)+K_d({\dot{q}}_{des}-\dot{q})}}\end{array}$$

• $K_p$，$K_d$:比例和微分增益矩阵
• $q_{des},{\dot{q}}_{des},{\ddot{q}}_{des}$是期望的位置、速度和加速度。

几点注意事项：

1. 动力学模型精度：前馈项依赖精确的$M(q)$、$C(q,\dot{q})$、$G(q)$模型，需通过系统辨别或者CAD参数获取
2. 增益调节，通过实验或者优化算法(如Ziegler-Nichols）调整$K_p$和$K_d$
3. 执行器饱和：需对输出力矩进行限幅
4. 状态估计：实际系统中$\dot{q}$需要通过观测器（如卡尔曼滤波）估计。
5. 上面的是一个关节，波士顿动力Spot可能融合了全身控制（Whole-Body Control）协调多个关节的运动，此外也可能融合了MPC（模型预测控制）来优化未来时刻的控制输入。