四轴飞行器软件控制的分析和计算

四轴飞行器软件控制的分析和计算旋转矩阵、欧拉角与四元数姿态表示方法广泛应用于动力学方程中，而欧拉角和四元数表示方法比起旋转矩阵具有节省存储空间和方便插值的优点。欧拉角表示方法具有简洁、几何意义明显等特点，同时姿态敏感器可以直接测出这些参数，能较为方便解用这些姿态参数描述的姿态动力学方程，但是采用欧拉角的姿态描述方法存在奇点问题，且需要大量的三角运算。而采用四元数表示方法可以避免这些问题，并且还提供了平滑插值，所以目前很多工程中选用四元数来描述飞行器运动及动力学方程中的姿态，但在设计控制规律时，仍然采用欧拉角描述。所以控制算法采用欧拉角与四元数相互结合相互转化的方法来实现,积分和修正用四元数算法（因为运算量较少、无奇点误差），最后用欧拉角输出控制PID（因为角度比较直观）。定义ψ、θ、φ分别为绕Z轴、Y轴、X轴的旋转角度（即为欧拉角表示），如果用Tait-Bryanangle表示，分别为Yaw、Pitch、Roll。四元数的定义如下：)2/cos(wTzyxwq　　　，122222zyxwq通过旋转轴和绕该轴旋转的角度可以构造一个四元数：)2/cos(w；)cos()2/sin(xx；)cos()2/sin(yy；)cos()2/sin(zz欧拉角到四元数的转化:)2/cos()2/sin()2/sin()2/sin()2/cos()2/cos()2/sin()2/cos()2/sin()2/cos()2/sin()2/cos()2/sin()2/sin()2/cos()2/cos()2/cos()2/sin()2/sin()2/sin()2/sin()2/cos()2/cos()2/cos(zyxwq四元数到欧拉角的转化：))(21),(2(2arctan))(2arcsin())(21),(2(2arctan2222zyxywzzxwyyxyzwx