视觉引导的机器人关节空间B样条轨迹规划方法

摘要

本发明涉及一种视觉引导的机器人关节空间B样条轨迹规划方法，其包括如下步骤：第一步、在立体支架内安装二自由度机器人，在前端安装工业相机，传送带的运动方向垂直于二自由度机器人的运动平面；第二步、在工业相机获取到传送带上第一个轨迹点后，并在二自由度机器人运动到第一个轨迹点的时间内，根据得到若干关节时间节点序列，以构造B样条曲线；第三步、采用增加节点向量和控制顶点的方式来延长上述构造的B样条曲线，以使得B样条曲线经过增加的关节位置点；第四步、采用德布尔递推算法计算B样条曲线上的位置点，以驱动二自由度机器人运动。本发明实现机器人在视觉引导下的平滑运动，提高机器人轨迹跟踪的精度。

说明书

技术领域

本发明涉及一种B样条轨迹规划方法，尤其是一种视觉引导的机器人关节 空间B样条轨迹规划方法，属于机器人关节空间轨迹规划的技术领域。

背景技术

自1962年第一台工业机器人应用于General Motor公司的生产线以来，机器 人技术不断发展、完善和成熟，目前已广泛应用于制造业、服务业、娱乐业、 科学探索、医学、军事和航天等各个领域。工业机器人可以代替人类完成大量 高质量要求的工作，如汽车制造，电产品制造，工及食品包装，物料的搬运等。 汽车制造业利用机器人在汽车自动化生产线完成焊接、喷漆、装配等工作。在 服务业中，各种服务用智能机器人如导游机器人、护士助手、步行功能训练机 器人、导盲机器人等已逐步进入市场。21世纪的服务用智能机器人除了可以为 家庭从事简单的家务劳动和健康护理工作外，还可以从事一些劳动强度大，工 作重复性强的工作，如环境保护、打扫以及公共交通服务等。在医学方面机器 人主要作为医疗设备辅助医生和医务工作者从事医疗、诊断、医学研究和教学。 如远程手术机器人、医用微型机器人等。机器人在军事领域的应用包括代替士 兵执行拆除炸弹、扫雷、侦察和攻击等各种任务。20世纪70年代，美国率先提 出了再宇宙空间利用机器人系统的概念，旨在通过机器人的应用进行设备的组 装、回收、维修，以及在极其恶劣的空间环境下完成一些人类难以完成的舱外 活动。目前，空间机器人研究只要集中在卫星、航天飞机以及国际空间站上使 用的空间机器人。

目前，国内外机器人路径规划大体可以分为三类：人工示教、离线编程和 自主示教。在结构化环境下，目标轨迹的几何形状和位置以及它的周围环境是 已知的，可以利用计算机的离线编程技术进行路径规划；而对于复杂路径和非 结构化环境下的轨迹，仍然采用现场人工示教的方法进行路径规划，这是目前 应用最多的方法。示教方式虽然操作简单、重复性好，避免了复杂的离线编程， 但是它存在示教工作繁重、轨迹跟踪精度低、无法应对过程中可能发生的变法 等缺点。随着机器人智能化水平的不断提高和机器视觉技术的日益成熟，人们 开始研究以视觉测量信息作为反馈，由计算机控制机器人进行路径规划的自主 示教。机器人关节空间轨迹规划是机器人轨迹跟踪的基础，传统的在人工示教 下采用B样条方法规划关节空间轨迹，能够保证运动连续平滑；但是在自主示 教方式下，示教点是通过视觉传感器动态获得的，传统的样条规划方法无法解 决。

发明内容

本发明的目的是克服现有技术中存在的不足，提供一种视觉引导的机器人 关节空间B样条轨迹规划方法，其操作方便，实现机器人在视觉引导下的平滑 运动，提高机器人轨迹跟踪的精度。

按照本发明提供的技术方案，所述视觉引导的机器人关节空间B样条轨迹 规划方法，所述机器人关节空间B样条轨迹规划方法包括如下步骤：

