机器人工具箱 Robotics Toolbox

大约 15 分钟

机器人工具箱 Robotics Toolbox

为 matlab 下机器人工具箱的 Python 版本

安装

通过 pip install roboticstoolbox-python 安装
参考文档 https://petercorke.github.io/robotics-toolbox-python/intro.htmlopen in new window

模块导入

使用以下命令导入模块
作为辅助, 通常还会导入模块 matplotlib, numpy 与 spatialmath.base (空间运算)

import roboticstoolbox as rtb
import numpy as np
import spatialmath.base as stm
import matplotlib.pyplot as plt

使用注意

若安装模块失败, 需要安装 VS2019 及其 C++ 运行组件
无法在 Jupyter 环境下查看机器人
python 中的矩阵乘法应使用运算符 @

机械臂建模

关节位置描述

标准型

基座 (关节 $0$ ) 处以水平向右为 $x$ 轴正方向, 以竖直向上为 $z$ 轴正方向
之后的关节均以上一个关节作为起点进行变换
关节的 $z$ 轴为关节的旋转方向

使用参数 $\theta,d,a,\alpha$ 描述两个关节之间的相对位置

$\theta$ 表示将基准坐标系的 $z$ 轴旋转弧度
$d$ 表示沿旋转后的坐标系的 $z$ 轴正方向移动距离
$a$ 表示沿移动后的坐标系的 $x$ 轴正方向移动距离
$\alpha$ 表示将移动后坐标系的 $x$ 轴旋转弧度

注意

当关节的旋转方向与连杆方向平行时, 关节可以在连杆上任意位置

改进型

与标准型相同, 但是转换顺序改变, 对于改进型描述使用 $\alpha,a,\theta,d$ 的顺序即

首先沿 $x$ 轴旋转 $\alpha$
再沿 $x$ 轴正方向移动 $a$
然后沿 $z$ 轴旋转 $\theta$
最后沿 $z$ 轴正方向移动 $d$

建模编程

确定位置 (定义连杆)

使用函数 rtb.DHLink 确定关节位置 (即定义连杆)

函数原型 L = rtb.DHLink(d, alpha, theta, a, sigma, mdh, offset, flip, qlim)
参数
- theta, d, a, alpha 实数
  即关节相对位置的描述参数
- sigma 实数 0, 1
  关节类型
  - 0 表明旋转关节
  - 1 表明移动关节
- mdh 布尔值
  建模方式
  - True 标准型建模, 默认值
  - False 改进型建模
- offset 实数
  关节偏置
  - 对于旋转关节, theta 应取 0, 并将其作为偏置 offset = theta
  - 对于移动关节, d 应取 0, 并将其作为偏置 offset = d
- flip 布尔值
  关节旋转方向, 取 True 时表示关节反向旋转
- qlim 2 元素列表
  关节限位, 第一个元素为负限位, 第二个元素为正限位
返回值
返回创建的连杆对象

创建机械壁 (串联连杆)

使用函数 rtb.SerialLink 创建机械臂 (即串联连杆)

函数原型 SL = SerialLink(LINKS, name =)
参数
- LINKS 连杆对象列表
  按机器人连杆顺序组成的列表
- name 字符串
  机械臂名称
返回值返回创建的机械臂对象

设置基座

默认情况下, 第 $0$ 个关节即基座, 不通过 Link 定义
而是设置第 $1$ 个关节相对基座的位置
设置位置使用如下代码

SL.base = stm.transl(x, y, z)

SL.base 为机械臂对象的 base 成员
transl 为创建一个平移变换矩阵

显示模型

通过调用机械臂对象的 SL.teach(None) 成员函数, 绘制创建好的机械臂模型

建模例程

L = []
L.append(rtb.DHLink(0.216, np.pi / 2, 0, 0, 0, offset = 0))
L.append(rtb.DHLink(0, 0, 0, 0.5, 0, offset = np.pi / 2)) # type: ignore
L.append(rtb.DHLink(0, 0, 0, np.sqrt(0.145 ** 2 + 0.4246 ** 2), 0, offset = -np.arctan(427.46 / 145)))
L.append(rtb.DHLink(0, np.pi / 2, 0, 0, 0, offset = np.arctan(427.46 / 145)))
L.append(rtb.DHLink(0.258, 0, 0, 0, 0, offset = 0))

Five_dof = rtb.SerialLink(L, name = '5-dof')
Five_dof.base = stm.transl(0, 0, 0.28)
Five_dof.teach(None)

空间数学

概念解释

欧拉角

