博客

————————–以下方式没实际使用上———————-

pitch的范围是-90°-0-90°
roll的范围是-180°-180°
yaw的范围是-180°-180°

1.首先确定坐标系，是ENU坐标系还是NED坐标系，
方法是看x,y,z的正方向，x指向正东，y指向正北，z指向天则为ENU坐标系。x指向正北，y指向正东，z指向地则为NED坐标系
已经确认我用的是ENU坐标系，z对应yaw ， y对应pitch ，x对应roll

2.确定旋转方向
方法用右手定则，大拇指先指向Z，绕Z的正方向（其余手指）旋转，yaw变化，其余pitch和roll不变。第二步 大拇指指向Y，pitch变化，其余yaw和roll不变化，第三步，大拇指指向X的正方向，roll变化，其余两轴不变化。
则确认欧拉角的旋转方向是Z-Y-X，确认旋转方向至关重要。

3.理解欧拉角、姿态角、四元数、旋转矩阵

端午节期间在疑似新冠头晕后遗症、娃的爱的奥特曼铁拳以及老婆的鼓励下，终于发现点眉目

发现roll偏转一定角度后，pitch怎么旋转，角度都不能到90°，在pitch由0°-90°-0°旋转时，得出的角度都是0°-30°-0°这样，于是查阅资料，发现有旋转矩阵这个东西，找个各个版本的，重新确认旋转方向，将角度带进去，不对，求转置矩阵，还是不对，最后把三个向量[1 0 0 ][0 1 0] [0 0 1]带进去，好像是对了 – -，以下是代码

—————————————————————————————————————
#include #include
#include
#include

#define MI 0.017453292519943295
#define M_PI 3.14159265358979323846

struct _Quat{
double w;
double x;
double y;
double z;
};

struct _Quat Quat;

void ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
// Abbreviations for the various angular function

yaw = yaw*MI;
pitch = pitch*MI;
roll = roll*MI;

double w,x,y,z;
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);

Quat.w = cy * cp * cr + sy * sp * sr;
Quat.x = cy * cp * sr – sy * sp * cr;
Quat.y = sy * cp * sr + cy * sp * cr;
Quat.z = sy * cp * cr – cy * sp * sr;
// printf(“四元数:\r\n”);
// printf(“w = %f\r\nx = %f\r\ny = %f\r\nz = %f\r\n”,Quat.w,Quat.x,Quat.y,Quat.z);
}

