本文最后更新于 103 天前，其中的信息可能已经有所发展或是发生改变。

机器人学

1.1 空间形位与齐次变换

机器人在空间的位姿通常有两种表达方式，一种是在笛卡尔空间表示，通过在世界坐标系和机器人末端上建立坐标系，使用笛卡尔坐标描述机器人的位姿；另一种是通过关节空间表示，使用机器人六个关节的角度来描述机器人的形位。

KUKA KR60-3机器人的姿态通常使用笛卡尔坐标进行控制，机器人基坐标系为XB-YB-ZB，在机器人刀尖上建立工具坐标系XT-YT-ZT，则末端工具在基坐标系中的位姿就可以用XT-YT-ZT相对于XB-YB-ZB的位姿描述。工具坐标系在基坐标系的位姿关系可以用一个的齐次变换矩阵T表示:

1.2机器人运动学建模

1.2.1 基于旋量法的运动学建模

KUKA KR60-3结构简图如图所示，XB-YB-ZB为基坐标系，XT-YT-ZT为末端工具坐标系，L1~L6为机器人的结构参数， $\boldsymbol{\xi }_1 - \boldsymbol{\xi }_6$ 分别为六个关节的旋量坐标，r1~r6 分别为关节轴线上任意一点，其中r4、r5、r6为同一点，即后三关节轴线交点。

在旋量理论中，刚体在笛卡尔空间中的运动速度由角速度 $\omega$ 和线速度 $v$ 组成，这个向量被称为运动旋量，把运动旋量写成螺旋轴S与绕该转轴速度的合成形式，即：

对于纯转动关节，机器人上一点的形位的变换可以由指数坐标 $\boldsymbol{\xi }\theta$ 表示，也可以将其写为4×4的齐次变换矩阵形式：

由机器人的结构参数可以得到机器人的旋量参数，如表所示。

$i$	$\omega_i$	$q_i$	$v_i$
1	[0,0,-1]	[0,0,0]	[0,0,0]
2	[0,1,0]	[350,0,815]	[-815,0,350]
3	[0,1,0]	[1200,0,815]	[-815,0,1200]
4	[-1,0,0]	[2020,0,960]	[0,-960,0]
5	[0,1,0]	[2020,0,960]	[-960,0,2020]
6	[-1,0,0]	[2020,0,960]	[0,-960,0]

在KUKA KR60-3工业机器人中，每个连杆通过关节的螺旋运动施加运动给后面的连杆。末端坐标系经由6个关节串联的运动链，做多次螺旋运动，从初始位姿M变换到当前位姿T。在确定基坐标系和机器人初始位姿后，在完成指定的关节运动后，可以通过正向运动学表达式表示机械臂的末端坐标系：

1.2.2 正运动学代码实现

#Normalizes a vector
def Normalize(V):
    return V / np.linalg.norm(V)

#Converts an so(3) representation to a 3-vector
def so3ToVec(so3mat):
    return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]])

#Converts a spatial velocity vector into a 4x4 matrix in se3
def VecTose3(V):    
    return np.r_[np.c_[VecToso3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]],
                 np.zeros((1, 4))]

#Converts a 3-vector of exponential coordinates for rotation into axis-angle form
def AxisAng3(expc3):
    return (Normalize(expc3), np.linalg.norm(expc3))

#Computes the matrix exponential of a matrix in so(3)
def MatrixExp3(so3mat):   
    omgtheta = so3ToVec(so3mat)
    if NearZero(np.linalg.norm(omgtheta)):
        return np.eye(3)
    else:
        theta = AxisAng3(omgtheta)[1]
        omgmat = so3mat / theta
        return np.eye(3) + np.sin(theta) * omgmat \
               + (1 - np.cos(theta)) * np.dot(omgmat, omgmat)

#Computes the matrix exponential of an se3 representation of exponential coordinates
def MatrixExp6(se3mat):
    se3mat = np.array(se3mat)
    omgtheta = so3ToVec(se3mat[0: 3, 0: 3])
    if NearZero(np.linalg.norm(omgtheta)):
        return np.r_[np.c_[np.eye(3), se3mat[0: 3, 3]], [[0, 0, 0, 1]]]
    else:
        theta = AxisAng3(omgtheta)[1]
        omgmat = se3mat[0: 3, 0: 3] / theta
        return np.r_[np.c_[MatrixExp3(se3mat[0: 3, 0: 3]),
                           np.dot(np.eye(3) * theta \
                                  + (1 - np.cos(theta)) * omgmat \
                                  + (theta - np.sin(theta)) \
                                    * np.dot(omgmat,omgmat),
                                  se3mat[0: 3, 3]) / theta],
                     [[0, 0, 0, 1]]]
