Arduino自平衡小车的问题，恳请各位帮忙
小弟才接触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= { 0, 0}; //y,z accelerameter
int sensorZero = { 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=analogRead(n+1)-sensorZero;
y_average = y_average + sensorValue;
z_average = z_average + sensorValue;
}
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;
} 不懂，帮忙顶顶 kalman滤波后，角度反应太慢了。
页:
[1]