作为无人机方面做嵌入式编写的飞控总结6--IMU惯性系统和GPS导航系统融合小结1（惯性导航算法）

导：IMU即惯性测量单元，区别于以下几个概念

     （1） AHRS(9轴<--算法-->3姿态角) Attitude   and Heading Reference System

称为 航姿参考系统 包括多个轴向传感器，能够为飞行器提供航向，横滚和侧翻信息(PITCHA,ROLL,YAW)，这类系统用来为飞行器提供准确可靠的姿态与航行信息。 航姿参考系统 包括基于MEMS的 三轴陀螺仪，加速度计和磁强计 。

航姿参考系统与惯性测量单元IMU的区别在于: (Inertial   Measurement   Unit)

航姿参考系统（AHRS）包含了嵌入式的姿 态数据解算单元与航向信息 ;

惯性测量单元（IMU）仅仅提供 传感器数据 ，并不具有提供准确可靠的姿态数据的功能。

目前常用的航姿参考系统（AHRS）内部采用的多传感器数据融合进行的航姿解算单元为卡尔曼滤波器(KF or EKF)。

（2）惯性导航系统（INS）： Inertial   Navigation System是一个使用 加速计 和 陀螺仪来测量物体的 加速度和 角速度，并用 计算机来连续估算运动物体 位置、姿态和速度的辅助 导航系统。

惯 性导航是使用装载在运载体上的陀螺仪和加速度计来测定运载体姿态、 速度、 位置等信 息的技术方法。实现惯性导航的软、硬件设备称为 惯性导航系统 ，简称惯导系统

1.关于简单的姿态结算算法（适合于低慢小型型系统），可以参考博客：大家多看资料，相互印证！！！

    有视频总结姿态结算的几个步骤（比较简化）：https://blog.csdn.net/xiaoxilang/article/details/80326013

为何简单？因为这只是单纯的惯性系统（有缺陷），以下将总结惯性导航系统（惯性-惯性传感器，导航-gps信息）的估计流程

1.捷联式惯性导航算法

               2.组合导航算法

2.惯性导航算法

(1)捷联惯性导航系统（SINS）的导航解算流程如图所示。在程序初始化部分，主要是获得SINS的初始姿态阵 、初始位置矩阵 以及初值四元数 ；并读取SINS数据更新频率等SINS的工作参数

 (2)2个传感器，2个矩阵

` 

（3）加速度，陀螺仪，磁力计的作用分布图

(4)由姿态矩阵推导姿态角

因为俯仰角θ定义在[-90°，+90°]区间，与反正弦函数主值一致，因此不存在多值得问题。横滚角γ定义在[-180°,+180°]和航向角ψ[0°,360°]的定义区间与反正切函数的主值不一致，这时候需要根据 中元素其他值辅助判断，才确定横滚角和航向角的真值。判断依据下表。

 

见代码分析如下

void qZitai_all(void)
{
	//float x,y,z,t11,t12,t21,t22,t31,t32,t33;
	t11 = Tbn[0][0];
	t12 = Tbn[0][1];
	t21 = Tbn[1][0];
	t22 = Tbn[1][1]; 
	t31 = Tbn[2][0];	 
	t32 = Tbn[2][1];  
	t33 = Tbn[2][2];
	if (fabsf(t32)<(1-RES))          //1
	{
		x=atan(t32/__sqrtf(1-t32*t32));  
		zitai[1]=x;
		if(fabsf(t22)>RES)             //2
		{
			y=atan(t12/t22);
			if(t22>0)               //3
			{
				if(fabsf(t12)>RES)    //4
				{
					if(t12>0)      //5
					{
						zitai[0]=y;
					}
					else
					{
						zitai[0]=y+2.0f*pi;
					}               //5
				}
				else
				{
					zitai[0]=0;
				}                   //4
			}
			else
			{
				zitai[0]=y+pi;
			}                       //3
		}
		else
		{
			if(-t12>0)             //6
			{
				zitai[0]=pi*0.5f;
			}
			else
			{
				zitai[0]=3*pi*0.5f;
			}                       //6
		}                          //2
		if (fabsf(t33)>RES)         //7
		{
			z=atan(-t31/t33);
			if(t33>0)              //8
			{
				zitai[2]=z;
			}
			else
			{
				if(fabsf(t31)>RES)  //9
				{
					if(-t31<0)    //10
					{
						zitai[2]=z-pi;
					}
					else
					{
						zitai[2]=z+pi;
					}            //10
				}
				else
				{
					zitai[2]=pi;
				}  //9
			}     //8
		}
		else
		{
			if(-t31>0)         //11
			{
				zitai[2]=pi*0.5f;
			}
			else
			{
				zitai[2]=-pi*0.5f;
			}//11
		}//7
	}
	else
	{
		if(t32>0) //12
		{
			zitai[1]=pi*0.5f;
		}
		else
		{
			zitai[1]=-pi*0.5f;
		}//12
		zitai[2]=0.0f;
		if(fabsf(t11)>RES)  //13
		{
			y=atan(t21/t11);
			if(t11>0)   //14
			{
				if(fabsf(t21)>RES)  //15
				{
					if(t21>0)    //16
					{
						zitai[0]=y;					
					}
					else
					{
						zitai[0]=y+2.0f*pi;	
					}          //16
				}
				else
				{
					zitai[0]=0.0f;	
				}   //15
			}
			else
			{
				zitai[0]=y+pi;	
			}  //14
		}
		else
		{
			if(t21>0)  //17
			{
				zitai[0]=pi*0.5f;	
			}
			else
			{
				zitai[0]=3*pi*0.5f;	
			}  //17
		} //13
	}//1 
}

3 导航数据（速度 位置）计算流程