双闭环位置反馈控制

1. 概述

1.1 反馈方式比较

	位置指令跟随	负载扰动静差	系统稳定性
半闭环	能跟随	有静差，系统刚度越低，静差越大	稳定，能设置大增益
全闭环	能跟随	无静差	反共振频率有稳定性风险，只能设置较小增益
双闭环	能跟随	无静差	稳定，能设置大增益

双闭环控制结合两者的优势，既能够像半闭环一样设置大的增益参数，又能像全闭环一样消除负载扰动静差。

1.2 双惯量模型

关节模组的结构简化示意图如下，可以看成是双惯量系统。双编码器可以同时采集到$\theta_m$与$\theta_l$。

$$\left\{\begin{matrix} J_m\theta_ms^2=T_e-(K+Ds)(\theta_m-\theta_l) \\ J_l\theta_ls^2=(K+Ds)(\theta_m-\theta_l)-T_l \end{matrix}\right.$$

1.3 全闭环、半闭环与双闭环

1.3.1 单闭环

全闭环控制的优势是当负载受到负载端扰动 $T_l$时，可以保证末端实际位置 $\theta_l$与指令位置 $\theta_{ref}$无静差。

但如下图所示的全闭环控制的系统开环 bode 图可以看出，在反谐振频率点附近，幅频特性突变呈凸起态势。对比两种控制模式下抗谐振频率附近的幅频特性可以看出，高位置环增益会导致全闭环控制在抗谐振频率附近幅值增益的突增。因此，在实际应用中，为了避免系统产生超调和振荡， 全闭环控制的增益裕度要明显小于半闭环控制的增益裕度。（相位-180°时增益大于0dB不稳定）

1.3.2 双闭环

双闭环控制采用一个互补滤波器，融合末端与轴端位置反馈。

当$\tau=0$时，$Q=1$，为全闭环；当 $\tau=\infty$时，$Q=0$，为半闭环；其余情况下，为双闭环。

双闭环控制主要解决的是全闭环可能导致的系统失稳产生的振荡。

调节低通滤波器的$$\tau

$$时，应该从大向小调，即从稳定的半闭环逐渐向激进的全闭环调节。在工程应用中，通常不会专门测量双惯量系统的抗谐振频率，此时，滤波时间常数可根据实际系统手动调节，一般情况下可以先将滤波时间常数调大，保证系统不再振荡后，逐渐将时间常数调小，以获取更好地响应性能。受位置环带宽及机械传动系统参数的影响，一般全闭环低频振动频率在几到几十赫兹不等，通常取低通滤波器滤波频率为 10Hz\~50Hz 可以满足抑制低频振荡及高频噪声，在此基础上根据实际情况再进行微调，可以实现更好的动态响应。 如果知道被控对象的反共振频率，可以作为参考。  # 2. 半闭环控制  ## 2.1 传函推导 由位置环、速度环，可得： $$\omega_{cmd}=K_p(\theta_{ref}-\theta_m)$$ $$T_e = \frac{K_vK_m(s+K_i)}{s}(\omega_{cmd}-\omega_m)=\frac{K_vK_m(s+K_i)}{s}(K_p\theta_{ref}-(K_p+s)\theta_m)$$

将 $T_e$代入：

$$\left\{\begin{matrix} J_m\theta_ms^2=T_e-(K+Ds)(\theta_m-\theta_l) \\ J_l\theta_ls^2=(K+Ds)(\theta_m-\theta_l)-T_l \end{matrix}\right.$$

得到：

$$\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)(s+K_p)}{s}+(K+Ds)& -(K+Ds)\\ -(K+Ds) & J_ls^2+(K+Ds) \end{bmatrix} \begin{bmatrix} \theta_m\\\theta_l \end{bmatrix} = \begin{bmatrix} \frac{K_pK_vK_m(s+K_i)}{s}\theta_{ref}\\-T_l \end{bmatrix}$$

由Cramer法则得到：

$$\theta_l = \frac{\det {\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)(s+K_p)}{s}+(K+Ds)& \frac{K_pK_vK_m(s+K_i)}{s}\theta_{ref} \\ -(K+Ds) & -T_l \end{bmatrix} } } {\det {\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)(s+K_p)}{s}+(K+Ds)& -(K+Ds)\\ -(K+Ds) & J_ls^2+(K+Ds) \end{bmatrix}} }$$

设阻尼$D=0$，整理得到：

$$\theta_l=G_\theta\theta_{ref}+G_TT_l$$

