网站建设方案推荐,中国四大软件外包公司是哪四个,成都网站优化教程,企业网站结构图工业6轴机械臂运动学逆解#xff08;解析解#xff09; 通常工业机械臂采用6旋转轴串连的形式#xff0c;保证了灵活性#xff0c;但为其运动学逆解#xff08;即已知机械臂末端的位姿 P P P#xff0c;求机械臂各个旋转轴的旋转角#xff09;带来了较大的困难#xff…工业6轴机械臂运动学逆解解析解 通常工业机械臂采用6旋转轴串连的形式保证了灵活性但为其运动学逆解即已知机械臂末端的位姿 P P P求机械臂各个旋转轴的旋转角带来了较大的困难通常没有解析解。为了提高实时性经过前辈们不懈的研究当6轴机械臂满足pieper准则时可以得出其运动学逆解的解析解。pieper准则如下
三个相邻关节轴线交于一点如fanuc m10系列三个相邻关节轴线相互平行如ur5系列 以下将通过一个简单实例介绍机械臂在pieper第一准则的情况的运动学逆解。
机械臂运动学模型 机械臂的简化模型如图1所示。 图1 采用DH矩阵的形式对机械臂进行建模DH矩阵如下
关节连杆夹角连杆长度连杆偏距初始关节角1000.1020.5 π \pi π000300.500.5 π \pi π40.5 π \pi π00.505-0.5 π \pi π00060.5 π \pi π000 从图1中可以看出在初始状态下机械臂的第2、3关节轴与基座坐标系的Y轴相互平行机械臂的第4、5、6关节轴线相交于一点 P P P满足pieper第一准则且点 P P P与基座坐标系的XOZ平面重合。在这些条件下虽然限制了机械臂的设计和构型但极大地简化了逆解的过程。
运动学逆解 由于机械臂的结构比较简单故采用几何法的方式求解机械臂的运动学逆解。 机械臂末端初始位姿 P 0 P_0 P0运动学逆解末端的位姿 P ( x , y , z ) P(x,y,z) P(x,y,z)
求解第1关节轴的关节角
以基坐标系为参考机械臂末端位置示意图如图2所示。 图2 在图1中点 P 0 P_0 P0为机械臂末端的初始状态位置当前位置点 P P P为机械臂末端的期望位置由于初始状态时机械臂末端的初始位置与 X 0 Y 0 X_0Y_0 X0Y0平面重合故关节轴1需要旋转 θ 1 \theta_1 θ1或 θ 1 π \theta_1 \pi θ1π才能使期望位置 P P P与 X 0 Y 0 X_0Y_0 X0Y0平面重合故关节轴1存在两个解 θ 1 { a t a n 2 ( y , x ) a t a n 2 ( y , x ) π (1) \theta_1 \begin{cases} atan2(y, x)\\ atan2(y, x) \pi\\ \end{cases} \tag1 θ1{atan2(y,x)atan2(y,x)π(1)
求解第2、3关节轴的关节角 关节轴1经过旋转 θ 1 a t a n 2 ( y , x ) \theta_1atan2(y, x) θ1atan2(y,x)角度以第一关节轴的坐标系为参考得示意图如图3所示。 P 1 P_1 P1为机械臂末端的当前位置 P P P为末端期望位置 O A 0 OA_0 OA0、 O A 1 OA_1 OA1为机械臂的第2连杆 A 0 P A_0P A0P、 A 1 P A_1P A1P为机械臂的第3连杆。 图3 设 A 0 ( x 0 , 0 , z 0 ) A_0(x_0, 0, z_0) A0(x0,0,z0)可得方程 x 0 2 z 0 2 D H [ 2 , 1 ] 2 ( x − x 0 ) 2 ( z − D H [ 0 , 2 ] − z 0 ) 2 D H [ 3 , 2 ] 2 (2) 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 \tag2 x02z02DH[2,1]2(x−x0)2(z−DH[0,2]−z0)2DH[3,2]2(2) 对2元2次方程组2进行求解得 ( x 00 , z 00 ) (x_{00}, z_{00}) (x00,z00)、 ( x 01 , z 01 ) (x_{01}, z_{01}) (x01,z01)、 ( x 02 , z 02 ) (x_{02}, z_{02}) (x02,z02)、 ( x 03 , z 03 ) (x_{03}, z_{03}) (x03,z03)四组解去除其中的非实数解。由此可得到关节轴2、3的旋转角。 θ 2 a t a n 2 ( z 0 , x 0 ) θ 3 a t a n 2 ( z − D H [ 0 , 2 ] − z 0 , x − x 0 ) − a t a n 2 ( z 0 , x 0 ) (2) \theta_2 atan2(z_0, x_0)\\ \theta_3 atan2(z - DH[0, 2] - z_0, x - x_0) - atan2(z_0, x_0) \tag2 θ2atan2(z0,x0)θ3atan2(z−DH[0,2]−z0,x−x0)−atan2(z0,x0)(2) 如图2所示解方程2可得到关节角 θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3的两组解。 同理当取 θ 1 a t a n 2 ( y , x ) π \theta_1atan2(y, x) \pi θ1atan2(y,x)π时亦可得到关节角 θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3的两组解。 至此 θ 1 \theta_1 θ1、 θ 2 \theta_2 θ2、 θ 3 \theta_3 θ3存在8组解可去除其中相同的解。
求解第4、5、6关节轴的关节角
经过对机械臂前3根轴的旋转已经机械臂的末端位置与期望的末端位置相重合由于关节轴4、5、6相交与末端位置对此3轴的旋转不会改变末端的位置故单独对此3轴进行姿态解算即可得到关节角。 设 R 1 ( θ 1 ) R_1(\theta_1) R1(θ1)、 R 2 ( θ 2 ) R_2(\theta_2) R2(θ2)、 R 3 ( θ 3 ) R_3(\theta_3) R3(θ3)、 R 4 ( θ 4 ) R_4(\theta_4) R4(θ4)、 R 5 ( θ 5 ) R_5(\theta_5) R5(θ5)、 R 6 ( θ 6 ) R_6(\theta_6) R6(θ6)表示各轴的变换矩阵。将对关节轴4、5、6的旋转看成是动欧拉角ZYZ的旋转模式其旋转矩阵为 R ( θ 4 , θ 5 , θ 6 ) R(\theta_4,\theta_5,\theta_6) R(θ4,θ5,θ6)。 R ( θ 4 , θ 5 , θ 6 ) [ c o s θ 4 − s i n θ 4 0 s i n θ 4 c o s θ 4 0 0 0 1 ] [ c o s θ 5 0 s i n θ 5 0 1 0 − s i n θ 5 0 c o s θ 5 ] [ c o s θ 6 − s i n θ 6 0 s i n θ 6 c o s θ 6 0 0 0 1 ] (3) R(\theta_4,\theta_5,\theta_6) \begin{bmatrix} cos\theta_4-sin\theta_40\\ sin\theta_4cos\theta_40\\ 001\\ \end{bmatrix} \begin{bmatrix} cos\theta_50sin\theta_5\\ 010\\ -sin\theta_50cos\theta_5\\ \end{bmatrix} \begin{bmatrix} cos\theta_6-sin\theta_60\\ sin\theta_6cos\theta_60\\ 001\\ \end{bmatrix} \tag3 R(θ4,θ5,θ6) cosθ4sinθ40−sinθ4cosθ40001 cosθ50−sinθ5010sinθ50cosθ5 cosθ6sinθ60−sinθ6cosθ60001 (3) 具体计算得 R ( θ 4 , θ 5 , θ 6 ) [ c o s θ 4 c o s θ 5 c o s θ 6 − s i n θ 4 s i n θ 6 − c o s θ 4 c o s θ 5 s i n θ 6 − s i n θ 4 c o s c o s θ 6 θ 4 s i n θ 5 s i n θ 4 c o s θ 5 c o s θ 6 c o s θ 4 s i n θ 6 − s i n θ 4 c o s θ 5 s i n θ 6 c o s θ 4 c o s θ 6 s i n θ 4 s i n θ 5 − s i n θ 5 c o s θ 6 s i n θ 5 s i n θ 6 c o s θ 5 ] (4) R(\theta_4,\theta_5,\theta_6) \begin{bmatrix} cos\theta_4cos\theta_5cos\theta_6-sin\theta_4sin\theta_6-cos\theta_4cos\theta_5sin\theta_6-sin\theta_4coscos\theta_6\theta_4sin\theta_5\\ sin\theta_4cos\theta_5cos\theta_6cos\theta_4sin\theta_6-sin\theta_4cos\theta_5sin\theta_6cos\theta_4cos\theta_6sin\theta_4sin\theta_5\\ -sin\theta_5cos\theta_6sin\theta_5sin\theta_6cos\theta_5\\ \end{bmatrix} \tag4 R(θ4,θ5,θ6) cosθ4cosθ5cosθ6−sinθ4sinθ6sinθ4cosθ5cosθ6cosθ4sinθ6−sinθ5cosθ6−cosθ4cosθ5sinθ6−sinθ4cos−sinθ4cosθ5sinθ6cosθ4cosθ6sinθ5sinθ6cosθ6θ4sinθ5sinθ4sinθ5cosθ5 (4) 由机械臂的正运动学可得 R ( θ 1 ) R ( θ 2 ) R ( θ 3 ) R ( θ 4 0 ) R ( θ 4 , θ 5 , θ 6 ) R P (5) R(\theta_1)R(\theta_2)R(\theta_3)R(\theta_40)R(\theta_4, \theta_5, \theta_6)R_P \tag5 R(θ1)R(θ2)R(θ3)R(θ40)R(θ4,θ5,θ6)RP(5) 在公式5中 R P R_P RP为机械臂末端点 P P P的姿态。对公式5进行移项得 R ( θ 4 , θ 5 , θ 6 ) [ R ( θ 1 ) R ( θ 2 ) R ( θ 3 ) R ( θ 4 0 ) ] − 1 R P [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] (6) R(\theta_4, \theta_5, \theta_6)[R(\theta_1)R(\theta_2)R(\theta_3)R(\theta_40)]^{-1}R_P \begin{bmatrix} r_{11}r_{12}r_{13}\\ r_{21}r_{22}r_{23}\\ r_{31}r_{32}r_{33}\\ \end{bmatrix} \tag6 R(θ4,θ5,θ6)[R(θ1)R(θ2)R(θ3)R(θ40)]−1RP r11r21r31r12r22r32r13r23r33 (6) 联立公式46可得两组解 { θ 4 a t a n 2 ( r 23 , r 13 ) θ 5 a t a n 2 ( s q r t ( r 31 2 r 32 2 ) , r 33 ) θ 6 a t a n 2 ( r 32 , − r 31 ) (7) \begin{cases} \theta_4atan2(r_{23}, r_{13})\\ \theta_5atan2(sqrt(r_{31}^2r_{32}^2), r_{33})\\ \theta_6atan2(r_{32}, -r_{31}) \end{cases} \tag7 ⎩ ⎨ ⎧θ4atan2(r23,r13)θ5atan2(sqrt(r312r322),r33)θ6atan2(r32,−r31)(7) { θ 4 a t a n 2 ( r 23 , r 13 ) π θ 5 − a t a n 2 ( s q r t ( r 31 2 r 32 2 ) , r 33 ) θ 6 a t a n 2 ( r 32 , − r 31 ) π (8) \begin{cases} \theta_4atan2(r_{23}, r_{13}) \pi\\ \theta_5-atan2(sqrt(r_{31}^2r_{32}^2), r_{33})\\ \theta_6atan2(r_{32}, -r_{31}) \pi \end{cases} \tag8 ⎩ ⎨ ⎧θ4atan2(r23,r13)πθ5−atan2(sqrt(r312r322),r33)θ6atan2(r32,−r31)π(8) 综上完成机械臂的运动学逆解的解析解求解过程可能存在8个以上的解可根据一些约束调节对求得的解进行删选如关节限位、碰撞检测等。
示例程序
import numpy as np
import math
from pyquaternion import Quaternionnp.set_printoptions(suppressTrue)# 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(matrixnp.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*xy*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*xy*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*xy*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*xy*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(##########################)注意实现
此运动学逆解求解器不具备通用性只适用于满足以上DH矩阵格式的6轴串联机器人示例程序中没有对所求的解进行筛选存在超出限位、碰撞的情况在求解2元2次方程组时注意其中的非实数解
