CN103802113A - 基于任务和样条曲线的工业机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于任务和样条曲线的工业机器人路径规划方法，其包括有机器人控制器、输入信息到控制器的运动学模块、任务模块、轨迹规划器，该规划方法包括以下步骤：S1、建立工业机器人的运动学模型，求得工业机器人的运动学的正反解；S2、由所述的任务模块给出工业机器人的任务点的位置信息和姿态信息：S3、由工业机器人的轨迹规划器结合所述的任务模块，给出基于样条曲线的路径规划曲线：S4、工业机器人轨迹规划器将产生的路径曲线，结合运动学正反解得到关节空间的信息发给工业机器人的驱动器。采用本发明的技术方案，不但可以解决工业机器人在应用中需要经过多个中间位姿的问题，并且为了保证工业机器人在运动中的加速度无急动。

Description

基于任务和样条曲线的工业机器人路径规划方法

技术领域

本发明涉及一种基于任务和样条曲线的工业机器人路径规划方法。

背景技术

工业机器人是工作机器，其可以装备用于对对象进行自动处理和/或加工的工具，并可以对多个运动轴，例如就方向、位置和工作流程进行编程。工业机器人通常包括具有多个轴的机器人臂以及可编程控制器（控制装置），控制器在运行中控制或调整工业机器人的运动过程。

为了实现运动，控制器可以通过轨迹规划来计划这种运动。

常用的轨迹规划方法有：3-4-5正则多项式；4-5-6-7正则多项式；摆线运动与抛物线拟合的线性函数等。4-5-6-7正则多项式相对3-4-5正则多项式的优点是，可以保证加速度的变化没有急动；摆线运动与抛物线拟合的线性函数方法对要求通过中间位姿的情况处理起来较为复杂。当机器人要求通过中间位姿时，大多采用高阶正则多项式，Kahaner，Moler和Nash等指出中间位姿数量增加时这种采用多项式插值的方法变得不切实际，这时因为中间位姿的增加会使得正则多项式的次数增加，多项式系数的方程组的条件数变大，相对舍入误差等于舍入误差乘以一个放大系数—方程组条件数，从而方程组的解失真。

发明内容

本发明提供一种基于任务和样条曲线的工业机器人路径规划方法，其可克服上述缺陷，能解决工业机器人在应用中需要经过多个中间位姿的问题，并且可保证工业机器人在运动中的加速度无急动。

为实现上述目的，本发明采用如下技术方案：

一种基于任务和样条曲线的工业机器人路径规划方法，其包括有机器人控制器、输入信息到控制器的运动学模块、任务模块、轨迹规划器，该规划方法包括以下步骤：

S1、建立工业机器人的运动学模型，求得工业机器人的运动学的正反解；

S2、由所述的任务模块给出工业机器人的任务点的位置信息和姿态信息：

S3、由工业机器人的轨迹规划器结合所述的任务模块，给出基于样条曲线的路径规划曲线：

S4、工业机器人轨迹规划器将产生的路径曲线，结合运动学正反解得到关节空间的信息发给工业机器人的驱动器：

上述的任务模块，设有两个信息输入口，所述的第一个信息输入口，是直接与所述的机器人控制器相连；所述的第二个信息输入口，是利用串口与PC相连。

在步骤S2中的任务点，是利用示教或离线编程给出的，所述的工业机器人要完成任务需要通过的点。

上述的路径规划曲线，由下述步骤所得：

定义所述的任务模块给出的工业机器人通过点的个数用N表示，这些点用P_k(x_k,y_k)(k＝1,2...N)表示；这里用三次样条曲线s(x_k)连接已知的N个位姿点P_k(x_k,y_k)(k＝1,2...N)间的N-1个区间，在连接点P_k上有s(x_k)＝y_k，而且定义样条函数在x₁≤x≤x_N上式二次可微的，即这样的样条函数称为C²函数，也就是有连续的二阶导数；设两个连续点P_k(x_k,y_k)和P_k+1(x_k+1，y_k+1)间的三次多项式s_k(x)及相应的一阶导数和二阶导数如公式(1)：

