直立两轮平衡车核心代码

---

两轮平衡车相关算法

---

---

卡尔曼滤波函数

/**************************************************************
函数名称:float KalmanFilter (float ACC_Angle, float GYRO_RealY) 
函数功能:加速度计和陀螺仪角度融合(卡称曼滤波)
输入参数: ACC_ Angle ( 加速度计计算出的角度)
GYRO RealY (陀螺仪测量的角速度)
输出参数: Attitude_ Angle_ Y融合角度
**************************************************************/
//卡尔曼滤波的部分参数
#define Peried    1/500.0f   //此参数基本不改变
#define Q 20.2f				 //增加Q，可以增加高频响应，但会影响融合曲线的平滑性  
#define R 35.999f			 //
float KalmanGain =1.0f;	     //

float KalmanFilter(float ACC_Angle,float GYRO_RealY)
{
//卡尔曼滤波局部参量
    static float Priori_Estimation = 0;     //先验估计
    static float Posterior_Estimation = 0;//后验估计
    static float Priori_Convariance = 0;  //先验方差
    static float Posterior_Convariance = 0;//后验方差
    float Attitude_Angle_Y;
		
//卡尔曼滤波
//1.时间更新预测 : X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k) 
    Priori_Estimation = Posterior_Estimation - GYRO_RealY*Peried; //先验估计，积分获得角度
	if (Priori_Estimation != Priori_Estimation)
	{
		Priori_Estimation = 0;
	}
	
//2.更新先验协方差  : P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k) 
    Priori_Convariance = (float)sqrt( Posterior_Convariance * Posterior_Convariance + Q * Q );
	if (Priori_Convariance != Priori_Convariance)
	{
		Priori_Convariance = 0;
	}

//卡尔曼后验估计
//1.计算卡尔曼增益  : K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k)) 
    KalmanGain = (float)sqrt( Priori_Convariance * Priori_Convariance / ( Priori_Convariance * Priori_Convariance + R * R ) );
	if (KalmanGain != KalmanGain)
	{
		KalmanGain = 1;
	}
	
//2.测量更新（校正）: X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1)) 
    Posterior_Estimation  = Priori_Estimation + KalmanGain * (ACC_Angle - Priori_Estimation );
	if (Posterior_Estimation != Posterior_Estimation)
	{
		Posterior_Estimation = 0;
	}
	
//3.更新后验协方差  : P(k|k) =(I-K(k)*H(k))*P(k|k-1) 
    Posterior_Convariance = (float)sqrt(( 1 - KalmanGain ) * Priori_Convariance * Priori_Convariance );
	if (Posterior_Convariance != Posterior_Convariance)
	{
		Posterior_Convariance = 0;
	}
	
//4.得到最终融合角度
    Attitude_Angle_Y = Posterior_Estimation;
	
	if (Attitude_Angle_Y != Attitude_Angle_Y)
	{
		Attitude_Angle_Y = 1;
	}
	return Attitude_Angle_Y;
}

---

直立角度环PID算法

/************************************************************
函数名称: Vertical_ control (float   angle, float  angular_ velocity) ;
函数功能:直立平衡控制函数
输入参数:angle   angular_ velocity (角度 (融合的角度)，角速度)
输出参数:直立PID输出量
************************************************************/
#define Vertical_Speed_Value_Max  800  //直立环最大输出(正转)
#define Vertical_Speed_Value_Min -800  //(反转)

typedef struct{
	float P_ANGLE;
	float D_ANGLE;
    float Set_Angle;	
	
}Vertical_MPU6050;
Vertical_MPU6050 Vertical_PID;

float  Vertical_control(float  angle,float  angular_velocity)
{
	float Bias_Angle;
	float AngleControlOut；

//速度环pid输出加到平衡角度上，串级控制,---**SpeedControlOut是速度环的PID输出量，缩小了40倍**，使用时先自己定义
	Bias_Angle= (Vertical_PID.Set_Angle-(int)(SpeedControlOut/40.0))-angle;
//PD控制器得到输出
	AngleControlOut= Bias_Angle*Vertical_PID.P_ANGLE+angular_velocity *Vertical_PID.D_ANGLE ;

//直立PID输出限幅
	if(AngleControlOut>=Vertical_Speed_Value_Max ){
			
		 AngleControlOut=Vertical_Speed_Value_Max;
		
	}
	else if(AngleControlOut<=Vertical_Speed_Value_Min){
			
		  AngleControlOut=Vertical_Speed_Value_Min;
		
	} 
	return AngleControlOut；
}

