利用加速度传感器模糊检测设备被人为移动的算法

项目需要做一个安全装置盒，防止人为移动，破坏。选用一款数字式3轴加速度传感器，原思路是通过加速度传感器的3轴加速度值进行二次积分获取设备的移动位置，通过位置判断设备是否被移动；通过瞬时加速度值来判断设备是否为强力破坏；
经过测试发现加速度本身噪声很大，二次积分累计误差很大，重力加速度值会根据位置不同3轴有偏差等诸多问题，不得不根据实际情况进行简化模型，加速度进行10次平均滤波（采样时间为100ms）；
将整个运动分成2种状态，稳定态与移动态，通过连续n次3个方向加速度微分值进行判断当前状态，假定n为5次判定，连续5次3个方向加速度微分值都小于设定阈值，则认为当前处于稳定状态，将最后一次加速度值作为当前重力加速度，随后进行重力加速度平均，若出现3个方向加速度微分值大于阈值，则当前处于移动态，使用以前重力加速度进行计算，开始进行两次积分，获取位置信息。
移动态时加速度与重力加速度进行比较，将两次变化小于3的值强制变成0，进一步滤掉微小误差。在人为移动装置时候很难出现人为匀速运动的情况，故在加速度为0的时候，将速度置为0。
最后根据位置变化输出设备被人为移动报警信息。

#include <stdio.h>#include <stdlib.h>#include <unistd.h> #include <string.h>#include"libfahw.h"#define BUF_SIZE            32#define WEND_MAX            5#define PWEND_MAX           5#define WEND_CI             5void stringtoxyz(char st[],int *a,int *b,int *c);//基本思路 判断处于动态还是稳态 5次加速度值很接近表示稳态 稳态下累计计算g值 出现动态时以g值作为基本参考 计算二次积分 过程中计算稳态，稳态判定成功重新累计g值//计算平均值时加速度int ax2=0,ay2=0,az2=0;//重力加速度int gx=0,gy=0,gz=0;//累计位置信息int px=0,py=0,pz=0;//累计速度信息int vx=0,vy=0,vz=0;//前一次加速度信息int preax=0,preay=0,preaz=0;//前一次速度信息int prevx=0,prevy=0,prevz=0;//前一次位置信息int prepx=0,prepy=0,prepz=0;//稳定计数值int wendcnt=0;//在稳态5次之后重新矫正重力加速度void SteadystateCal(){    if((abs(preax-ax2)<=WEND_MAX&&abs(preay-ay2)<=WEND_MAX)&&abs(preaz-az2)<=WEND_MAX)    {        wendcnt++;        if(wendcnt==WEND_CI)        {            gx = ax2;            gy = ay2;            gz = az2;        }        else if(wendcnt>WEND_CI)        {            gx = (gx+ax2)/2;            gy = (gy+ay2)/2;            gz = (gz+az2)/2;        }    }    else     {        wendcnt=0;    }}int main (int argc, char* argv[]){    char position[BUF_SIZE];    int board;    int ax,ay,az;    int i;    int j=0;    if ((board = boardInit()) < 0) {        printf("Fail to init board\n");        return -1;    }    system("modprobe adxl34x");    system("modprobe adxl34x-i2c");    for(i=0;i<10;i++)    {        usleep(100*1000);        memset(position, 0, BUF_SIZE);        if (adxl34xRead(position) > 0)         {            stringtoxyz(position,&ax,&ay,&az);            //去掉重力加速度            ax2 = ax2+ax;            ay2 = ay2+ay;            az2 = az2+az;        }    }    gx = ax2/i;    gy = ay2/i;    gz = az2/i;    while(1)    {        ax2 = 0;        ay2 = 0;        az2 = 0;        for(i=0;i<10;i++)        {            usleep(100*1000);            memset(position, 0, BUF_SIZE);            if (adxl34xRead(position) > 0)             {                stringtoxyz(position,&ax,&ay,&az);                //去掉重力加速度                ax2 = ax2+ax;                ay2 = ay2+ay;                az2 = az2+az;            }        }        ax2 = ax2/10;        ay2 = ay2/10;        az2 = az2/10;        SteadystateCal();        //计算水平方向位移 时间为1s        if(wendcnt<WEND_CI)        {            if(abs(ax2-gx)<WEND_MAX)            {                ax2 = 0;            }            else            {                ax2 = ax2-gx;            }            if(abs(ay2-gy)<WEND_MAX)            {                ay2 = 0;            }            else            {                ay2 = ay2-gy;            }            if(abs(az2-gz)<WEND_MAX)            {                az2 = 0;            }            else            {                az2 = az2-gz;            }            if(ax2!=0)            {                vx = vx + preax + (ax2-preax)/2;                preax = ax2;                px = px + prevx + (vx-prevx)/2;                prevx = vx;            }            else            {                vx = 0;                preax = ax2;                prevx = vx;            }            if(ay2!=0)            {                vy = vy + preay + (ay2-preay)/2;                preay = ay2;                py = py + prevy + (vy-prevy)/2;                prevy = vy;            }                       else            {                vy = 0;                preay = ay2;                prevy = vy;            }            if(az2!=0)            {                vz = vz + preaz + (az2-preaz)/2;                preaz = az2;                pz = pz + prevz + (vz-prevz)/2;                prevz = vz;            }            else            {                vz = 0;                preaz = az2;                prevz = vz;            }               }        if(j>5)        {            if((abs(prepx-px)>PWEND_MAX||abs(prepy-py)>PWEND_MAX)||abs(prepz-pz)>PWEND_MAX)            {                //写入报警标记及报警级别                printf("position %d,%d,%d\n", px-prepx,py-prepy,pz-prepz);                prepx = px;                prepy = py;                prepz = pz;            }            j=0;        }        else        {            j++;        }    }    system("rmmod adxl34x-i2c");    system("rmmod adxl34x");    return 0;}void stringtoxyz(char st[],int *a,int *b,int *c){    int i,j=0,k;    //char st[30]="(78, 276, -78)";    char sa[10],sb[10],sc[10];    //sscanf("(78, 276, -78)","%s,%s,%s",sa,sb,sc);    k=0;    for(i=0;i<strlen(st);i++)    {        if(('0'<=st[i]&&st[i]<='9')||st[i]=='-')        {            if(k==0)            {                sa[j]=st[i];            }            else if(k==1)            {                sb[j]=st[i];            }            else if(k==2)            {                sc[j]=st[i];            }            j++;        }        else if(st[i]==','||st[i]==')')        {            if(k==0)            {                sa[j]='\0';            }            else if(k==1)            {                sb[j]='\0';            }            else if(k==2)            {                sc[j]='\0';            }            j=0;            k++;        }    }    *a = atoi(sa);    *b = atoi(sb);    *c = atoi(sc);}