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

11.2 误差动力学（定义）

误差动力学（Error dynamic）：决定受控系统关节误差θe(t)\theta_e(t)θe​(t)的演化的微分方程称为误差动力学。其中，θe(t)=θd(t)−θ(t)\theta_e(t)=\theta_d(t)-\theta(t)θe​(t)=θd​(t)−θ(t)

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

• 稳态响应esse_{ss}ess​：表示在θe(t)\theta_e(t)θe​(t)在t→∞t\rightarrow\inftyt→∞时的渐进误差；
• 瞬态响应：通过超调和（2%）调节时间进行表征。
  超调=∣θe,min⁡−essθe(0)−ess∣×100%\text{超调}=\left| \frac{\theta _{\mathrm{e},\min}-e_{ss}}{\theta _e\left( 0 \right) -e_{ss}} \right|\times 100\% 超调=​θe​(0)−ess​θe,min​−ess​​​×100%
  误差响应示例

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

单个连杆动力学模型：
τ=Mθ¨+bθ˙+mgrcos⁡θ\tau =M\ddot{\theta}+b\dot{\theta}+mgr\cos \theta τ=Mθ¨+bθ˙+mgrcosθ
PID控制器为：
τ=Kpθe+Ki∫θe(t)dt+Kdθ˙e\tau =K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t+K_d\dot{\theta}_e} τ=Kp​θe​+Ki​∫θe​(t)dt+Kd​θ˙e​

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

考虑Ki=0K_i=0Ki​=0，假设机器人在平面内运动（无重力影响），将PD控制律带入运动学模型：
Mθ¨+bθ˙=Kpθe+Kdθ˙e(1)M\ddot{\theta}+b\dot{\theta}=K_p\theta _e+K_d\dot{\theta}_e \tag{1} Mθ¨+bθ˙=Kp​θe​+Kd​θ˙e​(1)
若控制目标为设定点控制，即θd\theta_dθd​恒定，则有θ˙d=θ¨d=0\dot{\theta}_d=\ddot{\theta}_d=0θ˙d​=θ¨d​=0。
那么θe=θd−θ,θ˙e=−θ˙,θ¨e=−θ¨\theta _e=\theta _d-\theta ,\dot{\theta}_e=-\dot{\theta},\ddot{\theta}_e=-\ddot{\theta}θe​=θd​−θ,θ˙e​=−θ˙,θ¨e​=−θ¨. 上式可改写为：
−Mθ¨e−bθ˙e=Kpθe+Kdθ˙e-M\ddot{\theta}_e-b\dot{\theta}_e=K_p\theta _e+K_d\dot{\theta}_e−Mθ¨e​−bθ˙e​=Kp​θe​+Kd​θ˙e​
⇒Mθ¨e+(Kd+b)θ˙e+Kpθe=0(2)\Rightarrow M\ddot{\theta}_e+\left( K_d+b \right) \dot{\theta}_e+K_p\theta _e=0 \tag{2}⇒Mθ¨e​+(Kd​+b)θ˙e​+Kp​θe​=0(2)
将上式转换为标准二阶形式：
θ¨e+Kd+bMθ˙e+KpMθe=0\ddot{\theta}_e+\frac{K_d+b}{M}\dot{\theta}_e+\frac{K_p}{M}\theta _e=0θ¨e​+MKd​+b​θ˙e​+MKp​​θe​=0
⇒θ¨e+2ζωnθ˙e+ωn2θe=0\Rightarrow \ddot{\theta}_e+2\zeta \omega _n\dot{\theta}_e+\omega _{n}^{2}\theta _e=0⇒θ¨e​+2ζωn​θ˙e​+ωn2​θe​=0
式中，ζ=b+Kd2KpM,ωn=KpM\zeta =\frac{b+K_d}{2\sqrt{K_pM}},\omega _n=\sqrt{\frac{K_p}{M}}ζ=2Kp​M​b+Kd​​,ωn​=MKp​​​.

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

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

Mθ¨e+(Kd+b)θ¨e+Kpθ˙e+Kiθe=τ˙distM\ddot{\theta}_e+\left( K_d+b \right) \ddot{\theta}_e+K_p\dot{\theta}_e+K_i\theta _e=\dot{\tau}_{dist} Mθ¨e​+(Kd​+b)θ¨e​+Kp​θ˙e​+Ki​θe​=τ˙dist​
为确保上式所有根都具有负实部，其控制增益必须满足以下条件以确保稳定性，
{Kd>−b,Kp>0,(b+Kd)KpM>Ki>0\begin{cases} K_d>-b,\\ K_p>0,\\ \frac{\left( b+K_d \right) K_p}{M}>K_i>0\\ \end{cases}⎩⎨⎧​Kd​>−b,Kp​>0,M(b+Kd​)Kp​​>Ki​>0​

前馈控制

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

前馈加反馈线性化

考虑如下误差动力学：
θe+Kpθe+Ki∫θe(t)dt+Kdθ˙e=0\theta _e+K_p\theta _e+K_i\int{\theta _e\left( t \right) \mathrm{d}t}+K_d\dot{\theta}_e=0θe​+Kp​θe​+Ki​∫θe​(t)dt+Kd​θ˙e​=0
注意，上述误差动力学与前面的并不相同，可以理解为采用PID控制器对误差θe\theta_eθe​进行控制，由于θ¨e=θ¨d−θ¨\ddot{\theta}_e=\ddot{\theta}_d-\ddot{\theta}θ¨e​=θ¨d​−θ¨，选取指令加速度为θ¨=θ¨d−θ¨e\ddot{\theta}=\ddot{\theta}_d-\ddot{\theta}_eθ¨=θ¨d​−θ¨e​，可以得到：
θ¨=θ¨d+Kpθe+Ki∫θe(t)dt+Kdθ˙e⏟−θ¨e(3)\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}} \tag{3}θ¨=θ¨d​+−θ¨e​Kp​θe​+Ki​∫θe​(t)dt+Kd​θ˙e​​​(3)
将公式(3)带入机器人的动力学模型{M~,h~}\left\{ \widetilde{M},\widetilde{h} \right\}{M,h}中，即可得到前馈加反馈线性化控制器，也称为逆动力学控制器，或计算力矩控制器：
τ=M~(θ)[θ¨d+Kpθe+Ki∫θe(t)dt+Kdθ˙e]+h~(θ,θ˙)(11.33)\tau =\widetilde{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] +\widetilde{h}\left( \theta ,\dot{\theta} \right) \tag{11.33}τ=M(θ)[θ¨d​+Kp​θe​+Ki​∫θe​(t)dt+Kd​θ˙e​]+h(θ,θ˙)(11.33)