旋转矩阵与欧拉角之间互换公式

/*弧度 角度 */

#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/Nx

E[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+Oy

return 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;

}