CN101733750B - 基于动态响应谱的关节含间隙机器人轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于动态响应谱的关节含间隙机器人轨迹规划方法。这种方法针对特定的机器人任务规划，在路径关键点之间以运动多项式函数进行机器人关节空间轨迹规划，把机器人每段运动轨迹的轨迹时间长度作为轨迹规划变量，获得关节反力随时间和轨迹规划变量改变而发生变化的动态响应谱。从动态响应谱中可以便捷地选择合理的轨迹时间长度，使机器人运动过程中含间隙关节连接的两构件始终不发生分离。这种方法不需要附加弹簧或配重等硬件物体，只需通过设计软件算法控制机器人轨迹即可取得效果。通过构造轨迹时间长度连续的动态响应谱，可以指导设计人员如何选择轨迹时间长度以避免关节元素发生分离。

Description

基于动态响应谱的关节含间隙机器人轨迹规划方法

技术领域

本发明涉及一种基于动态响应谱的关节含间隙机器人轨迹规划方法。

背景技术

机器人关节(即运动副)连接相邻的两个构件，并使构件之间能够产生相对运动。关节元素之间装配时必然留有间隙，随着机器人长时间的运转，关节间隙有增大的趋势。关节间隙的存在影响了机器人的运动精度，并使关节元素之间发生冲击碰撞，产生噪音，并加剧关节磨损，因此，对于机器人之类的精密机器，避免运动过程中关节元素发生分离，对于提高机器人操作的准确性与动态性能有重要的意义。

目前，人们主要从含间隙机构学的角度，研究一般连杆机构运动过程运动副避免分离方法。例如，Park于1987年在《Mechanism and Machine Theory》(22(6)：549-556)的论文“Counterweight optimization for reducing dynamic effects ofclearance at a revolute joint”中采用附加配重的方法提出了含间隙关节元素始终保持接触的方法。Li于1992年在《Mechanism and Machine Theory》(27(5)：535-541)的论文“Optimum balancing of linkages with clearances”中采用附加弹簧的方法使机构运动过程中含间隙关节元素不发生分离。Feng于2002年在《Journalof Mechanical Design》(124(1)：68-73)的论文“A new optimization method fordynamic design of planar linkage with clearances at joints-Optimizing the massdistribution of links to reduce the change of joint forces”中采用改变连杆形状的方法避免含间隙关节元素在平面连杆机构运动过程中发生分离。这些方法的缺点是需要附加弹簧或配重等硬件物体。

对于机器人操作臂，原动件由伺服电机驱动。机器人操作臂运动的位置、速度、加速度是可控的，加速度的可控意味着机构的惯性力是可控的。在考虑各关节速度、加速度和加速度变化约束的基础上，对机器人在关节空间进行不同的轨迹规划，机器人运动过程含间隙关节的接触状态也不相同。因此，轨迹规划是避免机器人运动过程中关节元素发生分离的有效方法。这种方法的优点是无需附加硬件物体，也无需改变机器人硬件，可通过软件方法实现。

发明内容

本发明的目的在于提供一种基于动态响应谱的关节含间隙机器人轨迹规划方法，来避免机器人运动过程中关节元素发生分离。

本发明采用的技术方案包括以下步骤：

(1)为机器人主动关节选择运动多项式函数：在机器人关节空间中进行轨迹规划，设笛卡尔空间中有P+1个路径点，通过反向运动学求解，把这P+1个路径点映射到机器人关节空间，设机器人有K个主动关节，每个主动关节的运动轨迹用P段多项式函数描述

q_k，p(t)＝A_k，p，Ht^H+A_k，p，H-1t^H-1+…+A_k，p，ht^h+…+A_k，p，1t+A_k，p，0      (式1)

t∈[0，T_p]，h＝0，1，…，H，p＝1，2，…，P，k＝1，2，…，K

其中q_k，p(t)表示第k个主动关节的第p段运动轨迹，A_k，p，h为第k个主动关节第p段轨迹的h次项系数，T_p为第p段轨迹的时间长度，t表示时间，H为运动多项式阶次，对(式1)求导获得关节速度的函数，再次求导将获得关节加速度的函数；

(2)计算运动多项式函数的待定系数：取运动多项式阶次H的初值为5，根据主动关节的各段运动轨迹上路径起点与终点的位置、速度与加速度的约束条件，求解五次运动多项式函数含有的6个待定系数；

(3)求解含间隙关节的关节反力：设机器人有C个含间隙关节，则每个含间隙关节中驱动构件受力

也是P段多项式函数

$\stackrel{\→}{F_{i,p}}=\left[W_{i,p,U}\right]t^U+\left[W_{i,p,U-1}\right]t^{U-1}+\·\·\·+\left[W_{i,p,u}\right]t^u+\·\·\·+\left[W_{i,p,1}\right]t+\left[W_{i,p,0}\right]$

