004｜Resolved Motion Rate：让操作者直接指定手的运动方向

系列首页 · 上一篇 · 下一篇

原论文：D. E. Whitney，Resolved Motion Rate Control of Manipulators and Human Prostheses，1969，IEEE Transactions on Man-Machine Systems 10(2):47–53。原始出版记录。

先接住前作的问题

D-H回答给定角度时手在哪里，却没有替操作者分配“手向右走”所需的关节速度。本篇把任务速度接到局部雅可比逆映射。

五个前置概念

五个前置概念是：雅可比描述局部敏感性；奇异值刻画可运动方向；伪逆提供最小范数解；零空间表达冗余；速度闭环与位置闭环分别针对不同误差。

一、问题设定：夹爪的速度意图怎样分配成各关节速度

操作者希望夹爪向右移动，但机械臂可能需要肩、肘和腕同时转动。逐关节发指令把协调负担交给人。Resolved-rate思想则接收任务空间的速度意图，再根据当前构型计算各关节速度，让人直接控制有意义的手部运动。

从位置关系到速度关系

若正向运动学写为 $x=f(q)$，在适当局部坐标下求导可得：

$$\dot x=J(q)\dot q.$$

J是雅可比矩阵。它的第i列表示仅让第i个关节运动时，对末端速度产生什么贡献。任务坐标中的线速度与角速度需要使用一致的表示；欧拉角导数并不总等于物理角速度。

这里没有环境奖励、回合终止或策略网络。任务停止时速度命令可以变为零，但零速度并不自动纠正已经累积的位置误差。

不用对整块齐次矩阵逐项求导，也能构造几何雅可比。把所有量写在同一基座系，设第 $i$ 个转动轴的单位方向为 $z_i$、轴上一点为 $o_i$、末端点为 $p$。单位角速度使末端产生 $z_i\times(p-o_i)$ 的线速度，同时产生 $z_i$ 的角速度，因此按“线速度在前、角速度在后”排列，

$$J_i=\begin{bmatrix}z_i\times(p-o_i)\\z_i\end{bmatrix} \quad\text{（转动）},\qquad J_i=\begin{bmatrix}z_i\\0\end{bmatrix} \quad\text{（移动）}.$$

这里轴索引是局部说明；若接入标准D-H，关节 $i$ 对应其 $z_{i-1}$，必须按约定转换。换一个末端工具点，也会改变叉积中的力臂，即使关节角完全没变。对空间旋量雅可比，线性分量还涉及选定原点，不能把它与“工具点的线速度”不加解释地混用；官方现代教材的空间雅可比讲解提供了另一套明确的旋量排列。

同一个 $J$ 还有对偶用途。由瞬时功率 $f^\top\dot x=\tau^\top\dot q$ 和 $\dot x=J\dot q$，得到 $\tau=J^\top f$。它是力映射，并不是速度逆映射；$J^\top$ 不会自动代替 $J^{-1}$。混合力位控制会把这两种接口组合使用，阻尼逆解则直接针对速度逆映射的奇异问题继续发展。

二、从正运动学求导，区分局部逆映射与全局逆解

逆映射及其实际含义

当J为方阵且可逆，$\dot q=J^{-1}\dot x_d$。这不是把非线性正向运动学全局求逆，而是在当前构型附近求一个线性的瞬时速度关系。每个控制周期都要重新计算，因为构型改变后J也会变化。

当关节数多于任务维数，可用现代教学形式的伪逆给出最小二范数解：

$$\dot q=J^\dagger\dot x_d+(I-J^\dagger J)z.$$

第一项完成主任务，第二项位于局部零空间，可用于其他运动。这里把它作为理解冗余的扩展公式，而非声称原文已经包含后来的所有任务优先级方法。零空间只保证当前线性化下不影响主任务，有限步积分和模型误差仍会改变末端行为。

限速会改变任务速度。例如逐关节简单裁剪后，J乘裁剪速度不再等于原命令。若任务要求方向尽量不变，可以缩放整组速度或求带约束的优化问题，但这些选择应作为工程扩展明示。

同一个末端速度在不同构型下需要不同关节组合。下面让两根连杆一正一反转，先验算横向贡献抵消，再检查控制循环是否随运动更新雅可比。

