非线性干扰观测器应用于四通道架构

对于一般的机器人动力学方程：
$$M(q)\ddot{q}+N(q,\dot{q})\ddot{q}=\tau +{\tau }_d$$
其中：
$N(q,\dot{q})\ddot{q}=C(q,\dot{q})\dot{q}+G(q)$

换一种形式，写为：
$$M_x(q)\ddot{x}+N_x(q,\dot{q})=f+f_d$$
其中：
$M_s(q)=J{J}_{\ast }^{-1}M(q)J_{\ast }^{-1}{J}^T$
$N_x(q,\dot{q})=J{J}_{\ast }^{-1}N(q,\dot{q})-M_x(q)\dot{J}{J}_{\ast }^{-1}{J}^T\dot{x}$
$f=J{J}_{\ast }^{-1}\tau$
$f_d=J{J}_{\ast }^{-1}{\tau }_d$

主从机器人的控制方程可写为：
$$M_{xm}(q_m){\ddot{x}}_m+N_{xm}(q_m,{\dot{q}}_m)=f_m+f_h+f_{d,m}\text{\hspace{1em}(1)}$$
$$M_{xs}(q_s){\ddot{x}}_s+N_{xs}(q_s,{\dot{q}}_s)=f_s-f_e+f_{d,s}\text{\hspace{1em}(2)}$$
m,s分别代表机器人的主从，$f_{d,m}，f_{d,s}$ 表示笛卡尔空间中施加到主末端执行器和从末端执行器的（等效）干扰力，$f_m,f_s$是笛卡尔空间中应用于主末端执行器和从末端执行器的（等效）控制力, $f_h,f_e$是由人类操作者和远程环境施加到主末端执行器和从末端执行器的力。
基于干扰观测器的四通道遥操作控制系统如下：

众所周知，想要在没有干扰和延迟的情况下，系统变得完全透明，控制器应该这么设计：
$$\begin{array}{rl}{C}_1& =Z_s+C_s\\ C_2& =I+C_6\\ C_3& =I+C_5\\ C_4& =-Z_m-C_m\\ 0& \end{array}\text{\hspace{1em}(3)}$$
这里的设计和田老师书上基于加速度的四通道控制不同，基于加速度的控制律设计无法保证理想的透明性，但是更加简单直观，低频带内系统可以近似理想的透明度。2022/9/28 想了一下基于加速度控制原理是和设计力位混合控制器像，似乎不太能用于多自由度系统
${\hat{f}}_{d,m}$和${\hat{f}}_{d,s}$是干扰的估计量，设计控制律：

