机械臂正逆解的过程（python）

图1是参考博客炎黄智能的图片。1. 正运动学python代码描述我实际的机械臂的DH矩阵fromroboticstoolboximport*fromspatialmathimport*importnumpyasnpfrommathimportpiimporttimeimportmath##参数0 角度转为DH参数0defangle0_to_DH0(angle):returnangle*(-pi/180)##参数1 角度转为DH参数1defangle1_to_DH1(angle):returnangle*(-pi/180)-pi/2##参数2 角度转为DH参数2defangle2_to_DH2(angle):returnangle*(-pi/180)1*pi/2##参数3 角度转为DH参数3defangle3_to_DH3(angle):returnangle*(-pi/180)###逆解definverse_angle1_to_DH1(angle_pi):return(angle_pipi/2)*(-180/pi)###逆解definverse_angle2_to_DH2(angle_pi):return(angle_pi-pi/2)*(-180/pi)###逆解definverse_angle3_to_DH3(angle_pi):returnangle_pi*(-180/pi)# 定义一个函数来构造旋转矩阵defconstruct_rotation_matrix(U): 根据给定的 U 值构造旋转矩阵。 :param U: 旋转角度弧度 :return: 3x3 的旋转矩阵 returnnp.array([[math.cos(U),0,-math.sin(U)],[-math.sin(U),0,-math.cos(U)],[0,1,0]])DFbotDHRobot([RevoluteDH(d0.06,a0,alpha1*pi/2),##参数0RevoluteDH(d0,a-0.31,alpha-0*pi/2,qlimnp.array([-pi,0])),###RevoluteDH(d0,a0.21,alpha-0*pi/2,qlimnp.array([0,pi])),###RevoluteDH(d0,a0,alpha0*pi/2),##参数3,旋转RevoluteDH(d0,a0.18,alpha-0*pi/2,qlimnp.array([-1*pi/2,1*pi/2])),##参数4RevoluteDH(d0,a0,alpha0*pi/2),##参数5,旋转],nameDFbot)a0angle0_to_DH0(30)###以机械臂调用参数为准正前方为0度向右为正向左为负a1angle1_to_DH1(0)a2angle2_to_DH2(0)a3angle3_to_DH3(0)##绘制DH矩阵并和实际机械臂完成空间上的对应DFbot.plot([a0,a1,a2,0,a3,0],dt10)state0[a0,a1,a2,0,a3,0]T0DFbot.fkine(state0)print(T0)2. DH矩阵DFbotDHRobot([RevoluteDH(d0.06,a0,alpha1*pi/2),##参数0RevoluteDH(d0,a-0.31,alpha-0*pi/2,qlimnp.array([-pi,0])),###RevoluteDH(d0,a0.21,alpha-0*pi/2,qlimnp.array([0,pi])),###RevoluteDH(d0,a0,alpha0*pi/2),##参数3,旋转RevoluteDH(d0,a0.18,alpha-0*pi/2,qlimnp.array([-1*pi/2,1*pi/2])),##参数4RevoluteDH(d0,a0,alpha0*pi/2),##参数5,旋转],nameDFbot)我暂时没有加入关节4的旋转3. 逆运动学求解3.1 求解目标位姿正如第一个图描述一个目标物在机械臂基坐标系下有旋转和平移向量。T1SE3([[0.866,0,0.5,0.3377],[0.5,0,-0.866,0.195],[0,1,0,0.37],[0,0,0,1]])如果一个物体只是绕Z轴有旋转没有绕其他轴旋转可以参考我推出的下面的旋转矩阵np.array([[math.cos(U),0,-math.sin(U)],[-math.sin(U),0,-math.cos(U)],[0,1,0]Y,Z,X因为我的机械臂绕Z轴逆时针为负角度所以我这里是-30度大家可以带入-30度到我的旋转矩阵里和T1参数一致。T1中的第4列就是相对于机械臂基坐标系的X,Y,Z在求解各个关节的角度时需要提供目标的位姿。以下是代码xyz_to_T4.pyfromspatialmathimport*importnumpyasnpfrommathimportpiimportmath# 定义一个函数来构造旋转矩阵defconstruct_rotation_matrix(U): 根据给定的 U 值构造旋转矩阵。 :param U: 旋转角度弧度 :return: 3x3 的旋转矩阵 returnnp.array([[math.cos(U),0,-math.sin(U)],[-math.sin(U),0,-math.cos(U)],[0,1,0]])defxyz_to_T4(x,y,z): 根据给定的 x, y, z 和 U 值构造包含旋转和平移的齐次变换矩阵。 :param x: x 轴平移量, 机械臂世界坐标系下 :param y: y 轴平移量 :param z: z 轴平移量 T1 SE3([[0.866, 0, 0.5, 0.3377], [0.5, 0, -0.866, 0.195], [0, 1, 0, 0.37], [0, 0, 0, 1]]) # 从位置向量中提取 x 坐标x_x y_y z_z Umath.atan2(y_,x_)# U U * 180 / pi# print(U:, U)ify_0andU0:U-U###把X轴向右的情况下求得的角度转化为机械臂调用向右为负值elify_0andU0:U-U###把X轴向左的情况下求得的角度转化为机械臂调用向左为正值# 调用函数构造旋转矩阵, 弧度制rotation_matrixconstruct_rotation_matrix(U)print(旋转矩阵:,rotation_matrix)# 定义平移向量translation_vector[x_,y_,z_]# 手动构造 4x4 齐次变换矩阵homogeneous_matrixnp.eye(4)homogeneous_matrix[:3,:3]rotation_matrix homogeneous_matrix[:3,3]translation_vector# 使用齐次变换矩阵初始化 SE3 对象TSE3(homogeneous_matrix)print(\n包含旋转和平移的齐次变换矩阵:)print(T)returnTif__name____main__:xyz_to_T4(0.3377,0.195,0.37)3.2 求解各个关节的角度在模拟的实际机械臂的DH矩阵和能根据目标的X,Y,Z求得位姿T1后调用xyz_to_T4.py可执行以下代码反解出各个关节的角度。fromroboticstoolboximport*fromspatialmathimport*importnumpyasnpfrommathimportpiimporttimefromxyz_to_T4importxyz_to_T4##参数0 角度转为DH参数0defangle0_to_DH0(angle):returnangle*(-pi/180)##参数1 角度转为DH参数1defangle1_to_DH1(angle):returnangle*(-pi/180)-pi/2##参数2 角度转为DH参数2defangle2_to_DH2(angle):returnangle*(-pi/180)1*pi/2##参数3 角度转为DH参数3defangle4_to_DH4(angle):returnangle*(-pi/180)###逆解definverse_angle0_to_DH0(angle_pi):returnangle_pi*(-180/pi)###逆解definverse_angle1_to_DH1(angle_pi):return(angle_pipi/2)*(-180/pi)###逆解definverse_angle2_to_DH2(angle_pi):return(angle_pi-pi/2)*(-180/pi)###逆解definverse_angle3_to_DH3(angle_pi):returnangle_pi*(-180/pi)#DH参数DFbotDHRobot([RevoluteDH(d0.06,a0,alpha1*pi/2,qlimnp.array([-1*pi/2,1*pi/2])),##参数0RevoluteDH(d0,a-0.31,alpha-0*pi/2,qlimnp.array([-pi,0])),###RevoluteDH(d0,a0.21,alpha-0*pi/2,qlimnp.array([0,pi])),###RevoluteDH(d0,a0,alpha0*pi/2),##参数3,旋转RevoluteDH(d0,a0.18,alpha-0*pi/2,qlimnp.array([-1*pi/2,1*pi/2])),##参数4RevoluteDH(d0,a0,alpha0*pi/2),##参数5,旋转],nameDFbot)a0angle0_to_DH0(0)a1angle1_to_DH1(0)a2angle2_to_DH2(0)a3angle4_to_DH4(0)# DFbot.plot([a0, a1, a2, 0, a3, 0], dt5)###初始位姿state0[0,a1,a2,0,a3,0]T0DFbot.fkine(state0)# print(初始位姿, T0)##逆解导入xyz_to_T4.py###目标位姿需要给定的是目标的位姿以X轴为正前方T1xyz_to_T4(0.49,-0.28,0.15)solDFbot.ikine_LM(T1,state0,ilimit300,slimit300,joint_limitsFalse)print(sol)ifsol.success:print(找到解)print(sol.q)# DFbot.plot(sol.q, blockTrue, dt30)DFbot.plot(sol.q,dt10)time.sleep(8)angle_a0inverse_angle0_to_DH0(sol.q[0])angle_a1inverse_angle1_to_DH1(sol.q[1])angle_a2inverse_angle2_to_DH2(sol.q[2])angle_a4inverse_angle3_to_DH3(sol.q[4])##保留为整数,四舍五入angle_a0int(round(angle_a0,0))angle_a1int(round(angle_a1,0))angle_a2int(round(angle_a2,0))angle_a4int(round(angle_a4,0))print(角度0,angle_a0)print(角度1,angle_a1)print(角度2,angle_a2)print(角度3,angle_a4)# ##验证# b0 angle0_to_DH0(angle_a0)# b1 angle1_to_DH1(angle_a1)# b2 angle2_to_DH2(angle_a2)# b3 angle4_to_DH4(angle_a4)# DFbot.plot([b0, b1, b2, 0, b3, 0], dt30)else:print(未找到解)4. 效果展示这个是XZ平面上机械臂到达的位置就是机械臂的正前面。这个是XYZ坐标下机械臂到达的位置就是机械臂的侧前面。参考博客https://blog.csdn.net/knightsinghua/article/details/125328920?ops_request_misc%257B%2522request%255Fid%2522%253A%25225c54623491d76d4b21417ce9412c3396%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257Drequest_id5c54623491d76d4b21417ce9412c3396biz_id0utm_mediumdistribute.pc_search_result.none-task-blog-2alltop_positive~default-1-125328920-null-null.142v102pc_search_result_base3utm_term%E6%9C%BA%E6%A2%B0%E8%87%82spm1018.2226.3001.4187