我们希望机械臂把夹爪沿直线移到物体旁边。这个要求很容易描述,真正执行时,电机需要分别驱动各个关节转动。某个关节转过相同的角度,机械臂姿态不同,夹爪移动的方向也可能不同。要生成关节指令,先得弄清每个关节的运动怎样影响夹爪,再判断指定的运动能否实现,以及哪些关节变化可以相互补偿。

这些运动关系通常用矩阵表示。于是,机械臂能朝哪些方向运动,就转化为矩阵的秩和零空间问题;当目标超出当前运动能力,或几组测量彼此有差异时,投影和最小二乘可以给出近似解。计算这些解,还会用到 QR 分解和奇异值分解(SVD)。

机器人运动会随时间连续变化,仿真器却只能逐步计算,因此还需要数值积分。本章从简单的矩阵、直线拟合和微分方程算例讲起,先解释每个工具在做什么,再把它们接回机器人的运动学、动力学和控制问题。

1. 秩与零空间

一条七自由度机械臂,在某些构型下可以保持工具的位置和姿态,同时调整肘部位置。这是因为完成末端任务以后,关节还有调整的余地。我们需要分清哪些关节变化会移动工具,哪些变化能够互相抵消;在线性映射中,秩与零空间正好描述这两部分。先从一般矩阵算起,第 7 节再将它用于关节速度与末端速度的关系。

1.1 零空间

矩阵 $A$ 的零空间由所有满足 $Ax=0$ 的向量 $x$ 组成,也就是经过映射后输出为零的输入。求解时可以使用消元法:即使 $A$ 不可逆,仍能通过行变换找到这些输入。方程右端已经是零,无须添加非零的增广列。

考虑矩阵

先用第一行消去第二、三行的第一项,再用第二行消去第三行:

行变换把同一组方程重新组合,操作可逆,方程的解也就保持不变。因此,消元前后的零空间相同。

找列空间的基时,需要多做一步。行变换可能改变列空间,所以应先通过消元找到主元的位置,再回到原矩阵选取对应的列。消元后的列方便分析约束,原矩阵的列才保留了原来的输出方向。

矩阵的秩等于主元的数量。这里第一列和第三列包含主元,所以 $\operatorname{rank}(A)=2$。第三行成为零,是因为原来的第三行可以由前两行线性组合得到。

1.2 自由变量

第一、三列是主元列,第二、四列是自由列。因此可以先选定 $x_2$ 和 $x_4$,再通过回代求另外两个变量。自由变量给出选择的余地,主元变量随之调整,整组数值仍要满足原来的方程。

令 $x_2=1,x_4=0$,由

得到一个特殊解

再令 $x_2=0,x_4=1$,得到

零空间是这两个特殊解的所有线性组合,即

一般地,若 $A$ 有 $n$ 列、秩为 $r$,自由变量就有 $n-r$ 个,零空间的维数也是 $n-r$。

继续消元,将主元归一为一,再消去主元上方的元素,可以得到简化行阶梯形矩阵。适当交换列后,其非零部分可写成 $[I\ F]$;对应的特殊解可以集中写成 $[-F^{\mathsf T}\ I]^{\mathsf T}$。矩阵中的两部分分别记录自由变量的选择,以及为满足约束而求出的主元变量。

将这个矩阵换成机械臂当前构型的 Jacobian,零空间中的关节速度就对应末端瞬时速度为零的运动。冗余机械臂可以利用这些方向调整肘部、远离关节限位或辅助避障。

关节运动后,构型和零空间也会改变。要在持续运动中保持工具位姿,需要不断更新映射,并通过反馈修正误差;这里求得的是当前时刻可用的方向。

2. 正交投影

如果目标向量超出了模型能够表示的范围,该怎样求解?这种情况既会出现在带噪声的测量中,也会出现在机械臂无法产生指定末端速度时。我们可以先找到模型范围内最接近目标的向量,再保留它与目标的差值。采用欧氏距离时,这个过程就是正交投影。

先看二维中的例子。假设模型只能表示水平向量,目标为 $b=(2,3)^{\mathsf T}$。最近的水平向量是 $p=(2,0)^{\mathsf T}$,两者相差 $e=(0,3)^{\mathsf T}$。对任意水平向量 $(a,0)^{\mathsf T}$,它与目标的距离平方是 $(2-a)^2+9$,在 $a=2$ 时最小。

水平分量可以匹配,长度为三的竖直分量则留下来作为残差。把水平轴换成由矩阵各列张成的子空间,同样可以寻找这个最近点。

