智能车学习（十三）——角度控制一、手册代码以及图示二、流程说明

一、手册代码以及图示

 

二、流程说明

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内进行陀螺仪零点比对