公式速查卡

五章共 70 余条编号公式、9 张 DH 参数表、3 套逆解公式——它们散在 40 余个小节里。本页把它们按「表示 → 传递 → 反解 → 微分 → 力 → 驱动与测量」的顺序压成一页，每条后面给出正文出处，供做题与复习时单点查阅。

A · 姿态表示（第 2 章）

A1 旋转矩阵的三条性质

三重身份（同一矩阵、三种读法）：姿态描述 / 坐标变换 / 旋转算子——定理 2.2.B。

A2 基本旋转

$$R_x(\vartheta)=\begin{bmatrix}1&0&0\0&c_\vartheta&-s_\vartheta\0&s_\vartheta&c_\vartheta\end{bmatrix},\quad R_y(\vartheta)=\begin{bmatrix}c_\vartheta&0&s_\vartheta\0&1&0\-s_\vartheta&0&c_\vartheta\end{bmatrix},\quad R_z(\vartheta)=\begin{bmatrix}c_\vartheta&-s_\vartheta&0\s_\vartheta&c_\vartheta&0\0&0&1\end{bmatrix}$$

A3 合成规则（一切序列的母规则）

口诀：右乘当前轴、左乘固定轴。ZYZ / RPY 的对偶、DH 的 $A_i$ 连乘、角和轴的五步序列，全是这一条的换装登场。

A4 ZYZ 欧拉角（绕当前轴，右乘）

