具身智能导论

机器人学基础#

1 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$ 。有可能无解，但是如果有解往往无穷多解，因为在设计自由度的时候我们会留出来一些冗余，以增加避障等灵活性

2 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$ 轴 $y''$
• 但是不是一一对应，同一个旋转矩阵可以有多个 Euler Angle 与之对应，三个变量不独立，万向节死锁插值如果碰到奇异点会乱飞 也可以用 Axis Angle 来表示，$\hat{\omega}$ 表示转轴单位向量，$\theta$ 代表转动角度，但是只在 $\theta \in(0,\pi)$ 上的时候是一一对应的，一旦 $\theta=\pi$ 就会出现一对多

四元数（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)$。

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

$$\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}$$

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

[!summary] Why Quaternions？

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

3 Lecture 4#

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

3.1 Collision Check#

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

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

3.2 路径搜索#

Probabilistic Roadmap Method

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

RT-based 算法

• RRT：绕着试试（exploration） 和贪婪前进（exploitation） 的结合
• RRT-connect：从起点和终点同时开搜 为了路径平滑性，我们会引入 short cutting

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

3.3 Control System#

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

PD-controller P-term：只考虑和误差 proportion 相关，容易要么过冲要么太慢 D-term：考虑误差的导数，缓解了过冲 一般的操作是先调好 $K_{p}$ ，然后再加大 $K_{d}$ ，偶尔可能还会涉及到 $K_{i}$ 对于多自由度系统的 PD control，由于关节之间是耦合的，所以我们是先分别调，合起来再调。