CN105302147A - 一种串联机构轨迹规划方法 - Google Patents

Abstract

本发明提供一种串联机构轨迹规划的方法，所述方法通过分段高阶多项式插值法计算串联机构的各机构关节角、角速度和角加速度的变化规律，并对超出加速度约束值的轨迹段的连杆进行加速度约束，获得多项式最高阶次数，得到修正后的轨迹规划结果，在工作过程中，串联机构各关节的力矩会急剧增大，对结构损害非常大，在进行多项式轨迹规划时，对该段轨迹内，通过对角加速度约束在规定范围内，进而反求出该段轨迹多项式的最高阶的次数，完成该段轨迹多项式二次规划，保证串联机构各关节能够平稳的进行工作。

Description

一种串联机构轨迹规划方法

技术领域

本发明属于串联机构自动化作业规划技术领域，具体涉及一种串联机构轨迹规划的方法。

背景技术

随着工业水平的发展，机器人自动化作业日益广泛，如各类工业机器人、工程机械机器人、生活中应用的机器人，为了精确完成机器人的工作任务，其中轨迹规划是一项非常重要的工作，它能够保证各串联机构能够以合理的位置点，速度，加速度来完成它的工作任务。在串联机构轨迹规划的方法中，有许多国内外学者提出了许多种方法，主要有不同条件下的分段多项式轨迹规划方法如3-5-3、4-3-4、3-3-3···5等多种多项式插值法，5次和7次多项式插值法，3次和5次NURBS曲线插值法，外国学者Lin和Luh等人提出了规划机器人连续运动轨迹的三次样条函数方法，以及过渡段抛物线插值法，以上方法均可以实现串联机构自动化作业过程中的轨迹规划任务，但是许多规划方法存在一定的缺点。

在自动化作业串联机构的规划中，其中避免各机构有较大的振动冲击、较大的力变化，防止结构损坏，是非常重要的，为了解决这个问题，在关节空间采用分段高阶多项式插值法进行轨迹规划研究，得到各关节位置、速度、加速度变化规律，通过加速度最大值约束来确定多项式最高项的次数，得到合理的轨迹多项式，很好的避免了串联机构在自动化作业中由于振动、冲击强度过大而导致结构损坏。

发明内容

为了解决上述串联机构在自动化作业中由于振动、冲击强度过大而导致结构损坏的问题，本发明提供一种串联机构轨迹规划的方法，所述方法通过分段高阶多项式插值法计算串联机构的各机构关节角、角速度和角加速度的变化规律，并对超出加速度约束值的轨迹段的连杆进行加速度约束，获得多项式最高阶次数，得到修正后的轨迹规划结果；

进一步地，所述方法包括：

S1：规划串联机构轨迹曲线；

S2：在关节空间进行轨迹规划；

S3：轨迹规划仿真；

S4：通过加速度约束确定多项式最高项；

S5：仿真验证确定轨迹规划多项式；

进一步地，所述S1具体为：在串联机构工作过程中，结合机构结构参数，设定其工作任务，规定工作路径中关键点，通过空间样条插值来得到平滑的路径曲线；

进一步地，所述S2具体为：在关节空间进行轨迹规划时，采用分段高阶多项式进行关节空间轨迹规划，从而得到各关节动态变化规律，得到各关节角、角速度、角加速度变化规律，所述分段高阶多项式为3-3-5-3-3五段式：

引入无量纲时间变量t∈[0,1]，定义变量其中τ∈[τ_i-1,τ_i]为各段轨迹末端的实际时间；τ_i-1为第i段轨迹起始时间；τ_i第i段轨迹末端的实际时间；t_i为第i段轨迹所需要的时间；机器人关节j(j＝1,2,3)的轨迹由多项式序列h_i(t)(i＝1,2,3,4,5)组成；

令串联机构任意一关节五段轨迹多项式表达式如下：