其中：

$$G_\theta=\frac{\left(K K_{m} K_{p} K_{v} \right) s+K K_{i} K_{m} K_{p} K_{v} }{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left(J_{l} K+J_{m} K+J_{l} K_{i} K_{m} K_{v}+J_{l} K_{m} K_{p} K_{v}\right) s^{3}+\left(K K_{m} K_{v}+J_{l} K_{i} K_{m} K_{p} K_{v}\right) s^{2}+\left(K K_{i} K_{m} K_{v}+K K_{m} K_{p} K_{v}\right) s+K K_{i} K_{m} K_{p} K_{v}}$$ $$G_T=-\frac{J_{m} s^{3}+K_{m} K_{v}s^{2}+\left(K+K_{i} K_{m} K_{v}+K_{m} K_{p} K_{v}\right) s+K_{i} K_{m} K_{p} K_{v} }{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left(J_{l} K+J_{m} K+J_{l} K_{i} K_{m} K_{v}+J_{l} K_{m} K_{p} K_{v}\right) s^{3}+\left(K K_{m} K_{v}+J_{l} K_{i} K_{m} K_{p} K_{v}\right) s^{2}+\left(K K_{i} K_{m} K_{v}+K K_{m} K_{p} K_{v}\right) s+K K_{i} K_{m} K_{p} K_{v}}$$

2.2 稳态误差分析

对于阶跃响应，根据终值定理可得：

$${G_\theta}_{\infty}=\lim_{s \to 0} s\cdot \frac{1}{s} G_{\theta}= \frac{K K_{i} K_{m} K_{p} K_{v}}{K K_{i} K_{m} K_{p} K_{v}}=1$$

表明在没有负载端扰动时，跟随效果良好。

$${G_T}_\infty =\lim_{s \to 0} s\cdot \frac{1}{s} G_{T}= -\frac{K_{i} K_{m} K_{p} K_{v}}{K K_{i} K_{m} K_{p} K_{v}}=-\frac{1}{K}$$

这意味着，对于负载端的扰动力矩 $T_l$时，在无限长时间后，$\theta_l$与指令位置 $\theta_{ref}$依然保持一个静差，且该静差与刚度系数$K$成反比。

因此半闭环不能够完全消除位置静差，需要采取其他补偿方式。

2.3 稳定性分析

计算开环传递函数：

$$G=\frac{({J_l} {K_m} {K_p} {K_v}) s^{3}+\left({J_l} {K_i} {K_m} {K_p} {K_v)} s^{2}+(K {K_m} {K_p} {K_v}) s+K {K_i} {K_m} {K_p} {K_v}\right.}{({J_l} {J_m}) s^{5}+({J_l} {K_m} {K_v}) s^{4}+({J_l} K+{J_m} K+{J_l} {K_i} {K_m} {K_v}) s^{3}+(K {K_m} {K_v}) s^{2}+(K {K_i} {K_m} {K_v}) s}$$

整体相位都在-180°以上，不存在稳定性问题。

3. 全闭环控制

3.1 传函推导

由位置环、速度环，可得：

$$\omega_{cmd}=K_p(\theta_{ref}-\theta_l)$$ $$T_e = \frac{K_vK_m(s+K_i)}{s}(\omega_{cmd}-\omega_m)=\frac{K_vK_m(s+K_i)}{s}(K_p\theta_{ref}-K_p\theta_l-\theta_ms)$$

将 $T_e$代入：

$$\left\{\begin{matrix} J_m\theta_ms^2=T_e-(K+Ds)(\theta_m-\theta_l) \\ J_l\theta_ls^2=(K+Ds)(\theta_m-\theta_l)-T_l \end{matrix}\right.$$

得到：

$$\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)s}{s}+(K+Ds)& \frac{K_pK_vK_m(s+K_i)}{s}-(K+Ds)\\ -(K+Ds) & J_ls^2+(K+Ds) \end{bmatrix} \begin{bmatrix} \theta_m\\\theta_l \end{bmatrix} = \begin{bmatrix} \frac{K_pK_vK_m(s+K_i)}{s}\theta_{ref}\\-T_l \end{bmatrix}$$

由Cramer法则得到：

$$\theta_l = \frac{\det {\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)s}{s}+(K+Ds)& \frac{K_pK_vK_m(s+K_i)}{s}\theta_{ref} \\ -(K+Ds) & -T_l \end{bmatrix} } } {\det {\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)s}{s}+(K+Ds)& \frac{K_pK_vK_m(s+K_i)}{s}-(K+Ds)\\ -(K+Ds) & J_ls^2+(K+Ds) \end{bmatrix}} }$$

