你好!欢迎来到深圳市颖特新科技有限公司!
语言
当前位置:首页 >> 技术中心 >> 单片机入门 >> 基于9轴惯性运动传感器的三阶卡尔曼滤波器算法

基于9轴惯性运动传感器的三阶卡尔曼滤波器算法

关键字:卡尔曼算法 滤波器 作者: 来源: 发布时间:2019-07-31  浏览:5

最近在玩九轴的惯性传感器,很是有挑战性.九轴说的是三轴的加速度计、三轴的陀螺仪以及三轴的磁场传感器。但是只是单纯的测出九个轴的数据没什么用,关键是要能够融合这九轴数据得出我们想要的结果。这里就运用三阶卡尔曼滤波算法来融合这九轴运动数据为三轴的角度。运用这三个角度可以用来做自平衡车或者四轴飞行器.

一、卡尔曼算法理解

其实如果不去考虑kalman算法是怎么来的,我们只需要知道有下面几个式子就可以了,具体意思可以看上面的wikipedia链接

kalman_embbnux_blog

二 卡尔曼滤波算法的实现

这里我的算法是运行在avr单片机上的,所以采用的是c语言写的。下面的代码是要放到avr的定时器中断测试刷新的。用示波器测试了一下,这个算法在16M晶振下的运行时间需要0.35ms,而数据采集需要3ms左右,所以选定定时器时间为8ms.之前也写过一阶的kalman算法,运用在自平衡车上,这边是三阶的,主要是矩阵运算.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 //kalman.cfloatdtTimer   = 0.008;floatxk[9] = {0,0,0,0,0,0,0,0,0};floatpk[9] = {1,0,0,0,1,0,0,0,0};floatI[9]  = {1,0,0,0,1,0,0,0,1};floatR[9]  = {0.5,0,0,0,0.5,0,0,0,0.01};floatQ[9] = {0.005,0,0,0,0.005,0,0,0,0.001};   voidmatrix_add(float* mata,float* matb,float* matc){    uint8_t i,j;    for(i=0; i<3; i++){       for(j=0; j<3; j++){          matc[i*3+j] = mata[i*3+j] + matb[i*3+j];       }    }}   voidmatrix_sub(float* mata,float* matb,float* matc){    uint8_t i,j;    for(i=0; i<3; i++){       for(j=0; j<3; j++){          matc[i*3+j] = mata[i*3+j] - matb[i*3+j];       }    }}   voidmatrix_multi(float* mata,float* matb,float* matc){  uint8_t i,j,m;  for(i=0; i<3; i++)  {    for(j=0; j<3; j++)   {      matc[i*3+j]=0.0;      for(m=0; m<3; m++)     {        matc[i*3+j] += mata[i*3+m] * matb[m*3+j];      }    } }}   voidKalmanFilter(float* am_angle_mat,float* gyro_angle_mat){uint8_t i,j;floatyk[9];floatpk_new[9];floatK[9];floatKxYk[9];floatI_K[9];floatS[9];floatS_invert[9];floatsdet;   //xk = xk + ukmatrix_add(xk,gyro_angle_mat,xk);//pk = pk + Qmatrix_add(pk,Q,pk);//yk =  xnew - xkmatrix_sub(am_angle_mat,xk,yk);//S=Pk + Rmatrix_add(pk,R,S);//S求逆invertsdet = S[0] * S[4] * S[8]          + S[1] * S[5] * S[6]          + S[2] * S[3] * S[7]          - S[2] * S[4] * S[6]          - S[5] * S[7] * S[0]          - S[8] * S[1] * S[3];   S_invert[0] = (S[4] * S[8] - S[5] * S[7])/sdet;S_invert[1] = (S[2] * S[7] - S[1] * S[8])/sdet;S_invert[2] = (S[1] * S[7] - S[4] * S[6])/sdet;   S_invert[3] = (S[5] * S[6] - S[3] * S[8])/sdet;S_invert[4] = (S[0] * S[8] - S[2] * S[6])/sdet;S_invert[5] = (S[2] * S[3] - S[0] * S[5])/sdet;   S_invert[6] = (S[3] * S[7] - S[4] * S[6])/sdet;S_invert[7] = (S[1] * S[6] - S[0] * S[7])/sdet;S_invert[8] = (S[0] * S[4] - S[1] * S[3])/sdet;//K = Pk * S_invertmatrix_multi(pk,S_invert,K);//KxYk = K * Ykmatrix_multi(K,yk,KxYk);//xk = xk + K * ykmatrix_add(xk,KxYk,xk);//pk = (I - K)*(pk)matrix_sub(I,K,I_K);matrix_multi(I_K,pk,pk_new);//update pk//pk = pk_new;for(i=0; i<3; i++){    for(j=0; j<3; j++){        pk[i*3+j] = pk_new[i*3+j];    }  }}

三 运用卡尔曼滤波器

这里的kalman滤波器是离散数字滤波器,需要迭代运算。这里把算法放到avr的定时器中断里面执行,进行递归运算.

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 //isr.c#include "kalman.h"floatmpu_9dof_values[9]={0};floatam_angle[3];floatgyro_angle[3];floatam_angle_mat[9]={0,0,0,0,0,0,0,0,0};floatgyro_angle_mat[9]={0,0,0,0,0,0,0,0,0};   //8MSISR(TIMER0_OVF_vect){//设置计数开始的初始值TCNT0 = 130 ;  //8mssei();//采集九轴数据//mpu_9dof_values 单位为g与度/s//加速度计和陀螺仪mpu_getValue6(&mpu_9dof_values[0],&mpu_9dof_values[1],&mpu_9dof_values[2],&mpu_9dof_values[3],&mpu_hmc_values[4],&mpu_hmc_values[5]);//磁场传感器compass_mgetValues(&mpu_9dof_values[6],&mpu_9dof_values[7],&mpu_9dof_values[8]);   accel_compass2angle(&mpu_9dof_values[0],&mpu_9dof_values[6],am_angle);gyro2angle(&mpu_9dof_values[3],gyro_angle);   am_angle_mat[0] = am_angle[0];am_angle_mat[4] = am_angle[1];am_angle_mat[8] = am_angle[2];   gyro_angle_mat[0] = gyro_angle[1];gyro_angle_mat[4] = - gyro_angle[0];gyro_angle_mat[8] = - gyro_angle[2];   //卡尔曼KalmanFilter(am_angle_mat,gyro_angle_mat);   //输出三轴角度//xk[0] xk[4] xk[8]}

实测可以准确的输出三轴的角度,为了获得更好的响应速度和跟踪精度还需调整参数.

编辑:admin  最后修改时间:2019-07-31

联系方式

0755-82591179

传真:0755-82591176

邮箱:vicky@yingtexin.net

地址:深圳市龙华区民治街道民治大道973万众润丰创业园A栋2楼A08

Copyright © 2014-2023 颖特新科技有限公司 All Rights Reserved.  粤ICP备14043402号-4