知新_RL

这个屌丝很懒，什么也没留下！

热门标签

Mahony算法六轴姿态解算学习笔记（icm20602六轴姿态解算）

作者：知新_RL | 2024-05-15 21:24:30

踩

六轴姿态解算

最近在学四旋翼飞行器，所以开始学这些，看了好多文章终于算是通透了一点，感觉不写下来之后就忘了，大多是个人理解，有错误欢迎指出。本文不推导公式，姿态矩阵等，只讲用公式，推导过程见秦永元《惯性导航（第三版）》，这本书对捷联式惯导系统的讲解很清楚。

1.轴角与四元数

1.1轴角表示刚体旋转

先不引入xzy直角坐标系，轴角顾名思义就是绕一个单位轴旋转一个角度，以此表示机体旋转，即用一个单位向量和一个角度表示刚体旋转。

表示刚体旋转：设空间中有一刚体，刚体原始状态记为A，将其绕已知单位向量u旋转指定ɵ角后得到确定位置刚体，旋转后记为A’。

1.2四元数

暂且对四元数有个初步了解就行。

1.2.1四元数定义

四元数即为四个元构成的数：Q（q0，q1，q2，q3）=q0+q1i+q2j+q3k

式中q0,q1,q2,q3为实数，i，j，k为互相相交的单位向量，又是虚单位（-1）^0.5。

1.2.2四元数表达方法

只列举理解捷联式惯性系统有帮助的

①矢量式：Q=q0+q(q0为标量部分，q为矢量部分)

②复数式：Q=q0+q1i+q2j+q3k

③三角式：Q=cos(ɵ/2)+usin(ɵ/2) (三角式可以结合轴角理解)

④矩阵式：

$Q=\begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{bmatrix}$

1.2.3四元数计算

求解四元数微分方程需要，本文直接套公式，故不赘述。

四元数计算见秦永元《惯性导航（第三版）》9.2.1

1.3轴角到四元数

此时引入xyz直角坐标系，设地理坐标系为R系，刚体坐标系为b系(b系与刚体固联)。以轴角为基础构建四元数，将单位向量u在R系中表示为（l，m，n），现得到四个变量，用这四个变量（n,m,l,ɵ）构建四元数（q0,q1,q2,q3）：

$q_0=\cos\frac{\theta }{2}\\ \: q_1=l\sin\frac{\theta }{2}\\ \: q_2=m\sin\frac{\theta }{2}\\\: q_3=n\sin\frac{\theta }{2}$

1.4坐标变换矩阵

直接给结论

四元数可确定出b系至R系坐标变换矩阵：

$C_{b}^{R}=\begin{bmatrix} 1-2\left ( {q_2{}}^{2}+{q_3{}}^{2} \right ) &2(q_1q_2-q_0q_3) &2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & 1-2\left ( {q_1{}}^{2}+{q_3{}}^{2} \right ) &2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & 1-2\left ( {q_1{}}^{2}+{q_2{}}^{2} \right ) \end{bmatrix}$

具体推导见秦永元《惯性导航（第三版）》9.2.2

关于坐标变换矩阵具体干什么用的？怎么用？

先设地理坐标系为R系，刚体坐标系为b系(b系与刚体固联)，刚开始两坐标系重合，现在我们找一到重力向量（方向大小始终不变），在b系中（初始位置R系b系重合）记为g（x，y，z），然后旋转刚体，即b系旋转，向量g大小方向仍不变，g虽然不变，但是以b系为参照的话，向量g在b系xyz轴的投影改变了，即有g’（x’，y’，z’）。

用矩阵表示g和g’关系： $\begin{bmatrix} x\\ y\\ z \end{bmatrix}=C_{b}^{R}\begin{bmatrix} x'\\ y'\\ z' \end{bmatrix}$

