基于三次非均匀B样条的6DOF机械臂轨迹规划

基于三次非均匀B样条的6DOF机械臂轨迹规划

赵皓 刘满禄 张华

（西南科技大学特殊环境机器人技术四川省重点实验室，四川绵阳，621010)

摘 要：针对在关节空间中轨迹规划时机械臂末端执行器运行轨迹不受控的问题，本文提出了一种基于随机分布示教点的三次非均匀B样条曲线插值方法，实现在任务空间中连续路径约束条件的轨迹规划。实验结果表明，结合示教点的三次非均匀B样条曲线插值算法能够准确到达任务空间中任意分布的示教点，末端执行器的运行轨迹受控。

关键词：机器人，机械臂，任务空间，样条插值，轨迹规划

0 引言

现代工业中机械臂在装配、码垛、喷涂、焊接等作业中应用广泛，而对机械臂按照一定约束条件的轨迹规划是解决其实际应用的前提与基础。

机械臂轨迹规划泛指机械臂运动过程中运动点的位移、速度和加速度的时间序列[1]。近年来，讨论轨迹规划主要针对点到点过程中机械臂最优控制、运行平稳连续等问题。参考文献[2]提出关节空间中五次多项式插值轨迹规划方法，主要考虑各关节角度、角速度、角加速度的连续性,但文中没有考虑任务空间中存在不可达路径点的问题；参考文献[3]讨论了6自由度机械臂在关节空间中三次多项式和五次多项式插值轨迹规划方法，但未分析末端执行器在任务空间中的运动轨迹；参考文献[4]中采用7次B样条曲线插值方式，满足了最优脉动连续轨迹规划，但样条插值次数越高，曲线出现扭摆的可能性就越大，难以得到光滑曲线，甚至可能出现发散问题；参考文献[5]研究了NURBS曲线插补方式轨迹规划，但其控制顶点的权值难以确定，且计算量大；参考文献[6]利用一种三次均匀B样条曲线插值进行轨迹规划，这种方法只适合曲线型值点间隔相等或者相近的情况，不具有一般性。

为了描述机械臂完整的作业过程，在理论上需要给出其轨迹上若干个节点，用一条轨迹通过或者逼近所有节点，此轨迹按照一定规划方法优化，使规划的轨迹曲线满足机械臂运动连续、平稳等要求。传统的机械臂轨迹规划方法只涉及控制方面的问题，但其末端执行器运行轨迹不容易预测[7]。本文提出了一种在任务空间中三次非均匀B样条曲线插值轨迹规划方法，实现对末端执行器运行轨迹的控制。该在任务空间中取示教点的方式避免了直接取点可能出现不可达中间点的问题；同时，利用三次非均匀B样条曲线的参数连续性、几何连续性、凸包性、局部支承性质[8]完成在任务空间中对示教点的拟合规划，实现对末端执行器连续运行轨迹形状的控制。

1 K次非均匀B样条算法

K次非均匀B样条曲线定义为：

其中，=0,1,2，…,n为控制点,顺序连接成为B样条控制多边形，是定义在非均匀节点矢量上的k次规范B样条基函数。其中，非均匀节点矢量通过弦长参数化实现对控制点参数化过程，如式(2)所示：

第一个节点和最后一个节点具有k+1的重复度r。不同类型的B样条曲线有不同的节点矢量U。同理，B样条基函数根据不同的参数插值曲线也不相同，例如参数多项式的幂基、贝齐尔曲线的贝齐尔基，非均匀B样曲线的B样条基函数[8-9]定义为：

在公式(3)中规定为第i个k次B样条基，定义在共k+2个节点区间上，k+2个节点构成的区间为的支承区间。曲线方程(1)中n+1个控制顶点对应n+1个k次B样条基函数。根据B样条曲线的局部性质[8]可知:曲线上一点至多与k+1控制顶点相关，如式(4)所示：

与贝齐尔曲线相比，B样条曲线的形状可控。在CAGD实践中[8]，通过插值曲线上的一系列型值点反算出B样条曲线的控制顶点的方式较正算过程更为实用。

反算K次非均匀B样条插值曲线的控制点。

用于插值n+1个型值点的K次B样条曲线的方程可表示为公式(5)：

将定义域内的节点值依次带入式(5)方程，可以得到n+1个关于控制点的线性方程组。对于周期K次B样条闭曲线，，式(4)方程缺少一个约束条件。由于周期B样条闭曲线和开曲线统一表示，有，因此，可以从n个方程构成的线性方程组求出n个未知控制点；对于K次B样条开曲线不要求个型值点构成的n+1个线性方程外加两个边界条件（切矢条件），可得到式(6)线性方程组：

通过式(6)方程组可唯一确定一组与n+1个数据点相对应的n+3个控制顶点相邻两点之间用一段B样条曲线连接，整个曲线由n+1条样条曲线拼接而成。由于B样条曲线的参数连续性和几何连续性都与其非零区间上k次样条基的导矢相关，根据上式(5)与式(7):

