《自动驾驶与机器人中的SLAM技术》ch2:基础数学知识

目录

2.1 几何学

        向量的内积和外积

        旋转矩阵

        旋转向量

        四元数 

         李群和李代数

        SO(3)上的 BCH 线性近似式 

 2.2 运动学

        李群视角下的运动学

        SO(3) + t 上的运动学

         线速度和加速度

        扰动模型和雅可比矩阵

         典型算例:对向量进行旋转

        典型算例:旋转的复合

2.3 滤波器和最优化理论 

        状态估计问题与最小二乘

           KF 卡尔曼滤波(线性系统)

        EKF 扩展卡尔曼 (非线性系统)

         最优化方法和图优化​编辑

        优化和滤波


2.1 几何学

        向量的内积和外积

        旋转矩阵

        旋转向量

        四元数 

        三维旋转也可以由单位四元数表示。注意:单位四元数的逆等于其共轭。即 q_{unit}^{-1}=q_{unit}^{*} 。任意的旋转都可以由两个互为相反数的四元数表示。

        旋转向量和四元数的转换关系如下:

         李群和李代数

        SO(3)上的 BCH 线性近似式 

        J_{l}(\phi) 和 J_{r}(\phi) 的括号里面只能是 \phi ,或者 log[(R)^{\vee}] ,或者 Log(R)。如果是 R 或者没有括号,表示省略。

 2.2 运动学

        李群视角下的运动学

        SO(3) + t 上的运动学

        其中 t 为平移向量。

         线速度和加速度

        注意:能被各种传感器(车速传感器,轮速计)测量到的速度是车体系速度,v_{b}

        线速度的变换式:

        加速度的变换式:

        在实际的处理中,由于测量传感器只能测量离散化的值,在精度不高的应用场景中,我们通常会选择忽略后面三项,只保留最简单的转换关系。

        扰动模型和雅可比矩阵

         典型算例:对向量进行旋转

        设扰动 \Delta R 对应的李代数为 \Delta\phi :

 \Delta R=exp[(\Delta\phi)^{\wedge}]=Exp(\Delta\phi) 

\Delta\phi=log[(\Delta R)^{\vee}]=Log(\Delta R)

       对 Exp(\Delta\phi) 进行泰勒展开并保留一阶项:

Exp(\Delta\phi)=exp[(\Delta\phi)^{\wedge}]=I+(\Delta\phi)^{\wedge}

         右扰动:

\begin{aligned} \frac{\partial Ra}{\partial R} & =\lim_{\Delta\phi\to0}\frac{RExp\left(\Delta\phi\right)a-Ra}{\Delta\phi} \\ & =\lim_{\Delta\phi\to0}\frac{R\left[I+\left(\Delta\phi\right)^{\wedge}\right]a-Ra}{\Delta\phi} \\ & =\lim_{\Delta\phi\to0}\frac{R\left(\Delta\phi\right)^{\wedge}a}{\Delta\phi} \\ & = \lim_{\Delta\phi\to0}\frac{-Ra^{\wedge}\Delta\phi}{\Delta\phi}\\ &=-Ra^{\wedge} \end{aligned}

        左扰动:

\begin{aligned} \frac{\partial Ra}{\partial R} & =\lim_{\Delta\phi\to0}\frac{Exp\left(\Delta\phi\right)Ra-Ra}{\Delta\phi} \\ & =\lim_{\Delta\phi\to0}\frac{\left[I+\left(\Delta\phi\right)^{\wedge}\right]Ra-Ra}{\Delta\phi} \\ & =\lim_{\Delta\phi\to0}\frac{\left(\Delta\phi\right)^{\wedge}Ra}{\Delta\phi} \\ & = \lim_{\Delta\phi\to0}\frac{-(Ra)^{\wedge}\Delta\phi}{\Delta\phi}\\ &=-(Ra)^{\wedge} \end{aligned} 

        典型算例:旋转的复合

        BCH 的一阶线性近似式 (视觉SLAM十四讲,p82):

