惯导笔记 - 位姿估计误差分析

by luoshi006
参考： An introduction to inertial navigation, Oliver J. Woodman, 2007
参考：https://wenku.baidu.com/view/6aad9bf85f0e7cd1852536a9.html

1. 姿态估计

1.1 原理

旋转矩阵的更新方程为：

R˙(t)Ω(t)=R(t)Ω(t)=⎡⎣⎢0ωbz(t)−ωby(t)−ωbz(t)0ωbx(t)ωby(t)−ωbx(t)0⎤⎦⎥(1)(2) (1) R ˙ ( t ) = R ( t ) Ω ( t ) (2) Ω ( t ) = [ 0 − ω b z ( t ) ω b y ( t ) ω b z ( t ) 0 − ω b x ( t ) − ω b y ( t ) ω b x ( t ) 0 ]

离散化后得到：

R(t+δt)=R(t)⋅exp(∫t+δttΩ(t)dt)=R(t)(I+sinσσB+1−cosσσ2B2)(3)(4) (3) R ( t + δ t ) = R ( t ) ⋅ e x p ( ∫ t t + δ t Ω ( t ) d t ) (4) = R ( t ) ( I + s i n σ σ B + 1 − c o s σ σ 2 B 2 )

B=∫t+δttΩ(t)dt=⎛⎝⎜0ωbz(t)δt−ωby(t)δt−ωbz(t)δt0ωbx(t)δtωby(t)δt−ωbx(t)δt0⎞⎠⎟(5)(6) (5) B = ∫ t t + δ t Ω ( t ) d t (6) = ( 0 − ω b z ( t ) δ t ω b y ( t ) δ t ω b z ( t ) δ t 0 − ω b x ( t ) δ t − ω b y ( t ) δ t ω b x ( t ) δ t 0 )

取一阶近似：

R(t+δt)=R(t)(I+B) R ( t + δ t ) = R ( t ) ( I + B )

1.2 误差传播

对于 MEMS 器件，gyro 的白噪声和零偏是误差的主要来源。

• 白噪声（white noise）引入角度随机游走，其 标准差 随时间的开方次递增；
• 零偏（bias）引入的角度误差，则随时间线性增长；

还包括 gyro 的量化误差（A-D）和 旋转矩阵更新的数值计算误差。

2. 位置估计

2.1 原理

位置更新的计算都在 global 坐标系（NED）。

位置、速度的更新方程：

vg(t)=vg(0)+∫t0(ag(t)−gg)dt v g ( t ) = v g ( 0 ) + ∫ 0 t ( a g ( t ) − g g ) d t

sg(t)=sg(0)+∫t0vg(t)dt s g ( t ) = s g ( 0 ) + ∫ 0 t v g ( t ) d t

离散化：

vg(t+δt)=vg(t)+δt⋅(ag(t+δt)−gg) v g ( t + δ t ) = v g ( t ) + δ t ⋅ ( a g ( t + δ t ) − g g )

sg(t+δt)=sg(t)+δt⋅vg(t+δt) s g ( t + δ t ) = s g ( t ) + δ t ⋅ v g ( t + δ t )

2.2 误差传播

加速度计的误差通过两次积分带入位置估计。

gyro 的误差也同样会带入位置估计;

旋转矩阵将 acc 投影到 global 坐标系。姿态的误差将会导致 acc 投影偏差，在垂直方向上无法准确减去 重力加速度 gg g g 。

假设水平倾角（tilt）存在误差 ϵ ϵ ，重力加速度 g g 就会在水平方向上投影出 g⋅sin(ϵ) g ⋅ s i n ( ϵ ) 的加速度误差，同时，在垂直方向上产生 g⋅(1−cos(ϵ)) g ⋅ ( 1 − c o s ( ϵ ) ) 的误差。所以，角度的误差主要体现在 xy x y 平面。

几乎所有 INS 计算中，gyro 的误差都是位置误差的主要来源。因为，大部分应用场景中，重力加速度 g g 都远大于 运动加速度。

eg. e g .
设，水平倾角误差 ϵ ϵ 为 0.05∘ 0.05 ∘ ， g g 在水平方向投影为 0.0086m/s2 0.0086 m / s 2 ，30s 后，水平位置的偏差将达到 7.7m 7.7 m 。