CN106863306A - 一种机器人关节空间平滑轨迹规划方法 - Google Patents

Abstract

本发明公开了一种机器人关节空间平滑轨迹规划方法，包括步骤：首先，由机器人运动学逆解算法，实现机器人笛卡尔空间运动轨迹向关节空间运动轨迹的映射，获得关节空间关键插补点；然后，依据机器人各个关节的最大速度约束，调整笛卡尔插补周期；接着，借助三次多项式曲线进行关节空间关键插补点的角速度和角加速度估计，为下一步的五次样条曲线拟合提供条件；最后，从第一个关节空间关键插补点开始，依次使用相邻两个关节空间关键插补点构造五次样条曲线，作为关节空间的运动轨迹，再进行关节空间插补。本发明解决机器人关节空间运动轨迹不平滑的问题。

Description

一种机器人关节空间平滑轨迹规划方法

技术领域

本发明涉及串联机器人平滑轨迹规划方法，特别涉及一种机器人关节空间平滑轨迹规划方法。

背景技术

串联机器人在制造业、农业、娱乐业等行业得到越来越广泛的应用。机器人运动最终依靠关节运动实现，关节空间运动轨迹不平滑会造成机器人关节的冲击振动，不仅影响机器人作业质量，而且加快零部件磨损，影响机器人寿命。

目前机器人关节空间平滑运动轨迹规划的研究多以离线的形式进行，需要先进行笛卡尔空间轨迹规划和机器人运动学逆解得到全部关节角时间-节点序列，再进行曲线拟合或者轨迹优化得到平滑关节空间运动轨迹。但是在实际应用中，由于成本等因素限制，机器人控制器的内存资源非常有限，无法一次性储存和运算大量的轨迹点数据。另外，由于机器人运动控制实时性的要求，普遍控制器的插补周期在1ms左右，在如此短时间内无法完成复杂的曲线拟合和优化计算。

发明内容

本发明的目的是提供种机器人关节空间平滑轨迹规划方法，旨在解决在最短时间和最小内存消耗下实现实时在线机器人关节空间平滑运动轨迹规划。

本发明的目的通过下述技术方案来实现：

一种机器人关节空间平滑轨迹规划方法，包括如下步骤：

S1、由机器人运动学逆解算法，实现机器人笛卡尔空间运动轨迹向关节空间运动轨迹的映射，获得关节空间关键插补点；

S2、依据机器人各个关节的最大速度约束，调整笛卡尔插补周期；

S3、借助三次多项式曲线进行关节空间关键插补点的角速度和角加速度估计，为下一步的五次样条曲线拟合提供条件；

S4、从第一个关节空间关键插补点开始，依次使用相邻两个关节空间关键插补点构造五次样条曲线，作为关节空间的运动轨迹，再进行关节空间插补。

进一步地，所述步骤S2包含如下步骤：

S2.1、第j个笛卡尔插补周期机器人6个关节电机的最大转速估计如式(1)所示：

式中，和分别为关节i第j+1个和第j个插补点，T_D为笛卡尔空间插补时间，rⁱ为关节i的减速比；

S2.2、第j个笛卡尔插补周期机器人各关节伺服电机计算转速与最大转速之比为

如果Q小于1，则说明为完成此插补段运动，机器人6个关节中存在电机转速超出最大转速；由于插补运动需要保证各轴在约定插补周期内同时完成运动，机器人6个关节完成此插补段运动所需的插补周期均需要增加；

S2.3、重新计算第j个笛卡尔空间插补周期

式中，int为取整函数；Q为转速比；T_D为默认笛卡尔空间插补周期，T_D＝10ms。

进一步地，所述步骤S3包含如下步骤：

S3.1、令为笛卡尔空间插补和进行机器人运动学逆解之后得到机器人i关节t_j时刻转角值为θ_j的关节空间关键插补点；插补点的角速度和角加速度的估计通过构造三次多项式曲线进行估计；取插补点前一点和后两个点和构造三次多项式曲线；

S3.2、三次多项式曲线表达式如式(4)所示：

θ(t)＝at³+bt²+ct+d (4)

将四个插补点的时间和位置数值代入式(4)可得

将式(5)写成矩阵形式得

则可以解得

S3.3、则插补点的角速度估计为

插补点的角加速度估计为

进一步地，所述步骤S4包含如下步骤：

S4.1、五次样条曲线表达式如式(10)所示