$\left\{\begin{array}{c}{h}_1\left(t\right)=a_{13}{t}^3+a_{12}{t}^2+a_{11}t+a_{10}\\ h_2\left(t\right)=a_{23}{t}^3+a_{22}{t}^2+a_{21}t+a_{20}\\ h_3\left(t\right)=a_{35}{t}^5+a_{34}{t}^4+a_{33}{t}^3+a_{32}{t}^2+a_{31}t+a_{30}\\ h_4\left(t\right)=a_{43}{t}^3+a_{42}{t}^2+a_{41}t+a_{40}\\ h_5\left(t\right)=a_{53}{t}^3+a_{52}{t}^2+a_{51}t+a_{50}\end{array}\right.---\left(1\right)$

对轨迹多项式中实际时间变量τ求一阶导数得到各关节速度：

$v_i\left(t\right)=\frac{{\mathrm{dh}}_i\left(t\right)}{d\mathrm{\τ}}=\frac{{\mathrm{dh}}_i\left(t\right)}{dt}\·\frac{dt}{d\mathrm{\τ}}=\frac{\stackrel{\·}{h_i\left(t\right)}}{ti}---\left(2\right)$

对轨迹多项式中实际时间变量τ求二阶导数得到各关节加速度：

$a_i=\frac{d^2{h}_i\left(t\right)}{{\mathrm{d\τ}}^2}=\frac{\stackrel{\·\·}{h_i\left(t\right)}}{{t_i}^2}---\left(3\right)$

初始点和终止点满足位置、速度、加速度条件有：

$\begin{array}{cc}\left\{\begin{array}{c}{h}_1\left(0\right)=q_0\\ v_1\left(0\right)=v_0\\ a_1\left(0\right)=a_0\end{array}\right.& \left\{\begin{array}{c}{h}_5\left(1\right)=q_f\\ v_5\left(1\right)=v_f\\ a_5\left(1\right)=a_f\end{array}\right.\end{array}---\left(4\right)$

由中间点满足位置要求及位置、速度、加速度连续可得：

$\begin{array}{cc}\left\{\begin{array}{c}{h}_1\left(1\right)=q_1\\ h_2\left(0\right)=h_1\left(1\right)\\ h_2\left(1\right)=q_2\\ h_3\left(0\right)=h_2\left(1\right)\\ h_3\left(1\right)=q_3\\ h_4\left(0\right)=h_3\left(1\right)\\ h_4\left(0\right)=q_4\\ h_5\left(0\right)=h_4\left(1\right)\end{array}\right.& \left\{\begin{array}{c}{v}_2\left(0\right)=v_1\left(1\right)\\ v_3\left(0\right)=v_2\left(1\right)\\ v_4\left(0\right)=v_3\left(1\right)\\ v_5\left(0\right)=v_4\left(1\right)\\ a_2\left(0\right)=a_1\left(1\right)\\ a_3\left(0\right)=a_2\left(1\right)\\ a_4\left(0\right)=a_3\left(1\right)\\ a_5\left(0\right)=a_4\left(1\right)\end{array}\right.\end{array}---\left(5\right)$

结合(5)式和已知条件可以求得多项式的各个系数；

$\begin{array}{cc}\left\{\begin{array}{c}{a}_{10}=q_0\\ a_{11}=v_0{t}_1\\ a_{12}=\frac{1}{2}{a}_0{t_1}^2\\ a_{13}=q_1-\left(a_{10}+a_{11}+a_{12}\right)\end{array}\right.& \left\{\begin{array}{c}{a}_{20}=q_1\\ a_{21}=\left(3a_{13}+2a_{12}+a_{11}\right)\·\frac{t_2}{t_1}\\ a_{22}=\left(3a_{13}+a_{12}\right)\·\frac{{t_2}^2}{{t_1}^1}\\ a_{23}=q_2-\left(a_{20}+a_{21}+a_{22}\right)\end{array}\right.\end{array}$

$\begin{array}{cc}\left\{\begin{array}{c}{a}_{50}=q_4\\ a_{51}=3q_f-2v_f{t}_5+\frac{1}{2}{a}_f{t_5}^2-3a_{50}\\ a_{52}=3v_f{t}_5-a_f{t_5}^2-3q_f+3a_{50}\\ a_{53}=q_f-\left(a_{50}+a_{51}+a_{52}\right)\end{array}\right.& \left\{\begin{array}{c}{a}_{40}=q_3\\ a_{42}=3a_{51}\·\frac{t_4}{t_5}-2a_{52}\frac{{t_4}^2}{{t_5}^2}-3q_4+3a_{40}\\ a_{43}=a_{52}\·\frac{{t_4}^2}{{t_5}^2}-a_{51}\·\frac{t_4}{t_5}+q_4-a_{40}\\ a_{41}=q_4-\left(a_{40}+a_{41}+a_{42}\right)\end{array}\right.\end{array}$

$\left\{\begin{array}{c}{a}_{30}=q_3\\ a_{31}=\left(3a_{23}+2a_{22}+a_{21}\right)\·\frac{t_3}{t_2}\\ a_{32}=\left(3a_{23}+a_{22}\right)\·\frac{{t_3}^2}{{t_2}^2}\\ a_{34}=7a_{41}\·\frac{t_3}{t_4}-2a_{42}\frac{{t_3}^2}{{t_4}^2}-15q_3+3a_{32}+8a_{31}+15a_{30}\\ a_{35}=a_{42}\·\frac{{t_3}^2}{{t_4}^2}-3a_{41}\·\frac{t_3}{t_4}+6q_3-a_{32}-3a_{31}-6a_{30}\\ a_{33}=q_3-\left(a_{30}+a_{31}+a_{34}+a_{35}\right)\end{array}\right.---\left(6\right);$

进一步地，所述S3由S1中设定的参数得出串联机构各工作关键点数据，通过运动学逆解，从笛卡尔空间转化到关节空间得到各工作关键点处各机构关节角度值，从而得到工作中间连续点值，结合开始设定的初始速度、加速度值以及S2中求得的各关节轨迹多项式以及角速度、角加速度的表达式，通过仿真软件MATLAB进行编程，得到各机构的关节角、角速度、角加速度变化曲线；

进一步地，所述S4由S3轨迹规划仿真中得出了各关节角加速度的变化曲线图，在进行多项式轨迹规划时，对该段轨迹内，通过对角加速度约束在规定范围内，进而反求出该段轨迹多项式的最高阶的次数，所述中间段轨迹多项式为：

h₃(t)＝a₃₅x^b+a₃₄x⁴+a₃₃x³+a₃₂x²+a₃₁x+a₃₀(7)

则角加速度为：

a₃＝h₃(t)”＝b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x(8)

根据已知连续性条件求得该段多项式的系数为：

$\left\{\begin{array}{c}{a}_{30}=q_3\\ a_{31}=\left(3a_{23}+2a_{22}+a_{21}\right)\frac{t_3}{t_2}\\ a_{32}=\left(3a_{23}+a_{22}\right)\frac{{t_3}^2}{{t_2}^2}\\ a_{34}=\left(1+\frac{6}{b-4}\right)\frac{a_{41}{t}_3}{t_4}-\frac{2a_{42}{t_3}^2}{\left(b-4\right){t_4}^2}-\left(3+\frac{12}{b-4}\right)q_3\left(1+\frac{2}{b-4}\right)a_{32}+\left(2+\frac{6}{b-4}\right)a_{31}+\left(3+\frac{12}{b-4}\right)a_{30}\\ a_{35}=\frac{\frac{2a_{42}{t_3}^2}{{t_4}^2}-\frac{6a_{41}{t}_3}{t4}+12q_3-2a_{32}-6a_{31}-12a_{30}}{\left(b-3\right)\left(b-4\right)}\\ a_{33}=q_3-\left(a_{30}+a_{31}+a_{32}+a_{34}+a_{35}\right)\end{array}\right.---\left(9\right)$

设该段关节角加速度约束为a_max，则有：|a_3j|≤a_max(j＝1,2,3)即：

|b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x|≤a_max(10)

在MATLAB中通过给定范围内的b值取一定步长进行试算，求解

l_i＝|b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x|_max(i＝1,2,3……n)，x∈[01](11)

得出满足l≤a_max的多项式最高阶次数b值；

进一步地，所述S5具体为将S4得到的满足l≤a_max的多项式最高阶次数b值代入该段多项式，进行各关节轨迹规划仿真验证，从而分别求出每个关节中间段最高阶次数修正后的合理的轨迹多项式。

本发明的有益效果是：

1)各关节轨迹规划仿真验证，分别求出每个关节中间段最高阶次数修正后的合理的轨迹多项式，不仅保证了路径偏差小，加速度，角加速度连续，而且将工作过程中机构关节角角加速度限定在规定约束范围内；

2)减少了在工作过程振动冲击强度过大，造成精度不准确，以及串联机构的结构损坏；

3)在中间5次多项式段，加速度阶跃比较大，在工作过程中，串联机构各关节的力矩会急剧增大，对结构损害非常大，在进行多项式轨迹规划时，对该段轨迹内，通过对角加速度约束在规定范围内，进而反求出该段轨迹多项式的最高阶的次数，完成该段轨迹多项式二次规划，保证串联机构各关节能够平稳的进行工作。

附图说明

图1为串联机构工作装置结构简图；

图2为串联机构D-H坐标系下结构简图；

图3为挖掘机器人工作装置简图；

图4为挖掘机器人D-H坐标系下工作装置简图；

图5为挖掘机器人完成一次挖掘任务空间轨迹曲线；

图6为XZ平面内自主挖掘过程轨迹曲线；

图7为动臂关节角、角速度、角加速度变化规律曲线图；

图8为斗杆关节角、角速度、角加速度变化规律曲线图；

图9为铲斗关节角、角速度、角加速度变化规律曲线图；

图10为修正后动臂关节角、角速度、角加速度变化规律曲线图；

图11为修正后斗杆关节角、角速度、角加速度变化规律曲线图；

图12为修正后铲斗关节角、角速度、角加速度变化规律曲线图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。

下面结合附图和具体实施例对本发明作进一步说明，但不作为对本发明的限定。下面为本发明的举出最佳实施例：

如图1所示，图1为本发明轨迹规划方法建立串联机构工作装置结构图，如图2所示，图2为D-H坐标系下串联机构结构图，本发明方法通过分段高阶多项式插值法计算串联机构的各机构关节角、角速度和角加速度的变化规律，并对超出加速度约束值的轨迹段的连杆进行加速度约束，获得多项式最高阶次数，得到修正后的轨迹规划结果，所述方法包括：

4)S1：规划串联机构轨迹曲线；

5)在串联机构工作过程中，结合机构结构参数，设定其工作任务，规定工作路径中关键点，通过空间样条插值来得到平滑的路径曲线

6)S2：在关节空间进行轨迹规划；

7)在关节空间进行轨迹规划时，采用分段高阶多项式进行关节空间轨迹规划，从而得到各关节动态变化规律，得到各关节角、角速度、角加速度变化规律，这里采用5段式3-3-5-3-3多项式进行规划：

8)为了使得计算方便，引入无量纲时间变量t∈[0,1]，定义变量其中τ∈[τ_i-1,τ_i]为各段轨迹末端的实际时间；τ_i-1为第i段轨迹起始时间；τ_i第i段轨迹末端的实际时间；t_i为第i段轨迹所需要的时间；机器人关节j(j＝1,2,3)的轨迹由多项式序列h_i(t)(i＝1,2,3,4,5)组成；

9)设串联机构任意一关节五段轨迹多项式表达式如下：

10) $\left\{\begin{array}{c}{h}_1\left(t\right)=a_{13}{t}^3+a_{12}{t}^2+a_{11}t+a_{10}\\ h_2\left(t\right)=a_{23}{t}^3+a_{22}{t}^2+a_{21}t+a_{20}\\ h_3\left(t\right)=a_{35}{t}^5+a_{34}{t}^4+a_{33}{t}^3+a_{32}{t}^2+a_{31}t+a_{30}\\ h_4\left(t\right)=a_{43}{t}^3+a_{42}{t}^2+a_{41}t+a_{40}\\ h_5\left(t\right)=a_{53}{t}^3+a_{52}{t}^2+a_{51}t+a_{50}\end{array}\right.---\left(1\right)$

11)对轨迹多项式中实际时间变量τ求一阶导数得到各关节速度：

12) $v_i\left(t\right)=\frac{{\mathrm{dh}}_i\left(t\right)}{d\mathrm{\τ}}=\frac{{\mathrm{dh}}_i\left(t\right)}{dt}\·\frac{dt}{d\mathrm{\τ}}=\frac{\stackrel{\·}{h_i\left(t\right)}}{ti}---\left(2\right)$

