CN106647282A - 一种考虑末端运动误差的六自由度机器人轨迹规划方法 - Google Patents

Abstract

本发明公开了一种考虑末端运动误差的六自由度机器人轨迹规划方法，更具体是提出了一种结合旋量理论、三次样条插值算法及粒子群优化算法对机器人末端连续轨迹进行精确规划的方法。该方法首先基于旋量理论建立机器人正逆运动学模型，简化计算过程；在关节空间内采用三次样条插值，从而保证运动平滑；最后以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹。

Description

一种考虑末端运动误差的六自由度机器人轨迹规划方法

技术领域

本发明涉及到一种考虑运动误差的六自由度机器人轨迹规划方法，属于机器人运动控制研究领域，具体涉及通过结合旋量理论、三次样条插值算法以及粒子群优化算法对机器人末端连续轨迹进行规划，进而达到满足追踪精度要求、提高规划效率同时获得光滑运动轨迹的研究目的。

背景技术

轨迹规划作为机器人控制研究的基础，对机器人的综合运动性能具有重要影响。按规划空间进行划分，规划方法一般包括关节空间规划和操作空间规划。关节空间规划方法是指直接对关节变量进行插值规划，最终建立关节变量随时间的变化曲线，但由于无法预知运动过程中末端的轨迹变化情况，只适用于末端点到点的简单操作任务。对于末端具有连续轨迹的操作任务，则需采用操作空间规划方法，该方法得到的运动轨迹虽然具有较高的追踪精度，但无法保证运动平稳性。为了能够在追踪末端轨迹的基础上保证运动的柔顺性，一些学者首先将在操作空间内选取能够保证末端轨迹的关键路径点，然后基于运动学逆解模型计算各关节空间内的轨迹节点，再在关节空间内进行轨迹规划，从而同时保证了基本末端轨迹和运动平稳性。在该类方法中规划轨迹仅能够保证关键路径点处的位置精度，由于关节插值带来的连续轨迹的追踪误差依然无法控制。

在轨迹规划中，末端关键路径点个数越多越能够保证追踪精度，但路径点越多关节角位置间距越小，过小的间距难以体现插值算法在平滑轨迹方面起到的真实作用，且逆解次数越多，轨迹规划计算越复杂，从而降低规划效率。其次插值算法不仅影响着轨迹曲线，其算法关键参数(如时间间隔)也对追踪精度也有一定影响。为了提升规划效率，本专利基于旋量理论建立了正逆运动学模型，与传统的D-H模型相比，具有几何意义清晰、表达无奇异及运算量很少等优点。为获得平滑的运动，关节空间轨迹规划采用三次样条插值算法，该方法规划得到的关节角速度，角加速度连续。为使追踪误差在满足要求的条件下尽量小，本专利采用粒子群优化算法对轨迹进行优化，能够通过获取合理的关键路径点个数N和合适的时间间隔组合达到减小追踪误差的目的。

发明内容

本发明旨在提供一种考虑运动误差的六自由度机器人轨迹规划方法。该方法通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹。

本发明是采用以下技术手段实现的：

S1、基于旋量理论，对机器人模型建立正逆运动学模型。

S2、在末端的连续轨迹上等距取N+1个关键路径点，得到N个轨迹段，N表示轨迹段的数量；并通过运动学逆解模型得到各关节的轨迹节点，采用三次修正样条曲线进行插值规划，从而得到各关节角位移、速度、加速度及加加速度随时间的变化曲线。

S3、在角位移曲线上每隔20ms取点，并通过正运动学模型计算末端位置，建立跟踪误差模型，计算各位置点的追踪误差，并提取最大轨迹跟踪误差max(E_m)。E_m为轨迹跟踪误差，m为

S4、T_n表示第n轨迹段的时间间隔，取T_n＝t,1≤n≤N，按照以上步骤进行规划轨迹，计算最大跟踪误差max(E_m)，如果不满足精度要求max(E_m)＜E_max E_max为根据操作任务限定的最大跟踪误差，取N+1，再计算误差，依次循环计算，直到跟踪误差满足条件。时间间隔t与运动精度限制E_max根据具体任务要求确定。

