高翔【自动驾驶与机器人中的SLAM技术】学习笔记（三）基变换与坐标变换；微分方程；李群和李代数；雅可比矩阵

逍遥郎wj

已于 2024-07-25 18:18:15 修改

阅读量1.5k

点赞数 22

分类专栏：高翔书《自动驾驶与机器人中的SLAM技术》学习笔记文章标签：自动驾驶机器人学习线性代数

于 2024-07-22 23:22:26 首次发布

本文链接：https://blog.csdn.net/xiaoyaolangwj/article/details/140502803

版权

高翔书《自动驾驶与机器人中的SLAM技术》学习笔记专栏收录该内容

15 篇文章

订阅专栏

一、基变换与坐标变换

字小，事不小。

因为第一反应：坐标咋变，坐标轴就咋变呀。事实却与我们想象的相反。这俩互为逆矩阵。

第一次读没有读明白，后面到事上才明白。

起因是多传感器标定：多传感器，就代表了多个坐标系，多个基底。激光雷达和imu标定。这个标定程序，网上，我搜了一下。

浙大的LI-Calib，
港大的LiDAR_IMU_Init

跑测的结果，始终是相反的。跟上面的情况对上了。所以我们必须要弄懂这个互为逆矩阵咋回事。

1、基变换公式：

旧基底： $x_1,x_2,...,x_n$

新基底： $y_1,y_2,...,y_n$

新基底下向量用旧基底线性表示：

