自平衡车，直立控制，倾角控制车速
我在做平衡小车，在卡尔曼滤波计算出倾角之后我再进行PID调节的时候，因为在PID调节函数里面加上了if(Angle>-3.0&&Angle<3.0){Angle=0.0;}，然后发现倾角在我缓慢变化至3度后，刚过3度时倾角会突然猛的增加，然后减小最后稳定在正常值上，如果我不加这句话，倾角变化一直正常，百思不得其解，希望有大神能帮帮我。。。。 另外，卡尔曼滤波和速度PID控制我都是写在中断里的，周期5ms，PID参数我暂时只加了一个P调节，其他均为零。 //******卡尔曼参数************ const float Q_angle=0.001;const float Q_gyro=0.003;const float R_angle=0.5;const float dt=0.07; //dt为kalman滤波器采样时间;或0.005,0.07const char C_0 = 1;float Q_bias=0.0, Angle_err=0.0;float PCt_0=0, PCt_1=0, E=0;float K_0=0, K_1=0, t_0=0, t_1=0;float Pdot ={0,0,0,0};float PP = { { 1, 0 },{ 0, 1 } };float position;static const float Kp= 40.0; //PID参数Anglestatic const float Kd= 0.0; //PID参数Gyro_ystatic const float Ksp = 0.0; //PID参数motor_speedstatic const float Ksi = 0.0; //PID参数 position /*****************卡尔曼滤波**************************************************/void Kalman_Filter(float Accel,float Gyro) { Angle+=(Gyro- Q_bias) * dt; //先验估计 Pdot=Q_angle- PP - PP; // Pk-先验估计误差协方差的微分 Pdot=-PP; Pdot=-PP; Pdot=Q_gyro; PP+= Pdot * dt; // Pk-先验估计误差协方差微分的积分 PP+= Pdot * dt; // =先验估计误差协方差 PP+= Pdot * dt; PP+= Pdot * dt; Angle_err= Accel - Angle; //zk-先验估计 PCt_0= C_0 * PP; PCt_1= C_0 * PP; E= R_angle + C_0 * PCt_0; K_0= PCt_0 / E; K_1= PCt_1 / E; t_0= PCt_0; t_1= C_0 * PP; PP-= K_0 * t_0; //后验估计误差协方差 PP-= K_0 * t_1; PP-= K_1 * t_0; PP-= K_1 * t_1; Angle += K_0 * Angle_err; //后验估计 Q_bias += K_1 * Angle_err; //后验估计 Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度 } void Angle_Calculate(void){ /****************************加速度****************************************/ Accel_x= MPU6050_Real_Data.Accel_X; //读取X轴加速度 Angle_ax= Accel_x*1.2*180/3.14; //弧度转换为度/****************************角速度****************************************/ Gyro_y = MPU6050_Real_Data.Gyro_Y; Angle_gy += (Gyro_y)*0.003; /***************************卡尔曼融合*************************************/ Kalman_Filter(Angle_ax,Angle_gy); //卡尔曼滤波计算倾角 } void Speed_Pid_Calculate(void){ if(Angle>-3.0&&Angle<3.0){Angle=0.0;} Speed_control= (Kp*Angle + Kd*Gyro_y); //PID：角速度和角度调节 Speed_control +=(Ksp*motor_speed + Ksi*position); //PID：车速度调节 speed_control_l= (int)Speed_control ; //左轮速度 speed_control_r= (int)- Speed_control; //右轮速度 if(speed_control_l> MAX_SPEED) speed_control_l = MAX_SPEED; if(speed_control_l< -MAX_SPEED) speed_control_l = -MAX_SPEED; if(speed_control_r> MAX_SPEED) speed_control_r = MAX_SPEED; if(speed_control_r< -MAX_SPEED) speed_control_r = -MAX_SPEED; }楼主希望的是个线性函数变化，但是显然是个非线性的变化，你的公式计算对吗？
页:
[1]