所以根据旋转轴单位向量u与旋转角ɵ可得四元数，四元数代入坐标变换矩阵可得固定重力向量g在刚体旋转后新系b所对应坐标，只要对四元数进行更新，即可对刚体姿态更新。本质上是在求刚体坐标系b中g向量坐标，从而知道刚体现在旋转成什么样子了。

1.5*四元数微分方程

现在目标很明确了，想知道刚体姿态，知道此时的四元数就行，那怎么知道呢？求解四元数微分方程得到新的四元数。

一阶龙哥-古塔法：

那要怎么知道∆t内w的值？陀螺仪测量。

2.六轴传感器

2.1加速度计陀螺仪介绍

关于加速度计和陀螺仪，这篇文章写得很详细

https://blog.csdn.net/csshuke/article/details/80514201?spm=1001.2014.3001.5506

我用的icm20602，因为匿名飞控用的这个，这个比mpu6050好在哪里我也不太知道，等一个大佬解答。

2.2icm20602代码

直接从商家给的资料改的

头文件主要就是结构体的定义和一些宏定义

2.2.1icm20602.h


#ifndef _icm20602_h_
#define _icm20602_h_
 
#include "stm32f10x.h"
 
 
#define PI 3.1415926f
 
typedef struct //储存数据结构体
{
	float icm_gyro_x;
	float icm_gyro_y;
	float icm_gyro_z;
 
	float icm_acc_x;
	float icm_acc_y;
	float icm_acc_z;
}ICM20602_DataTypeDef;
 
typedef struct //储存adc数据结构体
{
	int16_t icm_gyroadc_x;
	int16_t icm_gyroadc_y;
	int16_t icm_gyroadc_z;
		 
	int16_t icm_accadc_x;
	int16_t icm_accadc_y;
	int16_t icm_accadc_z;
}ICM20602_ADCDataTypeDef;
 
 
//引脚定义
#define ICM20602_SCK_Pin GPIO_Pin_5
#define ICM20602_SCK_GPIO_Port GPIOA
 
#define ICM20602_MISO_Pin GPIO_Pin_6
#define ICM20602_MISO_GPIO_Port GPIOA
 
#define ICM20602_MOSI_Pin GPIO_Pin_7
#define ICM20602_MOSI_GPIO_Port GPIOA
 
#define ICM20602_CS_Pin GPIO_Pin_4
#define ICM20602_CS_GPIO_Port GPIOA
 
#define ICM20602_SCK(x)		GPIO_WriteBit (ICM20602_SCK_GPIO_Port ,ICM20602_SCK_Pin , (BitAction)x);
#define ICM20602_MOSI(x) 	GPIO_WriteBit (ICM20602_MOSI_GPIO_Port ,ICM20602_MOSI_Pin , (BitAction)x);
#define ICM20602_CSN(x)  	GPIO_WriteBit (ICM20602_CS_GPIO_Port ,ICM20602_CS_Pin , (BitAction)x);
#define ICM20602_MISO    	(uint8_t)GPIO_ReadInputDataBit(ICM20602_MISO_GPIO_Port ,ICM20602_MISO_Pin );
 
 
//操作指令
#define     ICM20602_DEV_ADDR           0x69 //SA0接地：0x68   SA0上拉：0x69  模块默认上拉
 
#define     ICM20602_SPI_W              0x00
#define     ICM20602_SPI_R              0x80
 