推导曲线的一阶导矢如公式(8)所示：

由于区间内型值点的重复度为r，故曲线连接点处是可微[8]。

2 基于随机分布型值点的三次非均匀B样条轨迹规划

给定一组型值点，对其进行三次非均匀B样条曲线插值算法拟合。首先，对型值点进行参数化，本算法中为了对比常用的均匀参数化法[6]而采用积累弦长参数化方法,如(9)所示：

通过式(9)得到一个节点矢量，反映了型值点按弦长的分布情况，可以解决均匀参数化法中两邻点间距小时发生的过冲问题[8]，由于其切矢模长接近单位长，故插值曲线具有较好的光顺性。

根据三次非均匀B样条曲线的局部性质可知，首段曲线由前k+1(k=3)个顶点定义，末段曲线由后k+1(k=3)个顶点定义，为使插值曲线的首末段和型值点的首末端点值相等，分别在节点矢量左右扩展k个节点。为了便于计算，对节点矢量进行归一化处理，得到节点矢量：，已知型值点集合和新的节点矢量，按照反算B样条曲线控制点的方法计算出与n+1个型值点相对应的n+3个控制点。

表1 型值点集合

P0 P1 P2 P3 P4 P5 P6 P7 P8 P9 1 3 5 7 9 11 13 15 17 19 1 2.5 4 5 7 4.5 4 6 4 8

表2 数据点集合

d0 1 1 d6 10.3750 4.3342 d1 1 1 d7 13.4607 3.2875 d2 3.3379 2.6658 d8 14.9098 7.7178 d3 4.7653 4.1169 d9 17.4576 1.2285 d4 7.3043 4.6604 d10 19 8 d5 9.1955 8.4539 d11 19 8

依据型值点和相应的控制点在MATLAB中绘制成型值点多边形和控制多边形，如图1所示。

图1 型值点多边形与控制多边形

在节点区间上的样条曲线部分的形状最多只与k+1个控制顶点相关，即公式(10)：

本算法中有10个型值点，除去首末端型值点，需要9段三次非均匀B样条曲线连接形成，每段曲线由式(10)生成，在MATLAB中仿真结果如图2所示。

图2 三次非均匀B样条曲线

三次非均匀B样条曲线插补方式[9]能够有效地解决三次均匀B样条曲线插补方式的过冲问题，相比二次均匀B样条曲线插值，依据反算的控制点的三次非均匀B样条算法拟合的曲线能够更准确地通过轨迹的中间路径点，三者比较如图3所示。

图3 几种样条曲线比较

通过图3中曲线比较可知，三次非均匀B样条曲线插值克服均匀样条参数化后对插值曲线造成的尖点和扁平现象，且能有效地到达每个中间路径点，其强凸包性质有效地将轨迹幅度限制在控制多边形区域内，还避免了利用NURBS曲线插补权值难以确定[5]、计算量大等缺点。

3 实验结果与分析

3.1 实验模型

本文以MATLAB b2012a为仿真平台，实验模型是ABB公司的一款6DOF6R旋转关节机械臂，即IRB140型机器人。该机器人广泛应用于科研和工业领域，主要从事抓取、焊接等工作。本文调用在SolidWorks中制作的IRB140机械臂各个关节模型的*.stl文件，结合其使用手册给出的D-H参数在MATLAB中建立机械臂仿真模型。

IRB140机械臂的D-H参数如表3所示。

表3 连杆D-H参数

在MATLAB中结合*.stl文件调用Link([theta, d , a, alpha, sigma], option)命令建立每一个连杆模型[10-11]，使用SerialLink(L, Properties)命令实现对各个部件的组装。结合表1中D-H参数建立机械臂模型的具体命令如下：

L(1)=Link([0,0.352, 0.070, -pi/2, 0],′mod′)

L(2)=Link([-pi/2,0,0.360,0,0],′mod′);

L(3)=Link([0,0,0,-pi/2,0],′mod′);

L(4)=Link([0,0.380,0,pi/2,0],′mod′);

L(5)=Link([0,0,0,-pi/2,0],′mod′);

L(6)=Link([pi,0.065,0,0,0],′mod′);

ABB_IRB140_Test=SerialLink(L,′name′,′ABB_IRB140_ M2000′);

本文仿真实验模型和实体机械臂如图4所示。

图4 实验模型与实体机械臂

3.2 点到点与连续路径轨迹规划对比

3.2.1 点到点的轨迹规划

点到点的轨迹规划是指机械臂在关节空间中从初始位姿点到期望位姿点运动过程。在此过程中，利用插值算法能够满足机械臂运动的平稳、连续等约束条件，但是机械臂末端轨迹不容易预测[7]。

使用机器人工具箱中的jtraj(qz, qr, t)命令[11]实现对末端探点位姿矩阵建立。其中t是采样时间向量t=[0:0.02:1.5]，采样频率为50Hz，在实验过程中设置的起始关节矢量为：

