Keyboard shortcuts

Press ← or → to navigate between chapters

Press S or / to search in the book

Press ? to show this help

Press Esc to hide this help

4.3.3 指向

位置规划最后落在标量 s(t)s(t) 上，姿态就没这么便宜——因为姿态的表示本身就有约束。最自然的想法（把旋转矩阵的元素直接插值）会破坏正交性，于是教材给出两条正路：把姿态换成三个欧拉角分别插值，或者换成一根轴 + 一个转角，把三维问题压成只规划一个标量 θ(t)\theta(t)。本节先讲清楚为什么不能直接插值，再依次走这两条路。录播 2:22:28–2:28:51。

4.3.3.1 旋转矩阵为什么不能直接插值

末端执行器的指向通常由（时变的）末端执行器坐标系相对基坐标系的旋转矩阵指定——众所周知，旋转矩阵的三列，表示的正是该坐标系相对基坐标系的 3 个单位向量。任务指定也常用旋转矩阵（2:23:08）。但为了生成轨迹，若在初始指向与最终指向的单位向量上做线性内插，就无法保证这些向量在每一瞬时时刻具有正则性：

定理 4.3.E · 旋转矩阵的线性插值破坏正交性

设初始与最终姿态分别为 RiR_i、RfR_f。若对两矩阵逐元素做线性插值

R(λ)=(1−λ)Ri+λRf,λ∈[0,1],R(\lambda)=(1-\lambda)R_i+\lambda R_f,\qquad \lambda\in[0,1],

则除 λ=0,1\lambda=0,1 外一般有 R(λ)TR(λ)≠IR(\lambda)^{\mathrm T}R(\lambda)\ne I。

理由（老师 2:23:33 口述）：旋转矩阵的正交性要求任意两列的内积为 0、每一列的模为 1。把两个正交矩阵的元素按同一比例混合，得到的矩阵一般不再是正交矩阵——「如果简单的插值，从初始到终止每个元素直接插值的话，它不能保证（正交性），这是有问题的」。

所以不能直接从旋转矩阵做插值。教材给出两种可行的表示（2:24:01）：欧拉角与角和轴。

4.3.3.2 方法一：欧拉角

定义 4.3.6 · 欧拉角时间律

为了生成轨迹，改用欧拉角三元组 φe=(φ,ϑ,ψ)\varphi_e=(\varphi,\vartheta,\psi) 描述指向，并为它指定时间律。通常让 φe\varphi_e 沿连接初始值 φi\varphi_i 与最终值 φf\varphi_f 的线段移动。在这种情形下，同样方便的做法是选用三次多项式时间律，或混合抛物线线性分段时间律——这样一来，时变坐标系的角速度 ωe\omega_e 具有连续性，并通过 定理 3.6.A 的线性关系 ωe=T(φe)φ˙e\omega_e=T(\varphi_e)\dot\varphi_e 与 φe\varphi_e 相关。

因此，对于给定的 φi\varphi_i、φf\varphi_f 以及时间律，位置、速度和加速度曲线为

φe=φi+s∥φf−φi∥(φf−φi),φ˙e=s˙ φf−φi∥φf−φi∥,φ¨e=s¨ φf−φi∥φf−φi∥,(4.51)\varphi_e=\varphi_i+\frac{s}{\|\varphi_f-\varphi_i\|}(\varphi_f-\varphi_i),\qquad \dot\varphi_e=\dot s\,\frac{\varphi_f-\varphi_i}{\|\varphi_f-\varphi_i\|},\qquad \ddot\varphi_e=\ddot s\,\frac{\varphi_f-\varphi_i}{\|\varphi_f-\varphi_i\|}, \tag{4.51}

其中必须为 s(t)s(t) 指定时间律（三次、梯形、样条均可——又是那个标量 s(t)s(t)）。

回到基坐标系（2:25:00）：插值虽然是在欧拉角空间里做的，但实际控制需要的是基坐标系（零空间）中的量，所以还差最后一步——先由三个欧拉角算出旋转矩阵 ReR_e（ZYZ 的关系见 原理 2.4.A），再由 ReR_e 与分析雅可比的关系算出基系下的角速度与角加速度（3.6）：

