工业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 Quaternionnp.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 * T3def 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 * T5def 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 / ay = y / az = z / aw = w / areturn 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.wdef 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')returnelse:ox = ox / q_lengthoy = oy / q_lengthoz = oz / q_lengthow = ow / q_lengthjs = []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 * aif 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_jsif __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次方程组时，注意其中的非实数解；