|
|
先前研究arduino +MPU6050用卡尔曼滤波和PID处理，现在得到了初步成果
感谢一下坛友的文章：YWD19890423的 形象解释PID算法+PID算法源代码
博友ling3ye 的 http://blog.csdn.net/ling3ye/article/details/51360568
极客工坊中 johnsonzzd 前辈的 http://www.geek-workshop.com/thread-2471-1-1.html
另，极客工坊是一个很好的网站，大家如果在本社区内无法找到的东西可以去那里看看
#include <MPU6050.h>
#include <I2Cdev.h>
#include <MPU6050.h>
#include "Wire.h"
typedef struct PID_Value
{
double liEkVal[3]; //差值保存，给定和反馈的差值
char Flag[3];
double KP = 1; //比例系数
double KI = 1; //积分常数
double KD = 1; //微分常数
double PriVal;
double CurVal;
double SetVal = 0;
} PID_ValueStr;
PID_ValueStr PID;//此处缺少让pid隔一段时间运行一次的程序
double input;
int pinId = 1; //定义接口
int pinIc = 2;
int pinIb = 3;
int pinIa = 4;
int ENA = 6;
int ENB = 5;
double PID_Operation(double input)
{
PID.CurVal = input;
double Temp[3] = {0}; //中间临时变量
double PostSum = 0; //正数和
double NegSum = 0; //负数和
if (PID.SetVal > PID.CurVal) //设定值大于实际值否？
{
if (PID.SetVal - PID.CurVal > 10) //偏差大于10否？
PID.PriVal = 100; //偏差大于10为上限幅值输出(全速加热)
else //否则慢慢来
{
Temp[0] = PID.SetVal - PID.CurVal; //偏差<=10,计算E(k)
PID.Flag[1] = 0; //E(k)为正数,因为设定值大于实际值
//数值进行移位，注意顺序，否则会覆盖掉前面的数值
PID.liEkVal[2] = PID.liEkVal[1];
PID.liEkVal[1] = PID.liEkVal[0];
PID.liEkVal[0] = Temp[0];
// ===================================================================
if (PID.liEkVal[0] > PID.liEkVal[1]) //E(k)>E(k-1)否？
{
Temp[0] = PID.liEkVal[0] - PID.liEkVal[1]; //E(k)>E(k-1)
PID.Flag[0] = 0; //E(k)-E(k-1)为正数
}
else
{
Temp[0] = PID.liEkVal[1] - PID.liEkVal[0]; //E(k)<E(k-1)
PID.Flag[0] = 1; //E(k)-E(k-1)为负数
}
// ===================================================================
Temp[2] = PID.liEkVal[1] * 2; //2E(k-1)
if ((PID.liEkVal[0] + PID.liEkVal[2]) > Temp[2]) //E(k-2)+E(k)>2E(k-1)否？
{
Temp[2] = (PID.liEkVal[0] + PID.liEkVal[2]) - Temp[2];
PID.Flag[2] = 0; //E(k-2)+E(k)-2E(k-1)为正数
}
else //E(k-2)+E(k)<2E(k-1)
{
Temp[2] = Temp[2] - (PID.liEkVal[0] + PID.liEkVal[2]);
PID.Flag[2] = 1; //E(k-2)+E(k)-2E(k-1)为负数
}
// ===================================================================
Temp[0] = PID.KP * Temp[0]; //KP*[E(k)-E(k-1)]
Temp[1] = PID.KI * PID.liEkVal[0]; //KI*E(k)
Temp[2] = PID.KD * Temp[2]; //KD*[E(k-2)+E(k)-2E(k-1)]
// 以下部分代码是讲所有的正数项叠加，负数项叠加
// ========= 计算KP*[E(k)-E(k-1)]的值 =========
if (PID.Flag[0] == 0)
PostSum += Temp[0]; //正数和
else
NegSum += Temp[0]; //负数和
// ========= 计算KI*E(k)的值 =========
if (PID.Flag[1] == 0)
PostSum += Temp[1]; //正数和
else
; /* 空操作。就是因为PID.iSetVal > PID.iCurVal（即E(K)>0）才进入if的，
那么就没可能为负，所以打个转回去就是了 */
// ========= 计算KD*[E(k-2)+E(k)-2E(k-1)]的值 =========
if (PID.Flag[2] == 0)
PostSum += Temp[2]; //正数和
else
NegSum += Temp[2]; //负数和
/* ========= 计算U(k) ========= */
PostSum += PID.PriVal;
if (PostSum > NegSum) //是否控制量为正数
{
Temp[0] = PostSum - NegSum;
if (Temp[0] < 100 ) //小于上限幅值则为计算值输出
PID.PriVal = Temp[0];
else PID.PriVal = 100; //否则为上限幅值输出
}
else //控制量输出为负数，则输出0(下限幅值输出)
PID.PriVal = 0;
}
}
else PID.PriVal = 0;
double output = PID.PriVal;
return output; //同上，嘿嘿
}
/*Constructor (...)*********************************************************
The parameters specified here are those for for which we can't set up
reliable defaults, so we need to have the user set them.
***************************************************************************/
MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt; //微分时间
int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据
float aax = 0, aay = 0, aaz = 0, agx = 0, agy = 0, agz = 0; //角度变量
long axo = 0, ayo = 0, azo = 0; //加速度计偏移量
long gxo = 0, gyo = 0, gzo = 0; //陀螺仪偏移量
float pi = 3.1415926;
float AcceRatio = 16384.0; //加速度计比例系数
float GyroRatio = 131.0; //陀螺仪比例系数
uint8_t n_sample = 8; //加速度计滤波算法采样个数
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y轴采样队列
long aax_sum, aay_sum, aaz_sum; //x,y轴采样和
float a_x[10] = {0}, a_y[10] = {0}, a_z[10] = {0} , g_x[10] = {0} , g_y[10] = {0}, g_z[10] = {0}; //加速度计协方差计算队列
float Px = 1, Rx, Kx, Sx, Vx, Qx; //x轴卡尔曼变量
float Py = 1, Ry, Ky, Sy, Vy, Qy; //y轴卡尔曼变量
float Pz = 1, Rz, Kz, Sz, Vz, Qz; //z轴卡尔曼变量
static int dianji = 0;
static int SampleTime;
static double Input;
static double Drive;
static double Output;
static double Setpoint;
void setup()
{
Wire.begin();
Serial.begin(115200);
accelgyro.initialize(); //初始化
unsigned short times = 200; //采样次数
for (int i = 0; i < times; i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
axo += ax; ayo += ay; azo += az; //采样和
gxo += gx; gyo += gy; gzo += gz;
}
axo /= times; ayo /= times; azo /= times; //计算加速度计偏移
gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移
pinMode(pinIa, OUTPUT);
pinMode(pinIb, OUTPUT);
pinMode(pinIc, OUTPUT);
pinMode(pinId, OUTPUT);
}
void loop()
{
unsigned long now = millis(); //当前时间(ms)
dt = (now - lastTime) / 1000.0; //微分时间(s)
lastTime = now; //上一次采样时间(ms)
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值
float accx = ax / AcceRatio; //x轴加速度
float accy = ay / AcceRatio; //y轴加速度
float accz = az / AcceRatio; //z轴加速度
aax = atan(accy / accz) * (-180) / pi; //y轴对于z轴的夹角
aay = atan(accx / accz) * 180 / pi; //x轴对于z轴的夹角
aaz = atan(accz / accy) * 180 / pi; //z轴对于y轴的夹角
aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法
aay_sum = 0;
aaz_sum = 0;
for (int i = 1; i < n_sample; i++)
{
aaxs[i - 1] = aaxs;
aax_sum += aaxs * i;
aays[i - 1] = aays;
aay_sum += aays * i;
aazs[i - 1] = aazs;
aaz_sum += aazs * i;
}
aaxs[n_sample - 1] = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11 * n_sample / 2.0)) * 9 / 7.0; //角度调幅至0-90°
aays[n_sample - 1] = aay; //此处应用实验法取得合适的系数
aay_sum += aay * n_sample; //本例系数为9/7
aay = (aay_sum / (11 * n_sample / 2.0)) * 9 / 7.0;
aazs[n_sample - 1] = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11 * n_sample / 2.0)) * 9 / 7.0;
float gyrox = - (gx - gxo) / GyroRatio * dt; //x轴角速度
float gyroy = - (gy - gyo) / GyroRatio * dt; //y轴角速度
float gyroz = - (gz - gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //x轴角速度积分
agz += gyroz;
/* kalman start */
Sx = 0; Rx = 0;
Sy = 0; Ry = 0;
Sz = 0; Rz = 0;
for (int i = 1; i < 10; i++)
{ //测量值平均值运算
a_x[i - 1] = a_x; //即加速度平均值
Sx += a_x;
a_y[i - 1] = a_y;
Sy += a_y;
a_z[i - 1] = a_z;
Sz += a_z;
}
a_x[9] = aax;
Sx += aax;
Sx /= 10; //x轴加速度平均值
a_y[9] = aay;
Sy += aay;
Sy /= 10; //y轴加速度平均值
a_z[9] = aaz;
Sz += aaz;
Sz /= 10;
for (int i = 0; i < 10; i++)
{
Rx += sq(a_x - Sx);
Ry += sq(a_y - Sy);
Rz += sq(a_z - Sz);
}
Rx = Rx / 9; //得到方差
Ry = Ry / 9;
Rz = Rz / 9;
Px = Px + 0.0025; // 0.0025在下面有说明...
Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
Px = (1 - Kx) * Px; //更新p值
Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;
Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;
input = agx;
Serial.println (PID_Operation(input));
if (agx > 5)
{
dianji = dianji - 1;
delay(20);
}
if (agx < -5)
{
dianji = dianji + 1;
delay(20);
}
if (agx > -5 && agx < 5)
{
dianji = 90;
}
/* kalman end */
Serial.print(agx); Serial.print(",");
Serial.print(agy); Serial.print(",");
Serial.print(agz); Serial.println();
}
这个版本还比较粗糙，之后会在帖子后面再发最后的完成稿
|
|
发表于 2017-3-25 09:02
楼主
