机器人误差动力学与PID控制

#机器人 #控制 #误差动力学 #设定点控制 #计算力矩控制器 #PID控制
详细可参考书本:《现代机器人学：机构规划与控制》第十一章

11.2 误差动力学（定义）

误差动力学（Error dynamic）：决定受控系统关节误差${\theta }_e(t)$的演化的微分方程称为误差动力学。其中，$${\theta }_e(t)={\theta }_d(t)-\theta (t)$$

误差响应（Error response)：定义为受控系统在初始条件为${\theta }_e(0)=1$，且${\dot{\theta }}_e(0)={\ddot{\theta }}_e(0)=\cdots =0$。${\theta }_e(t)(t>0)$的响应。
典型的误差响应可以通过瞬态响应和稳态响应进行描述。

• 稳态响应$e_{ss}$：表示在${\theta }_e(t)$在$t\to \infty$时的渐进误差；
• 瞬态响应：通过超调和（2%）调节时间进行表征。
  $$\text{超调}=\left|\frac{{\theta }_{\mathrm{e},\min }-e_{ss}}{{\theta }_e\left(0\right)-e_{ss}}\right|\times 100\%$$
  误差响应示例

力或力矩输入的设定点控制

单个连杆动力学模型：
$$\tau =M\ddot{\theta }+b\dot{\theta }+mgr\cos \theta$$
PID控制器为：
$$\tau =K_p{\theta }_e+K_i\int {\theta }_e\left(t\right)\mathrm{d}t+K_d{\dot{\theta }}_e$$

水平面内的设定点控制（PD）

考虑$K_i=0$，假设机器人在平面内运动（无重力影响），将PD控制律带入运动学模型：
$$M\ddot{\theta }+b\dot{\theta }=K_p{\theta }_e+K_d{\dot{\theta }}_e\text{\hspace{1em}(1)}$$
若控制目标为设定点控制，即${\theta }_d$恒定，则有${\dot{\theta }}_d={\ddot{\theta }}_d=0$。
那么${\theta }_e={\theta }_d-\theta ,{\dot{\theta }}_e=-\dot{\theta },{\ddot{\theta }}_e=-\ddot{\theta }$. 上式可改写为：
$$-M{\ddot{\theta }}_e-b{\dot{\theta }}_e=K_p{\theta }_e+K_d{\dot{\theta }}_e$$
$$\Rightarrow M{\ddot{\theta }}_e+\left(K_d+b\right){\dot{\theta }}_e+K_p{\theta }_e=0\text{\hspace{1em}(2)}$$
将上式转换为标准二阶形式：
$${\ddot{\theta }}_e+\frac{K_d+b}{M}{\dot{\theta }}_e+\frac{K_p}{M}{\theta }_e=0$$
$$\Rightarrow {\ddot{\theta }}_e+2\zeta {\omega }_n{\dot{\theta }}_e+{\omega }_n^2{\theta }_e=0$$
式中，$\zeta =\frac{b+K_d}{2\sqrt{K_{p}M}},{\omega }_n=\sqrt{\frac{K_p}{M}}$.

垂直面内的设定点控制（PID）

