CN102298391A - 一种重载工业机器人操作空间内运动轨迹规划方法 - Google Patents

Abstract

一种重载工业机器人操作空间内运动轨迹规划方法，它涉及一种机器人运动轨迹的规划方法。本发明为了解决现有重载机器人运动规划方法中在空间多点样条起始点和终止点加速度不连续的问题，没有实现运动指令光滑性；以及现有重载机器人运动规划没有实现时间最优、能耗最优两者的最佳折衷的问题。技术要点：本方法通过附加项对传统三次样条进行了修正。使其在整个过程中速度、加速度上均保持连续。同时综合考虑时间和能耗最优问题，基于动力学模型选取系统能耗和执行时间作为优化目标，速度、转矩、加加速度作为约束条件，建立多目标优化模型，采用NSGA2非支配遗传算法进行优化模型求解。适用于重载工业机器人的轨迹规划、运动规划等离线计算过程。

Description

一种重载工业机器人操作空间内运动轨迹规划方法

技术领域

本发明涉及一种机器人运动轨迹的规划方法，属于机器人运动控制技术领域。

背景技术

高速重载工业机器人广泛用于汽车制造，航空制造业，石化冶金，港口物流行业。高速重载机器人对于提高生产效率，增加生产线柔性制造能力，提高工业自动化水平有着重要意义。重载机器人的运动轨迹规划是保证重载工业机器人高效、可靠、平稳运行的关键核心技术之一。目前重载机器人运动规划存在以下两点基本要求：

1、要生成运动轨迹必须足够光滑。由此运动规划算法需要选择一个足够光滑的基函数，其需要具备位置指令C2连续，速度指令C1连续，加速度指令连续，加加速度指令有界。如果运动规划不够合理(例如不可微的速度信号，不连续的加速度信号，无界的加加速度信号)，会导致机器人系统定位精度和轨迹跟踪精度的降低，残余振动的发生，从而使机器人性能严重降低，甚至损坏精密传动部件，减少机器人的使用寿命。

2、要满足任务要求的各点之间所生成的运动轨迹保证运行时间最短，运行能耗最小。运行时间最小有利于生产效率提高、生产节拍的加快，降低生产成本。运行能耗最小则是从降低能耗的角度对运动规划进行要求，这点尤其在港口，冶金行业重载堆垛搬运机器人应用上更为重要。

对于以上要求，目前国内外机器人及相关运动控制系统所采用的类似技术存在以下问题。首先目前重载机器人和主流的运动控制系统在空间多点样条插补时无法保证在起始点和终止点的加速度连续，即无法保证加速度边界为0。这点需要进行改进。其次国内外已知公开算法皆存在不足。如针对固定路径，Shin KG提出了时间和能耗最优的运动规划算法[参见学术论文“K.G.Shin，N.D.McKay.“Minimum-time control of robotic manipulators with geometricpath constraints”IEEE Trans Automat Control，vol.30，pp.531-541，Jun.1985”和“K.G.Shin，N.D.McKay.“A Dynamic programming approach to trajectoryplanning of robotic manipulators”IEEE Trans Automat Control.vol.31，pp.491-500，Jun.1986.“]，但是其产生的加速度和力矩指令是不连续的，不能满足上面第一点的要求，无法实际应用。通过对力矩指令限幅，Constantinescu在保证光滑性的同时，提出了一种针对时间最优的算法，但是其缺乏对能耗的考虑[参见学术论文“B.J.Martin，J.E.Bobrow.Minimum effort motions for openchain manipulators with task-dependent end-effector constraints.Int J RobotRes，vol.18，pp.213-224，Feb.1999.”]。Saramago and Steffen针对能耗最优提出了相应运动规划算法，其将执行时间和能耗作为优化目标，但是其所得优化结果依赖于两个目标各自的权重，对权重的选取较为敏感[参见学术论文“S.F.P.Saramago，J.V.Steffen，Optimization of the trajectory planning of robotmanipulators taking into account the dynamics of the system.Mech and MachTheory，vol.33，pp.883-894，Oct.1998.”]。

