多传感器融合定位------基于ESKF的组合导航

🕗 发布于 2024-10-17 03:40 机器人

前言

本博客中，R(旋转)，p(平移)类变量默认取下标 ()wb；v(线速度)，w(角速度)，a(加速度)，g(重力)默认在世界坐标系下； $\tilde{a}$ (加速度计测量值)， $\tilde{\omega}$ (陀螺仪测量值)默认在车体坐标系下。

机器人连续时间的运动学方程为：

$\begin{aligned}&\dot{R}_{wb}=R_{wb}\omega^{\wedge}_{w},\quad\text{or }\dot{q}=\frac{1}{2}q\omega,\\&\dot{p}_{wb}=v_{w},\\&\dot{v}_{w}=a_{w}.\end{aligned}$

根据默认下标规则可简写为：

$\begin{aligned}&\dot{R}=R\omega^{\wedge},\quad\text{or }\dot{q}=\frac{1}{2}q\omega,\\&\dot{p}=v,\\&\dot{v}=a.\end{aligned}$

一、定位相关的传感器介绍

1.IMU（惯性测量单元）

典型的六轴IMU由陀螺仪（gyroscope）和加速度计（accelerometer）组成。

1.1 IMU 的测量

IMU 测量值：IMU 坐标系下 xyz 三轴的角速度和加速度。如果 IMU 安装在车辆中心（本博客中该假设成立），车体坐标系和 IMU 坐标系重合，测量值即为车体坐标系下的角速度和加速度。当 IMU 水平放置时，其加速度计的测量值为 (0, 0, 9.8)；IMU 自由落体时，其加速度计的测量值为 (0, 0, 0)

IMU 的噪声：IMU的噪声由两部分组成，即测量噪声（Measurement Noise）与零偏（Bias）。由于IMU内部机电测量装置、温度和时间的影响，即使在车辆静止时，IMU 的角速度和加速度输出也不一定形成均值为 0 的白噪声，而是带有一定的偏移，这个偏移量即为零偏。

IMU 的测量方程：

$\begin{aligned}&\tilde{a}_{b}=R_{wb}^{\top}(a_{w}-g_{w})+b_{a}+\eta_{a},\\&\tilde{\omega}_{b}=\omega_{w}+b_{g}+\eta_{g}.\end{aligned}$

这里角速度从世界坐标系到车体坐标系为什么没有乘 R 变换？？？ 解释正确吗？？？

根据默认下标规则可简写为：

$\begin{aligned}&\tilde{a}=R^{\top}(a-g)+b_{a}+\eta_{a},\\&\tilde{\omega}=\omega+b_{g}+\eta_{g}.\end{aligned}$

其中陀螺仪和加速度计的测量噪声分别为 $\eta_{g}$ ， $\eta_{a}$ ，零偏分别为 $b_{g}$ ， $b_{a}$ ，下标 ${g}$ 表示陀螺仪， ${a}$ 表示加速度计。

在连续时间下，我们认为 IMU 测量噪声是一个方差为 $\mathrm{Cov}(\boldsymbol{\eta}_{g}),\mathrm{Cov}(\boldsymbol{\eta}_{a})$ 的零均值白噪声高斯过程 (Zero-mean White Gaussian process) 。

同时，认为零偏是一个维纳过程（Wiener Process），或称布朗运动（Brownian Motion）或随机游走（Random Walk）。一个通常的零偏 $b$ 的随机游走过程可以建模为：

$\dot{b}(t)=\boldsymbol{\eta}_b(t)$

其中 $\eta_b(t)$ 也是一个高斯过程。

1.2 使用 IMU 数据进行短时间航迹（位姿）推算

将 IMU的测量方程带入机器人连续时间的运动方程，忽略测量噪声 $\eta_{g}$ 和 $\eta_{a}$ ，可得连续时间下 IMU 的积分模型：

$\begin{aligned}&\dot{p_{wb}}=v_{w},\\&\dot{v_{w}}=R_{wb}(\tilde{a_{b}}-b_{a})+g_{w},\\&\dot{R_{wb}}=R_{wb}(\tilde{\omega}_{b}-b_{g})^{\wedge},\quad\text{or }\dot{q}=q\left[0,\frac{1}{2}\left(\tilde{\omega}-b_{g}\right)\right].\end{aligned}$

