三轴加速计数据计算倾斜角算法

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