﻿ 基于带约束S型速度曲线的机械手笛卡尔空间轨迹规划
«上一篇
 文章快速检索 高级检索

 智能系统学报  2019, Vol. 14 Issue (4): 655-661  DOI: 10.11992/tis.201806025 0

### 引用本文

LI Zhenna, WANG Tao, WANG Binrui, et al. Trajectory planning for manipulator in Cartesian space based on constrained S-curve velocity[J]. CAAI Transactions on Intelligent Systems, 2019, 14(4): 655-661. DOI: 10.11992/tis.201806025.

### 文章历史

Trajectory planning for manipulator in Cartesian space based on constrained S-curve velocity
LI Zhenna , WANG Tao , WANG Binrui , GUO Zhenwu , CHEN Dijian
College of Mechanical and Electrical Engineering, China Jiling University, Hangzhou 310018, China
Abstract: To solve the problem of trajectory planning for manipulator in Cartesian coordinate, a space line and arc interpolation algorithm is designed based on the S-type velocity curve. By increasing the path length constraint, the S-type speed planning is divided into four types: seven-segment, six-segment, five-segment, and four-segment. A pose model of quaternion-based linear and circular movements is established, and the arc length increment interpolation technology is used to achieve trajectory planning. Experimental verification was performed on a real-time control platform of a six-degrees-of-freedom manipulator. The results show that the algorithm can automatically adjust the speed and acceleration under the path length constraint and ensure continuous acceleration at the end of the manipulator, and therefore reduces the impact effectively.
Key words: manipulator    Cartesian space    S-curve velocity    path length constraint    trajectory planning    interpolation algorithm    quaternion    arc length increment

