好難啊 姿態解算 算是解決了

在干什么

現在進行到四軸的大坑了。姿態解算和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);

}