第一步、在立体支架内安装二自由度机器人，所述二自由度机器人能在立 体支架内运动；在立体支架的前端采用全局方式安装工业相机，立体支架内设 置传送带，传送带的运动方向垂直于二自由度机器人的运动平面；在工业相机 所在的位置建立相机坐标系，二自由度机器人邻近传送带的端部建立机器人末 端工具坐标系；

第二步、传送带在立体支架内运动，通过工业相机获取传送带上的轨迹点； 在工业相机获取到传送带上第一个轨迹点后，驱动二自由度机器人运动到所述 获取第一个轨迹点；在二自由度机器人运到第一个轨迹点的时间内，同时工业 相机继续获取传送带的轨迹点；根据工业相机的采样频率、传送带运动的线速 度及相机坐标系与机器人末端工具坐标系之间的纵向距离，并对工业相机获取 的轨迹点进行逆运算，得到若干关节时间节点序列，以构造B样条曲线；

第三步、二自由度机器人依照上述构造的B样条曲线运动，工业相机继续 获取传送带上的轨迹点，并对所述轨迹点进行逆运算，得到增加的关节位置点； 采用增加节点向量和控制顶点的方式来延长上述构造的B样条曲线，以使得B 样条曲线经过增加的关节位置点；

第四步、根据上述得到的B样条曲线，采用德布尔递推算法计算B样条曲 线上的位置点，所述位置点作为伺服位置闭环控制的位置输入，以驱动二自由 度机器人运动，直至二自由度机器人运动终止。

所述第二步包括如下步骤：

2.1、在确认工业相机得到传送带上的第一个轨迹点时，控制二自由度机器 人以点到点的方式运行到上述第一个轨迹点；在二自由度机器人运动到第一个 轨迹点的时间内，对工业相机提取的传送带的轨迹点进行逆运算，得到关节时 间节点序列{p_i,t_i}i＝0,1,…,n，其中p_i为关节角度矢量，t_i为时间节点矢量，n为 关节时间节点序列的数量；

2.2、根据关节时间节点序列{p_i,t_i}i＝0,1,…,n，利用C_Nd=p方程计算B样条 曲线的控制顶点；其中，C_N表示，d表示控制顶点向量，p表示关节位置点向 量；

2.3、根据上述得到的关节时间节点序列{p_i,t_i}i＝0,1,…,n及控制顶点向量d， 构造B样条曲线，得到

$P\left(u\right)=\underset{j=i-k}{\overset{j}{\Sigma }}{d}_j{N}_{j,k}\left(u\right),u\in [u_i,u_{i+1}],j=i-k,i-k+1,...,i;$

其中，d_j是控制顶点，N_j,k(u)为k次规范B样条基函数，k表示B样条次数，i 表示B样条序号，u_i表示节点向量为非递减序列；P(u)表示样条曲线，u表示样 条节点值。

所述第三步中：增加的关节位置点为{p_τ,t_τ}(τ=n+1,n+2,…)，其中n为关节 时间节点序列的数量；根据增加的关节位置点计算得到增加的控制顶点，得到

$[N_{\tau ,k}\left(u_{\tau +k}\right),N_{\tau +1,k}\left(u_{\tau +k}\right),...,N_{\tau +k-1,k}\left(u_{\tau +k}\right)]\left(\begin{array}{c}{d}_{\tau }\\ d_{\tau +1}\\ .\\ .\\ .\\ d_{\tau +k-1}\end{array}\right)=q_{\tau }$

其中，q_τ表示增加关节位置点的关节角度向量。

所述德布尔递推算法的递推公式为：

$p\left(u\right)=\underset{j=i-k+1}{\overset{i}{\Sigma }}{d}_j^l{N}_{j,k-l}\left(u\right)=...=d_i^k,u_i\le u<u_{i+1}$

$d_j^l=\left(\begin{array}{cc}\left(\begin{array}{c}{d}_j\\ (1-{\alpha }_j^l)d_{j-1}^{l-1}+{\alpha }_j^l{d}_j^{l-1}\end{array}\right)& \left(\begin{array}{c}l=0\\ l=\mathrm{1,2},...k\\ j=i-k+l,...,i\end{array}\right)\end{array}\right);$

${\alpha }_j^l=\frac{u-u_j}{u_{j+k+1-l}-u_j};$

