机器人的现代数学知识(Lie Groups)¶

参考：
paper: A micro Lie theory for state estimation in robotics
lib: manif
lib: sophus
book:《Modern Robotics Mechanics, Planning, and Control》Kevin M. Lynch, Frank C. Park (讲解视频) book:《机器人学的现代数学理论基础》丁希仑
videos: 南科大

1 基础知识¶

自由向量（free vector）

无坐标的（coordinate free）

角速度（angular velocity） $$ w= \hat{w} \dot{\theta} $$

$\hat{w}$ 表示单位转轴（unit vector），$\dot{\theta}$表示角速度（scalar）

反对称矩阵（skew-symmetric） $$ w=[w_1,w_2,w_3]^T $$

\[ [w]= \begin{bmatrix} 0 & -w_3 & w_2 \\ w_3 & 0 & -w_1 \\ -w_2 & w_1 & 0 \end{bmatrix} \]

\[ [w]=-[w]^T \]

$[w]$ is calleda skew-symmetric matrix representation of the vector $w$

旋转矩阵 $R \in SO(3) 、R \in \mathbb{R}^{3 \times 3}$ $$ R^TR^{-1} =I $$

\[ R^{-1} = R^T\]

齐次变换矩阵 $T=(R,p) \in SE(3)、T \in \mathbb{R}^{4 \times 4}$

欧式空间、非欧空间

欧式空间：距离、面积、体积等
非欧空间：黎曼几何、曲率等

2 群¶

看这个视频总结的

群是 一种集合 + 一种运算 的代数结构：记集合为$A$，运算为$\cdot$，当运算满足封闭性、结合率、幺元、逆四条性质，称$(A,\cdot)$为群

旋转矩阵集合 + 矩阵乘法构成群 $\to$ 三维特殊正交群/旋转矩阵群$SO(3)$
变换矩阵集合 + 矩阵乘法构成群 $\to$ 三维特殊欧式群/变换矩阵群$SE(3)$
常见的其他群
一般线形群$GL(n)$: $n\times n$的可逆（非奇异）实矩阵（二元运算：矩阵乘法），也可以记作: $GL(n,\mathbb{R})$
特殊正交群$SO(n)$: $n\times n$的单位正交实数矩阵（二元运算：矩阵乘法），$SO(2)$、$SO(3)$是两个比较重要的子群。
特殊欧式群$SE(n)$: 三维特殊正交群$SO(3)$和向量空间$\mathbb{R}^3$的半直积为特殊欧式群$SE(3)$: $SE(3)=SE(3)\otimes \mathbb{R}^3$，$SE(3)$也是特殊欧式群的一个子群。

3 李群¶

群的4个基本性质：封闭性、结合律、幺元律、可逆性

交换群(commutative group)、阿贝尔群(Abelian group)

李群：群的4个基本性质+3个特殊条件（可微流形、群的积是可微分的映射、群的逆也是可微分的映射）一般意义的李群指的的乘法，不是加法。即矩阵李群。

典型例子： - n维向量空间$\mathbb{R}^n$ - 单模复数群 - 一般线性群 - 正交群、特殊正交群 - 三维旋转群、三维特殊欧式群$SE(3)$ - 幺模群 - 特殊幺模群

4 李代数¶

李代数对应李群的正切空间，它描述了李群局部的导数。

李群: 大写，例如$SE(3)$
李代数: 小写，例如$se(3)$

$SE(3)$的李代数是$se(3)$
$SO(3)$的李代数是$so(3)$

李群表示刚体运动、李代数表示刚体运动速度，李群和李代数是指数映射关系。

5 总结¶

5.1 SE(3)、SO(3)、se(3)、so(3)¶

1、$3 \times 3$ 旋转矩阵组成的集合称为特殊正交群（special orthogonal group）$SO(3)$

特殊正交群$SO(3)$称为旋转矩阵群
特殊正交群$SO(2)$是所有$2 \times 2$实数矩阵的集合，二维的

2、特殊欧氏群（special Euclidean group）$SE(3) $，称为刚体运动群、齐次变换矩阵（homogeneous transformation matrice）群

刚体的位形空间可以表示为$SO(3)$与$\mathbb{R}^3$的乘积空间（半直积）记作$SE(3)$

\[ SE(3) = SO(3) \otimes \mathbb{R}^3\]

3、大写是李群、小写是李代数；他们之间是指数映射的关系；如下面两个exp的关系；

针对刚体运动，无论齐次坐标表达，还是伴随表达，都是具有指数映射关系$A=e^{\theta S}$;$Ad(g)=e^{\theta ad(j)}$

4、所有$3 \times 3$反对称矩阵的集合称为$so(3)$，$so(3)$是三维旋转群的李代数 For any unit vector $[\hat w] \in so(3)$ and any $\theta \in \mathbb{R}$ $$ e^{[\hat w]\theta} \in SO(3)$$

For any $R \in SO(3)$,there exists $[\hat w] \in \mathbb{R^3}$ with $||\hat w||=1$ and $\theta \in \mathbb R$ such that $$ R=e^{[\hat w]\theta} $$

\[exp:[\hat w]\theta \in so(3) \to R \in SO(3)\]

\[log:R \in SO(3) \to [\hat w]\theta \in so(3)\]

The vector $\hat w\theta \in \mathbb{R^3}$ is called the exponential coordinate for $R$

Python
import packages.Python.modern_robotics.core as mr
import numpy as np
from scipy.linalg import expm, logm

R = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])

omega_operation_skew = logm(R)
print('omega_operation_skew = \n', omega_operation_skew)
R_test = expm(omega_operation_skew)
print('omega_operation_skew = \n', R_test)

