新手想做网站赚钱保定制作网站软件

理想的惯性测量单元包含三个正交、线性的加速度计和三个正交的陀螺仪。它们分别测量三个相互正交的坐标轴的加速度和角速率；但是现实加速度计和陀螺仪分别制造，坐标系中心不一致，甚至轴之间也是不完全正交的。此外，单个传感器也不是完美点：通常用于将传感器的数字输出转换为实际物理量的比例因子对于同一传感器的不同实例是不同的，而制造商只提供默认的标称比例因子。并且输出信号总是受到非零可变的偏置的影响。
加速度计坐标系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 $ϵ$ in the accelerometer’s output signal	A steadily growing angular error$$\theta (t)=ϵ\cdot t$$	A quadratically growing position error$$s(t)=ϵ\cdot \frac{t^2}{2}$$
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 $${\sigma }_{\theta }(t)=\sigma \cdot \sqrt{\delta t\cdot t}$$grows with the square root of time	A second-order random walk. The standard deviation of the position error grows as$${\sigma }_s(t)=\sigma \cdot t^{3/2}\cdot \sqrt{\frac{\delta t}{3}}$$
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随机游走.

确定性误差

1. Bias：理论上，当没有外部作用时，IMU传感器的输出应该为0，但是实际存在一个偏置$\mathbf{b}$。

   • 陀螺仪的偏置是陀螺仪不进行任何旋转时的平均输出(即:输出与真实值的偏移量)，单位是${}^{\circ }/h$
     一个恒定的偏置误差${\mathbf{b}}_{\omega }$随着时间积分，造成的角度偏差随时间线性积累$\theta (t)={\mathbf{b}}_{\omega }t$
   • 加速度计bias对位姿估计的影响：
     $${\mathbf{v}}_{err}={\mathbf{b}}_{a}t,{\mathbf{p}}_{err}=\frac{1}{2}{\mathbf{b}}_a{t}^2$$

2. Scale：可以看作是实际数值和传感器输出值之间的比值