S5、当N确定之后，以各段时间间隔为设计变量，以各关节角速度、加速度及加加速度为约束条件，以跟踪误差最小为优化目标得到优化轨迹。

本发明的特点是通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量、各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹。

附图说明

图1机器人末端轨迹规划流程图；

图2某六轴工业机器人参数坐标系；

图3各关节角速度、角加速度及角加速度曲线图；

图4末端追踪误差曲线；

具体实施方式

步骤(1)基于旋量理论建立机器人正逆运动学模型

正运动学模型

如图2所示，已知该机器人在初始状态第i个关节所在位置矢量r_i及旋转矢量ω_i如下：

根据旋量理论，关节间的转换矩阵表示为指数积形式，

式中表示第i个关节旋量，θ_i为第i个关节角位移；可由ω_i＝[ω₁ ω₂ ω₃]定义为则ν_i是第i个关节运动的旋转线速度，ν_i＝-ω_i×r_i。

则机器人正运动学模型g_st(θ)表示如下：

逆运动学模型

本方法将各关节角的求解转化为三个Paden-Kahan子问题，由于该机器人末端的位置决定于关节1,2和3，而其位姿决定于关节4,5和6。首先将前三个关节的逆向运动描述为：末端位置向量r_e绕关节1旋转-θ₁至r_e1，再绕关节2旋转-θ₂至r_e2，然后绕关节3旋转-θ₃至r₅，则θ₁，θ₂和θ₃分别通过以下三个表达式求出，其中式(5)属于子问题1，而式(6)和(7)属于子问题3。

其中r_e1可以由末端位置向量r_e＝[x y z]决定；δ₂,δ₃为定距离，δ₂＝||r_e1-r₂||，δ₃＝||r_e-r₃||。

其次θ₄，θ₅和θ₆分别通过以下三个表达式求出，其中式(8)属于子问题2，式(9)属于子问题1，

其中r₀₄在旋转轴6上而不在旋转轴4和5上，取r₀₄＝[0 744 0]；r₀₆不在旋转轴6上，取r₀₆＝[0 150 860]；

步骤(2)求取各关节轨迹节点并进行插值规划

将末端连续轨迹曲线定义如下，其姿态保持Ω＝[0 0 1]，且操作任务总时间T不大于1min。

其中0≤α≤360°，为等距离取N+1个关键路径点，取α＝(360n/N)°,n＝0,1,2,…N。

将得到的末端位姿代入逆运动学模型可求得各关节轨迹节点N+1个。本专利采用三次样条曲线对关节轨迹节点进行插值运算，对某一关节而言，关节轨迹被分为N个子段，第n段(t∈[t_n-1,t_n])轨迹角位移S_n(t)、角速度S′_n(t)、角加速度S″_n(t)可表示如下：

其中T_n为第n段时间间隔；θ_n,分别为时间t_n所对应的关节角位移，角速度和角加速度，且可通过边界条件S_n(t_n-1)＝θ_n-1,S_n(t_n)＝θ_n及S′_n(t_n)＝S′_n+1(t_n)计算得到。

步骤(3)建立末端追踪误差模型

将规划得到的各关节角位移曲线上每隔20ms取值，得到M＝T/0.02个关节点组，并通过正运动学模型计算各关节点组对应的末端位姿如下，

其中θ_1m…θ_6m为第m个关节点组；提取g_m中对应的位置矢量P_m。

建立末端误差模型E_m如下，

E_m＝||P_m-O||-R (15)

其中O为轨迹圆心的位置矢量，R为连续轨迹上各点曲率半径，R＝150mm。

步骤(4)确定末端关键路径点个数n

取T_n＝2，对末端轨迹按照步骤2对各关节进行轨迹规划，再按照步骤3求出末端最大误差值max(E_m)，该任务要求末端误差满足以下条件，max(E_m)≤E_max＝1mm，若计算结果不满足任务要求，则增大路径点个数N，继续规划并计算末端误差，依次循环计算直到满足末端精度要求，从而确定N＝24。

步骤(5)以末端误差最小为目标函数优化轨迹

根据步骤(2)求得各关节角速度，角加速度和角加加速度最大值如下，

S′_max＝max(|{S′_n(t),1≤n≤24}|) (19)

S″_max＝max(|{S″_n(t),1≤n≤24}|) (20)