u＝0，1，…，U，i＝1，2，…，C

其中表示第i个含间隙关节中驱动构件在第p段运动轨迹中所受的力，多项式函数的阶次U取决于各主动关节运动轨迹函数的阶次；[W_i，p，u]为第i个含间隙关节的关节反力在第p段运动轨迹上的多项式函数的u次项系数，这是一个3行1列的矩阵，每行元素是运动轨迹q_k，p(t)的系数A_k，p，h的函数；

(4)根据含间隙关节的关节反力构造动态响应谱：关节反力的模是T_p和t的函数，其中t∈[0，T_p]，动态响应谱是关节反力的模随时间和轨迹时间长度改变而发生变化的图谱；

(5)从动态响应谱中选择合适的轨迹时间变量，使得关节连接的两构件在运动过程中始终保持接触：轨迹规划中轨迹时间变量T_p的取值依据为，T_p的取值必须满足对任意t∈[0，T_p]，各关节反力的模大于0；

(6)若动态响应谱中没有符合要求的轨迹时间变量时，则增加运动多项式函数的阶次H，返回步骤(1)；否则，若找到了合适的轨迹时间变量，则轨迹规划完成。

根据步骤(4)构造的关节反力的动态响应谱是一个三维曲面，有两个自变量T_p和t，其中t∈[0，T_p]，函数值为关节反力的模。

根据步骤(5)，轨迹时间变量直接从动态响应谱中选取，使含间隙关节元素始终保持接触状态。

本发明具有的有益效果是：

1)不需要附加弹簧或配重等硬件物体，只需设计软件算法即可取得效果；

2)计算量较小；

3)通过构造轨迹时间长度连续的动态响应谱，可以指导设计人员如何选择轨迹时间长度以避免关节元素发生分离。

附图说明

图1是基于动态响应谱的关节含间隙机器人轨迹规划的流程图。

图2是5R机器人机构示意图。

图3是第三关节5的动态响应谱。

图4是轨迹时间变量T_p＝0.3时的第三关节5的关节反力响应曲线。

图5是轨迹时间变量T_p＝1.0时的第三关节5的关节反力响应曲线。

图中：1、第一关节，2、第一杆件，3、第二关节，4、第二杆件，5、第三关节，6、第三杆件，7、第四关节，8、第四杆件，9、第五关节。

具体实施方式

下面结合5R机器人对本发明作进一步说明。基于动态响应谱的关节含间隙机器人轨迹规划的流程图见图1所示，包括以下关键步骤：

(1)为机器人主动关节选择的运动多项式函数；

(2)计算多项函数的待定系数；

(3)求解含间隙关节的关节反力；

(4)根据含间隙关节的关节反力构造动态响应谱；

(5)从动态响应谱中选择合适的轨迹时间变量，使得运动副始终保持接触；

(6)若动态响应谱中没有符合要求的轨迹时间变量时，则增加轨迹规划多项式函数的阶次，返回步骤(1)；否则，若找到了合适的轨迹时间变量，则轨迹规划完成。

以图2所示的5R并联机器人为例。第一关节1、第二关节3是主动关节，设第三关节5含有间隙。

(1)选择5R机器人主动关节的运动多项式函数为五次多项式。设各主动关节运动轨迹的五次多项式函数为：

q_k＝A_k，5t⁵+A_k，4t⁴+A_k，3t³+A_k，2t²+A_k，1t+A_k，0

其中k＝1，2。

(2)计算多项函数的待定系数。设机器人运动轨迹的起始点在关节坐标系中描述为(q_1o，q_2o，q_3o)，终点为(q_1e，q_2e，q_3e)，且起点和终点间仅含一段轨迹。在本方法中，主动关节的各段轨迹消耗的时间T_p是轨迹时间长度，它将由轨迹规划算法确定。则运动多项式的系数为

A_k，0＝q_ko

A_k，1＝0

A_k，2＝0

$A_{k,3}=\frac{10(q_{\mathrm{ke}}-q_{\mathrm{ko}})}{T_p^3}$

$A_{k,4}=-\frac{15(q_{\mathrm{ke}}-q_{\mathrm{ko}})}{T_p^4}$

$A_{k,5}=\frac{6(q_{\mathrm{ke}}-q_{\mathrm{ko}})}{T_p^5}.$