设 $A$ 的列线性独立。矩阵

把向量 $b$ 投影到 $A$ 的列空间。若 $b$ 本来就在列空间中,存在 $x$ 使得 $b=Ax$,于是 $Pb=b$。若 $b$ 与列空间垂直,它就在 $A^{\mathsf T}$ 的零空间中,此时 $Pb=0$。

对于一般向量,投影会取出列空间中的分量 $p$,其余部分 $e$ 与列空间垂直:

其中 $I$ 是相应维数的单位矩阵。$P$ 取出列空间内的分量,$I-P$ 取出左零空间内的分量。两部分彼此正交,后者就是模型无法表示的残差。

最近点是否适合实际任务,还取决于模型和误差假设。投影只按指定子空间分解向量:落在列空间内的噪声会被保留,模型遗漏的真实信息也可能进入残差。因此,使用投影处理观测之前,需要先检查模型能解释哪些信息。

3. 最小二乘

机器人标定时,通常会收集多组观测,用它们确定少量未知参数。更多观测提供了更多约束,测量噪声也可能让这些方程无法同时成立。最小二乘通过最小化残差平方和,求出一组折中的参数。下面用三个点拟合直线,逐步计算观测多于未知量时怎样求解。

3.1 直线拟合

希望为点 $(1,1),(2,2),(3,2)$ 找到一条最接近的直线

这里 $C$ 是截距,$D$ 是斜率。我们用三个点的纵向残差平方和衡量拟合误差。这样,正负残差不会相互抵消,较大的残差也会对结果产生更大影响。对于没有明显离群值的数据,这种线性回归很有用;若有严重异常值,则需要结合数据处理或稳健估计方法。

三个数据点及接近它们的直线

图1. 三个数据点及接近它们的直线

若要求直线同时穿过全部三个点,需要满足

这组方程无法同时满足。把未知数写成 $x=[C,D]^{\mathsf T}$,便有

最小二乘寻找使 $\|Ax-b\|^2$ 最小的 $\hat x$。它满足正规方程

即 $3\hat C+6\hat D=5$、$6\hat C+14\hat D=11$。解得

3.2 残差与正交性

拟合值与残差分别为

在刚才的二维图中,$e_i$ 是第 $i$ 个数据点与直线之间的纵向差值,$p_i$ 是直线在该横坐标处的预测值。换一个视角,把三次观测排在一起,$b$ 就成了 $\mathbb R^3$ 中的向量;拟合值 $p$ 是它在 $A$ 的列空间中的投影,残差 $e$ 位于左零空间中。

这里需要留意两个图景中的空间不同。正交性发生在三个观测组成的向量空间里;二维拟合图中的纵向残差线段,通常并不垂直于直线。残差与列空间正交,表示沿模型能够产生的任意方向继续调整拟合值,都无法再减小残差平方和。

也可以对二元函数

求偏导并令它们为零。这样仍然会得到同一组正规方程。几何投影与微积分求极值在这里给出了同一个答案。

把残差代回两列观测向量,可以直接核算

前两式对应 $A^{\mathsf T}e=0$ 的两个分量,最后一式给出最小残差平方和。下图将三个数据点、拟合值和纵向残差画在同一坐标中,可以对照检查每个点的预测误差。

最小二乘算例:三个观测点、拟合直线与纵向残差

图2. 最小二乘算例:三个观测点、拟合直线与纵向残差

图中直线为 $b(t)=2/3+t/2$。可下载绘图与核算脚本、直线拟合数据和数值结果,复算每个拟合值与残差。

3.3 正规方程

刚才能够解出参数,是因为 $A^{\mathsf T}A$ 可逆。下面证明,当 $A$ 的列线性独立时,这个条件成立。

假设 $A^{\mathsf T}Ax=0$,左乘 $x^{\mathsf T}$,有

所以 $Ax=0$。由于列线性独立,这只可能在 $x=0$ 时发生。因此 $A^{\mathsf T}A$ 的零空间只有零向量,它便是可逆的。于是,即使原来的线性方程没有精确解,只要列独立,仍可以通过线性回归求出唯一的最小二乘解。

有唯一解以后,还要检查计算对误差有多敏感。两列可能几乎平行,此时很小的观测变化也可能导致参数大幅变化。列独立保证了可逆,数值条件则决定这个解在浮点计算中是否可靠。实际求解通常通过矩阵分解完成,下一节会介绍常用的 QR 方法。

3.4 观测精度不同,怎样分配误差

