具身智能导论：机器人学

机器人学基础#

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，由于关节之间是耦合的，所以我们是先分别调，合起来再调。

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 就够了，因为调起来简单，且够用，而且不那么需要关注每步的误差；对于误差的过度要求，反而可能会导致不必要的动力加强（比如意外的障碍）

4 Lecture 5#

4.1 Grasping#

位置和方向

• 4-DoF grasp 也被称为 top-down grasp；只有一个方向，即 上下动
• 主要学习 6-DoF grasp，位置和方向都是 3 D 关节活动度（手指自由度）  
• 平行夹爪：1 个自由度，二指夹爪看起来两个手指，但实际上只有一个自由度
  • 在夹东西的时候常常是一把抓到死，即加上最大负载
• 灵巧手：最多可达 22 个自由度

6-DoF 怎么得到

• 对于已知物品（labeled）：一种方法是直接给出物体的 3 D 坐标加 3 D 旋转，然后从世界坐标系转移到机械臂坐标系
• 对于未知物品（unknown）：根据点云估计，泛化能力更强 但是这两种方法都是开环策略，即 control 与 grasp 分开，如果物品移动或已经碰撞到了则无法处理； 与之相对的是闭环策略，可以处理各种干扰，但是基本都需要用到大模型，可解释性不强（实际工作常常不容许半点差错，且干扰不那么多）

4.2 6D Estimation#

物体首先得是刚体，然后要确定他的标架

• 为了确定物体的 6 D 坐标，在相机内参确定时，RGB 图就足够了；
• 但是内参未知的时候，RGB 是不够的，还需要 depth（深度）得到点云
• 大部分情况下，Depth 相机获得点云就够了，RGB 不是重要的，但是对于对称性比较好的物体，RGB 是必需的。

PoseCNN

• 判断平移（translation）中心

  • 先判断像素属于哪个物体类别（segmentation）
  • 再让像素投票判断距离中心的方向和距离
  • 使用 Hough voting layer ，综合 segmentation 和投票的结果得到最终结果（相比直接卷积出结果，这样更加鲁棒）

• 判断旋转

  • 与判断平移类似，但是加了一步
  • 对每个物品做 ROI Pool 得到 feature map，然后再塞 MLP 实际操作中，平移和旋转的判断精度都不那么高。

4.3 Iterative Closest Point(ICP)#

• 对于已知物体，由于已经知道了它的形状和坐标，我们可以 sample 出初始点云，根据算出的 $R$ 和 $T$给出估计的新点云，将其与真实目标点云比较，迭代更新 $R,T$
• ICP 的 Loss 不可能做到 0，因为 1） sample 的点个数不一定相等 2）sample 是随机的
  • 对于一一对应的东西，我们只需要做 SVD，然后得到一个旋转矩阵
  • 对于没有一一对应的东西，我们把最近的点作为其估计（nearest estimation），估错了不要紧，因为可以反复迭代更新
• ICP 不一定迭代到最好的结果，而且 ICP 速度不快，在实时性要求高的领域很难应用。但是需要 depth，对于现实要求可能不符合。
• ICP 属于估计的 post process，本质是点云的配准；为了达到更快的收敛速度，不一定要 point-to-point，也可以 point-to-plane。

4.4 Rotation Regression#

对于旋转的判断，其目标函数可以选取 quaternion 的 L2 Loss，但是对神经网络这种连续函数，不连续点会出现很大误差。

可以证明在 $\mathbb{R}^5$ 以下的空间不存在到 $SO(3)$ 连续的双射，可以用 $9$ 维空间来预测，对其进行 SVD

4.5 Pose Fitting#

可以用坐标的 Pose-Fitting：每个像素输出其对应的 3 D 坐标（有 depth 是 3 D-3 D，否则需要相机内参），然后使用 RANSAC 并 SVD

4.6 Render-and-Compare: FoundationPose as an Example#

2D 版本的 ICP。先用基础模型粗略估计模型的坐标，然后渲染出粗略的估计，并输入下一个模块，让下一个模块猜测如何微调。 但是需要 CAD model，为了解决这个问题，我们又研究了 Category-Level 6 D Object Pose Estimation，只需要已知类别就可以泛化。

5 Lecture 6#

