『imu_tk』工具标定IMU的基本原理
IMU误差模型
- 对于加速度计和陀螺仪，定义正交系统AOF(GOF)和非正交系AF(GF)：
- AOF(GOF)系的x轴与AF(GF)系x轴重合
- AOF(GOF)系的y轴与AF(GF)系的xy平面重合
- 同时，定义正交系BF非正交系S是载体系统的传感器系统。当旋转角度较小时，可以定义非正交系统中的测量 s S s^S sS正交系B的转换为： s B = T s S , T = [ 1 ? β y z β z y β x z 1 ? β z x ? β x y β y x 1 ] s^B=Ts^S, \ \ \ T = \begin{bmatrix} 1 & -\beta_{yz} & \beta_{zy}\\ \beta_{xz} & 1 & -\beta_{zx} \\ -\beta_{xy} & \beta_{yx} & 1 \end{bmatrix} sB=TsS,T=??1βxz−βxy−βyz1βyxβzy−βzx1⎦⎤ 其中， β i j \beta_{ij} βij代表S系中的 i i i轴绕BF系中 j j j轴的旋转。
对于IMU标定，可以分为加速度计标定和陀螺仪标定两部分。其中，二者待标定参数的形式上是相同的。（其中，上标 s s s代表非正交系中的向量，上标 o o o代表正交系中的向量）
- 加速度计模型 ( a c c ) (acc) (acc)： a O = T a K a ( a S + b a + v a ) a^O = T^aK^a(a^S+b^a+v^a) aO=TaKa(aS+ba+va)
- 陀螺仪模型 ( g y r o ) (gyro) (gyro)： w O = T g K g ( w S + b g + v g ) w^O = T^gK^g(w^S+b^g+v^g) wO=TgKg(wS+bg+vg)
- 对于加速度计和陀螺仪的每个轴上都有比例因子误差和交轴耦合误差，分别定义为 K g , K a K^g,K^a Kg,Ka 和 T g , T a T^g,T^a Tg,Ta。对于二者的量测噪声 v g , v a v^g,v^a vg,va，可以看做是高斯白噪声，由于本文采用静止区间取测量均值的方法进行计算，。
其中， T g = [ 1 − γ y z γ z y γ x z 1 − γ z x − γ x y γ y x 1 ] ， K g = [ s x g 0 0 0 s y g 0 0 0 s z g ] T^g = \begin{bmatrix} 1 & -\gamma_{yz} & \gamma_{zy}\\ \gamma_{xz} & 1 & -\gamma_{zx} \\ -\gamma_{xy} & \gamma_{yx} & 1 \end{bmatrix}，K^g = \begin{bmatrix} s^g_x & 0 &0 \\ 0 & s^g_y & 0\\ 0 & 0 & s^g_z \end{bmatrix} Tg=⎣⎡1γxz−γxy−γyz1γyxγzy−γzx1⎦⎤，Kg=⎣⎡sxg000syg000szg⎦⎤ 我们定义AOF系与BF系完全重合，故 T a T^a Ta 可以简化为： T a = [ 1 − α y z α z y 0 1 − α z x 0 0 1 ] ， K a = [ s x a 0 0 0 s y a 0 0 0 s z a ] T^a = \begin{bmatrix} 1 & -\alpha_{yz} & \alpha_{zy}\\ 0 & 1 & -\alpha_{zx} \\ 0 & 0 & 1 \end{bmatrix}，K^a = \begin{bmatrix} s^a_x & 0 &0 \\ 0 & s^a_y & 0\\ 0 & 0 & s^a_z \end{bmatrix} Ta=⎣⎡100−αyz10αzy−αzx1⎦⎤，Ka=⎣⎡sxa000sya000sza⎦⎤
利用优化的方法求解
对于加速度计，我们定义损失函数：（ g g g是当地重力加速度） L ( θ a c c ) = ∑ k = 1 M ( ∣ ∣ g ∣ ∣ 2 − ∣ ∣ a O ∣ ∣ 2 ) 2 L(\theta^{acc})=\sum^M_{k=1}(||\bold{g}||^2-||\bold{a}^O||^2)^2 L(θacc)=k=1∑M(∣∣g∣∣2−∣∣aO∣∣2)2 对于陀螺仪，我们定义损失函数： L ( θ g y r o ) = ∑ k = 2 M ∣ ∣ u a , k − u g , k ∣ ∣ 2 L(\theta^{gyro})=\sum^M_{k=2}{||\bold{u}_{a,k}-\bol