好难啊 姿态解算 算是解决了

在干什么

现在进行到四轴的大坑了。姿态解算和pid控制了。好难，看都看不懂。看别人做四轴的文章到了这里也是言辞闪烁的，我也要闪烁一下。

姿态解算 参考

在姿态解算之前，要先收集完毕陀螺仪数据（转化为弧度每秒）、和加速度数据（m/s*s）。因为我手头里没有磁力计，所以没有用磁力计去修正陀螺仪数据

// 四元数变量,全局变量，处于不断更新的状态
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float exInt = 0, eyInt = 0, ezInt = 0;  //误差积分
float q0q0, q0q1, q0q2, q0q3;
float q1q1, q1q2, q1q3;
float q2q2, q2q3;
float q3q3;


void Angle_Calculate(float gx, float gy, float gz, float ax, float ay, float az){
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;//误差
	
	
	if((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))return;		//如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。

	norm = Q_rsqrt(ax*ax + ay*ay + az*az);       //把加速度计的数据进行归一化处理 使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax，ay，az的运算速度更快。通过归一化处理后，ax，ay，az的数值范围变成-1到+1之间。
	ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
	
	// 先把这些用得到的值算好，避免重复运算加大计算量
    q0q0 = q0*q0;
    q0q1 = q0*q1;
    q0q2 = q0*q2;
    q0q3 = q0*q3;
    q1q1 = q1*q1;
    q1q2 = q1*q2;
    q1q3 = q1*q3;
    q2q2 = q2*q2;
    q2q3 = q2*q3;
    q3q3 = q3*q3;
	
	// 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正。
    vx = 2*(q1q3 - q0q2);								//四元素中xyz的表示
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;
	
	//使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
	ex = (ay*vz - az*vy);// + (my*wz - mz*wy);                         	//向量外积在相减得到差分就是误差
	ey = (az*vx - ax*vz);// + (mz*wx - mx*wz);
	ez = (ax*vy - ay*vx);// + (mx*wy - my*wx);
	
	//把上述计算得到的重力差进行积分运算，积分的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。积分系数是Ki，如果Ki参数设置为0，则忽略积分运算。	
	exInt = exInt + Ki * VariableParameter(ex) * ex;								  //对误差进行积分
    eyInt = eyInt + Ki * VariableParameter(ey) * ey;
    ezInt = ezInt + Ki * VariableParameter(ez) * ez;
	
	
	//把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。
    gx = gx + Kp * VariableParameter(ex) * ex + exInt;	
	gy = gy + Kp * VariableParameter(ey) * ey + eyInt;	
	gz = gz + Kp * VariableParameter(ez) * ez + ezInt;	 
	
    //把由加速计修正过后的陀螺仪数据整合到四元数中。
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
	
	//运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
	norm = Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;
	
	EulerAngle.Pitch = asin(-2*q1q3 + 2*q0q2) * RtA; // pitch
	EulerAngle.Roll = atan2(2*q2q3 + 2*q0q1, -2*q1q1 - 2*q2q2 + 1) *RtA; // roll
	EulerAngle.Yaw = atan2(2*q1q2 + 2*q0q3, -2*q2q2 - 2*q3q3 +1) *RtA;	// yaw

}

2018年8月9日更新

我不知道什么缘故，数据很飘。然后不知道怎么改了点代码，现在解算出来的姿态角没有上次那么飘了，摆动幅度在0.1度（只看了yaw角）范围内。
下面说一下我的程序的步骤
姿态解算的频率是10Hz
姿态解算之前要收集加速度计数据，并转化为10进制的米每二次方秒，收集陀螺仪数据，并转化为10进制的弧度每秒。使用加速度计修正陀螺仪数据（考虑以后加入磁力计提供一个正北的参考方向，修正陀螺仪数据）。因为解算频率是10Hz，所以T=0.1，halfT=0.05

// 四元数变量,全局变量，处于不断更新的状态
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float exInt = 0, eyInt = 0, ezInt = 0;  //误差积分
float q0q0, q0q1, q0q2, q0q3;
float q1q1, q1q2, q1q3;
float q2q2, q2q3;
float q3q3;


void Angle_Calculate(float gx, float gy, float gz, float ax, float ay, float az){
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;//误差
	/*
	printf("EulerAngle.gx:%3.4f\n",gx);
	printf("EulerAngle.gy:%3.4f\n",gy);
	printf("EulerAngle.gz:%3.4f\n",gz);
	printf("EulerAngle.ax:%3.4f\n",ax);
	printf("EulerAngle.ay:%3.4f\n",ay);
	printf("EulerAngle.az:%3.4f\n",az);
	*/
	if((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))return;		//如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。

	norm = Q_rsqrt(ax*ax + ay*ay + az*az);       //把加速度计的数据进行归一化处理 使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax，ay，az的运算速度更快。通过归一化处理后，ax，ay，az的数值范围变成-1到+1之间。
	ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
	
	// 先把这些值算好，避免重复运算加大计算量
    q0q0 = q0*q0;
    q0q1 = q0*q1;
    q0q2 = q0*q2;
    q0q3 = q0*q3;
    q1q1 = q1*q1;
    q1q2 = q1*q2;
    q1q3 = q1*q3;
    q2q2 = q2*q2;
    q2q3 = q2*q3;
    q3q3 = q3*q3;
	
	// 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正。
    vx = 2*(q1q3 - q0q2);								//四元素中xyz的表示
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3 ;
	
	//使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
	ex = (ay*vz - az*vy);// + (my*wz - mz*wy);                         	//向量外积在相减得到差分就是误差
	ey = (az*vx - ax*vz);// + (mz*wx - mx*wz);
	ez = (ax*vy - ay*vx);// + (mx*wy - my*wx);
	
	//把上述计算得到的重力差进行积分运算，积分的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。积分系数是Ki，如果Ki参数设置为0，则忽略积分运算。	
	exInt = exInt + Ki * VariableParameter(ex) * ex;								  //对误差进行积分
    eyInt = eyInt + Ki * VariableParameter(ey) * ey;
    ezInt = ezInt + Ki * VariableParameter(ez) * ez;
	
	
	//把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。
    gx = gx + Kp * VariableParameter(ex) * ex + exInt;	
	gy = gy + Kp * VariableParameter(ey) * ey + eyInt;	
	gz = gz + Kp * VariableParameter(ez) * ez + ezInt;	 
	
    //把由加速计修正过后的陀螺仪数据整合到四元数中。
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
	
	//运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
	norm = Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;
	
	EulerAngle.Pitch = asin(-2*q1q3 + 2*q0q2) * RtA; // pitch
	EulerAngle.Roll = atan2(2*q2q3 + 2*q0q1, -2*q1q1 - 2*q2q2 + 1) *RtA; // roll
	EulerAngle.Yaw = atan2(2*q1q2 + 2*q0q3, -2*q2q2 - 2*q3q3 +1) *RtA;	// yaw
	
	printf("%3.4f,%3.4f,%3.4f\n",EulerAngle.Pitch,EulerAngle.Roll,EulerAngle.Yaw);
	//printf("EulerAngle.Roll:%3.4f\n",EulerAngle.Roll);
	//printf("EulerAngle.Yaw:%3.4f\n",EulerAngle.Yaw);

}