工业6轴机械臂运动学逆解（解析解）

工业6轴机械臂运动学逆解（解析解）

  通常工业机械臂采用6旋转轴串连的形式，保证了灵活性，但为其运动学逆解（即已知机械臂末端的位姿$P$，求机械臂各个旋转轴的旋转角）带来了较大的困难，通常没有解析解。为了提高实时性，经过前辈们不懈的研究，当6轴机械臂满足pieper准则时，可以得出其运动学逆解的解析解。pieper准则如下：

1. 三个相邻关节轴线交于一点，如fanuc m10系列；
2. 三个相邻关节轴线相互平行，如ur5系列；
   以下将通过一个简单实例，介绍机械臂在pieper第一准则的情况的运动学逆解。

机械臂运动学模型

  机械臂的简化模型如图1所示。

图1

  采用DH矩阵的形式对机械臂进行建模，DH矩阵如下：

关节	连杆夹角	连杆长度	连杆偏距	初始关节角
1	0	0	0.1	0
2	0.5$\pi$	0	0	0
3	0	0.5	0	0.5$\pi$
4	0.5$\pi$	0	0.5	0
5	-0.5$\pi$	0	0	0
6	0.5$\pi$	0	0	0

  从图1中可以看出，在初始状态下，机械臂的第2、3关节轴与基座坐标系的Y轴相互平行，机械臂的第4、5、6关节轴线相交于一点$P$，满足pieper第一准则，且点$P$与基座坐标系的XOZ平面重合。在这些条件下，虽然限制了机械臂的设计和构型，但极大地简化了逆解的过程。

运动学逆解

  由于机械臂的结构比较简单，故采用几何法的方式求解机械臂的运动学逆解。
  机械臂末端初始位姿$P_0$，运动学逆解末端的位姿$P(x,y,z)$

求解第1关节轴的关节角

以基坐标系为参考，机械臂末端位置示意图如图2所示。

图2

  在图1中，点$P_0$为机械臂末端的初始状态位置（当前位置），点$P$为机械臂末端的期望位置，由于初始状态时，机械臂末端的初始位置与$X_0{Y}_0$平面重合，故关节轴1需要旋转${\theta }_1$或${\theta }_1+\pi$才能使期望位置$P$与$X_0{Y}_0$平面重合，故，关节轴1存在两个解

$${\theta }_1=\{\begin{array}{l}atan2(y,x)\\ atan2(y,x)+\pi \end{array}\text{\hspace{1em}(1)}$$

求解第2、3关节轴的关节角

  关节轴1经过旋转${\theta }_1=atan2(y,x)$角度，以第一关节轴的坐标系为参考，得示意图如图3所示。$P_1$为机械臂末端的当前位置，$P$为末端期望位置，$O{A}_0$、$O{A}_1$为机械臂的第2连杆，$A_{0}P$、$A_{1}P$为机械臂的第3连杆。

图3

  设$A_0(x_0,0,z_0)$，可得方程：

$$\begin{gathered} x_0^2+z_0^2=DH[2,1{]}^2 \\ (x-x_0{)}^2+(z-DH[0,2]-z_0{)}^2=DH[3,2{]}^2\text{\hspace{1em}(2)} \end{gathered}$$

  对2元2次方程组（2）进行求解，得$(x_{00},z_{00})$、$(x_{01},z_{01})$、$(x_{02},z_{02})$、$(x_{03},z_{03})$四组解，去除其中的非实数解。由此可得到关节轴2、3的旋转角。

$$\begin{gathered} {\theta }_2=atan2(z_0,x_0) \\ {\theta }_3=atan2(z-DH[0,2]-z_0,x-x_0)-atan2(z_0,x_0)\text{\hspace{1em}(2)} \end{gathered}$$
  如图2所示，解方程（2），可得到关节角${\theta }_2$、${\theta }_3$的两组解。
  同理，当取${\theta }_1=atan2(y,x)+\pi$时，亦可得到关节角${\theta }_2$、${\theta }_3$的两组解。

  至此，${\theta }_1$、${\theta }_2$、${\theta }_3$存在8组解，可去除其中相同的解。

求解第4、5、6关节轴的关节角

经过对机械臂前3根轴的旋转，已经机械臂的末端位置与期望的末端位置相重合，由于关节轴4、5、6相交与末端位置，对此3轴的旋转不会改变末端的位置，故，单独对此3轴进行姿态解算即可得到关节角。
设$R_1({\theta }_1)$、$R_2({\theta }_2)$、$R_3({\theta }_3)$、$R_4({\theta }_4)$、$R_5({\theta }_5)$、$R_6({\theta }_6)$表示各轴的变换矩阵。将对关节轴4、5、6的旋转看成是动欧拉角ZYZ的旋转模式，其旋转矩阵为$R({\theta }_4,{\theta }_5,{\theta }_6)$。

$$R({\theta }_4,{\theta }_5,{\theta }_6)=\left[\begin{array}{ccc}cos{\theta }_4& -sin{\theta }_4& 0\\ sin{\theta }_4& cos{\theta }_4& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}cos{\theta }_5& 0& sin{\theta }_5\\ 0& 1& 0\\ -sin{\theta }_5& 0& cos{\theta }_5\end{array}\right]\left[\begin{array}{ccc}cos{\theta }_6& -sin{\theta }_6& 0\\ sin{\theta }_6& cos{\theta }_6& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(3)}$$
具体计算得：
$$R({\theta }_4,{\theta }_5,{\theta }_6)=\left[\begin{array}{ccc}cos{\theta }_{4}cos{\theta }_{5}cos{\theta }_6-sin{\theta }_{4}sin{\theta }_6& -cos{\theta }_{4}cos{\theta }_{5}sin{\theta }_6-sin{\theta }_{4}cos& cos{\theta }_6{\theta }_{4}sin{\theta }_5\\ sin{\theta }_{4}cos{\theta }_{5}cos{\theta }_6+cos{\theta }_{4}sin{\theta }_6& -sin{\theta }_{4}cos{\theta }_{5}sin{\theta }_6+cos{\theta }_{4}cos{\theta }_6& sin{\theta }_{4}sin{\theta }_5\\ -sin{\theta }_{5}cos{\theta }_6& sin{\theta }_{5}sin{\theta }_6& cos{\theta }_5\end{array}\right]\text{\hspace{1em}(4)}$$

  由机械臂的正运动学可得：
