想象一下,如果把盒子模型放在真空环境中会怎么样?在真空中,一切东西都处于无重力状态(失重),这时,小球也不例外,将悬浮在盒子的中间。在上面的图中,你可以看到我们给每个轴分配了一对墙(为了观察里面的情况,我们移除了
Y
+
Y+
Y+)。设想每面墙都能感测压力。如果我们突然把盒子向左移动(加速度为
g
=
9.8
m
/
s
2
g = 9.8 m/s^2
g=9.8m/s2),那么球会撞上
X
−
X-
X− 墙。然后我们检测球撞击墙面产生的压力,
X
X
X 轴输出值为
−
g
-g
−g。
众所周知地球上的物体都会受到重力
g
=
9.8
m
/
s
2
g = 9.8m/s^2
g=9.8m/s2 的作用。如果我们把模型放在地球上,这时圆球不再是悬浮的,而是会落在
Z
−
Z-
Z−墙面上并对其施加一个
1
g
1g
1g 的力,见下图:
图1-3
在这种情况下,盒子没有移动,但我们依然能读取到
Z
Z
Z 轴有
−
1
g
-1g
−1g 的值。因为球在墙面上施加的压力是由重力造成的。在理论上,它可以是不同类型的引力。例如,你可以想象盒子里的圆球是铁质的,将一个磁铁放在盒子旁边,小球就会撞上另一面墙,这时圆球受到的是磁场力。引用这个例子只是为了说明 加速度传感器的本质是检测力而非加速度。只是 加速度所引起的惯性力正好能被加速度传感器的检测装置所捕获。
R
x
R_x
Rx、
R
y
R_y
Ry、
R
z
R_z
Rz 是向量
R
R
R 分别在 X、Y、Z 轴上的投影;
R
x
z
R_{xz}
Rxz 是向量
R
R
R 在 XOZ 平面上的投影;
R
y
z
R_{yz}
Ryz 是向量
R
R
R 在 YOZ 平面上的投影;
A
x
z
A_{xz}
Axz 是
R
z
R_z
Rz 和
R
x
z
R_{xz}
Rxz 之间的夹角;
A
y
z
A_{yz}
Ayz 是
R
z
R_z
Rz 和
R
y
z
R_{yz}
Ryz 之间的夹角;
根据勾股定理,求得:
tan
A
x
z
=
R
x
R
z
\tan{A_{xz}}=\frac{R_x}{R_z}
tanAxz=RzRx
tan
A
y
z
=
R
y
R
z
\tan{A_{yz}}=\frac{R_y}{R_z}
tanAyz=RzRy
那么,通过反正切函数
arctan
(
)
\arctan()
arctan() 可得:
{
A
x
z
=
arctan
R
x
R
z
A
y
z
=
arctan
R
y
R
z
(1.1)
\begin{cases} A_{xz}=\arctan\frac{R_x}{R_z}\\ A_{yz}=\arctan\frac{R_y}{R_z}\\ \end{cases} \tag{1.1}
{Axz=arctanRzRxAyz=arctanRzRy(1.1) 我们感兴趣的角度是
A
x
z
、
A
y
z
A_{xz}、A_{yz}
Axz、Ayz。想象图1-5新模型中每个轴都分别垂直于原模型中各自的墙面。矢量
R
R
R 是加速度传感器所检测的矢量(它可能是重力或其它惯性力的合成)。
R
x
、
R
y
、
R
z
R_x、R_y、R_z
Rx、Ry、Rz 是矢量
R
R
R 在 X、Y、Z 轴上的投影。
根据三维空间勾股定理,可以得到下列关系:
R
2
=
R
x
2
+
R
y
2
+
R
z
2
(1.2)
R^2={R_x}^2+{R_y}^2+{R_z}^2 \tag{1.2}
R2=Rx2+Ry2+Rz2(1.2) 回到 MPU-6050 传感器中,在上一小节我们已经分别读取到 3 个轴的加速度数据,但是我们读到的加速度数字量的单位还不是
g
(
9.8
m
/
s
2
)
g(9.8m/s^2)
g(9.8m/s2) 。最后的转换,我们还需要引入加速度传感器的灵敏度(Sensitivity),单位通常是
L
S
B
/
g
\rm{LSB}/\it{g}
LSB/g 。比方说,加速度传感器的灵敏度为
16384
L
S
B
/
g
16384\ \rm{LSB}/\it{g}
16384LSB/g 。灵敏度值可以在加速度传感器规格书中查到。要获得最后的单位为
g
g
g 的加速度值,我们使用下列公式计算:
R
x
=
R
x
S
e
n
s
i
t
i
v
i
t
y
(1.3)
R_x=\frac{R_x}{Sensitivity} \tag{1.3}
Rx=SensitivityRx(1.3) 比如:
当设置 MPU-6050 加速度传感器的灵敏度为
16384
L
S
B
/
g
16384\ \rm{LSB}/\it{g}
16384LSB/g 时,读取到 MPU-6050 的 X、Z 轴加速度数据为 1122、16674 ,则可以计算出:
{
R
x
=
1122
/
16384
=
0.068
g
R
z
=
16674
/
16384
=
1.018
g
A
x
z
=
arctan
(
0.068
g
/
1.018
g
)
=
0.0667
r
a
d
(1.4)
\begin{cases} R_x=1122/16384=0.068g\\ R_z=16674/16384=1.018g\\ A_{xz}=\arctan(0.068g/1.018g)=0.0667\ rad\\ \end{cases} \tag{1.4}
⎩⎪⎨⎪⎧Rx=1122/16384=0.068gRz=16674/16384=1.018gAxz=arctan(0.068g/1.018g)=0.0667rad(1.4)
注意,此时计算出的角度单位为弧度(rad),需要转换成角度(°):
A
x
z
=
0.0667
/
π
∗
180
=
3.82
°
(1.5)
A_{xz}=0.0667/\pi*180=3.82° \tag{1.5}
Axz=0.0667/π∗180=3.82°(1.5)
2. 陀螺仪
陀螺仪的每个通道检测一个轴的旋转。例如,一个 2 轴陀螺仪检测绕 X 和 Y 轴的旋转。为了用数字来表达这些旋转,我们先引入一些符号。首先,我们定义:
R
x
z
R_{xz}
Rxz —— 惯性力矢量
R
R
R 在 XOZ 平面上的投影;
R
y
z
R_{yz}
Ryz —— 惯性力矢量
R
R
R 在 YOZ 平面上的投影。
在由
R
x
z
R_{xz}
Rxz 和
R
z
R_{z}
Rz 组成的直角三角形中,运用勾股定理可得:
R
x
z
2
=
R
x
2
+
R
z
2
(2.1)
{R_{xz}}^2={R_x}^2+{R_z}^2 \tag{2.1}
Rxz2=Rx2+Rz2(2.1) 同理:
R
y
z
2
=
R
y
2
+
R
z
2
(2.2)
{R_{yz}}^2={R_y}^2+{R_z}^2 \tag{2.2}
Ryz2=Ry2+Rz2(2.2) 同时注意:
{
R
2
=
R
x
2
+
R
y
z
2
R
2
=
R
y
2
+
R
x
z
2
(2.3)
\begin{cases} {R}^2={R_x}^2+{R_{yz}}^2\\ {R}^2={R_y}^2+{R_{xz}}^2\\ \end{cases} \tag{2.3}
{R2=Rx2+Ryz2R2=Ry2+Rxz2(2.3) 相反,我们按如下方法定义 Z 轴和
R
x
z
、
R
y
z
R_{xz}、R_{yz}
Rxz、Ryz 向量所成的夹角:
A
x
z
A_{xz}
Axz ——
R
x
z
R_{xz}
Rxz 和 Z 轴所成的夹角;
A
y
z
A_{yz}
Ayz ——
R
y
z
R_{yz}
Ryz 和 Z 轴所成的夹角。
现在我们离陀螺仪要测量的东西又近了一步。陀螺仪测量上面定义的角度的变化率。换句话说,它会输出一个与上面这些角度变化率线性相关的值。为了解释这一点,我们先假设在
t
0
t_0
t0 时刻,我们已经测得绕 Y 轴旋转的角度(即
A
x
z
A_{xz}
Axz),定义为
A
x
z
0
A_{xz0}
Axz0 ,之后在
t
1
t_1
t1 时刻我们再次测量这个角度,得到
A
x
z
1
A_{xz1}
Axz1 。则角度变化率按下面方法计算:
R
a
t
e
A
x
z
=
(
A
x
z
1
−
A
x
z
0
)
/
(
t
1
−
t
0
)
(2.4)
Rate\ A_{xz}=(A_{xz1}-A_{xz0})\ /\ (t_1-t_0) \tag{2.4}
RateAxz=(Axz1−Axz0)/(t1−t0)(2.4) 如果用度(°)来表示角度,秒(s)来表示时间,那这个值的单位就是度/秒(°/s)。这就是陀螺仪检测的东西。在实际运用中,陀螺仪一般都不会直接输出一个单位为度/秒的数值(除非它是个特殊的数字陀螺仪)。在 MPU-6050 传感器中,就像读取加速度数据一样,会得到一个经过内置 ADC 转换后得到的数字量,单位为 LSB 。参考对加速度传感器数据的处理,我们同样得到:
{
R
a
t
e
A
x
z
=
(
G
y
r
o
Y
−
Z
e
r
o
R
a
t
e
)
/
S
e
n
s
i
t
i
v
i
t
y
R
a
t
e
A
y
z
=
(
G
y
r
o
X
−
Z
e
r
o
R
a
t
e
)
/
S
e
n
s
i
t
i
v
i
t
y
(2.5)
\begin{cases} Rate\ A_{xz}=(GyroY-ZeroRate)\ /\ Sensitivity\\ Rate\ A_{yz}=(GyroX-ZeroRate)\ /\ Sensitivity\\ \end{cases} \tag{2.5}
{RateAxz=(GyroY−ZeroRate)/SensitivityRateAyz=(GyroX−ZeroRate)/Sensitivity(2.5) 其中,
G
y
r
o
X
,
G
y
r
o
Y
GyroX,GyroY
GyroX,GyroY —— 这两个值是陀螺仪数据,它们分别代表矢量 R 的投影在 XOZ 和 YOZ 平面内的转角,也可等价的说,旋转可分解为单独绕 X 和 Y 轴的运动;
Z
e
r
o
R
a
t
e
ZeroRate
ZeroRate —— 陀螺仪零偏值,它是陀螺仪静止不动时的输出值。理论上,陀螺仪静止不动时,应该输出为 0 的值,但是受制造工艺、外部因素的影响,陀螺仪静止不动时输出一个比较小的随机的不为 0 的值,我们称之为零偏值。
S
e
n
s
i
t
i
v
i
t
y
Sensitivity
Sensitivity —— 陀螺仪的灵敏度,单位
L
S
B
/
(
d
e
g
/
s
)
\rm{LSB}\ /\ \it(deg/s)
LSB/(deg/s) ,它的意思就是如果旋转速度增加 1°/s,陀螺仪的输出就会增加多少数值。
举个例子,假设我们读取到 MPU-6050 传感器的 X 轴数据:
G
y
r
o
X
=
12345
GyroX=12345
GyroX=12345
若此时设置陀螺仪的灵敏度为 16.4,并认为陀螺仪零偏值为 0 ,代入公式(2.5),得到:
R
a
t
e
A
y
z
=
(
12345
−
0
)
/
16.4
=
751
d
e
g
/
s
(2.6)
Rate\ A_{yz}=(12345-0)\ /\ 16.4=751\ deg/s \tag{2.6}
RateAyz=(12345−0)/16.4=751deg/s(2.6) 换句话说,传感器绕 X 轴以 751°/s 的速度旋转。