刚才的拟合对三个残差一视同仁。如果观测精度不同,就需要重新分配权重。仍用这三个点,假设前两个观测的标准差为 0.1,第三个为 0.5,误差彼此独立且均值为零。第三个观测的不确定性更大,拟合时应该允许它保留更大的原始残差。

将每个残差除以对应标准差,再最小化标准化残差的平方和,就得到加权最小二乘问题:

对这个二次型求导,得到 $A^{\mathsf T}WA\hat x=A^{\mathsf T}Wb$。本例中左右两部分为

解得 $C=2/15,D=0.9$,三个预测值约为 1.0333、1.9333、2.8333。第三个观测留下了更大的原始残差,前两个预测值则更贴近各自的观测。按给定的不确定性标准化后,总代价为 $10/3$。

这个代价与普通最小二乘的残差平方和采用不同尺度,数值大小无法直接比较。权重改变了我们对拟合结果的要求:现在优先贴近更精确的观测。

在机器人标定中,这些权重应来自传感误差模型或实验估计。混合米、像素和弧度等不同单位时,直接相加原始平方误差会使结果受单位选择影响,通常需要按协方差处理尺度。误差彼此相关时,应使用完整协方差的逆作为权重,让相关性也参与计算。

方差描述的是观测的随机波动。系统偏差或明显离群值还需要另外处理,仅调整方差无法修正这些问题。第 08 章会继续讨论状态估计中怎样传播和检验不确定性。

4. 正交化与 QR 分解

前面的拟合将参数与矩阵的各列联系起来。如果这些列彼此不垂直,投影到一个方向上的结果就会混入其他方向的贡献。换成一组互相垂直、长度为一的方向后,每个系数便能通过内积单独求出。正交化负责构造这些方向,QR 分解则用它们组织计算。它们帮助我们求解方程,机器人实际的动力学耦合仍由物理模型描述。

4.1 标准正交基

向量 $q_1,\ldots,q_n$ 如果互相垂直,称为正交;如果还各自具有单位长度,就称为标准正交。其内积满足

标准正交向量总是线性独立。将它们作为矩阵 $Q$ 的列,就有 $Q^{\mathsf T}Q=I$。如果 $Q$ 是方阵,它称为正交矩阵,且满足 $Q^{-1}=Q^{\mathsf T}$。

矩形矩阵也可以具有标准正交列。此时 $Q^{\mathsf T}Q=I$ 仍然成立,但矩阵形状决定了它没有通常意义上的方阵逆矩阵。使用这些关系时,需要同时看清矩阵维数。

使用标准正交列,可以简化刚才的投影关系:

如果 $Q$ 是方阵,它的列张成整个空间,投影就是单位矩阵。如果 $Q$ 是矩形,$QQ^{\mathsf T}$ 则取出它的列空间内的分量。使用这些标准正交列做最小二乘,正规方程简化为 $\hat x=Q^{\mathsf T}b$;第 $i$ 个系数直接由内积 $q_i^{\mathsf T}b$ 给出,各个系数可以分别计算。

4.2 Gram–Schmidt 正交化

怎样把原有的基变成标准正交基?Gram–Schmidt 过程逐个处理向量,并保持它们张成的空间不变。先看两个独立向量 $a,b$。第一个方向保留为 $a_\perp=a$,然后从 $b$ 中减去沿 $a_\perp$ 的分量:

左乘 $a_\perp^{\mathsf T}$,可以直接验证 $a_\perp^{\mathsf T}b_\perp=0$。然后分别除以长度,就得到 $q_1=a_\perp/\|a_\perp\|$ 和 $q_2=b_\perp/\|b_\perp\|$。

有第三个独立向量 $c$ 时,从它中同时减去沿前两个正交方向的分量:

对后续向量重复这一操作:先去掉它沿已有方向的投影,再将剩余部分归一化。得到的新方向彼此正交,与原来的基张成同一个子空间。

原始向量几乎相关时,减去投影后的剩余部分会很短,浮点误差就容易影响它的方向。反复运算还可能使计算出的方向失去正交性。实际程序因此常采用改进 Gram–Schmidt、重复正交化或 Householder 变换等方法,在实现中尽量保持所需的数值性质。

4.3 QR 分解

取 $a=(1,1,1)^{\mathsf T}$、$b=(1,0,2)^{\mathsf T}$。由于 $a^{\mathsf T}b=a^{\mathsf T}a=3$,减去投影后得到

因此

它的列空间与 $a,b$ 张成的平面相同。把原始向量放进 $A$ 的各列,正交化过程便写成 $A=QR$,其中 $R=Q^{\mathsf T}A$ 记录原始各列在新基中的系数。

