使用四元数计算俯仰角和横滚角

系列文章目录

---

前言

本文主要是将采集到的ICM20602的加速度计和陀螺仪原始数据，通过四元数法进行姿态解算，得到相对准确的俯仰角和横滚角数据

---

一、原始数据

ICM20602传感器和MPU6050传感器的数据几乎相同都位于0x3B到0x48这14个字节的寄存器中，每两个寄存器代表加速度计或者陀螺仪一个轴的数据，每个数据都是两个字节。

因为每两个寄存器代表一个轴的加速度计或者陀螺仪数据，所以使用IIC采集到原始数据后需要对数据进行整理，下面是从ICM20602的0x3B寄存器读取原始数据进行整理的代码。

    acc[0] = (int16)((datared[0]<<8) | datared[1]);  //加速度计的X轴数据ACC_X
	acc[1] = (int16)((datared[2]<<8) | datared[3]);  //加速度计的Y轴数据ACC_Y
	acc[2] = (int16)((datared[4]<<8) | datared[5]);  //加速度计的Z轴数据ACC_Z   
	gyr[0] = (int16)((datared[8]<<8) | datared[9] ); //角速度计的X轴数据GYR_X 
	gyr[1] = (int16)((datared[10]<<8)| datared[11]); //角速度计的Y轴数据GYR_Y 
	gyr[2] = (int16)((datared[12]<<8)| datared[13]); //角速度计的Z轴数据GYR_Z  

二、四元数法计算角度

1.四元数代码

下面首先介绍原始数据要代入的四元数代码

//------------------计算角度-----------------------

#define Pi 3.14159265f
#define Kp 0.8f                       // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.001f                // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.05f                // 采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差
 
float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滚角


 
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
    float norm;
	float vx, vy, vz;
	float ex, ey, ez;

	norm = sqrt(ax*ax + ay*ay + az*az);	//把加速度计的三维向量转成单维向量   
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;

		//	下面是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。 
		//	根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，正好是这三个元素
		//	所以这里的vx vy vz，其实就是当前的欧拉角（即四元数）的机体坐标参照系上，换算出来的
		//	重力单位向量。
	vx = 2*(q1*q3 - q0*q2);
	vy = 2*(q0*q1 + q2*q3);
	vz = q0*q0 - q1*q1 - q2*q2 + q3*q3 ;

	ex = (ay*vz - az*vy) ;
	ey = (az*vx - ax*vz) ;
	ez = (ax*vy - ay*vx) ;

	exInt = exInt + ex * Ki;
	eyInt = eyInt + ey * Ki;
	ezInt = ezInt + ez * Ki;

	gx = gx + Kp*ex + exInt;
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*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 = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;

	Pitch = asin(2*(q0*q2 - q1*q3 )) * 57.2957795f; // 俯仰   换算成度
	Roll = asin(2*(q0*q1 + q2*q3 )) * 57.2957795f; // 横滚
}

上面就是ICM20602原始数据要代入的四元数姿态解算代码，如果没有特殊情况，这个代码不需要改变参数。
但是特别要注意的就是，里面的halfT这个宏定义需要改为你自身采集数据的时间周期。

我运行代码时从串口助手上看到1s中采集了10组ICM20602传感器数据，所以采样频率是dt = 1/10，而halfT这个变量代表1/2*dt = 0.05，所以代码里定义为#define halfT 0.05f 。

这个数据一定要改好，我运行代码的时候，就是一开始这个时间不对，所以计算出来的俯仰角、横滚角一直在跳变。

2.计算角度

现在有了原始数据，有了四元数代码，只要将原始数据代入四元数函数代码中即可，但在原始数据代入四元数函数代码之前，还需要对原始数据进行处理。

代码如下（示例）：

void Compute_Angle(void)
{
  Angle_ax = acc[0]/8192.0;
  Angle_ay = acc[1]/8192.0;
  Angle_az = acc[2]/8192.0;
  
  Angle_gx = gyr[0]/131;
  Angle_gy = gyr[1]/131;
  Angle_gz = gyr[2]/131;

  IMUupdate(Angle_gx/57.3,Angle_gy/57.3,Angle_gz/57.3,Angle_ax,Angle_ay,Angle_az);
 
}

上面的数据可以看到加速度计的数据除以了8192，陀螺仪的数据除以了131。

首先分析加速度计的数据，加速度计除以8192是因为，我在使用IIC采集ICM20602传感器的原始数据时，对ICM20602传感器进行了初始化。

void Sensor_Init(void)
{ 
	writeRegister(0x1B,0x00);     //陀螺仪250dps
	writeRegister(0x1C,0x08);    //加速度计±4g
	writeRegister(0x6B,0x00);
	writeRegister(0x6C,0x00);
}

<hr style=" border:solid; width:100px; height:1px;" color=#000000 size=1">

可以看到，我将加速度计的量程设置为了±4g，下面是从数据手册中截取的图形，从图中可以看到加速度计量程±4g对应的精度是8192。

我们从数据寄存器采集到的数据是16位的，而且最高位是符号位，就是说ADC读取的值的范围是±32768，对应±4g，那么1g对应的ADC值就是32768/4 = 8192。

代码中将原始加速度数据除以8192，就相当于把单位换算成了g，因为1g就等于8192。

使用时将8192修改为自己初始化的加速度计量程对应的精度即可。

同理陀螺仪也是如此，将单位换算为了度。

陀螺仪的数据本应该减去偏移值再除以131的，偏移值就是静止时陀螺仪本该为0却不为零的数据。

但因为我采集到的原始数据在静止不动时，数据值都接近0，所以就没有对陀螺仪数据进行校准，没有减去偏移值。

在代入IMUupdate（）函数时，陀螺仪的数据又除以了57.3，这是将陀螺仪数据转换为度后又转为弧度，就是将陀螺仪弧度的数据代入了函数。

总结

以上就是今天要讲的内容，本文仅仅简单介绍了采集到陀螺仪和加速度的原始数据后，将数据代入四元数算法的代码中得到俯仰角和横滚角的过程。

因自身能力有限，若有问题欢迎指出。