Python 机器人学

2023-09-15 02:33| 来源: 网络整理| 查看: 265

设关节1、关节5的高 (圆柱高) 分别为 $h_{1}, h_{5}$ ，5个关节的转动角分别为 $\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}, \theta_{5}$

可得出DH参数表：

# $\theta$ $d$ $a$ $\alpha$ 1 - 2 $\theta_{1}$ $L_1-\frac{h_1}{2}$ $0$ $90$

2 - 3

$90+\theta_{2}$ $0$ $L_{2}$ $0$

3 - 4

$\theta_{3}$ $0$ $L_{3}$ $0$

4 - *

$-90+\theta_{4}$ $0$ $0$ $-90$

*- 5

$\theta_{5}$ $L_4-\frac{h_5}{2}$ $0$ $0$

5 - H

$0$ $\frac{h_5}{2}$ $0$ $0$

因为把各个关节的坐标系位置都定在了圆柱的中心，所以需要考虑关节1、关节5的高度。其次，关节1在全局坐标系中的位置应为： $[0, 0, \frac{h_1}{2}]$

根据5个关节的旋转角 + DH参数表，即可得到各个关节的坐标系，利用坐标系信息可完成图像的绘制

拟定义一个类，使其在输入5个关节的旋转角时，可完成绘制工作

环境设置

这次实验主要用到了 numpy、matplotlib，另外还用到了我自己编写的一个模块 coord.py

coord.py 中的 CoordSys_3d 用于描述齐次坐标系，并为图形的仿射变换提供了接口，源代码位于：https://blog.csdn.net/qq_55745968/article/details/129912954

from functools import partial import matplotlib.pyplot as plt import numpy as np # coord.py 详见: https://blog.csdn.net/qq_55745968/article/details/129912954 from coord import CoordSys_3d rot = CoordSys_3d.rot trans = CoordSys_3d.trans red = 'orangered' orange = 'orange' yellow = 'yellow' green = 'greenyellow' cyan = 'aqua' blue = 'deepskyblue' purple = 'mediumpurple' pink = 'violet' ROUND_EDGE = 30 # 圆等效多边形边数 DTYPE = np.float16 # 矩阵使用的数据类型 # 三个相同关节的尺寸参数 COMMON_HEIGHT = 20 COMMON_RADIUS_OUT = 10 COMMON_RADIUS_IN = 3 # 连杆外径 CONNECT_ROD_RADIUS = COMMON_RADIUS_OUT / 3 # 各个关节的尺寸参数 JOINTS_SHAPE = [[COMMON_HEIGHT * 0.8, COMMON_RADIUS_IN * 2, COMMON_RADIUS_OUT, 'Wistia_r', 10], [COMMON_RADIUS_OUT, COMMON_RADIUS_IN, COMMON_HEIGHT, 'cool', 2], [COMMON_RADIUS_OUT, COMMON_RADIUS_IN, COMMON_HEIGHT, 'cool', 2], [COMMON_RADIUS_OUT, COMMON_RADIUS_IN, COMMON_HEIGHT, 'cool', 2], [None for _ in range(5)], [COMMON_RADIUS_OUT * 0.8, COMMON_RADIUS_IN, COMMON_HEIGHT * 0.8, 'cool', 10]] PACE_NUM = 20 # 转动范围分解步数 THETA = np.linspace(-90, 90, PACE_NUM) def DH_PARAM(theta): ''' DH 参数表''' return [[theta[0], 56 - JOINTS_SHAPE[0][2] / 2, 0, 90], # 关节 1 -> 关节 2 [90 + theta[1], 0, 43, 0], # 关节 2 -> 关节 3 [theta[2], 0, 43, 0], # 关节 3 -> 关节 4 [-90 + theta[3], 0, 0, -90], # 关节 4 -> 关节 5 [theta[4], 45.5 - JOINTS_SHAPE[-1][2] / 2, 0, 0]]

绘图函数 def figure3d(): ''' 创建3d工作站''' figure = plt.subplot(projection='3d') tuple(getattr(figure, f'set_{i}label')(i) for i in 'xyz') return figure def cylinder(figure, state: CoordSys_3d, R: float, h: float, r: float = 0, axis: int = 2, smooth: int = 2): ''' 以 state 的 z 轴为主轴绘制圆柱 figure: 3D 工作站对象 state: CoordSys_3d 齐次坐标系 R: 圆柱底面外径 r: 圆柱底面内径 h: 圆柱高度 axis: 圆柱两底面圆心连线所在的轴索引 smooth: 图像细致程度 (至少 2)''' # 当主轴为 x,y 时, 对坐标系进行变换 if axis < 2: rotate = CoordSys_3d.rot(90, 'y' if axis == 0 else 'x') state = state.rela_tf(rotate) func = [] theta = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE) z = np.linspace(-h / 2, h / 2, smooth, dtype=DTYPE) theta, z = np.meshgrid(theta, z) # 绘制圆柱内外曲面: 以 z 轴为主轴, 原点为中心 x, y = np.cos(theta), np.sin(theta) func.append(partial(figure.plot_surface, *state.apply(x * R, y * R, z))) func.append(partial(figure.plot_surface, *state.apply(x * r, y * r, z))) phi = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE) radius = np.linspace(r, R, 2, dtype=DTYPE) phi, radius = np.meshgrid(phi, radius) # 绘制上下两底面: 法向量为 z 轴, 原点为中心, 在 z 轴上偏移得到两底面 x, y = np.cos(phi) * radius, np.sin(phi) * radius z = np.zeros_like(x) for dz in (-h / 2, h / 2): s = state.rela_tf(CoordSys_3d.trans(dz=dz)) func.append(partial(figure.plot_surface, *s.apply(x, y, z))) # 返回函数流的执行函数 return lambda cmap: tuple(f(cmap=cmap) for f in func)

