三軸加速計資料計算傾斜角演算法
阿新 • • 發佈：2019-01-07
1.傾斜角測量原理
對於軸加速度感測器，當它的感測方向和重力加速度方向一致時，假如此時為零傾斜角度，設加速度感測器測量結果為F(θ)，θ為傾斜角度，g為重力加速度，如圖1所示。
所以當傾斜角θ太小時，測量的解析度就會太小，當角度足夠大時精度才會上升。所以對一軸傾斜角感測器的運用是：把它的感測方向與重力加速度方向垂直時的狀態設為零傾斜角度，文獻運用此方法測量傾斜角，如圖2所示，此時：
此時傾斜角度小時測量精度高，而對於一軸加速度感測器而言，只能測一個方向的傾斜角。所以用一個兩軸加速度感測器，兩個感測方向皆垂直於重力加速度，當兩軸傾斜角傾斜時，加速度感測器測量結果為：
如何利用θx，θy求出傾斜角θ。首先定義兩組三軸向量：[x，y，z]為參考O傾斜向量，[u，v，r]為傾斜後的向量。如圖3所示，設向量[z，y，z]先繞y軸傾斜，再繞x軸傾斜，所以從[x，y，z]到[u，v，r]的轉換為：
設x,y為水平方向，z為垂直方向。（x，y，z）=（0，0，1）於是便有：
此時u=kF(θx)=kgsinθx,v=kF(θy)=kgsinθy，所以：
而感測器實際傾斜角為：
所以只要得出兩軸加速度感測器測量結果F(θx)和F(θy)就可以計算出θx和θy，進而知道總的傾斜度。
測量一個全方位，全擺幅的傾斜角就必須使用三軸加速度計。
運用三軸加速度計測量傾斜角就必須把測量範圍分為兩檔，一檔為傾斜角為-π／4～π／4，二檔為傾斜角為(-π／2～-π／4)＆(π／4～π／2)。當傾斜角度在±π／4之間時，
這裡以F(θz)的值作為劃分檔次的依據。在一檔中F(θx)，F(θy)的解析度很高，此時相當於運用一個兩軸加速度感測器測量全方位，低擺幅傾斜角，運用式(15)可以計算傾斜角。在二檔中F(θz)的解析度都很高，此時相當於運用一個一軸加速度感測器測量全方位，高傾斜角度的傾斜角，運用式(1)可以計算傾斜角。
2.零點偏移
零刻度偏移的解決方案，當傾斜角較小時，z軸資料的解析度極低，影響測量精度，所以此時運用其中兩軸進行傾斜角測量將更精確。為提高精度，需要對傾斜角感測器進行調零校正，校正後的結果將是測量值減去偏移值後的資料。偏移值的測量方法：
將感測器放置在一個校準水平面上，此時的輸出即為偏移值。
接下來就是用程式碼實現該演算法（注意根號不能有負數）：
float calculateTilt(float ax,float ay,float az,char flag_x,char flag_y,char flag_z)
{
float g=9.80665;
float temp;
float Tiltangle=0;
temp=((sqrt(2)/2)*g/10);
if(az>((sqrt(2)/2)*g/10))
{
Tiltangle=(1-ay*ay)-(1-ax*ax);
if(Tiltangle<0)
Tiltangle=-Tiltangle;
Tiltangle=acos(sqrt(Tiltangle));
Tiltangle=Tiltangle/PI*180;
if(flag_x==1||flag_y==1)
{
Tiltangle+=90;
}
else
{
Tiltangle=90-Tiltangle;
}
}
else
{
Tiltangle=asin(az);
Tiltangle=Tiltangle/PI*180;
if(flag_z==1)
{
Tiltangle+=90;
}
else
{
Tiltangle=90-Tiltangle;
}
}
return Tiltangle;
}
對於軸加速度感測器，當它的感測方向和重力加速度方向一致時，假如此時為零傾斜角度，設加速度感測器測量結果為F(θ)，θ為傾斜角度，g為重力加速度，如圖1所示。
所以當傾斜角θ太小時，測量的解析度就會太小，當角度足夠大時精度才會上升。所以對一軸傾斜角感測器的運用是：把它的感測方向與重力加速度方向垂直時的狀態設為零傾斜角度，文獻運用此方法測量傾斜角，如圖2所示，此時：
此時傾斜角度小時測量精度高，而對於一軸加速度感測器而言，只能測一個方向的傾斜角。所以用一個兩軸加速度感測器，兩個感測方向皆垂直於重力加速度，當兩軸傾斜角傾斜時，加速度感測器測量結果為：
如何利用θx，θy求出傾斜角θ。首先定義兩組三軸向量：[x，y，z]為參考O傾斜向量，[u，v，r]為傾斜後的向量。如圖3所示，設向量[z，y，z]先繞y軸傾斜，再繞x軸傾斜，所以從[x，y，z]到[u，v，r]的轉換為：
設x,y為水平方向，z為垂直方向。（x，y，z）=（0，0，1）於是便有：
此時u=kF(θx)=kgsinθx,v=kF(θy)=kgsinθy，所以：
而感測器實際傾斜角為：
所以只要得出兩軸加速度感測器測量結果F(θx)和F(θy)就可以計算出θx和θy，進而知道總的傾斜度。
測量一個全方位，全擺幅的傾斜角就必須使用三軸加速度計。
運用三軸加速度計測量傾斜角就必須把測量範圍分為兩檔，一檔為傾斜角為-π／4～π／4，二檔為傾斜角為(-π／2～-π／4)＆(π／4～π／2)。當傾斜角度在±π／4之間時，
這裡以F(θz)的值作為劃分檔次的依據。在一檔中F(θx)，F(θy)的解析度很高，此時相當於運用一個兩軸加速度感測器測量全方位，低擺幅傾斜角，運用式(15)可以計算傾斜角。在二檔中F(θz)的解析度都很高，此時相當於運用一個一軸加速度感測器測量全方位，高傾斜角度的傾斜角，運用式(1)可以計算傾斜角。
2.零點偏移
零刻度偏移的解決方案，當傾斜角較小時，z軸資料的解析度極低，影響測量精度，所以此時運用其中兩軸進行傾斜角測量將更精確。為提高精度，需要對傾斜角感測器進行調零校正，校正後的結果將是測量值減去偏移值後的資料。偏移值的測量方法：
將感測器放置在一個校準水平面上，此時的輸出即為偏移值。
接下來就是用程式碼實現該演算法（注意根號不能有負數）：
float calculateTilt(float ax,float ay,float az,char flag_x,char flag_y,char flag_z)
{
float g=9.80665;
float temp;
float Tiltangle=0;
temp=((sqrt(2)/2)*g/10);
if(az>((sqrt(2)/2)*g/10))
{
Tiltangle=(1-ay*ay)-(1-ax*ax);
if(Tiltangle<0)
Tiltangle=-Tiltangle;
Tiltangle=acos(sqrt(Tiltangle));
Tiltangle=Tiltangle/PI*180;
if(flag_x==1||flag_y==1)
{
Tiltangle+=90;
}
else
{
Tiltangle=90-Tiltangle;
}
}
else
{
Tiltangle=asin(az);
Tiltangle=Tiltangle/PI*180;
if(flag_z==1)
{
Tiltangle+=90;
}
else
{
Tiltangle=90-Tiltangle;
}
}
return Tiltangle;
}