CN113156978A

CN113156978A - 一种基于谱图模型引导的机器人采样运动规划方法及系统

Info

Publication number: CN113156978A
Application number: CN202110573761.6A
Authority: CN
Inventors: 夏崇坤; 梁斌; 王学谦; 刘厚德; 麦宋平
Original assignee: Shenzhen International Graduate School of Tsinghua University
Current assignee: Shenzhen International Graduate School of Tsinghua University
Priority date: 2021-05-25
Filing date: 2021-05-25
Publication date: 2021-07-23
Anticipated expiration: 2041-05-25
Also published as: CN113156978B

Abstract

本发明公开了一种基于谱图模型引导的机器人采样运动规划方法及系统，所述方法包括如下步骤：S1、通过谱图聚类来表征学习笛卡尔空间的非障碍物区域在机器人构型空间中的映射分布；并在特征空间中通过样本训练相关的谱图模型；S2、以训练好的谱图模型为标准采样规划算法提供带有引导性策略的样本生成器，生成采样规划所需要的样本；S3、引导规划探索过程在构型空间的非障碍物区域展开。本发明减少或避免了碰撞检测，提高了采样运动规划的实际运行效率。

Description

一种基于谱图模型引导的机器人采样运动规划方法及系统

技术领域

本发明涉及机器人运动规划领域，特别是涉及一种基于谱图模型引导的机器人采样运动规划方法及系统。

背景技术

进入21世纪以来，随着服务机器人的快速发展，机器人所面临的任务需求大幅增长，对灵巧操作提出了更高要求。运动规划是机器人灵巧操作的基础，特别是执行复杂操作任务时，高效的运动规划方法就显得非常重要。

对于复杂操作过程中，机器人使用采样运动规划方法时容易出现以下几个问题：

1.运动规划时间长。由于当前主流的采样运动规划方法如RRT,PRM等对于存在障碍物约束的场景规划时间较长，难以满足机器人执行实际操作任务的需要。

2.规划路径较差。规划路径质量主要取决于规划路径长度和规划路径复杂性。规划路径差通常意味着规划路径较长，在空间中的轨迹表现较为复杂。通常来说，规划路径质量较差意味着会消耗更多能量，可能造成不必要的机械故障。

3.规划成功率低。对于复杂障碍物约束的环境场景来说，采样规划算法由于需要随机采样，导致存在不确定性，规划成功率普遍较低，无法满足实际的规划需求，这也是当前研究的热点和难点。

2018年宾夕法尼亚大学Daniel D.Lee等提出一种基于高斯混合模型的采样运动规划方法，主要解决方案如下：

1.根据示教或已成功的规划信息进行预处理，整理出障碍物约束在构型空间对应的实际映射数据，利用GMMs来学习或表征这些实际映射数据对应的分布信息，获取笛卡尔空间下非障碍物区域在构型空间的映射分布。

2.根据运动规划问题(Xfree,xstart,xgoal)和已训练的高斯混合模型，利用已学习的高斯混合模型生成样本点，并将其作为基本采样规划算法(如RRT、RRT*、PRM等)的偏置性样本生成器。

3.在基于GMMs的样本生成器的基础上，正常运行采样运动规划算法即可。

该基于GMMs方法有如下不足：

1)GMMs的参数往往难以准确调整，需要花费大量时间来寻找最优参数。由于最优参数不易寻找，导致面对一些复杂场景，GMMs无法准确地表征构型空间的非障碍物区域，使得非均匀采样效率低下，无法有效缩短运动规划时间，规划路径质量和成功率也无法得到提高。

2)基于GMMs的方法无法在线快速调整与更新模型，对于缓慢变化的动态环境规划效果不佳。

发明内容

为了弥补上述现有技术的不足，本发明公开了一种基于谱图模型引导的机器人采样运动规划方法及系统，减少或避免碰撞检测，提高采样运动规划的实际运行效率。

本发明的技术问题通过以下的技术方案予以解决：

