加速度計求二次積分

阿新 • • 發佈：2019-02-05

摘要

此文件描述並使用MMA7260QT三軸加速計和低功耗的9S08QG8八位微控制器實現求解位置的演算法。
在今天先進的電子市場，有不少增加了許多特性和智慧的多功能的產品。定位和遊戲只是得益於獲取到的位置資訊的一部分市場。一個獲取這種資訊的可選方案是通過使用慣性感測器。從這些感測器中取得的訊號需要進行一些處理，因為在加速度和位置之間沒有一種直接轉換。
為了獲得位置，需要對加速度進行二次積分。本文介紹一種簡單的演算法實現加速度的二重積分。為了獲取加速度的二重積分，一個簡單的積分要進行兩次，因為這樣也可以順便獲取速度。
接下來要展示的演算法，能夠應該於任何感測軸，所以一維、二維、三維的位置都可以被計算出。當在獲取三維位置資訊時，要特別地除去重力加速度的影響。下面的演算法實現還包括了一個二維繫統的例子（比如滑鼠）。

應用潛力
這種演算法的應用潛力在於個人導航、汽車導航和（back-up）GPS、防盜裝置、地圖追蹤、3D遊戲、計算機滑鼠等等。這類產品都需要用到求解位置資訊的演算法。
本文所介紹的演算法在位移精度要求不是很嚴格的情況下很有用。其他的情況和影響特別是應用，當採用本文演算法時，需要考慮一下。對最終程式進行微小的修改和調整，這種演算法能夠達到更高的精度。

理論知識和演算法
理解本文演算法的最好方法是回顧一下數學上的積分知識。
加速度是一個物件速度的變化速率。同時，速度是同樣一個物件位置的變化速率。換句話說，速度是位置的導數，加速度是速度的導數，因此如下公式：

積分和導數相反。如果一個物體的加速度已知，那麼我們能夠利用二重積分獲得物體的位置。假設初始條件為0，那麼有如下公式：

一個理解這個公式的方法是將積分定義成曲線下面包圍的區域，積分運算結果是極小區域的總和，區域的寬度趨近於0。換句話說，積分的和表示了一個物理變數的大小（速度）。

利用前面的一個概念——曲線下面的區域，我們能得出一個結論：對一個訊號取樣，得到該訊號大小的瞬時值，所以能夠在兩次取樣之間得到一個小的區域。為了獲得連貫的值取樣時間必須相同。取樣時間代表這塊區域的寬，同時取樣得到的值代表區域的高。為了消除帶有分數的乘法（微秒或毫秒），我們假定時間為一個單位。
現在，我們知道了每個代表區域寬度的取樣時間等於1。下一個結論是：
積分的值可以約等於區域面積之和。
如果取樣時間趨近於0，那麼結論將是正確的。但在實際中，將會產生如下錯誤，在處理的過程中，這個誤差將會一直累積。

這些錯誤稱為取樣損失。為了減少這些錯誤，我們再做進一步的假設。結果區域能夠看成由兩塊小的區域的組合：

區域1是前一次取樣的值（方形），區域2是一個三角形，是前一次取樣和當前取樣之差的一半。
通過這種方法，我們現在有一個一階近似（插值）的訊號。

現在的錯誤比以前的近似的低得多。
儘管加速度有正有負，但取樣的值總是正的（基於MMA7260QT的輸出特性），因此需要做一個偏移判斷，換句話說，需要一個參考。這個程式即為校準程式。
校準程式用於在沒有移動情況下的加速度值上。這時，獲得的加速度值可以看成是零參考點。低於零參考點的值代表負值（減速），高於零參考點的值代表正值（加速）。

加速度計的輸出範圍為0v到Vdd，並且它通常由AD轉換器得到。0值接近Vdd/2。前面獲得的校準值會被晶片的方向和分解在各軸的靜態加速度（重力加速度）所影響。如果裝置剛好平行於地球表面，那麼校準值將會很接近Vdd/2。

接下來的這張圖用於展示校準程式的結果。

從取樣的訊號減去零參考值，我們獲得真正的取樣加速度。

A1代表正加速度，A2代表負加速度。
如果我們將這資料看作是取樣資料，那麼訊號值將和下圖非常接近。

通過使用上面的積分公式，Formula 1，我們將獲得速度的比例近似值。同樣，為了獲取位置，需要再進行一次積分。在獲得的速度數值上應用相同的公式和步驟，我們現在獲得了一個瞬時位置的比例近似值。（見圖6）

軟體設計相關注意事項
當在現實世界的實現中應用這種演算法，應該考慮一下下面的步驟和建議。

訊號存在一定的噪聲，所以訊號必須經過數字濾波。本演算法中的濾波是一種移動平均演算法，要處理的值是一定數量取樣值的平均結果。
即使以前過濾的一些資料可能由於機械噪聲導致錯誤，所以必須實現另一個濾波器。根據過濾的樣品數，一個真實加速度的視窗能夠被選擇（一般為16±2取樣次數）。
無運動狀態對獲得正確的資料是至關重要的。校準例程需要在應用程式的開頭。該校準值必須儘可能準確。
加速度的真實值等於取樣值減去校準值；它可以是正數或負數。當你在定義變數的時候，這絕對不能被忽略，需要定義為有符號數。
更快的取樣頻率意味著更精確的結果，因為取樣頻率越快誤差越少。但是需要更多的記憶體、時間和硬體方面的考慮。
兩次取樣之間的時間必須要相同。如果這個時間不相同，將會產生錯誤。
兩次取樣結果之間的線性近似值（插值）被推薦用於更精確的結果。

程式碼註釋