#define     ICM20602_XG_OFFS_TC_H       0x04
#define     ICM20602_XG_OFFS_TC_L       0x05
#define     ICM20602_YG_OFFS_TC_H       0x07
#define     ICM20602_YG_OFFS_TC_L       0x08
#define     ICM20602_ZG_OFFS_TC_H       0x0A
#define     ICM20602_ZG_OFFS_TC_L       0x0B
#define     ICM20602_SELF_TEST_X_ACCEL  0x0D
#define     ICM20602_SELF_TEST_Y_ACCEL  0x0E
#define     ICM20602_SELF_TEST_Z_ACCEL  0x0F
#define     ICM20602_XG_OFFS_USRH       0x13
#define     ICM20602_XG_OFFS_USRL       0x14
#define     ICM20602_YG_OFFS_USRH       0x15
#define     ICM20602_YG_OFFS_USRL       0x16
#define     ICM20602_ZG_OFFS_USRH       0x17
#define     ICM20602_ZG_OFFS_USRL       0x18
#define     ICM20602_SMPLRT_DIV         0x19
#define     ICM20602_CONFIG             0x1A
#define     ICM20602_GYRO_CONFIG        0x1B
#define     ICM20602_ACCEL_CONFIG       0x1C
#define     ICM20602_ACCEL_CONFIG_2     0x1D
#define     ICM20602_LP_MODE_CFG        0x1E
#define     ICM20602_ACCEL_WOM_X_THR    0x20
#define     ICM20602_ACCEL_WOM_Y_THR    0x21
#define     ICM20602_ACCEL_WOM_Z_THR    0x22
#define     ICM20602_FIFO_EN            0x23
#define     ICM20602_FSYNC_INT          0x36
#define     ICM20602_INT_PIN_CFG        0x37
#define     ICM20602_INT_ENABLE         0x38
#define     ICM20602_FIFO_WM_INT_STATUS 0x39 
#define     ICM20602_INT_STATUS         0x3A
#define     ICM20602_ACCEL_XOUT_H       0x3B
#define     ICM20602_ACCEL_XOUT_L       0x3C
#define     ICM20602_ACCEL_YOUT_H       0x3D
#define     ICM20602_ACCEL_YOUT_L       0x3E
#define     ICM20602_ACCEL_ZOUT_H       0x3F
#define     ICM20602_ACCEL_ZOUT_L       0x40
#define     ICM20602_TEMP_OUT_H         0x41
#define     ICM20602_TEMP_OUT_L         0x42
#define     ICM20602_GYRO_XOUT_H        0x43
#define     ICM20602_GYRO_XOUT_L        0x44
#define     ICM20602_GYRO_YOUT_H        0x45
#define     ICM20602_GYRO_YOUT_L        0x46
#define     ICM20602_GYRO_ZOUT_H        0x47
#define     ICM20602_GYRO_ZOUT_L        0x48
#define     ICM20602_SELF_TEST_X_GYRO   0x50
#define     ICM20602_SELF_TEST_Y_GYRO   0x51
#define     ICM20602_SELF_TEST_Z_GYRO   0x52
#define     ICM20602_FIFO_WM_TH1        0x60
#define     ICM20602_FIFO_WM_TH2        0x61
#define     ICM20602_SIGNAL_PATH_RESET  0x68
#define     ICM20602_ACCEL_INTEL_CTRL   0x69
#define     ICM20602_USER_CTRL          0x6A
#define     ICM20602_PWR_MGMT_1         0x6B
#define     ICM20602_PWR_MGMT_2         0x6C
#define     ICM20602_I2C_IF             0x70
#define     ICM20602_FIFO_COUNTH        0x72
#define     ICM20602_FIFO_COUNTL        0x73
#define     ICM20602_FIFO_R_W           0x74
#define     ICM20602_WHO_AM_I           0x75
#define     ICM20602_XA_OFFSET_H        0x77
#define     ICM20602_XA_OFFSET_L        0x78
#define     ICM20602_YA_OFFSET_H        0x7A
#define     ICM20602_YA_OFFSET_L        0x7B
#define     ICM20602_ZA_OFFSET_H        0x7D
#define     ICM20602_ZA_OFFSET_L        0x7E
 
 
void ICM20602_GPIO_Init(void);
uint8_t ICM20602_SPI_WriteByte(uint8_t byte); //SPI写一字节数据
void ICM20602_R_Reg_Byte(uint8_t cmd, uint8_t *val); //读指定寄存器值存储到指定地址
void ICM20602_W_Reg_Byte(uint8_t cmd, uint8_t data); //写指定寄存器数据
void ICM20602_R_Reg_Bytes(uint8_t cmd, uint8_t *val, uint8_t bytenum); //读多个字节数据
void ICM20602_Check(void); //检查设备
void ICM20602_Init(void);  //ICM20602初始化
void ICM20602_Get_AccData(ICM20602_ADCDataTypeDef *ICM_ADCData); //获取加速度计数据
void ICM20602_Get_GyroData(ICM20602_ADCDataTypeDef *ICM_ADCData); //获取陀螺仪数据
void ICM20602_Data_Change(ICM20602_DataTypeDef *ICM_Data,ICM20602_ADCDataTypeDef *ICM_ADCData); //数据转换
 
 
#endif