一种基于谱图模型引导的机器人采样运动规划方法，其特征在于包括如下步骤：S1、通过谱图聚类来表征学习笛卡尔空间的非障碍物区域在机器人构型空间中的映射分布；并在特征空间中通过样本训练相关的谱图模型；S2、以训练好的谱图模型为标准采样规划算法提供带有引导性策略的样本生成器，生成采样规划所需要的样本；S3、引导规划探索过程在构型空间的非障碍物区域展开。

在一些实施例中，本发明还包括如下特征：

还包括步骤S4：利用谱图模型的在线学习与更新机制对已训练的谱图模型进行在线调整，以应对环境的缓慢变化。

在特征空间中通过样本训练相关的谱图模型包括如下步骤：把过往成功的演示或规划数据生成的样本映射到2n维特征空间；利用谱聚类算法学习基于谱图模型的2n维特征空间表示模型。

使用两个谱图模型分别学习采样分布：一个学习碰撞区域的构型分布，另一个学习无碰撞区域的构型分布。

通过学习到的分布，利用M距离计算新样本与谱图模型生成的分布之间的距离。

训练分为离线训练和在线规划两个部分；所述离线训练部分包括：首先将根据过往演示或成功的规划经验生成样本，并将其映射到新的特征空间，接着利用增量谱聚类算法进行训练，对非障碍物区域的构型空间分布进行表征学习并获得已训练的谱图模型。

在线规划部分首先利用已训练的谱图模型作为采样规划方法的样本生成器以代替自身的随机采样，从而形成基于SGMs引导的偏置性样本生成策略，接着运行不包含碰撞检测的采样规划算法如RRT、PRM等并生成候选路径；如果发现环境发生微小缓慢的变化，则在变化周围重新生成样本，并利用在线学习与更新机制对谱图模型进行在线更新。

构型空间是由正余弦构成，采用曼哈顿距离作为两个构型之间的距离指标；使用规范化的拉普拉斯矩阵(随机游动规范化)来生成特征向量。

使用学习到的谱图模型来作为采样域，从而偏置性的引导采样运动规划算法的树或图扩展过程在非障碍物区域的构型空间展开，从而大幅减少碰撞检测所消耗的时间，进一步提高规划效率。

本发明还提出一种基于谱图模型引导的机器人采样运动规划系统，包括处理器和存储器，所述存储器中存储有计算机程序，所述计算机程序可被处理器执行以实现如上所述的方法。

本发明与现有技术对比的有益效果包括：本发明的基于谱图模型(Spectralgraph models,SGMs)引导的运动规划方法，通过运用谱图模型来表征三维空间障碍物约束在构型空间的非线性映射分布，并将其作为标准采样规划算法的样本生成器来生成采样规划所需要的样本，引导采样规划算法的探索过程在构型空间的非障碍物区域展开，从而减少或避免碰撞检测，提高采样运动规划的实际运行效率。

附图说明

图1是本发明实施例的谱聚类最优子图分割示意图。

图2是本发明实施例的基于SGMs引导的采样运动规划方法流程示意图。

图3是本发明实施例的面向复杂障碍物环境的6自由度平面机械臂运动规划仿真环境示意图。

图4是本发明实施例的面向三维缓慢变化场景的6自由度机械臂运动规划仿真环境示意图。

图5a、5b、5c、5d是本发明实施例的面向三维缓慢变化场景的运动规划实验结果示意图(以单次变化为例)。

具体实施方式

下面对照附图并结合优选的实施方式对本发明做进一步说明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

需要说明的是，本实施例中的左、右、上、下、顶、底等方位用语，仅是互为相对概念，或是以产品的正常使用状态为参考的，而不应该认为是具有限制性的。

本发明实施例所要解决的是当前采样规划算法(如RRT，PRM，FMT等)在缓慢动态变化障碍物环境条件下的如下运动规划问题：

第一，当前的主流采样规划算法来说动态变化的场景需要反复重新规划，降低了规划效率，难以满足动态变化场景的规划要求。