后构造的基向量与先前的原始方向垂直,所以对角线下方的系数为零,系数矩阵是上三角的。与消元得到三角分解类似,正交化也能组织成分解:QR 分解分别保留正交方向和三角系数。

用 QR 求解最小二乘,可以先将观测投影到正交基上,再通过上三角系统的回代,依次求出未知参数。这样就能绕过显式形成正规方程的步骤。

在二范数下,正规方程矩阵的条件数是原矩阵条件数的平方,计算时可能放大舍入误差。QR 避开了这一额外放大;如果原问题已经接近秩亏,还需要进一步判断秩,以及观测能否确定所求参数。

继续算完这个例子。由给定的 $Q,A$,先求出三角系数矩阵:

取新观测 $d=(2.1,0.9,3.0)^{\mathsf T}$,有 $Q^{\mathsf T}d=(2\sqrt3,2.1/\sqrt2)^{\mathsf T}$。解 $R\hat x=Q^{\mathsf T}d$ 时,第二行先给出 $\hat x_2=1.05$,第一行再给出 $\hat x_1=0.95$。预测向量为 $(2.00,0.95,3.05)^{\mathsf T}$,残差为 $(0.10,-0.05,-0.05)^{\mathsf T}$,平方和为 0.015。将残差分别与 $a,b$ 作内积,结果都为零,正好核对前面的投影条件。

多参数标定也可以沿着这条路线求解:先将观测投影到模型能够产生的方向,再算出各个原始参数。这里的 0.95 和 1.05 对应 $A$ 的原始两列,$Q^{\mathsf T}d$ 则是新正交基下的系数,两组数值的含义不同。

处理加权问题时,可以先对 $A,d$ 同时乘以权重平方根,再进行 QR 分解。权重先进入模型和观测,后面的投影与回代过程便能继续使用。

5. 对称矩阵与正定性

机械臂以某组关节速度运动时,动能有多大,取决于质量分布。将动能写成速度的二次型,表达式中既有各个速度的平方,也可能有不同速度相乘的交叉项。惯量矩阵把这些系数组织起来。下面学习对称矩阵和正定性,为第 04 章分析惯量矩阵的物理性质作准备。

实对称矩阵满足 $A=A^{\mathsf T}$。它具有实特征值,并且可以选出一组完整的标准正交特征向量。若把这些特征向量放在正交矩阵 $Q$ 中,把特征值放在对角矩阵 $\Lambda$ 中,就有

这个结果称为谱定理。它用于刚体惯量时,给出主轴定理的数学表达:沿合适的正交方向表示惯量,矩阵就能写成对角形式,交叉项也随之消失。反过来,形如 $Q\Lambda Q^{\mathsf T}$ 的矩阵也是对称矩阵。

可以从特征方程看出实对称矩阵为什么只有实特征值。假设 $Ax=\lambda x$,暂且允许 $x$ 为复向量。用 $x^*$ 表示共轭转置,左乘后得到 $x^*Ax=\lambda x^*x$。

再对特征方程取共轭转置,利用 $A^*=A$,得到 $x^*A=\bar\lambda x^*$,因而 $x^*Ax=\bar\lambda x^*x$。非零特征向量满足 $x^*x=\sum_i|x_i|^2>0$,比较两式便有 $\lambda=\bar\lambda$。也就是说,特征值等于自己的共轭,因而是实数。这里使用共轭转置,使复向量的长度平方仍为正实数。

还可以把分解写为

$q_i$ 是单位特征向量,$q_iq_i^{\mathsf T}$ 是投影到这个方向的矩阵。因此,一个对称矩阵可以看成一组相互正交的方向投影,每个方向分别按对应的特征值缩放。

如果对称矩阵的所有特征值都为正,就称它为正定矩阵。可以通过消元检查正定性,不过这里要保持对称结构,并按原顺序、不交换地消元。检查的是 $LDL^{\mathsf T}$ 分解中的对角项,它们应全部为正;任意交换行后的普通消元主元不适用这一判断。以下面的矩阵为例:

它的主元为 $5$ 和 $11/5$,都为正。特征方程为

所以特征值是 $4+\sqrt5$ 与 $4-\sqrt5$,也都为正。

正定矩阵的行列式为正。若只检查整个行列式,则会遗漏其他情况,例如 $\operatorname{diag}(-1,-3)$ 的行列式是三,两个特征值却都是负的。对实对称矩阵,应检查所有顺序主子式是否均为正:它们是左上角 $k\times k$ 子矩阵的行列式,$k=1,\ldots,n$。

