具身智能导论：机器人学

Lecture 2#

运动学 VS 动力学：运动学描述运动而不考虑力的来源

[!note] 记号假设 小写字母 $p$ 代表坐标，加粗字母 $\mathbf{v}$ 代表向量

运动信息可以用六维坐标 $(R_{s\to b}^{s},\mathbf{t}_{s\to b}^{s})$ 表示（三维是旋转，三维是平移）

$$p_{1}^{s} = R_{s\to b}^{s}p_{1}^{b}+\mathbf{t}_{s\to b}^{s}$$

为了让它变为线性变换，我们考虑齐次坐标

$$\tilde{x} :=\begin{pmatrix} x \\ 1 \end{pmatrix}\in \mathbb{R}^{4}, T_{s\to b}^{s}= \begin{bmatrix} R_{s\to b}^{s} &\mathbf{t}_{s\to b}^{s} \\ 0 & 1 \end{bmatrix},\quad \tilde{x}^{s} = T_{s\to b}^{s}\tilde{x}^{b}$$

变换可以复合而且可逆

$$T_{1\to {3}}^{1}= T_{1\to {2}}^{1}T_{2\to {3}}^{2},\quad T_{2\to 1}^{2}=(T_{1\to{2}}^{1})^{-1}$$

• Forward Kinematics：根据参数 $\theta$，计算 $T=f(\theta)$
• Inverse Kinematics：根据 $T(\theta)$ ，反解 $\theta$ 。有可能无解，但是如果有解往往无穷多解，因为在设计自由度的时候我们会留出来一些冗余，以增加避障等灵活性

Leture 3#

旋转矩阵的性质

• $\lVert Rp \rVert = \lVert p \rVert$ 保长度
• $Rp\times Rq =R (p\times q)$ 所以我们可以推出 $R R^{\top} = R^{\top}R=I,\det(R)=1$ 在三维空间中，所有的刚体变换只有旋转和平移，没有手性变换（反射）。但是平移不是线性变换，所以要引入第四维形成齐次坐标。

$$\begin{align} SO(3) &= \{ R \in \mathbb{R}^{3\times{3}}| R R^{\top} = R^{\top}R=I,\det(R)=1\} \\ SE(3) &= \{ (R,t)| R\in SO(3),t\in \mathbb{R}^{3}\} \end{align}$$

$SO(3)$ 里面的矩阵虽然是九个元素，但是其自由度只有 $3$ 个，可以用 Euler Angle 来解释。

• （Yaw-Pitch-Roll）的顺序
  • Yaw：绕着 $z$ 轴
  • Pitch：绕着新 $y$ 轴 $y'$
  • Roll：绕着新 $x$ 轴 $x''$

$$\begin{align} & R_{x}(\alpha):= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha \end{bmatrix}, R_{y}(\beta):= \begin{bmatrix} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \\ \end{bmatrix}, R_{z}(\gamma):= \begin{bmatrix} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & 0 \\ 0 & 0 & 1 \\ \\ \end{bmatrix} \\ & R(\alpha,\beta,\gamma) = R_{z}(\gamma)R_{y}(\beta)R_{x}(\alpha) \end{align}$$

• 缺点：但是不是一一对应，同一个旋转矩阵可以有多个 Euler Angle 与之对应，而且三个变量不独立，万向节死锁插值如果碰到奇异点会乱飞

Euler’s Theorem 证明也可以用 Axis Angle 来表示，$\hat{\omega}$ 表示转轴单位向量，$\theta$ 代表转动角度，但是只在 $\theta \in(0,\pi)$ 上的时候是一一对应的，一旦 $\theta=\pi$ 就会出现一对多，具体形式可以表示为

$$\mathrm{Rot} (\hat{\omega},\theta)x = \mathrm{e}^{ [\hat{\omega}] \theta}x$$

Rodrigues Formula：

$$\mathrm{e}^{ [\hat{\omega}] \theta} = I + \sin \theta [\hat{\omega}] + (1-\cos \theta)[\hat{\omega}]^2$$

给定 $R$，而且确定 $\theta \in(0,\pi)$ 那么 $\hat{\omega}$ 和 $\theta$ 可以唯一确定为