欧拉角使用 z-y-z 的顺序旋转物体, 采用的坐标系为旋转时的物体坐标系
首先物体绕 z 轴旋转, 之后绕 z 轴旋转后的 y 轴旋转, 最后绕 z, y 旋转后的 z 轴旋转
因此相当于

R=I\cdot R_z\cdot R_y\cdot R_z

即原始坐标系分别右乘 (每次旋转改变原始坐标系, 即矩阵的列) $z,y,z$ 轴上的旋转矩阵

方位角

方位角使用 x-y-z 的顺序旋转物体, 采用的坐标系为绝对坐标系物体依次按 x (横滚), y (俯仰), z (航偏) 的顺序旋转, 始终以绝对坐标系的轴为基准
因此相当于

R=R_z\cdot R_y\cdot R_x\cdot I

即原始坐标系分别左乘 (每次旋转基于原始坐标系) $x,y,z$ 轴上的旋转矩阵

变换矩阵

对于一般的 $3\times 3$ 的矩阵仅能体现坐标轴, 而无法体现物体的位置变换

因此将矩阵扩展到 $4\times 4$ , 将扩展的部分用于保存物体的位置变换信息, 成为变换矩阵
参考资料 https://zhuanlan.zhihu.com/p/356878461open in new window

变换矩阵在右乘 $A$ 时, 相当于在物体 $A$ 的坐标系的基础上进行变换 (移动或旋转)
变换矩阵在左乘 $A$ 时, 相当于在绝对坐标系的基础上对 $A$ 进行变换 (移动或旋转)

例如以下代码 R1 = stm.eul2tr(30, 60, 90, 'deg') @ stm.transl(1, 2, 3) 可使用以下方式理解

先以欧拉角在原始坐标系基础上旋转, 然后以物体坐标系为基准移动 (右乘)
先移动到绝对坐标 $1,2,3$ , 然后以绝对坐标系为中心旋转 (左乘)

数学函数

单轴旋转矩阵

使用函数 stm.rotx/y/z 获取单轴旋转矩阵 (此处以 stm.rotx 为例)

函数原型 r = stm.rotx(phi, unit)
参数
- phi 实数
  沿 x 轴的旋转角度, 默认使用弧度制
- unit 字符串
  角度单位, rad 表示弧度制 (默认), deg 表示角度制
返回值
对应的旋转矩阵
注意
通过函数 stm.trox/y/z 可用于获取相应的变换矩阵, 其余语法同

欧拉角旋转矩阵

使用函数 stm.eul2r 获取特定欧拉角对应的旋转矩阵

函数原型 r = stm.eul2r(z, y, x, unit)
参数
- z, y, x 实数
  欧拉角的旋转角度, 使用弧度制
- unit 字符串
  角度单位, rad 表示弧度制 (默认), deg 表示角度制
返回值对应的旋转矩阵, 相当于 stm.eul2r(z, y, x) = stm.rotz(z) @ stm.roty(y) @ stm.rotz(x)
注意
通过函数 stm.eul2tr 可用于获取相应的变换矩阵, 其余语法同

方位角旋转矩阵

使用函数 stm.rpy2r 获取特定方位角对应的旋转矩阵

函数原型 r = stm.rpy2r(x, y, z, unit)
参数
- x, y, z 实数
  方位角的旋转角度, 使用弧度制
- unit 字符串
  角度单位, rad 表示弧度制 (默认), deg 表示角度制
返回值对应的旋转矩阵, 相当于 stm.rpy2r(z, y, x) = stm.rotz(z) @ stm.roty(y) @ stm.rotx(x)
注意
通过函数 stm.rpy2tr 可用于获取相应的变换矩阵, 其余语法同

移动变换矩阵

使用函数 stm.transl 获取移动变换矩阵, 或获取变换矩阵的移动量 (相对原点)

函数原型 t = stm.transl(x, y, z)
参数
- x, y, z 实数
  沿 x, y, z 方向移动距离
返回值
对应的变换矩阵
注意
该函数也可用于求解变换矩阵的移动量

旋转矩阵转换为变换矩阵

使用函数 stm.r2t 将旋转矩阵转换为变换矩阵

函数原型 t = stm.r2t(r)
参数
- r 3 x 3 的旋转矩阵
返回值对应的 4 x 4 变换矩阵

变换矩阵转换为旋转矩阵

使用函数 stm.t2r 将变换矩阵转换为旋转矩阵

函数原型 r = stm.t2r(t)
参数
- t 4 x 4 的变换矩阵
返回值对应的 3 x 3 旋转矩阵

