1. EKO(Elastische Kompensation Optimierung)

Image Token: Fynrbfg5soyvydxQKNkcQPM9n3b

EKO根据负载转矩和关节柔性补偿位置指令。

EKO关注的是末端的位置精度，对于半闭环系统，末端位置$\theta_l$与指令位置是存在误差的，这个误差主要由负载$T_l$产生。对于单轴伺服，我们认为$T_l$可以由MCG矩阵计算出来，是一个已知量：

$T_l=M(q)\ddot{q}+C(q,\dot{q})\dot{q}+G(q)+f(q)\dot{q}$

末端位置$\theta_l$由指令和负载两部分组成：

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

如果要使末端位置和期望位置相同，即$\theta_l=\theta_d$，那么位置指令需要进行一定补偿：

$\theta_{ref}=G_\theta^{-1}\theta_{d}-G_\theta^{-1}G_TT_l$

EKO就是根据计算的 $T_l$，通过增益 $K_{EKO}$ 和低通滤波器（Q），补偿给位置指令的方法。

这两个参数与关节的阻尼和刚度相关：

$K_{EKO}\frac{1}{\tau s+1}=\frac{1}{Ds+K}$

在PPI-EKO的优化过程中的第二步就是调节这两个系数。

2. 传函推导

Image Token: N9l8bIkD4orO9FxMsgecNp62nCg

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

$\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}}$

3. EKO推导

如果要使末端位置和期望位置相同，即$\theta_l=\theta_d$，那么：

$\theta_{ref}=G_\theta^{-1}\theta_{d}-G_\theta^{-1}G_TT_l$

阻尼不为0时：

$G_\theta^{-1}G_T=\frac{J_{m} s^{3}+K_{m} K_{v}s^{2}+(Ds+K+K_{i} K_{m} K_{v})s+(K_{m} K_{p} K_{v} s+K_{i} K_{m} K_{p} K_{v})}{(Ds+K)( K_{m} K_{p} K_{v} s+ K_{i} K_{m} K_{p} K_{v})}=\frac{J_{m} s^{3}+K_{m} K_{v}s^{2}+(Ds+K+K_{i} K_{m} K_{v})s}{(Ds+K)( K_{m} K_{p} K_{v} s+ K_{i} K_{m} K_{p} K_{v})}+\frac{1}{Ds+K}=G_1+G_{LPF}$

其中$G_{LPF}=\frac{1}{Ds+K}=K_{EKO}\frac{1}{\tau s+1}$

EKO的方法：

$\theta_{refEKO}=\theta_{d}-G_{LPF}T_l$

与原式相比：

$\theta_{ref}=(\theta_{d}-G_{LPF}T_l)+((G_\theta^{-1}-1)\theta_{d}-G_1T_l)=\theta_{refEKO}+G_\Delta$

省略了后面部分：

$G_{\Delta}=((G_\theta^{-1}-1)\theta_{d}-G_1T_l)=\frac{\left(J_{l} J_{m}\right) s^{5}+\left(J_{l} K_{m} K_{v}\right) s^{4}+\left((Ds+K)(J_{l} +J_{m})+J_{l} K_{i} K_{m} K_{v}+J_{l} K_{m} K_{p} K_{v}\right) s^{3}+\left((Ds+K) K_{m} K_{v}+J_{l} K_{i} K_{m} K_{p} K_{v}\right) s^{2}+(Ds+K) K_{i} K_{m} K_{v} s}{(Ds+K)( K_{m} K_{p} K_{v} s+ K_{i} K_{m} K_{p} K_{v})}\theta_d-\frac{J_{m} s^{3}+K_{m} K_{v}s^{2}+(Ds+K+K_{i} K_{m} K_{v})s}{(Ds+K)( K_{m} K_{p} K_{v} s+ K_{i} K_{m} K_{p} K_{v})}T_l$

根据终值定理：

${G_{\Delta}}_{\infty}=\lim_{s \to 0} s\cdot \frac{1}{s} G_{\Delta}= 0$

当系统稳定时，未补偿部分无静差。无静差的前提的是弹性系数$K$完全准确，而这个系数无法完全测准，所以要根据实际末端效果进行调试。

~~不只是静差，因为是一阶低通，应该对一阶动态响应也有帮助。猜测这个滤波器可能二阶或者更高阶效果更好，但更难调。~~