其中，和表示中间辅助变量。

本发明的优点：采用增加节点向量和控制顶点的方法动态规划二自由度机 器人关节空间B样条轨迹，能够保证在视觉引导的下的二自由度机器人轨迹跟 踪运动平滑且关节速度、加速度均连续，提高了视觉引导的机器人轨迹跟踪精 度。

附图说明

图1为本发明的结构示意图。

图2为本发明工作流程图。

具体实施方式

下面结合具体附图和实施例对本发明作进一步说明。

B样条轨迹规划即根据任务空间或关节空间离散的坐标点，采用样条曲线方 法将这些离散点连接成一条平滑曲线（轨迹）。关节空间B样条轨迹规划可以 解释为：首先用运动学反解将任务空间路径点“转换”成关节矢量角度值，然 后对每个关节的离散的角度值采用样条曲线方法拟合成一条平滑曲线，即为关 节空间轨迹。

本发明提供了一种视觉引导的二自由度并联机器人轨迹跟踪时关节空间B 样条轨迹规划方法，实现了机器人在视觉引导下的平滑运动，提高了机器人轨 迹跟踪的精度。

如图1和图2所示：本发明视觉引导的机器人关节空间B样条轨迹规划方 法包括如下步骤：

第一步、在立体支架130内安装二自由度机器人110，所述二自由度机器人 110能在立体支架130内运动；在立体支架130的前端采用全局方式安装工业相 机100，立体支架130内设置传送带120，传送带120的运动方向垂直于二自由 度机器人110的运动平面；在工业相机100所在的位置建立相机坐标系，二自 由度机器人110邻近传送带120的端部建立机器人末端工具坐标系；

通过上述设置后，完成视觉引导二自由度机器人110系统环境的搭建。相 机坐标系的原点与机器人末端工具坐标系的原点有一段纵向距离；工业相机100 获取传送带120上的轨迹点超前于二自由度机器人110的运动；从而在二自由 度机器人110在运动到工业相机100之前获取的轨迹点时，机器人控制器能够 完成与工业相机100的通信，二自由度机器人110的逆运动学计算、关节空间B 样条轨迹规划和控制二自由度机器人110运动信号的给出。相机坐标系指以相 机拍摄到的图像的一个顶点为原点的XY坐标系，机器人末端坐标系是指以二 自由度机器人末端工件为原点的XY坐标系。

第二步、传送带120在立体支架130内运动，通过工业相机100获取传送 带120上的轨迹点；在工业相机100获取到传送带120上第一个轨迹点后，驱 动二自由度机器人110运动到所述获取第一个轨迹点；在二自由度机器人110 运到第一个轨迹点的时间内，同时工业相机100继续获取传送带120的轨迹点； 根据工业相机100的采样频率、传送带120运动的线速度及相机坐标系与机器 人末端工具坐标系之间的纵向距离，并对工业相机100获取的轨迹点进行逆运 算，得到若干关节时间节点序列，以构造B样条曲线；

2.1、本发明实施例中以工业相机100的采样频率为50Hz，传送带120的线 速度为250mm/s，相机坐标系到机器人末端工具坐标系的纵向距离为300mm为 例说明。为了方便提取传送带120运动轨迹上的点坐标，固定相机坐标系Y方 向坐标值，由于相机拍摄到的是一个面，而实施时需要提取的是一个点坐标， 所以固定一个方向上的坐标，就能得到轨迹曲线在一个面上的点坐标。。在确认 工业相机100得到传送带120的轨迹起点时，即工业相机100获取传送带120 的第一个轨迹点时，控制二自由度机器人110以点到点的方式先运行到第一个 轨迹点处等待跟踪。对工业相机100提取传送带120运动轨迹上的每一个坐标 点进行逆运算，得到二自由度机器人110的关节空间的关节位置-时间节点序列 {p_i,t_i}i＝0,1,…,n，其中p_i为关节角度矢量，t_i为时间节点矢量，n为关节空间节 点个数；本发明实施例中根据工业相机100采样频率、传送带120的运动线速 度及相机坐标系到机器人末端工具坐标系的距离，经计算且考虑工业相机100 通讯时间和B样条规划时间，选取n=49；具体实施时，也可以选取其他数量的 关节空间节点数；

