MIMO Backstepping | 制御系CAD

MIMO Mass-Damper-Spring System

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{x}=v\\ M\dot{v}+D(v)v+K(x)x=Bu \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{x}=x-x_d,\ \tilde{v}=v-v_d}\quad\Rightarrow\quad\dot{\tilde{x}}=\tilde{v} }$

$\displaystyle{(3)\quad v=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\tilde{v}+\Lambda\tilde{x}}=v-v_d+\Lambda\tilde{x}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=v-s=v_d-\Lambda\tilde{x}} }$

$\displaystyle{(6)\quad \dot{\tilde{x}}=\tilde{v}=v-v_d=-\Lambda\tilde{x}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{x}^TK_p\tilde{x}}\quad(K_p>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{x}^TK_p\dot{\tilde{x}}=\tilde{x}^TK_p(-\Lambda\tilde{x}+s) \quad\Rightarrow\quad \boxed{\dot{V}_1=-\tilde{x}^TK_p\Lambda\tilde{x}+s^TK_p\tilde{x}} }$

●Step 2

$\displaystyle{(9)\quad \dot{s}=\dot{\tilde{v}}+\Lambda\dot{\tilde{x}}=\dot{v}-\dot{v}_d+\Lambda\dot{\tilde{x}} }$

$\displaystyle{(10)\quad \begin{array}{l} M\dot{s}+D(v)s=M(\dot{v}-\dot{v}_d+\Lambda\dot{\tilde{x}})+D(v)(v-v_d+\Lambda\tilde{x})\\ =M\dot{v}+D(v)v-M(\dot{v}_d-\Lambda\dot{\tilde{x}})-D(v)(v_d-\Lambda\tilde{x})\\ =Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x \end{array} }$

$\displaystyle{(11)\quad \boxed{V_2=V_1+\frac{1}{2}s^TMs} }$

$\displaystyle{(12)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM\dot{s}\\ =-\tilde{x}^TK_p\Lambda\tilde{x}+s^TK_p\tilde{x}\\ +s^T(Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s)\\ =s^T(Bu-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s+K_p\tilde{x})\\ -\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(13)\quad \boxed{u=B^\dag(M(\dot{v}-\dot{s})+D(v)(v-s)+K(x)x-K_p\tilde{x}-K_ds)\quad(K_d>0) }$

$\displaystyle{(12')\quad \boxed{\dot{V}_2=-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

with Actuator Dynamics

$\displaystyle{(14)\quad \left\{\begin{array}{l} \dot{x}=v\\ M\dot{v}+D(v)v+K(x)x=Bu\\ T\dot{u}+u=u_c \end{array}\right. }$

●Step 3

$\displaystyle{(15)\quad Bu=z+\alpha_2\quad\Rightarrow\quad\dot{z}=B\dot{u}-\dot{\alpha}_2=BT^{-1}(u_c-u)-\dot{\alpha}_2 }$

$\displaystyle{(16)\quad \alpha_2=M(\dot{v}-\dot{s})+D(v)(v-s)+K(x)x-K_p\tilde{x}-K_ds }$

$\displaystyle{(12'')\quad \begin{array}{l} \dot{V}_2=s^T(z+\alpha_2-M(\dot{v}-\dot{s})-D(v)(v-s)-K(x)x-D(v)s+K_p\tilde{x})\\ -\tilde{x}^TK_p\Lambda\tilde{x}\\ =s^Tz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(17)\quad \boxed{V_3=V_2+\frac{1}{2}z^Tz} }$

$\displaystyle{(18)\quad \begin{array}{l} \dot{V}_3=\dot{V}_2+z^T\dot{z}\\ =s^Tz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x}+z^T(B\dot{u}-\dot{\alpha}_2)\\ =z^T(BT^{-1}(u_c-u)-\dot{\alpha}_2+s)-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} \end{array} }$

$\displaystyle{(19)\quad \boxed{u_c=u+TB^\dag(\dot{\alpha}_2-s-K_zz)} }$

$\displaystyle{(18')\quad \boxed{\dot{V}_3=-z^TK_zz-s^T(D(v)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$

MIMO Backstepping of Robots

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{q}=v\\ M(q)\dot{v}+C(q,v)v+g(q)=\tau \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{q}=q-q_d,\ \tilde{v}=v-v_d}\quad\Rightarrow\quad\dot{\tilde{q}}=\tilde{v} }$

$\displaystyle{(3)\quad v=s+\alpha_1 }$

$\displaystyle{(4)\quad \boxed{s=\tilde{v}+\Lambda\tilde{q}}=v-v_d+\Lambda\tilde{q}}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=v-s=v_d-\Lambda\tilde{q}} }$

$\displaystyle{(6)\quad \dot{\tilde{q}}=\tilde{v}=v-v_d=-\Lambda\tilde{q}+s }$

$\displaystyle{(7)\quad \boxed{V_1=\frac{1}{2}\tilde{q}^TK_q\tilde{q}}\quad(K_q>0) }$

$\displaystyle{(8)\quad \dot{V}_1=\tilde{q}^TK_q\dot{\tilde{q}}=\tilde{q}^TK_q(-\Lambda\tilde{q}+s) }$

●Step 2

$\displaystyle{(9)\quad \dot{s}=\dot{v}-\dot{\alpha}_1 }$

$\displaystyle{(10)\quad \begin{array}{l} M(q)\dot{s}=M(q)\dot{v}-M(q)\dot{\alpha}_1\\ =\tau-C(q,v)v-g(q)-M(q)\dot{\alpha}_1\\ =\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)-C(q,v)s \end{array} }$

$\displaystyle{(11)\quad \boxed{V_2=V_1+\frac{1}{2}s^TM(q)s} }$

$\displaystyle{(12)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM(q)\dot{s}+\frac{1}{2}s^T\dot{M}(q)s\\ =\tilde{q}^TK_q(-\Lambda\tilde{q}+s)\\ +s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)-C(q,v)s)\\ +\frac{1}{2}s^T\dot{M}(q)s\\ =s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)+K_q\tilde{q})\\ +\underbrace{s^T(\frac{1}{2}\dot{M}(q)-C(q,v))s}_{0}-\tilde{q}^TK_q\Lambda\tilde{q}\\ \end{array} }$

$\displaystyle{(13)\quad \boxed{\tau=M(q)(\dot{v}-\dot{s})+C(q,v)(v-s)+g(q)-K_ds-K_q\tilde{q}} }$

$\displaystyle{(12')\quad \begin{array}{l} \dot{V}_2=s^T(\tau-M(q)(\dot{v}-\dot{s})-C(q,v)(v-s)-g(q)+K_q\tilde{q})\\ -\tilde{q}^TK_q\Lambda\tilde{q}\\ =-s^TK_ds-\tilde{q}^TK_q\Lambda\tilde{q} \end{array} }$