第二，缓慢动态变化的场景会使得采样规划方法的规划低成功率大幅下降，导致机器人无法完成指定作业任务。

本实施例涉及智能机器人与运动规划领域，主要包括一种新型的运动规划方法的设计，其具备的功能有：

第一，本实施例所设计的SGMs可以较好地表征障碍物环境在构型空间的映射分布，在线更新机制也可以应对缓慢变化场景。

第二，本实施例所提出的SGMs具有较好的通用性，以训练好的谱图模型作为样本生成器，可应用于主流的采样运动规划算法如RRT,PRM,FMT等。

为了方便描述，先对相关术语解释如下：

采样运动规划：

对于机器人学来说，运动规划问题是搜索并寻找一条从起始状态qstart到目标状态qgoal的离散轨迹序列。对于地面移动机器人或空中飞行机器人来说，规划主要在二维地面或三维空间展开，涉及的规划空间维度较低，通常称为路径规划；对于多自由度串联机械臂来说，运动规划主要在各个关节构成的构型空间中展开，对应的规划维度与机械臂自由度一致，维度较高，通常称为运动规划。在机构学上，某一时刻这些刚体的关节运动可以用一组关节的转动角度表示，称为构型(Configuration)，由构型所形成的空间称为构型空间(Configuration space)。那么，多自由度串联关节型机械臂的运动规划就是给定机械臂的初始状态和目标状态，通过某种策略在构型空间中找到一系列构型序列，使其通过逆运动学可求解出一条符合约束条件的末端执行器连续运动路径。基于采样的运动规划方法(简称采样运动规划方法)是解决运动规划问题的主流方法，比较前沿的方法包括快速扩展随机树(Rapidly-exploring random tree,RRT)及其变种算法、随机路图法(Probabilisticroadmap，PRM)及其变种算法等。

谱图模型：

谱图模型(spectral clustering)是从图论中演化出来的一个聚类分支，我们将由谱聚类方法所获得的结果称为谱图。谱聚类的主要思想是把数据看作空间中带有多个属性标签的点，由这些点构成带权无向图，通过将无向图划分为多个最优子图，使得子图内部尽量相似，而子图间距离尽量远离，以达到数据聚类的目的。需要说明的是，最优指的是目标函数达到最优，目标函数可以是切割边最小分割，也可以是分割规模差不多且分割边最小，区别如图1所示。

所述方法如下：

1.基于SGMs的采样运动规划方法

为了利用SGMs对碰撞区域和无碰撞区域对应构型空间分布进行表征，需要在特征空间中通过样本训练相关的谱图模型。首先，把过往成功的演示或规划数据生成的样本映射到2n维特征空间。然后，利用谱聚类算法学习基于谱图模型的2n维特征空间表示模型。由于状态空间的二分性，我们使用两个谱图模型分别学习采样分布：一个学习碰撞区域的构型分布，另一个学习无碰撞区域的构型分布。通过学习到的分布，本实施例可以通过M距离计算新样本与谱图模型生成的分布之间的距离。此外，学习到的谱图模型可以随着每一个树扩展过程进行在线优化和更新。由于谱图模型为标准采样规划算法提供了带有引导性策略的样本生成器，从而保证规划过程主要在无碰撞区域对应的构型空间展开，进而减少碰撞检测时间，提高规划效率。

基于SGMs引导的采样运动规划方法流程示意图如图2所示。由图2可知，整个方法流程实际上分为离线训练和在线规划两个部分。离线训练部分，本实施例首先将根据过往演示或成功的规划经验生成样本，并将其映射到新的特征空间，接着利用增量谱聚类算法进行训练，对非障碍物区域的构型空间分布进行表征学习并获得已训练的谱图模型；在线规划部分，本实施例首先利用已训练的谱图模型作为采样规划方法的样本生成器以代替自身的随机采样，从而形成基于SGMs引导的偏置性样本生成策略，接着运行不包含碰撞检测的采样规划算法如RRT、PRM等并生成候选路径；如果发现环境发生微小缓慢的变化，那么可以在变化周围重新生成样本，并利用在线学习与更新机制对谱图模型进行在线更新。那么，以主流的采样规划算法RRT为例，基于SGMs引导的RRT运动规划方法对应的伪代码如算法1所示。

