Marine Craft | 制御系CAD

Marine Craft

$\displaystyle{(1)\quad \left\{\begin{array}{l} \dot{\eta}=J_\Theta(\eta)\nu\\ M\dot{\nu}+C(\nu)\nu+D(\nu)\nu+g(\eta)=\tau\\ \tau=Bu \end{array}\right. }$

$1^\circ$ $M=M^T$ , $\dot{M}=0$
$2^\circ$ $C(\nu)=-C^T(\nu)$
$3^\circ$ $D(\nu)>0$
$4^\circ$ $BB^T$ は正則
$5^\circ$ $J_\Theta(\eta)$ はオイラー角変換行列

$\displaystyle{(1')\quad \left\{\begin{array}{l} M^*(\eta)\ddot{\eta}+C^*(\nu,\eta)\dot{\eta}+D^*(\nu,\eta)\dot{\eta}+g^*(\eta)=J^{-T}_\Theta(\eta)\tau\\ M^*(\eta)=J^{-T}_\Theta(\eta)MJ^{-1}_\Theta(\eta)\\ C^*(\nu,\eta)=J^{-T}_\Theta(\eta)(C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta))J^{-1}_\Theta(\eta)\\ D^*(\nu,\eta)=J^{-T}_\Theta(\eta)D(\nu)J^{-1}_\Theta(\eta)\\ g^*(\eta)=J^{-T}_\Theta(\eta)g(\eta) \end{array}\right. }$

●Step 1

$\displaystyle{(2)\quad \boxed{\tilde{\eta}=\eta-\eta_d,\ \tilde{\nu}=\nu-\nu_d}\quad\Rightarrow\quad\dot{\tilde{\eta}}=J_\Theta(\eta)\tilde{\nu} }$

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

$\displaystyle{(4)\quad \boxed{s=\dot{\tilde{\eta}}+\Lambda\tilde{\eta}}=\dot{\eta}-\dot{\eta}_d+\Lambda\tilde{\eta}}\quad(\Lambda={\rm diag}\{\lambda_1,\dots,\lambda_m\}) }$

$\displaystyle{(5)\quad \boxed{\alpha_1=\dot{\eta}-s=\dot{\eta}_d-\Lambda\tilde{\eta}} }$

$\displaystyle{(6)\quad \dot{\tilde{\eta}}=\dot{\eta}-\dot{\eta}_d=-\Lambda\tilde{\eta}+s }$

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

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

●Step 2

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

$\displaystyle{(10)\quad \dot{s}=\ddot{\tilde{\eta}}+\Lambda\dot{\tilde{\eta}}=\ddot{\eta}-\ddot{\eta}_d+\Lambda\dot{\tilde{\eta}} \quad\Rightarrow\quad\ddot{\eta}=\dot{s}+\ddot{\eta}_d-\Lambda\dot{\tilde{\eta}} }$

$\displaystyle{(11)\quad \begin{array}{l} M^*(\eta)(\dot{s}+\ddot{\eta}_d-\Lambda\dot{\tilde{\eta}})\\ +C^*(\nu,\eta)(s+\dot{\eta}_d-\Lambda\tilde{\eta}) +D^*(\nu,\eta)(s+\dot{\eta}_d-\Lambda\tilde{\eta}) +g^*(\eta)=J^{-T}_\Theta(\eta)\tau \end{array} }$

$\displaystyle{(1')\quad \left\{\begin{array}{l} M^*(\eta)=J^{-T}_\Theta(\eta)MJ^{-1}_\Theta(\eta)\\ C^*(\nu,\eta)=J^{-T}_\Theta(\eta)(C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta))J^{-1}_\Theta(\eta)\\ D^*(\nu,\eta)=J^{-T}_\Theta(\eta)D(\nu)J^{-1}_\Theta(\eta)\\ g^*(\eta)=J^{-T}_\Theta(\eta)g(\eta) \end{array}\right. }$

$\displaystyle{(12)\quad \begin{array}{l} M^*(\eta)\dot{s}=-C^*(\nu,\eta)s-D^*(\nu,\eta)s+J^{-T}_\Theta(\eta)\tau\\ -M^*(\eta)(\ddot{\tilde{\eta}}_d-\Lambda\dot{\tilde{\eta}})-C^*(\nu,\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})-D^*(\nu,\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})-g^*(\eta)\\ =-C^*(\nu,\eta)s-D^*(\nu,\eta)s\\ +J^{-T}_\Theta(\eta)(\tau -\underbrace{J^{T}_\Theta(\eta)M^*(\eta)J_\Theta(\eta)}_{M} \underbrace{J^{-1}_\Theta(\eta)(\ddot{\tilde{\eta}}_d-\Lambda\dot{\tilde{\eta}})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)C^*(\nu,\eta)J_\Theta(\eta)}_{C(\nu)-MJ^{-1}_\Theta(\eta)\dot{J}_\Theta(\eta)} \underbrace{J^{-1}_\Theta(\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)D^*(\nu,\eta)J_\Theta(\eta)}_{D(\nu)} \underbrace{J^{-1}_\Theta(\eta)(\dot{\tilde{\eta}}_d-\Lambda\tilde{\eta})}_ {\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}}\\ -\underbrace{J^{T}_\Theta(\eta)g^*(\eta)}_{g(\eta)})\\ =-C^*(\nu,\eta)s-D^*(\nu,\eta)s\\ +J^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})-C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)) \end{array} }$

$\displaystyle{(13)\quad \boxed{V_2=V_1+\frac{1}{2}s^TM^*(\eta)s} }$

$\displaystyle{(14)\quad \begin{array}{l} \dot{V}_2=\dot{V}_1+s^TM^*(\eta)\dot{s}+\frac{1}{2}s^T\dot{M}^*(\eta)s\\ =-\tilde{\eta}^TK_p\Lambda\tilde{\eta}+s^TK_p\tilde{\eta}\\ +s^T(-C^*(\nu,\eta)s-D^*(\nu,\eta)s)\\ +s^TJ^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}}) -C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu}) -D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta))\\ +\frac{1}{2}s^T\dot{M}^*(\eta)s\\ =s^TJ^{-T}_\Theta(\eta)(\tau-M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})-C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)\\+J^{T}_\Theta(\eta)K_p\tilde{\eta}) +\frac{1}{2}\underbrace{s^T(\dot{M}^*(\eta)-C^*(\nu,\eta))s}_{0}\\ -s^TD^*(\nu,\eta)s-\tilde{\eta}^TK_p\Lambda\tilde{\eta} \end{array} }$

$\displaystyle{(15)\quad \boxed{\begin{array}{l} \tau=M(\ddot{\tilde{\nu}}_d-\Lambda\dot{\tilde{\nu}})+C(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-D(\nu)(\dot{\tilde{\nu}}_d-\Lambda\tilde{\nu})-g(\eta)\\ -J^{T}_\Theta(\eta)K_p\tilde{\eta}-J^{T}_\Theta(\eta)K_ds \end{array}} }$

$\displaystyle{(14')\quad \boxed{\dot{V}_2=-s^T(D^*(\nu,\eta)+K_d)s-\tilde{x}^TK_p\Lambda\tilde{x} }$