$$R(\boldsymbol\phi)=R_z(\varphi)R_{y'}(\vartheta)R_{z''}(\psi)= \begin{bmatrix} c_\varphi c_\vartheta c_\psi-s_\varphi s_\psi & -c_\varphi c_\vartheta s_\psi-s_\varphi c_\psi & c_\varphi s_\vartheta\ s_\varphi c_\vartheta c_\psi+c_\varphi s_\psi & -s_\varphi c_\vartheta s_\psi+c_\varphi c_\psi & s_\varphi s_\vartheta\ -s_\vartheta c_\psi & s_\vartheta s_\psi & c_\vartheta \end{bmatrix}$$

逆解（$s_\vartheta\neq0$，原书式 2.19；两支等价）：

$$\varphi=\operatorname{Atan2}(r_{23},r_{13}),\quad \vartheta=\operatorname{Atan2}!\Big(\sqrt{r_{13}^2+r_{23}^2},\ r_{33}\Big),\quad \psi=\operatorname{Atan2}(r_{32},-r_{31})$$

第二支：三个宗量整体变号（$\vartheta$ 取 $(-\pi,0)$）。奇点 $s_\vartheta=0$：只能定 $\varphi\pm\psi$，见 定理 2.4.B。

A5 RPY 角（绕固定轴，左乘）

$$R(\boldsymbol\phi)=R_z(\varphi)R_y(\vartheta)R_x(\psi)= \begin{bmatrix} c_\varphi c_\vartheta & c_\varphi s_\vartheta s_\psi-s_\varphi c_\psi & c_\varphi s_\vartheta c_\psi+s_\varphi s_\psi\ s_\varphi c_\vartheta & s_\varphi s_\vartheta s_\psi+c_\varphi c_\psi & s_\varphi s_\vartheta c_\psi-c_\varphi s_\psi\ -s_\vartheta & c_\vartheta s_\psi & c_\vartheta c_\psi \end{bmatrix}$$

逆解（$\vartheta\in(-\pi/2,\pi/2)$）：

$$\varphi=\operatorname{Atan2}(r_{21},r_{11}),\quad \vartheta=\operatorname{Atan2}!\Big(-r_{31},\sqrt{r_{11}^2+r_{21}^2}\Big),\quad \psi=\operatorname{Atan2}(r_{32},r_{33})$$

奇点 $c_\vartheta=0$（俯仰 $\pm90°$）。角名：roll 绕纵轴 $x$、pitch 绕 $y$、yaw 绕竖轴 $z$。

为什么反解一律用 Atan2：$\arccos$ 的值域只有 $[0,\pi]$（不唯一），且导数 $(\arccos x)'=-1/\sqrt{1-x^2}$ 在 $x\to\pm1$ 处无界（误差被无限放大）；$(\arctan x)'=1/(1+x^2)\le1$ 处处有界。见 原理 2.4.A。

A6 角和轴（4 参数，Rodrigues 公式）

$$R(\vartheta,\boldsymbol r)=\begin{bmatrix} r_x^2(1-c_\vartheta)+c_\vartheta & r_xr_y(1-c_\vartheta)-r_zs_\vartheta & r_xr_z(1-c_\vartheta)+r_ys_\vartheta\ r_xr_y(1-c_\vartheta)+r_zs_\vartheta & r_y^2(1-c_\vartheta)+c_\vartheta & r_yr_z(1-c_\vartheta)-r_xs_\vartheta\ r_xr_z(1-c_\vartheta)-r_ys_\vartheta & r_yr_z(1-c_\vartheta)+r_xs_\vartheta & r_z^2(1-c_\vartheta)+c_\vartheta \end{bmatrix}$$

A7 单位四元数（4 参数，无逆解奇点）

$$\eta=\cos\frac{\vartheta}{2},\qquad\boldsymbol\epsilon=\sin\frac{\vartheta}{2},\boldsymbol r,\qquad\eta^2+|\boldsymbol\epsilon|^2=1$$

$$R(\eta,\boldsymbol\epsilon)=\begin{bmatrix} 2(\eta^2+\epsilon_x^2)-1 & 2(\epsilon_x\epsilon_y-\eta\epsilon_z) & 2(\epsilon_x\epsilon_z+\eta\epsilon_y)\ 2(\epsilon_x\epsilon_y+\eta\epsilon_z) & 2(\eta^2+\epsilon_y^2)-1 & 2(\epsilon_y\epsilon_z-\eta\epsilon_x)\ 2(\epsilon_x\epsilon_z-\eta\epsilon_y) & 2(\epsilon_y\epsilon_z+\eta\epsilon_x) & 2(\eta^2+\epsilon_z^2)-1 \end{bmatrix}$$

$$\eta=\tfrac12\sqrt{r_{11}+r_{22}+r_{33}+1},\qquad \boldsymbol\epsilon=\tfrac12\begin{bmatrix}\operatorname{sgn}(r_{32}-r_{23})\sqrt{r_{11}-r_{22}-r_{33}+1}\ \operatorname{sgn}(r_{13}-r_{31})\sqrt{r_{22}-r_{33}-r_{11}+1}\ \operatorname{sgn}(r_{21}-r_{12})\sqrt{r_{33}-r_{11}-r_{22}+1}\end{bmatrix}$$

逆 $Q^{-1}={\eta,-\boldsymbol\epsilon}$；乘积 $Q_1\otimes Q_2={\eta_1\eta_2-\boldsymbol\epsilon_1^{\mathrm T}\boldsymbol\epsilon_2,\ \eta_1\boldsymbol\epsilon_2+\eta_2\boldsymbol\epsilon_1+\boldsymbol\epsilon_1\times\boldsymbol\epsilon_2}$。见 定义 2.6.1、原理 2.6.A。

A8 齐次变换（$SE(3)$）

$$\tilde{\boldsymbol p}=\begin{bmatrix}\boldsymbol p\1\end{bmatrix},\qquad A_1^0=\begin{bmatrix}R_1^0&\boldsymbol o_1^0\\boldsymbol 0^{\mathrm T}&1\end{bmatrix},\qquad A_0^1=(A_1^0)^{-1}=\begin{bmatrix}R_1^{0\mathrm T}&-R_1^{0\mathrm T}\boldsymbol o_1^0\\boldsymbol 0^{\mathrm T}&1\end{bmatrix}$$

连乘：$\tilde{\boldsymbol p}^0=A_1^0A_2^1\cdots A_n^{n-1}\tilde{\boldsymbol p}^n$（原理 2.7.A）。

两条纪律：$A^{-1}\neq A^{\mathrm T}$（正交性只在旋转块内成立）；方向向量（自由向量）的变换只用左上块 $R$，不吃平移。

B · 正运动学与 DH 法

B1 单关节变换 $A_i^{i-1}$（值得默写）

$$A_i^{i-1}(q_i)=\begin{bmatrix} c_{\vartheta_i} & -s_{\vartheta_i}c_{\alpha_i} & s_{\vartheta_i}s_{\alpha_i} & a_ic_{\vartheta_i}\ s_{\vartheta_i} & c_{\vartheta_i}c_{\alpha_i} & -c_{\vartheta_i}s_{\alpha_i} & a_is_{\vartheta_i}\ 0 & s_{\alpha_i} & c_{\alpha_i} & d_i\ 0&0&0&1 \end{bmatrix}$$

两步分解：先「沿 $z_{i-1}$ 平移 $d_i$ + 绕 $z_{i-1}$ 转 $\vartheta_i$」，再「沿 $x_i$ 平移 $a_i$ + 绕 $x_i$ 转 $\alpha_i$」，右乘合成（$A_i^{i-1}$：从建系几何直接读出两步分解）。

第 4 列就是三参数的「肉身」：$(a_ic_{\vartheta_i},\ a_is_{\vartheta_i},\ d_i)$；旋转块 = 绕 $z$ 转 $\vartheta$ $\oplus$ 绕 $x$ 转 $\alpha$。

B2 DH 四规则（每条各管一个参数）

记忆主线 · 看守模型（先把「系到底挂在哪」说透：看守模型）：系 $i-1$ 看守关节 $i$——$z_{i-1}$ 就是关节 $i$ 的轴线；杆 $i$ 固连系 $i$（原点在关节 $i+1$ 轴上）。建系四步实例见 例解。

总原则：能自由选取的地方「让参数尽量为零」（总原则：让参数尽量为零）——2.9 各表里成片的 $0$ 就是这条的兑现。

3 常 1 变：$a_i,\alpha_i$ 恒为常数；$d_i,\vartheta_i$ 中恰有一个是关节变量 $q_i$（转动关节 $q_i=\vartheta_i$、移动关节 $q_i=d_i$）——定义 2.8.3。

B3 十步流程（DH 十步操作流程）

1. 关节轴编号，定 $z_0,\dots,z_{n-1}$；2. 选系 0；3. 对 $i=1..n-1$ 定 $o_i$；
2. 沿公垂线取 $x_i$；5. 右手系补 $y_i$；6. 选系 $n$；7. 列 $4n$ 参数表；
3. 逐个算 $A_i^{i-1}$；9. 连乘得 $T_n^0(q)$；10. 串常值变换 $T_b^0,T_e^n$ 得 $T_e^b$。

课堂口诀：编号、建系（能零则零）、列参数表、代入、连乘。

B4 两个空间、工作空间与冗余

$$\boldsymbol x_e=k(\boldsymbol q),\qquad T_e^b(\boldsymbol q)=T_b^0,T_n^0(\boldsymbol q),T_e^n,\qquad T_n^0=A_1^0A_2^1\cdots A_n^{n-1}$$

B5 运动学标定

$$\Delta\boldsymbol x=\Phi,\Delta\boldsymbol\xi,\qquad \Delta\boldsymbol\xi=(\Phi^{*\mathrm T}\Phi^*)^{-1}\Phi^{*\mathrm T}\Delta\boldsymbol x^*,\qquad \boldsymbol\xi'=\boldsymbol\xi+\Delta\boldsymbol\xi$$

$\Phi$ 为 $m\times4n$ 标定矩阵，$\Delta\boldsymbol\xi$ 为 $4n$ 维参数偏差。量测条件 $tm\ge4n$（宜远大于，防 $\Phi$ 病态）；因 $k$ 非线性，须迭代。见 标定三步法。

C · 逆运动学

四难点（定理 2.12.A）：非线性（解方程而非算函数）、多解、可能无解 / 无穷多解、无通用解析法。

闭合解门槛（定理 2.12.B）：球形腕（三轴交一点）或三轴平行 ⇒ 可解耦。

解耦五步（解耦五步）——这是全书最实用的一套流程：

1. 由末端位姿反推腕心： $$\boldsymbol p_w=\boldsymbol p_e-d_6,\boldsymbol a_e$$ （$d_6$ = 腕心到末端原点的「外移长度」，沿接近向量 $\boldsymbol a_e$ 反推。）
2. 解臂：由 $\boldsymbol p_w$ 求 $(q_1,q_2,q_3)$（$\boldsymbol p_w$ 只依赖前三个关节）；
3. 算 $R_3^0=R_1^0R_2^1R_3^2$；
4. 解腕：由 $(R_3^0)^{\mathrm T}R_e=R_6^3$ 反解 $(q_4,q_5,q_6)$；
5. 整理全部解支。

腕 = ZYZ 反解：球形腕的姿态部分与 ZYZ 欧拉角反解同构（A4 的公式直接可用）。

数解支的口诀：位置解给「臂」的支数（平面臂 2 支 = 肘上 / 肘下，拟人臂 4 支），方向解给「腕」的支数（球腕 2 支 = 翻转），总数 = 臂 × 腕。

D · 微分运动学（第 3 章）

D1 微分运动学方程与旋转矩阵求导

$$\begin{bmatrix}\dot{\boldsymbol p}_e\\boldsymbol\omega_e\end{bmatrix}=J(\boldsymbol q),\dot{\boldsymbol q},\qquad J=\begin{bmatrix}J_P\J_O\end{bmatrix}\in\mathbb R^{6\times n}$$

$$\dot R=S(\boldsymbol\omega)R,\qquad S(\boldsymbol\omega)=\begin{bmatrix}0&-\omega_z&\omega_y\\omega_z&0&-\omega_x\-\omega_y&\omega_x&0\end{bmatrix},\qquad S(\boldsymbol\omega)\boldsymbol p\equiv\boldsymbol\omega\times\boldsymbol p$$

推导链：$RR^{\mathrm T}=I$ 求导 ⇒ $\dot RR^{\mathrm T}$ 反对称 ⇒ 用常模向量比对 $\boldsymbol\omega\times\boldsymbol r$ 认出 $S=S(\boldsymbol\omega)$（定理 3.1.A）。共轭关系 $RS(\boldsymbol\omega)R^{\mathrm T}=S(R\boldsymbol\omega)$（定理 3.1.B，已在 2.5 作为工具证明）。李代数读法：$\boldsymbol\omega=(\dot RR^{\mathrm T})^\vee$（基系读数），$R^{\mathrm T}\dot R$ 给刚体系读数（李代数视角：$\dot RR^{\mathrm T}$ 与 $R^{\mathrm T}\dot R$ 各读出谁的角速度）——「角速度 = $so(3)$ 的元素」（2.5.3）。

D2 逐列构造（绿框，必考）

$$J=\begin{bmatrix}J_1&\cdots&J_n\end{bmatrix},\qquad J_i=\begin{bmatrix}J_{P_i}\J_{O_i}\end{bmatrix},\qquad \begin{cases}\text{关节 }i\text{ 转动：}& J_{P_i}=z_{i-1}\times(\boldsymbol p_e-\boldsymbol p_{i-1}),\quad J_{O_i}=z_{i-1}\[1mm]\text{关节 }i\text{ 移动：}& J_{P_i}=z_{i-1},\quad J_{O_i}=\boldsymbol 0\end{cases}$$

见 定理 3.1.C。所需量全部从正运动学读取：

物理含义：其余关节全部锁死时，单个关节的单位速度对末端速度的贡献。

D3 换到别的坐标系

$$J^u=\begin{bmatrix}R^u&0\0&R^u\end{bmatrix}J$$

D4 奇点

$$\det J(\boldsymbol q)=0$$

D5 冗余与零空间

$$n>r\ \Longrightarrow\ \mathcal N(J)\neq{0},\qquad P=NN^{\mathrm T}\ (\text{秩 }n-r),\qquad \dot{\boldsymbol q}=\dot{\boldsymbol q}^*+P\dot{\boldsymbol q}_0$$

$P\dot{\boldsymbol q}_0$ 在零空间里，不惊动末端（$JP\dot{\boldsymbol q}_0=0$）——用来做内部运动 / 再配置（避障、躲关节极限、远离奇异）。见 定义 3.4.1、定理 3.4.B。

D6 逆微分运动学：三味药

见 定义 3.5.1、定义 3.5.2。

口诀：方阵求逆、矮胖求伪逆、退化加阻尼。 $K$ 的取法：奇点邻域取大（压住发散），远离奇点取小甚至趋 0（退回伪逆）。

$\dot{\boldsymbol q}_0$ 的三种取法（都取 $k_0(\partial g/\partial\boldsymbol q)^{\mathrm T}$，最大化 $g$）：

D7 分析雅可比

$$\boldsymbol\omega_e=T(\boldsymbol\varphi_e)\dot{\boldsymbol\varphi}_e,\qquad T(\boldsymbol\varphi)=\begin{bmatrix}0&-s_\varphi&s_\vartheta c_\varphi\0&c_\varphi&s_\vartheta s_\varphi\1&0&c_\vartheta\end{bmatrix}\ (\text{ZYZ}),\qquad J_A=T_A^{-1}J,\qquad T_A=\begin{bmatrix}I_3&0\0&T\end{bmatrix}$$

$T$ 的三列 = 欧拉角三根转轴在基系中的方向（未必正交）。线速度部分 $J$ 与 $J_A$ 相同，姿态部分差一个换基。$s_\vartheta=0$ 时 $T$ 奇异 ⇒ $J_A$ 失效（与 ZYZ 表示奇点同源）。见 定义 3.6.1、定理 3.6.A。

E · 静力学

$$\boxed{\ \boldsymbol\tau=J^{\mathrm T}(\boldsymbol q),F_e\ },\qquad F_e=\begin{bmatrix}\boldsymbol f_e\\boldsymbol\mu_e\end{bmatrix}\in\mathbb R^6$$

由虚功原理推出：$\dot{\boldsymbol q}^{\mathrm T}\boldsymbol\tau=\boldsymbol v_e^{\mathrm T}F_e=(J\dot{\boldsymbol q})^{\mathrm T}F_e=\dot{\boldsymbol q}^{\mathrm T}J^{\mathrm T}F_e$。速度变换的转置就是力变换（对偶），长方阵也成立，无需满秩。见 定义 3.8.1。

两坐标系间的速度 / 力变换：

$$\begin{bmatrix}\dot{\boldsymbol p}_1^0\\boldsymbol\omega_1^0\end{bmatrix}=\begin{bmatrix}R_1^0&-R_1^0S(\tilde{\boldsymbol p})\0&R_1^0\end{bmatrix}\begin{bmatrix}\boldsymbol v_1\\boldsymbol\omega_1\end{bmatrix}, \qquad \begin{bmatrix}\boldsymbol f_1^0\\boldsymbol\mu_1^0\end{bmatrix}=\begin{bmatrix}R_1^0&0\R_1^0S(\tilde{\boldsymbol p})&R_1^0\end{bmatrix}\begin{bmatrix}\boldsymbol f_1\\boldsymbol\mu_1\end{bmatrix}$$

右边（力）恰是左边（速度）的转置——平移块 $S(\tilde{\boldsymbol p})$ 在速度式里带负号、在力式里移位且不带负号，正是对偶的体现。

F · 轨迹规划（第 4 章）

F1 点对点：三次与五次

三次（4 个系数 ↔ 4 个约束）：$q(t)=a_3t^3+a_2t^2+a_1t+a_0$，由 $q(0)=q_i,\ \dot q(0)=\dot q_i,\ q(t_f)=q_f,\ \dot q(t_f)=\dot q_f$ 定系数。

规范化形式（初终速度为零时与数据无关，$q_i=0,q_f=\pi,t_f=1$ 时 $a_2=3\pi,a_3=-2\pi$）：

$$q(t)=q_i+(q_f-q_i)\Big[3\eta^2-2\eta^3\Big],\qquad \eta=\frac{t}{t_f}$$

五次（6 个系数 ↔ 位置/速度/加速度 × 两端）：

$$q(t)=q_i+(q_f-q_i)\Big[10\eta^3-15\eta^4+6\eta^5\Big]$$

关键点：能量最优解只能是三次；选五次就放弃该指标（定理 4.2.A、定义 4.2.2）。

F2 梯形速度曲线（LSPB，工业重点）

速度梯形：起段常加速度 $\ddot q_c$、中段巡航 $\dot q_c$、末段常减速度；位置 = 直线段 + 两端抛物线。

$$t_m=\frac{t_f}{2},\quad q_m=\frac{q_f+q_i}{2},\qquad \ddot q_ct_c=\frac{q_m-q_c}{t_m-t_c},\quad q_c=q_i+\tfrac12\ddot q_ct_c^2$$

$$\Longrightarrow\quad \ddot q_ct_c^2-\ddot q_ct_ft_c+(q_f-q_i)=0$$

$$t_c=\frac{t_f}{2}-\frac12\sqrt{\frac{t_f^2\ddot q_c-4(q_f-q_i)}{\ddot q_c}}, \qquad |\ddot q_c|\ge\frac{4|q_f-q_i|}{t_f^2}$$

指定巡航速度（替代指定加速度）：

$$\frac{|q_f-q_i|}{t_f}<|\dot q_c|\le\frac{2|q_f-q_i|}{t_f},\qquad t_c=\frac{q_i-q_f+\dot q_ct_f}{\dot q_c},\qquad \ddot q_c=\frac{\dot q_c^2}{q_i-q_f+\dot q_ct_f}$$

分段位置（(4.8)）：

$$q(t)=\begin{cases}q_i+\tfrac12\ddot q_ct^2 & 0\le t\le t_c\ q_i+\ddot q_ct_c(t-t_c/2) & t_c<t\le t_f-t_c\ q_f-\tfrac12\ddot q_c(t_f-t)^2 & t_f-t_c<t\le t_f\end{cases}$$

极限：下界取等 ⇒ 三角形速度曲线（无巡航段）；代价：能量比最优高 12.5%（定义 4.2.3、定理 4.2.C）。

F3 系列点：四条路线

① 指定路径点速度：$\Pi_k(t_k)=q_k,\ \Pi_k(t_{k+1})=q_{k+1},\ \dot\Pi_k(t_k)=\dot q_k,\ \dot\Pi_k(t_{k+1})=\dot q_{k+1}$（每段独立四方程）。

② 按规则计算速度（(4.12)，$v_k=\dfrac{q_k-q_{k-1}}{t_k-t_{k-1}}$）：

$$\dot q_1=0,\qquad \dot q_k=\begin{cases}0 & \operatorname{sgn}v_k\ne\operatorname{sgn}v_{k+1}\ \frac{v_k+v_{k+1}}{2} & \operatorname{sgn}v_k=\operatorname{sgn}v_{k+1}\end{cases},\qquad \dot q_N=0$$

③ 三次样条（加速度连续）——方程计数与降维：

$$\underbrace{4(N-2)+6}_{4N-2\ \text{个方程}}\ \text{vs}\ \underbrace{4(N-1)}_{\text{未知数}}\ \Rightarrow\ \text{差 }2\ \Rightarrow\ \text{两个虚拟点}\ \Rightarrow\ 4(N+1)=4(N+1)$$

$$\ddot\Pi_k(t)=\frac{\ddot\Pi_k(t_{k+1})}{\Delta t_k}(t_{k+1}-t)+\frac{\ddot\Pi_k(t_k)}{\Delta t_k}(t-t_k)\quad\Longrightarrow\quad A,\boldsymbol\Pi=b$$

$A$ 为三对角、非奇异，且只依赖 $\Delta t_k$——对所有关节相同（定理 4.2.H）。

④ 抛物线 + 线性混合（经由点）：

$$q_{k-1,k}=\frac{q_k-q_{k-1}}{\Delta t_k},\qquad \ddot q_k=\frac{q_{k,k+1}-q_{k-1,k}}{\Delta t'_k},\qquad b_{k0}=q_k+\frac{(\dot q_{k,k+1}-\dot q_{k-1,k})\Delta t'_k}{8}$$

轨迹不经过路径点（削角量 = 上式第二项），故称经由点（定义 4.2.7）。

F4 操作空间：路径基元与位置

参数化与 Frenet 标架：

$$p=f(s),\qquad t=\frac{\mathrm dp}{\mathrm ds},\qquad n=\frac{\mathrm d^2p/\mathrm ds^2}{|\mathrm d^2p/\mathrm ds^2|},\qquad b=t\times n$$

直线（$p_l\to p_f$）：$p(s)=p_l+\dfrac{s}{|p_f-p_l|}(p_f-p_l)$，$\dfrac{\mathrm d^2p}{\mathrm ds^2}=\mathbf 0$（法向无法定义）。

圆弧（轴 $r$、轴上点 $d$、圆上点 $p_l$，$\delta=p_l-d$，退化条件 $|\delta^{\mathrm T}r|<| \delta|$）：

$$c=d+(\delta^{\mathrm T}r)r,\qquad p(s)=c+R,p'(s),\qquad p'(s)=\big[\rho\cos(s/\rho),\ \rho\sin(s/\rho),\ 0\big]^{\mathrm T}$$

位置轨迹 = 几何 × 时间：

$$\dot p_e=\dot s,t,\qquad \ddot p_e=\underbrace{\frac{\dot s^2}{\rho}n}_{\text{向心：改方向}}+\underbrace{\ddot s,t}_{\text{切向：改大小}}$$

多段路径与时间超前：$p_e=p_0+\sum_j\frac{s_j}{|p_j-p_{j-1}|}(p_j-p_{j-1})$；$\Delta t_j=\Delta t_{j-1}+\delta t_j$（定理 4.3.D）。

F5 操作空间：指向

旋转矩阵不能逐元素插值（破坏正交性）⇒ 两条正路：

欧拉角：$\varphi_e=\varphi_i+\dfrac{s}{|\varphi_f-\varphi_i|}(\varphi_f-\varphi_i)$，再经 $\omega_e=T(\varphi_e)\dot\varphi_e$ 换回基系。

角和轴（$R_i^f=R_i^{\mathrm T}R_f$）：

$$\theta_f=\arccos!\Big(\frac{r_{11}+r_{22}+r_{33}-1}{2}\Big),\qquad r'=\frac{1}{2\sin\theta_f}\begin{bmatrix}r_{32}-r_{23}\ r_{13}-r_{31}\ r_{21}-r_{12}\end{bmatrix},\qquad \omega'=\dot\theta,r'$$

两次降维：位置落在 $s(t)$、姿态落在 $\theta(t)$——都是一维标量，直接复用关节空间的方法。

G · 执行器与传感器（第 5 章）

G1 执行系统功率链与功率符号

能源 → 功率放大器 → 伺服发动机 → 传动装置 → 关节（定义 5.1.1）。功率恒为「力 × 速度」，由此可辨功率种类（功率总是「力 × 速度」）。

$$P_c\ \text{（控制信号）}\Rightarrow P_a\ \text{（供给发动机）}\Rightarrow P_m\ \text{（发动机输出）}\Rightarrow P_u\ \text{（关节所需）}$$

三处损失：$P_{da}$ 放大器、$P_{dm}$ 发动机、$P_{dt}$ 传动装置（定义 5.1.2）。

G2 工况矛盾与传动装置三重作用

关节要低速度、高转矩；伺服发动机最优工况是高速度、低转矩（定理 5.1.B）⇒ 必须加传动装置。三重作用：定量（降速增矩）/ 定性（转动→平动）/ 减重优化有效载荷（定义 5.1.3）。无间隙刚性传动 ⇒ 输入输出纯比例（定理 5.1.F）。

G3 电气驱动的四个基本方程

$$V_a=(R_a+sL_a)I_a+V_g,\qquad V_g=k_v\Omega_m$$

$$C_m=(sI_m+F_m)\Omega_m+C_l,\qquad C_m=k_tI_a$$

$k_t=k_v$（标准国际单位下数值相等）（定义 5.2.1）。放大器 $\dfrac{V_a}{V_c}=\dfrac{G_v}{1+sT_v}$，$T_v$ 在 $10^{-5}$–$10^{-4}$ s 量级。

G4 两种发电机与统一传递函数

$$M(s)=\frac{k_m}{s(1+sT_m)}$$

速度控制抗干扰更好 ⇒ 独立关节控制不加电流反馈；保护改用死区非线性的电流反馈（定理 5.2.A、定理 5.2.B）。

G5 液压驱动的六条方程

$$Q=Q_m+Q_l+Q_c,\qquad Q_l=k_lp,\qquad Q_c=\gamma Vsp,\qquad Q_m=k_q\Omega_m,\qquad C_m=(sI_m+F_m)\Omega_m+C_l,\qquad C_m=k_pp$$

伺服阀 $\dfrac{x}{V_c}=\dfrac{G_c}{1+sT_c}$（毫秒级）；分布器线性化 $p=k_xx-k_qQ$（定义 5.2.3）。旋转式 $k_c=\gamma V$ 为常数，直线式随工作点变。

G6 减速比的两个折算因子（必背）

$$k_r=\frac{\omega_m}{\omega}=\frac{r}{r_m}\ (\text{典型 }几十\sim几百)$$

$$C_m=I_{eq}\dot\omega_m+F_{eq}\omega_m+\frac{C_l}{k_r},\qquad I_{eq}=I_m+\frac{I}{k_r^2},\qquad F_{eq}=F_m+\frac{F}{k_r^2}$$

惯量与摩擦按 $1/k_r^2$、负载力矩与非线性按 $1/k_r$（定理 5.2.C）。摆的例子：$C_m=I_{eq}\dot\omega_m+F_{eq}\omega_m+\dfrac{mgl}{k_r}\sin\dfrac{\vartheta_m}{k_r}$——$k_r$ 同时缩小幅值与自变量，故大减速比使非线性趋向线性化（例 5.1）。

G7 位置与速度反馈控制

位置环需要 积分（稳态消除干扰）+ 比例（稳定与阻尼）——纯比例给不出阻尼瞬态，原因是前向通路有两个原点极点（定理 5.2.D）。加测速反馈后：干扰点附近两个回路 ⇒ 瞬态抗扰更强、带宽更宽、振荡更少（方法 5.2.E）。推广到齿轮耦合：用 $I_{eq},F_{eq}$ 代替 $I_m,F_m$，干扰按 $1/k_r$（两条推广）。

G8 编码器分辨率

$n$ 轨绝对式：每圈 $2^n$ 个角位置，电机侧 $2\pi/2^n$；经减速比 $k_r$ 后关节侧分辨率提高 $k_r$ 倍（例 5.3.A）。12 轨 ⇒ $360^{\circ}/4096\approx0.0879^{\circ}$。格雷码：相邻码位只变一位，避免过渡错误的累积（定理 5.3.B）。

G9 力/力矩的坐标变换（作业 5.6 用）

$$\begin{bmatrix}f^c_c\ \mu^c_c\end{bmatrix}= \begin{bmatrix}R^c_s & O\ S(r^c_{cs})R^c_s & R^c_s\end{bmatrix} \begin{bmatrix}f_s\ \mu_s\end{bmatrix},\qquad S(r)=\begin{bmatrix}0&-r_z&r_y\ r_z&0&-r_x\ -r_y&r_x&0\end{bmatrix}$$