---

兩輪平衡車相關算法

---

---

卡爾曼濾波函數

/************************************************************** 函數名稱: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; }

---

總結

以上是生活随笔為你收集整理的直立两轮平衡车核心代码的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。