由变换矩阵获取欧拉角

使用函数 stm.tr2eul 获取变换矩阵对应的欧拉角

函数原型 r = stm.tr2eul(tr, unit)
参数
- tr 4 x 4 的变换矩阵
  用于求解的变换矩阵, 对于旋转矩阵, 可先使用 stm.r2t 转换为变换矩阵
- unit 字符串
  角度单位, rad 表示弧度制 (默认), deg 表示角度制
返回值 3 元素列表
变换矩阵对应的欧拉角
注意
通过函数 stm.tr2rpy 可用于获取变换矩阵的方向角, 其余语法同
使用变换矩阵类的成员函数 t.eul(unit) / t.rpy(unit) 也可用于获取欧拉角 / 方位角

获取变换矩阵的移动量

使用函数 stm.transl 获取变换矩阵相对原点的移动量

函数原型 v = stm.transl(t)
参数
- t 变换矩阵
返回值 3 元素列表
一个包含了三个坐标值的列表

矩阵对象

在使用空间数学库 spatialmath 中有两种变换矩阵形式 SO3 与 SE3

SO3 为 3 x 3 的旋转矩阵
SE3 为 4 x 4 的三维物体变换矩阵

对于以上数学函数的返回值均为一般矩阵, 即 numpy.ndarray 类型 (笔记中默认的矩阵类型)
对于机器人工具箱的返回值与参数均为 SE3 / SO3

对于 numpy.ndarray 类型与 SE3 / SO3 类型均可以使用 @ 作为矩阵乘法运算

转换为变换矩阵

通过 from spatialmath import SE3, SO3 可导入这两种矩阵的实例

使用 4 x 4 (3 x 3) 的 numpy 数组可用于实例化 SE3 (SO3)

成员函数

对于变换矩阵 se : SE3 有如下常用成员函数

se.t 成员变量, 三元素列表
获取前端坐标, 列表中三个元素分别对应 x, y, z 坐标
se.rpy(unit) 成员函数, 返回三元素列表
获取前端方位角, 列表中三个元素分别对应 x, y, z 角度
se.eul(unit) 成员函数, 返回三元素列表
获取前端欧拉角, 列表中三个元素分别对应 z, y, z 角度
se.A 4 x 4 变换矩阵 (numpy 数组)
获取前端姿态对应的变换矩阵

串联机械臂操作

通过串联机械臂对象 SerialLink 的成员函数对机械臂进行操作

示教

基本示教

使用函数 SL.teach() 对机器人进行示教

函数原型 SL.teach(q)
参数
- q n 个元素的列表或 None
  n 为机械臂的关节数, 通过此列表规定机器人的初始位置

机械臂动画

使用函数 env = SL.plot() 展示机器人模型动画

函数原型 SL.plot(q, dt = 0.05)
参数
- q m x n 的数组
  n 为机械臂的关节数, m 为运动的关节坐标数
- dt 实数
  机器人运动动画的时间间隔
返回值
- env 绘图环境对象
注意
可通过绘图环境对象获取图像的信息, 对图像进一步操作
- env.ax 获取图像的 Axes 对象
- env.fig 获取图像的 Figure 对象
- env.hold() 保持图像 (展示图像结果时使用)

例子 : 工作空间可视化

假设有六轴机器人对象 robot

env = robot.plot([0, 0, 0, 0, 0, 0])
# 绘制 3000 个点
times = 3000
res = np.zeros([times, 3])

for i in np.arange(times) :
    # 通过 robot 的成员函数 random_q() 获取一个随机的合法关节坐标
    q = robot.random_q()
    # 正向求解机械臂前端姿态
    se = robot.fkine(q)
    # 保存该随机姿态的末端坐标
    res[i, 0 : 3] = se.t

res = res.T
# 通过绘图环境成员获取 Axes 对象, 并在此对象上继续绘制点云
env.ax.scatter(res[0], res[1], res[2], s = 5) # type: ignore
# 展示结果
env.hold()

基本运动学

正向求解

正向求解即给出机械臂各个关节坐标, 求解机械臂最前端相对绝对坐标的状态 (变换矩阵)

使用函数 se = SL.fkine() 正向求解

函数原型 se = SL.fkine(q)
参数
- q n 个元素的列表
  n 为机械臂的关节数, 机器人的初始位置
返回值
SE3 类型的变换矩阵

反向求解

反向求解即给出机械臂最前端相对绝对坐标的状态 (变换矩阵), 求解出机械臂的各个关节坐标