正定性保证二次型在每个非零方向上都为正,因此可以用来检查惯量模型,或构造能量型稳定性函数。至于能量怎样随时间变化,则要结合外力做功、驱动输入、耗散和运动方程分析。这是动力学中的能量变化问题,与二次型的正负性质需要分别判断。

将上面的矩阵作为某个二自由度转动系统在固定构型下的惯量矩阵,取速度 $\dot q=(1,-1)^{\mathsf T}$ rad/s,动能就为 $\tfrac12\dot q^{\mathsf T}A\dot q=2$ J。正交分解还能给出任意速度下的动能界限:下界为 $\tfrac12(4-\sqrt5)\|\dot q\|^2$,上界为 $\tfrac12(4+\sqrt5)\|\dot q\|^2$。在速度向量长度相同的条件下,沿最小特征值方向运动的动能最少,沿最大特征值方向最多。

这里两个速度分量都使用 rad/s。混合转动和移动坐标时,需要先处理单位和尺度,才能比较方向上的数值大小。第 04 章会从连杆质量和速度出发,构造实际机械臂的惯量矩阵。

6. 奇异值分解

机械臂在当前构型下,沿不同方向运动的能力可能相差很大。让关节速度在单位球上变化,经 Jacobian 映射得到的末端速度通常形成一个椭球:有些方向的输出大,有些方向的输出小。SVD 会找出这些输入方向、对应的输出方向,以及各自的缩放量。

通过这个几何图景,我们也能重新理解秩和零空间。若要进一步比较平移与转动分量,需要先统一单位和尺度,使数值大小具有可比较的含义。

6.1 输入与输出基

奇异值分解通常简称 SVD,写成

其中 $U,V$ 是正交矩阵,$\Sigma$ 是非负对角形式的矩阵;当 $A$ 不是方阵时,$\Sigma$ 也相应为矩形。SVD 不要求 $A$ 对称,也不要求它可逆。

对于非零奇异值,它找到成对的标准正交方向:输入方向 $v_i$ 位于行空间中,输出方向 $u_i$ 位于列空间中,而且

$\sigma_i$ 是这对方向之间的缩放量,称为奇异值。SVD 选择的输入方向彼此正交,对应的输出方向也彼此正交;任意选择一组行空间的正交基,通常无法同时满足这两个条件。零空间方向对应 $\Sigma$ 中的零项,输入沿这些方向变化时,输出为零。

当矩阵对称正定时,可以取 $U=V=Q$,奇异值就是正特征值,因此谱分解是 SVD 的一个特殊情况。一般矩阵却需要分别选择输入和输出方向。特征向量分解 $A=S\Lambda S^{-1}$ 中的 $S$ 也未必正交;SVD 则始终使用标准正交方向,并且适用于矩形矩阵。

将 $A=U\Sigma V^{\mathsf T}$ 左乘其转置,得到

由此可以分别求出三个部分:$V$ 来自 $A^{\mathsf T}A$ 的单位特征向量,$U$ 来自 $AA^{\mathsf T}$ 的单位特征向量,非零奇异值是相应特征值的正平方根。这条推导路线把 SVD 与前面的对称矩阵分解联系起来。

实现时通常调用专门的 SVD 算法。直接形成这两个对称矩阵,会平方非零奇异值之间的比例,使小奇异值更难准确辨别。上面的关系适合推导,也可以用来核对算法结果。

6.2 奇异值与秩

考虑

$A^{\mathsf T}A$ 的两个单位特征向量可以选择为

其特征值分别为 $32,18$,所以奇异值为 $4\sqrt2,3\sqrt2$。由 $Av_i=\sigma_i u_i$,得到 $u_1=[1,0]^{\mathsf T}$、$u_2=[0,-1]^{\mathsf T}$。于是

从 $A^{\mathsf T}A$ 与 $AA^{\mathsf T}$ 分别求出的特征向量,还需要按符号配对,使 $Av_i=\sigma_i u_i$ 成立。检查这个关系,就能确认输入与输出方向相互对应。

这一组数值对应 $\sigma_1\approx5.657$、$\sigma_2\approx4.243$。让单位输入向量沿圆周变化,所有输出形成椭圆;两个特定输入方向 $v_1,v_2$ 分别被送到长度为 $\sigma_1,\sigma_2$ 的输出主轴上。下图中的箭头就是这两对方向,左右坐标的刻度分别显示输入与输出的大小。

