CN107957684A

CN107957684A - 一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法

Info

Publication number: CN107957684A
Application number: CN201711145217.1A
Authority: CN
Inventors: 翟敬梅; 李连中; 刘坤; 胡燕
Original assignee: South China University of Technology SCUT
Current assignee: South China University of Technology SCUT
Priority date: 2017-11-17
Filing date: 2017-11-17
Publication date: 2018-04-24

Abstract

本发明公开一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法，包括以下步骤：（1）建立机器人的运动学模型；（2）分析机器人的正逆运动学；（3）求解机器人雅可比矩阵；（4）基于扫掠球量对机器人机械臂进行模型简化；（5）建立机器人碰撞检测矩阵求取简化后机器人模型与障碍物的最近距离；（6）改进虚拟速度向量场的吸引速度、排斥速度、切向速度三个向量场函数进行多机器人的无碰运动规划；（7）基于路径优先策略对机器人速度进行优化。本发明对机器人无碰轨迹规划算法进行了优化，提高了机器人运行时的平稳性、轨迹的平滑度以及作业效率。

Description

一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法

技术领域

本发明涉及机器人运动规划领域，具体设计一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法。

背景技术

近年来，机器人的无碰运动规划算法研究取得了非凡成果，目前提出了多种规划算法。从控制方法上，机器人无碰运动算法可以分为基于加速度、力矩和速度的控制方法和基于速度水平的避障控制方法。从实时性上，可以分为基于离线的无碰轨迹规划方法和实时的无碰运动规划方法。这些方法虽然能够较高效地对机器人的路径进行规划得到无碰轨迹，但是要么需要建立精确的动力学模型，要么运用过程中可能会遇到一些奇异的问题，使得这些方法的实际应用领域并不广泛。

在众多无碰规划算法中，基于虚拟速度向量场的规划算法因其简单高效而收到广泛的应用，其基本概念是机器人在目标点产生的吸引势场和障碍物产生的排斥势场的作用下，无碰撞地由起点运动向终点。基于虚拟速度向量场机器人无碰运动规划算法在实际的应用中也存在着一些问题：(1)目前基于虚拟速度向量场的无碰规划算法在工业机器人领域应用并不广泛；(2)三维空间运行可行轨迹较多；(3)多机器人系统工作时避碰路径优先的问题；(4)机器人运行轨迹不平滑；

发明内容

针对上述技术问题，本发明提供一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法，可以解决工业机器人在三维作业空间中协同工作时避碰路径优化的问题，优化了三维空间中切向向量场以解决切向速度路径较多的问题，同时针对多机器人提出了基于路径优先级的策略。

为实现上述目的，本发明的技术方案是：

一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法，用于对工业机器人在三维作业空间中基于三个虚拟速度向量场的轨迹优化，应用于多机器人在协同作业时机器人间避碰、机器人与障碍物间的避碰及避碰路径优化，具体包括以下步骤：

(1)建立机器人的运动学模型；

(2)分析机器人的正、逆运动学，得到机器人末端位姿和关节变量之间的关系；

(3)求解机器人的雅可比矩阵以分析机器人的工作状态；

(4)根据机器人的几何特征选用不同的扫掠球量建立机器人混合层次简化模型；

(5)通过检测简化模型对机器人进行碰撞检测，并求得简化模型与障碍物之间的最近距离；

(6)改进虚拟速度向量场的吸引速度、排斥速度、切向速度三个向量场函数，综合三者得到关节空间的速度向量场函数；

(7)利用路径优先策略对机器人速度优化，实现多机器人的无碰运动规划。

所述步骤(2)的机器人正运动学是指根据机器人的关节变量值计算末端执行器的位置和姿态；机器人的逆运动学是指机器人末端执行器在笛卡尔空间的位置与姿态计算到机器人各关节变量值。

进一步地，所述步骤(1)用D‐H坐标系建立机器人模型，即用包含四个连杆参数的4×4齐次变换矩阵来描述两个相邻连杆的空间关系，把连杆间的空间几何问题转化成矩阵代数问题，在此基础上分析机器人的正逆运动学。