Re=Re(φe) ⟹ ωe=T(φe) φ˙e,ω˙e=T˙φ˙e+Tφ¨e.R_e=R_e(\varphi_e)\ \Longrightarrow\ \omega_e=T(\varphi_e),\dot\varphi_e,\qquad \dot\omega_e=\dot T\dot\varphi_e+T\ddot\varphi_e .

因此姿态规划也要规划角位置、角速度、角加速度三个量——「其实跟位置规划很像」（2:26:00）。

4.3.3.3 方法二：角和轴

还有一种轨迹生成方法，能在笛卡儿空间里更清楚地解释指向。它借助 2.5 的角和轴描述（定义 2.5.1）：给定两个在笛卡儿空间中具有相同原点和不同指向的坐标系，总可以确定一个单位向量，使得第一个坐标系绕该单位向量的轴旋转一个合适的角度，得到第二个坐标系（2:26:06）。

定理 4.3.F · 角和轴的轨迹生成

令 RiR_i、RfR_f 分别表示初始坐标系与最终坐标系绕基坐标系的旋转矩阵。回顾 Rf=RiRifR_f=R_iR_i^f，两坐标系之间的旋转矩阵 Rif=RiTRfR_i^f=R_i^{\mathrm T}R_f 可由 2.5 的表达式计算。若定义 R′(t)R'(t) 描述 RiR_i 到 RfR_f 的变换，则必有 R′(0)=IR'(0)=I、R′(tf)=RifR'(t_f)=R_i^f。

于是 RifR_i^f 可表示为绕空间中一固定轴的旋转矩阵；当 sin⁡θf≠0\sin\theta_f\ne0 时，用 定义 2.5.1 的公式计算轴的单位向量 r′r' 与旋转角 θf\theta_f：

θf=arccos⁡ ⁣(r11+r22+r33−12),r′=12sin⁡θf[r32−r23r13−r31r21−r12].(4.52)\theta_f=\arccos\!\left(\frac{r_{11}+r_{22}+r_{33}-1}{2}\right),\qquad r'=\frac{1}{2\sin\theta_f}\begin{bmatrix}r_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{bmatrix}. \tag{4.52}

（式 4.52 不是新公式——它就是 2.5.2 的逆解「取迹求角、反对称部分求轴」，即对数映射 log⁡Rif=θfS(r′)\log R_i^f=\theta_fS(r')（2.5.3）。下式的 R′(t)=R′(θ(t),r′)R'(t)=R'(\theta(t),r') 则是指数映射随时间「走」：R′(t)=exp⁡(θ(t)S(r′))R'(t)=\exp\big(\theta(t)S(r')\big)。）