S″′_max＝max(|{S″′_n(t),1≤n≤24}|) (21)

则优化约束条件可表示如下，

其中时间间隔T_n范围根据具体任务确定，和为第i个关节所允许的最大角速度、角加速度和角加加速度值，具体取值如下，

以T_n,1≤n≤24为设计变量，优化目标函数表示为，

f＝min(E_max) (23)

本专利采用PSO优化算法对以上问题进行优化求解，优化结果如下：

末端最大误差为0.60044mm，与初始时间间隔下的误差0.8019mm相比分别减小了25.12％，总时间由48s减小至47.1487s，且各关节角速度、角加速度及角加加速度均远小于限定值。

Claims (2)

1.一种考虑末端运动误差的六自由度机器人轨迹规划方法，其特征在于：该方法通过在末端连续轨迹上等距离取关键路径点，对采用旋量运动学模型逆解得到的各关节角位置进行插值规划，首先以关键点个数为变量将末端跟踪误差控制在要求范围以内，再以各段时间间隔为设计变量，各关节最大角速度、角加速度及角加加速度为约束条件，以追踪误差最小为优化目标对轨迹进行优化，从而获得规划效率高、跟踪误差小且运动平滑的规划轨迹；

S1、基于旋量理论，对机器人模型建立正逆运动学模型；

S2、在末端的连续轨迹上等距取N+1个关键路径点，得到N个轨迹段，N表示轨迹段的数量；并通过运动学逆解模型得到各关节的轨迹节点，采用三次修正样条曲线进行插值规划，从而得到各关节角位移、速度、加速度及加加速度随时间的变化曲线；

S3、在角位移曲线上每隔20ms取点，并通过正运动学模型计算末端位置，建立跟踪误差模型，计算各位置点的追踪误差，并提取最大轨迹跟踪误差max(E_m)；E_m为轨迹跟踪误差，m为

S4、T_n表示第n轨迹段的时间间隔，取T_n＝t,1≤n≤N，按照以上步骤进行规划轨迹，计算最大跟踪误差max(E_m)，如果不满足精度要求max(E_m)＜E_max E_max为根据操作任务限定的最大跟踪误差，取N+1，再计算误差，依次循环计算，直到跟踪误差满足条件；时间间隔t与运动精度限制E_max根据具体任务要求确定；

S5、当N确定之后，以各段时间间隔为设计变量，以各关节角速度、加速度及加加速度为约束条件，以跟踪误差最小为优化目标得到优化轨迹。

2.根据权利要求1所述的一种考虑末端运动误差的六自由度机器人轨迹规划方法，其特征在于：步骤(1)基于旋量理论建立机器人正逆运动学模型

正运动学模型

已知该机器人在初始状态第i个关节所在位置矢量r_i及旋转矢量ω_i如下：

$\begin{array}{c}\begin{array}{ccc}{r}_1=\[\begin{array}{ccc}0& 0& 0\end{array}\]& r_2=\[\begin{array}{ccc}0& 150& 250\end{array}\]& r_3=\[\begin{array}{ccc}0& 150& 800\end{array}\]\end{array}\\ r_4=r_5=r_6=\[\begin{array}{ccc}0& 744& 940\end{array}\]\end{array}---\left(1\right)$

$\begin{array}{ccc}{\mathrm{\ω}}_1=\[\begin{array}{ccc}0& 0& 1\end{array}\]& {\mathrm{\ω}}_2=\[\begin{array}{ccc}1& 0& 0\end{array}\]& {\mathrm{\ω}}_3=\[\begin{array}{ccc}1& 0& 0\end{array}\]\\ {\mathrm{\ω}}_4=\[\begin{array}{ccc}0& 1& 0\end{array}\]& {\mathrm{\ω}}_5=\[\begin{array}{ccc}1& 0& 0\end{array}\]& {\mathrm{\ω}}_6=\[\begin{array}{ccc}0& 0& 1\end{array}\]\end{array}---\left(2\right)$

根据旋量理论，关节间的转换矩阵表示为指数积形式，