进一步地，所述步骤(3)分析机器人工作状态的雅可比矩阵，能够描述机器人在操作空间的运行速度与其各关节的运行速度之间的转化关系。求解雅可比矩阵的方法一般有矢量积法和微分变换法两种，为了补偿机器人末端执行器位姿与目标物体之间的误差，以及解决两个不同坐标系之间的微位移关系问题，这里将通过微分变换法来求得机器人的雅可比矩阵。

进一步地，所述步骤(4)的基于扫掠球量的模型简化，采用集合特性简单的包围盒近似地描述机械臂的关节和末端执行器，得到方便测量的机械臂几何外形。目前的包围盒种类有很多，考虑简单有效、包围紧密性好、混合使用等优点，使用层次扫掠球包围盒来简化模型。

进一步地，所述步骤(5)根据扫掠球量和扫掠基体的半径建立多机器人碰撞检测矩阵进而求得机器人和障碍物之间的最近距离。

进一步地，所述步骤(6)中改进虚拟速度向量场的吸引速度、排斥速度、切向速度三个向量场函数包括以下步骤：

(61)将构建的吸引速度向量场函数与机器人角速度、机器人伪逆雅可比矩阵构成机器人的关节吸引速度；

(62)将构建的排斥速度函数与机器人角速度、机器人偏雅克比矩阵构成机器人的关节排斥速度；

(63)将构建的切向速度函数与机器人角速度、机器人偏雅克比矩阵构成机器人的关节切向速度；

进一步地，所述步骤(61)在吸引速度向量函数中引入饱和函数，使机器人接近目标点时降低吸引速度的大小，即根据目标点与末端点之间的距离和吸引速度的大小定义出吸引速度向量场函数，然后根据欧拉运动学方程定义出机器人的角速度，再结合机器人伪逆雅可比矩阵构成饱和函数改进吸引速度向量场。

进一步地，所述步骤(62)在排斥速度向量场中引入一个虚拟弹簧阻尼系统，采用模糊逻辑方法优化弹簧阻尼系数，得到基于弹簧阻尼系统的排斥速度函数。

进一步地，所述步骤(63)在三维空间中，基于路径最短的思想，综合动态障碍物运动方向，优化切向速度与机器人速度夹角之间的关系改进切向速度向量场函数，选取切向速度与机器人速度夹角最小，使机器人运行轨迹最短。

进一步地，所述步骤(7)根据机器人执行任务的重要程度引入优先级因子定义出速度向量场函数，根据机器人所执行任务的重要度分别赋予相应的优先级，机器人赋予的优先级越高，其排斥速度和切向速度就会越小，相应的其避障路径就会越短。

相比现有的机器人无碰运动规划算法，本发明对传统基于虚拟速度向量场的机器人无碰撞运动规划算法进行了改进，在保持原有简单高效的同时解决了工业机器人在三维作业空间中协同工作时避碰路径优化的问题，优化了三维空间中切向向量场以解决切向速度路径较多的问题，同时针对多机器人提出了基于路径优先级的策略进一步优化无碰运动规划算法。

附图说明

图1为本发明实施例的机器人三维无碰轨迹规划方法流程图。

图2a为面对静态障碍物时的切向速度势场示意图。

图2b为面对右向运动的动态障碍物时的切向速度势场示意图。

图2c为面对左向运动的动态障碍物时的切向速度势场示意图。

具体实施方式

下面通过具体实施例对本发明的目的作进一步详细地描述，实施例不能在此一一赘述，但本发明的实施方式并不因此限定于以下实施例。

如图1所示，一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法，用于对工业机器人在三维作业空间中基于三个虚拟速度向量场的轨迹优化，采用两个工业六自由度机器人和模拟障碍物共同实现多机器人无碰运动规划，包括以下步骤：

S1、建立机器人的运动学模型：用D‐H坐标系建立机器人模型，建立包含四个连杆参数的机器人4×4齐次连杆变换矩阵：

其中，α_i-1为连杆i‐1的连杆偏角，θ_i为转动关节的关节角，d_i为转动关节的连杆偏距。

