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

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

1. 姿态估计

1.1 原理

旋转矩阵的更新方程为：

$$\begin{align} \dot R(t) &= R(t) \Omega (t) \\ \Omega (t)& = \begin{bmatrix} 0 & -\omega _{bz}(t) & \omega _{by}(t) \\ \omega _{bz}(t) & 0 & -\omega _{bx}(t) \\ -\omega _{by}(t) & \omega _{bx}(t) & 0 \\ \end{bmatrix} \end{align}$$

离散化后得到：

$$\begin{align} R(t+\delta t) &=R(t) \cdot exp \begin{pmatrix} \int^{t+\delta t}_t \Omega(t) dt \end{pmatrix} \\ &= R(t) \begin{pmatrix} I+ \frac{sin\sigma}{\sigma} B + \frac{1-cos\sigma}{\sigma ^2}B^2 \end{pmatrix} \end{align}$$

$$\begin{align} B &= \int^{t+\delta t}_t \Omega(t) dt \\ &=\begin{pmatrix} 0 & -\omega _{bz}(t)\delta t & \omega _{by}(t)\delta t \\ \omega _{bz}(t)\delta t & 0 & -\omega _{bx}(t)\delta t \\ -\omega _{by}(t)\delta t & \omega _{bx}(t)\delta t & 0 \\ \end{pmatrix} \end{align}$$

取一阶近似：

$$\color{red}{ R(t+\delta t) = R(t) \begin{pmatrix} I+ B \end{pmatrix} }$$

1.2 误差传播

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

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

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

2. 位置估计

2.1 原理

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

位置、速度的更新方程：

$$v_g(t)=v_g(0)+\int^t_0(a_g(t) - g_g)dt$$

$$s_g(t)=s_g(0)+\int^t_0v_g(t)dt$$

离散化：

$$v_g(t+\delta t) = v_g(t)+\delta t \cdot (a_g (t+\delta t) - g_g)$$

$$s_g(t+\delta t) = s_g(t)+\delta t \cdot v_g (t+\delta t)$$

2.2 误差传播

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

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

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

假设水平倾角（tilt）存在误差 $\epsilon$ ，重力加速度 $g$ 就会在水平方向上投影出 $g \cdot sin(\epsilon)$ 的加速度误差，同时，在垂直方向上产生 $g \cdot (1-cos(\epsilon))$ 的误差。所以，角度的误差主要体现在 $xy$ 平面。

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

$eg.$
设，水平倾角误差 $\epsilon$ 为 $0.05 ^\circ$，$g$ 在水平方向投影为 $0.0086 m/s^2$，30s 后，水平位置的偏差将达到 $7.7m$。