omega_operation = mr.so3ToVec(omega_operation_skew)
print('omega_operation = \n', omega_operation)
omghat, thate = mr.AxisAng3(omega_operation)
print('omghat = \n', omghat, '\n', 'thate = \n', thate)

结果如下所示：

Rodrigues'Formula: Given any unit $[\hat w] \in so(3)$,we have $$ e^{[\hat{w}\theta]} = I + [\hat{w}]sin(\theta)+ [\hat{w}]^2(1-cos(\theta))$$

证明：将指数按照幂级数展开，再结合$sin(\theta)$和$cos(\theta)$的展开，化简。

刚体运动的指数坐标

\[exp:[\mathcal{S}]\theta \in se(3) \to T \in SE(3)\]

\[log:T \in SE(3) \to [\mathcal{S}]\theta \in se(3)\]

矩阵指数$e^{\mathcal{S} \theta}$

\[ e^{[\mathcal{S}]\theta} = \begin{bmatrix} e^{[w]\theta} & G(\theta)v\\ 0 & 1 \end{bmatrix} \]

\[G(\theta)=I\theta+(1-cos\theta)[w]+(\theta -sin\theta)[w]^2\]

Python
import packages.Python.modern_robotics.core as mr
import numpy as np
from scipy.linalg import expm, logm
np.set_printoptions(suppress=True)

T = np.array([[0, -1, 0, 0], [1, 0, 0, -1], [0, 0, 1, 0], [0, 0, 0, 1]])

s0_T_matrix = mr.MatrixLog6(T)
print('s0_T_matrix = \n', s0_T_matrix)
s0_T_matrix_log = logm(T)
print('s0_T_matrix_log = \n', s0_T_matrix_log)
s0_T_screw = mr.se3ToVec(s0_T_matrix)  # 得到Twist
print('s0_T_screw = \n', s0_T_screw)
T_test = expm(s0_T_matrix)
print('T_test = \n', T_test)

结果如下：

5.2 不同表达、不同运算¶

不同表达对应不同的运算，各有各的好处。

伴随表达也具有指数映射的关系$Ad(j)=e^{\theta ad(j)}$

奇次坐标表达的李群和其对应的李代数

1、齐次坐标表示 $$ T = \begin{bmatrix} R & p \ 0 & 1 \end{bmatrix} $$

\[ T^{-1} = \begin{bmatrix} R^T & -R^Tp \\ 0 & 1 \end{bmatrix} \]

伴随表达的李群和对应的李代数

2、伴随表示（adjoint rspresentation）$[Ad_T]$ $$ [Ad_T]= \begin{bmatrix} R & 0 \ [p]R & R \end{bmatrix}$$

6、旋量理论与机器人运动学¶

旋量是一条有节距的直线；当节距=0，旋量$\to$线矢量

6.1、齐次坐标与线矢量¶

点的齐次坐标

面的齐次坐标

线的齐次坐标

线矢量（line vector）：依附在空间某一直线上的向量（p29，戴建生）

\[ S=(s;s_0)=(s;r\times s)=(L,M,N;P,Q,R) \]

$s$称为原部矢量，$s_0$称为偶部矢量

线矢量，点乘没有意义；不过可以计算代数和、互易积（reciprocal product）

6.2、旋量理论¶

Twist / Spatial vector（运动旋量）、screw motion（螺旋运动）

\[ \mathcal{V}_b = \begin{bmatrix} w_b \\ v_b \end{bmatrix} \]

\[ \mathcal{V}' = [Ad_T]\mathcal{V} \]

运动旋量写成矩阵的形式 $$ [\mathcal{V}_b] = \begin{bmatrix} [w_b] & v_b \ 0 & 0 \end{bmatrix} \in se(3)$$

运动旋量 $\mathcal{V}$ 可以写成螺旋轴（screw axis）$\mathcal{S}$与绕该轴转动的速度的组合形式

1、Consider “unit velocity” $\mathcal{V} = \mathcal{S}$
2、if not unit speed $\mathcal{V} = \mathcal{S}\dot \theta$
3、$\mathcal{V} = ScrewToTwist(\hat w,h,q,\dot \theta)=ScrewToTwist(\hat w,h,q,\dot \theta=1)\dot \theta$

screw motion $$ \mathcal{V} = [w;v]=[\hat{s}\dot{\theta};-\hat{s}\dot{\theta}\times q+h\hat{s}\dot{\theta}]=[-w \times q+hw] $$

6.3、Forward Kinematics¶

calculation of the configuration（pose） $T=(R,p)$ of the end-effector frame from joint variables $\theta =(\theta_1,...,\theta_n)$

PoE（Product of Exponential）Formula $$ T_{sb}(\theta_1,...,\theta_n)=e^{[^{0}\mathcal{S}_1]\theta_1} e^{[^{0}\mathcal{S}_2]\theta_2} ... e^{[^{0}\mathcal{S}_n]\theta_n}M $$

步骤： 1、计算$\theta_1 = \theta_2= ...=\theta_n=0$时，$M$的值 2、从后往前，转动最后一个关节，其余关节为0 3、继续转动倒数第二关节，其余关节为0 4、。。。 5、按照screw motion计算，在计算PoE公式

6.4、Velocity Kinematics¶

deriving the Jacobian matrix:linearized map from the joint velocities $\dot \theta$ to the spatial velocity $\mathcal{V}$ of the end-effector

1、（重点） 空间雅可比（space Jacobian）,$J_s(\theta)$，简单理解为基于基坐标系的，或张巍老师讲的情况。公式如下： $$ J_s(\theta)=$$

2、物体雅可比（body Jacobian），$J_b(\theta)$，末端（物体）坐标形式下的雅可比矩阵。

6.5、Dynamics¶

6.6、旋量系、互异旋量系、运动旋量系、约束旋量系、等效运动副旋量系。。。¶

这部分内容偏机构学中的分析方法。