1 S型速度规划

 ${s_1} = \frac{{Jv_{\max }^2 + v_{\max }^{}a_{\max }^2}}{{2Ja_{\max }^{}}}$ (1)
 ${s_2} = \frac{{a_{\max }^3}}{{{J^2}}}$ (2)

 ${T_1} = {T_3} = {T_5} = {T_7} = \frac{{{a_{\max }}}}{J}$ (3)
 ${T_2} = {T_6} = \frac{{{v_{\max }}}}{{{a_{\max }}}} - \frac{{{a_{\max }}}}{J}$ (4)
 ${T_4} = \frac{{s - 2{s_1}}}{{{v_{\max }}}}$ (5)

 $v(t) = \left\{ \begin{array}{l} \displaystyle\frac{1}{2}J{t^2}, \qquad t < {T_1}\\ \displaystyle\frac{1}{2}JT_1^2 + {a_{\max }}(t - {T_1}), \\ \qquad{T_1} < t < \displaystyle\sum\limits_{i = 1}^2 {{T_i}} \\ {v_{\max }} - \displaystyle\frac{1}{2}J{\left(\displaystyle\sum\limits_{i = 1}^3 {{T_i}} - t\right)^2},\\ \qquad\displaystyle\sum\limits_{i = 1}^2 {{T_i}} < t < \sum\limits_{i = 1}^3 {{T_i}} \\ {v_{\max }}, \qquad \displaystyle\sum\limits_{i = 1}^3 {{T_i}} < t < \displaystyle\sum\limits_{i = 1}^4 {{T_i}} \\ {v_{\max }} - \displaystyle\frac{1}{2}J{\left(t - \displaystyle\sum\limits_{i = 1}^4 {{T_i}} \right)^2}, \\ \qquad \displaystyle\sum\limits_{i = 1}^4 {{T_i}} < t < \displaystyle\sum\limits_{i = 1}^5 {{T_i}} \\ {v_{\max }} - \displaystyle\frac{1}{2}JT_5^2 - {a_{\max }}\left(t - \sum\limits_{i = 1}^5 {{T_i}} \right), \\ \qquad \displaystyle\sum\limits_{i = 1}^5 {{T_i}} < t < \sum\limits_{i = 1}^6 {{T_i}} \\ \displaystyle\frac{1}{2}J{\left(\sum\limits_{i = 1}^7 {{T_i}} - t\right)^2}, \\ \qquad \displaystyle\sum\limits_{i = 1}^6 {{T_i}} < t < \sum\limits_{i = 1}^7 {{T_i}} \end{array} \right.$ (6)

 d(t) = \left\{ \begin{aligned} & \frac{{J{t^3}}}{6},\quad t < {T_1}\\ & {d_1} + \frac{J}{2}T_1^2(t - {T_1}) + \frac{{a_{\max }^{}}}{2}{(t - {T_1})^2},\quad{T_1} < t < \sum\limits_{i = 1}^2 {{T_i}} \\ & {d_2} + {v_{\max }}\left(t - \sum\limits_{i = 1}^2 {{T_i}} \right) + \frac{J}{6}{\left(\sum\limits_{i = 1}^3 {{T_i}} - t\right)^3} - \frac{{J}}{6}{T_3}^3,\\ & \qquad \sum\limits_{i = 1}^2 {{T_i}} < t < \sum\limits_{i = 1}^3 {{T_i}} \\ & {d_3} + {v_{\max }}\left(t - \sum\limits_{i = 1}^3 {{T_i}}\right),\quad\sum\limits_{i = 1}^3 {{T_i}} < t < \sum\limits_{i = 1}^4 {{T_i}} \\ & {d_4} + {v_{\max }}\left(t - \sum\limits_{i = 1}^4 {{T_i}} \right) - \frac{1}{6}J{\left(t - \sum\limits_{i = 1}^4 {{T_i}} \right)^3},\\ &\qquad \sum\limits_{i = 1}^4 {{T_i}} < t < \sum\limits_{i = 1}^5 {{T_i}} \\ & {d_5} + \left({v_{\max }} - \frac{1}{2}JT_5^2\right)\left(t - \sum\limits_{i = 1}^5 {{T_i}} \right) - \frac{{a_{\max }^{}}}{6}{\left(t - \sum\limits_{i = 1}^5 {{T_i}} \right)^2},\\ & \qquad \sum\limits_{i = 1}^5 {{T_i}} < t < \sum\limits_{i = 1}^6 {{T_i}} \\ & s - \frac{1}{6}J{\left(\sum\limits_{i = 1}^7 {{T_i}} - t\right)^3},\quad\sum\limits_{i = 1}^6 {{T_i}} < t < \sum\limits_{i = 1}^7 {{T_i}} \end{aligned} \right. (7)

 $\left\{ \begin{split} & {d_1} = \frac{{a_{\max }^3}}{{6{J^2}}}\\ & {d_2} = {d_1} + \frac{J}{2}T_1^2{T_2} + \frac{{{a_{\max }}T_2^2}}{2}\\ & {d_3} = {d_2} + \frac{{{V_{\max }}{a_{\max }}}}{J} - \frac{{a_{\max }^3}}{{6{J^2}}} = {s_1}\\ & {d_4} = s - {d_3}\\ & {d_5} = s - {d_2}\\ & {d_6} = s - {d_1} \end{split} \right.$

 ${v_{\max }} = \frac{{ - \displaystyle\frac{{a_{\max }^2}}{J} + \sqrt {{{(\displaystyle\frac{{a_{\max }^2}}{J})^2}} + 4{a_{\max }}s} }}{2}$ (8)

 ${T_4} = 0$ (9)

 ${a_{\max }} = \sqrt {{v_{\max }}J}$ (10)

 ${T_2} = {T_6} = 0$ (11)
 ${T_4} = \frac{{s - 2{v_{\max }}\sqrt {\displaystyle\frac{{v_{\max }^{}}}{J}} }}{{{v_{\max }}}} = \frac{s}{{{v_{\max }}}} - 2\sqrt {\frac{{v_{\max }^{}}}{J}}$ (12)

 ${a_{\max }} = \sqrt[3]{{\frac{{s{J^2}}}{2}}}$ (13)

 ${v_{\max }} = Jt_1^2$ (14)

 ${T_2} = {T_4} = {T_6} = 0$ (15)
2 笛卡尔空间轨迹规划

2.1 位置规划

1)直线插补。

 $s = \sqrt {{{({x_2} - {x_1})}^2} + {{({y_2} - {y_1})}^2} + {{({z_2} - {z_1})}^2}}$ (16)
 $\left\{ \begin{array}{l} {x_k} = {x_1} + {\lambda _k}({x_2} - {x_1})\\ {y_k} = {y_1} + {\lambda _k}({y_2} - {y_1})\\ {z_k} = {z_1} + {\lambda _k}({z_2} - {z_1}) \end{array} \right.$ (17)

2)圆弧插补。

①确定空间圆弧的圆心坐标 ${P_0}\left( {{x_0},{y_0},{z_0}} \right)$ 及半径 $r$ ${P_1}$ ${P_2}$ ${P_3}$ 三点确定的空间平面的方程为： $Ax + By + Cz + D = 0$ ，具体方法可参照文献[11]，这里不再赘述。

②在局部坐标系 $o - xyz$ 下建立空间圆弧所在平面的新坐标系 $o' - x'y'z'$ 。以圆心 ${P_0}$ 为新坐标系原点 $o'$ ，令 $x'$ 轴的方向为 ${P_0}{P_1}$ 图2所示。则 $x'$ 轴在局部坐标系内的方向余弦为

 ${{n}} = {\left[ {\frac{{{x_1} - {x_0}}}{r}{\rm{ }}\;\frac{{{y_1} - {y_0}}}{r}{\rm{ }}\;\frac{{{z_1} - {z_0}}}{r}} \right]^{\rm{T}}}$ (18)

$z'$ 轴方向为 ${{{{P_1}}}{{{P_2}}}} \times {{{{P_2}}}{{{P_3}}}}$ ，则 $z'$ 轴在局部坐标内的方向余弦为

 ${{a}} = {\left[ {\frac{A}{k}{\rm{ }}\;\frac{B}{k}{\rm{ }}\;\frac{C}{k}} \right]^{\rm{T}}}$ (19)

$y'$ 轴方向可由右手法则确定，其方向为 ${{o}} = {{a}} \times {{n}}$ 。令 ${{p}} = {\left[ {{x_0}\;{y_0}\;{z_0}} \right]^{\rm{T}}}$ ，由齐次变换原理[12]，可求得坐标系 $o' - x'y'z'$ 到局部坐标系 $o - xyz$ 的齐次变换矩阵 ${{T}}$

 ${{T}} = \left[ {\begin{array}{*{20}{c}} {{n}}&{{o}}&{{a}}&{{p}}\\ 0&0&0&0 \end{array}} \right]$ (20)

 ${\left[ {x'\ y'\ z'\ 1} \right]^{\rm{T}}} = {{{T}}^{ - 1}}{\left[ {x'\ y'\ z'\ 1} \right]^{\rm{T}}}$ (21)
 Download: 图 2 基于局部坐标系的圆弧插补 Fig. 2 Arc interpolation based on local coordinate system

3)求圆弧的夹角 $\theta$ 。由式(21)计算出空间三点 ${P_1}$ ${P_2}$ ${P_3}$ 在坐标系 $o' - x'y'z'$ 下相应坐标为： ${P'_1}({x'_1},{y'_1},0)$ ${P'_2}({x'_2},{y'_2},0)$ ${P'_3}({x'_3},{y'_3},0)$

 $\theta = \left\{ \begin{array}{l} \arctan 2({y_3}^\prime ,{x_3}^\prime ) + 2{\text{π}},\quad{\rm{ }}{y_3}^\prime < 0;\\ \arctan 2({y_3}^\prime ,{x_3}^\prime ),\quad{y_3}^\prime \geqslant 0.{\rm{ }} \end{array} \right.$ (22)

4)在圆弧所在平面上 ${\varPi _1}$ 进行平面规划。基于齐次变换原理，可得

 $\left\{ \begin{array}{l} {\theta _k} = {\lambda _k} \cdot \theta \\ {{x'}_i} = r \cdot \cos {\theta _k}\\ {{y'}_i} = r \cdot \sin {\theta _k}\\ {{z'}_i} = 0 \end{array} \right.$ (23)

${{\textit{λ}} _k}$ 进行S型速度规划便可完成圆弧运动的插补计算，根据弧长和速度等约束条件可分为七段、六段、五段、四段4种情况，可参考直线部分。

2.2 基于四元数的姿态规划

 $\left\{ \begin{array}{l} s = \pm \sqrt {{n_x} + {o_y} + {a_z} + {1 / 2}} \\ a = {{({o_z} - {a_y})} / {4s}}\\ b = {{({a_x} - {n_z})} / {4s}}\\ c = {{({n_y} - {o_x})} / {4s}} \end{array} \right.$ (24)

 $\cos \;\varOmega = {q_1} \cdot {q_2}$ (25)

 ${\rm{Slerp}}({q_1},{q_2},{\lambda _k}) = \frac{{{q_1}\sin (1 - {\lambda _k})\varOmega + {q_2}\sin ({q_2}\varOmega )}}{{\sin \;\varOmega }}$ (26)
2.3 轨迹规划流程

 ${\lambda _k} = \frac{{d({t_k})}}{{{d_i}}}{\rm{ }}\;k = 1,2, \cdots, N$ (27)

3 仿真分析及实验

3.1 直线插补实验

 Download: 图 4 直线插补运动的位移曲线 Fig. 4 The displacement curve of line interpolation motion
 Download: 图 5 直线插补运动的速度与加速度曲线 Fig. 5 The velocity and acceleration curves of line interpolation motion
 Download: 图 6 直线插补运动喷绘实验效果 Fig. 6 Experimental effect diagram of line interpolation motion spray painting

3.2 圆弧插补实验

 Download: 图 7 圆弧插补运动的位移曲线 Fig. 7 The displacement curve of arc interpolation motion
 Download: 图 8 圆弧插补运动的速度与加速度曲线 Fig. 8 Curve of velocity and acceleration of arc interpolation motion
 Download: 图 9 圆弧插补运动喷绘实验效果 Fig. 9 Experimental effect diagram of arc interpolation motion spray painting
4 结论

1)采用S型速度规划，基于四元数的直线与圆弧运动的位姿模型，可实现笛卡尔空间规划。

2)根据路径长度约束条件，能将速度规划类型分为4种情况，可实现速度和加速度的自动调整。

3)本文设计的S型速度规划算法可保证加速度连续，减小机械冲击。