根据默认下标规则可简写为：

$\begin{aligned}&\dot{p}=v,\\&\dot{v}=R(\tilde{a}-b_{a})+g,\\&\dot{R}=R(\tilde{\omega}-b_{g})^{\wedge},\quad\text{or }\dot{q}=q\left[0,\frac{1}{2}\left(\tilde{\omega}-b_{g}\right)\right].\end{aligned}$

对上述模型，从时间 $t$ 积分至 $t+\Delta t$ ，可以推出下一个时刻的状态情况：

$\begin{aligned}&p(t+\Delta t)=p(t)+v\Delta t+\frac{1}{2}\left(R(t)(\tilde{a}-b_{a})\right)\Delta t^{2}+\frac{1}{2}g\Delta t^{2},\\&v(t+\Delta t)=v(t)+R(t)(\tilde{a}-b_{a})\Delta t+g\Delta t,\\&R(t+\Delta t)=R(t)\mathrm{Exp}\left((\tilde{\omega}-b_{g})\Delta t\right),\quad\text{or }q(t+\Delta t)=q(t)\left[1,\frac{1}{2}\left(\tilde{\omega}-b_{g}\right)\Delta t\right].\end{aligned}$

根据该式，可以使用 一个时刻的状态 加上 下一个时刻的 IMU 观测数据 递推出 下一个时刻的状态 。

1.3 IMU航迹推算精度

以四元数表达的姿态整体上仍能保持稳定，但位移方面则很快发散。其原因是由于缺少外部观测，速度状态很快超出了控制，使得位置估计发散到一个很大的数值。

2.RTK（Real-Time Kinematic，实时动态差分）

2.1 RTK 的测量

双天线 RTK 的测量值：测量值为地理坐标系下的经度、纬度、高度、方向角和方向角有效位标志。RTK 的测量值不能直接使用，需要通过转换程序（库函数）将 RTK 测量值中的经纬度信息转换为米制的 UTM 坐标下的东北天坐标系（将正东视为 X 轴，正北视为 Y 轴，按照右手坐标系，Z 轴应该指向天空）或者北东地坐标系。UTM 坐标可视为 $T_{wg}$ （注意，这里的下标 $g$ 表示 GNSS 主天线接收器的坐标系），还需考虑 RTK 的安装外参 $T_{gb}$ 将 UTM 坐标转换为 $T_{wb}$ 。

$T_{wb}=T_{wg}T_{gb}.$

将旋转和平移拆开写得：

$R_{wb}=R_{wg}R_{gb},\quad t_{wb}=R_{wg}t_{gb}+t_{wg}.$

注意

① 在单天线 RTK 方案中，可通过 RTK 测量值得到 $t_{wg}$ ，通过 RTK 外参得到 $t_{gb}$ ，但由于单天线不能获得 RTK 的朝向 $R_{wg}$ ，此时不能真正确定 $t_{wb}$ 。当然，在状态估计算法中，车辆姿态 $R_{wb}$ 存在估计值，RTK 外参 $R_{bg}$ 已知，此时也可以使用 $R_{wb}R_{bg}$ 来作为当时的 $R_{wg}$ 。

② 通常在得到第一帧 $t_{wb}$ 后，将其作为原点坐标，后续的 $t_{wb}$ 均减去第一帧的 $t_{wb}$ 。

2.2 RTK 精度

使用 RTK 测量值计算得到的 $T_{wb}$ ，其中平移分量的 $x ,y$ 部分数据精度较高， $z$ 部分数据精度通常不如水平坐标。RTK 测量得到的姿态相比位置更不稳定。

2.3 总结

IMU 和 RTK 具有互补性。① IMU 有累积误差，但是没有位姿跳变，RTK 没有累积误差，但受干扰或遮挡时会跳变。二者融合，既能消除累积误差，又能避免跳变。② IMU 测量做航推算时，姿态 $R$ 精度较高；RTK 测量得到的位置 $t$ 精度较高。二者可融合得到 $R,t$ 精度都不错的位姿。

3.轮速计、轮式编码器、轮式里程计

轮式编码器的测量值：①轮速编码器可以输出轮子转过的距离，每隔固定的时间对该距离进行采样，可以得到轮子的速度 $v$ 。②轮速编码器只能测量车辆前进方向上的速度和位移，速度即为车体坐标系下的速度 $v_{b}$ 。