发明内容

本发明为了解决现有重载机器人运动规划方法中在空间多点样条起始点和终止点加速度不连续的问题，没有实现运动指令光滑性；以及现有重载机器人运动规划没有实现时间最优、能耗最优两者的最佳折衷的问题，进而提供了一种重载工业机器人操作空间内运动轨迹规划方法。

本发明为解决上述技术问题采取的技术方案是：

本发明所述的一种重载工业机器人操作空间内运动轨迹规划方法的具体过程为：

步骤A、针对空间内多点采用修正的样条函数进行插值：

步骤A1、采用三次样条曲线建立基础插补函数，在任意两个预设点之间采用三次多项式进行插补，基础插补函数的表达式如下：

S_j(t)＝a_j+b_j(t-t_j)+c_j(t-t_j)²+d_j(t-t_j)³

t∈[t_j，t_j+1]，j＝0，1，...，n-2                (1)

两个相邻预设点之间的时间间隔为h_j＝t_j+1-t_j；a_j、b_j、c_j、d_j为待求系数；n表示预设点数量；

步骤A2、同时在各预设点为保证连续性必须保证函数值S_j(t)的一阶导数值S′_j(t)和函数值S_j(t)的二阶导数值S″_j(t)相等，即为：

S_j(t_j)＝α_j，j＝0，1，..，n-2

S_n-2(t_n-1)＝α_n-1                               (2)

α_j表示各预设点坐标值；

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\′}\left(t_{j+1}\right)=S_{j+1}^{\′}\left(t_{j+1}\right)\\ S_j^{\′\′}\left(t_{j+1}\right)=S_{j+1}^{\′\′}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

步骤A3、采用固定边界条件，满足在起始点和终止点速度为0：

S′₀(t₀)＝0，S′_n-2(t_n-1)＝0                    (4)

步骤A3、建立三次样条整体协调方程如下：

H·C＝S                                         (5)

式中：

$C=\left[\begin{array}{c}{S}_0^{\′\′}\left(t_0\right)\\ S_1^{\′\′}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\′\′}\left(t_{n-3}\right)\\ S_{n-2}^{\′\′}\left(t_{n-2}\right)\\ S_{n-2}^{\′\′}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}(h_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{h_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

步骤A4、对式(1)进行修正，在起始段和终止段增加辅助项，修正后的样条表达式：

S₀(t)＝a₀+b₀(t-t₀)+c₀(t-t₀)²+d₀(t-t₀)³+A₀(t)

S_n-2(t)＝a_n-2+b_n-2(t-t_n-2)+c_n-2(t-t_n-2)²+d_n-2(t-t_n-2)³+A_n-2(t)    (6)

n个预设点数量，有n-1间隔段，即有n-1个表达式；起始段的表达式为S₀(t)，终止段的表达式为S_n-2(t)；A₀(t)，A_n-2(t)为多项式形式的辅助项；

步骤A5、求解多项式形式的辅助项A₀(t)和A_n-2(t)的辅助表达式：

A₀(t)和A_n-2(t)必须满足以下约束方程：

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\′}\left(t_0\right)=A_0^{\′}\left(t_1\right)=0\\ A_0^{\′\′}\left(t_0\right)=-S_0^{\′\′}\left(t_0\right)\\ A_0^{\′\′}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\′}\left(t_{n-2}\right)=A_{n-2}^{\′}\left(t_{n-1}\right)=0\\ A_{n-2}^{\′\′}\left(t_{n-2}\right)=-S_{n-2}^{\′\′}\left(t_{n-2}\right)\\ A_{n-2}^{\′\′}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

求解上述方程可得辅助表达式如下：

$A_0\left(t\right)=S_0^{\′\′}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2h}_0^3\right)$

$A_{n-2}\left(t\right)=S_{n-2}^{\′\′}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