(3)求解含间隙关节的关节反力。机器人机构的动力学参数的设定值如表1所示，其中l_a1为第一杆件2的长度，l_a2为第二杆件4的长度，l_a2c为第二杆件4质心到第二关节3的长度，m_a2为第二杆件4的质量，J_a2c为第二杆件4的转动惯量，l_b1为第四杆件8的长度，l_b2为第三杆件6的长度，l_b2c为第三杆件6质心到第四关节7的长度，m_b2为第三杆件6的质量，J_b2c为第三杆件6的转动惯量，l_ab为第一关节1到第五关节9的长度，g为重力加速度，q_A1o为第二关节4的起始角度，q_A1e为第二关节4的终止角度，q_B1o为第四关节8的起始角度，q_B1e为第四关节8的终止角度。采用多体动力学的牛顿-欧拉方程求解含间隙关节即第三关节5的关节反力。

表15R机器人的动力学参数

la1	0.3m	lb2	0.8m	qA1o	59.68°
						la2	0.8m	lb2c	0.4m	qA1e	55.83°
la2c	0.4m	mb2	12kg	qB1o	120.32°
						ma2	12kg	Jb2c	0.64kgm2	qB1e	3.54°
Ja2c	0.64kgm2	lab	0.3m
						lb1	0.3m	g	9.8N/s2

(4)根据含间隙关节的关节反力构造动态响应谱。第三关节5的关节反力模的动态响应谱如图3所示，其中，坐标T_p表示轨迹时间长度，坐标t表示时间，坐标H_c表示第三关节5关节反力的模。

(5)从动态响应谱中选择合适的轨迹时间变量，使得运动副始终保持接触。图4、图5分别描述了从动态响应谱中切取的T_p＝0.3和T_p＝1.0时的第三关节5关节反力模的曲线，图中关节反力模的数值超出350N的曲线被截去了。从图5可以看出，T_p＝1.0时曲线存在关节反力模等于0的情形，关节连接的两构件在机器人运动过程中会发生分离。从图4可以看出，T_p＝0.3时关节反力模的最小值离0最远，即曲线距发生分离的边缘最远，关节连接的两构件在机器人运动过程中不会发生分离，因此，选择轨迹时间变量T_p＝0.3。

(6)轨迹规划完成。

Claims (1)

1.一种基于动态响应谱的关节含间隙机器人轨迹规划方法，其特征在于该方法的步骤如下：

(1)为机器人主动关节选择运动多项式函数：在机器人关节空间中进行轨迹规划，设笛卡尔空间中有P+1个路径点，通过反向运动学求解，把这P+1个路径点映射到机器人关节空间，设机器人有K个主动关节，每个主动关节的运动轨迹用P段多项式函数描述：

q_k，p(t)＝A_k，p，Ht^H+A_k，p，H-1t^H-1+…+A_k，p，ht^h+…+A_k，p，1t+A_k，p，0

                                                                             (式1)

t∈[0，T_p]，h＝0，1，…，H，p＝1，2，…，P，k＝1，2，…，K

其中q_k，p(t)表示第k个主动关节的第p段运动轨迹，A_k，p，h为第k个主动关节第p段轨迹的h次项系数，T_p为第p段轨迹的时间长度，t表示时间，H为运动多项式阶次，对(式1)求导获得关节速度的函数，再次求导将获得关节加速度的函数；

(2)计算运动多项式函数的待定系数：取运动多项式阶次H的初值为5，根据主动关节的各段运动轨迹上路径起点与终点的位置、速度与加速度的约束条件，求解五次运动多项式函数含有的6个待定系数；

(3)求解含间隙关节的关节反力：设机器人有C个含间隙关节，则每个含间隙关节中驱动构件受力

也是P段多项式函数：

$\stackrel{\‾}{F_{i,p}}=\left[W_{i,p,U}\right]t^U+\left[W_{i,p,U-1}\right]t^{U-1}+...+\left[W_{i,p,U}\right]t^u+...+\left[W_{i,p,1}\right]t+\left[W_{i,p,0}\right]$

u＝0，1，…，U，i＝1，2，…，C

其中

表示第i个含间隙关节中驱动构件在第p段运动轨迹中所受的力，多项式函数的阶次U取决于各主动关节运动轨迹函数的阶次；[W_i，p，u]为第i个含间隙关节的关节反力在第p段运动轨迹上的多项式函数的u次项系数，这是一个3行1列的矩阵，每行元素是运动轨迹q_k，p(t)的系数A_k，p，h的函数；

(4)根据含间隙关节的关节反力构造动态响应谱：关节反力的模是T_p和t的函数，其中t∈[0，T_p]，动态响应谱是关节反力的模随时间和轨迹时间长度改变而发生变化的图谱；

(5)从动态响应谱中选择合适的轨迹时间变量，使得关节连接的两构件在运动过程中始终保持接触：轨迹规划中轨迹时间变量T_p的取值依据为，T_p的取值必须满足对任意t∈[0，T_p]，各关节反力的模大于0；

(6)若动态响应谱中没有符合要求的轨迹时间变量时，则增加运动多项式函数的阶次H，返回步骤(1)；否则，若找到了合适的轨迹时间变量，则轨迹规划完成。

Images