θ(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F (10)

且满足边界条件

写成矩阵的形式

则可以解得

S4.2、按照1毫秒的关节空间插补周期，依次将t代入式(13)计算得到的关节空间五次样条运动曲线，即可得到关节空间的插补点，完成关节空间插补。

本发明相对于现有技术具有如下的优点及效果：

本发明与离线的关节空间平滑轨迹规划和优化方法相比，每次只需要存储4个关键插补点，占用系统资源少，每个笛卡尔插补周期只需分别求解1次三次多项式曲线参数和1次五次多项式曲线参数，计算负担小，可以满足实时计算的要求，能够实现实时在线规划，更具广泛适用性。

附图说明

图1是本发明实施例的机器人关节空间平滑轨迹规划方法流程示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。

如图1所示，现以某六自由度垂直关节串联机器人为对象，按照以下步骤进行机器人关节空间平滑轨迹规划：

一种机器人关节空间平滑轨迹规划方法，包括步骤:

S1、由机器人运动学逆解算法，实现机器人笛卡尔空间运动轨迹向关节空间运动轨迹的映射，获得关节空间关键插补点；

S2、依据机器人各个关节的最大速度约束，调整笛卡尔插补周期；

S3、借助三次多项式曲线进行关节空间关键插补点的角速度和角加速度估计，为下一步的五次样条曲线拟合提供条件；

S4、从第一个关节空间关键插补点开始，依次使用相邻两个关节空间关键插补点构造五次样条曲线，作为关节空间的运动轨迹，再进行关节空间插补。

具体而言，所述步骤S2包含如下步骤：

S2.1、第j个笛卡尔插补周期机器人6个关节电机的最大转速估计如式(1)所示

式中，和分别为关节i第j+1个和第j个插补点，T_D为笛卡尔空间插补时间，rⁱ为关节i的减速比。

S2.2、第j个笛卡尔插补周期机器人各关节伺服电机计算转速与最大转速之比为

如果Q小于1，则说明为完成此插补段运动，机器人6个关节中存在电机转速超出最大转速。由于插补运动需要保证各轴在约定插补周期内同时完成运动，机器人6个关节完成此插补段运动所需的插补周期均需要增加。

S2.3、重新计算第j个笛卡尔空间插补周期

式中，int为取整函数；Q为转速比；T_D为默认笛卡尔空间插补周期，T_D＝10ms。

具体而言，所述步骤S3包含如下步骤：

S3.1、令为笛卡尔空间插补和进行机器人运动学逆解之后得到机器人i关节t_j时刻转角值为θ_j的关节空间关键插补点。插补点的角速度和角加速度的估计通过构造三次多项式曲线进行估计。取插补点前一点和后两个点和构造三次多项式曲线。

S3.2、三次多项式曲线表达式如式(4)所示

θ(t)＝at³+bt²+ct+d (4)

将四个插补点的时间和位置数值代入式(4)可得

将式(5)写成矩阵形式得

则可以解得

S3.3、则插补点的角速度估计为

插补点的角加速度估计为

具体而言，所述步骤S4包含如下步骤：

S4.1、五次样条曲线表达式如式(10)所示

θ(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F (10)

且满足边界条件

写成矩阵的形式

则可以解得

S4.2、按照1毫秒的关节空间插补周期，依次将t代入式(13)计算得到的关节空间五次样条运动曲线，即可得到关节空间的插补点，完成关节空间插补。

上述实施例为本方面较佳的实施方式，但本方明的实施方式并不受上述实施例的限制，其他的任何背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.一种机器人关节空间平滑轨迹规划方法，其特征在于，包括如下步骤：

S1、由机器人运动学逆解算法，实现机器人笛卡尔空间运动轨迹向关节空间运动轨迹的映射，获得关节空间关键插补点；

S2、依据机器人各个关节的最大速度约束，调整笛卡尔插补周期；

S3、借助三次多项式曲线进行关节空间关键插补点的角速度和角加速度估计，为下一步的五次样条曲线拟合提供条件；

S4、从第一个关节空间关键插补点开始，依次使用相邻两个关节空间关键插补点构造五次样条曲线，作为关节空间的运动轨迹，再进行关节空间插补。

2.根据权利要求1所述的一种机器人关节空间平滑轨迹规划方法，其特征在于，所述步骤S2包含如下步骤：

S2.1、第j个笛卡尔插补周期机器人6个关节电机的最大转速估计如式(1)所示：

${\mathrm{\ω}}_j^i=\frac{{\mathrm{\θ}}_{j+1}^i-{\mathrm{\θ}}_j^i}{T_D}{r}^i,i=1,2,...,6---\left(1\right)$

式中，和分别为关节i第j+1个和第j个插补点，T_D为笛卡尔空间插补时间，rⁱ为关节i的减速比；

S2.2、第j个笛卡尔插补周期机器人各关节伺服电机计算转速与最大转速之比为

$Q=min(\frac{{\mathrm{\ω}}_{\max }^1}{{\mathrm{\ω}}_j^1},\frac{{\mathrm{\ω}}_{\max }^2}{{\mathrm{\ω}}_j^2},\frac{{\mathrm{\ω}}_{\max }^3}{{\mathrm{\ω}}_j^3},\frac{{\mathrm{\ω}}_{\max }^4}{{\mathrm{\ω}}_j^4},\frac{{\mathrm{\ω}}_{\max }^5}{{\mathrm{\ω}}_j^5},\frac{{\mathrm{\ω}}_{\max }^6}{{\mathrm{\ω}}_j^6})---\left(2\right)$

如果Q小于1，则说明为完成此插补段运动，机器人6个关节中存在电机转速超出最大转速；由于插补运动需要保证各轴在约定插补周期内同时完成运动，机器人6个关节完成此插补段运动所需的插补周期均需要增加；

S2.3、重新计算第j个笛卡尔空间插补周期

$T_{Dj}=\mathrm{int}\left(\frac{Q}{T_D}\right)---\left(3\right)$

式中，int为取整函数；Q为转速比；T_D为默认笛卡尔空间插补周期，T_D＝10ms。

3.根据权利要求1所述的一种机器人关节空间平滑轨迹规划方法，其特征在于，所述步骤S3包含如下步骤：

S3.1、令为笛卡尔空间插补和进行机器人运动学逆解之后得到机器人i关节t_j时刻转角值为θ_j的关节空间关键插补点；插补点的角速度和角加速度的估计通过构造三次多项式曲线进行估计；取插补点前一点和后两个点和构造三次多项式曲线；

S3.2、三次多项式曲线表达式如式(4)所示：

θ(t)＝at³+bt²+ct+d (4)

将四个插补点的时间和位置数值代入式(4)可得

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i-1}={{\mathrm{at}}_{i-1}}^3+{{\mathrm{bt}}_{i-1}}^2+{\mathrm{ct}}_{i-1}+d\\ {\mathrm{\θ}}_i={{\mathrm{at}}_i}^3+{{\mathrm{bt}}_i}^2+{\mathrm{ct}}_i+d\\ {\mathrm{\θ}}_{i+1}={{\mathrm{at}}_{i+1}}^3+{{\mathrm{bt}}_{i+1}}^2+{\mathrm{ct}}_{i+1}+d\\ {\mathrm{\θ}}_{i+2}={{\mathrm{at}}_{i+2}}^3+{{\mathrm{bt}}_{i+2}}^2+{\mathrm{ct}}_{i+2}+d\end{array}\right.---\left(5\right)$

将式(5)写成矩阵形式得

$\left[\begin{array}{c}{\mathrm{\θ}}_{i-1}\\ {\mathrm{\θ}}_i\\ {\mathrm{\θ}}_{i+1}\\ {\mathrm{\θ}}_{i+2}\end{array}\right]=\left[\begin{array}{cccc}{t}_{i-1}^3& t_{i-1}^2& t_{i-1}& 1\\ t_i^3& t_i^2& t_i& 1\\ t_{i+1}^3& t_{i+1}^2& t_{i+1}& 1\\ t_{i+2}^3& t_{i+2}^2& t_{i\mathrm{+2}}& 1\end{array}\right]\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]---\left(6\right)$