步骤B、通过对各点运行时间间隔的调整，通过建立相应的优化模型，使得重载工业机器人以最优的性能执行完整个曲线段运动：

步骤B1、选择各设定点之间的执行时间h₀，h₁，...，h_n-1作为优化参数(优化变量)，共n个优化变量；

步骤B2、将整个轨迹过程中的执行能耗(电机总能耗)P和执行时间T作为优化目标；

步骤B3、将电机转速(i轴速度)、力矩和加加速度作为约束条件；

步骤B4、式(1)、式(6)和式(8)构成基础插值函数，在基础插值函数确定后，将整个运动规划转化为如下的优化模型(目标)：

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\Σ}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\Σ}}}\underset{i=1}{\overset{m}{\mathrm{\Σ}}}({\∫}_{t_j}^{t_{j+1}}{\mathrm{\τ}}_i\left(\mathrm{\θ}\right)\·d{\mathrm{\θ}}_i)\end{array}\right.---\left(9\right)$

m表示机器人关节数量，T表示执行时间，P表示执行能耗；θ表示机器人各关节转角所组成的转角向量，θ_i表示i轴关节转角；

建立约束条件如下：

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

τ_i表示i轴力矩，v_i表示i轴速度，J_i表示i轴加加速度，τ_imax表示i轴力矩上限，v_imax表示i轴速度上限，J_imax表示i轴加加速度上限；

步骤C、完成以上各步骤后，采用NSGAII型非支配遗传算法对优化参数h₀，h₁，...，h_n-1进行求解；

步骤D、将步骤C得到的优化参数h₀，h₁，...，h_n-1代入式(5)，进而求得待求系数a_j、b_j、c_j、d_j的解集，最终获得由式(1)和式(2)确定的分段式基础插补函数，所述分段式基础插补函数用以表达重载工业机器人操作空间内的运动轨迹。

本发明的有益效果是：

本发明方法综合考虑了指令光滑性、时间和能耗，同时使时间和能耗均达到最优，是针对重载搬运机器人运动指令生成提出了一种具有足够光滑，并且能耗和时间最优的运动轨迹规划方法。本方法适用于包括垂直关节型，直角坐标型，并联机构型在内的各种重载搬运机械手。本方法通过附加项对传统三次样条进行了修正。使其在整个过程中(包括起始点和终止点)速度、加速度上均保持连续。同时综合考虑时间和能耗最优问题，基于动力学模型选取系统能耗和执行时间作为优化目标，速度、转矩、加加速度作为约束条件，建立多目标优化模型。针对多目标求解结果对于权重选择敏感的问题，采用NSGA2非支配遗传算法进行优化模型求解。本发明主要涉及基础样条函数，目标函数，约束条件及求解算法的选择和改进，适用于重载工业机器人的轨迹规划、运动规划等离线计算过程。

针对空间内多点采用修正的样条函数进行插值，保证了所生成的运动指令足够光滑，减少对机械和电气系统冲击。通过对各点运行时间间隔的调整，通过建立相应的优化模型，使得以最优的性能执行完整个曲线段运动。在求解最优参数过程中采用特定的优化算法，可避免多个优化目标求解下对于权值选择的依赖。

附图说明

图1是本发明方法中采用的NSGA-II算法进化流程图；图2是采用的NSGA-II算法过程中整个Pareto解集两个目标函数值曲线图(图中的横坐标为执行时间，单位为秒；纵坐标为执行能耗，单位为焦耳；椭圆区域表示Pareto解集)；

