两轮平衡车相关算法
卡尔曼滤波函数
/**************************************************************
函数名称: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<Speed_Value_Min)
{
SpeedControlIntegral=Speed_Value_Min;
}
return 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;
}
来源：CSDN
作者：浮诩
链接：https://blog.csdn.net/weixin_43999800/article/details/103461050