SVD 算例:输入单位圆、奇异向量与输出椭圆

图3. SVD 算例:输入单位圆、奇异向量与输出椭圆

这张图与上一张图由同一脚本生成,SVD 曲线数据保存了单位输入和对应输出。矩阵、方向和奇异值均可独立代回 $Av_i=\sigma_i u_i$ 核验。

若改为 $A=\left[\begin{smallmatrix}4&3\\8&6\end{smallmatrix}\right]$,其秩只有一。此时 $A^{\mathsf T}A$ 的特征值是 $125,0$,所以只有一个非零奇异值 $\sqrt{125}$。完整的 $V$ 同时提供行空间与零空间的基,完整的 $U$ 同时提供列空间与左零空间的基。这把秩、四个基本子空间与方向缩放连接在了一起。

在这个秩为一的例子中,行空间由 $(4,3)^{\mathsf T}$ 的倍数组成,列空间由 $(4,8)^{\mathsf T}$ 的倍数组成。取单位方向 $v_1=(0.8,0.6)^{\mathsf T}$ 和 $u_1=(1,2)^{\mathsf T}/\sqrt5$,再分别选择与它们垂直的单位向量作为零空间和左零空间的基,得到

第二个输入基方向的奇异值为零,所以经过映射后输出为零;第一个方向则以 $\sqrt{125}$ 的比例映射到列空间。这样,四个基本子空间都得到了表示。补齐零空间基时可以选择不同方向,非零方向仍须成对满足 $Av_i=\sigma_i u_i$。

当矩阵是 Jacobian,零奇异值对应当前构型下无法产生的瞬时末端运动方向。改变构型以后,运动能力也可能改变,所以判断整个工作空间的可达性,还需要分析其他构型。

接近奇异构型时,某些输出方向对应很小的奇异值。要沿这些方向维持给定末端速度,逆解可能要求很大的关节速度。可以使用阻尼伪逆、限制速度或调整轨迹来缓解,再根据任务要求取舍跟踪精度与可用的运动能力。

7. 全微分与 Jacobian

回到开头让夹爪沿直线移动的任务,现在可以建立所需的矩阵了。末端位置与关节角之间的关系包含三角函数,同样的关节变化在不同构型下产生不同效果。先对位置关系作全微分,就能求出当前位置附近的变化关系;将偏导数排成矩阵,得到的便是 Jacobian。

一个两关节平面机械臂的末端位置满足

$\ell_1,\ell_2$ 是连杆长度,$\theta_1,\theta_2$ 是关节角。若两个角度发生很小的变化,全微分为

令 $x=[x_e,y_e]^{\mathsf T}$、$q=[\theta_1,\theta_2]^{\mathsf T}$,便有 $dx=J(q)dq$。矩阵 $J$ 由各坐标对各关节变量的偏导数组成,称为 Jacobian:

矩阵的每一列描述一个关节的变化对末端坐标的影响。沿随时间变化的关节轨迹应用链式法则,得到 $\dot x=J(q)\dot q$。给定构型后,末端速度就是关节速度的线性组合,这也是前面各项矩阵工具能够用于运动计算的原因。

使用这两个关系时,要区分瞬时速度和有限位移。上面的瞬时速度关系来自精确的链式法则;用当前 Jacobian 估计一小段关节位移造成的末端位移,则是局部线性近似。

关节运动后,构型和 Jacobian 都会改变。沿直线跟踪时,通常需要反复计算和更新;秩、零空间、投影与 SVD 的结果,也都对应计算时的构型。

令两杆都长 1 m,在 $q=(0,\pi/2)^{\mathsf T}$ 时,末端位于 $(1,1)$ m,位置 Jacobian 化为 $J=\left[\begin{smallmatrix}-1&-1\\1&0\end{smallmatrix}\right]$。请求末端速度 $(0.1,0)^{\mathsf T}$ m/s,第二行先给出 $\dot q_1=0$,第一行再给出 $\dot q_2=-0.1$ rad/s。这个时刻只需转动第二个关节。

持续执行这组关节速度,末端会沿以肘部为圆心的圆弧移动。圆弧在起点的切线水平,后续的切线方向却会变化;要走完整段直线,就得随构型更新关节速度。

换到伸直构型 $q=(0,0)^{\mathsf T}$,有 $J=\left[\begin{smallmatrix}0&0\\2&1\end{smallmatrix}\right]$。第一行恒为零,表示这一时刻任意有限关节速度产生的水平末端速度都是零;第二行给出的零空间方向为 $(1,-2)^{\mathsf T}$。如果仍请求 $(0.1,0)^{\mathsf T}$,将它投影到可实现的速度空间,结果为零,残差就是原请求。

