CN104090492B - 基于指数函数的scara机器人ptp轨迹规划 - Google Patents

Abstract

本发明提供了一种基于指数函数的SCARA机器人PTP轨迹规划方法，在保证其速度曲线、加速度曲线和加加速度曲线平滑和连续的情况下使运动轨迹时间最优。具有公式简单、计算量小的优点。首先建立SCARA机器人运动学模型，然后根据目标位姿求取各关节位移，随后根据需要运动的关节空间位移求取延时时间T_d；由执行器工作时的限制条件获取时间增益α；最后根据获得的延时时间和时间增益确定指数函数速度轨迹曲线完成规划。

Description

基于指数函数的SCARA机器人PTP轨迹规划

技术领域

本发明涉及机器人轨迹规划领域，具体针对SCARA(Selective ComplianceAssembly Robot Arm)机器人PTP(Point to Point motion)轨迹运动形式，提出一种基于指数函数的速度曲线规划方法，以保证机器人在关节空间轨迹的速度、加速度、加加速度曲线连续与平滑。

背景技术

随着机器人应用的各个领域对作业精度和工作效率要求的不断提高，要求机器人能够快速准确地完成作业，也就是说机器人在作业时既要保证轨迹的最优又要保证作业时间的最短，因此机器人的轨迹规划成为重点研究内容。

轨迹规划是指在满足机器人运动学和动力学约束条件下，设计出一条合理的机器人运动轨迹。工业机器人轨迹规划的方法一般分为在任务空间的轨迹规划和在关节空间的轨迹规划。任务空间的轨迹规化是指机器人末端运动轨迹必须经过或逼近给定的位置点。关节空间的轨迹规划的目的是为了使关节轨迹曲线及其各阶导数连续平滑，因为若不连续平滑将导致机器人在运动时出现振动现象，这将严重影响到机器人的使用寿命和运动精度。

SCARA机器人PTP轨迹运动形式只需要规划关节空间，为达到时间最优的目的，传统的轨迹规划方式大多采用T形或S形速度曲线，但是这些速度曲线虽然时间最优但是不能满足其一阶导数加速度或二阶导数加加速度曲线连续平滑的要求。

发明内容

本发明的目的是针对SCARA机器人关节空间时间最优轨迹的速度、加速度和加加速度曲线不能同时连续平滑问题，提出基于指数函数的速度轨迹规划方法，其不仅具有连续平滑的速度、加速度和加加速度曲线，而且公式计算简单，规划方法只需设置两个参数：时间增益α和延时时间T_d。基于指数函数的速度曲线与S形速度曲线轮廓相似，因此可以认为其是近似时间最优轨迹。

为达此目的，本发明技术方案如下：

第一步，建立SCARA机器人各连杆坐标系，确定各连杆的D-H(Denavit-Hartenberg)参数，求取机器人运动学正解和逆解的表达式。

第二步，由传感器获取目标在笛卡尔坐标系下坐标，经过机器人运动学逆解求取SCARA机器人各关节需要运动的位移S_i(i＝1，2，3，4)。

第三步，根据上一步的位移确定各关节轨迹延时时间T_di(i＝1，2，3，4)。

第四步，获取SCARA机器人运动时的各关节约束条件：最大速度V_max，最大加速度A_max，最大加加速度J_max。由约束条件确定各关节轨迹时间增益α_i(i＝1，2，3，4)。

第五步，根据获得的各关节轨迹延时时间T_di(i＝1，2，3，4)和时间增益α_i(i＝1，2，3，4)，确定各关节速度轨迹表达式V_i(t)(i＝1，2，3，4)，其中t表示时间。

本发明的有益效果：提出一种基于指数函数的新速度轨迹规划方法，不仅具有连续平滑的速度、加速度和加加速度曲线，可以降低机器人在高速运动过程中的抖动现象，而且公式计算简单，规划方法只需设置两个参数：延时时间T_d和时间增益α，即可确定规划的轨迹。

附图说明

图1 SCARA机器人模型示意图；

图2 本发明轨迹规划流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本发明作进一步详细说明。

本发明的目的：针对SCARA机器人关节空间时间最优轨迹的速度、加速度和加加速度曲线不能同时连续平滑问题，提出基于3次指数函数的速度轨迹规划方法，其不仅具有连续平滑的速度、加速度和加加速度曲线，而且公式计算简单，规划方法只需设置两个参数：时间增益α和延时时间T_d，即可确定规划的轨迹。