$$f_m=M_{xm}(q_m)[-C_m{x}_m-C_2{f}_e-C_4{x}_s+C_6{f}_h+f_h]+N_{xm}(q_m,{\dot{q}}_m)-f_h-{\hat{f}}_{d,m}$$
$$f_s=M_{xs}(q_s)[-C_s{x}_s+C_1{X}_m+C_3{f}_h-C_5{f}_e-f_e]+N_{xs}(q_s,{\dot{q}}_s)+f_e-{\hat{f}}_{d,s}$$
带入（1）（2）得
$${\ddot{x}}_m=-C_m{x}_m-C_2{f}_e-C_4{x}_s+C_6{f}_h+f_h+M_{xm}^{-1}(q_m)\Delta f_{d,m}\text{\hspace{1em}(4)}$$
$${\ddot{x}}_s=-C_s{x}_s+C_1{x}_m+C_3{f}_h-C_5{f}_e-f_e+M_{xs}^{-1}(q_s)\Delta f_{d,s}\text{\hspace{1em}(5)}$$
其中$\Delta f_{d,m}=f_{d,m}-{\hat{f}}_{d,m}$和$\Delta f_{d,s}=f_{d,s}-{\hat{f}}_{d,s}$其中（4）和（5）描述了一种N自由度常规无干扰四通道遥操作系统，其中主机器人和从机器人由单位惯性矩阵给出$Z_m(s)=s^{2}I$，$Z_s(s)=s^{2}I$（这里不理解，什么意思呢）
假如主从机器人的控制器是比例微分控制器：
$$C_m=K_{mv}s+K_{mp}$$
$$C_s=K_{sv}s+K_{sp}$$
将式3中的力反馈增益项($C_2,C_3$)设计为：
$$C_2=C_{mf},C_3=C_{sf}$$
选择的其他控制器以满足（3）给出的完全透明条件:
$$\begin{array}{rl}{C}_1& =s^{2}I+K_{sv}s+K_{sp}\\ C_2& =-(s^{2}I+K_{mv}s+K_{mp})\\ C_3& =C_{sf}-I\\ C_4& =C_{mf}-I\end{array}\text{\hspace{1em}(3)}$$
得出控制律：
$$f_m=M_{xm}(q_m)[{\ddot{x}}_s-K_{mv}\Delta {\dot{x}}_m-K_{mp}\Delta x_m+C_{mf}\Delta f]+N_{xm}(q_m,{\dot{q}}_m)-f_h-{\hat{f}}_{d,m}\text{\hspace{1em}(6)}$$
$$f_s=M_{xs}(q_s)[{\ddot{x}}_m-K_{sv}\Delta {\dot{x}}_s-K_{sp}\Delta x_s+C_{sf}\Delta f]+N_{xs}(q_s,{\dot{q}}_s)+f_e-{\hat{f}}_{d,s}\text{\hspace{1em}(7)}$$
令$\Delta x_m=x_m-x_s$,$\Delta x_s=x_s-x_m$,$\Delta f=f_h-f_e$带入(1),(2)得：
$$\Delta {\ddot{x}}_m=-K_{mv}\Delta {\dot{x}}_m-K_{mp}\Delta x_m+C_{mf}\Delta f+M_{xm}^{-1}(q_m)\Delta f_{d,m}\text{\hspace{1em}(8)}$$
$$\Delta {\ddot{x}}_s=K_{sv}\Delta {\dot{x}}_s+K_{sp}\Delta x_s-C_{sf}\Delta f-M_{xs}^{-1}(q_s)\Delta f_{d,s}\text{\hspace{1em}(9)}$$
由于$\Delta x=\Delta x_m=-\Delta x_s，\Delta \dot{x}=\Delta {\dot{x}}_m=-\Delta {\dot{x}}_s，\Delta \ddot{x}=\Delta {\ddot{x}}_m=-\Delta {\ddot{x}}_s$,假如 $C_{sf}^{-1}+C_{mf}^{-1}$可逆，将式(8)(9)分别乘 $C_{sf}^{-1},C_{mf}^{-1}$ 再相加得：
$$\Delta \ddot{x}+K_v\Delta \dot{x}+K_p\Delta x={\Psi }_{xm}(q_m)\Delta f_{dm}+M_{xm}^{-1}(q_m)\Delta f_{d,m}\text{\hspace{1em}(8)}$$
其中:
$$\begin{array}{rl}{K}_v& =(C_{sf}^{-1}+C_{mf}^{-1}{)}^{-1}(C_{sf}^{-1}{K}_{sv}-C_{mf}^{-1}{K}_{mv})\\ K_p& =(C_{sf}^{-1}+C_{mf}^{-1}{)}^{-1}(C_{sf}^{-1}{K}_{sp}-C_{mf}^{-1}{K}_{mp})\\ {\Psi }_{xm}(q_m)& =(C_{sf}^{-1}+C_{mf}^{-1}{)}^{-1}+C_{mf}^{-1}{M}_{xm}^{-1}(q_m)\\ {\Psi }_{xs}(q_s)& =(C_{mf}^{-1}+C_{sf}^{-1}{)}^{-1}+C_{sf}^{-1}{M}_{xs}^{-1}(q_s)\end{array}\text{\hspace{1em}(9)}$$
根据（1）（2）动力学方程，设计非线性干扰观测器：
$${\dot{\hat{f}}}_{d,m}=-L_m{\hat{f}}_{d,m}+L_m[M_{xm}(q_m){\ddot{x}}_m+N_{xm}(q_m,{\dot{q}}_m)-f_m-f_h]+{\Psi }_{xm}^T(q_m)(\Delta \dot{x}+\gamma \Delta x)(10)$$
$${\dot{\hat{f}}}_{d,s}=-L_s{\hat{f}}_{d,s}+L_s[M_{xs}(q_s){\ddot{x}}_s+N_{xs}(q_s,{\dot{q}}_s)+f_s-f_h]+{\Psi }_{xs}^T(q_s)(\Delta \dot{x}+\gamma \Delta x)(11)$$
这里没有懂干扰观测器的后坠部分，是相对于传统干扰观测器对其作出的改进
将式(1)(2)带入(10)(11)得：
$${\dot{\hat{f}}}_{d,m}=L_m\Delta f_{d,m}+{\Psi }_{xm}^T(q_m)(\Delta \dot{x}+\gamma \Delta x)$$
$${\dot{\hat{f}}}_{d,s}=L_s\Delta f_{d,s}+{\Psi }_{xs}^T(q_s)(-\Delta \dot{x}-\gamma \Delta x)$$
由${\dot{f}}_{d,m}={\dot{\hat{f}}}_{d,m}+\Delta {\dot{f}}_{d,m}$得：
$$\Delta {\dot{f}}_{d,m}={\dot{f}}_{d,m}-L_m\Delta f_{d,m}-{\Psi }_{xm}^T(q_m)(\Delta \dot{x}+\gamma \Delta x)$$
$$\Delta {\dot{f}}_{d,s}={\dot{f}}_{d,s}-L_s\Delta f_{d,s}-{\Psi }_{xs}^T(q_s)(-\Delta \dot{x}-\gamma \Delta x)$$