这描述了伸直构型的一阶运动能力。有限转角仍可能改变水平位置,例如先弯曲肘部再运动,只是初始水平变化属于更高阶效应。分析瞬时速度时用当前 Jacobian,分析一整段运动时则需要考虑构型怎样变化。

本节的 $J$ 表示二维末端位置速度与关节速度的关系。完整的空间 Jacobian 还需要包含角速度,并约定所用的坐标系和参考点。第 02、03 章会继续建立这些表示,让位置、姿态和六维速度的计算能够一致地衔接。

8. 数值积分

前面算出了关节速度与末端速度的关系。要预测机器人随后怎样运动,还需要通过动力学求出速度随力和时间的变化。动力学通常写成连续微分方程,仿真器和控制器则按离散时刻更新。数值积分从当前状态及其变化率出发,计算下一时刻的状态。选用什么方法、每步走多长,会影响计算量、误差和稳定性。

8.1 Euler 法

复杂的机器人模型往往没有便于使用的解析解。即使能够写出解析表达,逐步计算也更便于接入传感器、控制器和接触模型。先从一个简单的初值问题学习怎样走这一步:

目标是估计 $y(b)$。将从 $a$ 到 $b$ 的区间分成宽度为 $h$ 的小步,记 $x_k=a+kh$。在已知点 $(x_k,y_k)$ 处,函数给出的切线斜率为 $F(x_k,y_k)$。暂时沿这条切线前进,就得到下一步的估计:

这些小线段连接起来,就是 Euler 折线。初始值 $y_0$ 已知,后续的 $y_k$ 是对 $y(x_k)$ 的近似,近似程度会受步长影响。Euler 法使用起点斜率,相当于将这一小步内的变化率保持不变。真实曲线在步内弯曲得越明显,沿切线前进产生的偏差就越大。

考虑 $y'=y^2-x,y(0)=-1$。若只用一步 $h=1$,初始斜率是一,得到 $y(1)\approx0$,这是很差的近似。若用两步 $h=1/2$,第一步得到 $y_1=-1/2$;此时斜率为 $(-1/2)^2-1/2=-1/4$,第二步得到 $y_2=-5/8$。继续减小步长,$h=1/4$ 时约为 $-0.75$,$h=1/8$ 时约为 $-0.80$,逐渐接近真实值约 $-0.83$。

8.2 数值误差

数值计算中有两类误差。浮点数只能保留有限精度,每一步运算都可能产生舍入误差,并在后续步骤中传播。截断误差则来自更新方法本身:Euler 法用切线段近似真实曲线,方向场变化越快,曲线就越容易偏离这段直线。

减小步长可以改善截断误差,同时也需要更多步骤,增加浮点运算次数。评估结果时,需要分别考虑这两类误差及其传播。

局部截断误差衡量从准确起点走完一步后产生的偏差。全局误差衡量经过许多步以后,数值状态与真实解的差异,其中还包含先前误差在后续步骤中的传播。下面所说的方法阶数,描述满足相应条件时,固定区间上的全局误差怎样随步长减小。

对上面的例子,$y''=2yy'-1$,所以 $y''(0)=-3$。解曲线在起点向下弯曲,初始切线给出的估计偏高。在适当的光滑性条件下,Euler 方法在固定区间上的误差随小步长按 $O(h)$ 缩小,因此称为一阶方法。

缩小步长也会增加成本:在固定时长内,步长减半,步数通常约加倍,增长与步长的倒数成比例。另一个要求是稳定性。对于快慢时间尺度差异较大的刚性问题,显式方法可能需要很小的步长才能保持稳定,即使精度阶数较高,也要检查这一限制。

用一个自行衰减的系统检查稳定性。设 $\dot z=-100z$,时间以秒计,真实解每过 $h$ 秒乘以 $e^{-100h}$;显式 Euler 每步则乘以 $1-100h$。当 $h=0.03$ s 时,真实状态缩小为原来的约 0.0498 倍,离散状态却变成 −2 倍,反复更新就会振荡放大。要让离散幅值严格衰减,必须满足 $|1-100h|<1$,即 $0<h<0.02$ s。

这里连续系统稳定,数值结果却发散,原因在于积分步长。排查机器人仿真时,也需要检查离散更新是否稳定。提高控制增益或接触刚度以后,模型最快的时间尺度可能改变,步长便需要重新评估。

