机械手时间最优脉动连续轨迹规划算法_朱世强 2010

0 前言*
轨迹规划是机械手轨迹控制的基础，其性能对 机械手工作效率、运动平稳性和能量消耗具有决定 性意义，因此轨迹优化成为机器人领域的研究热点 之一。出于提高机械手作业单元生产效率的需要， 时间最优轨迹规划成为最早研究的轨迹优化问 题[1]。为了避免机械手振动、延长关节使用寿命和 提高轨迹跟踪精度，最小脉动和脉动连续轨迹规划
S 中的每个位置—时间约束，因此需要根据该约束
反求 B 样条插值轨迹曲线的控制顶点。以累计时长
参数化方法对时间节点 ti 归一化，得到 k 次 B 样条 轨迹曲线的定义域节点矢量 u = (u0 u1 " un+2k ) ，有
u0 = u1 = " = uk = 0 un+k = un+k +1 = " = un+2k = 1
ui
= ui−1
+
| ∆ti−k
n −1
−1
|
∑| ∆t j |
j=0
i = k + 1, k + 2,", n + k −1
并可列出与位置—时间序列 S 中的位置—时间约
束对应的 n +1 个方程
i+k
∑ p(ui+k ) = d j N j,k (ui+k ) = pi
(3)
j=i
式中 i=0,1,…,n，d j 为 B 样条轨迹曲线的 n + k 个控
i
∑ pr (u) =
d
r j
N
j
,
k
−
r
(u )
j =i −k +r
⎧d j
d
r j
=
⎪ ⎨ ⎪(k ⎩
+1− l)
d
l −1 j
−
d
l −1 j −1
u j+k +1−l − u j
l=0 l = 1, 2,", r (2) j = i − k + l,",i
由于轨迹曲线需要严格满足位置—时间序列
Abstract：In order to optimize the productivity and ensure the running stability of manipulators, a new optimal trajectory planning algorithm is proposed. Position series in joint space are obtained by applying inverse kinematic algorithm to a specified trajectory in task space, and B-splines of seven degree are exploited to interpolate joint position series and generate joint trajectories with continuous velocity, acceleration and jerk, as well as controllable start-stop kinematic parameters. By converting kinematic constraints of manipulators to constraints on control points of B-splines, optimal time nodes are solved by using sequential quadratic programming strategy, then time-optimal and jerk-continuous trajectories which satisfy nonlinear kinematic constraints are planned. Simulating and experimental results show that the proposed trajectory planning algorithm provides ideal trajectories for joint controller, and ensures manipulators to track any specified trajectory in task space stably with the minimum traveling time. Key words：Manipulator Trajectory planning Optimal execution time Continuous jerk Sequential quadratic programming
本文采用 C6 连续的 7 次 B 样条曲线插值关节 位置序列，得到脉动连续且关节启停速度、加速度 和脉动可分别设定的轨迹曲线。将各关节的运动速 度、加速度和脉动约束转化为 B 样条轨迹曲线控制 顶点约束，以遍历关节位置序列的最短运行时间作 为目标函数，利用序列二次规划方法求解非线性约 束优化问题，从而规划出时间最优脉动连续的关节 轨迹。
机械手时间最优轨迹既要求轨迹的执行时间
最短，又要求各关节的运动满足运动学约束。设机 械手各关节的速度、加速度和脉动约束分别为 cvm 、 cam 和 c jm ，由式(2)可得各关节的速度、加速度和脉
度、加速度和脉动轨迹曲线。
以矩阵方程的形式描述第 m 个关节的轨迹曲 线控制顶点反求方程
Amdm = pm
(5)
式中 Am ∈ R(n+7)×(n+7) 为系数矩阵，且
⎧⎪dm = (dm0 dm1 " dm(n+6) )T
⎨ ⎪⎩ pm = (pm0
pm1
"
pmn
vms
vme
ams
ame
jms
jme )T
制顶点矢量， j = 0,1,", n + k −1。理想的关节轨迹
要求关节启停速度、加速度和脉动可以设定，提供
了 6 个边界条件，因此取 k = 7 ，得到 n + 7 个方程
∑ ⎧
⎪ p(ui+7 )
=
i+7
d j N j,7 (ui+7 )
=
pi
i = 0,1,", n
⎪
j=i
⎪ ⎨
p′(u)
20090204 收到初稿，20090913 收到修改稿
受到密切关注[2-3]。时间最优轨迹规划可描述为构造 满 足 运 动 学 约 束 的 轨 迹 曲 线 pm = fm (t) ， 使 得
n−1
∑ pmi = fm (ti ) ，且 T = (ti+1 − ti ) 最小，其中 m=1, i=0
采用 Ck−1 连续的 k 次非均匀有理 B 样条开曲线
构造机械手关节轨迹。所有关节轨迹曲线统一描
述为
n
i
∑ ∑ p(u) = d j N j, k (u) = d j N j, k (u)
(1)
j=0
j=i−k
式 中 d j ∈ RN×1 为 控 制 顶 点 矢 量 ， j=0,1, … ,n ，
定，且有 d0
=
p0 、 dn+6
=
pn 、 d11
= vs
、
d
1 n+6
=
ve 、
d
2 2
= as 、dn2+6
= ae 、d33
=
js 、dn3+6
=
je 。其中，d11 、
d 22
、
d
3 3
、
d n1 + 6
、
d
2 n+6
和
d
wk.baidu.com
3 n+
6
可由式(2)求出。由式
(5)可求出 7 次 B 样条轨迹曲线的控制顶点矢量
dm = Am−1 pm
(7)
根据控制顶点矢量 dm 和归一化时间节点矢量 u ，即
月 2010 年 2 月
朱世强等：机械手时间最优脉动连续轨迹规划算法
49
可求出关节 m 在时刻 ti 经过位置 pi 的 C6 连续的关 节轨迹，且各关节轨迹具有统一的描述形式，不受 关节数目 N 的限制。
2 关节运动学约束转化
48
机械工程学报
第 46 卷第 3 期期
动约束问题。TAN 等[7]采用二次多项式加余弦函数 的形式构造相邻关节位置之间的轨迹曲线，通过强 制关节位置节点处的脉动为零来保证整个轨迹脉动 的连续特性，时间最优性能有待进一步的提高。 GASPARETTO 等[8]将运行时间、脉动积分之和作为 目标函数，通过在首末端点处添加两个虚拟关节位 置节点，以 5 次 B 样条曲线插值关节位置，得到满 足运动学约束的时间、平滑性综合最优的轨迹曲线。
1 脉动连续轨迹构造
根据机械手轨迹跟踪精度的要求，将任务空间
几何形状一定的轨迹离散化，得到位姿矩阵序列 Ti 和相应的时间节点序列 ti ，通过逆运动学计算求出 Ti 对应的关节位置矢量 pi ，进而构成关节位置—时 间序列
S = ( pi , ti ) i = 0,1,", n 式中 pi = ( p1i p2i " pNi )T 。脉动连续轨迹要求各关 节 轨 迹 曲 线 函 数 pm = fm (t) 至 少 C3 连 续 ， 且 pmi = fm (ti ) ，即经过关节位置序列。
Time-optimal and Jerk-continuous Trajectory Planning Algorithm for Manipulators
ZHU Shiqiang1 LIU Songguo1, 2 WANG Xuanyin1 WANG Huifang1
(1. The State Key Lab of Fluid Power Transmission and Control, Zhejiang University, Hangzhou 310027; 2. Ningbo Institute of Measurement and Testing, Ningbo 315041)
2, …, N，i=0, 1, …, n，N 为机械手关节数目，n 为 关节轨迹必须经过的关节位置数。当 fm(t)至少 C3 连续时，才能保证关节脉动连续。BALKAN[4]采用 动态规划方法求解时间最优轨迹，生成的轨迹加速 度不连续，易导致机械手振动。同样，基于三次样 条曲线的时间最优轨迹规划方法[5-6]只能保证速度、 加速度连续，而不能保证脉动连续，也无法处理脉
第 46 卷第 3 期 2010 年 2 月
机械工程学报
JOURNAL OF MECHANICAL ENGINEERING
Vo l . 4 6 N o . 3
Feb.
2010
DOI：10.3901/JME.2010.03.047
机械手时间最优脉动连续轨迹规划算法*
朱世强 1 刘松国 1, 2 王宣银 1 王会方 1
u ∈[ui , ui+1] ⊆ [uk , un+1] ⊂ [u0 , un+k +1] ，为归一化时
间变量， p(u) ∈ RN×1 为时刻 u 处的关节位置矢量，
N j, k (u) 为 k 次规范 B 样条基函数。关节轨迹曲线继
承了 B 样条曲线的局部支承和 Ck−1 连续的品质。时 刻 u ∈[ui , ui+1] 处的关节速度、加速度和脉动分别与 B 样条轨迹曲线的 r 阶导数 pr(u)对应，r=1,2,3，可 采用德布尔递推公式求出
|u
=u7
=
vs
p′(u) |u=un+7 = ve
(4)
⎪ ⎪
p′′(u)
|u
=u7
=
as
p′′(u) |u=un+7 = ae
⎩⎪ p′′′(u) |u=u7 = js p′′′(u) |u=un+7 = je
式中 vs 、as 和 js 分别为关节起始速度、加速度和脉 动矢量，ve 、ae 和 je 分别为关节终止速度、加速度 和脉动矢量， p′(u) 、 p′′(u) 和 p′′′(u) 分别为关节速
令 Am (nr nc ) 表示矩阵 Am 的第 nr 行第 nc 列个元素，
则有
( ) Am nr nc = Nnr +nc −1,7 (unr +7 )
(6)
式中 nr = 1, 2,", n −1 ， nc = 1, 2,", 7 。 Am 中的其他
元素可根据关节启停位置、速度、加速度和脉动确
(1. 浙江大学流体传动及控制国家重点实验室 杭州 310027； 2. 宁波市计量测试研究院 宁波 315041)
摘要：为使机械手的作业效率达到最优，同时确保运动的平稳性，提出一种新的最优轨迹规划方法。通过逆运动学运算得到 与任务空间轨迹对应的关节空间位置序列，采用 7 次 B 样条曲线插值方法构造启动和停止运动参数可控，且速度、加速度 和脉动均连续的关节轨迹。将机械手运动学约束转化为 B 样条曲线的控制顶点约束，采用序列二次规划方法求解最优运动 时间节点，进而规划出满足非线性运动学约束的时间最优脉动连续轨迹。仿真和试验结果表明，提出的轨迹规划方法为关节 控制器提供理想的轨迹，使机械手在最短的时间平稳地跟踪任务空间的任意指定轨迹。 关键词：机械手 轨迹规划 时间最优 脉动连续 序列二次规划 中图分类号：TP242.2