int Att_Convert(float y,float p,float r)
{
float cx ,cy ,cz;
float yx ,yy ,yz;

r = r*MI;
p = p*MI;
y = y*MI;

float yc11 = cos(y);
float yc12 = -sin(y);
float yc13 = 0;

float yc21 = sin(y);
float yc22 = cos(y);
float yc23 = 0;

float yc31 = 0;
float yc32 = 0;
float yc33 = 1;

float rc11 = 1;
float rc12 = 0;
float rc13 = 0;

float rc21 = 0;
float rc22 = cos(r);
float rc23 = -sin(r);

float rc31 = 0;
float rc32 = sin(r);
float rc33 = cos(r);

float c11 = cos(y)*cos(p);
float c21 = cos(p)*sin(y);
float c31 = -sin(p);

float c12 = cos(y)*sin(r)*sin(p)-cos(r)*sin(y);
float c22 = cos(y)*cos(r)+sin(y)*sin(r)*sin(p);
float c32 = cos(p)*sin(r);

float c13 = sin(y)*sin(r)+cos(y)*cos(r)*sin(p);
float c23 = cos(y)*sin(r)+cos(r)*sin(y)*sin(p);;
float c33 = cos(r)*cos(p);
/*
cx = y*c11+ p*c12 + r*c13;
cy = y*c21+ p*c22 + r*c23;
cz = y*c31+ p*c32 + r*c33;
*/

cx = r*c11+ p*c21 + y*c31;
cy = r*c12+ p*c22 + y*c32;
cz = r*c13+ p*c23 + y*c33;

yx = r * yc11 + p * yc12 + y * yc13;
yy = r * yc21 + p * yc22 + y * yc23;
yz = r * yc31 + p * yc32 + y * yc33;

cx = cx / MI;
cy = cy / MI;
cz = yz / MI;

yx = yx / MI;
yy = yy / MI;
yz = yz / MI;

printf(“yx’=%f\r\nyy’=%f\r\nyz’=%f\r\n”,cx,cy,cz);
}
int main(int argc,char * argv[])
{
float yaw = atof(argv[1]);
float pitch = atof(argv[2]);
float roll = atof(argv[3]);

// float q3 = atof(argv[4]);

float q0,q1,q2,q3;

double c11,c12,c13,c21,c22,c23,c31,c32,c33;

double g1;
double g2;
double g3;
double g4;
double g5;

float ax=0;
float ay=0;
float az=0;

float px,py,pz;

float cx1,cx2,cx3,cy1,cy2,cy3,cz1,cz2,cz3;

ToQuaternion(yaw,pitch,roll);

q0 = Quat.w;
q1 = Quat.x;
q2 = Quat.y;
q3 = Quat.z;

// printf (“四元数:\r\nq0=%f\r\nq1=%f\r\nq2=%f\r\nq3=%f\r\n”,q0,q1,q2,q3);

g1 = 2*(q1*q3-q0*q2);
g2 = 2*(q2*q3+q0*q1);
g3 = q0*q0-q1*q1-q2*q2+q3*q3;
g4 = 2*(q1*q2+q0*q3);
g5 = q0*q0+q1*q1-q2*q2-q3*q3;

c11 = q0*q0+q1*q1-q2*q2-q3*q3;
c21 = 2*(q1*q2+q0*q3);
c31 = 2*(q1*q3-q0*q2);

c12 = 2*(q1*q2 -q0*q3);
c22 = q0*q0-q1*q1+q2*q2-q3*q3;
c32 = 2*(q2*q3+q0*q1);

c13 = 2*(q1*q3 +q0*q2);
c23 = 2*(q2*q3-q0*q1);
c33 = q0*q0-q1*q1-q2*q2+q3*q3;
/*
cx1 = c11*0.6+c12 * 0 + c13 * 0;
cx2 = c21*0.6+c22 * 0 +c23 * 0;
cx3 = c31*0.6+c32 * 0 + c33 * 0;

cy1 = c11*0+c12 * -0.6 + c13 * 0;
cy2 = c21*0+c22 * -0.6 +c23 * 0;
cy3 = c31*0+c32 * -0.6 + c33 * 0;

cz1 = c11*0+c12 * 0 + c13 * -0.6;
cz2 = c21*0+c22 * 0 +c23 * -0.6;
cz3 = c31*0+c32 * 0 + c33 * -0.6;
*/

cx1 = c11*1+c12 * 0 + c13 * 0;
cx2 = c21*1+c22 * 0 + c23 * 0;
cx3 = c31*1+c32 * 0 + c33 * 0;

cy1 = c11*0+c12 * 1 + c13 * 0;
cy2 = c21*0+c22 * 1 + c23 * 0;
cy3 = c31*0+c32 * 1 + c33 * 0;

cz1 = c11*0+c12 * 0 + c13 * 1;
cz2 = c21*0+c22 * 0 + c23 * 1;
cz3 = c31*0+c32 * 0 + c33 * 1;

//printf(“旋转矩阵\r\n”);
// printf(“c11=%f c12=%f c13=%f\r\nc21=%f c22=%f c23=%f\r\nc31=%f c32=%f c33=%f\r\n”,c11,c12,c13,c21,c22,c23,c31,c32,c33);
printf(“向量\r\n”);
printf(“cx1=%f cx2=%f cx3=%f\r\n”,cx1,cx2,cx3);
printf(“cy1=%f cy2=%f cy3=%f\r\n”,cy1,cy2,cy3);
printf(“cz1=%f cz2=%f cz3=%f\r\n”,cz1,cz2,cz3);

ax = atan2(cx2,sqrt(cx1*cx1 + cx3*cx3))/MI; //pitch ,绕y轴旋转，取y-xoz平面的角度
ay = atan2(cz1,sqrt(cz2*cz2 + cz3*cz3))/MI; //yaw ,绕z轴旋转，取z-xoy平面的角度
az = atan2(cy3,sqrt(cy1*cy1 + cy2*cy2))/MI; //roll ,绕X轴旋转，取x-yoz平面的角度
/*
if(cx1 >=0)
{
ax = atan2(cx2,sqrt(cx1*cx1 + cx3*cx3))/MI;
}
else
{
ax = 180 – atan2(cx2,sqrt(cx1*cx1 + cx3*cx3))/MI;
}

if(ax > 180)
{
ax = ax-360;
}

if (cx1 >=0 )
{
ay = -atan2(cx3,sqrt(cx1*cx1 + cx2*cx2))/MI; //PITCH 绕
}
else
{
ay = 180+atan2(cx3,sqrt(cx1*cx1 + cx2*cx2))/MI; //PITCH 绕
}
if (ay>180)
{
ay = ay-360;
}

if (cy2>=0)
{
az = atan2(cy3,sqrt(cy1*cy1 + cy2*cy2))/MI; //PITCH 绕
}
else
{
az = 180 – atan2(cy3,sqrt(cy1*cy1 + cy2*cy2))/MI; //PITCH 绕
}
if (az > 180)
{
az = az-360;
}
*/
/*
ax = atan2(cz1,sqrt(cz2*cz2 + cz3*cz3))/MI;
ay = atan2(cz2,sqrt(cz1*cz1 + cz3*cz3))/MI;
az = atan2(cz3,sqrt(cz1*cz1 + cz2*cz2))/MI;
*/
// ay = atan2(cy2,sqrt(cy1*cy1 + cy3*cy3)); //
// az = atan2(cx3,sqrt(cx1*cx1 + cx2*cx2)); //
/*
ay = atan2(cy2,sqrt(cy1*cy1 + cy3*cy3));

ay = atan2(cy2,sqrt(cy1*cy1 + cy3*cy3));
az = atan2(cz3,sqrt(cz1*cz1 + cz2*cz2));

*/
printf(“角度\r\n”);
printf(“ax =%f \r\n”,ax);
printf(“ay=%f \r\n”,ay);
printf(“az =%f \r\n”,az);

// Att_Convert(y,p,r);
}