则可以解得

$\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]={\left[\begin{array}{cccc}{t}_{i-1}^3& t_{i-1}^2& t_{i-1}& 1\\ t_i^3& t_i^2& t_i& 1\\ t_{i+1}^3& t_{i+1}^2& t_{i+1}& 1\\ t_{i+2}^3& t_{i+2}^2& t_{i\mathrm{+2}}& 1\end{array}\right]}^{-1}\left[\begin{array}{c}{\mathrm{\θ}}_{i-1}\\ {\mathrm{\θ}}_i\\ {\mathrm{\θ}}_{i+1}\\ {\mathrm{\θ}}_{i+2}\end{array}\right]---\left(7\right)$

S3.3、则插补点的角速度估计为

$\frac{d\mathrm{\θ}}{dt}=3{{\mathrm{at}}_i}^2+2{\mathrm{bt}}_i+c---\left(8\right)$

插补点的角加速度估计为

$\frac{d^2\mathrm{\θ}}{{\mathrm{dt}}^2}=6{\mathrm{at}}_i+2b.---\left(9\right)$

4.根据权利要求1所述的一种机器人关节空间平滑轨迹规划方法，其特征在于，所述步骤S4包含如下步骤：

S4.1、五次样条曲线表达式如式(10)所示

θ(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F (10)

且满足边界条件

$\left\{\begin{array}{c}{\mathrm{\θ}}_i={\mathrm{At}}_i^5+{\mathrm{Bt}}_i^4+{\mathrm{Ct}}_i^3+{\mathrm{Dt}}_i^2+{\mathrm{Et}}_i+F\\ {\mathrm{\θ}}_{i+1}={\mathrm{At}}_{i+1}^5+{\mathrm{Bt}}_{i+1}^4+{\mathrm{Ct}}_{i+1}^3+{\mathrm{Dt}}_{i+1}^2+{\mathrm{Et}}_{i+1}+F\\ {\stackrel{\·}{\mathrm{\θ}}}_i=5{\mathrm{At}}_i^4+4{\mathrm{Bt}}_i^3+3{\mathrm{Ct}}_i^2+2{\mathrm{Dt}}_i+E\\ {\stackrel{\·\·}{\mathrm{\θ}}}_i=20{\mathrm{At}}_i^3+12{\mathrm{Bt}}_i^2+6{\mathrm{Ct}}_i+2D\\ {\stackrel{\·}{\mathrm{\θ}}}_{i+1}=5{\mathrm{At}}_{i+1}^4+4{\mathrm{Bt}}_{i+1}^3+3{\mathrm{Ct}}_{i+1}^2+2{\mathrm{Dt}}_{i+1}+E\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i+1}=20{\mathrm{At}}_{i+1}^3+12{\mathrm{Bt}}_{i+1}^2+6{\mathrm{Ct}}_{i+1}+2D\end{array}\right.---\left(11\right)$

写成矩阵的形式

$\left[\begin{array}{c}\mathrm{\θ}\\ {\mathrm{\θ}}_{i+1}\\ {\stackrel{\·}{\mathrm{\θ}}}_i\\ {\stackrel{\·}{\mathrm{\θ}}}_{i+1}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_i\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i+1}\end{array}\right]=\left[\begin{array}{cccccc}{t}_i^5& t_i^4& t_i^3& t_i^2& t_i& 1\\ t_{i+1}^5& t_{i+1}^4& t_{i+1}^3& t_{i+1}^2& t_{i\mathrm{+1}}& 1\\ 5t_i^4& 4t_i^3& 3t_{i+1}^2& 2t_{i+1}& 1& 0\\ 5t_{i+1}^4& 4t_{i+1}^3& 3t_{i+1}^2& 2t_{i+1}& 1& 0\\ 20t_i^3& 12t_i^2& 6t& 2& 0& 0\\ 20t_{i+1}^3& 12t_{i+1}^2& 6t_{i+1}& 2& 0& 0\end{array}\right]\left[\begin{array}{c}A\\ B\\ C\\ D\\ E\\ F\end{array}\right]---\left(12\right)$