注意：①能够被各种传感器（例如车速传感器、轮速编码器等）测量到的速度均为车体坐标系下的速度 $v_{b}$ 。②在 GINS系统（GNSS/INS：Global Navigation Satellite System/Inertial Navigation System）中，如果长时间缺少 RTK 观测数据，ESKF 就变成纯依靠 IMU 积分的递推模式，该模式下位移 $t$ 由于缺少速度 $v$ 的观测将很快发散。此时引入速度观测有助于 GINS
系统的稳定性。

二、使用ESKF实现IMU和RTK的组合导航

把 IMU 视为运动模型，并把 RTK 观测视为观测模型，推导 ESKF（误差状态卡尔曼滤波器）。

原文地址：https://blog.csdn.net/m0_49384824/article/details/142986573

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

上一篇：mysql 09 独立表空间结构
下一篇：电子界桩在森林公园的应用方案

【Linux】进程优先级&&进程切换
在操作系统中，进程优先级（Process Priority）是指操作系统对进程进行调度时所赋予的优先级值，用于决定进程何时获得处理器时间。进程优先级是一种实现多任务操作系统中任务调度的重要机制，它允许
阅读更多2024-10-17
外包干了5天，技术明显退步
我是一名本科生，自2019年起，我便在南京某软件公司担任功能测试的工作。这份工作虽然稳定，但日复一日的重复性工作让我逐渐陷入了舒适区，失去了前进的动力。两年的时光匆匆流逝，我却在原地踏步，技术没有丝
阅读更多2024-10-17
《深度学习》OpenCV 风格迁移、DNN模块案例解析及实现
风格迁移是一种计算机视觉技术，通过将一幅图像的风格与另一幅图像的内容结合，生成一个新的图像。在OpenCV中，风格迁移通常使用神经网络来实现。import cv2def resize(image,wi
阅读更多2024-10-17
搜维尔科技：力反馈五指灵巧手数据手套解决方案
搜维尔科技：力反馈五指灵巧手数据手套解决方案。五指灵巧手数据手套解决方案。
阅读更多2024-10-17
mysql的重置
今天用Navicat16去连接mysql突然就连不上了。一直报错连接本地mysql时出现2003-Can‘t connect to MySql server on ‘localhost‘(10061)
阅读更多2024-10-17
【智能大数据分析 | 实验三】Storm实验：实时WordCountTopology
智能大数据分析实验三，Storm实验：实时WordCountTopology。掌握如何用Java代码来实现Storm任务的拓扑，理解一个拓扑中Spout和Bolt的关系及如何组织它们之间的关系，最后将
阅读更多2024-10-17
C++和OpenGL实现3D游戏编程【连载15】——着色器初步
OpenGL着色器（Shader）是用着色器语言(OpenGL Shading Language, GLSL)写的，是一种在图形渲染管线中用于执行特定渲染计算的小程序。当今大多数显卡都有成千上万的小处
阅读更多2024-10-17
【无标题】
本文提出了一种处理账户余额模型的分片系统中跨分片交易的处理协议，基于账户间的交易图切分以及账户分割机制来平衡分片间的负载，同时引入做市商账户协调跨片交易的处理过程，过程中利用“状态时间锁”保证跨片交易
阅读更多2024-10-17
Spring Boot异步任务、任务调度与异步请求线程池的使用及原理
默认情况下，Spring会使用来执行异步任务。然而，不是真正的线程池，每次调用都会创建一个新的线程，这在高并发情况下会导致性能问题。因此，通常建议自定义线程池。@Override本文详细讲解了Spri
阅读更多2024-10-17
centos nvidia-docker2安装及使用
可选：您还可以选择配置存储库，以便使用实验性软件包。idia-docker2安装。
阅读更多2024-10-17

多传感器融合定位------基于ESKF的组合导航

前言

一、定位相关的传感器介绍

1.IMU（惯性测量单元）

1.1 IMU 的测量

1.2 使用 IMU 数据进行短时间航迹（位姿）推算

1.3 IMU航迹推算精度

2.RTK（Real-Time Kinematic，实时动态差分）

2.1 RTK 的测量

2.2 RTK 精度

2.3 总结

3.轮速计、轮式编码器、轮式里程计

二、使用ESKF实现IMU和RTK的组合导航

相关文章