3. Nonorthogonality/Misalignment Errors：多轴IMU传感器制作时，由于制作工艺问题，会使xyz轴不正交
   scale+Misalignment
   $$\left[\begin{array}{c}{a}_{m_x}\\ a_{m_y}\\ a_{m_z}\end{array}\right]=\left[\begin{array}{ccc}{s}_{xx}& m_{xy}& m_{xz}\\ m_{yx}& s_{yy}& m_{yz}\\ m_{zx}& m_{zy}& s_{zz}\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$

随机误差

高斯白噪声

IMU数据连续时间上受到一个均值为0，方差为$\sigma$，各时刻之间相互独立的高斯过程$n(t)$：
$$\begin{gathered} E[n(t)]\equiv 0 \\ E[n(t_1)n(t_2)]={\sigma }^2\delta (t_1-t_2) \end{gathered}$$
其中$\delta ()$表示狄拉克函数。
实际上，IMU传感器获取的数据为离散采样，采样保持时间为$\Delta t$，离散和连续高斯白噪声的方差之间存在如下转换关系：
$$\begin{array}{rl}{n}_d\left[K\right]& \simeq \frac{1}{\Delta t}{\int }_{t_0}^{t_0+\Delta t}n\left(t\right)\mathrm{d}t\\ E\left(n_d{\left[k\right]}^2\right)& =E\left(\frac{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)\mathrm{d}\tau \mathrm{d}t\right)\\ & =E\left(\frac{{\sigma }^2}{\Delta t^2}{\int }_{t_0}^{t_0+\Delta t}{\int }_{t_0}^{t_0+\Delta t}\delta \left(t-t\right)\mathrm{d}\tau \mathrm{d}t\right)\\ & =E\left(\frac{{\sigma }^2}{\Delta t}\right)\end{array}$$
即
$$n_d[k]={\sigma }_{d}w[k]$$
其中
$$\begin{gathered} w[k]\sim \mathcal{N}(0,1) \\ {\sigma }_d=\sigma \frac{1}{\Delta t} \end{gathered}$$
也就是说高斯白噪声的连续时间到离散时间之间差一个$\frac{1}{\sqrt{\Delta t}}$，$\Delta t$是传感器采样时间。

Bias随机游走

通常用维纳过程(wiener process)来建模bias随时间连续变化的过程，离散时间下称之为随机游走。
$$\dot{b}(t)=n_b(t)={\sigma }_{b}w(t)$$
其中$w$是方差为1的白噪声
同样，离散和连续之间的转换：
$$b_d\left[k\right]\triangleq b\left(t_0\right)+{\int }_{t_0}^{t_0+\Delta t}{n}_b\left(t\right)dt$$
$$\begin{array}{rl}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}^{t_0+\Delta t}{\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{array}$$
即：
$$b_d\left[k\right]=b_d\left[k-1\right]+{\sigma }_{bd}w\left[k\right]$$
其中
$$\begin{gathered} w[k]\sim \mathcal{N}(0,1) \\ {\sigma }_{bd}={\sigma }_b\sqrt{\Delta t} \end{gathered}$$
bias随机游走的噪声方差从连续时间到离散之间需要乘以$\sqrt{\Delta t}$

随机误差噪声的标定需要用到Allan方差。IMU内参标定的开源代码实现 。

SLAM中的IMU模型

忽略Scale和Misalignment的影响，只考虑白噪声和bias随机游走：
$$\begin{array}{rl}{\mathbf{\omega }}_m& ={\mathbf{\omega }}^{\mathrm{gt}}+{\mathbf{b}}_{\omega }+{\mathbf{n}}_{\omega }\\ {\mathbf{a}}_m& ={\mathbf{a}}^{\mathrm{gt}}+{\mathbf{b}}_a+{\mathbf{n}}_a\end{array}$$
其中下标$\omega$表示gyro，$\mathrm{a}$表示acc，${\mathbf{\omega }}_m,{\mathbf{a}}_m$代表IMU的测量值，上标${\mathbf{\omega }}^{\mathrm{gt}},{\mathbf{a}}^{\mathrm{gt}}$表示IMU测量的真值ground truth。
由于始终受重力加速度影响，理想加速度计的测量与IMU相对世界坐标系的加速度相差一个重力加速度。而理想陀螺仪的测量就是IMU系相对世界系的旋转角速度在IMU系下的表示。
$$\begin{array}{rl}{\mathbf{\omega }}^{\mathrm{gt}}& ={\mathbf{\omega }}_I\\ {\mathbf{a}}^{\mathrm{gt}}& ={}^I{\mathbf{R}}_G({}^G{\mathbf{a}}_I-{}^G\mathbf{g})\end{array}$$
于是
$$\begin{array}{rl}{\mathbf{\omega }}_m& ={\mathbf{\omega }}_I+{\mathbf{b}}_{\omega }+{\mathbf{n}}_{\omega }\\ {\mathbf{a}}_m& ={}^I{\mathbf{R}}_G({}^G{\mathbf{a}}_I-{}^G\mathbf{g})+{\mathbf{b}}_a+{\mathbf{n}}_a\end{array}$$
陀螺仪和加速度计的偏置被建模为随机游走，分别被高斯白噪声${\mathbf{n}}_{\mathrm{b}\omega }$和${\mathbf{n}}_{\mathrm{ba}}$驱动。
$$\begin{gathered} {\dot{\mathbf{b}}}_{\omega }={\mathbf{n}}_{\mathrm{b}\omega } \\ {\dot{\mathbf{b}}}_a={\mathbf{n}}_{\mathrm{ba}} \end{gathered}$$
P(ose),V(elocity),Q(uaternion)对时间的导数可写成：
$$\begin{gathered} {}^G{\dot{\mathbf{p}}}_I={}^G{\mathbf{v}}_I \\ {}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{a}}_I \\ {}^G{\dot{\mathbf{q}}}_I={}^G{\mathbf{q}}_I\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}_I\end{array}\right] \end{gathered}$$
根据上面的导数关系，可以从第i时刻的PVQ，通过对IMU的测量值进行积分，得到第j时刻的PVQ：
$$\begin{array}{rl}{}^G{\mathbf{v}}_{I_j}& ={}^G{\mathbf{v}}_{I_i}+{\int }_{t_i}^{t_j}{}^G{\mathbf{a}}_I(t)\mathrm{d}t\\ & ={}^G{\mathbf{v}}_{I_i}+{\int }_{t_i}^{t_j}\left[{}^G{\mathbf{R}}_I(t){\mathbf{a}}^{\mathrm{gt}}(t)+{}^G\mathbf{g}\right]\mathrm{d}t\end{array}$$
$$\begin{array}{rl}{}^G{\mathbf{p}}_{I_j}& ={}^G{\mathbf{p}}_{I_i}+{\int }_{t_i}^{t_j}{}^G{\mathbf{v}}_I(t)\mathrm{d}t\\ & ={}^G{\mathbf{p}}_{I_i}+{\int }_{t_i}^{t_j}\left[{}^G{\mathbf{v}}_{I_i}+{\int }_{t_i}^t\left[{}^G{\mathbf{R}}_I(\tau ){\mathbf{a}}^{\mathrm{gt}}(\tau )+{}^G\mathbf{g}\right]\mathrm{d}\tau \right]\mathrm{d}t\\ & ={}^G{\mathbf{p}}_{I_i}+{}^G{\mathbf{v}}_{I_i}\Delta t+{\int }_{t_i}^{t_j}{\int }_{t_i}^t\left[{}^G{\mathbf{R}}_I(\tau ){\mathbf{a}}^{\mathrm{gt}}(\tau )+{}^G\mathbf{g}\right]\mathrm{d}\tau \mathrm{d}t\end{array}$$

$${}^G{\mathbf{q}}_{I_j}={\int }_{t_i}^{t_j}{}^G{\mathbf{q}}_I(t)\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}_I(t)\end{array}\right]\mathrm{d}t$$