图3a是初始时间间隔的运动间隔分配图(在三维坐标系下，图中有5个预设点；三个坐标轴X、Y、Z的单位为米)，图3b是优化所得时间间隔的运动间隔分配图(在三维坐标系下，图中有5个预设点；三个坐标轴X、Y、Z的单位为米)；图4a是初始时间间隔下机器人各轴功率图(图中的横坐标为运行时间，单位为秒；纵坐标为能耗，单位为焦耳；“Joint”表示“轴”；“sum”表示“总和”)，图4b是优化所得的时间间隔下机器人各轴功率图(图中的横坐标为运行时间，单位为秒；纵坐标为能耗，单位为焦耳；“Joint”表示“轴”；“sum”表示“总和”)；图5a为初始时间间隔下关节速度(轴转速)曲线图(图中的横坐标为运行时间，单位为秒；纵坐标为关节速度，单位为度/秒)，图5b为优化所得时间间隔下关节速度(轴转速)曲线图(图中的横坐标为运行时间，单位为秒；纵坐标为关节速度，单位为度/秒)；图6a为初始时间间隔下关节加速度(轴加速度)曲线图(图中的横坐标为运行时间，单位为秒；纵坐标为关节速度，单位为度/平方秒)，图6b为优化所得时间间隔下关节加速度(轴加速度)曲线图(图中的横坐标为运行时间，单位为秒；纵坐标为关节速度，单位为度/平方秒)；图7a为初始时间间隔下关节力矩(轴力矩)曲线图(图中的横坐标为运行时间，单位为秒；纵坐标为关节力矩，单位为牛顿·米)，图7b为优化所得时间间隔下关节力矩(轴力矩)曲线图(图中的横坐标为运行时间，单位为秒；纵坐标为关节力矩，单位为牛顿·米)。

具体实施方式

具体实施方式一：本实施方式所述的重载工业机器人操作空间内运动轨迹规划方法的具体过程为：

步骤A、针对空间内多点采用修正的样条函数进行插值：

步骤A1、采用三次样条曲线建立基础插补函数，在任意两个预设点之间采用三次多项式进行插补，基础插补函数的表达式如下：

S_j(t)＝a_j+b_j(t-t_j)+c_j(t-t_j)²+d_j(t-t_j)³

t∈[t_j，t_j+1]，j＝0，1，...，n-2                    (1)

两个相邻预设点之间的时间间隔为h_j＝t_j+1-t_j；a_j、b_j、c_j、d_j为待求系数；n表示预设点数量；

步骤A2、同时在各预设点为保证连续性必须保证函数值S_j(t)的一阶导数值S′_j(t)和函数值S_j(t)的二阶导数值S″_j(t)相等，即为：

S_j(t_j)＝α_j，j＝0，1，...，n-2

S_n-2(t_n-1)＝α_n-1                                  (2)

α_j表示各预设点坐标值；

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;}\left(t_{j+1}\right)\\ S_j^{\&prime;\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;\&prime;}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

步骤A3、采用固定边界条件，满足在起始点和终止点速度为0：

S′₀(t₀)＝0，S′_n-2(t_n-1)＝0                        (4)

步骤A3、建立三次样条整体协调方程如下：

H·C＝S                                            (5)

式中：

$C=\left[\begin{array}{c}{S}_0^{\&prime;\&prime;}\left(t_0\right)\\ S_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}(h_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{h_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

步骤A4、对式(1)进行修正，在起始段和终止段增加辅助项，修正后的样条表达式：

S₀(t)＝a₀+b₀(t-t₀)+c₀(t-t₀)²+d₀(t-t₀)³+A₀(t)

S_n-2(t)＝a_n-2+b_n-2(t-t_n-2)+c_n-2(t-t_n-2)²+d_n-2(t-t_n-2)³+A_n-2(t)    (6)

n个预设点数量，有n-1间隔段，即有n-1个表达式；起始段的表达式为S₀(t)，终止段的表达式为S_n-2(t)；A₀(t)，A_n-2(t)为多项式形式的辅助项；

步骤A5、求解多项式形式的辅助项A₀(t)和A_n-2(t)的辅助表达式：

A₀(t)和A_n-2(t)必须满足以下约束方程：

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ A_0^{\&prime;\&prime;}\left(t_0\right)=-S_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

求解上述方程可得辅助表达式如下：

$A_0\left(t\right)=S_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2h}_0^3\right)$

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