8.3 Heun 法

Euler 法只看起点斜率。如果再看一眼预测终点的斜率,就能把步内变化纳入估计。改进 Euler 方法先做一次 Euler 预测,再计算预测终点的斜率,最后用两个斜率的平均值前进:

它也称 Heun 或 RK2 方法。对刚才 $h=1/2$ 的第一步,两次斜率为 $1,-1/4$,平均为 $3/8$,因此比单独使用初始斜率更接近向下弯曲的解。它的全局误差为 $O(h^2)$,但每一步需要两次函数计算。

比较效率时,需要把每步的函数计算次数也算进去。RK4 每步计算四次斜率,在适用的光滑问题上具有四阶精度。自适应求解器会根据误差估计调整步长,在变化较快的区间走得更细;刚性问题则可能需要适合刚性系统的隐式方法。

若每次方向场计算成本相同,可以比较步长为 $h$ 的 Euler 与步长为 $2h$ 的 RK2。在同一区间内,两者调用函数的次数相同,前者误差约为 $C_1h$,后者约为 $C_2(2h)^2$。当 $h$ 足够小时,平方项比线性项减小得更快,因此高阶方法有机会在相同计算预算下获得更好的近似。

方法的阶数与方程的阶数是两个概念。这里求解的方程只包含一阶导数,但可以使用一阶、二阶或四阶数值方法。“一阶方法”描述误差随步长的缩放规律。

每步多计算一些信息,有时能用更少的步骤达到所需精度。将这些方法用于机器人仿真,还要检查快速动力学、接触变化和控制更新周期是否被正确处理。积分器的选择因此需要结合模型、误差容限和稳定性要求。

8.4 用相同计算预算核对方法

继续计算 $y'=y^2-x,y(0)=-1$,把到达 $x=1$ 的方向场调用预算固定为八次。Euler 可以走八步,步长 0.125,得到约 −0.797709;Heun 可以走四步,步长 0.25,得到约 −0.844469;经典 RK4 可以走两步,步长 0.5,得到约 −0.834280。

比较误差之前,先计算一个数值参考。用步长 $10^{-4}$ 的 RK4 得到约 −0.833383391,再将参考步长减半,结果变化小于 $10^{-11}$。这表明该数值参考在所检查的精度下已经收敛,后面用它计算误差;它仍是数值近似。

三种方法相对该参考的绝对误差依次约为 0.035675、0.011086 和 0.000897。在这个光滑初值问题中,同样八次方向场计算,RK4 得到了更小的误差。

还可以用缩小步长来检查方法阶数。分别使用 20、40、80 步,步长每减半,Euler 的误差比约为 2.03、2.02,Heun 约为 4.11、4.06,RK4 约为 16.53、16.27,与一阶、二阶和四阶的渐近规律相符。有限步长下,误差常数和高阶项仍在起作用,所以比例不必恰好等于 2、4、16。

机器人仿真会发生碰撞、摩擦状态切换和控制信号更新,应用光滑问题上的阶数结论时,需要确认区间内的相应假设是否成立。实际检查可以先固定模型和控制输入,逐次减半积分步长,观察轨迹、能量和接触载荷是否趋于一致,再比较不同积分器的成本。这样才能发现运动动画中不容易看出的时间离散误差。

投影、加权拟合、QR、惯量二次型、Jacobian 和积分比较,都可以用数学算例核验脚本复算。脚本仅使用 Python 标准库,保留未舍入的输出,并核对关键等式。结合前面的绘图脚本,可以一边观察几何关系,一边检查计算结果。

参考资料

Gilbert Strang / MIT OpenCourseWare. 18.06SC Linear Algebra, Fall 2011. Lecture summaries 7, 16, 17, 25, 29: nullspace, least squares, orthogonalization, positive definiteness, singular value decomposition. © Massachusetts Institute of Technology;中文节选重排、记号校订与例题补充;译编采用 CC BY-NC-SA 4.0。

H. Harry Asada. Introduction to Robotics: Chapter 5, Differential Motion. MIT OpenCourseWare, Fall 2005. Section 5.1, pp. 1–2.

Haynes Miller / MIT OpenCourseWare. 18.03 Differential Equations, Class 2: Numerical Methods, Spring 2010. Sections 2–4, pp. 1–5.

Lynch, Kevin M., and Frank C. Park. Modern Robotics, 5.1.1: Space Jacobian. 空间速度、关节速度与 Jacobian 列向量的解释;本文二维位置算例与新增核验脚本为独立教学计算。