机器人 / 系统课时
将末端目标速度映射到关节速度
将末端目标速度映射到关节速度
给定末端希望沿 X、Y 方向移动的速度,机器人必须算出各关节应该转多快。本篇用平面机械臂建立雅可比速度方程,分别求解方阵、冗余与欠驱动情形,并通过回代误差判断目标能否实现,以及何时需要防范奇异位形。
学习目标
- 根据末端速度维度、关节数与雅可比秩选择求解方法。
- 用矩阵求解或 Moore-Penrose 伪逆计算关节角速度。
- 通过末端速度残差和奇异性检查验证计算结果。
先确认速度任务与坐标系
雅可比矩阵把关节速度映射为末端速度:
其中 是 矩阵: 为任务速度维度, 为关节数。若只控制平面末端位置,,单位为 m/s;旋转关节的 单位为 rad/s。若控制空间位姿速度,则可以使用包含三维线速度和三维角速度的六维速度向量,而不是直接把欧拉角变化率当作角速度。
必须保证目标速度与雅可比在同一坐标系中表达。例如,雅可比输出的是基坐标系下的 ,就不能直接代入工具坐标系下的目标分量。求出关节速度后还应检查 是否与目标一致。
满秩方阵直接求关节速度
设平面双旋转关节机械臂两段连杆均长 1 m,关节角为 、,末端位置为 m。此时控制末端 X、Y 两个速度分量,雅可比是满秩的 方阵:
这是 的结果。回代得 m/s,两个目标分量都满足。
只有 且 时才能使用普通逆。 程序中优先用 numpy.linalg.solve(J, v),不用先显式构造逆矩阵;当两连杆完全伸直,方阵仍然是 ,但秩降为 1,普通逆已经不存在。
用伪逆处理冗余与欠驱动
非方阵统一使用 Moore-Penrose 伪逆 得到最小二乘、最小范数解:
| 任务与关节数 | 满秩条件下的结果 | 目标速度 |
|---|---|---|
| 冗余:,例如 3R 控制二维位置 | 在可行解中选关节速度二范数最小的一组 | 可精确达到 |
| 欠驱动:,例如 1R 控制二维位置 | 找到速度残差二范数最小的关节速度 | 不一定可达到 |
对于三关节控制二维位置,伪逆默认选择“整体关节速度最小”的解,并不自动回避关节限位。需要额外姿态优化时,可加入零空间速度 ;只有它不会改变所要求的末端速度时,才可用于关节回中等次要目标。
欠驱动时必须查看残差。例如一根 1 m 连杆处于水平位置,其单个转动关节只能瞬时产生 Y 向速度;即使要求 m/s,伪逆也只能达到 m/s,X 向仍差 0.2 m/s。
用 Python 复现并检查回代
下面的独立示例使用 Python 3 和 NumPy。三个关节链都只控制末端的二维线速度,不要求控制末端朝向;关节角以弧度传入,连杆长度以米计。
import numpy as np
def planar_jacobian(lengths, angles):
absolute_angles = np.cumsum(angles)
jac = np.zeros((2, len(lengths)))
# 第 i 个关节的转动影响它以及后续所有连杆
for i in range(len(lengths)):
jac[0, i] = -sum(lengths[k] * np.sin(absolute_angles[k])
for k in range(i, len(lengths)))
jac[1, i] = sum(lengths[k] * np.cos(absolute_angles[k])
for k in range(i, len(lengths)))
return jac
def main():
target = np.array([0.2, 0.1])
cases = [
("方阵 2R", [1.0, 1.0], [0.0, np.pi / 2]),
("冗余 3R", [1.0, 1.0, 1.0], [0.0, np.pi / 2, -np.pi / 2]),
("欠驱动 1R", [1.0], [0.0]),
("奇异 2R", [1.0, 1.0], [0.0, 0.0]),
]
for name, lengths, angles in cases:
jac = planar_jacobian(lengths, angles)
rank = np.linalg.matrix_rank(jac)
# 满秩方阵直接求解,其余情形采用伪逆
qdot = (np.linalg.solve(jac, target)
if jac.shape[0] == jac.shape[1] == rank
else np.linalg.pinv(jac) @ target)
error = np.linalg.norm(jac @ qdot - target)
print(f"{name}: J={jac.shape}, rank={rank}")
print(f" dq={np.round(qdot, 4)} rad/s, error={error:.4f} m/s")
if __name__ == "__main__":
main()预期输出(浮点数按四位小数显示):
方阵 2R: J=(2, 2), rank=2
dq=[ 0.1 -0.3] rad/s, error=0.0000 m/s
冗余 3R: J=(2, 3), rank=2
dq=[ 0.0333 -0.2333 0.2667] rad/s, error=0.0000 m/s
欠驱动 1R: J=(2, 1), rank=1
dq=[0.1] rad/s, error=0.2000 m/s
奇异 2R: J=(2, 2), rank=1
dq=[0.04 0.02] rad/s, error=0.2000 m/s注意欠驱动与奇异两例都得到非零残差。pinv 返回了最接近目标的速度,却没有让不可达的 X 向速度凭空出现;不能只根据函数成功返回就判定运动任务执行成功。
接近奇异位形时限制速度
双连杆完全伸直时,。两个关节产生的瞬时末端速度方向都沿 Y 轴,所以 m/s 不可实现;但在接近伸直而尚未完全奇异时,直接求逆可能产生很大的关节角速度。

控制器中可改用阻尼最小二乘:
针对上述 1 m 连杆示例,可先试 (与雅可比奇异值采用一致的单位尺度),再按实际关节速度上限限幅,例如仿真中设置每关节不超过 1 rad/s。阻尼和限速都会牺牲一部分目标跟踪精度,必须在处理后重新计算 ;只有残差低于任务容差、关节速度未超限时,才把本周期速度指令视为可执行。