---

速度环PID算法

/************************************************************
函数名称:float Speed_Control (int16 Encoder Left,int16 Encoder Right)
函数功能:速度PID控制函数（采用增量式PID）
输入参数: Left_speed，Right_speed，SetSpeed (左轮速度，右轮速度，设定速度)
输出参数:速度环PID输出量（SpeedControlIntegral增量和）
************************************************************/
#define Speed_Value_Max 150      //速度环PID输出量最大值
#define Speed_Value_Min -150     //速度环PID输出量最小值
typedef struct{
	float Kp;
	float Ki;
	float Low_SetSpeed;          //低速
	float Intermediate_SetSpeed; //中速
	float Fast_SetSpeed;         //高速
	float err;                   //当前偏差
	float err_next;              //上一次偏差
}Speed_Ring;
Speed_Ring Speed_Control_PID;
float SpeedControlIntegral=0; //速度增量和(全局变量)

float Speed_Control (int16 Encoder_Left , int16 Encoder_Right , float SetSpeed)
{
	float Transform_Speed;    
	float increment;
    float SpeedControlOut;

//取两轮平均速度为当前小车的速度
	Transform_Speed=(Encoder_Left+Encoder_Right)/2;

//计算出小车的速度偏差	 
	Speed_Control_PID.err=SetSpeed-Transform_Speed;

//计算增量（PID增量式）
	increment=Speed_Control_PID.Kp*(Speed_Control_PID.err-Speed_Control_PID.err_next)+Speed_Control_PID.Ki*Speed_Control_PID.err;
//增量累加	  
    SpeedControlIntegral+=increment;

//将此次偏差赋值给上一次	 
	Speed_Control_PID.err_next=Speed_Control_PID.err;

//速度PID输出限幅
	if(SpeedControlIntegral>Speed_Value_Max)
    { 
	 SpeedControlIntegral=Speed_Value_Max; 
	}
	 
	if(SpeedControlIntegral

---

方向环PID算法

/**********************************************************
函数名称:float direction (uint16 Inductance_1,uint16 Inductance_2,int16 Left_speed,int16 Right_speed,float mpu_gyro_z)
函数功能:方向控制函数（电磁）
输入参数: uint16 Inductance_1,uint16 Inductance_2,Left_speed,Right_speed,mpu_gyro_z(绕z轴角速度)
输出参数:方向PID输出量
*********************************************************/
int t;
float  outangle;
float  kpr=56.5;   //(先调kpr,kdr参数，再调kpg,kdg)
float  kdr=38.95;  //
float  nowgray;
float  errgray;
float  weifenggray;
float  lastgray;
float  kpg=0.20;   //
float  kdg=0.915;  //

float direction(uint16 Inductance_1,uint16 Inductance_2,int16 Left_speed,int16 Right_speed,float mpu_gyro_z)
{
	float Fvalue;	
	float outpwm;
	float LeftRightAdd,LeftRightSub	
	uint16 Left_Inductance,Right_Inductance;
	
	t=0;
	Left_Inductance=Inductance_1;    //左边电感
	Right_Inductance=Inductance_2;   //右边电感  
	LeftRightAdd=Left_Inductance+Right_Inductance; 
	LeftRightSub=(Left_Inductance-Right_Inductance);

//采用差比和减少数据波动的影响（相当于归一化）	
	Fvalue=(float)(150*(LeftRightSub/(LeftRightAdd*1.0)));

//mpu_gyro_z为绕Z轴的角速度	   
    outangle=(kpr+t)*Fvalue+kdr*(-(mpu_gyro_z*0.5));

//Right_speed,Left_speed(左右轮速度)
    nowgray= 0.4*(Right_speed-Left_speed)+0.6*(-(mpu_gyro_z*0.5));
   
	errgray=outangle-nowgray;
	
    weifenggray=nowgray-lastgray;
	
    lastgray=nowgray;
	
    outpwm=kpg*errgray+kdg*weifenggray; 
			
	return outpwm;
}

---