qz=[ 0,-2*pi/3,pi/18,0,0,0]

qr=[ pi/2,-pi/3,pi/6,0,0,0]

故只有机械臂关节1、2、3的位置、角速度会发生变化。

机械臂运动过程如图5所示，分别在机械臂运动的第0.3s时刻、0.45s时刻、0.6s时刻和1.5s时刻观察其轨迹形态。

图5 机械臂运动过程

在机械臂模型的五次多项式插值运动过程中，关节1、2、3的位置-时间曲线如图6a所示，角速度-时间曲线如图6b所示。

可以看出，机械臂模型末端执行器运动轨迹平滑连续，运行过程平稳。但在关节空间点到点的轨迹规划过程中，机械臂各关节的位移、速度和加速度等约束条件[7]符合要求，而末端执行器的运动轨迹不可控。

3.2.2 任务空间连续路径轨迹规划

针对在关节空间中机械臂末端执行器轨迹不可预测问题，利用基于任务空间任意分布示教点的三次非均匀B样条插值算法，在MATLAB机器人工具箱中示教一系列关节角矢量型值点，如表4所示；利用正运动学算法将其转化为任务空间中一系列控制点，如表5所示。

图6 a 位置-时间曲线

图6 b 角速度-时间曲线

表4 6自由度关节角矢量型值点

表5 6自由度关节角矢量控制点

依据上述型值点，在任务空间中型值点多边形和相应控制点多边形仿真结果如图7所示:

图7 控制多边形

结合三次非均匀B样条插值算法，对表4中型值点进行拟合，拟合曲线如图8所示。

图8 三次非均匀B样条曲线

在任务空间中取任意示教点，利用本文研究的三次非均匀B样条插值曲线算法对连续示教点进行插值，在两中间路径点之间，利用逆运动学算法求出控制机械臂运动的关节角矢量序列，从而实现具有中间点约束条件的机械臂轨迹规划，且末端执行器运行轨迹可控制。机械臂运行过程如图9所示。

图9 机械臂运动过程

4 结论

针对在关节空间中点对点的轨迹规划，机械臂末端执行器运行轨迹不可控问题，本文通过示教点将关节空间中机械臂的位形点转换到任务空间，在任务空间中运用三次非均匀B样条插值方法对示教点进行曲线插值拟合，利用三次非均匀B样条曲线的连续性、局部支承性、凸包性实现对曲线形状的平滑控制[12]，从而解决了机械臂在关节空间中插值规划时，末端执行器轨迹不可预测的问题[7,13],实现了任务空间中连续路径约束条件的轨迹规划。

参考文献：

[1] Jhon J.Craig. 机器人学导论[M]. 员超，李成群，宁凤艳，译. 北京：机械工业出版社，2006.

[2]付西光, 颜国正. 7-DOF核工业机器人的轨迹规划与仿真[J]. 系统仿真学报, 2005, 17(8):1948-1950.

[3]孙亮, 马江, 阮晓钢. 六自由度机械臂轨迹规划与仿真研究[J]. 控制工程, 2010, 17(3):388-392.

[4]朱世强, 刘松国, 王宣银，等. 机械手时间最优脉动连续轨迹规划算法[J]. 机械工程学报, 2010, 46(3):47-52.

[5]晋文科, 张华, 刘满禄. 基于NURBS算法的冗余机械臂轨迹规划[J]. 科学技术与工程, 2014, 14(28):226-230.

[6]李林峰, 马蕾. 三次均匀B样条在工业机器人轨迹规划中的应用研究[J]. 科学技术与工程, 2013, 13(13):3621-3625.

[7]杨锦涛, 姜文刚, 林永才. 工业机器人冲击最优的轨迹规划算法[J]. 科学技术与工程, 2014, 14(28):64-69.

[8]施法中，计算机辅助几何设计与非均匀有理B样条[M]. 北京：高等教育出版社, 2013.

[9] Li C, Wang A, Li K, et al. A Geometric Interpolation Algorithm by Non-uniform Cubic B-Splines[C]// International Conference on Digital Home. IEEE, 2014:132-135.

[10]贾召敏, 曹兵. 排爆机械臂的运动学仿真[J]. 计算机仿真, 2015, 32(3):1-4.

[11] Corke P. MATLAB toolboxes: robotics and vision for students and teachers[J]. IEEE Robotics & Automation Magazine, 2008, 14(4):16-17.

[12]游玮, 孔民秀, 肖永强. 基于遗传算法的机械手时间能耗最优平滑轨迹规划[J]. 机器人技术与应用, 2015(5):25-30.

[13] Chen D, Zhang B, Wang M. Cartesian space trajectory planning on 7-DOF manipulator[C]// IEEE International Conference on Robotics and Biomimetics. IEEE, 2015.

本课题是四川省科技支撑计划项目（2015），项目编号2015GZ0027。