2.基于SGMs的新型特征空间设计

本小节主要考虑图2中离线训练部分的特征空间设计，为训练数据映射到特征空间提供理论基础。特征空间是采样运动规划算法的主要工作空间。对于串联型多自由度机械臂来说，用构型空间来表示特征空间。每个关节转动的正角度和负角度在180度时所处的位置是一样的，即某个关节转动正角度180度和负角度180时对应的机械臂构型是一致的。由于关节在负角度180度到正角度180度之间存在若干奇异值(即该处的构型失效)，为保证构型空间的规划成功率，根据输入角度θ＝{θ1,θ2,θn}(n表示机械臂的关节数目，即构型空间的原始维度)，本实施例设计了新型的特征空间Ωnew＝{sinθ,cosθ}，其维度为2n。在表征构型空间分布时，传统的方法是采用欧式距离作为两个构型之间的距离指标。由于构型空间是由角度构成的，本实施例设计的新型构型空间是由正余弦构成，因此，本实施例采用曼哈顿距离作为两个构型之间的距离指标。

3.基于增量谱聚类的特征空间表征学习

本小节主要考虑离线训练部分地利用增量谱聚类方法对非障碍物区域的构型空间分布进行表征学习并形成谱图模型。在新设计的特征空间中，本实施例采用谱图模型对非障碍物区域对应的特征空间分布进行学习。表征学习的主要方式是采用谱聚类方法。聚类方法依赖于分析近邻矩阵的特征结构来获得潜在分布，而不是估计显式的数据分布模型。本实施例以谱聚类算法为核心的聚类方法构建初始SGMs。本实施例使用规范化的拉普拉斯矩阵(随机游动规范化)来生成特征向量。初始谱图模型构建算法伪代码如图3所示。

近邻矩阵W可以通过带有伸缩因子σ的样本之间距离计算，计算公式如下：

其中d(θ_i，θ_j)表示构型θ_i和θ_j之间的曼哈顿距离。σ值是计算近邻矩阵的一个非常重要的参数。如果σ的值非常高，与真实构型数据规模相比，大多数样本将显示相似性。如果σ值太低，即使是相近的样本之间的相似度也会很低。这两种情况都会导致次优聚类。

4.SGMs的在线学习和更新机制

本小节主要介绍在采样运动规划过程中当树或图扩展时在线学习和更新图模型的过程。在更新SGMs时，有两类新样本：一类是从碰撞检测中获得的，另一类是在最终候选路径中检测到碰撞时生成的。这些条件表明，增量机制应该处理节点的插入/删除以及现有节点之间的相似性变化。本节用关联向量来求解。相关定义和算法描述如下。

定义1：假设一个向量r_ij(w)是一个只有两个非零的行向量：第i个元素等于根号

第j个元素等于

表示点i和点j具有一个相似度w。那么，称这个向量为入射向量。

定义2：由入射向量构成行元素的矩阵R称为入射矩阵。

本质上，基于学习的谱图模型等于附加到入射矩阵R上的关联向量r_ij(Δw_ij)。通过更新r_ij(Δw_ij)，我们可以在聚类过程中模拟近似特征值和特征向量之间的增量。此外，我们将持续逼近，并给出归一化切割的广义特征值系统Lu＝λDu。需要注意的是向量的长度与样本的数量一致。

(1)计算特征值增量Δλ：对于对称广义特征值系统的特征值增量，我们有一个

闭合解。假设广义的特征值为Ax＝λBx，其中

都是对称矩阵。对应参数的增量变化Δλ可按照下式计算得到：

如果对于更新

那么对于广义特征值系统

Lu＝λDu的Δλ可描述如下：

对上式进行正则化可得到：

(2)计算特征向量增量Δu：使用一种近似方法来快速计算Δu，以便将其应用于