$$\theta = \cos^{-1} \left( \frac{\mathrm{tr}(R)-1}{2} \right),\quad \hat{\omega} = \dfrac{1}{2\sin \theta}(R-R^{\top})$$

1 Quaternion#

四元数（Quaternion）：真正我们经常使用的表示方法。

$$\begin{align} & q=w + x \mathbf{i}+y \mathbf{j}+z \mathbf{k} \\ & \mathbf{i}^{2}=\mathbf{j}^{2}=\mathbf{k}^{2}=\mathbf{i}\mathbf{j}\mathbf{k}=-1 \\ & \mathbf{i}\mathbf{j}=\mathbf{k}=-\mathbf{j}\mathbf{i},\mathbf{j}\mathbf{k}=\mathbf{i}=-\mathbf{k}\mathbf{j},\mathbf{k}\mathbf{i}=\mathbf{j}=-\mathbf{i}\mathbf{k} \end{align}$$

其中 $\omega$ 为实部，$\vec{v}=(x,y,z)$ 为虚部 四元数乘法的性质：

• 向量形式 (Vector form): $q = (w, \vec{v})$
• 乘法 (Product):
  • 对于 $q_1 = (w_1, \vec{v}_1)$ 和 $q_2 = (w_2, \vec{v}_2)$， $q_1 q_2 = (w_1 w_2 - \vec{v}_1^T \vec{v}_2, w_1 \vec{v}_2 + w_2 \vec{v}_1 + \vec{v}_1 \times \vec{v}_2)$
  • 不可交换 (Not commutable)（注意：$\vec{v}_1 \times \vec{v}_2 \neq \vec{v}_2 \times \vec{v}_1$）
• 共轭 (Conjugate): $q^* = (w, -\vec{v})$
• 范数 (Norm): $\| q \|^2 = w^2 + \vec{v}^T \vec{v} = q q^* = q^* q$
• 逆 (Inverse): $q^{-1} := \frac{q^*}{\| q \|^2}$ 所有模为 $1$ 的四元数用来表示一个旋转，恰好有 $3$ 个自由度。从 Axis Angle 可以直接转化为四元数 $q=[\cos(\theta / 2),\sin(\theta / 2)\hat{\omega}]$ 如何用四元数旋转

• 首先扩展 $\vec{x}$ 为 $x = (0, \vec{x})$
• 然后共轭变换 $x' = qxq^{-1}$ 四元数旋转可以直接复合：先 $q_{1}$ 再 $q_{2}$ ，则相当于 $q_{2}q_{1}$

[! note] 坐标表示 注意不同引擎下坐标顺序不同，可能是 $(w,x,y,z)$ 或 $(x,y,z,w)$。

从 quaternion 到 axis angle：

$$\theta = 2\cos^{-1}(w),\quad \hat{\omega} = \frac{1}{\sin(\theta/2)}\vec{v} \mathbb{1}_{\theta \ne 0}$$

1.1 插值#

在 $\mathbb{S}^{3}$ 上的四元数之间夹角 $<p,q> = \arccos (|p\cdot q|)$，对应的旋转的夹角是 $dist(p,q)=2\arccos(\lvert p\cdot q \rvert)$，所以在四元数上做插值等价于在 $\mathbb{S}^{3}$ 上做插值。

四元数插值（slerp）

$$\begin{align} \psi & =\cos ^{-1}(q_{0}\cdot q_{1} ) \\ q(t) & = \frac{q_{0}\sin(1-t)\psi+q_{1}\sin t\psi}{\sin \psi} \end{align}$$

[! note] 为什么不能直接对四元数进行线性插值

• 首先线性运算得到的未必是归一化的
• 即使归一化之后，也不能保持在球面上恒定的旋转速率

1.2 采样#

球面上均匀分布的采样方法，可以高斯采样然后归一化

$$x \sim \mathcal{N}(0, I),\quad q = \frac{x}{\| x \|}$$

两个旋转 $R_{1},R_{2}$ 之间的距离：

$$(R_{2}R_{1}^{\top})R_{1}=R_{2}\implies \operatorname{dist}(R_{1},R_{2})=\arccos \left( \frac{\mathrm{tr}(R_{2}R_{1}^{\top})-1}{2} \right)$$

[!summary] Why Quaternions？

