51单片机

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

2019-07-16 05:24发布

站内问答 / 51单片机
13295 2 1049
小弟才接触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 accelerometerconst int ypin = A1;                  // y-axisconst 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 accelerameterint 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;  }
友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。
2条回答
danni
2019-07-16 08:03
学习了！！！！！

一周热门 更多>