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

先说结论，再解释原理
假设世界坐标系下重力向量为 g=(001)T g = ( 0 0 1 ) T
机体坐标系下，将加速度计测量得到的数据归一化后得到 a=(axayaz)T a = ( a x a y a z ) T
横滚角、俯仰角、偏航角可简单理解为加速度计绕X、Y、Z轴旋转的角度
若使用mpu6050，则将芯片上的X轴对准前方，Y轴向左，Z轴向上(mpu6050芯片上显示的坐标轴为陀螺仪的坐标轴，mpu6050中加速度计和陀螺仪坐标轴正方向相反)即将加速度计X轴对准后方，Y轴向右，Z轴向下(这样表示的好处是如果加速度方向为前，则加速度计所测得的加速度为正)
使用右手定则，大拇指分别指向陀螺仪X、Y、Z轴正方向，四指所指方向角度为正
俯仰角 θ=asin(−ax) θ = a s i n ( − a x )
横滚角 ϕ=atan(ay/az) ϕ = a t a n ( a y / a z )
在只有加速度计的情况下，无法测得偏航角 ψ ψ

以下为原理
假设加速度计绑定在机体上，并且加速度X、Y、Z轴与机体X、Y、Z轴重合，在机体不剧烈运动的情况下，可认为加速度计测出的加速度表示重力加速度，可根据这一特性，解算得出姿态。

在初始未旋转的状态，即机体坐标系与世界坐标系重合时，将加速度计测得的数据归一化后得到 a=g=(001)T a = g = ( 0 0 1 ) T
假设经过旋转 Rn R n ，得到加速度计归一化后的数据 a=(axayaz)T a = ( a x a y a z ) T
则 a=Rn∗g a = R n ∗ g
其中
旋转矩阵 R= R =

⎡⎣⎢⎢cosθcosψcosψsinθsinϕ−sinψcosϕcosψsinθcosϕ+sinψsinϕcosθsinψsinψsinθsinϕ+cosψcosϕsinψsinθcosϕ−cosψsinϕ−sinθsinϕcosθcosϕcosθ⎤⎦⎥⎥ [ c o s θ c o s ψ c o s θ s i n ψ − s i n θ c o s ψ s i n θ s i n ϕ − s i n ψ c o s ϕ s i n ψ s i n θ s i n ϕ + c o s ψ c o s ϕ s i n ϕ c o s θ c o s ψ s i n θ c o s ϕ + s i n ψ s i n ϕ s i n ψ s i n θ c o s ϕ − c o s ψ s i n ϕ c o s ϕ c o s θ ]

Rn=R(−θ,−ψ,−ϕ) R n = R ( − θ , − ψ , − ϕ )
其中 θ θ 为俯仰角， ψ ψ 为偏航角， ϕ ϕ 为横滚角
由此，得到 ax=sinθ a x = s i n θ ， ay=−sinϕcosθ a y = − s i n ϕ c o s θ ， az=cosϕcosθ a z = c o s ϕ c o s θ
因此得到
θ=asin(ax) θ = a s i n ( a x )
ϕ=atan(−ay/az) ϕ = a t a n ( − a y / a z )
使用mpu6050加速度计和陀螺仪数据时，分别用这两种数据解算得到的姿态角是相反的，一般将加速度计的旋转矩阵取反，即以芯片标明的XYZ轴方向为正方向
因此本文开头部分计算姿态角公式时对旋转矩阵取反
θ=asin(−ax) θ = a s i n ( − a x )
ϕ=atan(ay/az) ϕ = a t a n ( a y / a z )

向量的旋转与坐标系的旋转

向量旋转
旋转矩阵 R R 表示向量 n n 按照 Z−Y−X Z − Y − X 的顺序，绕这三个轴分别旋转 ψ、ϕ、θ ψ 、 ϕ 、 θ
得到向量旋转后的坐标为 n′=R∗n n ′ = R ∗ n
注意上面的旋转矩阵表示的是向量的旋转，而非坐标系的旋转
坐标系旋转
如果向量 n n 不旋转，而向量n所在坐标系按照 Z−Y−X Z − Y − X 的顺序旋转
则可以得到坐标系旋转后n向量的坐标为 n″=Rn∗n=RT∗n n ″ = R n ∗ n = R T ∗ n

至于为什么 Rn=R(−θ,−ψ,−ϕ) R n = R ( − θ , − ψ , − ϕ ) ，可类比二维向量的旋转
二维旋转矩阵

r(θ)=[cosθsinθ−sinθcosθ] r ( θ ) = [ c o s θ − s i n θ s i n θ c o s θ ]

向量 b=(xy)T b = ( x y ) T 绕 Z Z 轴（沿纸面向外）逆时针旋转 θ θ 可表示为
b′=r(θ)∗b b ′ = r ( θ ) ∗ b
若向量 b b 不旋转，而 Y−X Y − X 坐标系绕 Z Z 轴逆时针旋转 θ θ 后， b b 的坐标变为
b″=r(−θ)∗b b ″ = r ( − θ ) ∗ b
三维的旋转矩阵与二维的旋转矩阵有相似的规律