设阻尼$D=0$，整理得到：

$$\theta_l=G_\theta\theta_{ref}+G_TT_l$$

其中：

$$G_\theta=\frac{\left(K K_{m} K_{p} K_{v} \right) s+K K_{i} K_{m} K_{p} K_{v} }{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left(J_{l} K+J_{m} K+J_{l} K_{i} K_{m} K_{v}\right) s^{3}+\left(K K_{m} K_{v}\right) s^{2}+\left(K K_{i} K_{m} K_{v}+K K_{m} K_{p} K_{v}\right) s+K K_{i} K_{m} K_{p} K_{v}}$$ $$G_T=-\frac{J_{m} s^{3}+K_{m} K_{v}s^{2}+\left(K+K_{i} K_{m} K_{v}\right) s }{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left(J_{l} K+J_{m} K+J_{l} K_{i} K_{m} K_{v}\right) s^{3}+\left(K K_{m} K_{v}\right) s^{2}+\left(K K_{i} K_{m} K_{v}+K K_{m} K_{p} K_{v}\right) s+K K_{i} K_{m} K_{p} K_{v}}$$

3.2 稳态误差分析

对于阶跃响应，根据终值定理可得：

$${G_\theta}_{\infty}=\lim_{s \to 0} s\cdot \frac{1}{s} G_{\theta}= \frac{K K_{i} K_{m} K_{p} K_{v}}{K K_{i} K_{m} K_{p} K_{v}}=1$$

表明在没有负载端扰动时，跟随效果良好。

$${G_T}_\infty =\lim_{s \to 0} s\cdot \frac{1}{s} G_{T}= -\frac{0}{K K_{i} K_{m} K_{p} K_{v}}=0$$

这意味着，对于负载端的扰动力矩 $T_l$时，在无限长时间后，$\theta_l$与指令位置 $\theta_{ref}$无静差。

因此全闭环能够完全消除位置静差。

3.3 稳定性分析

计算开环传递函数：

$$G=\frac{(K {K_m} {K_p} {K_v}) s+K {K_i} {K_m} {K_p} {K_v}}{({J_l} {J_m}) s^{5}+({J_l} {K_m} {K_v}) s^{4}+({J_l} K+{J_m} K+{J_l} {K_i} {K_m} {K_v}) s^{3}+(K {K_m} {K_v}) s^{2}+(K {K_i} {K_m} {K_v}) s}$$

低频反共振点处可能存在稳定性问题。

4. 双闭环控制

4.1 传函推导

由位置环、速度环，可得：

$$\omega_{cmd}=K_p(\theta_{ref}-Q\theta_l-(1-Q)\theta_m)$$ $$T_e = \frac{K_vK_m(s+K_i)}{s}(\omega_{cmd}-\omega_m)=\frac{K_vK_m(s+K_i)}{s}(K_p\theta_{ref}-K_pQ\theta_l-K_p(1-Q)\theta_m-\theta_ms)$$

将 $T_e$代入：

$$\left\{\begin{matrix} J_m\theta_ms^2=T_e-(K+Ds)(\theta_m-\theta_l) \\ J_l\theta_ls^2=(K+Ds)(\theta_m-\theta_l)-T_l \end{matrix}\right.$$

得到：

$$\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)(s+K_p(1-Q))}{s}+(K+Ds)& \frac{K_pQK_vK_m(s+K_i)}{s}-(K+Ds)\\ -(K+Ds) & J_ls^2+(K+Ds) \end{bmatrix} \begin{bmatrix} \theta_m\\\theta_l \end{bmatrix} = \begin{bmatrix} \frac{K_pK_vK_m(s+K_i)}{s}\theta_{ref}\\-T_l \end{bmatrix}$$

由Cramer法则得到：

$$\theta_l = \frac{\det {\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)(s+K_p(1-Q))}{s}+(K+Ds)& \frac{K_pK_vK_m(s+K_i)}{s}\theta_{ref} \\ -(K+Ds) & -T_l \end{bmatrix} } } {\det {\begin{bmatrix} J_ms^2+\frac{K_vK_m(s+K_i)(s+K_p(1-Q))}{s}+(K+Ds)& \frac{K_pQK_vK_m(s+K_i)}{s}-(K+Ds)\\ -(K+Ds) & J_ls^2+(K+Ds) \end{bmatrix}} }$$