矩阵 R′(t)R'(t) 可视为 R′(θ(t),r′)R'(\theta(t),r')，从而只需为标量 θ\theta 指定一个时间律，其类型与单个关节的时间律相同，且 θ(0)=0\theta(0)=0、θ(tf)=θf\theta(t_f)=\theta_f。因为 r′r' 为常量，得到的速度和加速度分别为

ω′=θ˙ r′,ω˙′=θ¨ r′.(4.53,4.54)\omega'=\dot\theta\,r',\qquad \dot\omega'=\ddot\theta\,r' . \tag{4.53,4.54}

最后，为表征末端执行器关于基坐标系的指向轨迹，做一次变换：

Re(t)=RiR′(t),ωe=Ri ω′(t),ω˙e=Ri ω˙′(t).(4.55)R_e(t)=R_iR'(t),\qquad \omega_e=R_i\,\omega'(t),\qquad \dot\omega_e=R_i\,\dot\omega'(t) . \tag{4.55}

为什么这一招高明？ 老师 2:27:14 点破：「这个轴是固定的、不变的，那我们只要规划 θ\theta 就可以了——相当于把它变成一个一维的问题。θ\theta 可以从 0 变化到 θf\theta_f，而且 θ\theta 的时间率也完全可以用咱们上节课讲的（点对点 / 系列点、三次 / 梯形速度曲线）。」——三维姿态规划被压成了一个标量规划，与位置规划的 s(t)s(t) 完全同构。

考点 · 老师原话（录播时间点）

本节老师明确「不详细讲」，但原理必须知道。 「这个（角和轴）有时间关系不能详细地讲，但是原理可以给大家说一下」（2:26:06）。姿态规划在考试中属于了解层面，但两条思路（欧拉角 / 角和轴）要能说清。

核心障碍是正交性。 「旋转矩阵我指定初始的姿态和终止的姿态，但是从初始到终止，如果采用简单的插值的方法，它不能保证整个过程这个旋转矩阵它都是正交的……这是有问题的」（2:23:14）。这是姿态规划的出发点，也是必答的一句。

角和轴把三维压成一维。 「任何一个旋转，它都可以表示成空间的一个轴以及绕着这个轴的一个旋转……那么这个轴是固定的、不变的，那我们只要规划 θ\theta 就可以了——相当于把它变成一个一维的问题」（2:26:50）。

欧拉角法要记得「回到基坐标系」。 「实际用的过程中，我们还要把它表示在基坐标系，或者是零空间内」（2:25:01）——由 ReR_e 经分析雅可比换算，别忘了这一步。

默写点 · 合上书再写（主动回忆）

不看正文与速查表，先写出下面几条，再展开核对。写不出来就说明这一节还没「拿走」。

  1. 旋转矩阵为什么不能逐元素线性插值？
  2. 写出欧拉角法的位置 / 速度 / 加速度三式 (4.51)，并说明插值完还要做什么。
  3. 写出角和轴法的 θf\theta_f 与 r′r' 公式，并说明为什么只需规划一个标量。
核对

正交性要求任意两列内积为 0、每列模为 1；两个正交矩阵逐元素按同一比例混合后，得到的矩阵一般不再满足这两条——R(λ)TR(λ)≠IR(\lambda)^{\mathrm T}R(\lambda)\ne I。

φe=φi+s∥φf−φi∥(φf−φi)\varphi_e=\varphi_i+\dfrac{s}{\|\varphi_f-\varphi_i\|}(\varphi_f-\varphi_i)，φ˙e=s˙φf−φi∥φf−φi∥\dot\varphi_e=\dot s\dfrac{\varphi_f-\varphi_i}{\|\varphi_f-\varphi_i\|}，φ¨e=s¨φf−φi∥φf−φi∥\ddot\varphi_e=\ddot s\dfrac{\varphi_f-\varphi_i}{\|\varphi_f-\varphi_i\|}。插值完成后还要由欧拉角算出 ReR_e，再经 ωe=T(φe)φ˙e\omega_e=T(\varphi_e)\dot\varphi_e（定理 3.6.A）换算到基坐标系。

θf=arccos⁡ ⁣((r11+r22+r33−1)/2)\theta_f=\arccos\!\big((r_{11}+r_{22}+r_{33}-1)/2\big)，r′=12sin⁡θf[r32−r23; r13−r31; r21−r12]Tr'=\dfrac{1}{2\sin\theta_f}[r_{32}-r_{23};\ r_{13}-r_{31};\ r_{21}-r_{12}]^{\mathrm T}（Rif=RiTRfR_i^f=R_i^{\mathrm T}R_f 的元素）。因为 r′r' 是常量，姿态轨迹完全由 θ(t)\theta(t) 决定——三维姿态规划降为一维标量规划，时间律直接复用点对点 / 系列点的方法。

4.3.3 小结与常见错误

  • 一条主线：旋转矩阵不能直接插值（正交性）⇒ 两条正路：欧拉角（三标量分别插值，再经 TT 换算回基系）与角和轴（轴固定、只规划 θ\theta）。
  • 一个统一：位置规划落在 s(t)s(t)、姿态规划落在 θ(t)\theta(t)——都化成一维标量时间律，都能复用 4.2.1 / 4.2.2 的全部武器。
  • 一句话：「姿态规划比位置规划复杂，复杂在表示；一旦选对表示（欧拉角或角和轴），剩下的还是那几套一维时间律。」
常见错误清单 · 自查三条
  1. 逐元素插值旋转矩阵：得到的是「中间矩阵」，不是旋转矩阵——R(λ)R(\lambda) 既不正交，也不能当作姿态使用。
  2. 欧拉角插值后忘了回基坐标系：插出来的是 φe\varphi_e，控制要的是 ωe\omega_e；中间必须过 T(φe)T(\varphi_e)（3.6），且 TT 本身有奇点（sin⁡ϑ=0\sin\vartheta=0）。
  3. 角和轴法忘了 r′r' 是在哪个系里：r′r' 是 Rif=RiTRfR_i^f=R_i^{\mathrm T}R_f 的等效轴；ω′\omega' 还要左乘 RiR_i 才是基系下的 ωe\omega_e。