利用加速度求解位置的算法——三轴传感器
时间：2023-09-05 07:37:00
转载的文章类似于自己做的车载项目，可以算是标记。
---------------------------------------分割线----------------------------
cposture
小白的技术成长之路
三轴传感器
http://cache.freescale.com/files/sensors/doc/app_note/AN3397.pdf?fsrch=1&sr=2
摘要
描述并使用本文档MMA7260QT三轴加速计和低功耗9S08QG实现求解位置算法的88位单片机 。 在当今先进的电子市场，有许多产品增加了许多功能和智能。定位和游戏只是获得位置信息的一部分市场。通过使用惯性传感器获取此信息的可选方案。由于加速度和位置之间没有直接转换，因此需要处理从这些传感器中获得的信号。 为了获得位置，需要对加速度进行二次积分。本文介绍了实现加速度的二重积分的简单算法。为了获得加速度的二重积分，一个简单的积分需要两次，因为也可以顺便获得速度。 接下来要显示的算法应该在任何传感轴上，因此可以计算出一维、二维和三维的位置。在获取三维位置信息时，应特别消除重力加速度的影响。下面的算法实现还包括二维系统(如鼠标)的例子。 应用潜力
该算法的应用潜力在于个人导航、汽车导航和（back-up）GPS、防盗设备，地图跟踪，3D游戏、计算机鼠标等。所有这些产品都需要算法来解决位置信息。
本文介绍的算法在位移精度要求不是很严格的情况下非常有用。当使用本算法时，需要考虑其他情况和影响，特别是应用程序。对最终程序进行微小的修改和调整，可以达到更高的精度。
理论知识与算法
理解本文算法的最好方法是回顾数学积分知识。
加速度是对象速度的变化率。同时，速度是同一对象位置的变化率。换句话说，速度是位置导数，加速度是速度导数，因此公式如下：
Ashampoo_Snap_2015.03.27_23h09m35s_001_.png
积分与导数相反。如果已知物体的加速度，我们可以使用二重积分来获得物体的位置。假设初始条件为0，则有以下公式：
Ashampoo_Snap_2015.03.27_23h13m58s_002_.png
理解这个公式的一种方法是将积分定义为曲线下包围的区域。积分计算结果为极小区域的总和，区域宽度接近0。换句话说，积分和表示物理变量的大小（速度）。
Ashampoo_Snap_2015.03.27_23h19m44s_003_.png
利用之前的概念-曲线下的区域，我们可以得出一个结论：对于一个信号采样，获得信号大小的瞬时值，从而在两个采样之间获得一个小区域。采样时间必须相同，才能获得连贯的值。采样时间代表该区域的宽度，采样得到的值代表该区域的高度。假设时间是一个单位，以消除分数乘法(微秒或毫秒)。 现在，我们知道每个代表区域宽度的采样时间等于1。下一个结论是： 积分的值可以约等于区域面积之和。 若采样时间接近0，则得出正确的结论。但在实践中，会出现以下错误，这种错误会在处理过程中积累。 Ashampoo_Snap_2015.03.27_23h50m46s_004_.png
这些错误被称为采样损失。为了减少这些错误，我们进一步假设。因此，该区域可以被视为两个小区域的组合：
Ashampoo_Snap_2015.03.27_23h57m54s_005_.png
区域1是前一次采样的值(方形)，区域2是三角形，是前一次采样与当前采样的一半。
通过这种方法，我们现在有一个阶近似(插值)信号。
Ashampoo_Snap_2015.03.28_00h06m26s_006_.png
现在的错误比以前的近似错误低得多。
尽管加速度有正有负，但采样的值总是正的（基于MMA7260QT因此，需要做出偏移判断，换句话说，需要参考。这个程序是校准程序。
校准程序用于未移动的加速度值。此时，获得的加速度值可视为零参考点。低于零参考点的值代表负值（减速），高于零参考点的值代表正值（加速）。
加速度计的输出范围为0v到Vdd，而且它通常是由AD转换器换器。0值接近Vdd/2。前面获得的校准值会被芯片的方向和分解在各轴的静态加速度（重力加速度）所影响。如果装置刚好平行于地球表面，那么校准值将会很接近Vdd/2。
下图用于显示校准程序的结果。
Ashampoo_Snap_2015.03.28_16h59m36s_001_.png
从采样的信号减去零参考值，我们获得真正的采样加速度。 Ashampoo_Snap_2015.03.28_17h02m55s_002_.png
A1代表正加速度，A2代表负加速度。 假如我们将这些数据视为采样数据，那么信号值将非常接近下图。 Ashampoo_Snap_2015.03.28_17h06m01s_003_.png
使用上述积分公式，Formula 1.我们将获得速度的比例近似值。同样，为了获得位置，我们需要再次得分。同样的公式和步骤应用于获得的速度值，我们现在获得了瞬时位置的比例近似值。（见图6） Ashampoo_Snap_2015.03.28_17h15m21s_004_.png 软件设计相关注意事项
在实现现实世界时，应考虑以下步骤和建议。
信号有一定的噪声，因此信号必须通过数字滤波器。该算法中的滤波器是一种移动平均算法，需要处理的值是一定数量采样值的平均结果。
即使以前过滤过的一些数据可能是由机械噪声引起的，也必须实现另一个过滤器。根据过滤样品的数量，可以选择真正的加速窗口（通常是16±2采样次数)。
无运动状态对于获取正确的数据至关重要。校准程序需要在应用程序的开始。校准值必须尽可能准确。
加速度的真实值等于采样值减去校准值；它可以是正的或负的。当你定义变量时，它永远不会被忽需要定义为符号数。
采样频率更快意味着结果更准确，因为采样频率越快，误差越小。但需要更多的内存、时间和硬件考虑。
两次采样之间的时间必须相同。如果时间不同，就会出错。
两次采样结果之间的线性近似值(插值)被推荐用于更准确的结果。
代码注释
-
校准程序
由于重力加速度(静态加速度)，该校准程序去除了加速度传感器的输出偏移分量。
校准程序在加速度计处于无运动状态时，对加速度求平均值。采样的数量越多，加速度的校准结果越精确。
复制代码
1 voidCalibrate(void)
2 {
3 unsignedint count1;
4 count1 =0;
5 do{
6 ADC_GetAllAxis();
7 sstatex = sstatex Sample_X;// Accumulate Samples
8 sstatey = sstatey Sample_Y;
9 count1 ;
10 }while(count1!=0x0400);// 1024 times
11 sstatex=sstatex>>10;// division between 1024
12 sstatey=sstatey>>10;
13 }
复制代码 -
滤波
低通滤波器是消除加速度计中信号噪声（包括机械和电子）的好方法。为了减少信号积分时的大部分错误，降低噪声对定位应用至关重要。
一个简单的低过滤采样信号是执行滚动平均值。过滤简单地获得一组采样的平均值，并获得稳定采样总数的平均值是非常重要的。采样过多可能导致数据丢失，结果不准确。
复制代码
1 do{
2 accelerationx[1]=accelerationx[1] Sample_X;//filtering routine for noise attenuation
3 accelerationy[1]=accelerationy[1] Sample_Y;//64 samples are averaged. The resulting
4 count2 ;// average represents the acceleration of
5 // an instant.
6 }while(count2!=0x40);// 64 sums of the acceleration sample
7 accelerationx[1]= accelerationx[1]>>6;// division by 64
8 accelerationy[1]= accelerationy[1]>>6;
复制代码 -
机械滤波窗
Ashampoo_Snap_2015.03.28_23h31m51s_005_.png
当处于无运动状态时，加速度上的小错误可能被视为常量速度，因为采样值加和后不等于0。理想情况下，所有采样值为0。常速表示连续运动和不稳定位置。
即使之前过滤过的一些数据可能不正确，一个用于区分非运动状态"有效数据"和"无效数据"必须实现窗口。
1 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied to
2 {
3 accelerationx[1] = 0;
4 } // the X axis acceleration variable -
定位
二重积分是利用加速度数据获取位置所需要的步骤。第一次积分获得速度，第二次积分获得位置。
Ashampoo_Snap_2015.03.28_23h38m26s_007_.png
1 //first integration
2 velocityx[1]= velocityx[0]+ accelerationx[0]+((accelerationx[1]- accelerationx[0])>>1)
3 //second integration
4 positionX[1]= positionX[0]+ velocityx[0]+((velocityx[1]- velocityx[0])>>1); -
数据传输
这部分主要目的是用于调试和显示；这个函数中32位的结果被切割分4个8位数据。
复制代码
1 if (positionX[1]>=0)
2 { //This line compares the sign of the X direction data
3 direction= (direction | 0x10); // if its positive the most significant byte
4 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
5 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
6 // subsequent lines in order to be sent.
7 posx_seg[2]= (positionX[1]>>16) & 0x000000FF;// The 32 bit variable must be split into
8 posx_seg[3]= (positionX[1]>>24) & 0x000000FF;// 4 different 8 bit variables in order to
9 // be sent via the 8 bit SCI frame
10 }
11 else
12 {
13 direction=(direction | 0x80);
14 positionXbkp=positionX[1]-1;
15 positionXbkp=positionXbkp^0xFFFFFFFF;
16 posx_seg[0]= positionXbkp & 0x000000FF;
17 posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
18 posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
19 posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
20 }
复制代码 -
"移动结束"检查
基于积分表示曲线下方区域的概念，速度是加速曲线下方的区域的结果。
如果我们观察一个典型的移动:一个物体沿一个轴从点A移动到点B，一个典型的加速度结果如下图:
Ashampoo_Snap_2015.03.28_23h57m09s_008_.png
观察上面的图，加速度先增加后减少直到速度达到最大值（加速度始终为正代表往同一方向加速）。然后以相反的方式加速，直到它再次到达0。在这一点上达到一个稳定的位移和新的位置。
在真实世界中，其中曲线正侧下方的区域面积不等于负侧上方的区域面积，积分结果将永远不会达到零速度，因此将是一个倾斜定位（从未稳定）。
正因为如此，将速度强制减为0非常关键。这是通过不断读取加速度值和0进行比较而实现的。如果在一定数量的采样中，这种情况存在（sample==0）的话，速度将简单地返回为0。
复制代码
1 if (accelerationx[1]==0) // we count the number of acceleration samples that equals zero
2 {
3 countx++;
4 }
5 else
6 {
7 countx =0;
8 }
9 if (countx>=25) // if this number exceeds 25, we can assume that velocity is zero
10 {
11 velocityx[1]=0;
12 velocityx[0]=0;
13 }
复制代码
Ashampoo_Snap_2015.03.29_00h24m36s_010_.png
- 源代码
复制代码
1 #include
2 #include “derivative.h”
3 #include “adc.h”
4 #include “buzzer.h”
5 #include “SCItx.h”
6 #pragma DATA_SEG MY_ZEROPAGE
7 unsigned char near Sample_X;
8 unsigned char near Sample_Y;
9 unsigned char near Sample_Z;
10 unsigned char near Sensor_Data[8];
11 unsigned char near countx,county ;
12 signed int near accelerationx[2], accelerationy[2];
13 signed long near velocityx[2], velocityy[2];
14 signed long near positionX[2];
15 signed long near positionY[2];
16 signed long near positionZ[2];
17 unsigned char near direction;
18 unsigned long near sstatex,sstatey;
19 #pragma DATA_SEG DEFAULT
20 void init(void);
21 void Calibrate(void);
22 void data_transfer(void);
23 void concatenate_data(void);
24 void movement_end_check(void);
25 void position(void);
26 void main (void)
27 {
28 init();
29 get_threshold();
30 do
31 {
32 position();
33 }while(1);
34 }
35 /*******************************************************************************
36 The purpose of the calibration routine is to obtain the value of the reference threshold.
37 It consists on a 1024 samples average in no-movement condition.
38 /
39 void Calibrate(void)
40 {
41 unsigned int count1;
42 count1 = 0;
43 do{
44 ADC_GetAllAxis();
45 sstatex = sstatex + Sample_X; // Accumulate Samples
46 sstatey = sstatey + Sample_Y;
47 count1++;
48 }while(count1!=0x0400); // 1024 times
49 sstatex=sstatex>>10; // division between 1024
50 sstatey=sstatey>>10;
51 }
52 //
53 /*********************************************************************************
54 This function obtains magnitude and direction
55 In this particular protocol direction and magnitude are sent in separate variables.
56 Management can be done in many other different ways.
57 /
58 void data_transfer(void)
59 {
60 signed long positionXbkp;
61 signed long positionYbkp;
62 unsigned int delay;
63 unsigned char posx_seg[4], posy_seg[4];
64 if (positionX[1]>=0) { //This line compares the sign of the X direction data
65 direction= (direction | 0x10); //if its positive the most significant byte
66 posx_seg[0]= positionX[1] & 0x000000FF; // is set to 1 else it is set to 8
67 posx_seg[1]= (positionX[1]>>8) & 0x000000FF; // the data is also managed in the
68 // subsequent lines in order to
69 posx_seg[2]= (positionX[1]>>16) & 0x000000FF; // be sent. The 32 bit variable must be
70 posx_seg[3]= (positionX[1]>>24) & 0x000000FF; // split into 4 different 8 bit
71 // variables in order to be sent via
72 // the 8 bit SCI frame
73 }
74 else {direction=(direction | 0x80);
75 positionXbkp=positionX[1]-1;
76 positionXbkp=positionXbkp^0xFFFFFFFF;
77 posx_seg[0]= positionXbkp & 0x000000FF;
78 posx_seg[1]= (positionXbkp>>8) & 0x000000FF;
79 posx_seg[2]= (positionXbkp>>16) & 0x000000FF;
80 posx_seg[3]= (positionXbkp>>24) & 0x000000FF;
81 }
82 if (positionY[1]>=0) { // Same management than in the previous case
83 direction= (direction | 0x08); // but with the Y data.
84 posy_seg[0]= positionY[1] & 0x000000FF;
85 posy_seg[1]= (positionY[1]>>8) & 0x000000FF;
86 posy_seg[2]= (positionY[1]>>16) & 0x000000FF;
87 posy_seg[3]= (positionY[1]>>24) & 0x000000FF;
88 }
89 else {direction= (direction | 0x01);
90 positionYbkp=positionY[1]-1;
91 positionYbkp=positionYbkp^0xFFFFFFFF;
92 posy_seg[0]= positionYbkp & 0x000000FF;
93 posy_seg[1]= (positionYbkp>>8) & 0x000000FF;
94 posy_seg[2]= (positionYbkp>>16) & 0x000000FF;
95 posy_seg[3]= (positionYbkp>>24) & 0x000000FF;
96 }
97 delay = 0x0100;
98 Sensor_Data[0] = 0x03;
99 Sensor_Data[1] = direction;
100 Sensor_Data[2] = posx_seg[3];
101 Sensor_Data[3] = posy_seg[3];
102 Sensor_Data[4] = 0x01;
103 Sensor_Data[5] = 0x01;
104 Sensor_Data[6] = END_OF_FRAME;
105 while (–delay);
106 SCITxMsg(Sensor_Data); // Data transferring function
107 while (SCIC2 & 0x08);
108 }
109 //
110 /******************************************************************************************
111 This function returns data format to its original state. When obtaining the magnitude and
112 direction of the position, an inverse two’s complement is made. This function makes the two’s
113 complement in order to return the data to it original state.
114 It is important to notice that the sensibility adjustment is greatly impacted here, the amount
115 of “ones” inserted in the mask must be equivalent to the “ones” lost in the shifting made in
116 the previous function upon the sensibility modification.
117 /
118 void data_reintegration(void)
119 {
120 if (direction >=10)
121 {positionX[1]= positionX[1]|0xFFFFC000;} // 18 “ones” inserted. Same size as the
122 //amount of shifts
123 direction = direction & 0x01;
124 if (direction ==1)
125 {positionY[1]= positionY[1]|0xFFFFC000;}
126 }
127 /
128 This function allows movement end detection. If a certain number of acceleration samples are
129 equal to zero we can assume movement has stopped. Accumulated Error generated in the velocity
130 calculations is eliminated by resetting the velocity variables. This stops position increment
131 and greatly eliminates position error.
132 /
133 void movement_end_check(void)
134 {
135 if (accelerationx[1]==0) //we count the number of acceleration samples that equals cero
136 { countx++;}
137 else { countx =0;}
138 if (countx>=25) //if this number exceeds 25, we can assume that velocity is cero
139 {
140 velocityx[1]=0;
141 velocityx[0]=0;
142 }
143 if (accelerationy[1]==0) //we do the same for the Y axis
144 { county++;}
145 else { county =0;}
146 if (county>=25)
147 {
148 velocityy[1]=0;
149 velocityy[0]=0;
150 }
151 }
152 //
153 /*****************************************************************************************
154 This function transforms acceleration to a proportional position by integrating the
155 acceleration data twice. It also adjusts sensibility by multiplying the “positionX” and
156 “positionY” variables.
157 This integration algorithm carries error, which is compensated in the “movenemt_end_check”
158 subroutine. Faster sampling frequency implies less error but requires more memory. Keep in
159 mind that the same process is applied to the X and Y axis.
160 /
161 void position(void)
162 {
163 unsigned char count2 ;
164 count2=0;
165 do{
166 ADC_GetAllAxis();
167 accelerationx[1]=accelerationx[1] + Sample_X; //filtering routine for noise attenuation
168 accelerationy[1]=accelerationy[1] + Sample_Y; //64 samples are averaged. The resulting
169 //average represents the acceleration of
170 //an instant
171 count2++;
172 }while (count2!=0x40); // 64 sums of the acceleration sample
173 accelerationx[1]= accelerationx[1]>>6; // division by 64
174 accelerationy[1]= accelerationy[1]>>6;
175 accelerationx[1] = accelerationx[1] - (int)sstatex; //eliminating zero reference
176 //offset of the acceleration data
177 accelerationy[1] = accelerationy[1] - (int)sstatey; // to obtain positive and negative
178 //acceleration
179 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied
180 {accelerationx[1] = 0;} // to the X axis acceleration
181 //variable
182 if ((accelerationy[1] <=3)&&(accelerationy[1] >= -3))
183 {accelerationy[1] = 0;}
184 //first X integration:
185 velocityx[1]= velocityx[0]+ accelerationx[0]+ ((accelerationx[1] -accelerationx[0])>>1);
186 //second X integration:
187 positionX[1]= positionX[0] + velocityx[0] + ((velocityx[1] - velocityx[0])>>1);
188 //first Y integration:
189 velocityy[1] = velocityy[0] + accelerationy[0] + ((accelerationy[1] -accelerationy[0])>>1);
190 //second Y integration:
191 positionY[1] = positionY[0] + velocityy[0] + ((velocityy[1] - velocityy[0])>>1);
192 accelerationx[0] = accelerationx[1]; //The current acceleration value must be sent
193 //to the previous acceleration
194 accelerationy[0] = accelerationy[1]; //variable in order to introduce the new
195 //acceleration value.
196 velocityx[0] = velocityx[1]; //Same done for the velocity variable
197 velocityy[0] = velocityy[1];
198 positionX[1] = positionX[1]<<18; //The idea behind this shifting (multiplication)
199 //is a sensibility adjustment.
200 positionY[1] = positionY[1]<<18; //Some applications require adjustments to a
201 //particular situation
202 //i.e. mouse application
203 data_transfer();
204 positionX[1] = positionX[1]>>18; //once the variables are sent them must return to
205 positionY[1] = positionY[1]>>18; //their original state
206 movement_end_check();
207 positionX[0] = positionX[1]; //actual position data must be sent to the
208 positionY[0] = positionY[1]; //previous position
209 direction = 0; // data variable to direction variable reset
210 }
211 //
复制代码
原理图
Ashampoo_Snap_2015.03.29_00h29m01s_011_.png
总结
本文档为利用加速度计实现定位算法提供了一些基本的概念。
这种特定的算法在对位移精度要求不是很严格中非常有用。其他注意事项和具体的应用程序的影响应该在实现这个示例时被考虑。
这种积分算法适合于低端嵌入式应用，因为它的简单性和少量的指令。它也没有涉及任何浮点运算。
原文链接：http://cache.freescale.com/files/sensors/doc/app_note/AN3397.pdf?fsrch=1&sr=2
本文链接：http://www.cnblogs.com/cposture/p/4378922.html
