现代机器人学（6）-转动的指数坐标

八度浅离

分类：机器人学

发布时间 2022.03.13阅读数 3476 评论数 0

【中英字幕】现代机器人学 | Modern Robotics_哔哩哔哩 (゜-゜)つロ干杯~-bilibiliwww.bilibili.com/video/BV1KV411Z7sC?p=14

数学知识：

线性常微分方程 $\dot{x}(t)=ax(t)$

在 $x(0)=x_0$ 已知下的解为 $x(t)=e^{at}x_0$

$e^{at}$ 的泰勒展开式为

$e^{at}=1+at+\frac{(at)^2}{2!}+\frac{(at)^3}{3!}+...$

将常数a换为矩阵 $A\in R^{n\times n}$ 的向量线性常微分方程：

$\dot{x}(t)=Ax(t)$

解为： $x(t)=e^{At}x_0$

$e^{At}$ 称为矩阵指数，其泰勒展开式为：

$e^{At}=1+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+...$

若A为反对称矩阵，则有 $A^3=-A、A^4=-A^2$ ...,由此推

$e^{At}=I+At+\frac{(At)^2}{2!}+\frac{(At)^3}{3!}+...\\ = I+(t-\frac{(t)^3}{3!}+\frac{(t)^5}{5!}-...)A+(\frac{(t)^2}{2!}-\frac{(t)^4}{4!}+\frac{(t)^6}{6!}-...)\\ =I+sin(t)A+(1-cos(t))A^2$

单位转轴为 $\hat{w}$ ,绕其旋转角度 $\theta$ ，我们将两者的相乘就得到三维向量 $\hat{w}\theta$ 。我们将 $\hat{w}\theta\in R^3$ 称为该转动的三参数指数坐标。

在上一个笔记中，用 $\dot{R}=[w_s]R$ 将 $\dot{R}$ 与 $R$ 建立了联系。

现在，我们选取任意一个三维向量p(0)，将其绕转轴 $\hat{w}$ 旋转角度 $\theta$ 到 $p(\theta)$ ，则 $\dot{p}=\hat{w}\times p=[w]p$ 。

可以解得 $p(\theta)=e^{[\hat{w}]\theta}p(0)$

由于 $[\hat{w}]$ 为反对称矩阵，故

$Rot(\hat{w},\theta)=e^{[\hat{w}]\theta}=I+sin\theta[w]+(1-cos\theta)[w]^2\in SO(3)$

我们将上式称为罗德里格斯公式（Rodrigues's formula）。

现在我们通过转动的三参数指数坐标 $\hat{w}\theta$ 得到了其旋转矩阵R；若对R进行矩阵质数的逆运算（即对数）求得 $[\hat{w}]\theta$ 。 $[\hat{w}]\theta$ 被称为R的矩阵对数。

已知 $R \in SO(3)$ ,求 $[\hat{w}]\theta \in so(3)$ 。

算法：给定 $R \in SO(3)$ ，总是能找到单位转轴 $\hat{w} \in R^3$ ,且 $||\hat{w}||=1$ ， $\theta \in(0,\pi)$ ,使得 $R=e^{[\hat{w}]\theta}$ 。其中向量 $\hat{w}\theta \in R^3$ 是 $R$ 的指数坐标，而反对称矩阵 $[\hat{w}]\theta \in so(3)$ 是R的矩阵对数。