\log\left(\left(R_1R_2\right)^ {\vee}\right)=\log\left(\left[exp(\phi _{1}^{\wedge})exp(\phi _{2}^{\wedge})\right]^ {\vee}\right) \approx \begin{cases} J_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2}\\ J_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} \end{cases}

      ①  Log(R_1R_2) 对 R_1 求导,对 R_1 进行右扰动:

     \begin{aligned} \frac{\partial Log\left(R_1R_2\right)}{\partial R_1} &= \lim_{\Delta\phi\to0}\frac{Log\left(R_1\mathrm{Exp}\left(\Delta\phi\right)R_2\right) - Log\left(R_1R_2\right)}{\Delta\phi} \\ &= \lim_{\Delta\phi\to0}\frac{Log\left(R_1R_2 \mathrm{Exp}\left(R_2^\top\Delta\phi\right)\right) - Log\left(R_1R_2\right)}{\Delta\phi} \\ &= \lim_{\Delta\phi\to0}\frac{Log\left(R_1R_2\right) + J_{r}^{-1}[Log\left(R_1R_2\right)]R_2^\top \Delta\phi - Log\left(R_1R_2\right)}{\Delta\phi} \\ &= \lim_{\Delta\phi\to0}\frac{ J_{r}^{-1}[Log\left(R_1R_2\right)]R_2^\top \Delta\phi }{\Delta\phi}\\ &= J_{r}^{-1}[Log\left(R_1R_2\right)]R_2^\top \end{aligned}

         其中第 3 行的 Log(R_1 R_2 Exp(R_2^\top \Delta\phi)),根据 BCH 的一阶线性近似式得:

\phi_{1}=Log(R_1R_2)

\phi_{2}={R}_2^\top {\Delta\phi}

Log(R_1 R_2 Exp(R_2^\top \Delta\phi))=Log(R_1R_2)+J_{r}^{-1}[Log(R_1R_2)]{R}_2^\top {\Delta\phi}

        ②  Log(R_1R_2) 对 R_2 求导,对 R_1 进行右扰动:

     \begin{aligned} \frac{\partial Log\left(R_1R_2\right)}{\partial R_1} &= \lim_{\Delta\phi\to0}\frac{Log\left(R_1R_2\mathrm{Exp}\left(\Delta\phi\right)\right) - Log\left(R_1R_2\right)}{\Delta\phi} \\ &= \lim_{\Delta\phi\to0}\frac{Log\left(R_1R_2\right) + J_{r}^{-1}[Log\left(R_1R_2\right)]\Delta\phi - Log\left(R_1R_2\right)}{\Delta\phi} \\ &= \lim_{\Delta\phi\to0}\frac{ J_{r}^{-1}[Log\left(R_1R_2\right)] \Delta\phi }{\Delta\phi}\\ &= J_{r}^{-1}[Log\left(R_1R_2\right)] \end{aligned}

         其中第 2 行的 Log(R_1 R_2 Exp(R_2^\top \Delta\phi)),根据 BCH 的一阶线性近似式得:

\phi_{1}=Log(R_1R_2)

\phi_{2}={\Delta\phi}

Log(R_1 R_2 Exp(\Delta\phi))=Log(R_1R_2)+J_{r}^{-1}[Log(R_1R_2)] {\Delta\phi}

2.3 滤波器和最优化理论 

        状态估计问题与最小二乘

        注意:这里的运动噪声为 R_k ,观测噪声为 Q_k ,后续噪声的符号会变化,但表示的意义不变。

           KF 卡尔曼滤波(线性系统)

        EKF 扩展卡尔曼 (非线性系统)

        矢量函数 f(x) 在 x_k 点处进行线性化 。f(x) 在某一点 x_k 进行线性化的意思是:矢量函数 f(x) 对状态 x 的雅可比矩阵,代入状态 x_k 的具体值。

f(x)=f(x_k+\Delta x)=f(x_k)+J(x_k)\Delta x+\frac{1}{2}\Delta x^\top H(x_k)\Delta x+O(\Delta x^2)         

\mathbf{J}(x_k)=\frac{\partial f(\mathbf{x})}{\partial\mathbf{x}}|_{\mathbf{x}=x_k} 

        F_k 为运动方程在上一时刻状态 x_{k-1} 进行线性化得到的雅可比矩阵,即运动方程对状态 x 的雅可比矩阵,代入上一时刻状态 x_{k-1} 的具体值:

