智能车学习(十三)——角度控制

时间:2024-01-22 08:49:44

一、手册代码以及图示

智能车学习(十三)——角度控制

二、流程说明

1、角度计算函数说明

//============================================================================
//函数名称:void AngleCalculate(void)
//函数返回:无
//参数说明:无
//功能概要:车身角度计算。
//============================================================================
void AngleCalculate(void)
{
//读取加速度计
MMA8451_Read(); //读取陀螺仪
LS3G4200D_Read(); //取陀螺仪的X轴数据
VOLTAGE_GYRO = Gyro_X; //取加速度读计的Y轴数据
VOLTAGE_GRAVITY = Gray_Y; //陀螺仪零偏跟随
Gyro_Offset_Calculate(); if(VOLTAGE_GRAVITY > 1050) //限制加速度计读数-1050 ~1050
{
VOLTAGE_GRAVITY = 1050;
} if(VOLTAGE_GRAVITY <= -1050)
{
VOLTAGE_GRAVITY = - 1050;
} //加速度计读取的Y轴数值转换为角度
g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO; //陀螺仪提取角速度
g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO; //车身角度计算
g_fCarAngle = g_fGyroscopeAngleIntegral;
fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
}

首先是通过加速度计测得重力加速度,然后通过归一化(因为g和角度成正比),计算出角度。然后由于车子运动过程中,车子加速度会产生一定的影响,导致不是正比关系,所以使用陀螺仪进行修正,陀螺仪测得是角速度,积分之后就是角度,然后再和加速度计测得角度进行融合,然后反馈回去,使得系统平衡。

2、陀螺仪零点跟随

/============================================================================
//函数名称:void Gyro_Offset_Calculate()
//函数返回:无
//参数说明:无
//功能概要://陀螺仪三个轴的零点跟随 Cut_Gyro_X为经过处理后的值, Gyro_X为实际的读值,Gyro_X_Offset 为零点
//============================================================================
void Gyro_Offset_Calculate()
{
float Cut_Gyro_X; //减去偏移值得到的实时值
Cut_Gyro_X = Gyro_X - GYROSCOPE_OFFSET; //根据实时值,修改零偏置
if(Cut_Gyro_X>=2)
{
GYROSCOPE_OFFSET += 0.001;
}
else if(Cut_Gyro_X<=-2)
{
GYROSCOPE_OFFSET -= 0.001;
}
else
{
GYROSCOPE_OFFSET = GYROSCOPE_OFFSET * 0.99 + 0.01 * Gyro_X;
}
  //开机2秒进行跟随
if (Clocks_Gyro <= 400)
{
GYROSCOPE_OFFSET = GYROSCOPE_OFFSET * 0.5 + 0.5 * Gyro_X;
} }

根据实时值,修改零偏值。

3、其中有一些参数设置:

/***************直立控制*****************/

float g_fGravityAngle = 0;                                              //加速度计转换来的角度
float g_fGyroscopeAngleSpeed = 0; //陀螺仪转换来的角速度
float g_fCarAngle = 0; //车身角度 float VOLTAGE_GYRO = 0; //定义陀螺仪(选X,Y,Z)
float VOLTAGE_GRAVITY = 0; //定义加速度计(选X,Y,Z) float GRAVITY_OFFSET = 27; //加速度计零偏
float GRAVITY_ANGLE_RATIO = (90 /(1050.0 - 27)) ; //加速度计归一化因子 float GYROSCOPE_OFFSET = 0 ; //陀螺仪零偏移
float GYROSCOPE_ANGLE_RATIO = 1.780; //归一化因子 float GRAVITY_ADJUST_TIME_CONSTANT = 2; //陀螺仪加速度计加和的比例因子
float GYROSCOPE_ANGLE_SIGMA_FREQUENCY = 200; //积分的频率 float Clocks_Gyro = 0; //开机2s内进行陀螺仪零点比对