$\left\{\begin{array}{c}{s}_k\left(x\right)=A_k{(x-x_k)}^3+B_k{(x-x_k)}^2+C_k(x-x_k)+D\\ s_k^{\′}\left(x\right)=3A_k{(x-x_k)}^2+{2B}_k(x-x_k)+C_k\\ s_k^{\′\′}\left(x\right)={6A}_k(x-x_k)+{2B}_k,x_k\≤x\≤x_{k+1}\end{array}\right.---\left(1\right)$

为了便于书写设定如下恒等式，由公式(1)计算可得公式(2)：

s_k≡s(x_k),s′_k≡s(x′_k),s″_k≡s(x″_k)

Δx_k≡x_k+1-x_k,Δs_k≡s_k+1-s_k

$\left\{\begin{array}{c}{B}_x=s_k^{\′\′}/2\\ C_x=s_k^{\′}\\ D_x=s_k\end{array}\right.---\left(2\right)$

接下来，将多项式系数都用样条曲线的二阶导数和样条函数值表示；考虑两个样条曲线在连接处函数值及一阶和二阶导数连续，可以得到如下方程(3)：

$\left\{\begin{array}{c}{s}_k\left(x_{k+1}\right)=s_{k+1}\left(x_{k+1}\right)\\ s_k^{\′}\left(x_{k+1}\right)=s_{k+1}^{\′}\left(x_{k+1}\right)\\ s_k^{\′\′}\left(x_{k+1}\right)=s_{k+1}^{\′\′}\left(x_{k+1}\right)\end{array}---\left(3\right)\right.$

由上式的第一个等式和最后一个等式，可以分别得到公式(4)：

$\left\{\begin{array}{c}{A}_k=(s_{k+1}^{\′\′}-s_k^{\′\′})/\left({6\mathrm{\Δ}}_k\right)\\ A_k{\left({\mathrm{\Δx}}_k\right)}^3+B_k{\left({\mathrm{\Δx}}_k\right)}^2+C_k{\mathrm{\Δx}}_k+s_k=s_{k+1}\end{array}\right.---\left(4\right)$

于是各多项式的系数用样条函数及其二阶导数表示如公式(5)：

$\left\{\begin{array}{c}{A}_k=(s_{k+1}^{\′\′}-s_k^{\′\′})/\left({6\mathrm{\Δ}}_k\right)\\ B_k=s_k^{\′\′}/2\\ C_k={\mathrm{\Δs}}_k/{\mathrm{\Δx}}_k-{\mathrm{\Δx}}_k(s_{k+1}^{\′\′}+{2s}_k^{\′\′})/6\\ D_x=s_k\end{array}\right.---\left(5\right)$

将(3)的第二个方程的k改为k-1,并将上式带入，得到含有N个未知数的N-2方程组(6)：

(Δx_k)s″_k+1+2(Δx_k-1+Δx_k)s″_k+(Δx_k-1)s″_k-1

＝6(Δs_k/Δx_k-Δs_k-1/Δx_k-1),k＝2,3...N-1                    (6)

将式(6)整理成矩阵形式，如公式(7)：

As″＝6Cs                                    (7)

公式(7)中的符号说明如下：

s＝[s₁ s₂ ... s_N]^T，s″＝[s₁″s₂″...s″_N]^T

$A=\left[\begin{array}{ccccccc}{\mathrm{\α}}_1& 2{\mathrm{\α}}_{\mathrm{1,2}}& {\mathrm{\α}}_2& 0& ...& 0& 0\\ 0& {\mathrm{\α}}_2& {2\mathrm{\α}}_{\mathrm{2,3}}& {\mathrm{\α}}_3& M& 0& 0\\ M& M& O& O& O& M& M\\ 0& 0& ...& {\mathrm{\α}}_{N^{\′\′\′}}& {2\mathrm{\α}}_{N^{\′\′\′},N^{\′\′}}& {\mathrm{\α}}_{N^{\′\′}}& 0\\ 0& 0& 0& ...& {\mathrm{\α}}_{N^{\′\′}}& {2\mathrm{\α}}_{N^{\′\′},N^{\′}}& {\mathrm{\α}}_{N^{\′}}\end{array}\right]$ $C=\left[\begin{array}{ccccccc}{\mathrm{\β}}_1& {-\mathrm{\β}}_{\mathrm{1,2}}& {\mathrm{\β}}_2& 0& ...& 0& 0\\ 0& {\mathrm{\β}}_2& {-\mathrm{\β}}_{\mathrm{2,3}}& {\mathrm{\β}}_3& M& 0& 0\\ M& M& O& O& O& M& M\\ 0& 0& ...& {\mathrm{\β}}_{N^{\′\′\′}}& {-\mathrm{\β}}_{N^{\′\′\′},N^{\′\′}}& {\mathrm{\β}}_{N^{\′\′}}& 0\\ 0& 0& 0& ...& {\mathrm{\β}}_{N^{\′\′}}& {-\mathrm{\β}}_{N^{\′\′},N^{\′}}& {\mathrm{\β}}_{N^{\′}}\end{array}\right]$

对于i,j,k＝1,2...N-1有如下恒等式。

α_K≡Δx_k,α_i，j≡α_i+α_j,N'≡N-1,N″≡N-2

β_k≡1/α_k,β_i，j≡β_i+β_j,N″′≡N-3

使用自然样条曲线，即设s₁″＝s″_N＝0。

在步骤S4中，其中利用工业机器人的控制器内所输入的运动学模块，根据轨迹规划器给出的路径曲线，计算关节空间值，发送到工业机器人各关节驱动器。

采用上述的技术方案，不但可以解决工业机器人在应用中需要经过多个中间位姿的问题，并且为了保证工业机器人在运动中的加速度无急动。

附图说明

图1为实现本发明工业机器人路径规划流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参考图1所示，本发明，公开了一种基于任务和样条曲线的工业机器人路径规划方法，该工业机器人包括有机器人控制器、输入信息到控制器的运动学模块、任务模块、轨迹规划器，其中：

该任务模块，是一个软件模块，它有主要的两个信息输入口，第一个信息输入口，是直接与机器人控制器相连，这主要是为了示教的输入；第二个信息输入口，是利用串口与PC相连，这主要为编程的输入。

其包括以下步骤：

S1、建立工业机器人的运动学模型，求得工业机器人的运动学的正反解。

该任务点是利用DH方法建立工业机器人各连杆坐标系，并利用连杆参数求出各个坐标系的位姿传递矩阵，将各连杆位姿传递矩阵相乘，得到运动学正解，给定机器人末端位置，求解机器人各关节角度，得到机器人运动学反解。

S2、由任务模块给出工业机器人的任务点的位置信息和姿态信息：

该任务点，是利用示教或利用离线编程给出的，这里的任务点是工业机器人要完成任务需要通过的点，由任务模块给出通过点的位置信息和姿态信息。同时，任务模块还给出完成任务所需要的总时间。

S3、由工业机器人的轨迹规划器结合任务模块，给出基于样条曲线的路径规划曲线：

任务模块给出的工业机器人通过点的个数用N表示，这些点用P_k(x_k,y_k)(k＝1,2...N)表示。这里用三次样条曲线s(x_k)连接已知的N个位姿点P_k(x_k,y_k)(k＝1,2...N)间的N-1个区间，在连接点P_k上有s(x_k)＝y_k，而且定义样条函数在x₁≤x≤x_N上式二次可微的，即这样的样条函数称为C²函数，也就是有连续的二阶导数。设两个连续点P_k(x_k,y_k)和P_k+1(x_k+1，y_k+1)间的三次多项式s_k(x)及相应的一阶导数和二阶导数如公式(1)：

$\left\{\begin{array}{c}{s}_k\left(x\right)=A_k{(x-x_k)}^3+B_k{(x-x_k)}^2+C_k(x-x_k)+D\\ s_k^{\′}\left(x\right)=3A_k{(x-x_k)}^2+{2B}_k(x-x_k)+C_k\\ s_k^{\′\′}\left(x\right)={6A}_k(x-x_k)+{2B}_k,x_k\≤x\≤x_{k+1}\end{array}\right.---\left(1\right)$

为了便于书写设定如下恒等式，由公式(1)计算可得公式(2)：

s_k≡s(x_k),s′_k≡s(x′_k),s″_k≡s(x″_k)

Δx_k≡x_k+1-x_k,Δs_k≡s_k+1-s_k

$\left\{\begin{array}{c}{B}_x=s_k^{\′\′}/2\\ C_k=s_k^{\′}\\ D_x=s_k\end{array}---\left(2\right)\right.$

接下来，将多项式系数都用样条曲线的二阶导数和样条函数值表示。考虑两个样条曲线在连接处函数值及一阶和二阶导数连续，可以得到如下方程(3)：

$\left\{\begin{array}{c}{s}_k\left(x_{k+1}\right)=s_{k+1}\left(x_{k+1}\right)\\ s_k^{\′}\left(x_{k+1}\right)=s_{k+1}^{\′}\left(x_{k+1}\right)\\ s_k^{\′\′}\left(x_{k+1}\right)=s_{k+1}^{\′\′}\left(x_{k+1}\right)\end{array}---\left(3\right)\right.$

由上式的第一个等式和最后一个等式，可以分别得到公式(4)。

$\left\{\begin{array}{c}{A}_k=(s_{k+1}^{\′\′}-s_k^{\′\′})/\left({6\mathrm{\Δ}}_k\right)\\ A_k{\left({\mathrm{\Δx}}_k\right)}^3+B_k{\left({\mathrm{\Δx}}_k\right)}^2+C_k{\mathrm{\Δx}}_k+s_k=s_{k+1}\end{array}\right.---\left(4\right)$

于是各多项式的系数用样条函数及其二阶导数表示如公式(5)。

$\left\{\begin{array}{c}{A}_k=(s_{k+1}^{\′\′}-s_k^{\′\′})/\left({6\mathrm{\Δ}}_k\right)\\ B_k=s_k^{\′\′}/2\\ C_k={\mathrm{\Δs}}_k/{\mathrm{\Δx}}_k-{\mathrm{\Δx}}_k(s_{k+1}^{\′\′}+{2s}_k^{\′\′})/6\\ D_x=s_k\end{array}\right.---\left(5\right)$

将(3)的第二个方程的k改为k-1,并将上式带入，得到含有N个未知数的N-2方程组(6)。

(Δx_k)s″_k+1+2(Δx_k-1+Δx_k)s″_k+(Δx_k-1)s″_k-1

＝6(Δs_k/Δx_k-Δs_k-1/Δx_k-1),k＝2,3...N-1                (6)

将式(6)整理成矩阵形式，如公式(7)。

As″＝6Cs                                                (7)

公式(7)中的符号说明如下：

s＝[s₁ s₂ ... s_N]^T，s″＝[s₁″ s″₂ ... s″_N]^T

$A=\left[\begin{array}{ccccccc}{\mathrm{\α}}_1& 2{\mathrm{\α}}_{\mathrm{1,2}}& {\mathrm{\α}}_2& 0& ...& 0& 0\\ 0& {\mathrm{\α}}_2& {2\mathrm{\α}}_{\mathrm{2,3}}& {\mathrm{\α}}_3& M& 0& 0\\ M& M& O& O& O& M& M\\ 0& 0& ...& {\mathrm{\α}}_{N^{\′\′\′}}& {2\mathrm{\α}}_{N^{\′\′\′},N^{\′\′}}& {\mathrm{\α}}_{N^{\′\′}}& 0\\ 0& 0& 0& ...& {\mathrm{\α}}_{N^{\′\′}}& {2\mathrm{\α}}_{N^{\′\′},N^{\′}}& {\mathrm{\α}}_{N^{\′}}\end{array}\right]$ $C=\left[\begin{array}{ccccccc}{\mathrm{\β}}_1& {-\mathrm{\β}}_{\mathrm{1,2}}& {\mathrm{\β}}_2& 0& ...& 0& 0\\ 0& {\mathrm{\β}}_2& {-\mathrm{\β}}_{\mathrm{2,3}}& {\mathrm{\β}}_3& M& 0& 0\\ M& M& O& O& O& M& M\\ 0& 0& ...& {\mathrm{\β}}_{N^{\′\′\′}}& {-\mathrm{\β}}_{N^{\′\′\′},N^{\′\′}}& {\mathrm{\β}}_{N^{\′\′}}& 0\\ 0& 0& 0& ...& {\mathrm{\β}}_{N^{\′\′}}& {-\mathrm{\β}}_{N^{\′\′},N^{\′}}& {\mathrm{\β}}_{N^{\′}}\end{array}\right]$

对于i,j,k＝1,2...N-1有如下恒等式。

α_k≡Δx_k,α_i，j≡α_i+α_j,N′≡N-1,N″≡N-2

β_k≡1/α_k,β_i，j≡β_i+β_j,N″′≡N-3

需要额外的两个方程来求解，这里使用自然样条曲线，即设s″₁＝s″_N＝0。

S4、工业机器人轨迹规划器将产生的路径曲线，结合运动学正反解得到关节空间的信息发给工业机器人的驱动器：

在操作时，利用工业机器人的控制器内所输入的运动学模块，根据轨迹规划器给出的路径曲线，计算关节空间值，发送到工业机器人各关节驱动器。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (5)

1.一种基于任务和样条曲线的工业机器人路径规划方法，其包括有机器人控制器、输入信息到控制器的运动学模块、任务模块、轨迹规划器，该规划方法包括以下步骤：

S1、建立工业机器人的运动学模型，求得工业机器人的运动学的正反解；

S2、由所述的任务模块给出工业机器人的任务点的位置信息和姿态信息：

S3、由工业机器人的轨迹规划器结合所述的任务模块，给出基于样条曲线的路径规划曲线：

S4、工业机器人轨迹规划器将产生的路径曲线，结合运动学正反解得到关节空间的信息发给工业机器人的驱动器。

2.根据权利要求1所述的基于任务和样条曲线的工业机器人路径规划方法，其特征在于：所述的任务模块，设有两个信息输入口，所述的第一个信息输入口，是直接与所述的机器人控制器相连；所述的第二个信息输入口，是利用串口与PC相连。

3.根据权利要求1或2所述的基于任务和样条曲线的工业机器人路径规划方法，其特征在于：在步骤S2中的任务点，是利用示教或离线编程给出的，所述的工业机器人要完成任务需要通过的点。

4.根据权利要求3所述的基于任务和样条曲线的工业机器人路径规划方法，其特征在于：所述的路径规划曲线，由下述步骤所得：

定义所述的任务模块给出的工业机器人通过点的个数用N表示，这些点用P_k(x_k,y_k)(k＝1,2...N)表示；这里用三次样条曲线s(x_k)连接已知的N个位姿点P_k(x_k,y_k)(k＝1,2...N)间的N-1个区间，在连接点P_k上有s(x_k)＝y_k，而且定义样条函数在x₁≤x≤x_N上式二次可微的，即这样的样条函数称为C²函数，也就是有连续的二阶导数；设两个连续点P_k(x_k,y_k)和P_k+1(x_k+1，y_k+1)间的三次多项式s_k(x)及相应的一阶导数和二阶导数如公式(1)： 

为了便于书写设定如下恒等式，由公式(1)计算可得公式(2)：

s_k≡s(x_k),s′_k≡s(x′_k),s″_k≡s(x″_k)

Δx_k≡x_k+1-x_k,Δs_k≡s_k+1-s_k

接下来，将多项式系数都用样条曲线的二阶导数和样条函数值表示；考虑两个样条曲线在连接处函数值及一阶和二阶导数连续，可以得到如下方程(3)：

由上式的第一个等式和最后一个等式，可以分别得到公式(4)：

于是各多项式的系数用样条函数及其二阶导数表示如公式(5)： 

将(3)的第二个方程的k改为k-1,并将上式带入，得到含有N个未知数的N-2方程组(6)：

(Δx_k)s″_k+1+2(Δx_k-1+Δx_k)s″_k+(Δx_k-1)s″_k-1

＝6(Δs_k/Δx_k-Δs_k-1/Δx_k-1),k＝2,3...N-1                    (6)

将式(6)整理成矩阵形式，如公式(7)：

As″＝6Cs(7)

公式(7)中的符号说明如下：

s＝[s₁ s₂ ... s_N]^T，s″＝[s₁″ s″₂ ... s″_N]^T

对于i,j,k＝1,2...N-1有如下恒等式。

α_k≡Δx_k,α_i，j≡α_i+α_j,N'≡N-1,N'≡N-2

β_k≡1/α_k,β_i，j≡β_i+β_j,N″′≡N-3

使用自然样条曲线，即设s₁″＝s″_N＝0。 

5.根据权利要求3所述的基于任务和样条曲线的工业机器人路径规划方法，其特征在于：在步骤S4中，其中利用工业机器人的控制器内所输入的运动学模块，根据轨迹规划器给出的路径曲线，计算关节空间值，发送到工业机器人各关节驱动器。 

Images