/*弧度 角度 */#define PAI  3.141592653589793#define RADIAN(PAI / 180.0 )  //弧度 = 角度 * π  / 180#define ANGLE   (180.0 / PAI )  //角度 = 弧度 * 180 / π

2、旋轉矩陣轉換為尤拉角 

/**  * @brief  已知旋轉矩陣求尤拉角  * @param  arr : 旋轉矩陣陣列 N  O  A   * @retval 尤拉角(弧度值)  偏航角，俯仰角，滾動角  */float *Matrix_Eulerian_Angle(float *arr){float T[9];float E[3];for(int i =0;i<9;i++){T[i]=arr[i];}E[0] = atan2(T[1],T[0]);//Ny/NxE[1] = atan2(-1*T[2],(T[0]*cos(E[0])+T[1]*sin(E[0])));//Nz/(Nx+Ny)E[2] = atan2((T[6]*sin(E[0])-T[7]*cos(E[0])),(-1*T[3]*sin(E[0])+T[4]*cos(E[0])));//Ax-Ay/-Ox+Oyreturn E;}

3、尤拉角轉換為旋轉矩陣

/**  * @brief  已知尤拉角（角度值）求旋轉矩陣  * @param  arr : 尤拉角(弧度值)  偏航角，俯仰角，滾動角  * @retval   */Eulerian_Angle_Matrix(float *arr){Q_D(FitControlRobotWidget);float E[3];float T[9];int i=0;for(i=0;i<3;i++){E[i]=arr[i]*RADIAN;}/*N*/T[0] = 1*(cos(E[0])*cos(E[1]));T[1] = 1*(sin(E[0])*cos(E[1]));T[2] = -1*(sin(E[1]));/*O*/T[3] = 1*(cos(E[0])*sin(E[1])*sin(E[2])-sin(E[0])*cos(E[2]));T[4] = 1*(sin(E[0])*sin(E[1])*sin(E[2])+cos(E[0])*cos(E[2]));T[5] = 1*(cos(E[1])*sin(E[2]));/*A*/T[6] = 1*(cos(E[0])*sin(E[1])*cos(E[2])+sin(E[0])*sin(E[2]));T[7] = 1*(sin(E[0])*sin(E[1])*cos(E[2])-cos(E[0])*sin(E[2]));T[8] = 1*(cos(E[1])*cos(E[2]));return T;}