2.2.2icm20602.c

GPIO初始化


void ICM20602_GPIO_Init()
{
	GPIO_InitTypeDef GPIO_INITStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
	
	//片选初始化
	GPIO_INITStructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_INITStructure.GPIO_Pin=ICM20602_CS_Pin ;
	GPIO_INITStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(ICM20602_CS_GPIO_Port,&GPIO_INITStructure);
	
	//MISO初始化
	GPIO_INITStructure.GPIO_Mode=GPIO_Mode_IPU;
	GPIO_INITStructure.GPIO_Pin=ICM20602_MISO_Pin ;
	GPIO_INITStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(ICM20602_MISO_GPIO_Port,&GPIO_INITStructure);
	
	//MOSI初始化
	GPIO_INITStructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_INITStructure.GPIO_Pin=ICM20602_MOSI_Pin ;
	GPIO_INITStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(ICM20602_MOSI_GPIO_Port,&GPIO_INITStructure);
	
	//时钟线初始化
	GPIO_INITStructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_INITStructure.GPIO_Pin=ICM20602_SCK_Pin ;
	GPIO_INITStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(ICM20602_SCK_GPIO_Port,&GPIO_INITStructure);
}

软件SPI


uint8_t ICM20602_SPI_WriteByte(uint8_t byte) //SPI写一字节数据
{
	uint8_t i;
	for(i=0; i<8; i++)
	{
			ICM20602_MOSI((BitAction)(byte&0x80)); //写一位数据
			byte <<= 1;								
			ICM20602_SCK (0);    
			byte |= ICM20602_MISO;    //读一位数据    
			ICM20602_SCK (1);
	}	
	return byte;      //返回status寄存器值                                		
}
 
void ICM20602_R_Reg_Byte(uint8_t cmd, uint8_t *val) //读指定寄存器值存储到指定地址
{
	ICM20602_CSN (0);
	cmd |= ICM20602_SPI_R;
	ICM20602_SPI_WriteByte(cmd);   //发送                            	
	*val =  ICM20602_SPI_WriteByte(0);                           	
	ICM20602_CSN (1);                                    	
}
 
void ICM20602_W_Reg_Byte(uint8_t cmd, uint8_t data) //写指定寄存器数据
{
	ICM20602_CSN (0);
	cmd |= ICM20602_SPI_W;
	ICM20602_SPI_WriteByte(cmd);                      	
	ICM20602_SPI_WriteByte(data);                               	
	ICM20602_CSN (1);                                    	
}
 
void ICM20602_R_Reg_Bytes(uint8_t cmd, uint8_t *val, uint8_t bytenum) //读多个字节数据
{
	uint16_t i;
	ICM20602_CSN (0);
	cmd |= ICM20602_SPI_R;
	ICM20602_SPI_WriteByte(cmd);   
	for(i=0; i<bytenum; i++)	
			val[i] = ICM20602_SPI_WriteByte(0);                      	
	ICM20602_CSN (1);                                    		
}

设备初始化


void ICM20602_Check()
{
    uint8_t val;
    do
    {
        ICM20602_R_Reg_Byte(ICM20602_WHO_AM_I,&val);
        //卡在这里原因有以下几点
        //1 设备坏了，如果是新的这样的概率极低
        //2 接线错误或者没有接好
        //3 可能你需要外接上拉电阻，上拉到3.3V
    }while(0x12 != val);
}
 
