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

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
2, …, N，i=0, 1, …, n，N 为机械手关节数目，n 为 关节轨迹必须经过的关节位置数。当 fm(t)至少 C3 连续时，才能保证关节脉动连续。BALKAN[4]采用 动态规划方法求解时间最优轨迹，生成的轨迹加速 度不连续，易导致机械手振动。同样，基于三次样 条曲线的时间最优轨迹规划方法[5-6]只能保证速度、 加速度连续，而不能保证脉动连续，也无法处理脉
本文采用 C6 连续的 7 次 B 样条曲线插值关节 位置序列，得到脉动连续且关节启停速度、加速度 和脉动可分别设定的轨迹曲线。将各关节的运动速 度、加速度和脉动约束转化为 B 样条轨迹曲线控制 顶点约束，以遍历关节位置序列的最短运行时间作 为目标函数，利用序列二次规划方法求解非线性约 束优化问题，从而规划出时间最优脉动连续的关节 轨迹。
令 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 中的其他
元素可根据关节启停位置、速度、加速度和脉动确
制顶点矢量， 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)
采用 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
3 n+
6
可由式(2)求出。由式
(5)可求出 7 次 B 样条轨迹曲线的控制顶点矢量
度、加速度和脉动轨迹曲线。
以矩阵方程的形式描述第 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
48
机械工程学报
第 46 卷第 3 期期
动约束问题。TAN 等[7]采用二次多项式加余弦函数 的形式构造相邻关节位置之间的轨迹曲线，通过强 制关节位置节点处的脉动为零来保证整个轨迹脉动 的连续特性，时间最优性能有待进一步的提高。 GASPARETTO 等[8]将运行时间、脉动积分之和作为 目标函数，通过在首末端点处添加两个虚拟关节位 置节点，以 5 次 B 样条曲线插值关节位置，得到满 足运动学约束的时间、平滑性综合最优的轨迹曲线。
机械手时间最优轨迹既要求轨迹的执行时间
最短，又要求各关节的运动满足运动学约束。设机 械手各关节的速度、加速度和脉动约束分别为 cvm 、 cam 和 c jm ，由式(2)可得各关节的速度、加速度和脉
20090204 收到初稿，20090913 收到修改稿
受到密切关注[2-3]。时间最优轨迹规划可描述为构造 满 足 运 动 学 约 束 的 轨 迹 曲 线 pm = fm (t) ， 使 得
n−1
∑ pmi = fm (ti ) ，且 T = (ti+1 − ti ) 最小，其中 m=1, i=0
1 脉动连续轨迹构造
根据机械手轨迹跟踪精度的要求，将任务空间
几何形状一定的轨迹离散化，得到位姿矩阵序列 Ti 和相应的时间节点序列 ti ，通过逆运动学计算求出 Ti 对应的关节位置矢量 pi ，进而构成关节位置—时 间序列
S = ( pi , ti ) i = 0,1,", n 式中 pi = ( p1i p2i " pNi )T 。脉动连续轨迹要求各关 节 轨 迹 曲 线 函 数 pm = fm (t) 至 少 C3 连 续 ， 且 pmi = fm (ti ) ，即经过关节位置序列。
dm = Am−1 pm
(7)
根据控制顶点矢量 dm 和归一化时间节点矢量 u ，即
月 2010 年 2 月
朱世强等：机械手时间最优脉动连续轨迹规划算法
49
可求出关节 m 在时刻 ti 经过位置 pi 的 C6 连续的关 节轨迹，且各关节轨迹具有统一的描述形式，不受 关节数目 N 的限制。
2 关节运动学约束转化
第 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
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 个控
|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) 分别为关节速