1. 校準程式
這個校準程式移除了加速度感測器的輸出偏移分量，因為存在重力加速度（靜態的加速度）。
校準程式在加速度計處於無運動狀態時，對加速度求平均值。取樣的數量越多，加速度的校準結果越精確。

[C] 純文字檢視複製程式碼

voidCalibrate(void)

{

unsignedint 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;

}

2. 濾波
低通濾波是消除加速度計中訊號噪音(包括機械的和電子的)很好的方法。為了當對訊號進行積分時減少大部分的錯誤，減少噪音對定位應用是至關重要的。
一個簡單的低通過濾取樣訊號的方式是執行滾動平均值。過濾簡單地獲得一組取樣的平均值，獲得穩定的取樣總數的平均值是很重要的。取樣數太多可能造成資料丟失，而太少又可能導致結果不精確。

[C] 純文字檢視複製程式碼

do{

accelerationx[1]=accelerationx[1]+Sample_X;//filtering routine for noise attenuation

accelerationy[1]=accelerationy[1]+Sample_Y;//64 samples are averaged. The resulting

count2++;// average represents the acceleration of

// an instant.

}while(count2!=0x40);// 64 sums of the acceleration sample

accelerationx[1]= accelerationx[1]>>6;// division by 64

accelerationy[1]= accelerationy[1]>>6;

3. 機械濾波視窗
當處於無運動狀態時，加速度上的微小錯誤可能會被當作一個常量速度，因為在取樣值加和之後，它並不等於0。在沒有運動的理想情況下，所有的取樣值都為0。該常速表示一個連續的移動和不穩定的位置。
即使之前過濾的一些資料可能不正確，因此一個用於區別無運動狀態的"有效資料"和"無效資料"的視窗必須實現。

[C] 純文字檢視複製程式碼

1 if ((accelerationx[1] <=3)&&(accelerationx[1] >= -3)) //Discrimination window applied to

{

accelerationx[1] = 0;

} // the X axis acceleration variable

4. 定位
二重積分是利用加速度資料獲取位置所需要的步驟。第一次積分獲得速度，第二次積分獲得位置。

[C] 純文字檢視複製程式碼

//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);

5. 資料傳輸
這部分主要目的是用於除錯和顯示；這個函式中32位的結果被切割分4個8位資料。

[C] 純文字檢視複製程式碼

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;

}

6. "移動結束"檢查
基於積分表示曲線下方區域的概念，速度是加速曲線下方的區域的結果。
如果我們觀察一個典型的移動:一個物體沿一個軸從點A移動到點B，一個典型的加速度結果如下圖:

觀察上面的圖，加速度先增加後減少直到速度達到最大值（加速度始終為正代表往同一方向加速）。然後以相反的方式加速，直到它再次到達0。在這一點上達到一個穩定的位移和新的位置。
在真實世界中，其中曲線正側下方的區域面積不等於負側上方的區域面積，積分結果將永遠不會達到零速度，因此將是一個傾斜定位（從未穩定）。

正因為如此，將速度強制減為0非常關鍵。這是通過不斷讀取加速度值和0進行比較而實現的。如果在一定數量的取樣中，這種情況存在（sample==0）的話，速度將簡單地返回為0。

[C] 純文字檢視複製程式碼

if (accelerationx[1]==0) // we count the number of acceleration samples that equals zero

{

countx++;

}

else

{

countx =0;

}

if (countx>=25) // if this number exceeds 25, we can assume that velocity is zero

{

velocityx[1]=0;

velocityx[0]=0;

}

7. 原始碼

[C] 純文字檢視複製程式碼

001

002

003

004

005

006

007

008

009

010

011

012

013

014

015

016

017

018

019

020

021

022

023

024

025

026

027

028

029

030

031

032

033

034

035

036

037

038

039

040

041

042

043

044

045

046

047

048

049

050

051

052

053

054

055

056

057

058

059

060

061

062

063

064

065

066

067

068

069

070

071

072

073

074

075

076

077

078

079

080

081

082

083

084

085

086

087

088

089

090

091

092

093

094

095

096

097

098

099

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

#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

}

/*****************************************************************************************/

原理圖

總結
本文件為利用加速度計實現定位演算法提供了一些基本的概念。
這種特定的演算法在對位移精度要求不是很嚴格中非常有用。其他注意事項和具體的應用程式的影響應該在實現這個示例時被考慮。

這種積分演算法適合於低端嵌入式應用，因為它的簡單性和少量的指令。它也沒有涉及任何浮點運算。

加速度計求二次積分

加速度計求二次積分

HDU6128 二次剩余/二次域求二次剩余解/LL快速乘法取模

高等數學之直角座標二次積分轉換成極座標二次積分

求一元二次函數的根

二次不等式恒成立求參數範圍

用C語言求一元二次方根（包括虛根）

Matlab實現二次取中法求第K小

poj 1755 Triathlon （半平面交解一元二次不等式）（切割求半平面交）

關於二次函式的求根

用python求一重積分和二重積分

C語言求一元二次函式的解

二次型（求梯度） —— 公式的簡化

Hadoop鏈式MapReduce、多維排序、倒排索引、自連線演算法、二次排序、Join效能優化、處理員工資訊Join實戰、URL流量分析、TopN及其排序、求平均值和最大最小值、資料清洗ETL、分析氣

discuz論壇積分體系二次開發攻略

AutoCad .Net二次開發求兩曲線最小距離

dedecms二次開發：dedetemplate.class.php 動態模板類

VB.Net實現下CATIA二次開發方法

Java基礎知識二次學習--第八章流

呼叫中心系統OA二次開發

Spring的quartz定時器重復執行二次的問題解決

加速度計求二次積分

相關推薦