13)对轨迹多项式中实际时间变量τ求二阶导数得到各关节加速度：

14) $a_i=\frac{d^2{h}_i\left(t\right)}{{\mathrm{d\τ}}^2}=\frac{\stackrel{\·\·}{h_i\left(t\right)}}{{t_i}^2}---\left(3\right)$

15)初始点和终止点满足位置、速度、加速度条件有：

16) $\begin{array}{cc}\left\{\begin{array}{c}{h}_1\left(0\right)=q_0\\ v_1\left(0\right)=v_0\\ a_1\left(0\right)=a_0\end{array}\right.& \left\{\begin{array}{c}{h}_5\left(1\right)=q_f\\ v_5\left(1\right)=v_f\\ a_5\left(1\right)=a_f\end{array}\right.\end{array}---\left(4\right)$

17)由中间点满足位置要求及位置、速度、加速度连续可得：

18) $\begin{array}{cc}\left\{\begin{array}{c}{h}_1\left(1\right)=q_1\\ h_2\left(0\right)=h_1\left(1\right)\\ h_2\left(1\right)=q_2\\ h_3\left(0\right)=h_2\left(1\right)\\ h_3\left(1\right)=q_3\\ h_4\left(0\right)=h_3\left(1\right)\\ h_4\left(0\right)=q_4\\ h_5\left(0\right)=h_4\left(1\right)\end{array}\right.& \left\{\begin{array}{c}{v}_2\left(0\right)=v_1\left(1\right)\\ v_3\left(0\right)=v_2\left(1\right)\\ v_4\left(0\right)=v_3\left(1\right)\\ v_5\left(0\right)=v_4\left(1\right)\\ a_2\left(0\right)=a_1\left(1\right)\\ a_3\left(0\right)=a_2\left(1\right)\\ a_4\left(0\right)=a_3\left(1\right)\\ a_5\left(0\right)=a_4\left(1\right)\end{array}\right.\end{array}---\left(5\right)$

19)结合上面式子和已知条件可以求得多项式的各个系数；

20)