2.2、根据关节位置-时间节点序列{p_i,t_i}i＝1,2,…n，计算所需构造B样条曲 线的控制顶点，其中p_i为关节角度矢量，t_i为时间节点矢量。计算控制顶点时， 使得曲线分段连接点与B样条曲线的内部数据（所述内部数据即是B样条曲线 上的曲线值）点一致，因此，p_i点处有节点值u_k+i,i＝0,1,…,n。B样条曲线将由n+k 个控制顶点d_i(i=0,1,…,n+k-1)定义，则节点向量相应为U=[u₀,u₁,…,u_n+2k]。构造B 样条曲线要解决两个问题：1）、对数据点参数化，即确定与关节角度矢量p_i对 应的参数值u_k+i,i＝0,1,…,n；2）、列出以n+k个控制顶点为未知量的线性方程组。

已知工业相机100为定时触发，采样间隔为20ms，关节空间的节点在时间 上是均匀分布的，为了计算方便，且主要考虑到还要通过增加节点向量和控制 顶点来延长B样条曲线，采用均匀的节点向量来对数据点参数化。由于B样条 曲线在定义域上一点u_i处有L重节点的连续性不低于k-L阶，采用均匀节点向量 避免了重节点，保证了在增加节点以后曲线的连续性。则节点向量为： U=[0,1,…,n+2k]。

用于插值n+1个关节角度矢量p_i的k次B样条曲线方程可写为

$p\left(u\right)=\underset{j=i-k}{\overset{i}{\Sigma }}{d}_j{N}_{j,k}\left(u\right),u\in [u_i,u_{i+1}]⋐[u_k,u_{n+k}]$

将曲线定义域[u_k,u_n+k]内的节点值以此带入方程，得到满足条件的n+1个方 程：

$p\left(u_{i+k}\right)=\underset{j=i}{\overset{i+k}{\Sigma }}{d}_i{N}_{j,k}\left(u_{i+k}\right)=p_i,u_{i+k}\in [u_k,u_{n+k}],i=\mathrm{0,1},...n$

简化为：

$p\left(u_{i+k}\right)=\underset{j=i}{\overset{i+k-1}{\Sigma }}{d}_i{N}_{j,k}\left(u_{i+k}\right)=p_i,u_{i+k}\in [u_k,u_{n+k}],i=\mathrm{0,1},...n$

这是因为：

$N_{i+k,k}\left(u_{i+k}\right)=\frac{u_{i+k}-u_{i+k}}{u_{i+k+k}-u_{i+k}}{N}_{i+k,k-1}\left(u_{i+k}\right)+\frac{u_{i+k+k+1}-u_{i+k}}{u_{i+k+k+1}-u_{i+k+1}}{N}_{i+k+1,k-1}\left(u_{i+k}\right)=0$

对于k次B样条曲线还需要增加k-1个条件，通常由边界条件给定。B样条 曲线上一点处的r阶导数p^r(u)可按如下递推公式计算：

$p^r\left(u\right)=\underset{j=i-k+r}{\overset{i}{\Sigma }}{d}_j^r{N}_{j,k-r}\left(u\right),u_i\le u<u_{i+1}$

$d_j^l=\left(\begin{array}{cc}\left(\begin{array}{c}{d}_j\\ (k+1-l)\frac{d_j^{l-1}-d_{j-1}^{l-1}}{u_{j+k+1-l}-u_j}\end{array}\right)& \left(\begin{array}{c}l=0\\ l=\mathrm{1,2},...r\\ j=i-k+l,...,i\end{array}\right)\end{array}\right)$

因此有：

$p_0^{\prime }=p^{\prime }\left(u_k\right)=\underset{j=k-k+1}{\overset{k}{\Sigma }}{d}_j^1{N}_{j,k-1}\left(u_k\right)$

$=k\frac{d_1-d_0}{u_{k+1}-u_1}+k\frac{d_2-d_1}{u_{k+2}-u_2}+...+k\frac{d_k-d_{k-1}}{u_{k+2}-u_2}$

$=d_k-d_0$