机器人手臂的构形空间中。首先，相对于Δw_ij，L的增量可以如下表示：

根据分析，角度矩阵D的增量如下：

ΔD＝Δw_ijdiag{z_ij} (6)

因此，Δu可根据下式计算得到：

Δq＝(J^TJ)^-1J^Th (7)

其中J＝L-λD，h＝(λΔD+ΔλD-ΔL)q。

(3)利用Δλ和Δq更新谱图模型：谱图模型的更新函数本质上是广义特征值系统的动态调优机制。通过交替地不断重复这个调优过程，我们可以更新完善Δλ和Δq。谱图模型将会在这个过程中不断被更新，直到Δλ和Δq在n次迭代后不再发生大的变化后停止

5.基于SGMs引导的偏置性采样策略

由于当前的采样运动规划算法采样域几乎都是通过随机的方式产生，但这样的方式会极大地降低树或图的探索和扩展，从而大幅降低实际规划效率。因此，本实施例使用学习到的谱图模型来作为采样域，从而偏置性的引导采样运动规划算法的树或图扩展过程在非障碍物区域的构型空间展开，从而大幅减少碰撞检测所消耗的时间，进一步提高规划效率。例如，对于采样规划算法的树扩展过程，随机样本可以通过谱图模型表征的分布生成，计算公式如下：

其中π_m是从m个组件生成一个新样本的概率值；λμ_2n作为一个超正则项，被用来阻止过拟合。

随机样本的选择规则依赖于所学习的谱图模型，但并不意味着随机样本必须在无碰撞区域。实际上，该操作只是表明树有明显地向无碰撞区域延伸的趋势。由于该方法大大提高了选择无碰撞样本的概率，可以有效地减少碰撞检测。因此，我们可以很容易地获得一个属于无碰撞空间的目标区域的候选路径。正则化的目的是减少在谱图空间外寻找更有效轨迹的可能性。当根据上述规则选择新样本时，有一个问题需要注意：如果仅根据学习的谱图模型选择新样本，则选择规则可能过拟合。不同的起始状态和目标区域可能导致效率低下。此外，在非均匀策略的基础上，通过在线学习和更新机制，选择样本对SGMs进行细化和改进。采用这种方法，可以使树拓展避开障碍物时更自然、更快速地伸展。

综上所述，本实施例提出的基于谱图模型(Spectral graph models，SGMs)引导的采样运动规划方法通过谱图聚类来表征学习笛卡尔空间的非障碍物区域在机器人构型空间中的映射分布，并将其作为采样规划算法的样本生成器来生成采样规划所需要的样本，用以代替传统的随机采样生成源，引导规划探索过程在非障碍物区域展开，减少规划时间，提高规划路径质量和成功率。同时，可以利用谱图模型的在线学习与更新机制对已训练的谱图模型进行在线调整，以应对环境的缓慢变化。

本实施例具有如下优点：

一、缩短复杂障碍物场景的机器人采样运动规划时间。

二、提高复杂障碍物场景的机器人采样运动规划的实际路径质量。

三、有效提高机器人运动规划的成功率。

四、对缓慢变化的动态场景具有较好的适应能力。

最后，我们创建了6DOFs平面机械臂和6DOFs三维机械臂对所提方法进行验证，并将其应用于三维6DOFs机械臂的缓慢动态变化场景，实验结果如下所示。

实验1:6维平面机械臂运动规划仿真环境

由表1可看出，与标准的采样规划算法RRT和RRT*相比，基于谱图模型(SGMs)的学习方法的规划时间平均分别减少了28.70％和18.91％；路径质量则分别提高7.90％和13.6％；随着迭代次数的增加，基于GSMs的方法成功率也比标准RRT和标准RRT*有了较为明显的提高。这表明，所提出的SGMs引导方法可应用于基本的采样规划算法，有效提高基准算法的规划效率，具有较好的广义适用性。此外，与基于GMMs的方法相比，基于SGMs的学习方法在运行时间和路径质量方面表现更好。例如，与GMMs-RRT和GMMs-RRT*方法相比，基于SGMs引导的方法在时间消耗上分别减少了9.63％和11.5％；在生成路径质量上，SGMs-RRT*比GMMs-RRT*平均提高了3.34％；在成功率上，基于SGMs的方法比基于GMMs的方法能在更短的时间内生成符合要求的规划路径，成功率更高。

