由加速度计解算得到姿态角

三、由加速度计解算得到姿态角

将加速度计安装在无人机上,并使得加速度计X、Y、Z轴与机体X、Y、Z轴重合,在机体不剧烈运动的情况下,可认为加速度计测出的加速度表示重力加速度,根据这一特性,可解算出无人机姿态。

世界坐标系下重力归一化矢量g=[0,0,1]T。机体坐标系下,将加速度计测量得到的数据归一化后得到a=[ax,ay,azT

假设经过旋转Rn,则a=Rn×g。

其中,旋转矩阵为

,可写成Rn=R(-θ,-ψ,-φ),其中θ为俯仰角,ψ为偏航角,φ为横滚角。

由此,得到ax=sinθ,ay=-sinφcosθ,az=cosφcosθ。因此得到θ=asinax,φ=atan(-ay/az)。

使用加速度计数据时,分别用这两种数据解算得到的姿态角是相反的,一般将加速度计的旋转矩阵取反,即以芯片标明的X、Y、Z轴方向为正方向,因此,计算姿态角公式时对旋转矩阵取反:θ=asin(-ax),φ=atan(ay/az)。

若使用MPU6050,则将芯片上的X轴对准前方,Y轴向左,Z轴向上。MPU6050芯片上显示的坐标轴为陀螺仪的坐标轴,MUP6050中加速度计和陀螺仪坐标轴正方向相反,即将加速度计X轴对准后方,Y轴向右,Z轴向下,这样表示的好处是如果加速度方向为前,则加速度计所测得的加速度为正。

使用右手定则,大拇指分别指向陀螺仪X、Y、Z轴正方向,四指所指方向角度为正(图2-2)。

由于无人机绕重力加速度的轴转动,加速度计测量值不会改变,加速度计也就无法感知机体的水平旋转,意味着在只有加速度计的情况下,无法通过加速度计测出偏航角ψ。

既然可以通过加速度计测得俯仰角和滚转角,为什么还要依靠陀螺仪获取姿态角呢?这是因为加速度计的动态性能不是很好,数据不是百分之百准确。一方面,使用加速度计计算姿态的前提是,无人机处于一个理想的静止状态下,然而现实中这是不可能的,那么当无人机处于飞行状态时,加速度计测量得到的数据,便不仅是重力加速度了,可能还包含了飞行过程中所产生的运动加速度;另一方面就算加速度计处于一个相对比较平稳的状态,它对一般的振动和机械噪声很敏感,这样就会得到一些有害加速度,这样计算得到的俯仰角和滚转角存在大量误差而不可用。而陀螺仪动态性能好,几乎不受外界环境干扰,测量的角速度值进行积分,便得到飞机姿态。所以,大部分的IMU系统都需要陀螺仪来使加速度计的输出更平滑。