$p_n^{\prime }=p^{\prime }\left(u_{n+k}\right)=\underset{j=n+k-1-k+1}{\overset{n+k-1}{\Sigma }}{d}_j^1{N}_{j,k-1}\left(u_{n+k}\right)=d_{n+k-1}-d_{n-1}$

同理可求得首末数据点的二阶、三阶导数等。上述导数公式的含义是求解B样 条曲线上一点u处的r阶导数p^r(u)，例如p′(u_k)即指曲线在u=u_k处的一阶导数； 其作用主要是：用n+1个关节角度矢量p_i不能求解出n+k个控制顶点矢量d_i，需 要补充k-1个边界条件，边界条件即为起点和终点的导数。

于是，可以联立n+k个方程式求出B样条插值曲线的控制顶点，求解方程为：

C_Nd=p

当构造三次B样条曲线时，则有：

d=[d₀ d₁…d_n d_n+1 d_n+2]^T

p=[p₀ p₁…p_n v_s v_e]^T

其中，v_s v_e为起始位置和终止位置设定的机器人关节角速度，C_N为行列值均为 （n+3）的方阵；。

2.3、根据上述得到的关节时间节点序列{p_i,t_i}i＝0,1,…,n及控制顶点向量d， 构造B样条曲线，B样条曲线方程为：

$p\left(u\right)=\underset{i=0}{\overset{n}{\Sigma }}{d}_i{N}_{i,k}\left(u\right),u_{\min }\le u\le u_{\max },2\le k\le n+1$

其中，d_i(i=0,1,…n)是n+1个控制顶点，N_i,k(u)(i＝0,1,…,n)为k次规范B样条基函 数，也称之为混合函数，u_min、u_max分别表示节点向量值u的最小值和最大值。 按照Cox-deBoor递归公式定义为：

$N_{i,k}\left(u\right)=\frac{u-u_i}{u_{i+k}-u_i}{N}_{i,k-1}\left(u\right)+\frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}{N}_{i+1,k-1}\left(u\right)$

式中，k表示B样条次数，i表示B样条序号，u_i表示节点向量，为非递减序列。 由B样条曲线性质可知，N_i，k(u)定在区间[u_i，u_i+k+1]上；B样条曲线的节点矢量为 U=[u₀,u₁,…,u_n+k+1]，且所生成的B样条曲线仅定义在节点子区间[u_k,u_n+1]上；在参 数u轴上的任意一点u∈[u_i，u_i+1]处，至多只有k+1个非零的k次B样条基函数 N_j，k(u)(j＝i-k,i-k+1,…,i)，其它k次B样条基函数在该处均为零，因此，B样条 曲线可以表示为：

$p\left(u\right)=\underset{j=i-k}{\overset{j}{\Sigma }}{d}_j{N}_{j,k}\left(u\right),u\in [u_i,u_{i+1}];$

因此，通过上述步骤构造出B样条曲线。

第三步、二自由度机器人110依照上述构造的B样条曲线运动，工业相机 100继续获取传送带120上的轨迹点，并对所述轨迹点进行逆运算，得到增加的 关节位置点；采用增加节点向量和控制顶点的方式来延长上述构造的B样条曲 线，以使得B样条曲线经过增加的关节位置点；

经第二步在起始一段距离内对二自由度机器人110各关节规划了关节空间 B样条轨迹，传送带120继续运动，工业相机100也会连续的捕获传送带120 运动轨迹上新的轨迹点坐标，同样，新增的轨迹点坐标经过坐标转换和逆运算， 所述逆运算指根据机器人末端坐标计算对应的各个关节的关节角度值。得到各 关节坐标，即关节空间B样条曲线的轨迹末端新增了插值点。B样条曲线允许 通过增加节点向量的值并同时增加控制顶点来辅助曲线的设计，在节点向量末 端增加节点值，并在末端增加新的控制顶点，即为延长B样条轨迹。