void ICM20602_Init()
{
	uint8_t val;
    
	ICM20602_GPIO_Init(); //初始化对应GPIO
	
	ICM20602_Check();//检测
	
	ICM20602_W_Reg_Byte (ICM20602_PWR_MGMT_1,0x80);//复位设备
	Delay_ms(2);
	do
	{//等待复位成功
			ICM20602_R_Reg_Byte(ICM20602_PWR_MGMT_1,&val);
	}while(0x41 != val);
	
	ICM20602_W_Reg_Byte(ICM20602_PWR_MGMT_1,     0x01);            //时钟设置
	ICM20602_W_Reg_Byte(ICM20602_PWR_MGMT_2,     0x00);            //开启陀螺仪和加速度计
	ICM20602_W_Reg_Byte(ICM20602_CONFIG,         0x01);            //176HZ 1KHZ
	ICM20602_W_Reg_Byte(ICM20602_SMPLRT_DIV,     0x07);            //采样速率 SAMPLE_RATE = INTERNAL_SAMPLE_RATE / (1 + SMPLRT_DIV)
	ICM20602_W_Reg_Byte(ICM20602_GYRO_CONFIG,    0x18);            //±2000 dps
	ICM20602_W_Reg_Byte(ICM20602_ACCEL_CONFIG,   0x10);            //±8g
	ICM20602_W_Reg_Byte(ICM20602_ACCEL_CONFIG_2, 0x03);            //Average 8 samples   44.8HZ
  //ICM20602_GYRO_CONFIG寄存器
	//设置为:0x00 陀螺仪量程为:±250 dps     获取到的陀螺仪数据除以131           可以转化为带物理单位的数据， 单位为：°/s
	//设置为:0x08 陀螺仪量程为:±500 dps     获取到的陀螺仪数据除以65.5          可以转化为带物理单位的数据，单位为：°/s
	//设置为:0x10 陀螺仪量程为:±1000dps     获取到的陀螺仪数据除以32.8          可以转化为带物理单位的数据，单位为：°/s
	//设置为:0x18 陀螺仪量程为:±2000dps     获取到的陀螺仪数据除以16.4          可以转化为带物理单位的数据，单位为：°/s
 
	//ICM20602_ACCEL_CONFIG寄存器
	//设置为:0x00 加速度计量程为:±2g          获取到的加速度计数据 除以16384      可以转化为带物理单位的数据，单位：g(m/s^2)
	//设置为:0x08 加速度计量程为:±4g          获取到的加速度计数据 除以8192       可以转化为带物理单位的数据，单位：g(m/s^2)
	//设置为:0x10 加速度计量程为:±8g          获取到的加速度计数据 除以4096       可以转化为带物理单位的数据，单位：g(m/s^2)
	//设置为:0x18 加速度计量程为:±16g         获取到的加速度计数据 除以2048       可以转化为带物理单位的数据，单位：g(m/s^2)
}

获取六轴数据并转化


void ICM20602_Get_AccData(ICM20602_ADCDataTypeDef *ICM_ADCData) //获取加速度计数据
{
	uint8_t dat[6];
	
	ICM20602_R_Reg_Bytes(ICM20602_ACCEL_XOUT_H, dat, 6);
	ICM_ADCData->icm_accadc_x = (int16_t)(((uint16_t)dat[0]<<8 | dat[1]));
  ICM_ADCData->icm_accadc_y = (int16_t)(((uint16_t)dat[2]<<8 | dat[3]));
  ICM_ADCData->icm_accadc_z = (int16_t)(((uint16_t)dat[4]<<8 | dat[5]));
}
 
