State Estimation and Localization for Self-Driving Cars 第五周作业组合导航

松耦合组合导航系统

最新推荐文章于 2025-02-06 15:20:11 发布

原创

最新推荐文章于 2025-02-06 15:20:11 发布 · 置顶 · 723 阅读

2 ·

CC 4.0 BY-SA版权

本文介绍了一种基于GNSS、IMU和LIDAR的松耦合组合导航方案，详细阐述了系统的运动模型、误差状态模型及滤波方程。通过融合多种传感器数据，实现车辆精准定位。

本文采用(GNSS+IMU+LIDAR)的组合导航方案实现车辆的导航。这里采用松耦合的导航方式，即通过对GNSS和LIDAR的测量数据进行预处理以直接获取车辆的位置，方案图如下：

1 系统模型

汽车的运动状态包括位置，速度和姿态
$xk=[pkvkqk]∈R10\mathbf{x}_{k}=\left[\begin{array}{c} \mathbf{p}_{k} \\ \mathbf{v}_{k} \\ \mathbf{q}_{k} \end{array}\right] \in R^{10}$
其中，四元素 $qk\mathbf{q}_{k}$ 采用Hamilton约定。
运动模型的输入是IMU测量的比力和角速度
$uk=[fkωk]∈R6\mathbf{u}_{k}=\left[\begin{array}{c} \mathbf{f}_{k} \\ \omega_{k} \end{array}\right] \in R^{6}$

1.1 状态运动模型

利用欧拉法进行离散化得到的运动模型如下：
$pk=pk−1+Δtvk−1+Δt22(Cnsfk−1+g)vk=vk−1+Δt(Cnsfk−1+g)qk=qk−1⊗q(ωk−1Δt)=Ω(q(ωk−1Δt))qk−1\begin{aligned} &\mathbf{p}_{k}=\mathbf{p}_{k-1}+\Delta t \mathbf{v}_{k-1}+\frac{\Delta t^{2}}{2}\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}+\mathbf{g}\right)\\ &\mathbf{v}_{k}=\mathbf{v}_{k-1}+\Delta t\left(\mathbf{C}_{n s} \mathbf{f}_{k-1}+\mathbf{g}\right)\\ &\mathbf{q}_{k}=\mathbf{q}_{k-1} \otimes \mathbf{q}\left(\omega_{k-1} \Delta t\right)=\Omega\left(\mathbf{q}\left(\omega_{k-1} \Delta t\right)\right) \mathbf{q}_{k-1} \end{aligned}$

其中，
$qv}×]q(θ)=[sin⁡∣θ∣2θ∣θ∣cos⁡∣θ∣2]\mathbf{C}_{n s}=\mathbf{C}_{n s}\left(\mathbf{q}_{k-1}\right) \quad \mathbf{\Omega}\left(\left[\begin{array}{c} q_{w} \\ \mathbf{q}_{v} \end{array}\right]\right)=q_{w} \mathbf{1}+\left[\begin{array}{cc} 0 & -\mathbf{q}_{v}^{T} \\ \mathbf{q}_{v} & -\left\{\mathbf{q}_{v}\right\}_{\times} \end{array}\right] \quad \mathbf{q}(\boldsymbol{\theta})=\left[\begin{array}{c} \sin \frac{|\boldsymbol{\theta}|}{2} \\ \frac{\boldsymbol{\theta}}{| \boldsymbol{\theta}|} \cos \frac{|\boldsymbol{\theta}|}{2} \end{array}\right]$