\mathbf{F}_k=\frac{\partial f(\mathbf{x})}{\partial\mathbf{x}}|_{\mathbf{x}=x_{k-1}}

        H_k 为观测方程在当前时刻预测状态 x_{k, pred} 进行线性化得到的雅可比矩阵,即观测方程对状态 x 的雅可比矩阵,代入当前时刻预测状态 x_{k, pred} 的具体值:

\mathbf{H}_k=\frac{\partial h(\mathbf{x})}{\partial\mathbf{x}}|_{\mathbf{x}=x_{k, pred}}

        这一块内容可以参考《机器人学中的状态估计》p89页,内容如下:

         最优化方法和图优化

        优化和滤波

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/892278.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

C语言教程——指针进阶(1)

目录 前言 1、字符指针 2、指针数组 3、数组指针 3.1数组指针 3.2&数组名VS数组名 3.3数组指针的使用 4、数组参数、指针参数 4.1一维数组传参 4.2二维数组传参 4.3一级指针传参 4.4二级指针传参 4.5总结 5、函数指针 5.1思考 总结 前言 我们在之前知道指针…

[应用类App] 轮廓线 aia源码 UI界面精美,画布实现手柄摇杆

屏幕数量:10个,仅主界面近3000代码块,请自行研究参考。 实现了手柄摇杆功能,界面做的比较好。 下载地址:轮廓线 aia源码 UI界面精美,画布实现手柄摇杆 - .aia 案例源码 - 清泛IT社区,为创新赋能…

C++—9、如何在Microsoft Visual Studio中调试C++

本文通过实例操作来介绍 Visual Studio 调试器的功能。调试器在运行过程中可提供许多方法让你查看代码的情况。 你可以逐步浏览代码、查看变量中存储的值、设置对变量的监视以查看值何时改变、检查代码的执行路径、查看代码分支是否正在运行等等。本实例主要是设置断点及查看内…

计算机网络之---数据传输与比特流

数据传输的概念 数据传输是指将数据从一个设备传输到另一个设备的过程。传输过程涉及将高层协议中的数据(如包、帧等)转化为比特流,在物理介质上传输。 比特流的概念 比特流是数据传输中最基本的单位,它是由0和1组成的连续比特…

SpringBoot项目实战(39)--Beetl网页HTML文件中静态图片及CSS、JS文件的引用和展示

使用Beetl开发网页时,在网页中使用的CSS、JS、图片等静态资源需要进行适当的配置才可以展示。大致的过程如下: (1)首先Spring Security框架需要允许js、css、图片资源免授权访问。 (2)网站开发时&#xff0…

GetMaterialApp组件的功能与用法

文章目录 1. 知识回顾2. 使用方法2.1 源码分析2.2 常用属性3. 示例代码4. 内容总结我们在上一章回中介绍了"Get包简介"相关的内容,本章回中将介绍GetMaterialApp组件.闲话休提,让我们一起Talk Flutter吧。 1. 知识回顾 我们在上一章回中已经介绍过GetMaterialApp组…

插入实体自增主键太长,mybatis-plaus自增主键

1、问题 spring-boot整合mybtais执行insert语句时,主键id为长文本数据。 2、分析问题 1)数据库主键是否自增 2)数据库主键的种子值设置的多少 3、解决问题 1)数据库主键设置的时自增 3)种子值是1 所以排查是数据库的问题 4、继…

【嵌入式硬件】嵌入式显示屏接口

数字显示串行接口(Digital Display Serial Interface) SPI 不过多赘述。 I2C-bus interface 不过多赘述 MIPI DSI MIPI (Mobile Industry Processor Interface) Alliance, DSI (Display Serial Interface) 一般用于移动设备,下面是接口…

(STM32笔记)十二、DMA的基础知识与用法 第三部分

