IMU 运动估计及预积分

IMU 运动估计及预积分

1. IMU 引入

SLAM (实时定位与地图构建)，是机器人与计算机视觉领域的热门研究课题之一，它是指搭载特定的传感器的主体 (比如搭载相机的无人机)，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。其中，SLAM 定位模块需要估计出主体位姿 (旋转量 + 位移量)，而传统 SLAM 算法多基于相机或激光雷达等单一传感器获取外界环境信息。以相机为例，是根据相机成像随运动的变化基于多视图几何原理反向估计相机运动，受外界环境因素影响较大且精度有限。

而 IMU (惯性传感器) 可实时并准确测量搭载主体的加速度与角速度，通过对加速度对时间双重积分计算位移量，对角速度时间积分得到旋转量，从而准确估计出位姿，实现实时定位功能。

2. IMU 运动估计

2.1 IMU 运动方程

设惯性传感器（IMU）测量的角速度、加速度为 $\{ \boldsymbol w_m^b, \boldsymbol a_m^b \}$，实际搭载主体的角速度、加速度为 $\{ \boldsymbol w^b, \boldsymbol a^w \}$，则有：

$$\boldsymbol w_m^b = \boldsymbol w^b + \boldsymbol b^g + \boldsymbol n^g, \\ \boldsymbol a_m^b = \boldsymbol R_{bw}(\boldsymbol a^w + \boldsymbol g^w) + \boldsymbol b^a + \boldsymbol n^a.$$

其中，上标 $w, b$ 分别表示世界坐标系和 IMU 坐标系， $\{ \boldsymbol b^g, \boldsymbol b^a \}$ 指陀螺仪和加速度计的高斯偏置， $\{ \boldsymbol n^g, \boldsymbol n^a \}$ 为随机噪声。

$t$ 时刻下，IMU 帧的位姿可表示为 $\{ \boldsymbol p_{wb_t}, \boldsymbol q_{wb_t} \}$，其中 $\boldsymbol p_{wb_t}$ 为平移量， $\boldsymbol q_{wb_t}$ 为旋转矩阵对应四元数，速度为 $\boldsymbol v_t^w$。因此，对于 $i \to j$ 时刻 IMU 运动方程为：

$$\boldsymbol p_{wb_j} = \boldsymbol p_{wb_i} + \boldsymbol v_i^w \Delta t + \int\int_{t\in[i, j]}(\boldsymbol R_{wb_t} \boldsymbol a^{b_t} - \boldsymbol g_w) dt^2, \\ \boldsymbol v_j^w = \boldsymbol v_i^w + \int_{t\in[i, j]}(\boldsymbol R_{wb_t}\boldsymbol a^{b_t} - \boldsymbol g^w) dt,\\ \boldsymbol q_{wb_j} = \int_{t\in[i,j]} \boldsymbol q_{wb_t} \otimes [0, \frac{1}{2} \boldsymbol w^{b_t}] dt.$$

其中， $\boldsymbol a^{b_t} = \boldsymbol a_m^{b_t} - \boldsymbol b^{a_t} - \boldsymbol n^a$，即 IMU 实际加速度； $\boldsymbol w^{b_t} = \boldsymbol w_m^{b_t} - \boldsymbol b^g - \boldsymbol n^g$ 为 IMU 实际角速度； $\Delta t = t_j - t_i$ 为时间间隙。

当然，实际测量的 IMU 数据时间上是离散的，不能直接积分，但可以采用 IMU 测量时间间隙前后的两次测量数据的平均值来近似积分。

2.2 IMU 运动估计——python代码实现

数据准备

这里使用 KITTI 官方提供的数据集 2011_09_26_driver_001_sync，其中 IMU 数据位于 oxt 目录下，其结构如下：

oxts
├──data	# 各 IMU 帧对应测量数据
│   ├── 0000000000.txt # 包含 IMU 数据在内的 30 个数据
│   ├── 0000000001.txt
│   ├── ...
│   └── 0000000107.txt
├── dataformat.txt	# 测量数据 txt 文件内容格式 (包括 GPS 数据)
└── timestamps.txt	# 各 IMU 帧对应时间戳