使用函数 SL.ikine_XX() 正向求解, 其中 XX 为求解器, 可以是 LM, QP, GN, NR 区别见文档
https://petercorke.github.io/robotics-toolbox-python/IK/ik.html#ikopen in new window

函数原型 iks = SL.ikine_XX(t, mask = [1, 1, 1, 1, 1, 1])
参数
- t SE3 类型的变换矩阵或 4 x 4 的 numpy.ndarray
  目标机械臂末端姿态
  通常使用时仅规定了机械臂的末端坐标, 此时机器人的前端将朝上 (绝对 z 轴). 因此最好左乘 roty(180, 'deg'), 是机械臂前端朝下
- mask 6 元素列表
  启用的自由度, 分别表示 x, y, z 方向的平动与旋转用于欠自由度机器人, 欠自由度机器人需要屏蔽部分无用的自由度才能进行反向求解
返回值反向求解结果对象 iks
- iks.q 反向求解的机械臂关节坐标
- iks.success 求解是否成功
- iks.residual 求解结果误差
- iks.reason 求解失败原因

轨迹规划

时间位移插值

即确定位移关于时间的函数 $x=f(t)$
通过合理的规划, 避免物体的速度与加速度出现突变, 防止冲击

五次多项式轨迹插值

使用五次多项式的形式规划轨迹, 同时要求初末位置的加速度与速度为 0

函数原型 traj = rtb.quintic(q0, qt, t)
参数
- q0, qt 实数
  初始与末尾的位移
- t 整数
  采样点数 (移动时间)
返回值轨迹对象 traj
- traj.q/qd/qdd t 元素数组, 各个时间间隔下的位置, 速度, 加速度
- traj.plot() 绘制出轨迹的位移, 速度, 加速度图像

五次多项式轨迹函数

同五次多项式轨迹, 但将返回一个可调用对象 (函数)

函数原型 func = rtb.quintic_func(q0, qt, T)
参数
- q0, qt 实数
  初始与末尾的位移
- T 实数
  移动时间
返回值可调用对象 func(T)
通过给出时间, 将返回一个三元素元组, 分别是此时的位置, 速度, 加速度

梯形速度轨迹插值

使用梯形速度的形式规划轨迹, 可以保证机器人尽量在最大速度运行, 但加速度存在突变

函数原型 traj = rtb.trapezoidal(q0, qt, t, V)
参数
- q0, qt 实数
  初始与末尾的位移
- t 实数
  移动时间
- V 实数
  最大速度
返回值轨迹对象 traj
注意
梯形速度轨迹也有函数版本 traj = rtb.trapezoidal_func(q0, qt, t, V), 使用方法与五次多项式轨迹函数类似

多维轨迹插值

对多维位移同时进行插值, 实现点到点的路线规划

函数原型 traj = rtb.mtraj(tfunc, q0, qf, t)
参数
- tfunc 可调用对象
  插值策略, 通常使用 rtb.quintic 或 rtb.trapezoidal
- q0, qf n 元素列表
  起始坐标与终点坐标
- t 整数
  采样点数 (规划时间)
返回值轨迹对象 traj
此时轨迹 traj.q 为一个 n x t 的数组

多段轨迹插值

对多维位移同时移动, 并且带有一定速度经过多个点的路线规划

函数原型 traj = rtb.mstraj(viapoints, dt, tacc)
参数
- viaplot n x m 数组
  由经过点组成的数组, n 为控制维度, m 为经过点个数 (按行排列经过点)
- dt 实数
  时间间隔
- tacc 实数
  最大加速度
返回值轨迹对象 traj

姿态插值

除了对坐标插值, 还可直接对变换矩阵 (姿态) 插值

函数原型 t_arr = rtb.ctraj(T0, T1, s = s)
参数
- T0 4 x 4 变换矩阵 (SE3 类型)
  起点姿态
- T1 4 x 4 变换矩阵 (SE3 类型)
  终点姿态
- s n 元素数组各时刻的采样位置 (使用百分比表示位置, 即 $[0, 1]$ 内的值表示)
  s 可通过五次多项式轨迹插值或梯形速度轨迹插值生成, 以实现较好的轨迹规划
  即使用 s = rtb.quintic(0, 1, time).q (注意使用成员 q 导出轨迹对应的数组)
返回值 n 元素的变换矩阵数组

轨迹规划思路

关节插值 - 点到点轨迹

# 根据初末位置的姿态求出对应的变换矩阵
T0 = stm.transl(x0, y0, z0) @ stm.rpy2tr(rz0, ry0 ,rx0)
T1 = stm.transl(x1, y1, z1) @ stm.rpy2tr(rz1, ry1 ,rx1)

