Ref : https://b94be14129454da9cf7f056f5f8b89a9b17da0be.googledrive.com/host/0B0ZbiLZrqVa6Y2d3UjFVWDhNZms/filter.pdf
先互補濾波器
從加速度計和陀螺儀讀到raw data....
Motor Output = Kp × (Angle) + Kd × (Angular Velocity)
以最簡單的方式來看 就是
angle = (0.98)*(angle + gyro * dt) + (0.02)*(x_acc);
Sample Period: The amount of time that passes between each program loop. If the sample rate is 100 Hz, the sample period is 0.01 sec.
The time constant, τ, of a digital low-pass filter,
x_acc 就是加速度計讀到的value做完校正...
gyro 就是陀螺儀讀到的value做完校正....
x_acc = (float)(x_acc_ADC – x_acc_offset) * x_acc_scale;
gyro = (float)(gyro_ADC – gyro_offset) * gyro_scale;
y = (a)*(y) + (1-a)*(x);
那 a到底怎麼算呢...首先你要先決定 T
以下文的例子是 0.8
a = 0.8/(0.8+ dt)
now = mills();
dt = now - preTime
算出a 之後再帶入
y = (a)*(y) + (1-a)*(x) 這個公式就可以求得 經過互補濾波後的angle....
// 互補演算法 sample code
void filter(){
//------------------互补滤波 ------------------------
unsigned long now = millis(); // 当前时间(ms)
float dt = (now - preTime) / 1000.0; // 微分时间(ms)
preTime = now;
float K = 0.8;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1-A) * angleA; // 互补滤波算法
}
// arduino code
//卡耳漫滤波 PD融合控制代码:
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
//#include <LiquidCrystal.h>;
//LiquidCrystal lcd( 12, 11, 10, 9, 8,7);
#define Gry_offset -20 // 陀螺仪偏移量
//#define Gyr_Gain 0.04 // 满量程2000dps时灵敏度(dps/digital)
#define Gyr_Gain 65.5
#define pi 3.14159
int16_t ax, ay, az;
int16_t gx, gy, gz;
/********** 互补滤波器参数 *********/
//unsigned long preTime = 0; // 采样时间
//float f_angle = 0.0; // 滤波处理后的角度值
/*********** PID控制器参数 *********/
//unsigned long lastTime;
float Output; //;, Setpoint,Input;
//double errSum, lastErr;
float kp, ki, kd,kpp;
//int SampleTime = 0.1; //1 sec
//float Outputa = 0.0;
float angleA,omega;
//double Kp, Ki, Kd;
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5,dtt=0.007,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
float angle, angle_dot; // aaxdot,aax;
float position_dot,position_dot_filter,positiono;
//double Speed_Need=0;
//float K_angle=2;
//float K_angle_dot=0.5; //换算系数：256/10 =25.6；
//float K_position=0.1; //换算系数：(256/10) * (2*pi/(64*12))=0.20944；//256/10:电压换算至PWM，256对应10V；
//float K_position_dot=1; //换算系数：(256/10) * (25*2*pi/(64*12))=20.944；
//--------------------------------------
int oommm;
int ooommm;
//int oooommmm;
//double OLH= 10,ORH = 10;
int TN1=22;
int TN2=23;
int TN3=24;
int TN4=25;
int ENA=2;
int ENB=3;
//-------------------------------------
void setup() {
Wire.begin();
//lcd.begin(16, 2);
//lcd.print("hello, world!");
//delay(1000);
accelgyro.initialize();
delay(500);
pinMode(22,OUTPUT);
pinMode(23,OUTPUT);
pinMode(24,OUTPUT);
pinMode(25,OUTPUT);
pinMode(2,OUTPUT);
pinMode(3,OUTPUT);
delay(100);
//Serial.begin(9600);
}
void loop() {
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//-----------------------------------------------------------------------------------------------------------------
angleA= atan2(ay , az) * 180 / pi-0.2;
// 根据加速度分量得到的角度(degree)
//180度至0至-180（360度）取0度为坚直时中立点 因为坚直时为有偏差，所以减去0.2度....
//omega= Gyr_Gain * (gx + Gry_offset); // 当前角速度(degree/s)
omega=(gx + Gry_offset)/Gyr_Gain; // 当前角速度(degree/s)
if (abs(angleA)<30) { //这个是误差达到一定程度后的系统关闭开关.
Kalman_Filter(angleA,omega);
PIDD();
PWMB();
//LCDD();
}
else {
analogWrite(ENA, 0); //PWM调速a==0-255
analogWrite(ENB, 0);
}
delay(10);
}
//=---------------------------------------------------------------
void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dtt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dtt;
P[0][1] += Pdot[1] * dtt;
P[1][0] += Pdot[2] * dtt;
P[1][1] += Pdot[3] * dtt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle+= K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//也许应该用last_angle-angle
}
//-----------------------
void PIDD(){
kp=analogRead(8)*0.01;
ki=analogRead(9)*0.007;
kd=analogRead(10)*0.007;
kpp=analogRead(11)*0.007;
//aaxdot=0.5*aaxdot+0.5*angle_dot;
//aax=0.5*aax+0.5*angle;
position_dot=Output*0.04; //利用PWM值代替轮速传感器的信号
position_dot_filter*=0.9; //车轮速度滤波
position_dot_filter+=position_dot*0.1;
positiono+=position_dot_filter;
//positiono+=Speed_Need;
positiono=constrain(positiono,-500,500);
Output= 2*angle *kp + 0.5*angle_dot*ki +0.02*positiono *kd + 1*position_dot_filter *kpp;
//Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono *kd + K_position_dot*position_dot_filter *kpp;
}
//-------------电机正反转 PWM输出-----------
void PWMB(){
if(Output<0)
{
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
if(Output>0)
{
digitalWrite(TN1, LOW);
digitalWrite(TN2, HIGH);
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
}
oommm=min(220,abs(Output));
analogWrite(ENA, oommm+35); //PWM调速a==0-255
analogWrite(ENB, oommm+30);
}
===========================================
#define Gry_offset -20 // 陀螺仪偏移量
//#define Gyr_Gain 0.04 // 满量程2000dps时灵敏度(dps/digital)
#define Gyr_Gain 65.5
这些参数应该按照怎样去调试呀~
先用串口测试静止时的数据，最减去这些数据就是 0 点了~
测试好了最组合程序代码
这个是 敏感度 65.5 ...
看 FS_SEL=1
Q2:咨询一下：
在 PIDD函数中，有以下语句：
kp=analogRead(8)*0.01;
ki=analogRead(9)*0.007;
kd=analogRead(10)*0.007;
kpp=analogRead(11)*0.007;
能否解释一下原因。感觉从8\9\10\11中输入的应该是随机数？
A2: 这个是可调电阻 0-1023 用来调节 PIDD 稳定性，
首先将可调电阻全部置 0 车子此时为没反应，
先从P 开始调节到车子前后摇摆，
然后最调 I 使其稳定，出现震动时，就减少 P I 的电阻量，使车子平衡 更稳定 不抖动
(KD,KPP 是调节前进后退量程,如果不摇控或没码盘可以全置0 （也可提高稳定性）)
沒有留言:
張貼留言