机器人 / 系统课时

将末端目标速度映射到关节速度

将末端目标速度映射到关节速度

给定末端希望沿 X、Y 方向移动的速度,机器人必须算出各关节应该转多快。本篇用平面机械臂建立雅可比速度方程,分别求解方阵、冗余与欠驱动情形,并通过回代误差判断目标能否实现,以及何时需要防范奇异位形。

学习目标

  • 根据末端速度维度、关节数与雅可比秩选择求解方法。
  • 用矩阵求解或 Moore-Penrose 伪逆计算关节角速度。
  • 通过末端速度残差和奇异性检查验证计算结果。

先确认速度任务与坐标系

雅可比矩阵把关节速度映射为末端速度:

v=J(q)q˙\boldsymbol v=J(\boldsymbol q)\dot{\boldsymbol q}

其中 JJ 是 m×nm\times n 矩阵:mm 为任务速度维度,nn 为关节数。若只控制平面末端位置,v=[vx,vy]T\boldsymbol v=[v_x,v_y]^{\mathsf T},单位为 m/s;旋转关节的 q˙\dot{\boldsymbol q} 单位为 rad/s。若控制空间位姿速度,则可以使用包含三维线速度和三维角速度的六维速度向量,而不是直接把欧拉角变化率当作角速度。

必须保证目标速度与雅可比在同一坐标系中表达。例如,雅可比输出的是基坐标系下的 vx,vyv_x,v_y,就不能直接代入工具坐标系下的目标分量。求出关节速度后还应检查 Jq˙J\dot{\boldsymbol q} 是否与目标一致。

满秩方阵直接求关节速度

设平面双旋转关节机械臂两段连杆均长 1 m,关节角为 q1=0∘q_1=0^\circ、q2=90∘q_2=90^\circ,末端位置为 (1,1)(1,1) m。此时控制末端 X、Y 两个速度分量,雅可比是满秩的 2×22\times2 方阵:

J=[−1−110],vd=[0.20.1],q˙=[0.1−0.3]J=\begin{bmatrix}-1&-1\\1&0\end{bmatrix},\quad \boldsymbol v_d=\begin{bmatrix}0.2\\0.1\end{bmatrix},\quad \dot{\boldsymbol q}=\begin{bmatrix}0.1\\-0.3\end{bmatrix}

这是 q˙=J−1vd\dot{\boldsymbol q}=J^{-1}\boldsymbol v_d 的结果。回代得 Jq˙=[0.2,0.1]TJ\dot{\boldsymbol q}=[0.2,0.1]^{\mathsf T} m/s,两个目标分量都满足。

只有 m=nm=n 且 rank⁡(J)=n\operatorname{rank}(J)=n 时才能使用普通逆。 程序中优先用 numpy.linalg.solve(J, v),不用先显式构造逆矩阵;当两连杆完全伸直,方阵仍然是 2×22\times2,但秩降为 1,普通逆已经不存在。

用伪逆处理冗余与欠驱动

非方阵统一使用 Moore-Penrose 伪逆 J+J^+ 得到最小二乘、最小范数解:

q˙=J+vd,e=vd−Jq˙\dot{\boldsymbol q}=J^+\boldsymbol v_d,\qquad \boldsymbol e=\boldsymbol v_d-J\dot{\boldsymbol q}
任务与关节数 满秩条件下的结果 目标速度
冗余:n>mn>m,例如 3R 控制二维位置 在可行解中选关节速度二范数最小的一组 可精确达到
欠驱动:n<mn<m,例如 1R 控制二维位置 找到速度残差二范数最小的关节速度 不一定可达到

对于三关节控制二维位置,伪逆默认选择“整体关节速度最小”的解,并不自动回避关节限位。需要额外姿态优化时,可加入零空间速度 (I−J+J)z(I-J^+J)\boldsymbol z;只有它不会改变所要求的末端速度时,才可用于关节回中等次要目标。

欠驱动时必须查看残差。例如一根 1 m 连杆处于水平位置,其单个转动关节只能瞬时产生 Y 向速度;即使要求 (vx,vy)=(0.2,0.1)(v_x,v_y)=(0.2,0.1) m/s,伪逆也只能达到 (0,0.1)(0,0.1) m/s,X 向仍差 0.2 m/s。

用 Python 复现并检查回代

下面的独立示例使用 Python 3 和 NumPy。三个关节链都只控制末端的二维线速度,不要求控制末端朝向;关节角以弧度传入,连杆长度以米计。

python
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()

预期输出(浮点数按四位小数显示):

text
方阵 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 向速度凭空出现;不能只根据函数成功返回就判定运动任务执行成功。

接近奇异位形时限制速度

双连杆完全伸直时,J=[0021]J=\begin{bmatrix}0&0\\2&1\end{bmatrix}。两个关节产生的瞬时末端速度方向都沿 Y 轴,所以 vx=0.2v_x=0.2 m/s 不可实现;但在接近伸直而尚未完全奇异时,直接求逆可能产生很大的关节角速度。

双连杆弯曲与伸直位形的末端速度可达方向对比

控制器中可改用阻尼最小二乘:

q˙=JT(JJT+λ2I)−1vd\dot{\boldsymbol q}=J^{\mathsf T}(JJ^{\mathsf T}+\lambda^2I)^{-1}\boldsymbol v_d

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

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

将末端目标速度映射到关节速度

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通