使用加速器实现的定位算法
英语和数学都不咋地，翻译不对的地方请指正
原文地址：http://www.freescale.com/files/sensors/doc/app_note/AN3397.pdf
本文地址: http://blog.csdn.net/wzq9706/article/details/7429611
这份文档描述和实现了一个基于"MMA7260QT"3轴加速度器和低成本的"9S08QG9"的微控制器单元的定位算法(MCU)。
当今天高科技电子市场，有许多功能产品正不断的增加新的功能和信息，市面上有一些追踪和游戏产品可以获得一些定位信息，一种可选的方法是通过使用惯性传感器，从传感器获得的信号是需要经过处理，因为在加速和坐标之间没有直接的转换。
为了获得双重积分，必须应用信号。这份文档描述了 一个简单的算法用于实现一个基于使用8字节微控制系统传感器获得信号的双重积分。为了获得双重积分，一个简单的积分必须处理两次，这可以很好地获得速率(velocity)信息。
以上展示的算法适用于任何的传感轴;1,2,3轴都可以定位。当实现一个三轴定位时,需要额外地处理地球的空重力效果。以下的实现还包含一个2轴系统的例子(基于加速度计的鼠标)
潜在的应用:
这类算法的潜在应用有个人导航、汽车导航、备份GPS、防盗装置、地图追踪、3D游戏、PC鼠标。。。还有很多，这类的应用都可以从定位算法中得到好处。
本文档中所述的情况下，该算法是有用的，位移精度是非常关键。适用本例的具体应用应考虑其它方面的注意事项和考虑。做适当的修改和适配，可以让这个算法在最终的应用中达到更高的精度。
巩固理论和算法:
最好的方法是理解这个算法与数学微积分的关系。加速度是物体速度变化的一个比率。速率是相同对象在同一时刻的位置变化率。换句话说，速率是位置的导数,而加速度又是速率的导数。因此:
积分是导数反操作,如果已知一个物体的加速度,使用双重积分我们可以得到坐标数据
理解这个公式一个方法是定义一个像下面这样的积分区域曲线图，每个位置的积分都是一些宽度几乎为0的很小的区域相加的结果。也就是说，积分的和是一个物理值的量级描述。
以上的关于"区域曲线图"的概念，我们可以这样简化:采样信号得到量级的时时数值，一个小的区域可以建立在两个样本之间,为了建立一组连续的值，采样的时间必须保持一致。采样的时间用来表示该区域的宽，同时采样的值表示高。为了消除乘法计算（毫秒，微秒）中涉及浮点型的运算，我们假设时间为单位时间（uint）。
现在我们知道每一个样本表示一个宽度为1的区域。再简化：将积分值简化为样本的和。如果采样的时间趋于0，那么这个假设是成立的，真实情况中，会产生一定偏差，如图1所示。这个偏差是时间不断累积的过程。
这些偏差被称为抽样损耗，为了减少这种偏差，我们再做进一步的假设：将一个区域看作是两个更小的区域相结合所产生的区域：
第一个区域是前一个采样的值/高(一个矩形)，第二个区域是一个三角形，它的取当前的样本(样本 n)高减前一个样本（样本 n-1）高然后除以2
用这种方法，我们现在有一阶近似信号（插值）
现在的偏差比以前低得多。
然而，不能为“现实世界”太精确的数据。即使加速度可以是正数或负数，但样本总是为正(MMA7260QT的输出特性)；因此，必须做一个偏移调整。换句话说，参考是必要的。这个功能被定义为校准。
对加速度计进行校准时，有没有运动条件。低于参考值代表负值（减速），更大于参考值代表正值（加速）。加速度计输出范围从0V到VDD，它通常由模拟信号向数字信号（A/D转换）。零值是靠近VDD / 2。每个轴的校准值都将受到水平方向的力和静态加速度（地球引力）的影响。如果模块是完全平行于地球表面，校准值应该非常接近VDD / 2。下图显示的校准例程的结果。
信号采样减去0参考，我们就能得到真实的加速度样本。
A1代表正加速度，A2代表负加速器
如果我们考虑一个数据为采样数据，那么需要像下面这样将模拟信号转化成数字信号。
通过应用积分公式（公式1），我们得到一个比较近似的速率。为了获得位置，积分运算必须再次执行，通过相同的公式和步骤来得到速率的数据，这样我们就得到一个瞬时位置的比例逼近（看图6）
第三页
软件设计注意事项：
这种算法在“现实世界”中实施时，应考虑以下的步骤和建议。
1.信号不无噪音，因此必须进行数字滤波。在该算法中使用的过滤器是一种移动平均线;要处理的值是平均一定数量的样品的结果。
2.实际上以前的过滤一些数据可能因为“机械”噪音而产生错误，所以还得实现一个过滤器。窗口"real acceleration"可以根据过滤的样品的数量进行选择(通常为16个左右样本的平均值)
3.一个"非移动"状态是获得正确数据的关键.校正程序要在应用的最开始的地方。校正的值应尽可能的精确。
4.真实的加速度值是样本值减去校正值；结果可能是正数也可能是负数，声明变量时不要忽略这一点(符号)
5.更高的采样率由于偏差的减少，所以意味着精度的提高。这需要考虑更多的内存，时间调度，和硬件的问题。
6.两个样本之间的时间差必须总是相同，否则就会产生错误。
7.建议用样本间的线性近似值(插值)来获得更高的精度
代码说明
校正:
这部分代码消除了由于地球重力(静态加速)造成的加速偏差,当加速器处于非运动状态时，校正例程对样本进得平均。获得的样本越多，精度也就越高。
void Calibrate(void)
{
unsigned int count1;
count1 = 0;
do{
ADC_GetAllAxis();
sstatex = sstatex + Sample_X; // Accumulate Samples
sstatey = sstatey + Sample_Y;
count1++;
}while(count1!=0x0400); // 1024 times
sstatex=sstatex>>10; // division between 1024
sstatey=sstatey>>10;
}过滤:
低通滤波过滤是消除加速度计中信号噪音(包括机械的和电子的)很好的方法。降低噪音是定位应用中积分信号时减少重大错误的关键。
一个简单的低通过滤采样信号的方式是执行滚动平均值。过滤简单地获得一组样本的平均值，获得稳定的样本总数的平均值是很重要的。样本数太多可能造成数据丢失，而太少又可能导致结果不精确。
do{
ADC_GetAllAxis();
accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation
accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
 //average represents the acceleration of
//an instant
count2++;
}while (count2!=0x40); // 64 sums of the acceleration sample
accelerationx[1]= accelerationx[1]>>6; // division by 64
accelerationy[1]= accelerationy[1]>>6;机械过滤窗口:
当存在非移动状态，加速中小的偏差可能被解释为一个恒定的加速度，这是由于样本是非0累加的事实；理想的非移动状况是所有的样本都为0，恒定速率表明一个连续运动的状态和一个不稳定的位置。即使之前过滤的一些数据可能不正确，因此一个用于区别非运动状态的"有效数据"和"无效数据"的窗口必须实现。
if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied to
{accelerationx[1] = 0;} // the X axis acceleration variable
定位:
双重积分的执行需要在加速度数据中得到位置信息，积分的步骤一定要执行一次获得速率然后重复获得位置。
//first integration velocityx[1] = velocityx[0] + accelerationx[0] + ((accelerationx[1] - accelerationx[0])>>1) //second integration positionX[1] = positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
数据传输:
这部分主要目的是用于调试和显示；这个函数中32位的结果被切割分4个8位数据。
if (positionX[1]>=0) { //This line compares the sign of the X direction data
direction= (direction | 0x10); // if its positive the most significant byte
posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
// subsequent lines in order to be sent.
posx_seg[2]= (positionX[1]>>16) & 0x000000FF;// The 32 bit variable must be split into
posx_seg[3]= (positionX[1]>>24) & 0x000000FF;// 4 different 8 bit variables in order to
// be sent via the 8 bit SCI frame
} else {
direction=(direction | 0x80);
positionXbkp=positionX[1]-1;
positionXbkp=positionXbkp^0xFFFFFFFF;
posx_seg[0]= positionXbkp & 0x000000FF;
posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
}"移动结束"检查:
下面曲线图中区域扫绘的积分的概念的基础上，速率是曲线图中区域的结果。
如果我们观察一个典型的移动:一个物体延一个轴从点A移动到点B，一个典型的加速度结果如下图:
观察上图,从一个初始加速度或减速度到一个最大的速率，然后以相反的方式加速，直到它再次到达0，在这一点上达到一个稳定的位移和一个新的位置。
在真实世界中，其中正曲线以下部分不会与负曲线以上的部分相同，积分的结果不会达到0速度，因此将是一个倾斜的定位（很稳定）。正因为如此,"迫使"速率下降到0至关重要.这是将不断读到的加速度和0进行比较。如果在样本中存在这种状况(sample == 0), 速率只是简单的返回0。
if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
{ countx++;}
else { countx =0;}
if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
{
velocityx[1]=0;
velocityx[0]=0;
}
源码:
#include <hidef.h>
#include "derivative.h"
#include "adc.h"
#include "buzzer.h"
#include "SCItx.h"
#pragma DATA_SEG MY_ZEROPAGE
unsigned char near Sample_X;
unsigned char near Sample_Y;
unsigned char near Sample_Z;
unsigned char near Sensor_Data[8];
unsigned char near countx,county ;
signed int near accelerationx[2], accelerationy[2];
signed long near velocityx[2], velocityy[2];
signed long near positionX[2];
signed long near positionY[2];
signed long near positionZ[2];
unsigned char near direction;
unsigned long near sstatex,sstatey;
#pragma DATA_SEG DEFAULT
void init(void);
void Calibrate(void);
void data_transfer(void);
void concatenate_data(void);
void movement_end_check(void);
void position(void);
void main (void)
{
init();
get_threshold();
do
{
position();
}while(1);
}
/*******************************************************************************
The purpose of the calibration routine is to obtain the value of the reference threshold.
It consists on a 1024 samples average in no-movement condition.
********************************************************************************/
void Calibrate(void)
{
unsigned int count1;
count1 = 0;
do{
ADC_GetAllAxis();
sstatex = sstatex + Sample_X; // Accumulate Samples
sstatey = sstatey + Sample_Y;
count1++;
}while(count1!=0x0400); // 1024 times
sstatex=sstatex>>10; // division between 1024
sstatey=sstatey>>10;
}
/*****************************************************************************************/
/******************************************************************************************
This function obtains magnitude and direction
In this particular protocol direction and magnitude are sent in separate variables.
Management can be done in many other different ways.
*****************************************************************************************/
void data_transfer(void)
{
signed long positionXbkp;
signed long positionYbkp;
unsigned int delay;
unsigned char posx_seg[4], posy_seg[4];
if (positionX[1]>=0) { //This line compares the sign of the X direction data
direction= (direction | 0x10); //if its positive the most significant byte
posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the subsequent lines in order to
posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // be sent. The 32 bit variable must be
posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // split into 4 different 8 bit
// variables in order to be sent via
// the 8 bit SCI frame
}
else {direction=(direction | 0x80);
positionXbkp=positionX[1]-1;
positionXbkp=positionXbkp^0xFFFFFFFF;
posx_seg[0]= positionXbkp & 0x000000FF;
posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
}
if (positionY[1]>=0) { // Same management than in the previous case
direction= (direction | 0x08); // but with the Y data.
posy_seg[0]= positionY[1] & 0x000000FF;
posy_seg[1]= (positionY[1]>>8) & 0x000000FF;
posy_seg[2]= (positionY[1]>>16) & 0x000000FF;
posy_seg[3]= (positionY[1]>>24) & 0x000000FF;
}
else {direction= (direction | 0x01);
positionYbkp=positionY[1]-1;
positionYbkp=positionYbkp^0xFFFFFFFF;
posy_seg[0]= positionYbkp & 0x000000FF;
posy_seg[1]= (positionYbkp>>8) & 0x000000FF;
posy_seg[2]= (positionYbkp>>16) & 0x000000FF;
posy_seg[3]= (positionYbkp>>24) & 0x000000FF;
}
delay = 0x0100;
Sensor_Data[0] = 0x03;
Sensor_Data[1] = direction;
Sensor_Data[2] = posx_seg[3];
Sensor_Data[3] = posy_seg[3];
Sensor_Data[4] = 0x01;
Sensor_Data[5] = 0x01;
Sensor_Data[6] = END_OF_FRAME;
while (--delay);
SCITxMsg(Sensor_Data); // Data transferring function
while (SCIC2 & 0x08);
}
/*****************************************************************************************/
/******************************************************************************************
This function returns data format to its original state. When obtaining the magnitude and
direction of the position, an inverse two's complement is made. This function makes the two's
complement in order to return the data to it original state.
It is important to notice that the sensibility adjustment is greatly impacted here, the amount
of "ones" inserted in the mask must be equivalent to the "ones" lost in the shifting made in
the previous function upon the sensibility modification.
******************************************************************************************/
void data_reintegration(void)
{
if (direction >=10)
 {positionX[1]= positionX[1]|0xFFFFC000;} // 18 "ones" inserted. Same size as the amount of shifts
direction = direction & 0x01;
if (direction ==1)
{positionY[1]= positionY[1]|0xFFFFC000;}
}
/******************************************************************************************
This function allows movement end detection. If a certain number of acceleration samples are
equal to zero we can assume movement has stopped. Accumulated Error generated in the velocity
calculations is eliminated by resetting the velocity variables. This stops position increment
and greatly eliminates position error.
******************************************************************************************/
void movement_end_check(void)
{
if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
{ countx++;}
else { countx =0;}
if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
{
velocityx[1]=0;
velocityx[0]=0;
}
if (accelerationy[1]==0) //we do the same for the Y axis
{ county++;}
else { county =0;}
if (county>=25)
{
velocityy[1]=0;
velocityy[0]=0;
}
}
/*****************************************************************************************/
/******************************************************************************************
This function transforms acceleration to a proportional position by integrating the
acceleration data twice. It also adjusts sensibility by multiplying the "positionX" and
"positionY" variables.
This integration algorithm carries error, which is compensated in the "movenemt_end_check"
subroutine. Faster sampling frequency implies less error but requires more memory. Keep in
mind that the same process is applied to the X and Y axis.
*****************************************************************************************/
void position(void)
{
unsigned char count2 ;
count2=0;
do{
ADC_GetAllAxis();
accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation
accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
 //average represents the acceleration of
//an instant
count2++;
}while (count2!=0x40); // 64 sums of the acceleration sample
accelerationx[1]= accelerationx[1]>>6; // division by 64
accelerationy[1]= accelerationy[1]>>6;
accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference offset of the acceleration data
accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative acceleration
if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied
{accelerationx[1] = 0;} // to the X axis acceleration variable
if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3))
{accelerationy[1] = 0;}
//first X integration:
velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0])>>1);
//second X integration:
positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
//first Y integration:
velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1);
//second Y integration:
positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1);
accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent to the previous acceleration
accelerationy[0] = accelerationy[1]; //variable in order to introduce the new acceleration value.
velocityx[0] = velocityx[1]; //Same done for the velocity variable
velocityy[0] = velocityy[1];
positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication) is a sensibility adjustment.
positionY[1] = positionY[1]<<18; //Some applications require adjustments to a particular situation
//i.e. mouse application
data_transfer();
positionX[1] = positionX[1]>>18; //once the variables are sent them must return to
positionY[1] = positionY[1]>>18; //their original state
movement_end_check();
positionX[0] = positionX[1]; //actual position data must be sent to the
positionY[0] = positionY[1]; //previous position
direction = 0; // data variable to direction variable reset
}
/*****************************************************************************************/