设阻尼$D=0$，整理得到：

$$\theta_l=G_\theta\theta_{ref}+G_TT_l$$

其中：

$$G_\theta=\frac{\left(K K_{m} K_{p} K_{v} \right) s+K K_{i} K_{m} K_{p} K_{v} }{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left(J_{l} K+J_{m} K+J_{l} K_{i} K_{m} K_{v}+J_{l} K_{p} K_{m} K_{v}(1-Q)\right) s^{3}+\left(K K_{m} K_{v}+J_{l} K_{i} K_{m} K_{v}K_{p}(1-Q)\right) s^{2}+\left(K K_{i} K_{m} K_{v}+K K_{m} K_{p} K_{v}\right) s+K K_{i} K_{m} K_{v}K_{p}}$$ $$G_T=-\frac{J_{m} s^{3}+K_{m} K_{v}s^{2}+\left(K+K_{i} K_{m} K_{v}+K_{m} K_{v} K_{p}(1-Q)\right) s+ \left(K_{i}K_{m} K_{v} K_{p}(1-Q)\right)}{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left(J_{l} K+J_{m} K+J_{l} K_{i} K_{m} K_{v}+J_{l} K_{p} K_{m} K_{v}(1-Q)\right) s^{3}+\left(K K_{m} K_{v}+J_{l} K_{i} K_{m} K_{v}K_{p}(1-Q)\right) s^{2}+\left(K K_{i} K_{m} K_{v}+K K_{m} K_{p} K_{v}\right) s+K K_{i} K_{m} K_{v}K_{p}}$$

4.2 稳态误差分析

对于阶跃响应，根据终值定理可得：

$${G_\theta}_{\infty}=\lim_{s \to 0} s\cdot \frac{1}{s} G_{\theta}= \frac{K K_{i} K_{m} K_{p} K_{v}}{K K_{i} K_{m} K_{p} K_{v}}=1$$

表明在没有负载端扰动时，跟随效果良好。

$${G_T}_\infty =\lim_{s \to 0} s\cdot \frac{1}{s} G_{T} = \lim_{s \to 0}-\frac{K_{i}K_{m} K_{v} K_{p}(1-Q)}{K K_{i} K_{m} K_{v}K_{p} } = \lim_{s \to 0}-\frac{(1-Q)}{K}=\lim_{s \to 0}\frac{\tau s}{(\tau s+1)K} =0$$

与全闭环相同，对于负载端的扰动力矩 $T_l$时，在无限长时间后，$\theta_l$与指令位置 $\theta_{ref}$无静差。

因此全闭环能够完全消除位置静差。

低通滤波器$Q$是用来协调半闭环与全闭环的融合滤波器。

只要$0\le\tau<\infty$，那么能够消除负载扰动造成的静差。且 $\tau$越小，静差消除越快。

4.3 稳定性分析

计算开环传递函数：

$$G=\frac{({J_l} {K_m} {K_p} {K_v}(1- Q)) s^{3}+({J_l} {K_i} {K_m} {K_p} {K_v}(1- Q)) s^{2}+(K {K_m} {K_p} {K_v}) s+K {K_i} {K_m} {K_p} {K_v}}{({J_l} {J_m}) s^{5}+({J_l} {K_m} {K_v}) s^{4}+({J_l} K+{J_m} K+{J_l} {K_i} {K_m} {K_v}) s^{3}+(K {K_m} {K_v}) s^{2}+(K {K_i} {K_m} {K_v}) s}$$

双闭环低通滤波器$Q$带宽内更像全闭环，带宽外像半闭环。在危险频率处可以改善幅值和相位。

低通滤波器极点应该在反共振点附近。调谐时，若不了解系统，应该从 $\tau$很大时开始调谐，逐渐减小，以获得更好的抗负载扰动能力。

5. 仿真实验

参数如下：

	0.000424
	0.0053
	40
	157.0796
	31.4169
	29.6041
	0.12
	0.05（对应反共振点）

对于开环传函，在相位-180°处全闭环幅值在0dB附近，有稳定性风险。

危险频率为反共振点频率：

$$\sqrt{\frac{K}{J_l}}=74.74rad/s$$

位置指令阶跃响应可以看出，全闭环振动较大，整定时间很长，双闭环效果虽然不如半闭环，但0.6s后也达到稳态。

对于扰动的阶跃响应，半闭环稳定后有0.034的静差，全闭环振动过大，而双闭环控制能够做到无静差和较短的整定时间。