#空间坐标系下位姿计算
def FKinBody(M, Blist, thetalist):
    T = np.array(M)
    for i in range(len(thetalist)):
        T = np.dot(T, MatrixExp6(VecTose3(np.array(Blist)[:, i] \
                                          * thetalist[i])))
    return T
#末端坐标系下位姿计算
def FKinSpace(M, Slist, thetalist):
    T = np.array(M)
    for i in range(len(thetalist) - 1, -1, -1):
        T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \
                                       * thetalist[i])), T)
    return T
#旋量坐标
Slist=np.array([[   0,    0,    0,   -1,    0,   -1],
                [   0,    1,    1,    0,    1,    0],
                [  -1,    0,    0,    0,    0,    0],
                [   0, -815, -815,    0, -960,    0],
                [   0,    0,    0, -960,    0, -960],
                [   0,  350, 1200,    0, 2020,    0]])
#法兰相对于基座标系姿态
M=np.array([[ 0,    0,    1,  2190],
            [ 0,    1,    0,     0],
            [-1,    0,    0,   960],
            [ 0,    0,    0,     1]])
#关节角
thetalist=np.array([0,-90/180*pi,90/180*pi,0,0,0])
#末端位姿计算
T = FKinSpace(M,Slist,thetalist)

1.3 基于旋量法的逆运动学求解

1.3.1 PK子问题

对于六自由度串联机器人而言，逆运动学可以描述成：已知机器人末端工具坐标系位姿，求解满足当前位姿的一组或多组关节角。由KR60-3机器人后三轴相交与一点的特性，利用Paden-Kahan子问题推导机器人关节逆解解析式。Paden-Kahan子问题是基于旋量理论的指数积公式求解各关节旋转角度的几何方法。基于旋量理论的运动学逆解可以分为三个Paden-Kahan子问题，分别是绕单轴旋转、绕两个轴旋转和旋转到给定距离，目前KR60-3的逆运动学求解中只用到了两类子问题，所以本文只介绍子问题1和子问题2，如图所示。

PK子问题1为一个点绕定轴旋转。描述为空间中一点p绕给定的轴线 $\boldsymbol{\xi }$ 旋转到q点位置，求解旋转角度 $\theta$ ，可用旋量法表示为

通过向量之间的几何关系，旋转角 $\theta$ 可以由以下公式计算：

，

PK子问题2为一个点绕两个相交轴连续旋转。描述为空间中一点p，绕两相交轴 $\boldsymbol{\xi }_1$ 和 $\boldsymbol{\xi }_2$ 进行旋转，先将p点绕旋转轴 $\boldsymbol{\xi }_2$ 旋转 $\theta_2$ 运动到s点，然后再绕旋转轴 $\boldsymbol{\xi }_1$ 旋转 $\theta_1$ 运动到q点， $\omega_1$ 和 $\omega_2$ 分别为轴线 $\boldsymbol{\xi }_1$ 和 $\boldsymbol{\xi }_2$ 的方向向量。设 $\boldsymbol{u}=\boldsymbol{p}-\boldsymbol{r}$ ， $\boldsymbol{v}=\boldsymbol{q}-\boldsymbol{r}$ ， $\boldsymbol{z}=\boldsymbol{s}-\boldsymbol{r}$ ，用公式表示为：

若两个旋转轴重合，问题就变成了PK子问题1，若两个旋转轴不重合，可通过几何关系求解出中间点z，将子问题2分解为两个子问题1进行求解。

1.3.2 KUKA KR60-3逆运动学求解

由螺旋理论可知，位于螺旋轴上的点 $\boldsymbol{q}$ 不会受到螺旋运动的影响，即 $e^{[\boldsymbol{\xi }]\theta}\boldsymbol{q}=\boldsymbol{q}$ 。由KR60-3机器人结构简图可知，4、5、6关节轴线相交于同一点 $\boldsymbol{q}_4$ ，在正运动学公式中右乘 $\boldsymbol{q}_4$ ，就可以消除正运动学公式中4、5、6关节的影响，将6个关节的逆运动学求解分为两部分进行。

上式表示腕部中心点 $\boldsymbol{q}_4$ 在前三关节中螺旋运动，指 $\boldsymbol{q}_4$ 经过前三个关节的运动到达 $\boldsymbol{p}_1$ 。由此可使用Paden-Kahan子问题1，对前3个关节逐一进行求解。基于几何法，使用PK子问题1即可求解出前三个关节的关节角，得到：

其中，上述公式中 $y_2$ 、 $x_2$ 、 $y_{3}$ 和 $x_3$ 的表达式如下：

