python 旋转坐标系_硬核推导火箭运动方程，并用python仿真实现

作者：yangjian

1 坐标系变换

火箭的运动方程是基于牛顿运动学方程推导，牛顿运动学方程是在惯性坐标系下成立的，目标的运动必须以固定的惯性坐标系为参考。本文研究带尾翼控制的火箭，火箭主要是由火箭发动机产生推力使箭体向上运动，由舵机控制尾翼产生使箭体旋转的力矩，这些在箭体坐标系下描述更合适，因此需要坐标系的变换。

惯性坐标系

​ 惯性坐标系是与地球固连的坐标系，通常选择NED坐标系，其原点在地面上一点，

轴指向北，

轴指向东方，

轴轴指向地心垂直向下。

箭体坐标系

箭体坐标系

轴指向火箭头部，

轴在火箭横轴对称面垂直于

轴，

轴按右手坐标系指向。

欧拉角

弹体（即弹体坐标系）相对地面坐标系的姿态，通常用3个角度（称欧拉角）来确定，

俯仰角

：火箭的纵轴与水平面 间的夹角。火箭纵轴指向水平面上方。

偏航角

：火箭纵轴在水平面内投影与地面坐标系

轴之间的夹角。

倾斜（滚动）角

：弹体坐标系的轴与包含导弹纵轴的铅垂平面之间的夹角。由箭体尾部顺纵轴前视。

惯性坐标系到箭体坐标系变换

箭体坐标可由惯性坐标系经过三次旋转变化得到。

旋转次序为：

、

、

。变换可用旋转矩阵

表示：

python代码实现如下：

   def _Euler2Rotation(self, _phi, _theta, _psi):
        phi = float(_phi)
        theta = float(_theta)
        psi = float(_psi)
        c_phi = np.cos(phi)
        s_phi = np.sin(phi)
        c_theta = np.cos(theta)
        s_theta = np.sin(theta)
        c_psi = np.cos(psi)
        s_psi = np.sin(psi)
        R_roll = np.array([[1, 0, 0],
                           [0, c_phi, s_phi],
                           [0, -s_phi, c_phi]])
        R_pitch = np.array([[c_theta, 0, -s_theta],
                            [0, 1, 0],
                            [s_theta, 0, c_theta]])
        R_yaw = np.array([[c_psi, s_psi, 0],
                          [-s_psi, c_psi, 0],
                          [0, 0, 1]])
        R = np.matmul(np.matmul(R_roll, R_pitch), R_yaw )
        return R.T 

四元数

因为火箭是以俯仰角90度的姿态垂直与地面发射，采用欧拉角变换矩阵会出现万象锁。使用四元数变换不会出现这种情况。

四元数与欧拉角的转换关系：