步骤B、通过对各点运行时间间隔的调整，通过建立相应的优化模型，使得重载工业机器人以最优的性能执行完整个曲线段运动：

步骤B1、选择各设定点之间的执行时间h₀，h₁，...，h_n-1作为优化参数(优化变量)，共n个优化变量；

步骤B2、将整个轨迹过程中的执行能耗(电机总能耗)P和执行时间T作为优化目标；

步骤B3、将电机转速(i轴速度)、力矩和加加速度作为约束条件；

步骤B4、式(1)、式(6)和式(8)构成基础插值函数，在基础插值函数确定后，将整个运动规划转化为如下的优化模型(目标)：

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

m表示机器人关节数量，T表示执行时间，P表示执行能耗；θ表示机器人各关节转角所组成的转角向量，θ_i表示i轴关节转角；

建立约束条件如下：

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

τ_i表示i轴力矩，v_i表示i轴速度，J_i表示i轴加加速度，τ_imax表示i轴力矩上限，v_imax表示i轴速度上限，J_imax表示i轴加加速度上限；

步骤C、完成以上各步骤后，采用NSGAII型非支配遗传算法对优化参数h₀，h₁，...，h_n-1进行求解；NSGAII型非支配遗传算法为现有技术范畴；

步骤D、将步骤C得到的优化参数h₀，h₁，...，h_n-1代入式(5)，进而求得待求系数a_j、b_j、c_j、d_j的解集，最终获得由式(1)和式(2)确定的分段式基础插补函数，所述分段式基础插补函数用以表达重载工业机器人操作空间内的运动轨迹。

各变量明细参见表1：

表1变量明细

m	机器人关节数量
		n	预设点数量
αj	各预设点坐标值
		hj	相邻预设点之间时间间隔
T	执行时间
		P	执行能耗
τi	i轴力矩
		vi	i轴速度
Ji	i轴加加速度
		τimax	i轴力矩上限
vimax	i轴速度上限
		Jimax	i轴力矩上限

针对上述具体实施方式再进一步具体化：

一、本专利在常规三次样条曲线的基础上进行相应改进。

具体如下：采用三次样条曲线在各预设的过渡点可以保证速度连续，加速度连续。任意两个预设点之间采用三次多项式进行插补如下所示

S_j(t)＝a_j+b_j(t-t_j)+c_j(t-t_j)²+d_j(t-t_j)³

t∈[t_j，t_j+1]，j＝0，1，...，n-2            (1)

两个预设点之间的时间间隔为h_j＝t_j+1-t_j，同时在各预设点为保证连续性，必须保证函数值S_j(t)，一阶导数值S′_j(t)，二阶导数值S″_j(t)相等，即为

S_j(t_j)＝α_j，j＝0，1，..，n-2

S_n-2(t_n-1)＝αn-1                        (2)

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;}\left(t_{j+1}\right)\\ S_j^{\&prime;\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;\&prime;}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

同时必须满足在起始点和终止点速度为0：

S′₀(t₀)＝0，S′_n-2(t_n-1)＝0             (4)

三次样条整体协调方程如下

H·C＝S                                  (5)

此处

$C=\left[\begin{array}{c}{S}_0^{\&prime;\&prime;}\left(t_0\right)\\ S_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}(h_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{h_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

通过求解上述方程，可以得到样条表达式，但是虽然在起始点和终止点速度边界可以保证为0，或其他设定值，但是其加速度边界不为0，从而导致在起始点和终止点加速度信号为阶跃信号，这会导致对系统低阶振动模态的激振，因此需要对普通的三次样条插值进行修正，在起始段和终止段增加辅助项

S₀(t)＝a₀+b₀(t-t₀)+c₀(t-t₀)²+d₀(t-t₀)³+A₀(t)

S_n-2(t)＝a_n-2+b_n-2(t-t_n-2)+c_n-2(t-t_n-2)²+d_n-2(t-t_n-2)³+A_n-2(t)    (6)

A₀(t)，A_n-2(t)为多项式形式的辅助项，其必须满足以下约束方程。

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ A_0^{\&prime;\&prime;}\left(t_0\right)=-S_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

求解上述方程可得辅助表达式如下：

$A_0\left(t\right)=S_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2h}_0^3\right)$

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