在得到前三个关节的关节角后，将上一步求取的前三个关节带入公式(2-10)中，然后在原公式中取在轴6上但不在4、5轴上一点进行解耦，即可消除第六轴的影响，将后三轴转换为一个子问题2和一个子问题1进行求解，在公式两边同乘以在轴6上但不在4、5轴上一点 $\boldsymbol{p}_6=[L_4+L_5,0,0]$ 。

上式可以看成一个子问题2， $\boldsymbol{p}_6$ 绕后4、5关节连续旋转，经过中间点 $\boldsymbol{p}_5$ ，移动到 $\boldsymbol{q}_6$ 位置，4、5关节轴线交点为 $\boldsymbol{q}_4$ 。使用子问题2求解出中间点 $\boldsymbol{p}_5$ 即可将问题转换为两个子问题1，对关节5、6进行求解。在求出关节角 $\theta _4$ 、 $\theta _5$ 后，代入公式，并取关节6轴线外一点 $\boldsymbol{p}_7=[L_4+L_5+1,0,0]$ ，计算得到绕关节6旋转后的 $\boldsymbol{q}_7=\left[ q_{7x},q_{7y},q_{7z} \right]$ ，根据旋量理论将公式进行解耦，即可求出关节角 $\theta _6$ ，关节角 $\theta _4$ 、 $\theta _5$ 、 $\theta _6$ 表达式如下：

1.3.3 基于旋量法的逆运动学数值求解

如果逆运动学方程无解析解时，可采用迭代数值方法求解。即使存在解析解,数值算法也经常用于改善求解的精度。

许多迭代方法可用于非线性方程的求解，’而是将运动学逆解方程转换成现有数值方法可解的形式。为此，将多次使用求解非线性方程的基本方法:牛顿-拉夫森法。此外，也将引入其他一些优化方法，以应对精确解不存在的情况，并利用这些方法得到最接近真实值的解或者相反情况，存在无穷多个运动学逆解（例如，运动学冗余情况）。为此，需要找到相对某种指标的最优解。因此，下面首先讨论用于求解非线性方程的牛顿-拉夫森法，接下来讨论优化第一必要条件。

（1）牛顿-拉夫森法

对于给定的微分方程方程 $g$ ，数值求解方程 $g(\theta)=0$ ，假设 $\theta^0$ 为初值，利用泰勒级数在 $\theta^0$ 处展开，并截取到第一项，得：

若只保留到第—阶，令 $g(\theta)=0$ ，求解 $\theta$ 得到：

再将上式求得的值作为初值，代人上述方程中，重复求解，得到下述方程：

上述过程不断迭代，直到满足某个指标值。例如，给定预先设定好的阂值满足：

对于KUKA KR60-3六自由度串联机器人，假设我们用坐标向量 $x$ 及其正向运动学方程 $x = f(\theta)$ 表示末端坐标，自然会得到一个从 $6$ 个关节坐标到 $6$ 个末端坐标的非线性向量方程。令 $x_d$ 为预期的末端坐标，牛顿-拉夫森法中的 $g(\theta)$ 可以定义成 $g(\theta)=x_d - f(\theta)$ , 目标是找到关节坐标 $\theta_d$ 且保证：

已知初始估计值 $\theta^0$ 接近真实解 $\theta_d$ ，运动学可以写成泰勒展开的形式，即：

截取泰勒展开第一项，假设 $J(\theta^0)$ 可逆，使用下式求解 $\Delta\theta$ ，即：

若正向运动学是 $\theta$ 的线性函数，这时新的估计值 $\theta^0 = theta^0 + \Delta\theta$ 精确满足 $x_d=f(\theta^1)$ 。相反，若正向运动学是 $\theta$ 的非线性函数，就如通常情况,这时新的估计值 $\theta^1$ 比 $\theta^0$ 更接近真实值，迭代过程不断重复，最终在 $\theta_d$ 处收敛。

（2）逆运动学的数值算法

实际上，由于计算效率等原因，经常不采用求逆 $J^{-1}(\theta^0)$ 的方式来求解。可以找到更高效的方法求解线性方程 $Ax＝b$ 中的 $x$ 。例如，对于可逆阵 $A$ ，基于 $A$ 的 $LU$ 分解可以用更少的运算得到 $x$ 。或者将 $J^{-1}$ 替换成Moore-Penrose伪逆形式 $J^+$ 。