对于已知物体，可以采取从 Object pose 到 grasping pose 的办法，但是未知物体需要直接预测 grasping pose 开环策略指的是只能 motion plan 然后去抓，应对不了位置改变， 闭环策略指的是循环操作，可以应对 object 位置的改变

5.1 Grasp Detection#

detect 的目标：6 D（R 和 T）+1 D（开合的宽度） 如何选取 input？我们很少直接使用 image（RGB） 作为预测，而是使用 3 D 特征，比如多方向图片得到 Voxel。但是 Voxel 的精度导致太昂贵了，我们一般选取物体表面的点云来进行预测。

抓取效果

• success rate：越高越好
• Percent cleared：越高越好
• Planning time：越低越好

5.1.1 GS-Net#

GS-Net：一个常用的 Grasp detection 网络 voxel-based 的网络设计较简单：先预测 voxel 是否是抓取中心，再输出抓取预测 点云怎么办：不能只对表面猜测，而是 where + how

• where 通过 U-Net 学习哪些位置附近适合抓取，给出 graspness 评分，再进一步筛选得到前 M 个
• how：xyz+view（视线朝向）+angle（旋转角度）+depth（插入深度）一共 7 DoF，其中每一步都是在离散得到，然后分别评分
• 为什么 GS-Net 优秀？
  • 抓取对象是点云，只有几何信息，与颜色无关
  • 抓取本身与物体关系不大，只看局部信息，所以好泛化
  • GS-Net 本身在确定抓取 angle 时只处理附近的局部信息，不看其他无关变量
• 但是 GS-Net 离散太多了，不是很自然

5.1.2 条件生成模型-Conditional Grasp Generative Model for Dexterous Hand#

但是扩散模型也很容易包含很多无关信息 所以借鉴 GS-Net 的思路，也要先选点，然后考虑点附近的局部信息。

• where：这部分和 GS-Net 差不多，也是找点然后预测是否合适抓
• how：这部分不一样，是将局部信息喂给 Diffusion 模型

5.2 抓取数据合成#

force-closure：物理上的合成方法，其核心思想是任意微小扰动都能维持抓取状态

• 所有接触点的 wrench cone 的正组合，可以覆盖整个 wrench space。
• 其中 wrench 包括力和力矩，$\omega=[f,\tau]^{\top}$
• 所谓 form-closure，指的是不靠摩擦，只用几何形状就锁住了物体

在 GS-Net 中，合成数据的过程其实尝试了很多 $\mu$ 下的 force closure 来训练模型，结果发现 $\mu=0.1$ 这种小的值效果最好，抓法也更保守。

5.3 Camera Model#

5.4 Hand-Eye Calibration#

• hand in eye: 机械臂上装了一个相机，在计算基座坐标系下位置时，要先从相机坐标系到机械臂末端坐标系，然后再从机械臂末端坐标系到基座坐标系

$$XYZ_{CAMERA} \to XYZ_{EE} \to XYZ_{ROBOT}$$

• hand to eye: 相机相对基座位置不变，可以直接从相机坐标系到基座坐标系

$$XYZ_{CAMERA} \to XYZ_{ROBOT}$$

5.4.1 Hand in Eye#

为了解决这个问题，其实就是要解决 camera 到 end-effector 的坐标变换 $H_{CAMERA}^{EE}$ 先移动机械臂获取两方面数据：机械臂相对基座的位置和相机看到的标定板位置 根据前者，我们可以知道机械臂的变换 $A: E_{1}\to E_{2}$；根据后者，我们可以计算出相机的变换 $B: C_{1}\to C_{2}$，于是我们就可以算出但是从 $E_{i}$ 到 $C_{i}$ 的变换 $H_{CAMERA}^{EE}$ 不变，所以只需要解方程

$$A H_{CAMERA}^{EE} = H_{CAMERA}^{EE} B$$

5.4.2 Hand to Eye#

类似地，我们把 calibration board 放在机械臂末端，然后移动机械臂获取两方面数据：机械臂相对基座的位置和相机看到的标定板位置，但是由于标定板在机械臂末端，所以 $E_{i}$ 到 $C_{i}$ 的变换 $H_{board}^{EE}$ 是不变的，我们也可以得到类似地方程

$$A H_{board}^{EE} = H_{board}^{EE} B$$

然后再根据它得到，相机相对基座的变换

$$H^{Camera}_{Base}= H^{Camera}_{board}H^{board}_{EE}H^{EE}_{Base}$$