三、逐周期重算雅可比，用两连杆验证协调运动

输入：任务坐标下的期望速度v，当前关节q，运动学模型
每个控制周期：
    读取q并检查时间戳
    计算J(q)，确认v与J使用同一任务坐标
    求解 J*dq = v 的适当解
    检查关节速度与位置边界以及数值条件
    把可执行的dq发送给低层速度控制器
    下一周期使用新测量重新计算
输出：实际关节运动以及由模型估计的末端速度

教学例子：两连杆如何配合向上走

两根长度为1的平面连杆，位置为 $x=\cos q_1+\cos(q_1+q_2)$、$y=\sin q_1+\sin(q_1+q_2)$。因此：

$$J=\begin{bmatrix} -\sin q_1-\sin(q_1+q_2)&-\sin(q_1+q_2)\\ \cos q_1+\cos(q_1+q_2)&\cos(q_1+q_2) \end{bmatrix}.$$

在 $q_1=0,q_2=90^\circ$ 时，J为 $[[-1,-1],[1,0]]$。要求末端以 $(0,0.1)$ 米每秒运动，可以得到 $\dot q_1=0.1$、$\dot q_2=-0.1$ 弧度每秒。一个关节正转，另一个反转，横向贡献抵消而纵向贡献保留。

这是一阶瞬时关系。不能直接让两个速度保持一分钟并期待末端仍沿同一直线，因为运动中的雅可比发生了变化。控制循环中的反复更新是方法的组成部分，而不是可选的数值优化。

四、区分现代雅可比教学图与1969原文证据

读图：蓝、橙箭头是两个关节在当前构型产生的速度贡献，绿色是合成速度。水平方向抵消只在此刻的雅可比下成立，持续运动后必须重新计算。

1969 年原版实验全文仍不可核查。以下雅可比推导采用现代记号；阻尼逆解与零空间优化的后来发展，不能计为这篇论文已经完成的结果。

若设计复现实验，应保持同一机械臂与低层控制，比较关节命令接口和任务速度接口的路径误差、任务时间及用户协调负担。需要分别报告自由空间、接近奇异点和关节限位的表现。原论文的具体试次与误差统计在当前证据范围中尚未核实，不能补写为现代对比表。

五、追踪奇异放大、积分漂移与执行器限速

当机械臂完全伸直，某个方向的瞬时运动可能无法由有限关节速度实现。在线性代数上表现为J失去秩，或最小奇异值接近零。此时直接求逆可能产生极大关节命令。

阻尼最小二乘以任务误差换取速度有界，是后续研究对这一问题的处理。它不消除物理奇异性，而是避免控制器为了不可实现的命令产生数值灾难。真正需要跨越不可达区域时，还要重新规划姿态或路径。

另一类问题来自积分。速度跟踪存在小偏差时，位置误差会随时间积累。现代闭环逆运动学常在速度命令中加入位置误差反馈，但姿态误差应使用合适的旋转表示。把三个欧拉角逐项相减在跨越表示边界时可能产生错误大转动。

六、追问：若把控制周期加倍，同一瞬时雅可比解还能保证同样的末端路径吗？

七、从奇异速度指令走向任务力矩

• 直接继承：奇异鲁棒逆解：允许一点偏差，换取可执行的关节运动。在奇异附近以任务误差换取有限关节指令。
• 模块组合：操作空间：直接设计夹爪的动力学行为。将速度层接口扩展到需要惯性模型的任务力矩控制。

八、自测与参考答案

问题1：为什么任务速度保持不变，关节速度通常仍要变化？

参考答案：J取决于构型。机械臂移动后，同一个关节对末端的速度贡献改变，所以要重新分配关节速度。固定逆矩阵只在很小运动范围内近似成立。

问题2：指令速度归零，为什么末端未必到达准确目标？

参考答案：速度控制只要求停止继续移动，不必消除此前累积的位置偏差。需要位置反馈或其他校正机制才能持续纠正该偏差。

问题3：伪逆求解成功是否意味着速度可安全执行？

参考答案：不意味着。数值解可能超出关节限速、接近机械限位或导致中间连杆碰撞。局部末端速度关系不包含这些约束，需要在控制或规划层显式处理。