表1实验结果对比

实验2:三维缓慢变化场景

本实施例设计了缓慢变化的三维复杂场景以验证所提方法对动态环境的适应能力，实验场景如图4所示。初始时，6个障碍物均处于静态，根据过往成功规划数据、示教等学习构型空间非障碍物区域的初始谱图模型。当这6个障碍物中间的某一个发生缓慢变化时，谱图模型将根据在线学习与更新机制进行快速调整以适应变化后的场景。具体的实验结果如图5a、5b、5c、5d所示。由图5a、5b、5c、5d可知，当环境发生缓慢变化后，即将于谱图模型引导的运动规划策略可快速根据环境改变对自身模型进行更新修正，保证规划效率。

综上所述，基于谱图模型的方法具有更为优异的表征学习能力，在不同算法上展现了良好的通用性，能有效地提高采样规划算法在复杂场景中的规划效率。与此同时，该方法对于缓慢变化的动态场景具有较好的适应能力。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims

1.一种基于谱图模型引导的机器人采样运动规划方法，其特征在于包括如下步骤：

S1、通过谱图聚类来表征学习笛卡尔空间的非障碍物区域在机器人构型空间中的映射分布，在特征空间中通过样本训练相关的谱图模型；

S2、以训练好的谱图模型为标准采样规划算法提供带有引导性策略的样本生成器，生成采样规划所需要的样本；

S3、引导规划探索过程在构型空间的非障碍物区域展开。

2.如权利要求1所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，还包括步骤S4：利用谱图模型的在线学习与更新机制对已训练的谱图模型进行在线调整，以应对环境的缓慢变化。

3.如权利要求1所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，步骤S1中，在特征空间中通过样本训练相关的谱图模型包括如下步骤：把过往成功的演示或规划数据生成的样本映射到2n维特征空间；利用谱聚类算法学习基于谱图模型的2n维特征空间表示模型。

4.如权利要求3所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，使用两个谱图模型分别学习采样分布：一个学习碰撞区域的构型分布，另一个学习无碰撞区域的构型分布。

5.如权利要求4所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，通过学习到的分布，利用M距离计算新样本与谱图模型生成的分布之间的距离。

6.如权利要求1所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，所述训练为离线训练；所述离线训练部分包括：首先将根据过往演示或成功的规划经验生成样本，并将其映射到新的特征空间，接着利用增量谱聚类算法进行训练，对非障碍物区域的构型空间分布进行表征学习并获得已训练的谱图模型。

7.如权利要求1所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，所述规则为在线规划，所述在线规划包好如下步骤：首先利用已训练的谱图模型作为采样规划方法的样本生成器以代替自身的随机采样，从而形成基于SGMs引导的偏置性样本生成策略，接着运行不包含碰撞检测的采样规划算法如RRT、PRM并生成候选路径；如果发现环境发生微小缓慢的变化，则在变化周围重新生成样本，并利用在线学习与更新机制对谱图模型进行在线更新。

8.如权利要求1所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，构型空间是由正余弦构成，采用曼哈顿距离作为作为两个构型之间的距离指标；；特征向量的生成是使用规范化的拉普拉斯矩阵来实现的。

9.如权利要求1所述的基于谱图模型引导的机器人采样运动规划方法，其特征在于，使用学习到的谱图模型来作为采样域，从而偏置性的引导采样运动规划算法的树或图扩展过程在非障碍物区域的构型空间展开。

10.一种基于谱图模型引导的机器人采样运动规划系统，包括处理器和存储器，所述存储器中存储有计算机程序，所述计算机程序可被处理器执行以实现如权利要求1-9中任一项所述的方法。