当考虑在垂直平面内的设定点控制$\left(g>0\right)$时，误差动力学为：
$$M{\ddot{\theta }}_e+\left(K_d+b\right){\dot{\theta }}_e+K_p{\theta }_e=mgr\cos \theta$$
这意味着关节停留在满足$K_p{\theta }_e=mgr\cos \theta$这一条件的位形$\theta$处，即当${\theta }_d\ne \pm \pi /2$时，最终误差${\theta }_e$非零。其原因是：为了让连杆在${\theta }_d\ne \pm \pi /2$处保持静止，机器人必须提供非零扭矩，但是只有当${\theta }_e\ne 0$时，PD控制律才能在静止时产生非零扭矩。一般而言，课题公国增加增益$K_p$来减小这个稳态误差。
为了消除稳态误差，设置$K_i>0$，即使在零位置误差情况下，它也允许非零的稳态扭矩，此时只有积分误差必须为零。此时有，
$$\{\begin{array}{l}\tau =M\ddot{\theta }+b\dot{\theta }+mgr\cos \theta \\ \tau =K_p{\theta }_e+K_i\int {\theta }_e\left(t\right)\mathrm{d}t+K_d{\dot{\theta }}_e\end{array}$$
若控制目标为设定点控制：${\dot{\theta }}_d={\ddot{\theta }}_d=0\Rightarrow {\theta }_e={\theta }_d-\theta ,{\dot{\theta }}_e=-\dot{\theta },{\ddot{\theta }}_e=-\ddot{\theta }$.则设定点的误差动力学为：
$$M{\ddot{\theta }}_e+\left(K_d+b\right){\dot{\theta }}_e+K_p{\theta }_e+K_i\int {\theta }_e\left(t\right)\mathrm{d}t={\tau }_{\mathrm{dist}}$$
式中，${\tau }_{\mathrm{dist}}=mgr\cos \theta$，代表代替重力项的扰动力矩。对上式两边求导，可得三阶误差动力学：

$$M{\ddot{\theta }}_e+\left(K_d+b\right){\ddot{\theta }}_e+K_p{\dot{\theta }}_e+K_i{\theta }_e={\dot{\tau }}_{dist}$$
为确保上式所有根都具有负实部，其控制增益必须满足以下条件以确保稳定性，
$$\{\begin{array}{l}{K}_d>-b,\\ K_p>0,\\ \frac{\left(b+K_d\right)K_p}{M}>K_i>0\end{array}$$

前馈控制

轨迹跟踪的另一个策略时使用机器人的动力学模型来主动产生力矩，而不是等待误差。 令控制器动力学模型为:
$$\tau =\tilde{M}\left(\theta \left(t\right)\right)\ddot{\theta }\left(t\right)+\tilde{h}\left(\theta \left(t\right),\dot{\theta }\left(t\right)\right)$$
式中，如果$\tilde{M}\left(\theta \right)=M\left(\theta \right),\tilde{h}\left(\theta ,\dot{\theta }\right)=h\left(\theta ,\dot{\theta }\right)$，那么模型是完美的。
给定轨迹发生器的${\theta }_d,{\dot{\theta }}_d,{\ddot{\theta }}_d$，则通过控制器得到的前馈力矩可表示为：
$$\tau =\tilde{M}\left({\theta }_d\left(t\right)\right){\ddot{\theta }}_d\left(t\right)+\tilde{h}\left({\theta }_d\left(t\right),{\dot{\theta }}_d\left(t\right)\right)$$
显然，对于任意一个模型，都无法准确实现$\tilde{M}\left(\theta \right)=M\left(\theta \right),\tilde{h}\left(\theta ,\dot{\theta }\right)=h\left(\theta ,\dot{\theta }\right)$，因此单独的前馈控制意义不大，一般总是将前馈控制与反馈控制一起使用。

前馈加反馈线性化

考虑如下误差动力学：
$${\theta }_e+K_p{\theta }_e+K_i\int {\theta }_e\left(t\right)\mathrm{d}t+K_d{\dot{\theta }}_e=0$$
注意，上述误差动力学与前面的并不相同，可以理解为采用PID控制器对误差${\theta }_e$进行控制，由于${\ddot{\theta }}_e={\ddot{\theta }}_d-\ddot{\theta }$，选取指令加速度为$\ddot{\theta }={\ddot{\theta }}_d-{\ddot{\theta }}_e$，可以得到：
$$\ddot{\theta }={\ddot{\theta }}_d+\underset{-{\ddot{\theta }}_e}{\underbrace{K_p{\theta }_e+K_i\int {\theta }_e\left(t\right)\mathrm{d}t+K_d{\dot{\theta }}_e}}\text{\hspace{1em}(3)}$$
将公式(3)带入机器人的动力学模型$\left\{\tilde{M},\tilde{h}\right\}$中，即可得到前馈加反馈线性化控制器，也称为逆动力学控制器，或计算力矩控制器：
$$\tau =\tilde{M}\left(\theta \right)\left[{\ddot{\theta }}_d+K_p{\theta }_e+K_i\int {\theta }_e\left(t\right)\mathrm{d}t+K_d{\dot{\theta }}_e\right]+\tilde{h}\left(\theta ,\dot{\theta }\right)\text{\hspace{1em}(11.33)}$$

计算力矩控制