$\left\{\begin{matrix} y_1 = c_{11}x_1 + c_{21}x_2 + ...+ c_{n1}x_n & & & \\ y_2 = c_{12}x_1 + c_{22}x_2 + ...+ c_{n2}x_n & & & \\ ...... & & & \\ y_n = c_{1n}x_1 + c_{2n}x_2 + ...+ c_{nn}x_n \end{matrix}\right.$
======>>>>>>>>>简化

$\left ( y_1, y_2, ..., y_n \right ) = \left ( x_1, x_2, ..., x_n \right )C$

Y = XC

$C = \begin{bmatrix} c_{11} & c_{12} & ... & c_{1n}\\ c_{21} & c_{22} & ... & c_{2n}\\ ...& ... & ... & ...\\ c_{n1} & c_{n2} & ... & c_{nn}\\ \end{bmatrix}$

C为旧基底改变为新基底的过度（变换）矩阵。

2、坐标变换公式：

向量 $\underset{P}{\rightarrow}$ 在新旧两组基下的坐标表示：

旧基坐标表示： $\alpha = \left ( \xi_{1}, \xi_{2}, ..., \xi_{n} \right )^{T}$

新基坐标表示： $\beta = \left ( \eta_{1}, \eta_{2}, ...., \eta_{n} \right )^{T}$

$\underset{P}{\rightarrow} =(x_1, x_2, ..., x_n)\alpha = (y_1, y_2,..., y_n)\beta=(x_1, x_2, ..., x_n) C\beta \\ \Rightarrow \alpha = C\beta \\ \Rightarrow\beta = C^{-1}\alpha$

结论：互为逆矩阵，也就解释了，为何标定时，求出来的是个相反的值了。

二、我那有限的微积分知识呀。

1、李群视角下运动学

我向读到此处的朋友请教，请给我一个指引，我需要看一下哪方面的知识。

我用有限的回忆，理解如下：如有不对，请大家给我指引一下。

${f}'(x) = f(x)\cdot w$

一个函数的导数，是它自身乘以一个常数，第一反应：这个函数应该是 $e^x$ 形式。然后有个系数w，这个函数呼之欲出了。

$f(x) = Ce^{wx}$

因为只考虑瞬时变化，所以：在 $t_0$ 时刻附近的形式为：

$f(x) = f(t_0)e^{(w(x - t_0))} = f(t_0)exp(w(x-t_0))$

换成随时间变换的R(t)形式，并带入泊松方程。

$R(t) = R(t_0)exp(w^{\wedge }(t - t_0)) = R(t_0)e^{w^{\wedge }(t - t_0)} = R(t_0)e^{w^{\wedge }\Delta t}$

然后联系指数映射形式：将时间差用 $\Delta t$ 代替，新的李群R(t)是在R(t0)上瞬时扰动产生的，所以有了2.55的公式。

下一步泰勒展开：一阶展开。后面公式通顺了。

关注一下：泊松公式：记忆一下，对李群视角下的运动学公式：

李群（旋转矩阵这个李群）求导之后，是一个自身，乘以一个公式2.52的反对称矩阵，反对称矩阵都可以写成一个向量形式。

所以结论：旋转矩阵这个李群求导之后，就是原矩阵乘以一个向量（这个向量是反对称矩阵转的）

PS：公式中微分结果为 $\dot{R} = R \omega^{\wedge }$ 中，物理意义上称 $\omega$ 为瞬时角速度。

后面读读第五部分：雅可比矩阵详解。感受更深。

二刷感受：第2.2节运动学。不同形式下，如何描述旋转更新量。

微分方程描述的就是这个旋转更新量。有这个更新量，就可以近似求解逼近时刻微小扰动之后的旋转姿态。2.54，2.55，2.56都是在描述这个逼近时刻的姿态（旋转部分）。只是不同的近似形式。

方法论：这一节结合2.3节，我反复读了好几遍。每次读完，都间隔几天，去读读其他人的理解。

我隐约记得：读过一本书中写过的学习方法：

渡边康弘《高效阅读》或者《如何高效学习：1年完成MIT4年33门课程的整体性学习法》中介绍的方法：如果你想快速成为一个领域内的专家，就把这方面的书全部拿来，全部通读一遍。因为书中大量重复内容，从不同视角反复解释。你总能明白。

不要觉得很慢，因为重复内容，所以很快就可以读完。

吃饼理论：

不要反复吃同一张饼，不要一张饼反复吃，要看看别人怎么吃，别人是蘸酱还是卷菜。
不要盯住游戏使劲玩，要看看别人怎么玩，学学别人的攻略。
听听别人对这个问题的不同看法和视角。

2、四元数视角下运动学

下一页的四元数求导：

四元数求导之后：就是一个原四元数乘以一个纯虚四元数。微分方程形式还是类似指数形式。

纯虚四元数有自己的运算特征。所以可以进一步简化。详情见书中内容。

只说结论：一个纯虚四元数的指数映射结果为单位四元数。

我们都是提李代数的指数映射为李群，这边是纯四元数的指数映射，所以李代数和四元数一定有一个对应关系。

3、四元数与李代数关系

结论：

四元数表达的角速度正好是SO(3)李代数的一半。
四元数在旋转一个矢量时要乘两遍。

这个地方的结论与我们之前在25页中乘两次对应上了。

线索再次串联起来。所以高博的书很有深度。很有结构。

乘一次，旋转一半，乘另一次，旋转另一半。两次乘，完成转个旋转。

这个四元数一半的问题，我们暂且记下。后面再来一遍。

三、李群和李代数

李代数加小的旋转，李群乘小的扰动，BCH线性近似。这里注意BCH中引入了一个雅可比矩阵。

四、雅可比矩阵

在讲BCH近似时：对于指数乘法的运算发现，两个同底指数相乘，并非我们理解的如下公式：

$e^{x+y} = e^x \cdot e^y \\ e^x \cdot e^y =e^{x+y}$

而是多了个雅可比矩阵。这是因为对于加法的定义不同。

当我们对旋转的函数求导时，有两种定义：

一种定义在向量层面，复杂
一种定义在扰动层面，简洁

求导时，可以基于四元数q或者旋转矩阵R都可以求导。

结论：对于四元数和旋转矩阵求导，雅可比矩阵都可以使用同一个。

基于四元数q时，需要记住，需要右乘 $\frac{1}{2} \left [ 1, \omega \right ]^{T}$ 。

代码演示如何更新姿态（旋转角度）：圆周运动，单位时间内角速度是个定值。所以旋转的角度也是个定值。用角速度omega乘以时间差dt即可得到角度。比如代码中的else中的代码。更新四元数却需要右乘 $\frac{1}{2} \left [ 1, \omega \right ]^{T}$ 。（四元数描述的变换角度是实际变换角度的一半。即一个四元数实际描述一半的角度。因此由角度构造的四元数是所求四元数的2倍。所以必须要乘以1/2）。用这个一半的角度的四元数，再构造实际旋转矩阵SO(3)时，才可以得到正确的值。

omega是角速度矢量，omega乘以dt就是一个旋转矢量 $\phi$

这个旋转矢量,通过else中的指数映射变成角度变换的旋转矩阵的李群SO3形式。

Vec3d omega(0, 0, angular_velocity_rad);  // 角速度矢量


while (ui.ShouldQuit() == false)
{
    // 更新自身位置
    Vec3d v_world = pose.so3() * v_body;
    pose.translation() += v_world * dt;

    // 更新自身旋转
    if (FLAGS_use_quaternion) {
        Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
        q.normalize();
        pose.so3() = SO3(q);
    } else {
        pose.so3() = pose.so3() * SO3::exp(omega * dt);
    }

Quatd q = pose.unit_quaternion() * 
Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);

首先获取物体当前的旋转四元数，pose.unit_quaternion()，
然后根据角速度更新四元数，通过四元数相乘的方式。角速度乘以一个更新时间：更新量为角速度乘以时间除以2。
normalize()归一化：由于浮点数的计算误差，四元数可能会逐渐偏离单位长度，归一化可以确保四元数保持单位长度，这是四元数表示旋转的一个重要性质。
`pose.so3() = SO3(q);` 这行代码将更新后的四元数转换为 `SO3` 类型，并设置为物体的新旋转。`SO3` 是一个类，它可能是用于表示三维空间中的旋转的类。
在代码中要知道四元数Quatd q描述的就是四元数形式下的旋转矩阵。这个四元数形式要变成旋转矩阵通过SO3(q)来实现变换。

四元数更新时，乘以上式中的 $\frac{1}{2} \left [ 1, \omega \right ]^{T}$ 。然后再乘以角速度omega乘以dt（时间刻度）。
旋转矩阵直接乘反而很简单。
这个w在代码中就是角速度矢量omega。

五、雅可比矩阵详解

参考的链接雅可比矩阵几何意义的直观解释及应用-CSDN博客我们知道我们要研究的关键专业词汇叫：多元向量值函数。

咱们先写自己懂得。从专业词汇切入。

【高等数学笔记】多元向量值函数的导数与微分（简单回忆一下性质即可）

1、多元向量值函数

概念定义：输入是一个n维向量，函数输出是一个m维向量。可以看成m个n元向量函数。

雅可比行列式是坐标变换理论的基础之一，在数学分析隐函数(无法知道或者无法描述的)理论中发挥着重要作用。

设有n元向量值函数

$\boldsymbol{f} : U(x_{0}) \subseteq \mathbb{R}^{n} \rightarrow \mathbb{R}^{m}$

该函数可以表示成： $\boldsymbol{x}$ 是n维的， $\boldsymbol{y}$ 是m维的。

$\boldsymbol{f}(\boldsymbol{x}) = \begin{bmatrix} f_{3}(\boldsymbol{x})\\ f_{3}(\boldsymbol{x})\\ ... \\ f_{m}(\boldsymbol{x})\\ \end{bmatrix} = \begin{bmatrix} f_{1}(x_{1}, x_{2}, x_{3}, ..., x_{n})\\ f_{2}(x_{1}, x_{2}, x_{3}, ..., x_{n})\\ ...\\ f_{m}(x_{1}, x_{2}, x_{3}, ..., x_{n})\\ \end{bmatrix} = \begin{bmatrix} y_{1}\\ y_{2}\\ ...\\ y_{m} \end{bmatrix}$

它将n维空间中的点 $\boldsymbol{x}$ 映射为m维空间中的点 $\boldsymbol{f}(\boldsymbol{x})$ 。

我找到一张神经网络n维输入、m维输出的网络图。便于横向对比理解。

从n维输入如何得到m维输出的。新来一个数据如何得到准确的预测。（神经网络干的活。或者说我们要求解的隐函数模型要干的活）
就是预测的活。人类对于预测未来（未知）这件事耿耿于怀。
不理解不影响大局。

若 $\boldsymbol{f}$ 的每个分量 $f_{1},f_{2}, ..., f_{n}$ 都在点 $x_{0}$ 处可微，则我们定义 $\boldsymbol{f}$ 在 $x_{0}$ 处的一阶导数（雅可比矩阵）为：

简洁表现的雅可比矩阵：

2、雅可比矩阵的用途

仿射变换：

所谓仿射变换（或者叫仿射映射），就是两个变换的复合：线性及平移。具体来说，仿射变换就是指将一个向量空间进行一次线性变换再加上一次平移，变换到另一个向量空间。

局部仿射逼近：

描述局部逼近。

雅可比矩阵用来表示向量值函数的局部仿射逼近。由多元函数的泰勒展开可得，

$f(x_{0}+\Delta x) = f(x_{0}) + J_{f}\Delta x + O(\left \| \Delta x \right \|^{2})$

我们把展开式中的高阶项省略掉，得到向量值函数的仿射逼近，

$f(x_{0}+\Delta x) \approx f(x_{0}) + J_{f}\Delta x$

右侧的两项正是一个仿射映射。

一般的函数 $\boldsymbol{f}$ 在 $x_{0}$ 处的附近区域不一定是线性的，不能用一个矩阵去精确表示它。但是，当这个局部非常小的时候，我们可以用一个仿射映射去近似逼近它。

就是站在点 $x_{0}$ 处，各个方向走一小步 $\Delta x$ ，函数值 $f(x_{0}+\Delta x)$ 与函数值 $f(x_{0})$ 的差（更新量）可以用步子 $\Delta x$ 的一个线性变换来近似表示，即有

$\Delta f = A\Delta x$

也就是说，用一个矩阵就可以去逼近表示函数的微小局部了。当然，每一点处的矩阵可能是不同的。那么这个矩阵具体是怎么样的呢？对，你已经看到过了，就是本文的主角，雅可比矩阵。

我们把雅可比矩阵，

代入下式（此处就直接用等号吧），

$\Delta f = f(x_{0}+\Delta x) - f(x_{0}) = J_{f}\Delta x$

得：

这样相当于：把更新量求解出来了。在SLAM中每次扰动的值，做了近似估计。

多元向量值函数 $f$ 在每个可微分的点处的导数或微分可以用一个矩阵表示，即所谓的雅可比矩阵。

导数怎么是一个矩阵呢？因为这是多元向量值函数的导数，跟一元数量函数的导数相比，在形式上自然应该有所升级。从形式上看有如下的关系，

详细地说，如果 $f(x)$ 在x处是可微的， $h$ 是一个位移向量，表示定义域内的一小步，则(矩阵)向量乘积 $J_{f}h$ 也是一个位移向量，用于近似表示 $h$ 这一小步引起的函数在值域中的位移向量 $f(x+h) - f(x)$

即：

$f(x+h) - f(x) = J_{f}(x)h$

这个映射将x附近的点y作如下映射：

$f(y) = f(x) +J_{f}(x)(y - x)$

它是对点x所在的微小区域的函数f(y)的最佳仿射近似。

该仿射映射中的线性部分的系数矩阵 $J_{f}(x)$ 称为 $f$ 在 $x$ 处的导数或微分。

再回顾书中内容：

因为不同李群下，对于加的定义不同，所以指数映射也有些不同，但是都在描述逼近微小扰动之后的旋转近似值。

2.56这种泰勒展开作为一种视角看到这个旋转更新问题。2.55是李群形势下描述

2.61跟2.53一样也在描述更新量问题。2.62跟2.54一样，表述了四元数形式下逼近微小扰动之后的旋转近似值。

总结：雅可比矩阵加微分方程：一阶泰勒展开近似表示。

方法论：如果一篇介绍这个概念的文章读不懂，那就多搜几篇，总有一篇符合你的胃口，直到弄懂为止。

雅可比矩阵配合微分方程：完成了对逼近局部值的一阶泰勒展开的近似。

为何要局部近似？因为这种多元向量值函数是一种隐函数，我们没法描述他的具体函数。只好做线性近似。是一种无奈之举。

但是只要足够逼近，那么就足够精确。
可微的意义：如果陡变，这种近似没有意义，不成立。所以这就是可微可导的实际物理意义。或者说几何意义。

六、拓展

主要搬运：雅可比矩阵几何意义的直观解释及应用-CSDN博客

原博主有个微信公众号：雅可比矩阵几何意义的直观解释及应用

Math1n51de 机器学习与数学

拓展一：雅可比行列式

多元向量值函数、导数（雅可比矩阵）、行列式、多元微积分等这些概念怎么就凑一起了呢？

当 $m=n$ 时，雅可比矩阵为方阵，因此其行列式是有定义的，称为 $f$ 的 雅可比行列式（Jacobian 行列式）。虽然它将 $n^{2}$ 个数浓缩成了一个数，但它还算是精华的，包含了有关 $f$ 的局部表现的重要信息。

从 $uv-$ 平面到 $xy-$ 平面，区域形状会变，面积也会变，每点处的局部面积缩放倍率就是该点处相应行列式的值。

矩阵行列式的几何意义，是指坐标变换前后面积的缩放倍数。

简单的看，上图中左边橙色小区域在坐标变换下变成右边的淡蓝色小区域，它的面积怎么变呢？因为站在每一点的局部，近似地看成一个仿射变换，而仿射变换对应一个矩阵，仿射变换对区域面积的具体缩放情况可以通过这个矩阵来估计。

对雅可比矩阵 $J$ 作SVD分解：

$J= U\Sigma V^{T}$

其中，矩阵 $U$ 和 $V$ 都是正交矩阵，而是对角矩阵 $\Sigma$ 。正交矩阵不改变向量的长度（向量的L2范数），因此雅可比矩阵 $J$ 对区域面积的改变主要体现在中间的对角矩阵 $\Sigma$ ，对角阵的对角线元素对应在各个坐标轴上的缩放倍数，它们的乘积就是整体面积的缩放倍数。而由柯西 - 比内公式可知：

再看坐标变换前后，对微小区域的面积可以作如下估计，

其中， $\mathbf{M}(\cdot)$ 是求区域的面积， $\Omega_i$ 是定义域中的小区域， $\mathbf{g}(\Omega_i)$ 是变换后得到的小区域。这里用橙色小区域的仿射变换来逼近表示淡蓝色小区域，
在这里插入图片描述
需要注意的是，这里实际的向量值函数是 $\mathbf{g}$ ，记得用它代替上文中局部仿射变换逼近公式中的 $\mathbf{f}$ 。