我用的是正点的STM32F103来进行学习,板子和教程是野火的指南者。 之后的这个系列笔记开头未标明的话,用的也是这个板子和教程。 DMA的基础知识与用法 三、DMA程序验证1、DMA 存储器到存储器模式实验(1)DMA结构体解释(2…

MySQL 如何赶上 PostgreSQL 的势头?

原文地址 我与 MySQL 社区的前辈交谈时,经常遇到这个问题:「为什么 MySQL 这么棒,而且(至少根据 DB-Engines 的计算)仍然比 PostgreSQL 更流行;但它的地位在下降,PostgreSQL 却势不可挡地越来越…

完全二叉树的删除

(1)删除叶子节点 找到要删除的节点 targetNode找到要删除节点的父节点parent(父节点是否存在)要删除的节点是父节点的左子树还是右子树如果是左子树,则parent.leftnull;如果是右子树则parent.rightnull。 (…

Docker入门之docker基本命令

Docker入门之docker基本命令 官方网站:https://www.docker.com/ 1. 拉取官方镜像并创建容器(以redis为例) 拉取官方镜像 docker pull redis# 如果不需要添加到自定义网络使用这个命令,如需要,直接看第二步 docker r…

【HarmonyOS 5.0】从0到1开发购物应用App(二):登录页对接口

【HarmonyOS Arkts笔记】http网络请求封装 【HarmonyOS Arkts笔记】ohos.data.preferences用户首选项实现存储信息 登录页 点击登录按钮调用login()方法 import { promptAction, router } from kit.ArkUI; import loginApi from "../../api/login" import Prefere…

玩转大语言模型——ollama导入huggingface下载的模型

ollama导入huggingface模型 前言gguf模型查找相关模型下载模型 导入Ollama配置参数文件导入模型查看导入情况 safetensfors模型下载模型下载llama.cpp配置环境并转换 前言 ollama在大语言模型的应用中十分的方便,但是也存在一定的问题,比如不能使用自己…

DFS之迭代加深、双向DFS、IDA*

迭代加深 迭代加深&#xff1a; 170. 加成序列 满足如下条件的序列 X X X&#xff08;序列中元素被标号为 1 、 2 、 3 … m 1、2、3…m 1、2、3…m&#xff09;被称为“加成序列”&#xff1a; X [ 1 ] 1 X[1]1 X[1]1 X [ m ] n X[m]n X[m]n X [ 1 ] < X [ 2 ] < …

Ansible之批量管理服务器

文章目录 背景第一步、安装第二步、配置免密登录2.1 生成密钥2.2 分发公钥2.3 测试无密连接 背景 Ansible是Python强大的服务器批量管理 第一步、安装 首先要拉取epel数据源&#xff0c;执行以下命令 yum -y install epel-release安装完毕如下所示。 使用 yum 命令安装 an…

调和级数不为整数的证明

文章目录 1. 问题引入2. 证明2.1 引理12.2 引理22.3 引理3&#xff1a;2.4 核心证明&#xff1a; 3. 参考 1. 问题引入 s ( n ) 1 1 2 1 3 ⋯ 1 n , n ∈ N ∗ , n ≥ 2 s(n) 1\frac{1}{2}\frac{1}{3}\cdots\frac{1}{n}, \quad \\n \in N^*, n \ge2 s(n)121​31​⋯n1​,…

【Linux网络编程】第二十二弹---深入理解 I/O 多路转接之 epoll:系统调用、工作原理、代码演示及应用场景

✨个人主页&#xff1a; 熬夜学编程的小林 &#x1f497;系列专栏&#xff1a; 【C语言详解】 【数据结构详解】【C详解】【Linux系统编程】【Linux网络编程】 目录 1、I/O 多路转接之 epoll 1.1、epoll 初识 1.2、epoll 的相关系统调用 1.2.1、epoll_create 1.2.2、epol…

pytest 常用插件

pytest 提供了许多功能强大的插件来增强测试体验和执行能力。以下是一些常用的 pytest 插件介绍&#xff0c;并结合 pytest.main() 进行使用的示例。 1. pytest-xdist pytest-xdist 插件用于并行化测试的执行&#xff0c;可以将测试分配到多个 CPU 核心并行运行&#xff0c;从…

openwrt 常见编译问题及编译提速

目录 friendlywrt介绍官方编译流程编译问题git clone errorbusybox和其他包冲突GCC编译错误ERROR: package/feeds/packages/gcc failed to build.手动编译编译提速ccache修改makefile去掉一些不必要的依赖固件定制:增加已经编译好的应用rootfs生成过程REF本文以friendlywrt22-…