• 尽量少的冗余，四个数就够用
• 乘法更简单，每次乘法 16 个乘运算+12 个加运算
• 数学性质
  • 旋转矩阵连乘会因为浮点数精度损失更多
  • 四元数正交化只要归一化就可以了
  • 复合运算很方便

Lecture 4#

Motion Planning 可以转换为路径搜索问题：如何在无碰撞空间 $C_{free}$ 里找到一条路径连接 $q_{start}$ 和 $q_{goal}$ ，不过该问题是在一个高维空间内的。

1 Collision Check#

由于维度过高，直接做搜索是不可能的。且如何确定一个 $q$ 是否属于 $C_{free}$ 都是困难的，这个工作称之为 Collision Check

• 我们通常使用球来做近似，因为它做碰撞检测更简单；但模拟情况下不碰撞不代表真实情况下不碰撞。碰撞十分致命，尤其对于灵巧手，没有力传感器做 Compliance ，非常容易坏。
• 更精确的是凸凸碰撞测试（convex-convex collision check），对于非凸的形状，我们用尽量少的分解 Approximate Convex Decomposition (ACD):将其转化为凸凸碰撞

2 路径搜索#

Probabilistic Roadmap Method

• 第一步在 $C_{free}$ 里面做 sample，连接其中相邻的节点，连接起点终点，变成一个图
  • sample 的算法：rejection sampling，但是 uniform sample 可能导致忽视狭窄通道，所以我们考虑 Gaussian sample（最终会贴近障碍物边缘） 和 bridge sample（最终会贴近狭窄通道）
  • edge 的建立：对于每个点，只看最近的几个点就可以。这个过程是可以并行的。
• 第二步做路径搜索，如 Dijkstra

RT-based 算法

• RRT：绕着试试（exploration） 和贪婪前进（exploitation） 的结合
  • 如何找到树中最近的点：需要使用 KD Trees 等方法
  • 如何确定 $\epsilon$ ：如果 $\epsilon$ 太大了，很容易撞；如果 $\epsilon$ 太小了，可能会导致搜索效率过低。
• RRT-connect：从起点和终点同时开搜，朝着同一个点的方向前进
• Shortcutting：RRT 找出来的路径通常很绕，为了路径平滑性，我们会在已知节点里面随机 sample 两点，找到不绕路的做法
  • 但是只是局部做法，只能基于已知的节点，可能会错过更好的路径
  • 在一开始非常好用，后面边际收益递减

从 path 到 trajectory：考虑每个点到达的时间——除了路径是否可能，我们还需要考虑 dynamics 上速度、加速度是否可能。

3 Control System#

controller：考虑如何从当前位置 $s_{t}$ 到达目标位置 $\tilde{s}_{t}$，可能会出现 over shoot 和 under shoot 以及外力干扰等，所以需要 closed-loop control

3.1 PD-controller#

对于一阶问题，我们使用系统动力学

$$\ddot x + d \dot{x}+kx = u$$

其中，$u$ 是外力控制 P-term：$u=K_{p}x_{e}(t)$ 只考虑和误差 proportion 相关，$K_{p}$ 越大反应越快，但是也可能会过冲 D-term：$u = K_{p}x_{e}+K_{d}\dot{x}_{e}$ 考虑误差的导数，缓解了过冲

一般的操作是先调好 $K_{p}$ ，然后再加大 $K_{d}$ ，偶尔可能还会涉及到 $K_{i}$ 对于多自由度系统的 PD control，由于关节之间是耦合的，所以我们是先分别调，合起来再调。

3.2 PID control#

I（积分）control term：$K_{i}\int x_{e}\mathrm{d}t$，代表对过去误差的累积，通常用来消除稳态误差

PID control law

$$u=K_{p}x_{e}+K_{i}\int x_{e}\mathrm{d}t+K_{d}\dot{x}_{e}$$

当时对于现代机器人学，很少用到 PID，往往 PD 就够了，因为调起来简单，且够用，而且不那么需要关注每步的误差；对于误差的过度要求，反而可能会导致不必要的动力加强（比如意外的障碍导致无法达到准确稳态，反而不安全）；对于 PD 系统，如果希望消除稳态误差，可以考虑更多基于模型的补偿方案