先说结论,再解释原理
假设世界坐标系下重力向量为
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
=
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
表示向量按照
Z−Y−X
Z
−
Y
−
X
的顺序,绕这三个轴分别旋转
ψ、ϕ、θ
ψ
、
ϕ
、
θ
得到向量旋转后的坐标为
n′=R∗n
n
′
=
R
∗
n
注意上面的旋转矩阵表示的是向量的旋转,而非坐标系的旋转
坐标系旋转
如果向量
n
n
不旋转,而向量n所在坐标系按照的顺序旋转
则可以得到坐标系旋转后n向量的坐标为
n″=Rn∗n=RT∗n
n
″
=
R
n
∗
n
=
R
T
∗
n
至于为什么
Rn=R(−θ,−ψ,−ϕ)
R
n
=
R
(
−
θ
,
−
ψ
,
−
ϕ
)
,可类比二维向量的旋转
二维旋转矩阵
向量 b=(xy)T b = ( x y ) T 绕 Z Z 轴(沿纸面向外)逆时针旋转 可表示为
b′=r(θ)∗b b ′ = r ( θ ) ∗ b
若向量 b b 不旋转,而坐标系绕 Z Z 轴逆时针旋转后, b b 的坐标变为
三维的旋转矩阵与二维的旋转矩阵有相似的规律