$\begin{array}{cc}\left\{\begin{array}{c}{a}_{10}=q_0\\ a_{11}=v_0{t}_1\\ a_{12}=\frac{1}{2}{a}_0{t_1}^2\\ a_{13}=q_1-\left(a_{10}+a_{11}+a_{12}\right)\end{array}\right.& \left\{\begin{array}{c}{a}_{20}=q_1\\ a_{21}=\left(3a_{13}+2a_{12}+a_{11}\right)\·\frac{t_2}{t_1}\\ a_{22}=\left(3a_{13}+a_{12}\right)\·\frac{{t_2}^2}{{t_1}^1}\\ a_{23}=q_2-\left(a_{20}+a_{21}+a_{22}\right)\end{array}\right.\end{array}$

21)

$\begin{array}{cc}\left\{\begin{array}{c}{a}_{50}=q_4\\ a_{51}=3q_f-2v_f{t}_5+\frac{1}{2}{a}_f{t_5}^2-3a_{50}\\ a_{52}=3v_f{t}_5-a_f{t_5}^2-3q_f+3a_{50}\\ a_{53}=q_f-\left(a_{50}+a_{51}+a_{52}\right)\end{array}\right.& \left\{\begin{array}{c}{a}_{40}=q_3\\ a_{42}=3a_{51}\·\frac{t_4}{t_5}-2a_{52}\frac{{t_4}^2}{{t_5}^2}-3q_4+3a_{40}\\ a_{43}=a_{52}\·\frac{{t_4}^2}{{t_5}^2}-a_{51}\·\frac{t_4}{t_5}+q_4-a_{40}\\ a_{41}=q_4-\left(a_{40}+a_{41}+a_{42}\right)\end{array}\right.\end{array}$

22) $\left\{\begin{array}{c}{a}_{30}=q_3\\ a_{31}=\left(3a_{23}+2a_{22}+a_{21}\right)\·\frac{t_3}{t_2}\\ a_{32}=\left(3a_{23}+a_{22}\right)\·\frac{{t_3}^2}{{t_2}^2}\\ a_{34}=7a_{41}\·\frac{t_3}{t_4}-2a_{42}\frac{{t_3}^2}{{t_4}^2}-15q_3+3a_{32}+8a_{31}+15a_{30}\\ a_{35}=a_{42}\·\frac{{t_3}^2}{{t_4}^2}-3a_{41}\·\frac{t_3}{t_4}+6q_3-a_{32}-3a_{31}-6a_{30}\\ a_{33}=q_3-\left(a_{30}+a_{31}+a_{34}+a_{35}\right)\end{array}\right.---\left(6\right)$

S3：轨迹规划仿真；

跟据S1中设定的参数得出串联机构各工作关键点数据，通过运动学逆解，从笛卡尔空间转化到关节空间得到各工作关键点处各机构关节角度值，从而得到工作中间连续点值，结合开始设定的初始速度、加速度值以及S2中求得的各关节轨迹多项式以及角速度、角加速度表达式，通过仿真软件MATLAB进行编程，得到各机构的关节角、角速度、角加速度变化曲线；

S4：通过加速度约束确定多项式最高项在上述的轨迹规划仿真中得出了各关节角加速度的变化曲线图，发现在中间5次多项式段，加速度阶跃比较大，这样一来，在挖掘过程中，挖掘机各关节的力矩会急剧增大，对挖掘机的结构损害非常大，所以在进行多项式轨迹规划时，对该段轨迹内，通过对角加速度约束在规定范围内，进而反求出该段轨迹多项式的最高阶的次数，完成该段轨迹多项式二次规划，保证串联机构各关节能够平稳的进行工作；

设中间段轨迹多项式为:

h₃(t)＝a₃₅x^b+a₃₄x⁴+a₃₃x³+a₃₂x²+a₃₁x+a₃₀(7)

角加速度为：

a₃＝h₃(t)”＝b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x(8)

根据已知连续性条件求得该段多项式的系数为：

$\left\{\begin{array}{c}{a}_{30}=q_3\\ a_{31}=\left(3a_{23}+2a_{22}+a_{21}\right)\frac{t_3}{t_2}\\ a_{32}=\left(3a_{23}+a_{22}\right)\frac{{t_3}^2}{{t_2}^2}\\ a_{34}=\left(1+\frac{6}{b-4}\right)\frac{a_{41}{t}_3}{t_4}-\frac{2a_{42}{t_3}^2}{\left(b-4\right){t_4}^2}-\left(3+\frac{12}{b-4}\right)q_3\left(1+\frac{2}{b-4}\right)a_{32}+\left(2+\frac{6}{b-4}\right)a_{31}+\left(3+\frac{12}{b-4}\right)a_{30}\\ a_{35}=\frac{\frac{2a_{42}{t_3}^2}{{t_4}^2}-\frac{6a_{41}{t}_3}{t4}+12q_3-2a_{32}-6a_{31}-12a_{30}}{\left(b-3\right)\left(b-4\right)}\\ a_{33}=q_3-\left(a_{30}+a_{31}+a_{32}+a_{34}+a_{35}\right)\end{array}\right.---\left(9\right)$

设挖掘过程中该段关节角加速度约束为a_max，则有：

|a_3j|≤a_max(j＝1,2,3)即：

|b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x|≤a_max(10)

在MATLAB中通过给定一定范围内的b值取一定步长进行试算，求解

l_i＝|b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x|_max(i＝1,2,3……n)，x∈[01](11)

S5：仿真验证确定轨迹规划多项式

得出满足l≤a_max的多项式最高阶次数b值然后将修正后的多项式最高阶次数b代入该段多项式，进行各关节轨迹规划仿真验证，从而分别求出每个关节中间段最高阶次数修正后的合理的轨迹多项式，不仅保证了路径偏差小，加速度，角加速度连续，并且将工作过程中机构关节角角加速度限定在规定约束范围内，防止工作过程振动冲击强度过大，造成精度不准确，以及串联机构的结构损坏。

以某型挖掘机器人为例，建立工作装置模型，其结构件图如图3所示，图3中：1—回转平台，2—动臂，3—动臂油缸，4—斗杆油缸，5—斗杆，6—铲斗油缸，7—连杆，8—摇杆，9—铲斗，其工作机构中共有11个铰点，即：A、B、C、D、E、F、G、H、K、N、Q。建立挖掘机器人工作装置的D-H坐标系如图4所示，在坐标系中a_i、α_i、d_i和θ_i的定义如下：

a_i—从z_i-1到x_i沿测量的距离；α_i—从z_i-1到x_i绕旋转的角度；

d_i一从x_i-1到z_i沿测量的距离；θ_i一从x_i-1到x_i沿z_i旋转的角度。

其中为θ₁回转关节变量，θ₂为动臂关节变量，θ₃为斗杆关节变量，θ₄为铲斗关节变量。

挖掘机器人空间挖掘轨迹曲线：

挖掘机在自主挖掘时完成梯形深坑挖掘任务时，由铲斗铲入土壤，拖拽，铲斗回旋出土三个步骤，在这三个步骤中由三个关节角复合运动来完成挖掘动作，在提升过程中由动臂关节运动来完成，接着通过回转装置进行旋转到指定卸载位置，铲斗关节变化来完成卸载，然后由三关节复合运动到达下一个指定挖掘点。

挖掘机器人完成一次挖掘任务的空间路径关键点坐标与挖掘机的工作参数有关，通过计算挖掘机器人工作装置的挖掘参数：挖掘深度、挖掘半径、卸载高度等，规划挖掘机空间挖掘路径：挖掘控制点坐标、提升坐标、卸载坐标、下一挖掘点坐标，其坐标点数据如表1，从而完成一次挖掘，绘制一次作业挖掘空间曲线如图5所示；

运动学逆解过程：

通过几何法对挖掘机器人进行运动学逆解求解：

令铲斗末端V点的坐标为(x,y,z)，Q点坐标为(x_q,y_q,z_q)，α、β、γ分别表示CF、CQ和CV与水平面的夹角，则有：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=a\tan \frac{y}{x}\\ {\mathrm{\θ}}_2=\mathrm{\α}+\mathrm{\β}+\mathrm{\γ}\\ {\mathrm{\θ}}_3=\mathrm{\π}-a\cos \frac{{a_2}^2+{a_3}^2-{\mathrm{CQ}}^2}{2a_2{a}_3}\\ {\mathrm{\θ}}_4=\mathrm{\ζ}-{\mathrm{\θ}}_2-{\mathrm{\θ}}_3\end{array}\right.---\left(12\right)$

其中 $CQ=\sqrt{{(x_a-a_1)}^2+y^2+{(z_q-d_1)}^2};CV=\sqrt{{(x-a_1)}^2+y^2+{(z-d_1)}^2}$

$\left\{\begin{array}{c}{x}_q=x-a_{4}sin\mathrm{\ζ}\\ y_q=y\\ z_q=z-a_4\sin \end{array}\right.---\left(13\right)$

其中：ζ＝θ₂+θ₃+θ₄

结合设定的具体任务以及挖掘机器人的结构参数，实现挖掘关键点从位姿空间空间的转化，数据如表1：

表1

在关节空间进行轨迹规划：

对于挖掘机器人机械臂在关节空间进行轨迹规划，通过分段高阶多项式进行计算，详见公式(1)(2)(3)(4)(5)(6)，得到轨迹规划的通式，本实例对自主挖掘过程进行研究，设y＝0；其在XZ平面内挖掘轨迹曲线如图6所示。

轨迹规划仿真：

结合轨迹多项式以及角速度、角加速度表达式公式(1)(2)(3)(4)(5)(6)，以及表1中各工作关键点处各关节角度值，在仿真软件MATAB中编写程序，进行仿真，得到挖掘机器人机械臂完成工作任务时的关节角、角速度、角加速度变化规律，如图7、8、9所示；

根据加速度约束进行多项式最高项确定及仿真验证，通过上一步得到挖掘机器人机械臂的轨迹规划结果，通过曲线变化规律，发现在中间5次多项式段，加速度阶跃比较大，超出规定约束值，这样一来，在挖掘过程中，挖掘机各关节的力矩会急剧增大，对挖掘机的结构损害非常大，本文中，根据熟练操作人员操作该型号挖掘机进行多次挖掘作业并记录工作装置各关节运动数据，并以此作为轨迹规划过程中各关关节的角加速度约束条件的参考，如表2中所示，结合步骤六中的轨迹规划仿真结果以及加速度约束公式(10)、(11)分别求出动臂、斗杆、铲斗关节轨迹多项式最高阶次数为b₁＝6.2,b₂＝7.5,b₃＝7.8，实现轨迹多项式二次规划，其轨迹规划结果如图10、图11、图12所示，修正前后各关节加速度变化范围如表2所示，结果表明多项式最高阶次数修正后轨迹规划使得挖掘机器人各机械臂转角更加平稳、连续、无较大振动冲击，保证挖掘机器人各关节结构能够平稳的进行挖掘作业。

表2

以上所述的实施例，只是本发明较优选的具体实施方式的一种，本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。

Claims (7)

1.一种串联机构轨迹规划方法，其特征在于，所述方法通过分段高阶多项式插值法计算串联机构的各机构关节角、角速度和角加速度的变化规律，并对超出加速度约束值的轨迹段的连杆进行加速度约束，获得多项式最高阶次数，得到修正后的轨迹规划结果。

2.根据权利要求1所述的方法，其特征在于，所述方法包括：

S1：规划串联机构轨迹曲线；

S2：在关节空间进行轨迹规划；

S3：轨迹规划仿真；

S4：通过加速度约束确定多项式最高项；

S5：仿真验证确定轨迹规划多项式。

3.根据权利要求2所述的方法，其特征在于，所述S1具体为：在串联机构工作过程中，结合机构结构参数，设定其工作任务，规定工作路径中关键点，通过空间样条插值来得到平滑的路径曲线。

4.根据权利要求2所述的方法，其特征在于，所述S2具体为：在关节空间进行轨迹规划时，采用分段高阶多项式进行关节空间轨迹规划，从而得到各关节动态变化规律，得到各关节角、角速度、角加速度变化规律，所述分段高阶多项式为3-3-5-3-3五段式：

引入无量纲时间变量t∈[0,1]，定义变量其中τ∈[τ_i-1,τ_i]为各段轨迹末端的实际时间；τ_i-1为第i段轨迹起始时间；τ_i第i段轨迹末端的实际时间；t_i为第i段轨迹所需要的时间；机器人关节j(j＝1,2,3)的轨迹由多项式序列h_i(t)(i＝1,2,3,4,5)组成；

令串联机构任意一关节五段轨迹多项式表达式如下：

$\left\{\begin{array}{c}{h}_1\left(t\right)=a_{13}{t}^3+a_{12}{t}^2+a_{11}t+a_{10}\\ h_2\left(t\right)=a_{23}{t}^3+a_{22}{t}^2+a_{21}t+a_{20}\\ h_3\left(t\right)=a_{35}{t}^5+a_{34}{t}^4+a_{33}{t}^3+a_{32}{t}^2+a_{31}t+a_{30}\\ h_4\left(t\right)=a_{43}{t}^3+a_{42}{t}^2+a_{41}t+a_{40}\\ h_5\left(t\right)=a_{53}{t}^3+a_{52}{t}^2+a_{51}t+a_{50}\end{array}\right.---\left(1\right)$

对轨迹多项式中实际时间变量τ求一阶导数得到各关节速度：

$v_i\left(t\right)=\frac{{\mathrm{dh}}_i\left(t\right)}{d\mathrm{\τ}}=\frac{{\mathrm{dh}}_i\left(t\right)}{dt}\·\frac{dt}{d\mathrm{\τ}}=\frac{\stackrel{\·}{h_i\left(t\right)}}{ti}---\left(2\right)$

对轨迹多项式中实际时间变量τ求二阶导数得到各关节加速度：

$a_i=\frac{d^2{h}_i\left(t\right)}{{\mathrm{d\τ}}^2}=\frac{\stackrel{\·\·}{h_i\left(t\right)}}{{t_i}^2}---\left(3\right)$

初始点和终止点满足位置、速度、加速度条件有：

$\begin{array}{cc}\left\{\begin{array}{c}{h}_1\left(0\right)=q_0\\ v_1\left(0\right)=v_0\\ a_1\left(0\right)=a_0\end{array}\right.& \left\{\begin{array}{c}{h}_5\left(1\right)=q_f\\ v_5\left(1\right)=v_f\\ a_5\left(1\right)=a_f\end{array}\right.\end{array}---\left(4\right)$

由中间点满足位置要求及位置、速度、加速度连续可得：

$\begin{array}{cc}\left\{\begin{array}{c}{h}_1\left(1\right)=q_1\\ h_2\left(0\right)=h_1\left(1\right)\\ h_2\left(1\right)=q_2\\ h_3\left(0\right)=h_2\left(1\right)\\ h_3\left(1\right)=q_3\\ h_4\left(0\right)=h_3\left(1\right)\\ h_4\left(1\right)=q_4\\ h_5\left(0\right)=h_4\left(1\right)\end{array}\right.& \left\{\begin{array}{c}{v}_2\left(0\right)=v_1\left(1\right)\\ v_3\left(0\right)=v_2\left(1\right)\\ v_4\left(0\right)=v_3\left(1\right)\\ v_5\left(0\right)=v_4\left(1\right)\\ a_2\left(0\right)=a_1\left(1\right)\\ a_3\left(0\right)=a_2\left(1\right)\\ a_4\left(0\right)=a_3\left(1\right)\\ a_5\left(0\right)=a_4\left(1\right)\end{array}\right.\end{array}---\left(5\right)$

结合(5)式和已知条件可以求得多项式的各个系数；

$\begin{array}{cc}\left\{\begin{array}{c}{a}_{10}=q_0\\ a_{11}=v_0{t}_1\\ a_{12}=\frac{1}{2}{a}_0{t_1}^2\\ a_{13}=q_1-\left(a_{10}+a_{11}+a_{12}\right)\end{array}\right.& \left\{\begin{array}{c}{a}_{20}=q_1\\ a_{21}=\left(3a_{13}+2a_{12}+a_{11}\right)\·\frac{t_2}{t_1}\\ a_{22}=\left(3a_{13}+a_{12}\right)\·\frac{{t_2}^2}{{t_1}^1}\\ a_{23}=q_2-\left(a_{20}+a_{21}+a_{22}\right)\end{array}\right.\end{array}$

$\begin{array}{cc}\left\{\begin{array}{c}{a}_{50}=q_4\\ a_{51}=3q_f-2v_f{t}_5+\frac{1}{2}{a}_f{t_5}^2-3a_{50}\\ a_{52}=3v_f{t}_5-a_f{t_5}^2-3q_f+3a_{50}\\ a_{53}=q_f-\left(a_{50}+a_{51}+a_{52}\right)\end{array}\right.& \left\{\begin{array}{c}{a}_{40}=q_3\\ a_{42}=3a_{51}\·\frac{t_4}{t_5}-2a_{52}\frac{{t_4}^2}{{t_5}^2}-3q_4+3a_{40}\\ a_{43}=a_{52}\·\frac{{t_4}^2}{{t_5}^2}-a_{51}\·\frac{t_4}{t_5}+q_4-a_{40}\\ a_{41}=q_4-\left(a_{40}+a_{41}+a_{42}\right)\end{array}\right.\end{array}$

$\left\{\begin{array}{c}{a}_{30}=q_3\\ a_{31}=\left(3a_{23}+2a_{22}+a_{21}\right)\·\frac{t_3}{t_2}\\ a_{32}=\left(3a_{23}+a_{22}\right)\·\frac{{t_3}^2}{{t_2}^2}\\ a_{34}=7a_{41}\·\frac{t_3}{t_4}-2a_{42}\frac{{t_3}^2}{{t_4}^2}-15q_3+3a_{32}+8a_{31}+15a_{30}\\ a_{35}=a_{42}\·\frac{{t_3}^2}{{t_4}^2}-3a_{41}\·\frac{t_3}{t_4}+6q_3-a_{32}-3a_{31}-6a_{30}\\ a_{33}=q_3-\left(a_{30}+a_{31}+a_{34}+a_{35}\right)\end{array}\right.---\left(6\right)$

5.根据权利要求3所述的方法，其特征在于，所述S3由S1中设定的参数得出串联机构各工作关键点数据，通过运动学逆解，从笛卡尔空间转化到关节空间得到各工作关键点处各机构关节角度值，从而得到工作中间连续点值，结合开始设定的初始速度、加速度值以及S2中求得的各关节轨迹多项式以及角速度、角加速度的表达式，通过仿真软件MATLAB进行编程，得到各机构的关节角、角速度、角加速度变化曲线。

6.根据权利要求5所述的方法，其特征在于，所述S4由S3轨迹规划仿真中得出了各关节角加速度的变化曲线图，在进行多项式轨迹规划时，对该段轨迹内，通过对角加速度约束在规定范围内，进而反求出该段轨迹多项式的最高阶的次数，所述中间段轨迹多项式为：

h₃(t)＝a₃₅x^b+a₃₄x⁴+a₃₃x³+a₃₂x²+a₃₁x+a₃₀(7)

则角加速度为：

a₃＝h₃(t)”＝b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x(8)

根据已知连续性条件求得该段多项式的系数为：

$\left\{\begin{array}{c}{a}_{30}=q_3\\ a_{31}=\left(3a_{23}+2a_{22}+a_{21}\right)\frac{t_3}{t_2}\\ a_{32}=\left(3a_{23}+a_{22}\right)\frac{{t_3}^2}{{t_2}^2}\\ a_{34}=\left(1+\frac{6}{b-4}\right)\frac{a_{41}{t}_3}{t_4}-\frac{2a_{42}{t_3}^2}{\left(b-4\right){t_4}^2}-\left(3+\frac{12}{b-4}\right)q_3+\left(1+\frac{2}{b-4}\right)a_{32}+\left(2+\frac{6}{b-4}\right)a_{31}+\left(3+\frac{12}{b-4}\right)a_{30}\\ a_{35}=\frac{\frac{2a_{42}{t_3}^2}{{t_4}^2}-\frac{6a_{41}{t}_3}{t4}+12q_3-2a_{32}-6a_{31}-12a_{30}}{\left(b-3\right)\left(b-4\right)}\\ a_{33}=q_3-\left(a_{30}+a_{31}+a_{32}+a_{34}+a_{35}\right)\end{array}\right.---\left(9\right)$

设该段关节角加速度约束为a_max，则有：|a_3j|≤a_max(j＝1,2,3)即：

|b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x|≤a_max(10)

在MATLAB中通过给定范围内的b值取一定步长进行试算，求解

l_i＝|b(b-1)a₃₅x^b-2+12a₃₄x²+6a₃₃x|_max(i＝1,2,3……n)，x∈[01](11)

得出满足l≤a_max的多项式最高阶次数b值。

7.根据权利要求6所述的方法，其特征在于，所述S5具体为将S4得到的满足l≤a_max的多项式最高阶次数b值代入该段多项式，进行各关节轨迹规划仿真验证，从而分别求出每个关节中间段最高阶次数修正后的合理的轨迹多项式。