mass: 20.0kg
max thrust: 400N
min thrust: 0N
二维矢量喷管摆动，最大摆动角度30°
箭体主体高度：1.3m
箭体总体高度(含着陆腿高度)：1.43m
推力作用点机体坐标系坐标：(0.0, 0.0, -0.55)m
惯性数据：
<inertial pos="0 0 0.55" mass="20.0" diaginertia="2.88279167 2.88279167 0.13225"/>Mujoco Actuator:
<actuator>
<motor class="VTOL" ctrlrange="0 400" gear="0 0 1 0 0 0" site="engine_site" name="thrust_z"/>
<motor class="VTOL" ctrlrange="-200 200" gear="1 0 0 0 0 0" site="engine_site" name="thrust_x"/>
<motor class="VTOL" ctrlrange="-200 200" gear="0 1 0 0 0 0" site="engine_site" name="thrust_y"/>
</actuator>
使用$\Omega = [\phi ,\theta, \psi ]^T$表示欧拉角，使用$\omega_B$表示机体坐标系中角速度，则欧拉角导数$\dot{\Omega}$和角速度的关系为： $$ \dot{\Omega} = \begin{bmatrix} \dot{\phi} \ \dot{\theta} \ \dot{\psi} \end{bmatrix} =\begin{bmatrix} 1 & \sin \phi\tan \theta & \cos \phi \tan\theta\ 0 & \cos\phi & -\sin\phi\ 0 & \frac{\sin\phi}{\cos\theta} & \frac{\cos\phi}{\cos\theta} \end{bmatrix} \begin{bmatrix} \omega _x \ \omega _y \ \omega _z \end{bmatrix} = \mathrm{M}^{-1}\omega_B $$ 其中矩阵$\mathrm{M}$： $$ \mathrm{M}=\begin{bmatrix} 1 & 0 & -\sin\theta \ 0 & \cos\phi & \cos\theta \sin\phi\ 0 & -\sin\phi & \cos\theta \cos\phi \end{bmatrix} $$ 欧拉角旋转矩阵 $$ \mathbf{R}_b^w(\psi, \theta, \phi) = \mathbf{R}_z(\psi) \cdot \mathbf{R}_y(\theta) \cdot \mathbf{R}_x(\phi) $$
其中： $$ s \phi, s \theta, s \psi = \sin(\phi), \sin(\theta), \sin(\psi) $$
旋转 $$ \begin{bmatrix} F \ \tau \end{bmatrix} = \begin{bmatrix} m , I_3 & 0 \ 0 & J \end{bmatrix} \begin{bmatrix} \dot{v} \ \dot{\omega} \end{bmatrix} + \begin{bmatrix} 0 \ \omega \times J\omega \end{bmatrix} $$ 矢量喷管
两个相互垂直独立的旋转轴，构成二维推力矢量。
对于矢量角度的定义：$\alpha>0$表示产生x轴正向旋转力矩，$\beta>0$表示产生y轴正向旋转力矩。
\begin{bmatrix} -t\cos\alpha\sin\beta \ t\sin\alpha \ t\cos\alpha\cos\beta \end{bmatrix} $$ 推力小角度近似： $$ \begin{bmatrix} t_x \ t_y \ t_z \end{bmatrix}
\begin{bmatrix} -t_{hov}\beta\ t_{hov}\alpha \ t \end{bmatrix} $$
设发动机推力作用点到机体重心的距离为$d$，则发动机推力产生的力矩，以及Z轴辅助RCS产生的力矩为： $$ \begin{bmatrix} \tau_x \ \tau_y \ \tau_z \end{bmatrix}
\begin{bmatrix} \frac{t\sin\alpha }{d} \ \frac{t\cos\alpha\sin\beta}{d} \ \tau_{RCS} \end{bmatrix} $$ 力矩小角度近似： $$ \begin{bmatrix} \tau_x \ \tau_y \ \tau_z \end{bmatrix}
\begin{bmatrix} \frac{t\alpha }{d} \ \frac{t\beta}{d} \ \tau_{RCS} \end{bmatrix} $$
机体坐标系角加速度和力矩的关系： $$ \begin{bmatrix} \dot{\omega }_x \ \dot{\omega }_y \ \dot{\omega }_z \end{bmatrix}
\begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} \times \mathbf{J} \begin{bmatrix} \omega_x \ \omega_y \ \omega_z \end{bmatrix} \right ) $$
\begin{bmatrix} \frac{t\alpha }{J_{xx}d} \ \frac{t\beta}{J_{yy}d} \ \frac{\tau_{RCS}}{J_{zz}} \end{bmatrix} $$
\begin{bmatrix} \frac{-t\cos\alpha\sin\beta}{m} \ \frac{t\sin\alpha}{m} \ \frac{t\cos\alpha\cos\beta}{m} \end{bmatrix} + \begin{bmatrix} g\sin\theta \ -g\cos\theta \sin\phi \ -g\cos\theta\cos\phi \end{bmatrix} $$ 机体坐标系速度导数(小角度近似)： $$ \begin{bmatrix} \dot{v}_x \ \dot{v}_y \ \dot{v}_z \end{bmatrix}
\begin{bmatrix} \frac{-t_{hov}\beta}{m} \ \frac{t_{hov}\alpha }{m} \ \frac{t }{m} \end{bmatrix} + \begin{bmatrix} g\theta \ -g\phi \ -g \end{bmatrix} $$ 使用的12维度状态向量： $$ \mathbf{x}=[x,y,z,v_x,v_y,v_z,\phi,\theta,\psi, \omega_x, \omega_y,\omega_z]^T $$ 4维度控制向量： $$ \mathbf{u}=[\alpha,\beta,\gamma, t]^T $$ 系统转移矩阵A： $$ \mathbf{A}= \begin{bmatrix} \mathbf{0}_3 & \mathbf{I}_3 & \mathbf{0}_3 & \mathbf{0}_3\ \mathbf{0}_3 & \mathbf{0}_3 &
\begin{bmatrix} 0 & g & 0\ -g & 0 & 0\ 0 & 0 &0 \end{bmatrix}
& \mathbf{0}_3\ \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{0}_3 & \mathbf{I}_3\ \mathbf{0}3 & \mathbf{0}3 & \mathbf{0}3 &\mathbf{0}3 \end{bmatrix} $$ 系统控制矩阵B： $$ \mathbf{B}= \begin{bmatrix} \mathbf{0}3 \ \begin{bmatrix} 0 & \frac{-t{hov}}{m} & 0\ \frac{t{hov}}{m} & 0 & 0\ 0 & 0 & \frac{1}{m} \end{bmatrix} \ \mathbf{0}3 \ \begin{bmatrix} \frac{t{hov}}{J{xx}d} & 0 & 0\ 0 & \frac{t{hov}}{J{yy}d} & 0\ 0 & 0 &0 \end{bmatrix} \end{bmatrix} $$ 系统连续方程： $$ \dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u} $$
python main.py