Arduino uno + mpu6050 陀螺仪 运用卡尔曼滤波姿态解算源代码(已验证)
程序员文章站
2022-07-12 10:20:14
...
Arduino uno + mpu6050 陀螺仪 运用卡尔曼滤波姿态解算源代码(编译通过)
关于怎么接线,请参照:https://blog.csdn.net/ling3ye/article/details/51360568
原创的作者的代码,不过如果用原创的代码,不用私有库会出现编译错误,修改为了私有库的调用就解决了,应该有很多人都遇到这个坎,误解而放弃了使用。
https://zhuanlan.zhihu.com/p/20082486
这是.ino程序文件:
//连线方法
//MPU-UNO
//VCC-VCC
//GND-GND
//SCL-A5
//SDA-A4
//INT-2 (Optional)
#include "Kalman.h"
#include <Wire.h>
#include <Math.h>
float fRad2Deg = 57.295779513f; //将弧度转为角度的乘数
const int MPU = 0x68; //MPU-6050的I2C地址
const int nValCnt = 7; //一次读取寄存器的数量
const int nCalibTimes = 1000; //校准时读数的次数
int calibData[nValCnt]; //校准数据
unsigned long nLastTime = 0; //上一次读数的时间
float fLastRoll = 0.0f; //上一次滤波得到的Roll角
float fLastPitch = 0.0f; //上一次滤波得到的Pitch角
Kalman kalmanRoll; //Roll角滤波器
Kalman kalmanPitch; //Pitch角滤波器
void setup() {
Serial.begin(9600); //初始化串口,指定波特率
Wire.begin(); //初始化Wire库
WriteMPUReg(0x6B, 0); //启动MPU6050设备
Calibration(); //执行校准
nLastTime = micros(); //记录当前时间
}
void loop() {
int readouts[nValCnt];
ReadAccGyr(readouts); //读出测量值
float realVals[7];
Rectify(readouts, realVals); //根据校准的偏移量进行纠正
//计算加速度向量的模长,均以g为单位
float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]);
float fRoll = GetRoll(realVals, fNorm); //计算Roll角
if (realVals[1] > 0) {
fRoll = -fRoll;
}
float fPitch = GetPitch(realVals, fNorm); //计算Pitch角
if (realVals[0] < 0) {
fPitch = -fPitch;
}
//计算两次测量的时间间隔dt,以秒为单位
unsigned long nCurTime = micros();
float dt = (double)(nCurTime - nLastTime) / 1000000.0;
//对Roll角和Pitch角进行卡尔曼滤波
float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt);
float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt);
//跟据滤波值计算角度速
float fRollRate = (fNewRoll - fLastRoll) / dt;
float fPitchRate = (fNewPitch - fLastPitch) / dt;
//更新Roll角和Pitch角
fLastRoll = fNewRoll;
fLastPitch = fNewPitch;
//更新本次测的时间
nLastTime = nCurTime;
//向串口打印输出Roll角和Pitch角,运行时在Arduino的串口监视器中查看
Serial.print("Roll:");
Serial.print(fNewRoll); Serial.print('(');
Serial.print(fRollRate); Serial.print("),\tPitch:");
Serial.print(fNewPitch); Serial.print('(');
Serial.print(fPitchRate); Serial.print(")\n");
delay(10);
}
//向MPU6050写入一个字节的数据
//指定寄存器地址与一个字节的值
void WriteMPUReg(int nReg, unsigned char nVal) {
Wire.beginTransmission(MPU);
Wire.write(nReg);
Wire.write(nVal);
Wire.endTransmission(true);
}
//从MPU6050读出一个字节的数据
//指定寄存器地址,返回读出的值
unsigned char ReadMPUReg(int nReg) {
Wire.beginTransmission(MPU);
Wire.write(nReg);
Wire.requestFrom(MPU, 1, true);
Wire.endTransmission(true);
return Wire.read();
}
//从MPU6050读出加速度计三个分量、温度和三个角速度计
//保存在指定的数组中
void ReadAccGyr(int *pVals) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.requestFrom(MPU, nValCnt * 2, true);
Wire.endTransmission(true);
for (long i = 0; i < nValCnt; ++i) {
pVals[i] = Wire.read() << 8 | Wire.read();
}
}
//对大量读数进行统计,校准平均偏移量
void Calibration()
{
float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
//先求和
for (int i = 0; i < nCalibTimes; ++i) {
int mpuVals[nValCnt];
ReadAccGyr(mpuVals);
for (int j = 0; j < nValCnt; ++j) {
valSums[j] += mpuVals[j];
}
}
//再求平均
for (int i = 0; i < nValCnt; ++i) {
calibData[i] = int(valSums[i] / nCalibTimes);
}
calibData[2] += 16384; //设芯片Z轴竖直向下,设定静态工作点。
}
//算得Roll角。算法见文档。
float GetRoll(float *pRealVals, float fNorm) {
float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]);
float fCos = fNormXZ / fNorm;
return acos(fCos) * fRad2Deg;
}
//算得Pitch角。算法见文档。
float GetPitch(float *pRealVals, float fNorm) {
float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]);
float fCos = fNormYZ / fNorm;
return acos(fCos) * fRad2Deg;
}
//对读数进行纠正,消除偏移,并转换为物理量。公式见文档。
void Rectify(int *pReadout, float *pRealVals) {
for (int i = 0; i < 3; ++i) {
pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f;
}
pRealVals[3] = pReadout[3] / 340.0f + 36.53;
for (int i = 4; i < 7; ++i) {
pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f;
}
}
这是Kalman.cpp文件,把这个文件和.h文件一起放在.ino程序文件同一目录下面,编译的时候调用私有的这个头文件,不要调用从库里面下载的Kalman库文件,不然那编译不通过,如果安装了kalman,就将其删除了。
#include "Kalman.h"
Kalman::Kalman() {
Q_angle = 0.001f;
Q_bias = 0.003f;
R_measure = 0.03f;
angle = 0.0f;
bias = 0.0f;
P[0][0] = 0.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.0f;
};
float Kalman::getAngle(float newAngle, float newRate, float dt) {
rate = newRate - bias;
angle += dt * rate;
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
float S = P[0][0] + R_measure;
float K[2];
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
float y = newAngle - angle;
angle += K[0] * y;
bias += K[1] * y;
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
};
void Kalman::setAngle(float angle) { this->angle = angle; };
float Kalman::getRate() { return this->rate; };
void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
float Kalman::getQangle() { return this->Q_angle; };
float Kalman::getQbias() { return this->Q_bias; };
float Kalman::getRmeasure() { return this->R_measure; };
这是kalman.h文件:
#ifndef _Kalman_h_
#define _Kalman_h_
class Kalman {
public:
Kalman();
float getAngle(float newAngle, float newRate, float dt);
void setAngle(float angle);
float getRate();
void setQangle(float Q_angle);
void setQbias(float Q_bias);
void setRmeasure(float R_measure);
float getQangle();
float getQbias();
float getRmeasure();
private:
float Q_angle;
float Q_bias;
float R_measure;
float angle;
float bias;
float rate;
float P[2][2];
};
#endif
上一篇: linux watchdog看门狗编程
下一篇: LINUX--看门狗WATCHDOG