SLAM进阶课程1.2（SO(3)上的微分方程；解析解与数值解；应用：陀螺仪的积分）

nirvana-dragon

已于 2024-03-03 17:09:31 修改

阅读量1k

点赞数 10

分类专栏： SLAM理论进阶文章标签：机器学习人工智能机器人

于 2024-03-03 17:08:33 首次发布

本文链接：https://blog.csdn.net/weixin_46919783/article/details/136431128

版权

SLAM理论进阶专栏收录该内容

3 篇文章 0 订阅

订阅专栏

本文按照UP主的思路主要记录一下SO(3)空间上的微分方程求解，在我的纯视觉和纯雷达SLAM认知中，很少会对SLAM问题求其微分方程组的解，一般都是推导或者记住扰动模型，即使加入了IMU这种需要预积分或者直接积分求解位姿的传感器，大多也都是利用扰动模型（左右雅可比）和伴随性质即可。

在我本科大学的高等数学中，学到的都是一维微分方程特殊形式的求解，对于高维例如向量的微分方程求解还是在自动控制原理中见到的，这里直接进行SO(3)流形上的微分方程，属于是碾压我的认知。

SO(3)微分形式与反对称矩阵的生成与转换

根据SO(3)的定义推到其微分形式这个思路是怎么来的，我也不知道，SLAM小白只能默认按照大佬的思路走:

$R^TR=RR^T=I(3*3)\\ \dot{R^T}R+R^T\dot{R}=\dot{R}R^T+R\dot{R^T}=0(3*3)\\ R^T\dot{R}=-(R^T\dot{R})^T||\dot{R}R^T=-(\dot{R}R^T)^T\\$

可以看出利用旋转矩阵自身的定义，可以推导出一些特殊的形式即R的微分和其自身的转置构成了一个反对称矩阵，为了方便描述这个反对称矩阵，我们用 $[w]_{\times}$ 进行记录，由于 $R^T$ 与 $\dot{R}$ 左乘或者右乘得到的反对称矩阵只有在R=I或者一些特殊时刻才相等，其余时刻均不相等，因此利用 $w_r$ 和 $w_l$ 进行区分，具体哪种形式对应哪一个旋转向量w，本人并不清楚严格意义上的表示，只是记住当RT移动到等式右边时，如果 $[w]_{\times}$ 在其右侧则为 $w_r$ ，在左侧即为 $w_l$ ，因此结果如下所示：

$R^T\dot{R}=[w_r]_{\times}||\dot{R}R^T=[w_l]_{\times}\\ \dot{R}=R[w_r]_{\times}||\dot{R}=[w_l]_{\times}R$

这两个反对称矩阵之间可以通过旋转矩阵的伴随性质进行转换

$theorem:R[a]_{\times}R^T=[Ra]_{\times}$

之前证明过，后来发现拿来用就行了不需要记住其推导过程，总感觉该推导过程背后有什么好东西被忽视了（当然推导思路也是看的别的博客大佬的思路）。

所以可以得出

$[w_l]_{\times}R=R[w_r]_{\times}\\$

$[w_l]_{\times}=R[w_r]_{\times}R^T||R^T[w_l]_{\times}R=[w_r]_{\times}\\$

$[w_l]_{\times}=[Rw_r]_{\times}||[R^Tw_l]_{\times}=[w_r]_{\times}\\ w_l=Rw_r||R^Tw_l=w_r$

一维时变非线性微分方程求解

所以这两个旋转向量可以通过旋转矩阵相互转换，因此我们可以发现只要我们知道了这个反对称矩阵的旋转向量，我么便得到了这个旋转矩阵的微分方程。如果这是连续时刻的微分方程我们可以写为

$\dot{R}(t)=R(t)[w_r(t)]_{\times}$

这个微分方程的求解有点不太好弄啊，我们先看看一维情况

$\dot{x(t)}=x(t)*u(t)\\ \frac{dx(t)}{dt}=x(t)*u(t)\\ \int_{0}^{t}\frac{dx(t)}{x(t)}=\int_{0}^{t}u(t)dt\\ ln^{x(t)}|_0^t=\int_{0}^{t}u(t)dt\\ ln^{x(t)}-ln^{x(0)}=\int_{0}^{t}u(t)dt\\ ln^{x(t)}=ln^{x(0)}+\int_{0}^{t}u(t)dt\\ x(t)=exp(ln^{x(0)}+\int_{0}^{t}u(t)dt)\\ x(t)=exp(ln^{x(0)})exp(\int_{0}^{t}u(t)dt)$

方阵的指数映射及其性质

这里一维最终的求解形式包含了指数函数，因此UP主开始聊起来SO(3)的指数映射，这里大家多看看exp(a+b)=exp(a)exp(b)，多好多和谐的东西被一群矩阵搞得复杂滴很（猛男落泪）