这里，下载路径为/home/whitby/Documents/data/kitti，则 IMU 时间戳文件和数据目录为：

import os

# data path
kitti_path = "/home/whitby/Documents/data/kitti"
sequence = "2011_09_26/2011_09_26_drive_0001_sync"

time_path = os.path.join(kitti_path, sequence, "oxts/timestamps.txt")
imu_data_path = os.path.join(kitti_path, sequence, "oxts/data")

读取 timestamps.txt 时间戳文件 python 代码为：

import numpy as np

def get_second(t):
    """时间格式转换"""
    t = [float(x) for x in t.split(':')]
    return t[0] * 3600 + t[1] * 60 + t[2]

times = []
with open(time_path) as f:
    for line in f.readlines():
        times.append(line.split(' ')[1])

times = np.array([get_second(x) for x in times], dtype=float)

数据集 2011_09_26_driver_001_sync IMU 帧总数为 108，对应有 108 个 txt 数据文件。其中，一个数据文件中共有 30 个测量数据，包含 GPS，这里仅读取 IMU 的加速度与角速度。

imu_nums = len(os.listdir(imu_data_path)) # IMU 数据文件数量，这里为 108

imu_a = []	# 加速度
imu_w = []	# 角速度
for i in range(imu_nums):
    with open(imu_data_path + "/%010d.txt" % i, 'r') as f:
        imu = f.readline().strip().split(' ')
        imu = np.array([x for x in imu], dtype=float)
        imu_a.append(imu[11:14])
        imu_w.append(imu[17:20])

IMU 里程计

对于 [a, b] 上连续函数 f(x)，已知 f(a) 和 f(b)，区间积分 \int_a^b f(x) \mathrm dx 运用梯形法则估计有：

$$\int_a^b f(x) \mathrm dx \approx \frac{b - a}{2}(f(a) + f(b))$$

应用梯形法则，IMU 运动方程可改为：

$$\boldsymbol p_{wb_j} \approx \boldsymbol p_{wb_i} + \boldsymbol v_i^w \Delta t + \frac{1}{4}(\boldsymbol R_{wb_i} \boldsymbol a^{b_i} + \boldsymbol R_{wb_j} \boldsymbol a^{b_j} - 2\boldsymbol g_w) \Delta t^2, \\ \boldsymbol v_j^w \approx \boldsymbol v_i^w + \frac{1}{2}(\boldsymbol R_{wb_i} \boldsymbol a^{b_i} + \boldsymbol R_{wb_j} \boldsymbol a^{b_j} - 2\boldsymbol g_w) \Delta t, \\ \boldsymbol q_{wb_j} = \boldsymbol q_{wb_i} \otimes [0, \frac{1}{4}(\boldsymbol w^{b_i} + \boldsymbol w^{b_j})] \Delta t.$$

现以第一 IMU 帧建立世界坐标系，设其位姿对应变换矩阵为单位矩阵，忽略 IMU 偏置及噪声影响，并假设重力加速度恒定不变，在世界坐标系下 $\boldsymbol g = [0, 0, 9.8]^T$，IMU 初始速度取 KITTI 数据初始速度 $\boldsymbol v_0 = [13.172716663769, -0.12475264293164, -0.032919903047354]^T$。

则各 IMU 帧位姿计算 python 代码为：

from liegoups.numpy import SO3

initial_velocity = np.array([13.172716663769, -0.12475264293164, -0.032919903047354], dtype=float)	# 初始速度 v_0
gravity = np.array([0, 0, 9.8], dtype=float)	# 假设重力加速度恒定

poses = {"R": [], "p": [],"v": [], "timestamps": []}

# 初始化第一帧
poses["R"].append(np.identity(3))
poses["p"].append(np.zeros(3))
poses["v"].append(initial_velocity)
poses["timestamps"].append(times[0])