—————————————————————————————
求完后还是感觉不对，压根就没解决pitch到不了90°的问题，于是还是从欧拉角下手，根据pitch-roll的旋转方向，打算推出roll对pitch的影响，即当pitch是30°时，roll从0-90°变化的时候，让pitch由30-90°变化，继续冥思苦想，说实话年纪大了空间想象能力是真的差，最后想到了切西瓜的模型，假设在半径为1的西瓜上来回切，最终通过三角函数解决了，以下是代码
————————————————————————————————————————————–
#include #include
#include
#include
#define MI 0.017453292519943295
#define M_PI 3.14159265358979323846

int main(int argc,char * argv[])
{
float a1,a2,a3;

float a = atof(argv[1]);
float b = atof(argv[2]);

float x1,x2,x3,x4;

float c = 0;

float theta = a*MI;

float A = 2*sin(theta/2);

float B = sqrt(2);
float C = sqrt(2);

float D = acos(A/2/C)/MI; //

float E = sin(45*MI);
float F = 0;
float H = 0;

float G = 0;
b = 45+b;

F = E / tan(b*MI);
F = E -F;

//A F D
G = sqrt(A*A+F*F-2*A*F*cos(D*MI));
//1 F 45
H = sqrt(1*1+F*F-2*1*F*cos(45*MI));

printf(“A = %f\r\n”,A);
printf(“b = %f\r\n”,b);
printf(“E = %f\r\n”,E);
printf(“F = %f\r\n”,F);
printf(“D = %f\r\n”,D);
printf(“G = %f\r\n”,G);
printf(“H = %f\r\n”,H);

// 1 G H

a = 1;
b = G;
c = H;

printf(“\r\n”);
printf(“D1 = %f\r\n”,a);
printf(“D2 = %f\r\n”,b);
printf(“D3 = %f\r\n”,c);

/*
cosA=（b²+c²-a²）/2bc；
cosB=(a²+c²-b²)/2ac；
cosC=(a²+b²-c²)/2ab。
*/

x1 = acos((b*b+b*b-a*a)/(2*b*c));
x2 = acos((a*a+c*c-b*b)/(2*a*c));
x3 = acos((a*a+b*b-c*c)/(2*a*b));

x1= x1/MI;
x2= x2/MI;
x3= x3/MI;

printf(“x1 = %f\r\n”,x1);
printf(“x2 = %f\r\n”,x2);
printf(“x3 = %f\r\n”,x3);
}

x2是最终求的数据
———————————————————————————————
求出来了带进飞控里看下呗，还真不行，如果roll转动的话直接导致pitch变化，当有roll的时候，pitch会基本等于roll，但是遥控器给出的pitch还是0，肯定会导致飞机转圈，以后再解决把，最后还是用的原始欧拉角- –

陆陆续续四轴搞了一年了，手搓的机架、飞控、遥控器、姿态调节、上位机程序、无线LoRa数传，也飞起来的，那就暂时告一段落吧，先搞搞linux，搞定图传后再研究四轴FPV