$\exp \left({\widehat{\mathrm{\ξ}}}_i{\mathrm{\θ}}_i\right)=\left[\begin{array}{cc}\exp \left({\widehat{\mathrm{\ω}}}_i{\mathrm{\θ}}_i\right)& (I-\exp ({\widehat{\mathrm{\ω}}}_i{\mathrm{\θ}}_i\left)\right)({\mathrm{\ω}}_i\×v_i)+{\mathrm{\θ\ω}}_i{{\mathrm{\ω}}_i}^T{v}_i\\ 0& 1\end{array}\right]---\left(3\right)$

式中表示第i个关节旋量，θ_i为第i个关节角位移；可由ω_i＝[ω₁ ω₂ ω₃]定义为则ν_i是第i个关节运动的旋转线速度，ν_i＝-ω_i×r_i；

则机器人正运动学模型g_st(θ)表示如下：

$g_{st}\left(\mathrm{\θ}\right)=\exp \left({\widehat{\mathrm{\ξ}}}_1{\mathrm{\θ}}_1\right)\exp \left({\widehat{\mathrm{\ξ}}}_2{\mathrm{\θ}}_2\right)...\exp \left({\widehat{\mathrm{\ξ}}}_6{\mathrm{\θ}}_6\right)g_{st}\left(0\right)---\left(4\right)$

逆运动学模型

本方法将各关节角的求解转化为三个Paden-Kahan子问题，由于该机器人末端的位置决定于关节1,2和3，而其位姿决定于关节4,5和6；首先将前三个关节的逆向运动描述为：末端位置向量r_e绕关节1旋转-θ₁至r_e1，再绕关节2旋转-θ₂至r_e2，然后绕关节3旋转-θ₃至r₅，则θ₁，θ₂和θ₃分别通过以下三个表达式求出，其中式(5)属于子问题1，而式(6)和(7)属于子问题3；

$\exp \left({\widehat{\mathrm{\ξ}}}_1{\mathrm{\θ}}_1\right)r_{e1}=r_e---\left(5\right)$

$\left|\right|r_{e2}-\exp \left({\widehat{\mathrm{\ξ}}}_2{\mathrm{\θ}}_2\right)r_{e1}\left|\right|={\mathrm{\δ}}_2---\left(6\right)$

$\left|\right|r_e-\exp \left({\widehat{\mathrm{\ξ}}}_3{\mathrm{\θ}}_3\right)r_{e2}\left|\right|={\mathrm{\δ}}_3---\left(7\right)$

其中r_e1由末端位置向量r_e＝[x y z]决定；δ₂,δ₃为定距离，δ₂＝||r_e1-r₂||，δ₃＝||r_e-r₃||；

其次θ₄，θ₅和θ₆分别通过以下三个表达式求出，其中式(8)属于子问题2，式(9)属于子问题1，

$\exp \left({\widehat{\mathrm{\ξ}}}_4{\mathrm{\θ}}_4\right)\exp \left({\widehat{\mathrm{\ξ}}}_5{\mathrm{\θ}}_5\right)r_{04}=g_1{r}_{04}---\left(8\right)$

$\exp \left({\widehat{\mathrm{\ξ}}}_6{\mathrm{\θ}}_6\right)r_{06}=g_2{r}_{06}---\left(9\right)$

其中r₀₄在旋转轴6上而不在旋转轴4和5上，取r₀₄＝[0 744 0]；r₀₆不在旋转轴6上，取r₀₆＝[0 150 860]；

步骤(2)求取各关节轨迹节点并进行插值规划

将末端连续轨迹曲线定义如下，其姿态保持Ω＝[0 0 1]，且操作任务总时间T不大于1min；

$\left\{\begin{array}{c}x=-850+150cos\mathrm{\α}\\ y=500+150sin\mathrm{\α}\\ z=300\end{array}\right.---\left(10\right)$

其中0≤α≤360°，为等距离取N+1个关键路径点，取α＝(360n/N)°,n＝0,1,2,…N；

将得到的末端位姿代入逆运动学模型可求得各关节轨迹节点N+1个；本专利采用三次样条曲线对关节轨迹节点进行插值运算，对某一关节而言，关节轨迹被分为N个子段，第n段(t∈[t_n-1,t_n])轨迹角位移S_n(t)、角速度S′_n(t)、角加速度S″_n(t)可表示如下：

$S_n\left(t\right)=\frac{{(t_n-t)}^3}{6T_n}{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}+\frac{{(t-t_{n-1})}^3}{6T_n}{\stackrel{\·\·}{\mathrm{\θ}}}_n+\left({\mathrm{\θ}}_{n-1}-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}{T_n}^2}{6}\right)\frac{t_n-t}{T_n}+\left({\mathrm{\θ}}_n-\frac{\stackrel{\·\·}{\mathrm{\θ}}{T_n}^2}{6}\right)\frac{t-t_{n-1}}{T_n}---\left(11\right)$

