基于重力补偿的 PD 控制

PD 控制是常规的控制方法，设计简单，用李雅普诺夫方法证明简单，不需要系统的模型，是无模型控制中的基本方法。

令 [q~Tq˙T] $\begin{bmatrix} \tilde q^T \dot q^T \end{bmatrix}$ 为系统的状态向量，
其中： q~=qd−q $\tilde q = q_d - q$

表示期望位姿和实际位姿之间的误差。选择一下正定二次型作为李雅普诺夫函数：

$$V( \dot q, \tilde q)=\frac{1}{2} \dot q^T B(q) \dot q + \frac{1}{2} \tilde q^T K_p \tilde q > 0, \forall \dot q, \tilde q \neq 0$$

其中， Kp $Kp$为 n∗n $n*n$的正定矩阵。（第一项表示系统的动能，第二项表示系统的势能）

对时间求导：

$$\dot V = \ddot q^T B(q) \ddot q + \frac{1}{2} \dot q^T \dot B(q) \dot q - \dot q^t K_p \tilde q$$

根据动力学公式：

$$B(q) \ddot q +C(q,\dot q) \dot q + F\dot q + g(q)=\tau$$
得：

$$\dot V = \dot q^T (\dot B(q) - 2C(q,\dot q) \dot q) \dot q - \dot q^T F \dot q - \dot q^t ( \tau - g(q) -K_p \tilde q ) = - \dot q^T F \dot q - \dot q^t ( \tau - g(q) -K_p \tilde q )$$

第一项负定， τ=g(q)+Kpq~−Kdq˙ $\tau = g(q) + K_p \tilde q - K_d \dot q$ 时，

$$\dot V = - \dot q^T ( F + K_d ) \dot q$$
只要 F+Kd $F + K_d$ 正定，即可得：

$$\dot V < 0 ,\forall q \neq 0$$

根据李雅普诺夫方法，可得系统是稳定的。

基于重力补偿的 PD $PD$控制的控制框图如下：