IMU误差模型
理想的惯性测量单元包含三个正交、线性的加速度计和三个正交的陀螺仪。它们分别测量三个相互正交的坐标轴的加速度和角速率;但是现实加速度计和陀螺仪分别制造,坐标系中心不一致,甚至轴之间也是不完全正交的。此外,单个传感器也不是完美点:通常用于将传感器的数字输出转换为实际物理量的比例因子对于同一传感器的不同实例是不同的,而制造商只提供默认的标称比例因子。并且输出信号总是受到非零可变的偏置的影响。
加速度计坐标系accelerometers frame (AF) 和陀螺仪坐标系gyroscopes frame (GF) 通常是非正交的,定义两个相关的正交、理想坐标系AOF和GOF:
- AOF的x轴与AF的x轴重合
- AOF的y轴在AF的xoy平面上
GOF与GF的关系同理。
Error Type | Description | Gyroscope (Result of Integration) | Accelerometer (Result of Double Integration) |
---|---|---|---|
Bias | A constant bias ϵ\epsilonϵ in the accelerometer’s output signal | A steadily growing angular errorθ(t)=ϵ⋅t\theta(t)=\epsilon\cdot tθ(t)=ϵ⋅t | A quadratically growing position errors(t)=ϵ⋅t22s(t)=\epsilon\cdot\dfrac{t^2}{2}s(t)=ϵ⋅2t2 |
Calibration | Deterministic errors in scale factors, alignments and accelerometer linearities | Orientation drift proportional to the rate and duration of motion | Position drift proportional to the squared rate and duration of acceleration |
White Noise | White noise with some standard deviation σ\sigmaσ | An angle random walk, whose standard deviation σθ(t)=σ⋅δt⋅t\sigma_\theta(t)=\sigma\cdot\sqrt{\delta t\cdot t}σθ(t)=σ⋅δt⋅tgrows with the square root of time | A second-order random walk. The standard deviation of the position error grows asσs(t)=σ⋅t3/2⋅δt3\sigma_s(t)=\sigma\cdot t^{3/2}\cdot\sqrt{\dfrac{\delta t}{3}}σs(t)=σ⋅t3/2⋅3δt |
Temperature Effects | Temperature dependent residual bias | Any residual bias is integrated into the orientation, causing an orientation error which grows linearly with time | Any residual bias causes an error in position which grows quadratically with time |
|Bias Instability | Bias fluctuations, usually modelled as a bias random walk|A second-order random walk| A third-order random walk in position|
加速度计和陀螺仪的误差可以分为:确定性误差,随机误差。
- 确定性误差可以事先标定确定,包括:bias,scale,misalignment
- 随机误差通常假设噪声服从高斯分布,包括:高斯白噪声,bias随机游走.
确定性误差
-
Bias:理论上,当没有外部作用时,IMU传感器的输出应该为0,但是实际存在一个偏置b\bold{b}b。
- 陀螺仪的偏置是陀螺仪不进行任何旋转时的平均输出(即:输出与真实值的偏移量),单位是∘/h{}^\circ/h∘/h
一个恒定的偏置误差bω\bold{b}_{\omega}bω随着时间积分,造成的角度偏差随时间线性积累θ(t)=bωt\theta(t)=\bold{b}_{\omega}tθ(t)=bωt - 加速度计bias对位姿估计的影响:
verr=bat,perr=12bat2\bold{v}_{err}=\bold{b}_a t,\quad\bold{p}_{err}=\dfrac{1}{2}\bold{b}_a t^2 verr=bat,perr=21bat2
- 陀螺仪的偏置是陀螺仪不进行任何旋转时的平均输出(即:输出与真实值的偏移量),单位是∘/h{}^\circ/h∘/h
-
Scale:可以看作是实际数值和传感器输出值之间的比值
-
Nonorthogonality/Misalignment Errors:多轴IMU传感器制作时,由于制作工艺问题,会使xyz轴不正交
scale+Misalignment
[amxamyamz]=[sxxmxymxzmyxsyymyzmzxmzyszz][axayaz]\begin{bmatrix}a_{m_x}\\a_{m_y}\\a_{m_z}\end{bmatrix}= \begin{bmatrix} s_{xx}&m_{xy}&m_{xz}\\ m_{yx}&s_{yy}&m_{yz}\\ m_{zx}&m_{zy}&s_{zz} \end{bmatrix} \begin{bmatrix} a_x\\ a_y\\ a_z \end{bmatrix} amxamyamz=sxxmyxmzxmxysyymzymxzmyzszzaxayaz
随机误差
高斯白噪声
IMU数据连续时间上受到一个均值为0,方差为σ\sigmaσ,各时刻之间相互独立的高斯过程n(t)n(t)n(t):
E[n(t)]≡0E[n(t1)n(t2)]=σ2δ(t1−t2)E[n(t)]\equiv 0\\ E[n(t_1)n(t_2)]=\sigma^2\delta(t_1-t_2) E[n(t)]≡0E[n(t1)n(t2)]=σ2δ(t1−t2)
其中δ()\delta()δ()表示狄拉克函数。
实际上,IMU传感器获取的数据为离散采样,采样保持时间为Δt\Delta tΔt,离散和连续高斯白噪声的方差之间存在如下转换关系:
nd[K]≃1Δt∫t0t0+Δtn(t)dtE(nd[k]2)=E(1Δt2∫t0t0+Δt∫t0t0+Δtn(τ)n(t)dτdt)=E(σ2Δt2∫t0t0+Δt∫t0t0+Δtδ(t−t)dτdt)=E(σ2Δt)\begin{aligned} n_{d}\left[ K\right] & \simeq \dfrac{1}{\Delta t}\int _{t_{0}}^{t_{0}+\Delta t}n \left( t\right) {\rm d}t \\ E\left( n_{d}\left[ k\right] ^{2}\right) &=E\left( \dfrac{1}{\Delta t^{2}}\int _{t_{0}}^{t_{0}+\Delta t}\int _{t_{0}}^{t_{0}+\Delta t}n\left( \tau \right) n\left( t\right) {\rm d}\tau {\rm d}t\right) \\ &=E\left( \dfrac{\sigma ^{2}}{\Delta t^{2}}\int _{t_{0}}^{t_{0}+\Delta t}\int _{t_{0}}^{t_{0}+\Delta t}\delta \left( t-t\right) {\rm d}\tau {\rm d}t\right) \\ &=E\left( \dfrac{\sigma ^{2}}{\Delta t}\right) \end{aligned} nd[K]E(nd[k]2)≃Δt1∫t0t0+Δtn(t)dt=E(Δt21∫t0t0+Δt∫t0t0+Δtn(τ)n(t)dτdt)=E(Δt2σ2∫t0t0+Δt∫t0t0+Δtδ(t−t)dτdt)=E(Δtσ2)
即
nd[k]=σdw[k]n_d[k]=\sigma_d w[k] nd[k]=σdw[k]
其中
w[k]∼N(0,1)σd=σ1Δtw[k]\sim \mathcal{N}(0,1)\\ \sigma_d=\sigma\dfrac{1}{\Delta t} w[k]∼N(0,1)σd=σΔt1
也就是说高斯白噪声的连续时间到离散时间之间差一个1Δt\dfrac{1}{\sqrt{\Delta t}}Δt1,Δt\Delta tΔt是传感器采样时间。
Bias随机游走
通常用维纳过程(wiener process)来建模bias随时间连续变化的过程,离散时间下称之为随机游走。
b˙(t)=nb(t)=σbw(t)\dot{b}(t)=n_b(t)=\sigma_b w(t) b˙(t)=nb(t)=σbw(t)
其中www是方差为1的白噪声
同样,离散和连续之间的转换:
bd[k]≜b(t0)+∫t0t0+Δtnb(t)dtb_{d}\left[ k\right] \triangleq b\left( t_{0}\right) +\int _{t_{0}}^{t_{0}+\Delta t}n_b\left( t\right) dt bd[k]≜b(t0)+∫t0t0+Δtnb(t)dt
E[(bd[k]−bd[k−1])2]=E(∫t0t0+Δt∫t0t0+Δtnb(τ)nb(t)dτdt)=E(σb2∫t0t0+Δt∫t0t0+Δtδ(t−τ)dτdt)=E(σb2Δt)\begin{aligned} E\left[ \left( b_{d}\left[ k\right] -b_d\left[ k-1\right] \right) ^{2}\right] &=E\left( \int _{t_{0}}^{t_{0}+\Delta t}\int _{t_{0}}^{t_{0}+\Delta t}n_b\left( \tau \right) n_b \left( t\right) d\tau dt\right) \\ &=E\left( \sigma _{b}^{2}\int ^{t_{0}+\Delta t}_{t_{0}}\int _{t_{0}}^{t_{0}+\Delta t}\delta\left( t-\tau \right) d\tau dt\right) \\ &=E\left( \sigma _{b}^{2}\Delta t\right) \end{aligned} E[(bd[k]−bd[k−1])2]=E(∫t0t0+Δt∫t0t0+Δtnb(τ)nb(t)dτdt)=E(σb2∫t0t0+Δt∫t0t0+Δtδ(t−τ)dτdt)=E(σb2Δt)
即:
bd[k]=bd[k−1]+σbdw[k]b_{d}\left[ k\right] =b_d\left[ k-1\right] +\sigma _{bd}w\left[ k\right] bd[k]=bd[k−1]+σbdw[k]
其中
w[k]∼N(0,1)σbd=σbΔtw[k]\sim \mathcal{N}(0,1)\\ \sigma_{bd}=\sigma_b\sqrt{\Delta t} w[k]∼N(0,1)σbd=σbΔt
bias随机游走的噪声方差从连续时间到离散之间需要乘以Δt\sqrt{\Delta t}Δt
随机误差噪声的标定需要用到Allan方差。IMU内参标定的开源代码实现 。
SLAM中的IMU模型
忽略Scale和Misalignment的影响,只考虑白噪声和bias随机游走:
ωm=ωgt+bω+nωam=agt+ba+na\begin{aligned} \bm{\omega}_{m}&=\bm{\omega}^{\rm gt}+\bold{b}_{\omega}+\bold{n}_{\omega}\\ \bold{a}_{m}&=\bold{a}^{\rm gt}+\bold{b}_{a}+\bold{n}_{a} \end{aligned} ωmam=ωgt+bω+nω=agt+ba+na
其中下标ω\omegaω表示gyro,a\rm aa表示acc,ωm,am\bm{\omega}_m,\bold{a}_mωm,am代表IMU的测量值,上标ωgt,agt\bm{\omega}^{\rm gt},\bold{a}^{\rm gt}ωgt,agt表示IMU测量的真值ground truth。
由于始终受重力加速度影响,理想加速度计的测量与IMU相对世界坐标系的加速度相差一个重力加速度。而理想陀螺仪的测量就是IMU系相对世界系的旋转角速度在IMU系下的表示。
ωgt=ωIagt=IRG(GaI−Gg)\begin{aligned} \bm{\omega}^{\rm gt}&=\bm{\omega}_I \\ \bold{a}^{\rm gt}&={^I}\bold{R}_G({}^{G}\bold{a}_{I}-{}^{G}\bold{g}) \end{aligned} ωgtagt=ωI=IRG(GaI−Gg)
于是
ωm=ωI+bω+nωam=IRG(GaI−Gg)+ba+na\begin{aligned} \bm{\omega}_{m}&=\bm{\omega}_{I}+\bold{b}_{\omega}+\bold{n}_{\omega}\\ \bold{a}_{m}&={^I}\bold{R}_G({}^{G}\bold{a}_{I}-{}^{G}\bold{g})+\bold{b}_{a}+\bold{n}_{a} \end{aligned} ωmam=ωI+bω+nω=IRG(GaI−Gg)+ba+na
陀螺仪和加速度计的偏置被建模为随机游走,分别被高斯白噪声nbω\bold{n}_{\rm{b}\omega}nbω和nba\bold{n}_{\rm ba}nba驱动。
b˙ω=nbωb˙a=nba\dot{\bold{b}}_{\omega}=\bold{n}_{\rm{b}\omega}\\ \dot{\bold{b}}_{a}=\bold{n}_{\rm ba} b˙ω=nbωb˙a=nba
P(ose),V(elocity),Q(uaternion)对时间的导数可写成:
Gp˙I=GvIGv˙I=GaIGq˙I=GqI⊗[012ωI]{^G}\dot{\bold{p}}_I={^G}\bold{v}_{I} \\ {^G}\dot{\bold{v}}_I={^G}\bold{a}_{I} \\ {}^{G}\dot{\bold{q}}_{I}={}^{G}\bold{q}_{I}\otimes\begin{bmatrix}0 \\ \frac{1}{2}\bm{\omega}_I \end{bmatrix} Gp˙I=GvIGv˙I=GaIGq˙I=GqI⊗[021ωI]
根据上面的导数关系,可以从第i时刻的PVQ,通过对IMU的测量值进行积分,得到第j时刻的PVQ:
GvIj=GvIi+∫titjGaI(t)dt=GvIi+∫titj[GRI(t)agt(t)+Gg]dt\begin{aligned} {}^{G}\bold{v}_{I_j}&={}^{G}\bold{v}_{I_i}+\int _{t_i}^{t_j}{}^{G}\bold{a}_{I}(t) {\rm d}t \\&={}^{G}\bold{v}_{I_i}+\int _{t_i}^{t_j}\left[{}^{G}\bold{R}_I(t)\ \bold{a}^{\rm gt}(t)+{}^{G}\bold{g}\right]{\rm d}t \end{aligned} GvIj=GvIi+∫titjGaI(t)dt=GvIi+∫titj[GRI(t) agt(t)+Gg]dt
GpIj=GpIi+∫titjGvI(t)dt=GpIi+∫titj[GvIi+∫tit[GRI(τ)agt(τ)+Gg]dτ]dt=GpIi+GvIiΔt+∫titj∫tit[GRI(τ)agt(τ)+Gg]dτdt\begin{aligned} {}^{G}\bold{p}_{I_j}&={}^{G}\bold{p}_{I_i}+\int _{t_i}^{t_j}{}^{G}\bold{v}_{I}(t){\rm d}t \\&={}^{G}\bold{p}_{I_i}+\int _{t_i}^{t_j}\left[{}^{G}\bold{v}_{I_i}+\int _{t_i}^{t}\left[{}^{G}\bold{R}_I(\tau)\ \bold{a}^{\rm gt}(\tau)+{}^{G}\bold{g}\right]{\rm d}\tau\right]{\rm d}t \\&={}^{G}\bold{p}_{I_i}+{}^{G}\bold{v}_{I_i}\Delta t+\int _{t_i}^{t_j}\int _{t_i}^{t}\left[{}^{G}\bold{R}_I(\tau)\ \bold{a}^{\rm gt}(\tau)+{}^{G}\bold{g}\right] {\rm d}\tau {\rm d}t \end{aligned} GpIj=GpIi+∫titjGvI(t)dt=GpIi+∫titj[GvIi+∫tit[GRI(τ) agt(τ)+Gg]dτ]dt=GpIi+GvIiΔt+∫titj∫tit[GRI(τ) agt(τ)+Gg]dτdt
GqIj=∫titjGqI(t)⊗[012ωI(t)]dt{}^{G}\bold{q}_{I_j}=\int _{t_i}^{t_j}{}^{G}\bold{q}_{I}(t)\otimes\begin{bmatrix}0 \\ \frac{1}{2}\bm{\omega}_I(t) \end{bmatrix}{\rm d}t GqIj=∫titjGqI(t)⊗[021ωI(t)]dt