void ICM20602_Get_GyroData(ICM20602_ADCDataTypeDef *ICM_ADCData) //获取陀螺仪数据
{
	uint8_t dat[6];
	
	ICM20602_R_Reg_Bytes(ICM20602_GYRO_XOUT_H, dat, 6);
	ICM_ADCData->icm_gyroadc_x = (int16_t)(((uint16_t)dat[0]<<8 | dat[1]));
  ICM_ADCData->icm_gyroadc_y = (int16_t)(((uint16_t)dat[2]<<8 | dat[3]));
  ICM_ADCData->icm_gyroadc_z = (int16_t)(((uint16_t)dat[4]<<8 | dat[5]));
}
 
void ICM20602_Data_Change(ICM20602_DataTypeDef *ICM_Data,ICM20602_ADCDataTypeDef *ICM_ADCData) //数据转换
{
	//ICM20602_GYRO_CONFIG寄存器
	//设置为:0x00 陀螺仪量程为:±250 dps     获取到的陀螺仪数据除以131           可以转化为带物理单位的数据， 单位为：°/s
	//设置为:0x08 陀螺仪量程为:±500 dps     获取到的陀螺仪数据除以65.5          可以转化为带物理单位的数据，单位为：°/s
	//设置为:0x10 陀螺仪量程为:±1000dps     获取到的陀螺仪数据除以32.8          可以转化为带物理单位的数据，单位为：°/s
	//设置为:0x18 陀螺仪量程为:±2000dps     获取到的陀螺仪数据除以16.4          可以转化为带物理单位的数据，单位为：°/s
 
	//ICM20602_ACCEL_CONFIG寄存器
	//设置为:0x00 加速度计量程为:±2g          获取到的加速度计数据 除以16384      可以转化为带物理单位的数据，单位：g(m/s^2)
	//设置为:0x08 加速度计量程为:±4g          获取到的加速度计数据 除以8192       可以转化为带物理单位的数据，单位：g(m/s^2)
	//设置为:0x10 加速度计量程为:±8g          获取到的加速度计数据 除以4096       可以转化为带物理单位的数据，单位：g(m/s^2)
	//设置为:0x18 加速度计量程为:±16g         获取到的加速度计数据 除以2048       可以转化为带物理单位的数据，单位：g(m/s^2)
	ICM_Data->icm_gyro_x = (float)ICM_ADCData->icm_gyroadc_x / 131.0f;
	ICM_Data->icm_gyro_y = (float)ICM_ADCData->icm_gyroadc_y / 131.0f;
	ICM_Data->icm_gyro_z = (float)ICM_ADCData->icm_gyroadc_z / 131.0f;
 
	ICM_Data->icm_acc_x = (float)ICM_ADCData->icm_accadc_x/4096;
	ICM_Data->icm_acc_y = (float)ICM_ADCData->icm_accadc_y/4096;
	ICM_Data->icm_acc_z = (float)ICM_ADCData->icm_accadc_z/4096;
	
	//转化为弧度/秒
	ICM_Data->icm_gyro_x *= (PI / 180.0f);
	ICM_Data->icm_gyro_y *= (PI / 180.0f);
  ICM_Data->icm_gyro_z *= (PI / 180.0f);
}

明确一下我们要求的是重力向量g在b系中坐标表示。

加速度计测量的是三轴的重力加速度，它本质上是个比力计，如果只受重力作用，它当然能准确输出三轴重力加速度的分量，通过单位化得到重力向量g在b系中坐标表示，但是现实哪有那么理想。

根据上文公式，更新四元数只需要角速度，也就是只需要陀螺仪数据，但是众所周知陀螺仪它会漂移啊，并不能稳定地提供准确的数据。

因为陀螺仪和加速度计都不能稳定提供准确数据，故需要将陀螺仪与加速度计进行数据融合。

3.六轴数据融合

现在有的数据：加速度计数据，陀螺仪数据，当前四元数（初始值q0=1，q1=0，q2=0，q3=0）

要求的数据：新的四元数