二、在基础插值函数确定后，整个运动规划问题可以转化为如下的优化问题。

优化目标如下：

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

约束条件如下：

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

优化变量是h₀，h₁，...，h_n-1，执行时间和执行能耗作为优化目标。执行时间被作为优化目标是应为其与生产效率直接相关。同时在重载机器人作业过程中，有效的能耗转化成机器人操作对象势能的变化，其他的能量(如系统的动能)均转化为热能耗散。该模型中将电机所做的总功率同样作为优化目标。在优化模型中含有三个约束条件，分别是转矩极限，速度极限和加加速度极限，其中转矩和速度极限是由驱动部件决定的，由于过大的加加速度会激起机械系统的谐振，增加机械系统的磨损，因此加加速度同样也做为约束条件之一。

三、针对整个优化模型，为避免多个优化目标求解下对于权值选择的依赖，采用多目标非支配遗传算法NSGA-II进行求解(参见图1和图2)。对于多目标优化问题，通常存在一个解集。就目标函数而言，这些解之间是无法比较优劣的，其特点是：无法在改进任何目标函数的同时不削弱至少一个其他目标函数，这些解称作Pareto最优解或非支配解。求解多目标优化问题的主要任务是：毫无偏好地找到尽可能多的具有代表性的符合要求的Pareto最优解，在计算得到均匀分布的Pareto最优解之后，根据具体实际要求从中客观地选择最满意的优化结果。NSGA-II算法进化流程，如图1和2所示，具体过程为：

(1)随机产生种群规模为N的初始父代种群P₀，并通过遗传算子(交叉、变异)产生子代种群Q₀，其种群大小也为N；

(2)将父代种群P_n和子代种群Q_n合并组成规模为2N的合成种群R_n；进行快速非支配排序，将R_n中的全部2N个个体按非支配序号(等级)重新分类，得到等级F₁，F₂，F₃…；计算每一非支配层的个体局部拥挤距离并排序。

(3)根据排序结果选取N个个体作为新的父代种群P_n+1；

(4)通过遗传算子(选择、交叉、变异)产生新子代种群Q_n+1；

(5)重复(2)至(4)步骤直到达到算法设置的最大迭代次数。

该算法中对合并的亲代种群和子代种群进行非支配排序并填充新种群的过程，如图1所示。

当将种群数量设为1000，进化代数设为50代时，整个进化所得结果见图2，整个优化求解耗时2.4秒。得到的Pareto的解集如表2所示，其中第一组解被选为最终解。

表2NSGA2获得最优运动规划参数PARETO解集

No.	h1(S)	h2(S)	h3(S)	h4(S)	T(S)	P(J)(J)
							1	0.1674	0.2394	0.2992	0.5788	1.2848	2601.261
2	0.1668	0.2394	0.2992	0.5789	1.2843	2602.001
							3	0.1667	0.2393	0.2987	0.5789	1.2836	2602.559
4	0.1672	0.2383	0.2994	0.5781	1.2831	2603.299
							5	0.1666	0.2381	0.2997	0.5783	1.2828	2604.093
6	0.1696	0.2363	0.3012	0.5745	1.2816	2605.092
							7	0.1660	0.2379	0.2998	0.5779	1.2816	2605.590
8	0.1658	0.2376	0.2998	0.5779	1.2810	2606.258
							9	0.1682	0.2361	0.3012	0.5746	1.2800	2606.725
10	0.1679	0.2357	0.3008	0.5746	1.2789	2607.665