本发明的基本思路：首先建立SCARA机器人运动学模型，然后根据目标位姿求取各关节位移，随后根据需要运动的关节空间位移求取延时时间T_d；由执行器工作时的限制条件获取时间增益α；最后确定指数函数速度轨迹规划的表达式。

附图1为SCARA机器人模型示意图。其中r₁，r₂，r₃分别为第一关节，第二关节和第三关节坐标系，T为末端旋转轴坐标系；θ₁，θ₂，θ₄分别为第一关节旋转角，第二关节旋转角和第四关节旋转角，d₃为第三关节平移量，l₁，l₂分别为第一连杆和第二连杆长度。

附图2为本发明轨迹规划流程图，其中α表示规划轨迹时间增益，T_d表示规划轨迹延时时间，V_max表示执行器约束最大速度，A_max执行器约束最大加速度，J_max执行器约束最大加加速度。

进一步，具体实现步骤为：

步骤一，建立SCARA机器人的运动学模型。

机器人动力学模型包括机器人正解和逆解。正解是指已知关节角求机器人末端位姿；逆解是指已知机器人末端位姿求解各关节。

设SCARA机器人空间坐标为(P_x，P_y，P_z)，关节坐标为(θ₁，θ₂，d₃，θ₄)。根据D-H参数方法可以求得机器人正解的表达式：