修改上述算法，使之能应用在预期末端位形为 $T_{sd}\in SE(3)$ 而不是坐标向量 $x_d$ 的情况。可将关节坐标雅可比 $J$ 替换成末端物体雅可比 $J_b$ 。注意，由于向量 $e=x_d -f(\theta^i)$ 表示的是从当前估计值（由正向运动学得到）到预期末端位形。不能简单地替换成 $T_{sd}-T_{sd}(\theta^i)$ ； $J_s$ 的伪逆应该作为物体速度旋量 $V_b \in \R^6$ 。为找到一个合理的对比，我们可以想象 $e=x_d -f(\theta^i)$ 为一个速度向量，如果遵循单位时间，可使运动从 $f(\theta^i)$ 到 $x_d$ 。类似的，我们应该能够找到—个速度旋量，如果遵循单位时间，可使运动从 $T_{sd}(\theta^i))$ 到预期行为 $T_{sd}$ 。

为找到这个 $V_b$ ，首先需计算相对物体坐标系的预期位形，即

然后,利用矩阵对数确定 $[V]$ 即:

由此可以给出逆运动学算法：

（a）初始化：已知 $T_{sd}$ ，初始估计值 $\theta_0$ ，设定 $i=0$ ;

（b）计算 $[V]$ ，当 $||\omega|| > \varepsilon_{\omega}$ 或者 $||V|| > \varepsilon_{v}$ 时：

计算 $\theta^{i+1} = \theta^{i} + J^+(\theta ^ i)V$
$i = i+1$

完成一定次数的迭代后，就都能获得真实值 $\theta_d$ 的近似解 $\theta^k$ 。

（3）数值解代码实现

def IKinBody(Blist, M, T, thetalist0, eomg, ev):
    #Computes inverse kinematics in the body frame for an open chain robot

    thetalist = np.array(thetalist0).copy()
    i = 0
    maxiterations = 20
    Vb = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \
                                                      thetalist)), T)))
    err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
          or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
    while err and i < maxiterations:
        thetalist = thetalist \
                    + np.dot(np.linalg.pinv(JacobianBody(Blist, \
                                                         thetalist)), Vb)
        i = i + 1
        Vb \
        = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \
                                                       thetalist)), T)))
        err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
              or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
    return (thetalist, not err)

def IKinSpace(Slist, M, T, thetalist0, eomg, ev):
    #Computes inverse kinematics in the space frame for an open chain robot
    thetalist = np.array(thetalist0).copy()
    i = 0
    maxiterations = 20
    Tsb = FKinSpace(M,Slist, thetalist)
    Vs = np.dot(Adjoint(Tsb), \
                se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T))))
    err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \
          or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev
    while err and i < maxiterations:
        thetalist = thetalist \
                    + np.dot(np.linalg.pinv(JacobianSpace(Slist, \
                                                          thetalist)), Vs)
        i = i + 1
        Tsb = FKinSpace(M, Slist, thetalist)
        Vs = np.dot(Adjoint(Tsb), \
                    se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T))))
        err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \
              or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev
    return (thetalist, not err)

eomg = 0.01
ev = 0.01
initial = np.array([40.050000, -50.910000, 110.140000, 120.540000, -11.390000, 97.930000])/180*math.pi
T = np.array([     [0.410150,    -0.123729,    -0.903586,  1183.631143] ,
                [-0.190306,    -0.980556,     0.047886, -1137.765860] ,
                 [-0.891942,     0.152317,    -0.425722,   756.413634 ],
                [ 0.000000,     0.000000,     0.000000,     1.000000] ])
[theta,success] = mr.IKinSpace(Slist,M,T,initial,eomg,ev)

1.4 机器人运动学

1.4.1 雅可比矩阵

除了关节角度和机器人末端执行器位置之间的关系外，还需要研究关节和末端执行器速度之间的关系。在本节中，推导了这种关系的公式，并研究其结构和性质，还研究了连杆之间力传递的关系。由第2章的公式(2-5)，当给定一组关节角 $\boldsymbol{\theta }=\left[ \theta _1,\theta _2,\theta _3,\theta _4,\theta _5,\theta _6 \right] ^{\mathrm{T}}$ 时，可以确定机器人末端的位置与姿态：

其中， $\boldsymbol{X}=\left[ X,Y,Z,A,B,C \right] ^{\mathrm{T}}$ ，将公式两边进行求导，可以得到：

式中， $\boldsymbol{J}\left( \boldsymbol{\theta } \right)$ 为雅可比矩阵。

一般情况下，雅可比矩阵通过矢量差乘积法、微分变换法得到。而使用指数积公式表示正运动学，可以更加明确、优雅的推导出雅可比矩阵。在机器人正运动学的指数积公式下，末端的速度 $\left[ \boldsymbol{V} \right]$ 的表达式为：

带入机器人正运动学公式，并使用伴随映射将上式写成向量形式，即：

