文章目录
本章介绍IMU传感器
本文涉及的代码见: Github。
一、旋转运动学
- 1、线速度与角速度
粒子在 z = 0 z = 0 z=0的平面运动(假设全局坐标系为家里的房间, 场景与简单理解VIO(一)中的照片类似),以 a = 1 a = 1 a=1为半径的圆上进行圆周运动,坐标为: r = ( cos θ , sin θ , 0 ) r = (\cos\theta,\sin\theta,0) r=(cosθ,sinθ,0).
对 θ \theta θ求导得:
r ˙ = [ − θ ˙ sin θ , θ ˙ cos θ , 0 ] T = [ 0 − θ ˙ 0 θ ˙ 0 0 0 0 0 ] [ cos θ sin θ 0 ] \dot{r} = [-\dot{\theta}\sin\theta, \dot{\theta}\cos\theta, 0]^T = [\begin{matrix}0& -\dot{\theta}&0\\ \dot{\theta}&0 & 0 \\ 0&0&0\end{matrix}] [\begin{matrix}\cos\theta \\ \sin\theta \\ 0 \end{matrix}] r˙=[−θ˙sinθ,θ˙cosθ,0]T=[0θ˙0−θ˙00000][cosθsinθ0]
其中, θ ˙ 为 角 速 度 w \dot{\theta}为角速度w θ˙为角速度w,后面写成的矩阵形式只是为了将偏导数与角度分开。 - 2、旋转坐标系下的运动学
站立面朝小车,人为body坐标系,假设此时全局坐标系即为0时刻人的坐标系,body坐标系匀速逆时针旋转1°/s. 在全局中,小车静止不动.在body坐标系下,小车的坐标:
r B = [ cos θ , − sin θ , 0 ] r_B = [\cos\theta, -\sin\theta, 0] rB=[cosθ,−sinθ,0]
在全局坐标系下,小车的坐标等于
r I = R I B r B r_I = R_{IB}r_B rI=RIBrB
因为body本身在旋转,因此 R I B R_{IB} RIB也是变化的, 用四元数表示为 q I B = [ cos ( θ / 2 ) , [ 0 , 0 , 1 ] sin ( θ / 2 ) ] q_{IB} = [\cos(\theta/2), [0,0,1]\sin(\theta/2)] qIB=[cos(θ/2),[0,0,1]sin(θ/2)], r B r_B rB也是变化的(理论上是变化的,这里为了简单前面说小车相对静止). 如果对上式求偏导,可以得到
r I ˙ = R I B r B ˙ + R I B ˙ r B \dot{r_I} = R_{IB}\dot{r_B} + \dot{R_{IB}}r_B rI˙=RIBrB˙+RIB˙rB
这里具体公式并不重要,但是有一个比较重要的结论:body坐标系下,质量块的速度或者角速度,转换到全局坐标系下去,不是直接 R I B r B ˙ R_{IB}\dot{r_B} RIBrB˙, 还受到了其他坐标系本身的影响.
再求二阶导,就可以得到质量块加速度之间的关系。此时多出来的部分,就是科氏力,离心力。这个地方用得最广的是障碍物运动估计,如:无人车的姿态为 R I B R_{IB} RIB, 在该坐标系下观察到前方车辆的速度 r B ˙ \dot{r_B} rB˙,在障碍物建模过程中,我们需要求的是障碍物在全局坐标系下的速度 r I ˙ \dot{r_I} rI˙
二、IMU测量模型及运动模型
1、MEMS加速度计工作原理
加速度测量最基本的原理是:
a
=
f
/
m
a = f/m
a=f/m. 即是牛顿第二定律: 加速度与作用力成正比,与物体质量成反比,那即可通过测量力,质量一般可以提前测得,得出加速度大小。
常见的测量力的方式:
- 弹簧
- 电容电阻
2、陀螺仪测量原理
常见的测量方式有两种:
- 光纤陀螺
- 震动陀螺
一般用的MEMS器件,都是震动陀螺。有两个关键点:
- 通过测量科氏力,求得角速度的。
- 通过两个震动音叉,抵消其他力的影响。
三、IMU误差模型
误差分为两类:确定性误差(bias, scale, misalign),随机游走误差(噪声)。
1、六面法标定加速度
当考虑二维情况时,只需要对xy轴进行处理。
1.1、bias与scale
水平静止放置四面就可以得到bias,scale.
1.2、轴间误差
水平放置四个面的时候,加速度的理论值可以知道,这样就可以联立方程组,构建误差方程
y
=
z
−
h
(
x
)
y = z - h(x)
y=z−h(x)
其中,x是bias, scale, 轴间误差等。这里利用的是重力加速度,其实也可以使用水平方向加速度为0来处理,可能精度会略低一些,此时就只用放置一面就可以了。即:“一面法标定角速度计”, 如下:
[
0
0
]
=
[
s
x
,
m
x
y
m
y
x
,
s
y
]
[
a
x
a
y
]
+
[
b
x
b
y
]
[\begin{matrix}0\\ 0\end{matrix}] = [\begin{matrix}s_x, m_{xy}\\ m_{yx}, s_y \end{matrix}] [\begin{matrix}a_x\\ a_y\end{matrix}] + [\begin{matrix}b_x\\ b_y\end{matrix}]
[00]=[sx,mxymyx,sy][axay]+[bxby]
一共6个变量,观测3组实验数据即可求解。
四、运动模型离散时间处理
中间的桥梁是泰勒展开
五、IMU数据仿真
1. 地图生成
把自己家房子做了个如下的结构图,生成了一个简单的地图:
2. 速度生成
位置对于时间的导数,就是速度。
x
=
m
(
t
+
n
)
x = m(t + n)
x=m(t+n)
y
=
c
sin
(
a
x
+
b
)
+
d
y = c\sin (ax+b) + d
y=csin(ax+b)+d
其中m,n,a,b,c,d是常数.
x
′
=
m
x' = m
x′=m
y
′
=
c
cos
(
a
x
+
b
)
a
x
′
=
a
∗
c
∗
m
∗
cos
(
a
x
+
b
)
y' = c\cos(ax+b)ax' = a*c*m*\cos(ax+b)
y′=ccos(ax+b)ax′=a∗c∗m∗cos(ax+b)
3. 加速度生成
位置对于时间的二阶导数,就是加速度。
x
′
′
=
0
x'' = 0
x′′=0
y
′
′
=
−
a
∗
c
∗
m
∗
sin
(
a
x
+
b
)
a
x
′
=
−
a
2
∗
c
∗
m
2
sin
(
a
x
+
b
)
y'' = -a*c*m*\sin(ax+b)ax' = - a^2*c*m^2\sin(ax+b)
y′′=−a∗c∗m∗sin(ax+b)ax′=−a2∗c∗m2sin(ax+b)
这是全局坐标系下的加速度
a
G
=
[
x
′
′
,
y
′
′
]
T
a^G = [x'', y'']^T
aG=[x′′,y′′]T,需要将其转换为局部坐标系:
R
T
a
G
=
a
L
R^Ta^G = a^L
RTaG=aL
4. 角速度生成
角度对于时间的导数,就是角速度。斜率k与角度之间的关系
k
=
tan
(
θ
)
k = \tan(\theta)
k=tan(θ)
而k就是y对于x的导数:
k
=
a
∗
c
∗
cos
(
a
x
+
b
)
k = a*c*\cos(ax+b)
k=a∗c∗cos(ax+b)
因此:
θ
=
arctan
(
k
)
\theta = \arctan (k)
θ=arctan(k)
θ
′
=
−
1
1
+
k
2
a
2
c
m
sin
(
a
x
+
b
)
\theta ' = -\frac{1}{1+k^2}a^2cm\sin(ax+b)
θ′=−1+k21a2cmsin(ax+b)
六、代码
- 生成IMU的数据
- 利用KF进行积分,得到轨迹
简单制作了一个GUI,使用Matlab可以运行,代码
七、参考文献
1: 第二节IMU传感器