$\left\{\begin{array}{c}{P}_x=l_2{C}_{12}+l_1{C}_1\\ P_y=l_2{S}_{12}+l_1{S}_1\\ P_z=-d_3\end{array}\right.$

其中C₁₂＝cos(θ₁+θ₂)，S₁₂＝sin(θ₁+θ₂)，C₁＝cos(θ₁)，S₁＝sin(θ₁)，l₁，l₂分别为第一连杆和第二连杆长度。

机器人的逆解表达式为：

${\mathrm{\θ}}_1=\arctan \left(\frac{A}{\±\sqrt{1-A^2}}\right)-\arctan \left(\frac{P_x}{P_y}\right)$

其中

$A=\frac{(l_1^2-l_2^2+P_x^2+P_y^2)}{{2l}_1\sqrt{P_x^2+P_y^2}}$

${\mathrm{\θ}}_2=\arcsin \left(\frac{\sqrt{P_x^2+P_y^2}\cos ({\mathrm{\θ}}_1+\arctan \left(\frac{P_X}{p_y}\right))}{l_2}\right)$

d₃＝-P_z

θ₄的值直接由工件需要旋转多少角度直接确定。

步骤二，由目标位姿求取各关节位移。

根据上一步的逆解表达式，当已知空间坐标(P_x，P_y，P_z)时，各关节的位移分别为：

$\left\{\begin{array}{c}{S}_1={\mathrm{\θ}}_1\\ S_2={\mathrm{\θ}}_2\\ S_3=d_3\\ S_4={\mathrm{\θ}}_4\end{array}\right.$

步骤三，根据上一步各关节位移确定各关节轨迹延时时间T_di(i＝1，2，3，4)。

$T_{\mathrm{di}}=\frac{S_i}{V_{i_{\max }}},(i=\mathrm{1,2,3,4})$

其中为第i关节运行时的最大速度。

步骤四，由执行器约束条件确定各关节轨迹时间增益α_i(i＝1，2，3，4)。

${\mathrm{\α}}_i=\min \{\frac{A_{i_{\max }}}{1.1754V_{i_{\max }}};\sqrt{\frac{J_{i_{\max }}}{2.1524V_{i_{\max }}}}\},(i=\mathrm{1,2,3,4})$

其中表示第i关节的最大速度，表示第i关节的最大加速度，表示第i关节的最大加加速度。

步骤五，确定各关节指数函数速度规划轨迹表达式。

根据以上几步确定的延时时间T_d和时间增益α可以确定最后的速度轨迹为：

v_i(t)＝f(t)-f(t-T_di)H(t-T_di)

其中，

$f\left(t\right)=V_{i_{\max }}(1-e^{-{u_i}^3})$

u_i＝α_it

$H\left(x\right)=\left\{\begin{array}{cc}0,& x<0\\ 1,& x\&GreaterEqual;0\end{array}\right.,(i=\mathrm{1,2},\mathrm{3,4})$

基于指数函数的速度轨迹规划方法，其曲线轮廓和S形速度曲线相似，可以认为是近似时间最优轨迹，并且克服了S形速度曲线加加速度不是连续、平滑的问题。具有公式简单、计算量小的优点，只需要控制两个参数：延时时间T_d和时间增益α，即可确定规划的轨迹。为了提高轨迹规划运算速度可以将指数函数的运算离线计算好，制作成一张表格，供在线规划时查表。

Claims (1)

1.一种基于指数函数的SCARA机器人PTP轨迹规划方法，其特征是不仅具有连续平滑的速度、加速度和加加速度曲线，而且公式计算简单，规划方法只需设置两个参数：时间增益α和延时时间T_d，即能够确定规划的轨迹；首先建立SCARA机器人运动学模型，然后根据目标位姿求取各关节位移，随后根据需要运动的关节空间位移求取延时时间T_d；由执行器工作时的限制条件获取时间增益α，包括以下几步骤：

(1)建立SCARA机器人运动学模型，根据SCARA机器人连杆关节之间关系参数，确定机器人D-H即Denavit-Hartenberg参数，求取机器人运动学正解和逆解的表达式；

设SCARA机器人空间坐标为(P_x′P_y′P_z)，关节坐标为(θ₁，θ₂，d₃，θ₄)，根据D-H参数方法求得机器人正解的表达式：

$\left\{\begin{array}{c}{P}_x=l_2{C}_{12}+l_1{C}_1\\ P_y=l_2{S}_{12}+l_1{S}_1\\ P_z=-d_3\end{array}\right.$

其中C₁₂＝cos(θ₁+θ₂)，S₁₂＝sin(θ₁+θ₂)，C₁＝cos(θ₁)，S₁＝sin(θ₁)，l₁，l₂分别为第一连杆和第二连杆长度；

机器人的逆解表达式为：

${\mathrm{\&theta;}}_1=arctan\left(\frac{A}{\&PlusMinus;\sqrt{1-A^2}}\right)-arctan\left(\frac{P_x}{P_y}\right)$

其中

${\mathrm{\&theta;}}_2=arcsin\left(\frac{\sqrt{P_x^2+P_y^2}cos({\mathrm{\&theta;}}_1+arctan(\frac{P_x}{p_y}))}{l_2}\right)$

d₃＝-P_z

θ₄的值直接由工件需要旋转多少角度直接确定；

(2)由传感器获取目标在笛卡尔坐标系下坐标，经过机器人运动学逆解求取SCARA机器人各关节需要运动的位移S_i(i＝1，2，3，4)；

各关节的位移分别为：

$\left\{\begin{array}{c}{S}_1={\mathrm{\&theta;}}_1\\ S_2={\mathrm{\&theta;}}_2\\ S_3=d_3\\ S_4={\mathrm{\&theta;}}_4\end{array}\right.;$

(3)根据求得各关节的位移确定各关节轨迹延时时间T_di(i＝1，2，3，4)；

$T_{di}=\frac{S_i}{V_{i_{\max }}}(i=1,2,3,4)$

其中为第i关节运行时的最大速度；

(4)获取SCARA机器人运动时的各关节约束条件：最大速度V_max，最大加速度A_max，最大加加速度J_max，由约束条件确定各关节轨迹时间增益α_i(i＝1，2，3，4)；

${\mathrm{\&alpha;}}_i=min\{\frac{A_{i_{\max }}}{1.1754V_{i_{\max }}};\sqrt{\frac{J_{i_{nax}}}{2.1524V_{i_{max}}}}\}(i=1,2,3,4)$

其中表示第i关节的最大速度，表示第i关节的最大加速度，表示第i关节的最大加加速度；

(5)根据获得的各关节轨迹延时时间T_di(i＝1，2，3，4)和时间增益α_i(i＝1，2，3，4)，确定各关节速度轨迹表达式V_i(t)(i＝1，2，3，4)，其中t表示时间；

各关节速度轨迹为：

v_i(t)＝f(t)-f(t-T_di)H(t-T_di)

其中，

$f\left(t\right)=V_{i_{\max }}(1-e^{-{u_i}^3})$

u_i＝α_it

$\begin{array}{c}H\left(x\right)=\left\{\begin{array}{cc}0,& x<0\\ 1,& x\&GreaterEqual;0\end{array}\right.\\ (i=1,2,3,4)\end{array}.$