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

小弟才接触Arduino

最近想做一个Arduino自平衡小车

三轴角加速度传感器用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;

  }

学习了！