$S_n^{\′}\left(t\right)=\frac{1}{2T_n}\[\left({\stackrel{\·\·}{\mathrm{\θ}}}_n-{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}\right)t^2-2\left({\stackrel{\·\·}{\mathrm{\θ}}}_n{t}_{n-1}-{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}{t}_n\right)t+{\stackrel{\·\·}{\mathrm{\θ}}}_n{t}_{n-1}^2-{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}{t}_n^2\]+\frac{{\mathrm{\θ}}_n-{\mathrm{\θ}}_{n-1}}{T_n}-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_n-{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}}{6}{T}_n---\left(12\right)$

$S_n^{\′\′}\left(t\right)=\frac{t_n-t}{T_n}{\stackrel{\·\·}{\mathrm{\θ}}}_{n-1}+\frac{t-t_{n-1}}{T_n}{\stackrel{\·\·}{\mathrm{\θ}}}_n---\left(13\right)$

其中T_n为第n段时间间隔；θ_n,分别为时间t_n所对应的关节角位移，角速度和角加速度，且可通过边界条件S_n(t_n-1)＝θ_n-1,S_n(t_n)＝θ_n及S′_n(t_n)＝S′_n+1(t_n)计算得到；

步骤(3)建立末端追踪误差模型

将规划得到的各关节角位移曲线上每隔20ms取值，得到M＝T/0.02个关节点组，并通过正运动学模型计算各关节点组对应的末端位姿如下，

$g_m=\exp \left({\widehat{\mathrm{\ξ}}}_1{\mathrm{\θ}}_{1m}\right)\exp \left({\widehat{\mathrm{\ξ}}}_2{\mathrm{\θ}}_{2m}\right)...\exp \left({\widehat{\mathrm{\ξ}}}_6{\mathrm{\θ}}_{6m}\right)g_{st}\left(0\right)---\left(14\right)$

其中θ_1m…θ_6m为第m个关节点组；提取g_m中对应的位置矢量P_m；

建立末端误差模型E_m如下，

E_m＝||P_m-O||-R (15)

其中O为轨迹圆心的位置矢量，R为连续轨迹上各点曲率半径，R＝150mm；

步骤(4)确定末端关键路径点个数n

取T_n＝2，对末端轨迹按照步骤2对各关节进行轨迹规划，再按照步骤3求出末端最大误差值max(E_m)，该任务要求末端误差满足以下条件，max(E_m)≤E_max＝1mm，若计算结果不满足任务要求，则增大路径点个数N，继续规划并计算末端误差，依次循环计算直到满足末端精度要求，从而确定N＝24；

步骤(5)以末端误差最小为目标函数优化轨迹

根据步骤(2)求得各关节角速度，角加速度和角加加速度最大值如下，

S′_max＝max(|{S′_n(t),1≤n≤24}|) (19)

S″_max＝max(|{S″_n(t),1≤n≤24}|) (20)

S″′_max＝max(|{S″′_n(t),1≤n≤24}|) (21)

则优化约束条件可表示如下，

$\left\{\begin{array}{c}1\≤T_n\≤3\\ S_{\max i}^{\′}\≤{\stackrel{\·}{\mathrm{\θ}}}_{\max i}\\ S_{\max i}^{\′\′}\≤{\stackrel{\·\·}{\mathrm{\θ}}}_{\max i}\\ S_{\max i}^{\′\′\′}\≤{\stackrel{\·\·\·}{\mathrm{\θ}}}_{\max i}\end{array}\right.---\left(22\right)$

其中时间间隔T_n范围根据具体任务确定，和为第i个关节所允许的最大角速度、角加速度和角加加速度值；

优化目标函数表示为，

f＝min(E_max) (23)。