$$R({\theta }_1)R({\theta }_2)R({\theta }_3)R({\theta }_4=0)R({\theta }_4,{\theta }_5,{\theta }_6)=R_P\text{\hspace{1em}(5)}$$

  在公式（5）中，$R_P$为机械臂末端点$P$的姿态。对公式（5）进行移项得：

$$R({\theta }_4,{\theta }_5,{\theta }_6)=[R({\theta }_1)R({\theta }_2)R({\theta }_3)R({\theta }_4=0){]}^{-1}{R}_P=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\text{\hspace{1em}(6)}$$

  联立公式（4）（6）可得两组解：

$$\{\begin{array}{l}{\theta }_4=atan2(r_{23},r_{13})\\ {\theta }_5=atan2(sqrt(r_{31}^2+r_{32}^2),r_{33})\\ {\theta }_6=atan2(r_{32},-r_{31})\end{array}\text{\hspace{1em}(7)}$$

$$\{\begin{array}{l}{\theta }_4=atan2(r_{23},r_{13})+\pi \\ {\theta }_5=-atan2(sqrt(r_{31}^2+r_{32}^2),r_{33})\\ {\theta }_6=atan2(r_{32},-r_{31})+\pi \end{array}\text{\hspace{1em}(8)}$$

  综上，完成机械臂的运动学逆解的解析解求解过程，可能存在8个以上的解，可根据一些约束调节对求得的解进行删选，如关节限位、碰撞检测等。

示例程序

import numpy as np
import math
from pyquaternion import Quaternion

np.set_printoptions(suppress=True)

# DH矩阵每列的含义：连杆夹角、连杆长度、连杆偏距、初始关节角
DH = np.mat([[             0,   0, 0.1,             0], 
             [ 0.5 * math.pi,   0,   0,             0], 
             [             0, 0.5,   0, 0.5 * math.pi], 
             [ 0.5 * math.pi,   0, 0.5,             0], 
             [-0.5 * math.pi,   0,   0,             0], 
             [ 0.5 * math.pi,   0,   0,             0]])

def transformToMatrix(alpha, a, d, theta):
    T0 = np.eye(4)
    T1 = np.mat([[1,               0,                0, 0], 
                 [0, math.cos(alpha), -math.sin(alpha), 0], 
                 [0, math.sin(alpha),  math.cos(alpha), 0], 
                 [0,               0,                0, 1]])
    T2 = np.mat([[1, 0, 0, a], 
                 [0, 1, 0, 0], 
                 [0, 0, 1, d], 
                 [0, 0, 0, 1]])
    T3 = np.mat([[math.cos(theta), -math.sin(theta), 0, 0], 
                 [math.sin(theta),  math.cos(theta), 0, 0], 
                 [              0,                0, 1, 0], 
                 [              0,                0, 0, 1]])
    return T1 * T2 * T3

def forwardKinematic(DH, j0, j1, j2, j3, j4, j5):
    T0 = transformToMatrix(DH[0, 0], DH[0, 1], DH[0, 2], DH[0, 3] + j0)
    T1 = transformToMatrix(DH[1, 0], DH[1, 1], DH[1, 2], DH[1, 3] + j1)
    T2 = transformToMatrix(DH[2, 0], DH[2, 1], DH[2, 2], DH[2, 3] + j2)
    T3 = transformToMatrix(DH[3, 0], DH[3, 1], DH[3, 2], DH[3, 3] + j3)
    T4 = transformToMatrix(DH[4, 0], DH[4, 1], DH[4, 2], DH[4, 3] + j4)
    T5 = transformToMatrix(DH[5, 0], DH[5, 1], DH[5, 2], DH[5, 3] + j5)
    #print(T0 * T1 * T2 * T3 * T4 * T5)
    return T0 * T1 * T2 * T3 * T4 * T5

def calcu3ForwardJointAngle(DH, j0, x0, y0, x, y, b, js):
    if abs((x0 - x) * (x0 - x) + (y0 - y) * (y0 - y) - b*b) < 0.0001:
        js.append([j0])
        js[-1].append(math.atan2(y0, x0))
        js[-1].append(math.atan2(y - y0, x - x0) - math.atan2(y0, x0) + 0.5 * math.pi - DH[2, 3])

        js.append([j0 + math.pi])
        js[-1].append(math.pi - math.atan2(y0, x0))
        js[-1].append(math.atan2(y0, x0) - math.atan2(y - y0, x - x0) + 0.5 * math.pi - DH[2, 3])
    if abs((x0 - x) * (x0 - x) + (-y0 - y) * (-y0 - y) - b*b) < 0.0001:
        js.append([j0])
        js[-1].append(math.atan2(-y0, x0))
        js[-1].append(math.atan2(y + y0, x - x0) - math.atan2(-y0, x0) + 0.5 * math.pi - DH[2, 3])

        js.append([j0 + math.pi])
        js[-1].append(math.pi - math.atan2(-y0, x0))
        js[-1].append(math.atan2(-y0, x0) - math.atan2(y + y0, x - x0) + 0.5 * math.pi - DH[2, 3])

def quaternionToRotationMatrix(x, y, z, w):
    # a = math.sqrt(x*x + y*y + z*z)
    # if a == 0:
    #     return np.eye(3)
    # v1x = 0
    # v1y = -z
    # v1z = y
    # b = math.sqrt(v1x*v1x + v1y*v1y + v1z*v1z)
    # if b == 0:
    #     v1y = 1.0
    #     v1z = 0.0
    #     b = 1.0
    # v2 = np.cross(np.array([x, y, z]), np.array([v1x, v1y, v1z]))
    # #print(np.array([x, y, z]), np.array([v1x, v1y, v1z]), v2)
    # c = math.sqrt(v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2])
    # R01 = np.mat([[x / a, v1x / b, v2[0] / c],
    #               [y / a, v1y / b, v2[1] / c],
    #               [z / a, v1z / b, v2[2] / c]])
    
    # theta = 2 * math.acos(w)
    # #print(R01)
    # R12 = np.mat([[1,               0,                0],
    #               [0, math.cos(theta), -math.sin(theta)],
    #               [0, math.sin(theta),  math.cos(theta)]])
    # return R01 * R12 * np.linalg.inv(R01)

    a = math.sqrt(x*x + y*y + z*z + w*w)
    if a == 0:
        print('quaternion is error')
        return np.eye(3)
    x = x / a
    y = y / a
    z = z / a
    w = w / a
    return np.mat([[1 - 2*y*y - 2*z*z,     2*x*y - 2*z*w,     2*x*z + 2*y*w],
                   [    2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z,     2*y*z - 2*x*w],
                   [    2*x*z - 2*y*w,     2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]])

def rotateMatrixToQuaternion(R):
    R1 = [[R[0, 0], R[0, 1], R[0, 2]],
          [R[1, 0], R[1, 1], R[1, 2]],
          [R[2, 0], R[2, 1], R[2, 2]]]
    q = Quaternion(matrix=np.array(R1))

    return q.x, q.y, q.z, q.w

def inverseKinematic(DH, x, y, z, ox, oy, oz, ow):
    q_length = math.sqrt(ox*ox + oy*oy + oz*oz + ow*ow)
    if q_length == 0:
        print('quaternion is error')
        return
    else:
        ox = ox / q_length
        oy = oy / q_length
        oz = oz / q_length
        ow = ow / q_length

    js = []
    j00 = math.atan2(y, x)
    a = DH[2, 1]
    b = DH[3, 2]
    c = math.sqrt(x*x + y*y + (z - DH[0, 2])*(z - DH[0, 2]))
    a0 = 4*(x*x+y*y) + 4*(z - DH[0, 2])*(z - DH[0, 2])
    a1 = -4*(a*a - b*b + c*c)*math.sqrt(x*x + y*y)
    a2 = (a*a - b*b + c*c) * (a*a - b*b + c*c) - 4 * (z - DH[0, 2]) * (z - DH[0, 2]) * a * a
    if a1*a1 - 4*a0*a2 > 0:
        x0 = (-a1 + math.sqrt(a1*a1 - 4*a0*a2)) / (2 * a0)
        y0 = math.sqrt(a*a - x0*x0)
        #print('x0: ', x0, 'y0: ', y0)
        calcu3ForwardJointAngle(DH, j00, x0, y0, math.sqrt(x*x+y*y), z - DH[0, 2], b, js)

        x0 = (-a1 - math.sqrt(a1*a1 - 4*a0*a2)) / (2 * a0)
        y0 = math.sqrt(a*a - x0*x0)
        #print('x1: ', x0, 'y1: ', y0)
        calcu3ForwardJointAngle(DH, j00, x0, y0, math.sqrt(x*x+y*y), z - DH[0, 2], b, js)
    elif a1*a1 - 4*a0*a2 == 0:
        x0 = (-a1) / (2 * a0)
        y0 = math.sqrt(a*a - x0*x0)
        #print('x0: ', x0, 'y0: ', y0)
        calcu3ForwardJointAngle(DH, j00, x0, y0, math.sqrt(x*x+y*y), z - DH[0, 2], b, js)
    else:
        print('no solve')
        js = [[]]

    new_js = []
    for j in js:
        R = quaternionToRotationMatrix(ox, oy, oz, ow)
        T01 = transformToMatrix(DH[0, 0], DH[0, 1], DH[0, 2], DH[0, 3] + j[0])
        T12 = transformToMatrix(DH[1, 0], DH[1, 1], DH[1, 2], DH[1, 3] + j[1])
        T23 = transformToMatrix(DH[2, 0], DH[2, 1], DH[2, 2], DH[2, 3] + j[2])
        T34 = transformToMatrix(DH[3, 0], DH[3, 1], DH[3, 2], DH[3, 3])
        R01 = np.mat([[T01[0, 0], T01[0, 1], T01[0, 2]],
                      [T01[1, 0], T01[1, 1], T01[1, 2]],
                      [T01[2, 0], T01[2, 1], T01[2, 2]]])
        R12 = np.mat([[T12[0, 0], T12[0, 1], T12[0, 2]],
                      [T12[1, 0], T12[1, 1], T12[1, 2]],
                      [T12[2, 0], T12[2, 1], T12[2, 2]]])
        R23 = np.mat([[T23[0, 0], T23[0, 1], T23[0, 2]],
                      [T23[1, 0], T23[1, 1], T23[1, 2]],
                      [T23[2, 0], T23[2, 1], T23[2, 2]]])
        R34 = np.mat([[T34[0, 0], T34[0, 1], T34[0, 2]],
                      [T34[1, 0], T34[1, 1], T34[1, 2]],
                      [T34[2, 0], T34[2, 1], T34[2, 2]]])
        R = (R34.T) * (R23.T) * (R12.T) * (R01.T) * R
        #print('RRRRRRRRR: \n', R)
        alpha = math.atan2(R[1, 2], R[0, 2])
        betla = math.atan2(math.sqrt(R[2, 0]*R[2, 0] + R[2, 1]*R[2, 1]), R[2, 2])
        gamal = math.atan2(R[2, 1], -R[2, 0])

        new_js.append([])
        new_js[-1].append(j[0])
        new_js[-1].append(j[1])
        new_js[-1].append(j[2])
        new_js[-1].append(alpha)
        new_js[-1].append(betla)
        new_js[-1].append(gamal)
        #print(new_js[-1])

        new_js.append([])
        new_js[-1].append(j[0])
        new_js[-1].append(j[1])
        new_js[-1].append(j[2])
        new_js[-1].append(alpha + math.pi)
        new_js[-1].append(-betla)
        new_js[-1].append(gamal + math.pi)
        #print(new_js[-1])

    return new_js

if __name__ == '__main__':
    print("hello world")
    #print(DH)
    print(forwardKinematic(DH, 1, 0, 0, 0, 0, 0))
    js = inverseKinematic(DH, 0.5, 0.11, 0.26, 0.0, 0.0, 0.8, 0.6)
    print('##########################')
    for j in js:
        print('joint angle: ', j)
        T = forwardKinematic(DH, j[0], j[1], j[2], j[3], j[4], j[5])
        #print('P: ', T[0, 3], T[1, 3], T[2, 3])
        #print('Q: ', rotateMatrixToQuaternion(T))
    print('##########################')

注意实现

1. 此运动学逆解求解器不具备通用性，只适用于满足以上DH矩阵格式的6轴串联机器人；
2. 示例程序中，没有对所求的解进行筛选，存在超出限位、碰撞的情况；
3. 在求解2元2次方程组时，注意其中的非实数解；