# 直接求出初末位置关节坐标
q0 = s.ikine_XX(T0)
q1 = s.ikine_XX(T1)

# 通过对初末位置的插值得到轨迹
Qs = rtb.mtraj(rtb.quintic, q0.q, q1.q, time)

关节插值能保证轨迹一定连续且运动不会跳变, 运算量小
通常无法得到直线轨迹

坐标插值 - 直线轨迹

# 首先对坐标插值 (使用六元素数组 P0/1 保存姿态信息)
traj = rtb.mtraj(rtb.quintic, P0, P1, time)

# 求解出各个姿态下对应的变换矩阵
Ts = np.zeros([time, 4, 4])
for i in np.arange(0, time) :
    tr = stm.transl(traj.q[0], traj.q[1], traj.q[2]) @ stm.rpy2tr(traj.q[3], traj.q[4] ,traj.q[5])
    Ts[i] = tr

# 求解各个姿态下的变换矩阵
Qs = s.ikine_XX(Ts)

可以保证直线轨迹, 但可能存在跳变或求解失败
到达目标角度时可以有两个不同的旋转方向, 需要具体确认 (可对目标角度 $\pm 360$ 进行改变)

变换矩阵插值 - 直线轨迹

# 根据初末位置的姿态求出对应的变换矩阵
T0 = SE3(stm.transl(x0, y0, z0) @ stm.rpy2tr(rz0, ry0 ,rx0))
T1 = SE3(stm.transl(x1, y1, z1) @ stm.rpy2tr(rz1, ry1 ,rx1))

# 对变换矩阵进行插值
Ts = rtb.ctraj(T0, T1, s = rtb.quintic(0, 1, time).q)

# 求解各个姿态下的变换矩阵
Qs = s.ikine_XX(Ts)

可以保证直线轨迹, 但可能存在跳变或求解失败

机器人工具箱 Robotics Toolbox

# 机器人工具箱 Robotics Toolbox

# 安装

# 模块导入

# 使用注意

# 机械臂建模

# 关节位置描述

# 标准型

# 改进型

# 建模编程

# 确定位置 (定义连杆)

# 创建机械壁 (串联连杆)

# 设置基座

# 显示模型

# 建模例程

# 空间数学

# 概念解释

# 欧拉角

# 方位角

# 变换矩阵

# 数学函数

# 单轴旋转矩阵

# 欧拉角旋转矩阵

# 方位角旋转矩阵

# 移动变换矩阵

# 旋转矩阵转换为变换矩阵

# 变换矩阵转换为旋转矩阵

# 由变换矩阵获取欧拉角

# 获取变换矩阵的移动量

# 矩阵对象

# 转换为变换矩阵

# 成员函数

# 串联机械臂操作

# 示教

# 基本示教

# 机械臂动画

# 例子 : 工作空间可视化

# 基本运动学

# 正向求解

# 反向求解

# 轨迹规划

# 时间位移插值

# 五次多项式轨迹插值

# 五次多项式轨迹函数

# 梯形速度轨迹插值

# 多维轨迹插值

# 多段轨迹插值

# 姿态插值

# 轨迹规划思路

# 关节插值 - 点到点轨迹

# 坐标插值 - 直线轨迹

# 变换矩阵插值 - 直线轨迹

# 多点轨迹规划

机器人工具箱 Robotics Toolbox

安装

模块导入

使用注意

机械臂建模

关节位置描述

标准型

改进型

建模编程

确定位置 (定义连杆)

创建机械壁 (串联连杆)

设置基座

显示模型

建模例程

空间数学

概念解释

欧拉角

方位角

变换矩阵

数学函数

单轴旋转矩阵

欧拉角旋转矩阵

方位角旋转矩阵

移动变换矩阵

旋转矩阵转换为变换矩阵

变换矩阵转换为旋转矩阵

由变换矩阵获取欧拉角

获取变换矩阵的移动量

矩阵对象

转换为变换矩阵

成员函数

串联机械臂操作

示教

基本示教

机械臂动画

例子 : 工作空间可视化

基本运动学

正向求解

反向求解

轨迹规划

时间位移插值

五次多项式轨迹插值

五次多项式轨迹函数

梯形速度轨迹插值

多维轨迹插值

多段轨迹插值

姿态插值

轨迹规划思路

关节插值 - 点到点轨迹

坐标插值 - 直线轨迹

变换矩阵插值 - 直线轨迹

多点轨迹规划