四旋翼姿态解算常用的两种算法-互补滤波和梯度下降算法

  上一篇讲了四元数、欧拉角和方向余弦的知识，不熟悉的请到这篇博客查看点击打开链接 

  介绍两种算法前，先定义两个坐标系。导航坐标系(参考坐标系)n，选取东北天右手直角坐标系作为导航坐标系n、载体坐标系(机体坐标系)b，选取右手直角坐标系定义：四轴向右为X正方向，向前为Y轴正方向，向上为Z轴正方向。

  互补滤波算法：通过加速度计的输出稳定性来修正陀螺仪的积分漂移误差。(既通过修正陀螺仪的测量角速度来实现)一句话就是利用加速度计来修正陀螺仪。

  取导航坐标系中的标准重力加速度，定义为，那么将转换至载体坐标系b中的表达式为

  定义载体坐标系b系中加速度计的输出为，由于重力加速度为标准重力加速度，所以这里要对加速度计输出归一化后才能继续运算，因此归一化后的载体坐标系加速度计输出为。

  对和做向量积运算，即可得到对陀螺仪的补偿校正误差

其中为向量积运算，坐标运算公式为，，

  采用PI控制器来消除误差

其中即为陀螺仪的校正补偿项，采用离散累加来计算。

将补偿到陀螺仪后，再由四元数微分方程计算出最后的姿态。下面给出互补滤波的算法原理框图

梯度下降法：梯度下降法的思想与互补滤波相同，都是用加速度计的稳定性来补偿陀螺仪的漂移。

不同点：梯度下降法是由加速度计计算出一组姿态四元数，再和陀螺仪计算出来的姿态四元数进行融合。互补滤波算法是利用中立即速度转换到b系后与归一化的b系加加速度计输出做向量积，求得陀螺仪的校正误差来校正陀螺仪。

因为在求解中用了梯度下降的原理，因此称为梯度下降算法。

  首选构造目标函数:

  对目标函数求偏导得到对应的雅可比矩阵:

  再由雅可比矩阵求出对应目标函数的梯度

 

 

  根据梯度下降法的定义有：

 

式中，为梯度下降的步长。然后对将陀螺仪计算出来的姿态和梯度下降法求出来的姿态进行融合，

其中为权重，并且满足。上述公式取得最优姿态解的条件为的收敛速度和的发散速度相等，因此可得，

式中，为系统的采样周期，为陀螺仪的测量误差，可以查询PDF得到。因为四旋翼在高速运行下，比较大，因此上式近似为，

由于比较大，则对由梯度下降计算公式来说，上一次的姿态可以忽略，直接由梯度负方向迭代到目标姿态，即可以重新定义为

 

由陀螺仪计算的姿态四元数公式为，

将、和公式代入

得到，

将上述公式简单定义为如下所示，

其中

因此可以得到梯度下降算法的姿态融合公式，

下面给出梯度下降的算法原理框图

这两种方法介绍完毕，都是自己的理解，欢迎大家交流指正。

由于梯度下降的代码，师兄已经在博客中贴出，这里我就不再给出，大家可以到这个博客中查看点击打开链接

互补滤波的代码给出如下，

  

	float recipNorm;					
	float halfvx, halfvy, halfvz;		
	float halfex, halfey, halfez;		
	float qa, qb, qc;
	float delta;						
	
	
	gx = gx * DEG2RAD;
	gy = gy * DEG2RAD;
	gz = gz * DEG2RAD;
	
	
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) 
	{
		
		arm_sqrt_f32(ax * ax + ay * ay + az * az, &recipNorm);
		ax /= recipNorm;
		ay /= recipNorm;
		az /= recipNorm; 

		
		halfvx = att.q[1] * att.q[3] - att.q[0] * att.q[2];
		halfvy = att.q[0] * att.q[1] + att.q[2] * att.q[3];
		halfvz = att.q[0] * att.q[0] - 0.5f + att.q[3] * att.q[3];
		
		
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		if(g_twoKi > 0.0f) 
		{
			g_integralFBx += g_twoKi * halfex * CNTLCYCLE;		
			g_integralFBy += g_twoKi * halfey * CNTLCYCLE;
			g_integralFBz += g_twoKi * halfez * CNTLCYCLE;
			gx += g_integralFBx;		
			gy += g_integralFBy;
			gz += g_integralFBz;
		}
		else	
		{
			g_integralFBx = 0.0f;
			g_integralFBy = 0.0f;
			g_integralFBz = 0.0f;
		}
		
		
		gx += g_twoKp * halfex;
		gy += g_twoKp * halfey;
		gz += g_twoKp * halfez;
	}
	/*
		
		* 
		* .	1      
		* q	= - * q x Omega    
		*	2
		*  .
		* [q0] [0  -wx	-wy	-wz] [q0]
		*  .				
		* [q1] [wx   0	 wz	-wy] [q1]
		*  .  =  				* 
		* [q2] [wy -wz	 0	 wx] [q2]
		*   . 			
		* [q3] [wz	wy	-wx  0 ] [q3]
	*/
	gx *= (0.5f * CNTLCYCLE);
	gy *= (0.5f * CNTLCYCLE);
	gz *= (0.5f * CNTLCYCLE);
	qa = att.q[0];
	qb = att.q[1];
	qc = att.q[2];
	
	delta = (CNTLCYCLE * gx) * (CNTLCYCLE * gx) + (CNTLCYCLE * gy) * (CNTLCYCLE * gy) + (CNTLCYCLE * gz) * (CNTLCYCLE * gz);
	att.q[0] = (1.0f - delta / 8.0f) * att.q[0] + (-qb * gx - qc * gy - att.q[3] * gz);
	att.q[1] = (1.0f - delta / 8.0f) * att.q[1] + ( qa * gx + qc * gz - att.q[3] * gy);
	att.q[2] = (1.0f - delta / 8.0f) * att.q[2] + ( qa * gy - qb * gz + att.q[3] * gx);
	att.q[3] = (1.0f - delta / 8.0f) * att.q[3] + ( qa * gz + qb * gy -   qc * gx);
	
	
	arm_sqrt_f32(att.q[0] * att.q[0] + att.q[1] * att.q[1] + att.q[2] * att.q[2] + att.q[3] * att.q[3], &recipNorm);
	att.q[0] /= recipNorm;
	att.q[1] /= recipNorm;
	att.q[2] /= recipNorm;
	att.q[3] /= recipNorm;
	
	
	att.angle[PITCH] = atan2(2.0f * att.q[2] * att.q[3] + 2.0f * att.q[0] * att.q[1], -2.0f * att.q[1] * att.q[1] - 2.0f * att.q[2]* att.q[2] + 1.0f) * RAD2DEG;	// Pitch
	att.angle[ROLL] = asin(-2.0f * att.q[1] * att.q[3] + 2.0f * att.q[0]* att.q[2]) * RAD2DEG;																		// Roll
	att.angle[YAW] = atan2(2.0f * att.q[1] * att.q[2] + 2.0f * att.q[0] * att.q[3], -2.0f * att.q[3] * att.q[3] - 2.0f * att.q[2] * att.q[2] + 1.0f) * RAD2DEG;		// Yaw