整个优化运动算法计算示例如下，现在存在5个预设点(见图3a，黑点标出)。不加优化，各点之间的运行时间为0.5，0.5，0.25，1.25秒。优化后从图3b可知，采用优化所得时间间隔后执行完整个轨迹段所需时间是1.2848秒，为初始时间间隔下总时间的一半。从图4b可知总消耗功率为2606焦耳，同样比初始时间间隔下的功率值要小。尽管优化所得运动要比初始运动快很多，但是每个关节的速度和力矩值均未上升很多，都在允许范围内，见图5a、5b、图7a和7b。由图6a可知在初始参数下，运动起始点和终止点的加速度均不为0，从而导致加加速度为无穷大。对系统将产生冲击。而采用改进三次样条计算所得的优化参数后，起始点和终止点加速度值为0，整个运动过程足够平滑。

Claims (1)

1.一种重载工业机器人操作空间内运动轨迹规划方法，其特征在于：所述规划方法的具体过程为：

步骤A、针对空间内多点采用修正的样条函数进行插值：

步骤A1、采用三次样条曲线建立基础插补函数，在任意两个预设点之间采用三次多项式进行插补，基础插补函数的表达式如下：

S_j(t)＝a_j+b_j(t-t_j)+c_j(t-t_j)²+d_j(t-t_j)³

t∈[t_j，t_j+1]，j＝0，1，...，n-2                (1)

两个相邻预设点之间的时间间隔为h_j＝t_j+1-t_j；a_j、b_j、c_j、d_j为待求系数；n表示预设点数量；

步骤A2、同时在各预设点为保证连续性保证函数值S_j(t)的一阶导数值S′_j(t)和函数值S_j(t)的二阶导数值S″_j(t)相等，即为：

S_j(t_j)＝α_j，j＝0，1，...，n-2

S_n-2(t_n-1)＝α_n-1                               (2)

α_j表示各预设点坐标值；

$\left\{\begin{array}{c}{S}_j\left(t_{j+1}\right)=S_{j+1}\left(t_{j+1}\right)\\ S_j^{\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;}\left(t_{j+1}\right)\\ S_j^{\&prime;\&prime;}\left(t_{j+1}\right)=S_{j+1}^{\&prime;\&prime;}\left(t_{j+1}\right)\end{array}\right.,j=\mathrm{0,1},...,n-3---\left(3\right)$

步骤A3、采用固定边界条件，满足在起始点和终止点速度为0：

S′₀(t₀)＝0，S′_n-2(t_n-1)＝0                    (4)

步骤A3、建立三次样条整体协调方程如下：

H·C＝S                                         (5)

式中：

$C=\left[\begin{array}{c}{S}_0^{\&prime;\&prime;}\left(t_0\right)\\ S_1^{\&prime;\&prime;}\left(t_1\right)\\ .\\ .\\ .\\ S_{n-3}^{\&prime;\&prime;}\left(t_{n-3}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ S_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)\end{array}\right],$ $S=\left[\begin{array}{c}\frac{S_0\left(t_1\right)-S_1\left(t_0\right)}{h_1{h}_0}(h_1+h_0)\\ \frac{S_2-S_1}{h_1}-\frac{S_1-S_0}{h_0}\\ .\\ .\\ .\\ \frac{S_{n-2}-S_{n-3}}{h_{n-2}}-\frac{S_{n-3}-S_{n-4}}{h_{n-3}}\\ \frac{S_0\left(t_{n-1}\right)-S_1\left(t_{n-2}\right)}{h_n{h}_0}(h_n+h_0)\end{array}\right]$

步骤A4、对式(1)进行修正，在起始段和终止段增加辅助项，修正后的样条表达式：

S₀(t)＝a₀+b₀(t-t₀)+c₀(t-t₀)²+d₀(t-t₀)³+A₀(t)

S_n-2(t)＝a_n-2+b_n-2(t-t_n-2)+c_n-2(t-t_n-2)²+d_n-2(t-t_n-2)³+A_n-2(t)    (6)

n个预设点数量，有n-1间隔段，即有n-1个表达式；起始段的表达式为S₀(t)，终止段的表达式为S_n-2(t)；A₀(t)，A_n-2(t)为多项式形式的辅助项；

步骤A5、求解多项式形式的辅助项A₀(t)和A_n-2(t)的辅助表达式：

A₀(t)和A_n-2(t)必须满足以下约束方程：

$\left\{\begin{array}{c}{A}_0\left(t_0\right)=A_0\left(t_1\right)=0\\ A_0^{\&prime;}\left(t_0\right)=A_0^{\&prime;}\left(t_1\right)=0\\ A_0^{\&prime;\&prime;}\left(t_0\right)=-S_0^{\&prime;\&prime;}\left(t_0\right)\\ A_0^{\&prime;\&prime;}\left(t_1\right)=0\\ A_{n-2}\left(t_{n-2}\right)=A_{n-2}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;}\left(t_{n-2}\right)=A_{n-2}^{\&prime;}\left(t_{n-1}\right)=0\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)=-S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right)\\ A_{n-2}^{\&prime;\&prime;}\left(t_{n-1}\right)=0\end{array}\right.---\left(7\right)$