for i in range(len(times) - 1):
    j = i + 1
    dt = times[j] - times[i]
    
    R_wi = poses["R"][i]
    R_wj = R_wi @ SO3.exp(0.25 * (imu_w[i] + imu_w[j]) * dt).mat
    
    v_wi = poses["v"][i]
    v_wj = v_wi + 0.5 * (R_wi @ imu_a[i] + R_wj @ imu_a[j]) * dt

    p_wi = poses["p"][i]
    p_wj = p_wi + v_wi * dt + 0.25 * (R_wi @ imu_a[i] + R_wj @ imu_a[j]) * dt * dt

    poses["p"].append(p_wj)
    poses["R"].append(R_wj)
    poses["v"].append(v_wj)
    poses["timestamps"].append(times[j])

3. 预积分

IMU 可以测量出搭载主体加速度与角速度，从而估计位姿，实现实时定位功能，但是要构建地图需要与相机、激光雷达等传感器结合使用。以视觉惯性 SLAM 系统 (相机 + IMU) 为例，要实现地图构建，需要使用 IMU 估计相机位姿。而 IMU 估计相机位姿需要将 IMU帧时间戳对齐相机帧，计算出对齐时间戳 IMU 位姿后通过外置参数估计相机位姿。

而 IMU 频率一般高于相机，以 KITTI 数据为例，一个相机时间大约等于 11 个 IMU 时间。设相机时间 $t_{c_0}$ 对应 IMU 时间 $t_{b_0}$, 相机时间 $t_{c_1}$ 对应 IMU 时间 $t_{b_{11}}$，则两相机时间内相机变换位姿 $\{\mathbf R_c, \mathbf t_c\}$ 可由 IMU 变换位姿 $\{ \boldsymbol q_{b_0 b_{11}}, \boldsymbol p_{b_0 b_{11}} \}$ 计算得出。

而要计算 IMU 11个时间内的变换位姿可以按上述 IMU 运动方程迭代计算 11 次，但这样比较耗费时间。实际上还存在另一中更简便的计算方法。在 IMU 运动方程中，重力加速度 $\boldsymbol g$ 和 初始速度 $\boldsymbol v_{b_0}$ 与后续时间内测量的加速度和角速度无关，可将 IMU 运动方程整理为：

\begin{align} & \Delta t = t_{b_{11}} - t_{b_0} \\ & \boldsymbol p_{b_0 b_{11}} = \boldsymbol v_{b_0} \cdot \Delta t - \frac{1}{2} \boldsymbol g \Delta t^2 + \int\int_{t\in[0, 11]} \mathbf R_{b_0b_t} \boldsymbol a_t \mathrm dt^2 \\ & \boldsymbol v_{b_{11}} = \int_{t\in[0, 11]} \mathbf R_{b_0 b_t} \boldsymbol a_t \mathrm dt - \boldsymbol g \Delta t \\ & \boldsymbol q_{b_0 b_{11}} = \int_{t\in[0, 11]} \boldsymbol q_{b_0 b_t} \otimes [0, \frac{1}{2} \boldsymbol w^{b_t}] \mathrm dt \end{align}

以上公式，仅积分部分依赖 IMU 测量数据，需要迭代计算。于是有 IMU 三个预积分量：

$$\boldsymbol\alpha_{b_i b_j} = \int\int_{t\in[i, j]}(\boldsymbol R_{b_i b_t} \boldsymbol a^{b_t}) \mathrm dt^2, \\ \boldsymbol \beta_{b_i b_j} = \int_{t\in[i,j]}(\boldsymbol R_{b_i b_t} \boldsymbol a^{b_t}) \mathrm d t, \\ \boldsymbol q_{b_i b_j} = \int_{t\in[i, j]} \boldsymbol q_{b_i b_t} \otimes [0, \frac{1}{2}\boldsymbol w^{b_t}]^T \mathrm dt.$$

通过这种计算方式，可进一步降低计算量，提高 SLAM 运行速度，保证实时性。