这里关于矩阵指数函数的相关知识我就跳过了，想了解的更多可以看看1.1提到了那个矩阵分析课程，但是我们需要记住几点结论以后用的上

$AB=BA\Rightarrow exp(A+B)=exp(A)exp(B)\\$
$det(exp(A))>0\\$
$exp(A)^{-1}=exp(-A)$
$exp(P^{-1}AP)=P^{-1}exp(A)P$
$A^T=-A\Leftrightarrow exp(A)\epsilon SO(n)$
$\dot{exp(At)}=A\dot{exp(At)}=\dot{exp(At)}A$
$\dot{x(t)}=Ax(t)\; when\ t=0,x(t_0)=A_0\\ x(t)=exp(A(t-t_0))A_0$

对于定理4，P可逆且是方阵，对于第一条有个疑问， $AB=BA\Rightarrow exp(A+B)=exp(A)exp(B)\\ \because exp(A+B)=exp(B+A) \\\therefore AB=BA\Rightarrow exp(A+B)=exp(A)exp(B)=exp(B)exp(A)\\ exp(A)=exp(-B)exp(A)exp(B)$

一个矩阵左乘一个矩阵的逆矩阵右乘逆矩阵的逆矩阵等于其自身，哦，有趣的线性变化

旋转矩阵的微分方程求解（解析解）

再看下面这个式子

$\dot{R}(t)=R(t)[w_r(t)]_{\times}$

当w为常向量(即向量的每一个值保持不变)时，积分结果为 $R(t)=R(t_0)exp([w_r]_{\times}(t-t_0))$

别问我怎么积出来的，我这里写一个不合理的版本

$\frac{dR(t)}{dt}=R(t)[w_r]_{\times}\\ \int\frac{dR(t)}{R(t)}=\int[w_r]_{\times}dt\\ ln^{R(t)}-ln^{R(t_0)}=[w_r]_{\times}(t-t_0)\\ ln^{R(t)}=ln^{R(t_0)}+[w_r]_{\times}(t-t_0)\\ R(t)=exp(ln^{R(t_0)}+[w_r]_{\times}(t-t_0))\\ R(t)=R(t_0)exp([w_r]_{\times}(t-t_0))$

这个版本真是错误百出，矩阵的分式， $R(t_0)$ 与 $[w]_{\times}$ 是否满足AB=BA

假如 $[w]_{\times}=R(t_0)^T\dot{R(t_0)}$ ，那么 $R(t_0)[w_r]_{\times}=R(t_0)R(t_0)^T\dot{R(t_0)}=\dot{R(t_0)}=[w_l]_{\times}R(t_0)$

很显然不成立啊，太菜了不会证，还好SLAM14讲说 $R(t_0)=I$ ，这样这个就成立了

旋转矩阵的微分方程数值解与陀螺仪预积分思想

工程上很多东西都是数值解，因为大多数传感器都是在连续时间内采样得到离散的数据，因此我们可以假设在 $\Delta t$ 时间内，w是保持恒定的，只要采样频率足够大，物理世界中这是成立的。因此可以得到：

$R(t_k+\Delta t)=R(t_k)exp([w(t_k)_r]_{\times}\Delta t)$

UP主提到陀螺仪的积分就是上式不断进行的过程，由于上式 $R(t_k+\Delta t)$ 的值不仅和当前时刻的测量值 $w(t_k)$ 有关，还有当前时刻的姿态 $R(t_k)$ 有关，而当前时刻的姿态可能由于其他观测量(如里程计)进行改变，因此就要根据新得到的姿态 $\hat{R}(t_k)$ 重新递推 $\hat{R}(t_k+\Delta t)$ ，因此提出了预积分的思想，即只关心两个时刻之间的位姿变化 $\Delta R_{ij}$ 与这段时间内陀螺仪测量值 $(w_i,w_{i+1},...,w_j)$ 之间的关系，通过作用到第一个时刻在世界坐标系下的姿态 $R_i$ 即可得到第二个时刻在世界坐标系下的姿态 $R_j$ ，R均为 $R^b_w$ 。

nirvana-dragon

关注

10
点赞
踩
13

收藏

觉得还不错? 一键收藏
1
评论
SLAM进阶课程1.2（SO(3)上的微分方程；解析解与数值解；应用：陀螺仪的积分）

本文按照UP主的思路主要记录一下SO(3)空间上的微分方程求解，在我的纯视觉和纯雷达SLAM认知中，很少会对SLAM问题求其微分方程组的解，一般都是推导或者记住扰动模型，即使加入了IMU这种需要预积分或者直接积分求解位姿的传感器，大多也都是利用扰动模型（左右雅可比）和伴随性质即可。在我本科大学的高等数学中，学到的都是一维微分方程特殊形式的求解，对于高维例如向量的微分方程求解还是在自动控制原理中见到的，这里直接进行SO(3)流形上的微分方程，属于是碾压我的认知。
复制链接

扫一扫