S2、分析机器人的正、逆运动学，得到机器人末端位姿和关节变量之间的关系，即基于D‐H坐标系列出机器人的正运动学方程式，根据机器人的关节变量值计算末端执行器的位置和姿态；根据机器人末端执行器在笛卡尔空间的位置与姿态进行机器人逆解计算机器人各关节变量值。

S3、求解机器人的雅可比矩阵以分析机器人的工作状态：通过微分变换法来求得机器人的雅可比矩阵，得到机器人在操作空间的运行速度与其各关节的运行速度之间的转化关系，以此判断机器人末端在工作空间的位置。

S4、建立机器人混合层次简化模型：根据机器人的几何特征选用不同的扫掠球量，分为多个层次混合包围被包围体，得到机器人简化模型，具体按以下步骤进行简化：

S41、采用简单的模型简化机制，视机械臂为一个整体，采用一个矩形扫掠球量包围，扫掠球的形状随着机器人的位置变化而变化，用于快速排除相距较远的障碍物；

S42、当第一层检测到有障碍物时，进入第二层检测，在此层机械臂用四个扫掠球量混合紧密包围，用于精确检测机器人与障碍物的最近距离。

S5、通过检测简化模型对机器人进行碰撞检测，求简化模型和障碍物之间的最近距离：根据机械臂扫掠球量包围量和障碍物扫掠球量包围量建立碰撞检测矩阵，近似计算两集合体的距离，即可得到机器人和障碍物之间的最近距离。

S6、改进基于虚拟速度向量场的无碰轨迹规划算法，对目前基于虚拟速度向量场机器人无碰运动规划算法按以下步骤进行优化，综合三者得到关节空间的速度向量场函数：

S61、根据目标点G(x_G,y_G,z_G)与末端点N(x,y,z)之间的距离和吸引速度的大小定义出吸引速度向量场函数：

其中，v_a为吸引速度的大小，是点N与G之间的欧氏距离；

根据欧拉运动学方程定义出机器人的角速度：

其中，θ_N、θ_N、ψ_N表示机器人的此时欧拉角，θ_G、ψ_G表示机器人的末态欧拉角；

结合机器人伪逆雅可比矩阵构成饱和函数改进吸引速度向量场，得到机械手不处于奇异位置时的关节吸引速度公式：

其中，piv(J)＝(J^T·J)^-1·J^T为机器人伪逆雅克比矩阵；

S62、在排斥速度向量场中引入一个虚拟弹簧阻尼系统，得到基于弹簧阻尼系统的排斥速度：

v_r＝k_p·(d_r-d₀)+k_d·(d_r-d₀)′

其中，k_p和k_d为弹簧阻尼系数，(d_r-d₀)′是(d_r-d₀)的一阶导数；

采用模糊逻辑方法优化弹簧阻尼系数，得到排斥速度向量场函数：

其中，M表示一个很大的正数，为O和R的欧氏距离；

为将操作空间的速度转化为机器人关节空间的速度，构建如下“偏雅克比矩阵”J′：

其中，m为关节数；

机器人操作空间的速度与关节空间的速度转化关系表示为：

其中，表示机器人的角速度；

假设机器人处于非奇异位置，则关节排斥速度可表示为：