机械臂对象

每次工作状态改变时 (即机械臂运动)，保存机械臂末端的点坐标，绘制成工作空间。在这个过程中，通过 numpy 判断新的工作点是否与旧的工作点重叠，并只保存不重叠的工作点

class Robot_Arm: ''' 机械臂对象''' fig = figure3d() state = CoordSys_3d().rela_tf(trans(0, 0, JOINTS_SHAPE[0][2] / 2)) def __init__(self): self.restart() def restart(self): ''' 重启工作空间记录器''' self.work_space = np.ones([0, 3]) def refresh(self, theta=[0] * 5, cla=True, draw_body=True, keep_all=False): ''' 根据旋转角度刷新画面 theta: 机械臂各个关节的旋转角 cla: 刷新时清空画布 draw_body: 绘制机械臂 keep_all: 不筛除重复工作点''' joints = self.search(theta) if cla: plt.cla() self.fig.view_init(elev=5, azim=-90) # 设置 3D 工作站边界 for set_ in self.fig.set_xlim3d, self.fig.set_ylim3d: set_(-150, 150) self.fig.set_zlim3d(-50, 200) plt.tight_layout() if draw_body: # 绘制关节及其坐标系 for (R, r, h, cmap, smooth), joint in zip(JOINTS_SHAPE, joints): if R: cylinder(self.fig, joint, R=R, r=r, h=h, axis=2, smooth=smooth)(cmap) joint.plot_coord_sys(linewidth=2, length=30) # 绘制连杆 for axis, length, rear in zip([1, 0, 0, None, 2], [56, 43, 43, 0, 45.5], joints[1:]): if length: move = -length / 2 connect_rod = rear.rela_tf(trans(*map(lambda i: (axis == i) * move, range(3)))) cylinder(self.fig, connect_rod, CONNECT_ROD_RADIUS, length, axis=axis)('winter_r') tail = joints[-1].rela_tf(trans(0, 0, JOINTS_SHAPE[-1][2] / 2)).position.reshape(1, -1) # 检测新工作点是否与旧工作点重叠 if keep_all or np.all(((tail - self.work_space) ** 2).sum(axis=1) > 18): self.work_space = np.append(self.work_space, tail, axis=0) if cla: self.fig.scatter(*map(lambda i: self.work_space[:, i], range(3)), c=red, alpha=0.4, s=10) plt.pause(0.001) def search(self, theta): ''' 搜索关节的位置''' joints = [self.state] for rot_z, trans_z, trans_x, rot_x in DH_PARAM(theta): joints.append(joints[-1].rela_tf(rot(rot_z, 'z') ).rela_tf(trans(trans_x, 0, trans_z) ).rela_tf(rot(rot_x, 'x'))) return joints

工作空间绘制 def draw_work_space(): ''' 绘制工作空间剖面轮廓''' out_points = [] in_points = [] # 绘制外圆上半部分 for t in THETA: theta = [0 for _ in range(5)] theta[1] = t arm.refresh(theta) out_points.append(arm.work_space) # 绘制外圆下半部分 for c in 90, -90: arm.restart() for t in THETA: arm.refresh([0, c, t, 0, 0]) out_points.append(arm.work_space) # 绘制外圆中下部分 for c in 90, -90: arm.restart() for t in THETA: arm.refresh([0, c, c, t, 0]) out_points.append(arm.work_space) # 绘制内圆 for c in 90, -90: arm.restart() for t in THETA: arm.refresh([0, t, c, c, 0]) in_points.append(arm.work_space) # 绘制轨迹 arm.restart() arm.refresh() for line in out_points + in_points: loc = list(map(lambda i: line[:, i], range(3))) arm.fig.plot(*loc, c=red, linewidth=2, alpha=0.7) arm.fig.scatter(*loc, c=red, alpha=0.4, s=10) def draw_point_clound(): ''' 绘制工作空间点云''' for a in THETA: for b in THETA: for c in THETA: for d in THETA: arm.refresh([a, b, c, d, 0], draw_body=False, cla=False, keep_all=True) arm.refresh() arm = Robot_Arm() draw_work_space() plt.show()

最终结果

当5个关节的转动角均为0时，机械臂处在工作原点 (如下图所示)。对于这5个旋转关节而言，其z轴 (粉色轴) 均处在其右手规则旋转的方向上；其x轴 (橙色轴) 均处在其z轴与上一个关节的z轴的公垂线方向上，满足DH表示法的要求