式中，对于任意 $\boldsymbol{\xi }\in \boldsymbol{R}^6$ ，与 $\boldsymbol{T}=\left( \boldsymbol{R},\boldsymbol{P} \right) \in \boldsymbol{SE}\left( 3 \right)$ 相关联的伴随映射为:

1.4.2 雅可比矩阵代码实现

def JacobianBody(Blist, thetalist):
    #Computes the body Jacobian for an open chain robot
    Jb = np.array(Blist).copy().astype(np.float)
    T = np.eye(4)
    for i in range(len(thetalist) - 2, -1, -1):
        T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i + 1] \
                                         * -thetalist[i + 1])))
        Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i])
    return Jb

def JacobianSpace(Slist, thetalist):
   #Computes the space Jacobian for an open chain robot
    Js = np.array(Slist).copy().astype(np.float)
    T = np.eye(4)
    for i in range(1, len(thetalist)):
        T = np.dot(T, MatrixExp6(VecTose3(np.array(Slist)[:, i - 1] \
                                * thetalist[i - 1])))
        Js[:, i] = np.dot(Adjoint(T), np.array(Slist)[:, i])
    return Js

1.5 涉及的函数说明

1.5.1 空间变换与齐次矩阵

1.计算旋转矩阵的逆
invR = RotInv(R)

2.将三维向量转换为3*3反对称矩阵
so3mat = VecToso3(omg)

3.将3*3反对称矩阵转换为三维向量
omg = so3ToVec(so3mat)

4.从旋转的三维指数坐标中提取出轴线和角度
[omghat,theta] = AxisAng3(expc3)

5.计算矩阵指数对应的旋转矩阵
R = MatrixExp3(so3mat)

6.计算旋转矩阵对应的矩阵对数
so3mat = MatrixLog3(R)

7.M到SO（3）旋转矩阵的空间距离的度量
d = DistanceToSO3(mat)

8.判断M是否为旋转矩阵
judge = TestIfSO3(mat)

9.返回与M最近的旋转矩阵
R = ProjectToSO3(mat)

10.将旋转矩阵和位置向量转换为变换矩阵
T = RpToTrans(R,p)

11.从变换矩阵中分离出旋转矩阵和位置向量
[R,p] = TransToRp(T)

12.变换矩阵求逆
invT = TransInv(T)

13.构造与六维向量形式的运动旋量V对应的
se3mat = VecTose3(V)

14.构造与se(3)矩阵对应的六维向量形式的运动旋量V
V = se3ToVec(se3mat)

15.计算变换矩阵T的6*6的伴随矩阵
AdT = Adjoint(T)

16.返回正则化的螺旋轴表达式
S = ScrewToAxis(q,s,h)

17.从六位向量形式的指数坐标中提取出螺旋轴和沿轴线移动的距离
[S,theta] = AxisAng6(expc6)

18.计算矩阵指数对应的变换矩阵
T = MatrixExp6(se3mat)

19.计算变换矩阵对应的矩阵对数
se3mat = MatrixLog6(T)

20.M到变换矩阵的空间距离
d = DistanceToSE3(mat)

21.判断是否为SE3变换矩阵
judge = TestIfSE3(mat)

22.离M最近的变换矩阵
T = ProjectToSE3(mat)

1.5.2 运动学

#正运动学
末端坐标系下。给定初始形位，关节旋量以及关节值，计算末端坐标系
T = FKinBody(M,Blist,thetalist)
空间坐标系下。给定初始形位，关节旋量以及关节值，计算末端坐标系
T = FKinSpace(M,Slist,thetalist)

#雅可比矩阵
给定物体坐标系下描述的各关节旋量Blist及关节角，计算物体雅可比JB
Jb=JacobianBody(Blist,thetalist)
给定物体坐标系下描述的各关节旋量Slist及关节角，计算物体雅可比Js
J=JacobianSpace(Slist,thetalist)

#逆运动学
［theta11st,success］＝IKinBody（B1ist,M,T,theta1ist,eomg,ev）
［theta11st,success］＝IKinSpace（B1ist,M,T,theta1ist,eomg,ev）

1.6 参考链接

机器人POE运动学代码网址：NxRLab/ModernRobotics: Modern Robotics: Mechanics, Planning, and Control Code Library --- The primary purpose of the provided software is to be easy to read and educational, reinforcing the concepts in the book. The code is optimized neither for efficiency nor robustness. (github.com)

2024 年 9 月
一	二	三	四	五	六	日
						1
2	3	4	5	6	7	8
9	10	11	12	13	14	15
16	17	18	19	20	21	22
23	24	25	26	27	28	29
30