其中，piv(J')为机器人伪逆雅克比矩阵；

S63、在三维空间中，为使机器人的运行路径尽量小，切向速度向量场定义在切向速度和机器人速度形成的平面P内且与以d_s为半径的圆相切的方向上，切向速度势场示意图如图2a、图2b、图2c所示，切向速度向量场函数定义为：

其中，M表示一个很大的正数，是切向速度势场的单向矢量，γ是通过模糊逻辑方法获得的势场强度；

为将操作空间的速度转化为机器人关节空间的速度，构建如下矩阵：

其中，m为关节数；

机器人操作空间的速度与关节空间的速度转化关系表示为：

其中，表示机器人的角速度；

假设机器人处于非奇异位置，则三维空间切向速度向量场中机器人的关节速度公式表示为：

其中，piv(J')为机器人伪逆雅克比矩阵；

S7、优化机器人速度：采用路径优先策略，引入优先级因子定义出关节空间的速度向量场函数：

其中，α∈[0,1]为优先级因子，α＝0时优先级最高，α＝1时优先级最低。n为环境障碍物的个数，m为机器人的个数，和分别为第i(j)个障碍物形成的排斥关节速度和切向关节速度；

将关节速度按照一定的离散周期转化为关节位置，进而控制机器人运动：

其中，q_i(t)为关节i在时刻t的位置，Δt为离散周期；

综合改进的虚拟向量场中的吸引速度、排斥速度、切向速度公式和优化后的机器人速度公式得到移动机器人的速度向量场函数：

其中，表示第i(j)个障碍物形成的排斥速度，表示第i(j)个障碍物形成的切向速度；

从而实现基于虚拟速度向量场的机器人无碰运动规划。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims

1.一种基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于，对工业机器人在三维作业空间中基于三个虚拟速度向量场的轨迹优化，应用于多机器人在协同作业时机器人间避碰、机器人与障碍物间的避碰及避碰路径优化，具体包括以下步骤：

（1）建立机器人的运动学模型；

（2）分析机器人的正、逆运动学，得到机器人末端位姿和关节变量之间的关系；

（3）求解机器人的雅可比矩阵以分析机器人的工作状态；

（4）根据机器人的几何特征选用不同的扫掠球量建立机器人混合层次简化模型；

（5）通过检测简化模型对机器人进行碰撞检测，并求得简化模型与障碍物之间的最近距离；

（6）改进虚拟速度向量场的吸引速度、排斥速度、切向速度三个向量场函数，综合三者得到关节空间的速度向量场函数；

（7）利用路径优先策略对机器人速度优化，实现多机器人的无碰运动规划。

2.根据权利要求1所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（1）采用D-H坐标系建立机器人模型。

3.根据权利要求1所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（3）中通过微分变换法分析机器人工作状态的雅可比矩阵。

4.根据权利要求1所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（4）通过层次扫掠球包围盒简化机器人模型，即采用集合特性简单的层次扫掠球包围盒近似地描述机械臂的关节和末端执行器，得到方便测量的机械臂几何外形。

5.根据权利要求1所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（5）根据扫掠球量和扫掠基体的半径建立多机器人碰撞检测矩阵进而求得机器人和障碍物之间的最近距离。

6.根据权利要求1所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（6）中改进虚拟速度向量场的吸引速度、排斥速度、切向速度三个向量场函数包括以下步骤：

（61）将构建的吸引速度向量场函数与机器人角速度、机器人伪逆雅可比矩阵构成机器人的关节吸引速度；

（62）将构建的排斥速度函数与机器人角速度、机器人偏雅克比矩阵构成机器人的关节排斥速度；

（63）将构建的切向速度函数与机器人角速度、机器人偏雅克比矩阵构成机器人的关节切向速度。

7.根据权利要求6所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（61）在吸引速度向量函数中引入饱和函数，使机器人接近目标点时降低吸引速度的大小，即根据目标点与末端点之间的距离和吸引速度的大小定义出吸引速度向量场函数，然后根据欧拉运动学方程定义出机器人的角速度，再结合机器人伪逆雅可比矩阵构成饱和函数改进吸引速度向量场。

8.根据权利要求6所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（62）在排斥速度向量场中引入一个虚拟弹簧阻尼系统，采用模糊逻辑方法优化弹簧阻尼系数，得到基于弹簧阻尼系统的排斥速度函数。

9.根据权利要求6所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（63）在三维空间中，基于路径最短的思想，综合动态障碍物运动方向，优化切向速度与机器人速度夹角之间的关系改进切向速度向量场函数，选取切向速度与机器人速度夹角最小，使机器人运行轨迹最短。

10.根据权利要求1所述的基于虚拟速度向量场的机器人三维无碰轨迹规划方法，其特征在于：所述步骤（7）根据机器人执行任务的重要程度引入优先级因子定义出速度向量场函数，根据机器人所执行任务的重要度分别赋予相应的优先级，机器人赋予的优先级越高，其排斥速度和切向速度就会越小，相应的其避障路径就会越短。