则可以解得

$\left[\begin{array}{c}A\\ B\\ C\\ D\\ E\\ F\end{array}\right]=\left[\begin{array}{cccccc}{t}_i^5& t_i^4& t_i^3& t_i^2& t_i& 1\\ t_{i+1}^5& t_{i+1}^4& t_{i+1}^3& t_{i+1}^2& t_{i\mathrm{+1}}& 1\\ 5t_i^4& 4t_i^3& 3t_{i+1}^2& 2t_{i+1}& 1& 0\\ 5t_{i+1}^4& 4t_{i+1}^3& 3t_{i+1}^2& 2t_{i+1}& 1& 0\\ 20t_i^3& 12t_i^2& 6t& 2& 0& 0\\ 20t_{i+1}^3& 12t_{i+1}^2& 6t_{i+1}& 2& 0& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\θ}\\ {\mathrm{\θ}}_{i+1}\\ {\stackrel{\·}{\mathrm{\θ}}}_i\\ {\stackrel{\·}{\mathrm{\θ}}}_{i+1}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_i\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i+1}\end{array}\right];---\left(13\right)$

S4.2、按照1毫秒的关节空间插补周期，依次将t代入式(13)计算得到的关节空间五次样条运动曲线，即可得到关节空间的插补点，完成关节空间插补。