图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_pi+pi/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]])DFbot=DHRobot([RevoluteDH(d=0.06,a=0,alpha=1*pi/2),##参数0RevoluteDH(d=0,a=-0.31,alpha=-0*pi/2,qlim=np.array([-pi,0])),###RevoluteDH(d=0,a=0.21,alpha=-0*pi/2,qlim=np.array([0,pi])),###RevoluteDH(d=0,a=0,alpha=0*pi/2),##参数3,旋转RevoluteDH(d=0,a=0.18,alpha=-0*pi/2,qlim=np.array([-1*pi/2,1*pi/2])),##参数4，RevoluteDH(d=0,a=0,alpha=0*pi/2),##参数5,旋转],name="DFbot")a0=angle0_to_DH0(30)###以机械臂调用参数为准，正前方为0度，向右为正，向左为负a1=angle1_to_DH1(0)a2=angle2_to_DH2(0)a3=angle3_to_DH3(0)##绘制DH矩阵，并和实际机械臂完成空间上的对应DFbot.plot([a0,a1,a2,0,a3,0],dt=10)state0=[a0,a1,a2,0,a3,0]T0=DFbot.fkine(state0)print(T0)2. DH矩阵
DFbot=DHRobot([RevoluteDH(d=0.06,a=0,alpha=1*pi/2),##参数0RevoluteDH(d=0,a=-0.31,alpha=-0*pi/2,qlim=np.array([-pi,0])),###RevoluteDH(d=0,a=0.21,alpha=-0*pi/2,qlim=np.array([0,pi])),###RevoluteDH(d=0,a=0,alpha=0*pi/2),##参数3,旋转RevoluteDH(d=0,a=0.18,alpha=-0*pi/2,qlim=np.array([-1*pi/2,1*pi/2])),##参数4，RevoluteDH(d=0,a=0,alpha=0*pi/2),##参数5,旋转],name="DFbot")
我暂时没有加入关节4的旋转
3. 逆运动学求解
3.1 求解目标位姿
正如第一个图，描述一个目标物在机械臂基坐标系下，有旋转和平移向量。
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]])如果一个物体只是绕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.py
fromspatialmathimport*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 U=math.atan2(y_,x_)# U = U * 180 / pi# print("U:", U)ify_>0andU>0:U=-U###把X轴向右的情况下，求得的角度，转化为机械臂调用向右，为负值elify_<0andU<0:U=-U###把X轴向左的情况下，求得的角度，转化为机械臂调用向左，为正值# 调用函数构造旋转矩阵, 弧度制rotation_matrix=construct_rotation_matrix(U)print("旋转矩阵:",rotation_matrix)# 定义平移向量translation_vector=[x_,y_,z_]# 手动构造 4x4 齐次变换矩阵homogeneous_matrix=np.eye(4)homogeneous_matrix[:3,:3]=rotation_matrix homogeneous_matrix[:3,3]=translation_vector# 使用齐次变换矩阵初始化 SE3 对象T=SE3(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_pi+pi/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参数DFbot=DHRobot([RevoluteDH(d=0.06,a=0,alpha=1*pi/2,qlim=np.array([-1*pi/2,1*pi/2])),##参数0RevoluteDH(d=0,a=-0.31,alpha=-0*pi/2,qlim=np.array([-pi,0])),###RevoluteDH(d=0,a=0.21,alpha=-0*pi/2,qlim=np.array([0,pi])),###RevoluteDH(d=0,a=0,alpha=0*pi/2),##参数3,旋转RevoluteDH(d=0,a=0.18,alpha=-0*pi/2,qlim=np.array([-1*pi/2,1*pi/2])),##参数4，RevoluteDH(d=0,a=0,alpha=0*pi/2),##参数5,旋转],name="DFbot")a0=angle0_to_DH0(0)a1=angle1_to_DH1(0)a2=angle2_to_DH2(0)a3=angle4_to_DH4(0)# DFbot.plot([a0, a1, a2, 0, a3, 0], dt=5)###初始位姿state0=[0,a1,a2,0,a3,0]T0=DFbot.fkine(state0)# print('初始位姿', T0)##逆解，导入xyz_to_T4.py###目标位姿，需要给定的是目标的位姿，以X轴为正前方T1=xyz_to_T4(0.49,-0.28,0.15)sol=DFbot.ikine_LM(T1,state0,ilimit=300,slimit=300,joint_limits=False)print(sol)ifsol.success:print("找到解：")print(sol.q)# DFbot.plot(sol.q, block=True, dt=30)DFbot.plot(sol.q,dt=10)time.sleep(8)angle_a0=inverse_angle0_to_DH0(sol.q[0])angle_a1=inverse_angle1_to_DH1(sol.q[1])angle_a2=inverse_angle2_to_DH2(sol.q[2])angle_a4=inverse_angle3_to_DH3(sol.q[4])##保留为整数,四舍五入angle_a0=int(round(angle_a0,0))angle_a1=int(round(angle_a1,0))angle_a2=int(round(angle_a2,0))angle_a4=int(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], dt=30)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%257D&request_id=5c54623491d76d4b21417ce9412c3396&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2alltop_positive~default-1-125328920-null-null.142v102pc_search_result_base3&utm_term=%E6%9C%BA%E6%A2%B0%E8%87%82&spm=1018.2226.3001.4187