求解上述方程可得辅助表达式如下：

$A_0\left(t\right)=S_0^{\&prime;\&prime;}\left(t_0\right){\left({t-t}_0\right)}^2{\left({t-t}_1\right)}^3/\left({2h}_0^3\right)$

(8)

$A_{n-2}\left(t\right)=S_{n-2}^{\&prime;\&prime;}\left(t_{n-2}\right){(t-t_{n-2})}^2{\left({t-t}_{n-1}\right)}^3/\left({2h}_{n-2}^3\right)---\left(8\right)$

步骤B、通过对各点运行时间间隔的调整，通过建立相应的优化模型，使得重载工业机器人以最优的性能执行完整个曲线段运动：

步骤B1、选择各设定点之间的执行时间h₀，h₁，...，h_n-1作为优化参数，共n个优化变量；

步骤B2、将整个轨迹过程中的执行能耗P和执行时间T作为优化目标；

步骤B3、将电机转速、力矩和加加速度作为约束条件；

步骤B4、式(1)、式(6)和式(8)构成基础插值函数，在基础插值函数确定后，将整个运动规划转化为如下的优化模型：

$\min \left\{\begin{array}{c}T=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}{h}_i\\ P=\underset{j=0}{\overset{n-1}{\mathrm{\&Sigma;}}}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}({\&Integral;}_{t_j}^{t_{j+1}}{\mathrm{\&tau;}}_i\left(\mathrm{\&theta;}\right)\&CenterDot;d{\mathrm{\&theta;}}_i)\end{array}\right.---\left(9\right)$

m表示机器人关节数量，T表示执行时间，P表示执行能耗；θ表示机器人各关节转角所组成的转角向量，θ_i表示i轴关节转角；

建立约束条件如下：

$s.t.\left\{\begin{array}{c}\max \left(\right|{\mathrm{\&tau;}}_i\left|\right)<{\mathrm{\&tau;}}_{i\max }\\ \max \left(\right|v_i\left|\right)<v_{i\max }\\ \max \left(\right|J_i\left|\right)<J_{i\max }\end{array}\right.---\left(10\right)$

τ_i表示i轴力矩，v_i表示i轴速度，J_i表示i轴加加速度，τ_imax表示i轴力矩上限，v_imax表示i轴速度上限，J_imax表示i轴加加速度上限；

步骤C、完成以上各步骤后，采用NSGAII型非支配遗传算法对优化参数h₀，h₁，...，h_n-1进行求解；

步骤D、将步骤C得到的优化参数h₀，h₁，...，h_n-1代入式(5)，进而求得待求系数a_j、b_j、c_j、d_j的解集，最终获得由式(1)和式(2)确定的分段式基础插补函数，所述分段式基础插补函数用以表达重载工业机器人操作空间内的运动轨迹。

Images