设新增加的关节时间节点序列为{p_τ，t_τ}(τ＝n+1,n+2,…)，其中n为上述步骤 中设置关节空间节点个数。每新增一个关节时间节点坐标，在节点向量 U＝[u₀,u₁,…]的末端增加一个节点u_τ(τ＝n+2k+1,n+2k+2,…)，新生成的节点向量仍 为均匀节点向量。对应的，需要增加一个新的控制点d_τ(τ＝n+k,n+k+1,…)，新增 控制顶点由新的节点向量和关节时间节点坐标计算得到。由B样条局部控制特 性可知，关节角度矢量p_τ处有节点值u_τ+k；由于新增加的关节时间节点坐标与B 样条曲线的分段连接点一致，所以新增加的关节角度矢量p_i受k个控制顶点 [d_τ,d_τ+1,…,d_τ+k-1]的影响。建立方程：

$[N_{\tau ,k}\left(u_{\tau +k}\right),N_{\tau +1,k}\left(u_{\tau +k}\right),...,N_{\tau +k-1,k}\left(u_{\tau +k}\right)]\left(\begin{array}{c}{d}_{\tau }\\ d_{\tau +1}\\ .\\ .\\ .\\ d_{\tau +k-1}\end{array}\right)=q_{\tau }$

其中，q_τ表示增加关节位置点的关节角度向量。

根据方程可以求解得到每一个新增关节角度矢量p_τ对应的控制顶点d_τ+k-1， 方程保证了B样条曲线在分段连接处与相应的新增关节角度矢量点一致。如果 判定p_τ为最后一个关节角度矢量，则修正最后一个控制顶点d_τ+k-1等于d_τ-1，使B 样条曲线的轨迹在末端点p_τ处的速度为零。由于节点向量在动态插值过程中均 匀递增，所以k阶B样条曲线仍具有k-1阶几何连续性。

第四步、根据上述得到的B样条曲线，采用德布尔递推算法计算B样条曲 线上的位置点，所述位置点作为伺服位置闭环控制的位置输入，以驱动二自由 度机器人110运动，直至二自由度机器人110运动终止。

设定二自由度机器人110的运动控制器的控制周期T，由于机器人运动控制 器的控制周期要远低于相机的采样周期，因此需要在运动控制器的每一个周期 内根据时间计算样条曲线上的位置点，即关节角度值，计算样条曲线上任意时 刻的位置点则需要用到德布尔递推算法；得到的关节角度作为伺服位置闭环控 制的位置给定信号，通过伺服位置闭环控制使机器人各关节沿规划的关节空间 轨迹运动，反应在笛卡尔空间则是机器人末端跟踪目标轨迹运动。

经第二步和第三步确定了节点矢量U，B样条次数k和控制顶点d，即可采 用德布尔递推算法计算B样条曲线上对应于每一个控制周期的位置点p(u)。其 递推公式为：

$p\left(u\right)=\underset{j=i-k+1}{\overset{i}{\Sigma }}{d}_j^l{N}_{j,k-l}\left(u\right)=...=d_i^k,u_i\le u<u_{i+1}$

$d_j^l=\left(\begin{array}{cc}\left(\begin{array}{c}{d}_j\\ (1-{\alpha }_j^l)d_{j-1}^{l-1}+{\alpha }_j^l{d}_j^{l-1}\end{array}\right)& \left(\begin{array}{c}l=0\\ l=\mathrm{1,2},...k\\ j=i-k+l,...,i\end{array}\right)\end{array}\right)$

${\alpha }_j^l=\frac{u-u_j}{u_{j+k+1-l}-u_j}$

其中，和表示中间辅助变量。

传统的人工示教方式下，首先对所有示教点求运动学逆解，然后根据所求 得的各关节空间点采用B样条曲线技术进行插值，拟合出平滑的、连续的关节 空间轨迹。加入视觉引导后，由于示教点是在机器人运动过程中动态获取的， 所以传统的B样条曲线轨迹规划算法就不再完全适用了。B样条曲线允许通过 改变控制点的个数和增加节点向量的值来控制曲线形状，本发明依据这一特征 采用增加节点向量和控制顶点的方法动态规划二自由度机器人110关节空间B 样条轨迹，能够保证在视觉引导的下的二自由度机器人110轨迹跟踪运动平滑 且关节速度、加速度均连续，提高了视觉引导的机器人轨迹跟踪精度。