Arduino自平衡小车的问题，恳请各位帮忙

henryham

小弟才接触Arduino
最近想做一个Arduino自平衡小车  有一块UNO
三轴角加速度传感器用ADXL335       陀螺仪用IDG300        电机采用Pololu的29:1减速电机( 用串口传输方式进行调速，范围0~127)
控制思路采用卡尔曼滤波+PID控制的方法
但是遇到两个问题
第一是卡尔曼滤波后发现计算所得的角度值有一定的滞后
其次是在调试PID参数Kp时发现小车在晃动的同时会不由自主的变快最后无法控制，不知这是否和卡尔曼滤波没有处理好有关
想请各位大侠帮忙，能否告知一二
一下是我的代码(PID参数还未设定)：
#include <SoftwareSerial.h>
#include <math.h>

SoftwareSerial mySerial(5, 6);
const int xpin = A0;                  // x-axis of the accelerometer
const int ypin = A1;                  // y-axis
const int zpin = A2;                  // z-axis (only on 3-axis models)
const int x_gyro = A4;
int e, motor_speed, x_g;
long z_average, y_average;
float z_acc, angle_sum, y_acc, x_rate;
double Kp, Ki,Kd, angle_acc, actAngle0, actAngle;
int sensorValue[2]  = { 0, 0}; //y,z accelerameter
int sensorZero[2]   = { 336, 340}; //y,z accelerameter

int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

void setup()
{
  Serial.begin(9600);
/***************电机初始化******************/
  mySerial.begin(19200);
  mySerial.write(0xAA);
  mySerial.write(0x88);
  mySerial.write(127);
  mySerial.write(0xAA);
  mySerial.write(0x8E);
  mySerial.write(127);
/*********************************************/
  Kp = 0;
  Ki = 0;
  Kd = 0;
}

void loop()
{
  getangle_accelerameter();
  getangle_gyro();
  
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         
  delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
  actAngle = kalmanCalculate(angle_acc, x_rate, lastLoopTime);
  /*******************PID算法**************************/
  e = actAngle - actAngle0;
  angle_sum += actAngle;
   actAngle0 = actAngle;
  if (angle_sum>50) angle_sum = 50;
  if (angle_sum<-50) angle_sum = -50;
  motor_speed = (Kp * actAngle) + (Ki * angle_sum) + (Kd * e);
/*****************************************************/
  
  if(motor_speed > 0)  //run forward
  {
    motor_speed = motor_speed;
    mySerial.write(0x8A);
    mySerial.write(motor_speed);
    mySerial.write(0x8C);
    mySerial.write(motor_speed);
  }
  if(motor_speed < 0)  //run backward
  {
    motor_speed = 0 - motor_speed;
    mySerial.write(0x88);
    mySerial.write(motor_speed);
    mySerial.write(0x8E);
    mySerial.write(motor_speed);
  }
  
}

void getangle_accelerameter()
{
  y_average = 0;
  z_average = 0;
  for(int i=0;i<15;i++)
  {
    for(int n=0;n<2;n++)
    sensorValue[n]=analogRead(n+1)-sensorZero[n];
    y_average = y_average + sensorValue[0];
    z_average = z_average + sensorValue[1];
  }
  y_acc = y_average / 15.0;
  z_acc = z_average / 15.0;
  angle_acc = atan(z_acc/y_acc) * 180 / 3.14 - 2;
}

void getangle_gyro()
{
  x_g = analogRead(4);
  x_rate = (x_g - 308) * 2.44;
}
/*************************卡尔曼滤波（从网上参考的）**************/
    float Q_angle  =  0.001;
    float Q_gyro   =  0.003;
    float R_angle  =  0.03;

    float x_angle = 0;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float dt, y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate,int looptime)
  {
    dt = float(looptime)/1000;
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;

    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;

    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;

    return x_angle;
  }

vigiles

不懂，帮忙顶顶

zhxzhx

kalman滤波后，角度反应太慢了。