求助，两轮小车~
小车用的是ARDUINO UNO+MPU6050+L289，目前再平衡状态，总是不停地抖动，下图是平衡状态角度波形图，javascript:;求大神们指点~#include <Wire.h>
#include <i2cdev.h>
#include <MPU6050.h>
#include <PID_v1.h>
#define Left1 10 //定义左直流电机输出
#define Left2 11 //定义左直流电机输出
#define Right1 5 //定义右直流电机输出
#define Right2 6 //定义右直流电机输出
#define drift 20 //小车角度调整范围
#define Gry_offset -72 //陀螺仪X轴的静态飘移。
#define Gyr_Gain -0.00763 //由陀螺仪X轴读数转换为角速度值（1/131）
//向前方向与X轴为反方向，故乘以-1
#define ACC_Gain 0.000061 //由读数转换为加速度值(1/16384)
#define pi 3.1415926 //定义PI值
//#define K 0.715 //一阶互补滤波权重取值，不知由来，可更改。
#define Q_angle 0.01 //角度数据置信度
#define Q_omega 0.0003 //角速度数据置信度
#define R_angle 0.01 //方差噪声
unsigned long preTime = 0;//定义积分初始时间
float angleG = 0;
double Klm_angle;
float bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
double Setpoint, Input, Output;
double temp_kp,temp_ki,temp_kd;
MPU6050 accelgyro; //MPU6050初始化
int16_t ax, ay, az;
int16_t gx, gy, gz;
PID myPID(&Input,&Output,&Setpoint,10,0.05,0,DIRECT);
void car_stop() //小车停止函数
{
analogWrite(Left1,0);
analogWrite(Right1,0);
analogWrite(Left2,0);
analogWrite(Right2,0);
}
void setup() //主程序初始化
{
Wire.begin();
Serial.begin(9600);
accelgyro.initialize(); //MPU6050初始化。
pinMode(Left1,OUTPUT);
pinMode(Left2,OUTPUT);
pinMode(Right1,OUTPUT);
pinMode(Right2,OUTPUT);
Wire.begin(); //I2C初始化
accelgyro.testConnection();
Setpoint=-2.00; //设置平衡角
myPID.SetSampleTime(10);
myPID.SetMode(AUTOMATIC);
}
void loop()
{
unsigned long now = millis(); //当前时间(ms)
float dt = (now - preTime) / 500.0; //微分时间(s)
preTime = now; //上一次采样时间(ms)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取六轴数值
float Y_Accelerometer = ay * ACC_Gain; //转换为向前的加速度(g)，为负值
float Z_Accelerometer = az * ACC_Gain; //转换为向下的加速度(g)
float angleA= atan(Y_Accelerometer/ Z_Accelerometer)* (-180)/ pi;//加速度仪，反正切获得弧度值，乘以180度/pi
//获得角度值，乘以-1得正值
float gx_revised = gx + Gry_offset; //陀螺仪x轴静态时修正后的角速度读数，向前为负值
float omega= Gyr_Gain* gx_revised; //陀螺仪，转换为向前的角速度(o/s)，Gyr_Gain取负，故负负得正
//卡尔曼滤波
Klm_angle += (omega - bias) * dt; // 先验估计
P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
P_01 += -P_11 * dt;
P_10 += -P_11 * dt;
P_11 += +Q_omega * dt; // 先验估计误差协方差
float K_0 = P_00 / (P_00 + R_angle);
float K_1 = P_10 / (P_00 + R_angle);
bias += K_1 * (angleA - Klm_angle);
Klm_angle += K_0 * (angleA - Klm_angle); // 后验估计
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01; // 后验估计误差协方差
Serial.print(angleA);
Serial.print(",");
Serial.println(Klm_angle);
//PID算法结合PWM输出
if((Klm_angle>(Setpoint-drift))&&(Klm_angle<(Setpoint+drift)))
{
if((Klm_angle>Setpoint)&&(Klm_angle<(Setpoint+drift)))
{
myPID.SetControllerDirection(REVERSE);
Input=Klm_angle;
myPID.Compute();
analogWrite(Left2,min(Output+150,255));
analogWrite(Right2,min(Output+150,255));
analogWrite(Left1,0);
analogWrite(Right1,0);
}
if((Klm_angle<Setpoint)&&(Klm_angle>(Setpoint-drift)))
{
myPID.SetControllerDirection(DIRECT);
Input=Klm_angle;
myPID.Compute();
analogWrite(Left1,min(Output+100,255));
analogWrite(Right1,min(Output+100,255));
analogWrite(Left2,0);
analogWrite(Right2,0);
}
}
else
{
car_stop();
}
} 程序没看，说下思路好了， 看来程序应该是检测陀螺仪状态，然后调整小车姿态，保持平衡。 因为机械精度问题微小的震动也可能会被检测到，再加之车子调整幅度可能比较大，所以车子会一直抖动，往复运动于左右偏差之间。
思路，增加调整姿态的阀值，减小调整姿态的幅度。 或者根据偏离程度来决定调整幅度的大小。 也就是说，偏离1度 用 相对微弱的幅度和速度调整姿态，偏离10度，就大幅度和速度的调整姿态。 i2cdev是干啥的啊给个呗[email protected]谢啦
i2cdev是干啥的啊给个呗[email protected]谢啦，刚才打错了
页:
[1]