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

2023-05-16

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

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

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

取导航坐标系中的标准重力加速度 $g$ ，定义为 $g^{n}=\begin{bmatrix} 0 & 0 & 1 \end{bmatrix}^{T}$ ，那么将 $g^{n}$ 转换至载体坐标系b中的表达式为 $g^{b}=C_{n}^{b}g^{n}$

$g^{b}=C_{n}^{b}g^{n}=\begin{bmatrix} 2(q_{1}q_{3}-q_{0}q_{2})\\ 2(q_{2}q_{3}+q_{0}q_{1})\\ q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{bmatrix}$

定义载体坐标系b系中加速度计的输出为 $a$ ，由于重力加速度为标准重力加速度，所以这里要对加速度计输出归一化后才能继续运算，因此归一化后的载体坐标系加速度计输出为 $a^{b}=\begin{bmatrix} a_{x}/\left \| a \right \| &a_{y}/\left \| a \right \| &a_{z}/\left \| a \right \| \end{bmatrix}$ 。

对 $a^{b}$ 和 $g^{b}$ 做向量积运算，即可得到对陀螺仪的补偿校正误差 $e(\omega )$

$e(\omega )=g^{b}\times a^{b}$

其中 $\times$ 为向量积运算，坐标运算公式为 $\vec{a}=(a_{x},a_{y},a_{z})$ ， $\vec{b}=(b_{x},b_{y},b_{z})$ ，

$\vec{a}\times \vec{b}=(a_{y}-b_{z})\vec{i}+(a_{z}b_{x}-a_{x}b_{z})\vec{j}+(a_{x}b_{y}-a_{y}b_{x})\vec{k}$

采用PI控制器来消除误差

$e^{\ast }(\omega )=K_{p}e\left ( \omega \right )+K_{i}\int_{0}^{t}e\left ( \omega \right )dt$

其中 $e^{\ast }(\omega )$ 即为陀螺仪的校正补偿项， $K_{i}\int_{0}^{t}e\left ( \omega \right )dt$ 采用离散累加来计算。

将 $e^{\ast }(\omega )$ 补偿到陀螺仪后，再由四元数微分方程计算出最后的姿态。下面给出互补滤波的算法原理框图

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

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

因为在求解 $q_{\bigtriangledown }$ 中用了梯度下降的原理，因此称为梯度下降算法。

首选构造目标函数 $f(g^{n},a^{b})$ :

$f(g^{n},a^{b})=C_{n}^{b}g^{n}-a^{b}=\begin{bmatrix} 2(q_{1}q_{3}-q_{0}q_{2})\\ 2(q_{2}q_{3}+q_{0}q_{1})\\ q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{bmatrix}$

对目标函数求偏导得到对应的雅可比矩阵 $J\left ( q \right )$ :

$J\left ( q \right )=\frac{\alpha f(g^{n},a^{b})}{\alpha q}=\begin{bmatrix} -2q_{2} &2q_{3} &-2q_{0} &2q_{1} \\ 2q_{1} &2q_{0} &2q_{3} &2q_{2} \\ 2q_{0}&-2q_{1} &-2q_{2} &2q_{3} \end{bmatrix}$

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

$\bigtriangledown f(g^{n},a^{b})=J^{T}\left ( q \right )f(g^{n},a^{b})=\begin{bmatrix} \4q_{0}q_{2}^{2}+2q_{2}a_{x}+4q_{0}q_{1}^{2}-2q_{1}a_{y}\\ 4q_{1}q_{3}^{2}-2q_{3}a_{x}+4q_{1}q_{0}^{2}-2q_{0}a_{y}-4q_{1}+8q_{1}^{3}+8q_{1}q_{2}^{2}+4q_{1}a_{z}\\ 4q_{2}q_{0}^{2}+2q_{0}a_{x}+4q_{2}q_{3}^{2}-2q_{3}a_{y}-4q_{2}+8q_{2}q_{1}^{2}+8q_{2}^{3}+4q_{2}a_{z}\\ 4q_{3}q_{1}^{2}-2q_{1}a_{x}+4q_{3}a_{x}+4q_{3}q_{2}^{2}-2q_{2}a_{y} \end{bmatrix}$

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

$q_{\bigtriangledown ,k}=q_{\bigtriangledown ,k-1}-\mu \frac{\bigtriangledown f\left ( g^{n},a^{b} \right )}{\left \|\bigtriangledown f\left ( g^{n},a^{b} \right ) \right \|}$

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

$q_{est,k}=\chi q_{\bigtriangledown ,k}+(1-\chi )q_{\omega ,k}$

其中 $\chi$ 为权重，并且满足 $0< \chi < 1$ 。上述公式取得最优姿态解的条件为 $q_{\bigtriangledown ,k}$ 的收敛速度和 $q_{\omega ,k}$ 的发散速度相等，因此可得，

$(1-\chi )\beta =\chi \mu /T_{s}$

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

$\chi \approx \beta T_{s}/\mu \approx 0$

由于 $\mu$ 比较大，则对由梯度下降计算公式 $q_{\bigtriangledown ,k}=q_{\bigtriangledown ,k-1}-\mu \frac{\bigtriangledown f\left ( g^{n},a^{b} \right )}{\left \|\bigtriangledown f\left ( g^{n},a^{b} \right ) \right \|}$ 来说，上一次的姿态可以忽略，直接由梯度负方向迭代到目标姿态，即可以重新定义为

$q_{\bigtriangledown ,k}=-\mu \frac{\bigtriangledown f\left ( g^{n},a^{b} \right )}{\left \|\bigtriangledown f\left ( g^{n},a^{b} \right ) \right \|}$

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

$q_{\omega ,k}=q_{est,k-1}+\dot{q}_{\omega ,k}T_{s}$

将 $\chi \approx \beta T_{s}/\mu \approx 0$ 、 $q_{\bigtriangledown ,k}=-\mu \frac{\bigtriangledown f\left ( g^{n},a^{b} \right )}{\left \|\bigtriangledown f\left ( g^{n},a^{b} \right ) \right \|}$ 和 $q_{\omega ,k}=q_{est,k-1}+\dot{q}_{\omega ,k}T_{s}$ 公式代入

$q_{est,k}=\chi q_{\bigtriangledown ,k}+(1-\chi )q_{\omega ,k}$

得到，

$q_{est,k}=\frac{\beta T_{s}}{\mu }\left ( -\mu \frac{\bigtriangledown f\left ( g^{n},a^{b} \right )}{\left \| \bigtriangledown f\left ( g^{n},a^{b} \right ) \right \|} \right )+(1-0)(q_{est,k-1}+\dot{q}_{w,k}T_{s})$

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

$q_{est,k}=q_{est,k-1}+\dot{q}_{est,k}T_{s}$

其中 $\dot{q}_{est,k}=\dot{q}_{\omega ,k}-\mu \frac{\bigtriangledown f\left ( g^{n},a^{b} \right )}{\left \| \bigtriangledown f\left ( g^{n},a^{b} \right ) \right \|}$

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

$q_{est,k}=q_{est,k-1}+(\dot{q}_{\omega ,k}- \mu \frac{\bigtriangledown f(g^{n},a^{b})}{\left \| \bigtriangledown f(g^{n},a^{b}) \right \|})T_{s}$

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

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

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

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

	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

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)