说一下大体思路，就是分别单位化加速度计和陀螺仪数据，都分别转化成重力向量g在b系三轴分量，再求出两组数据的差，把差放到PI控制器里，输出一个补偿值补偿陀螺仪的值，最后用补偿后的陀螺仪值代入解四元数微分方程，得到新的四元数。

宏定义和全局变量


#define sampleFreq	1000.0f			//采样频率（Hz）
#define Kp	44.0f      //PI控制器
#define Ki	0.2f
#define PI 3.1415926f
 
static float intex, intey, intez;  //误差积分
static float q[4];       //四元数

更新四元数函数


void Imu_AHRSupdate(ICM20602_DataTypeDef *Imu_Data) //根据六轴传感器更新四元数
{
	float norm;
	float ax, ay, az; //由加速度计得到的实际重力加速度向量
	float gx, gy, gz; //陀螺仪数据
	float vx, vy, vz; //由四元数计算得到的理论重力加速度向量
	float ex, ey, ez; //理论与实际误差
	float q0, q1, q2, q3;
 
	gx = Imu_Data->icm_gyro_x;
	gy = Imu_Data->icm_gyro_y;
	gz = Imu_Data->icm_gyro_z;
	
	if((Imu_Data->icm_acc_x != 0.0f) && (Imu_Data->icm_acc_y != 0.0f) && (Imu_Data->icm_acc_z != 0.0f))  //加速度计数据有效时计算
	{
		//加速度计所得向量单位化
		norm = sqrt((Imu_Data->icm_acc_x) * (Imu_Data->icm_acc_x) 
		            + (Imu_Data->icm_acc_y) * (Imu_Data->icm_acc_y) 
								+ (Imu_Data->icm_acc_z) * (Imu_Data->icm_acc_z)); //平方根 
		ax = (Imu_Data->icm_acc_x) / norm;
		ay = (Imu_Data->icm_acc_y) / norm;
		az = (Imu_Data->icm_acc_z) / norm;
		
		//根据四元数计算理论重力加速度向量
		//公式即向量从机体坐标系到大地坐标系的姿态矩阵第三行（大地坐标系到机体坐标系的姿态矩阵第三列）
		vx = 2 * (q[1] * q[3] - q[2] * q[0]);
		vy = 2 * (q[2] * q[3] + q[1] * q[0]);
		vz = 2 * (q[0] * q[0] + q[3] * q[3]) - 1;
		
		//求差
		ex = ay * vz - az * vy;
		ey = az * vx - ax * vz;
		ez = ax * vy - ay * vx;
		
		//求误差积分
		intex += (ex * Ki * (1.0f / sampleFreq));
		intey += (ey * Ki * (1.0f / sampleFreq));
		intez += (ez * Ki * (1.0f / sampleFreq));
		
		//PI控制器补偿陀螺仪
		gx += (intex + Kp * ex);
		gy += (intey + Kp * ey);
		gz += (intez + Kp * ez);
		
		//清空加速度计数据，便于判断下次数据是否有效
		Imu_Data->icm_acc_x = 0.0f;
		Imu_Data->icm_acc_y = 0.0f;
		Imu_Data->icm_acc_z = 0.0f;
	}
	
	q0 = q[0];
	q1 = q[1];
	q2 = q[2];
	q3 = q[3];
	//一阶龙格库塔法更新四元数
	q[0] -= ((q1 * gx + q2 * gy + q3 * gz) * (1.0f / sampleFreq) * 0.5);
	q[1] += ((q0 * gx + q2 * gz - q3 * gy) * (1.0f / sampleFreq) * 0.5);
	q[2] += ((q0 * gy - q1 * gz + q3 * gx) * (1.0f / sampleFreq) * 0.5);
	q[3] += ((q0 * gz + q1 * gy - q2 * gx) * (1.0f / sampleFreq) * 0.5);
	
	//单位化四元数
	norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
	q[0] /= norm;
	q[1] /= norm;
	q[2] /= norm;
	q[3] /= norm;
}

本人才疏学浅，还请雅正。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/知新_RL/article/detail/575065