一、单位矩阵坐标系平移与旋转三维空间中的刚体变换 若点m基于活动坐标系B的坐标用向量a[x][y][z][0]表示。A为固定坐标 B是由A旋转平移得到的. 旋转用旋转矩阵表示平移过程用向量d[tx][ty][tz]表示 那么: 使用4×4齐次变换矩阵 1. 绕X轴旋转旋转角度φ [X] [1 0 0 tx] [x] [Y] [0 cosφ -sinφ ty] [y] [Z] [0 sinφ cosφ tz] * [z] [1] [0 0 0 1] [1] Rx(φ) [1 0 0 ] [0 cosφ -sinφ ] [0 sinφ cosφ ] 2.绕Y轴旋转旋转角度θ [X] [cosθ 0 sinθ tx] [x] [Y] [0 1 0 ty] [y] [Z] [-sinθ 0 cosθ tz] * [z] [1] [0 0 0 1] [1] Ry(θ) [cosθ 0 sinθ ] [0 1 0 ] [-sinθ 0 cosθ ] 3.绕Z轴旋转旋转角度ψ [X] [cosψ -sinψ 0 tx] [x] [Y] [sinψ cosψ 0 ty] [y] [Z] [0 0 1 tz] * [z] [1] [0 0 0 1] [1] Rz(ψ) [cosψ -sinψ 0 ] [sinψ cosψ 0 ] [0 0 1 ] 齐次变换矩阵与齐次坐标向量的区别 齐次变换矩阵 Tx(a) [1, 0, 0, a], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1] 齐次变换矩阵是一个变换操作表示沿X轴平移距离a。它可以应用于任何点或向量。 齐次坐标向量 P [a] [0] [0] [1] 齐次坐标向量是一个具体的点表示位于(a, 0, 0)位置的点。 特性 变换矩阵 (Tx) 坐标向量 (P) 维度 4×4矩阵 4×1向量 作用 对点进行变换 表示一个点 可重复使用 可以应用于多个点 只表示一个特定点 组合性 可以与其他变换矩阵相乘 只能作为变换的输入/输出 物理意义 移动操作 位置结果 在DH参数法中我们需要的是变换矩阵本身因为 我们要将多个变换组合起来 我们需要将变换应用于不同的点不仅仅是原点 我们需要表示坐标系之间的关系而不仅仅是点的位置二、DH参数表达法表达法1(改进法)1、连杆长度aᵢ₋₁ 从关节轴 i-1 到关节轴 i 的公共垂线长度。空间中的任意两条线 有且仅有一条与他们都平行的线 这个线的长度叫连杆长度 它总是一个正值或非负值。 它是一个常量由机械臂的物理结构决定。 视频16.30s 2、连杆扭转角αᵢ₋₁ 关节轴 i-1 绕公共垂线旋转多少角度才能与关节轴 i 平行。 单位为弧度。根据右手定则确定正负。 它是一个常量。对于大多数常见的关节如果两个相邻关节轴是平行的 则 α 0如果是垂直的则 α 90° 或 π/2。 3、 连杆偏置dᵢ 沿着关节轴 i 的方向公共垂线 aᵢ₋₁ 与aᵢ之间的距离。 对于旋转关节d 是常量。 对于移动关节d 是变量即关节本身移动的距离。 4、 关节角θᵢ 绕关节轴 i 旋转的角度使上一个连杆的aᵢ₋₁ 与当前连杆的aᵢ 平行。 对于旋转关节θ 是变量。 对于移动关节θ 是常量。转轴定义为Z轴转轴之间的垂线定义为X轴 Y轴根据右手定则进行判断机械臂末端的坐标轴定义。Z轴就是自身轴的方向X轴延续上一个机构的X轴 这样最方便总图空间坐标系转换的四个步骤步骤1旋转使用参数连杆扭曲角α 让将轴0转动到与轴1在空间上平行步骤2平移使用参数连杆长度a将轴0移动到轴1上将他们Z轴统一步骤3旋转使用参数关节角θ让旋转轴0所在坐标系X轴与Y轴与旋转轴1所在坐标系X轴与Y轴平行步骤4平移使用参数连杆偏置d让他们坐标系完全重叠用矩阵相乘的形式表示 T_i^{i-1} Rot_x(α_{i-1}) × Trans_x(a_{i-1}) × Rot_z(θ_i) × Trans_z(d_i) 实际上这四次变换可以合并为一个单一的4×4齐次变换矩阵 T_i^{i-1} [cosθ_i, -sinθ_i, 0, a_{i-1}], [sinθ_i·cosα_{i-1}, cosθ_i·cosα_{i-1}, -sinα_{i-1}, -d_i·sinα_{i-1}], [sinθ_i·sinα_{i-1}, cosθ_i·sinα_{i-1}, cosα_{i-1}, d_i·cosα_{i-1}], [0, 0, 0, 1]表达法2螺旋卸船机实例旧新这就是标准DH单节变换矩阵后续连乘全部T即可得到机构末端对基座的总变换。DH参数举例 L3沿X轴方向距离是L3 所以他的向量就是[L3,0,0] 最后算出坐标系0与坐标系3的关系以后 还要再乘以这个机构末端的齐次矩阵才行 就像 [1, 0, 0, L3], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]方法1 例题比较特殊的情况 轴1与轴2垂直还相交 这时候Z2方向就有两种选择了 这里参数θi是对的 只不过此时机构θ是0°罢了 L3沿Z轴方向距离是L3 所以他的向量就是[0,0,L3] 最后算出坐标系0与坐标系3的关系以后 还要再乘以这个机构末端的齐次矩阵才行 就像 [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, L3], [0, 0, 0, 1]下面是例题 后面有时间自己分析一下看看对不对方法2例题常见应用实际代码与案例1、正运动学import numpy as np 正运动学 np.set_printoptions(precision6, suppressTrue)#限制数组打印格式保留到小数点后6位。确认限制科学计数法的使用 # 单个变换矩阵 theta(z轴旋转角) d(z轴平移) alpha(z轴旋转角) a(z轴平移) def singleT(theta, d, a, alpha): return np.array([ [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)], [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)], [0, np.sin(alpha), np.cos(alpha), d], [0, 0, 0, 1] ]) def convert2DHdegrees(q_list): # 转为DH模型角度 q_list中的分别是工程角度q1、q2、q3是DH参数中的角度 q1 -q_list[0] q2 q_list[1] q3 q_list[2] - 90 return np.round([q1, q2, q3], 3) # 定义DH # 以Episode 1 为例 a1, a2, a3 0, 20, 0 #水平螺旋20M d1, d2, d3 11.105, 0, 14 #底座高度11.105M垂直螺旋14M # 在robodk复制一个角度用来验证 target_q [0, 0, 50] # 转换为DH角度 target_q convert2DHdegrees(target_q) print(f输入角度{target_q}) # 转为弧度 theta np.deg2rad(target_q) # 计算单个变换矩阵 T_0_1 singleT(theta[0], d1, a1, np.pi / 2) T_1_2 singleT(theta[1], d2, a2, 0) T_2_3 singleT(theta[2], d3, a3, 0) # 连乘得到关节6相对于基座的齐次矩阵 T_0_6 T_0_1 T_1_2 T_2_3 # 打印与robodk对比 print(T_0_6)2、逆运动学多解、无限解、无解import time from roboticstoolbox import DHRobot, RevoluteDH import numpy as np 逆运动学求解 # 角度范围 此处描述的是DH参数中的角度 degree_range_list [ [-90, 90], # 挡板占位15520度ROBODK0-340 [-20, 20], # 水平挡板180ROBODK0-180 [-110, 40] # 两侧挡板ROBODK0-163 ] # 转为弧度 # radian_range_list [[np.deg2rad(degree) for degree in pair] for pair in degree_range_list] radian_range_list [np.deg2rad(pair) for pair in degree_range_list] # 上面这个代码遍历以后得到的元素如下:我试过代码依旧可以正常运行 # [array([-1.57079633, 1.57079633]), array([-0.34906585, 0.34906585]), array([-1.91986218, 0.6981317 ])] # print(radian_range_list) # 定义3轴机器人旋转J1 俯仰J2 钟摆J3 # DH参数RevoluteDH(d, a, alpha, offset) robot_3dof DHRobot([ # J1旋转轴绕基座Z轴d11.105, a0, alphaπ/2 RevoluteDH(d11.105, a0, alphanp.pi / 2, nameJ1, qlimnp.array(radian_range_list[0])), # J2俯仰轴大臂d0, a20, alpha0 RevoluteDH(d0, a20, alpha0, nameJ2, qlimnp.array(radian_range_list[1])), # J3钟摆轴小臂d0, a14, alpha0 RevoluteDH(d0, a14, alpha0, nameJ3, qlimnp.array(radian_range_list[2])) ], name3DOF_Robot) # 打印模型验证DH参数 print(robot_3dof) # 可视化机器人可选需matplotlib # robot_3dof.plot([0, 0, 0]) # 传入中位关节角显示机器人姿态 # 输入RoboDK原始角度如[30, 0, 0]转换为DH角度 def convert2DHdegrees(q_list): # 转为DH模型角度 q_list中的分别是工程角度q1、q2、q3是DH参数中的角度 q1 -q_list[0] q2 q_list[1] q3 q_list[2] - 90 return np.round([q1, q2, q3], 3) # 测试关节角 q_raw [30, 0, 0] # RoboDK原始角度 q_dh convert2DHdegrees(q_raw) #转换成工程角度 q_rad np.deg2rad(q_dh) # 转为弧度 # 正解计算末端位姿齐次矩阵 T robot_3dof.fkine(q_rad) # 提取末端位置x/y/z和你的正解对比[-30 0 -90] xyz T.t # T.t 等价于 T[:3, 3] print(f3轴正解位置x{xyz[0]:.2f}, y{xyz[1]:.2f}, z{xyz[2]:.2f}) a_time time.time() # 目标位置仅x/y/z不约束姿态 target_xyz [31, 1, 2.105973] # 构造目标齐次矩阵姿态部分任意仅位置有效 T_target np.eye(4) T_target[:3, 3] target_xyz # print(T_target) # 3轴逆解调用ikine_LM仅约束位置 sol robot_3dof.ikine_LM( T_target, q0np.deg2rad([0, 0, 0]), # 初始角中位提高成功率/算法迭代的起始点影响收敛到的解 弧度制的关节角度数组 # 自由度约束掩码 mask 始终只有 6 个参数 核心mask[x,y,z,rx,ry,rz]1约束0放开 指定需要满足的末端自由度 # 只约束位置不约束姿态3DOF机器人常用 # mask [1, 1, 1, 0, 0, 0] # 仅位置约束 # # # 完全约束6DOF机器人 # mask [1, 1, 1, 1, 1, 1] # 位置姿态全约束 # # # 只约束姿态特殊应用 # mask [0, 0, 0, 1, 1, 1] # 仅姿态约束 # # # 部分约束例如只约束xy平面位置和绕z轴旋转 # mask [1, 1, 0, 0, 0, 1] # xy位置 绕z旋转 # # # 冗余机器人可以放松某些约束 # mask [1, 1, 1, 0, 0, 0] # 3DOF机器人常见 mask[1, 1, 1, 0, 0, 0], tol0.01, # 位置误差容忍度/停止迭代的收敛阈值 与误差度量相关通常是位置误差的欧几里得距离 # tol 0.01 # 宽松迭代快精度较低 # tol 0.001 # 中等平衡精度和速度 # tol 0.0001 # 严格精度高迭代次数多 # tol 1e-6 # 非常严格接近数值极限 joint_limitsTrue #是否考虑机器人模型的关节角度限制 # 其他常用参数 # ilimit - 最大迭代次数 # slimit - 步长限制 # search - 搜索策略 ) b_time time.time() print(b_time - a_time) # 输出结果 print(f逆解是否成功{sol.success}) if sol.success: q_sol_rad sol.q q_sol_deg np.rad2deg(q_sol_rad) # 弧度转角度 print(f3轴逆解关节角DH角度度J1{q_sol_deg[0]:.2f}, J2{q_sol_deg[1]:.2f}, J3{q_sol_deg[2]:.2f}) # 还原为RoboDK原始角度你的转换规则 q_sol_raw [-q_sol_deg[0], q_sol_deg[1], q_sol_deg[2] 90] print(f还原为RoboDK角度{q_sol_raw}) # 验证正解回代检查位置误差 T_verify robot_3dof.fkine(q_sol_rad) pos_error np.linalg.norm(T_verify.t - target_xyz) print(f位置误差{pos_error:.3f}mm) else: print(逆解失败, sol.reason)