CN109940614B - 一种融合记忆机制的机械臂多场景快速运动规划方法 - Google Patents

Abstract

本发明属于机器人操控领域，提出一种融合记忆机制的机械臂多场景快速运动规划方法。通过模拟人类的记忆机制，在建立高斯混合模型的过程中将当前场景及对应的GMM模型通过感觉记忆、短期记忆和长期记忆三个阶段存储到记忆空间，让机器人能够记住出现过的环境。当机器人遇到类似环境时，便能够根据记忆适应当前场景，快速完成运动规划。该方法使用模型进行引导机械臂在关节空间采样并代替传统的FCL方法进行碰撞检测，与RRT*算法相比，有效提高了算法的规划成功率，减少了算法的运行时间。机器人能够通过记忆机制记住所遇到的场景及其对应模型，并在遇到相似场景时通过提取对应的场景模型快速的适应环境，不需要重新训练学习，提高了规划算法的适应能力。

Description

一种融合记忆机制的机械臂多场景快速运动规划方法

技术领域

本发明属于机器人操控领域，尤其涉及一种融合记忆机制的机械臂多场景快速运动规划方法。

背景技术

多自由度串联机械臂的运动规划是机器人操控领域的重要研究方向之一。机械臂的运动规划是指机械臂在有障碍的环境中，能够通过一种寻径算法找到一条从起始点到目标点的无碰撞路径。当前的运动规划算法有基于图搜索的方法如A*、D*和Dijkstra等启发式算法，基于人工势场的运动规划算法等，通常需要对环境进行严格的数学建模。随着机械臂自由度的增加，容易导致计算量呈指数增长甚至会导致算法失效，无法规划出合理的路径。

为了解决高维空间中的运动规划问题，可以采用RRT算法，在机械臂的关节空间通过扩展随机树的方式生成路径。它是一种基于随机采样的搜索方法，能够快速地完成在高维空间中的搜索，同时在采样过程中不依赖于精确的环境模型。RRT算法的本质是使用随机的数据结构寻找一条可行路径，但通常找到的不是最优路径。RRT算法可以解决高维空间机器人的运动规划问题，但是当机器人需要穿越狭窄通道才能到达目标点的时候，往往需要更稠密的采样才能找到一条路径。碰撞检测和最近邻搜索过程是RRT算法主要的性能瓶颈。在机器人运动规划过程中，RRT算法需要进行多次随机采样，每一次采样都需要进行碰撞检测，为此消耗的时间占据算法运行总时间的90％以上。随着工作场景复杂度增加、采样点增多，RRT算法的效率将持续降低。为了减少在采样过程中碰撞检测所需时间，可以采用延迟碰撞检测算法，在采样过程中不进行碰撞检测，直到生成完整路径时再进行碰撞检测，从而能够花费更短的时间生成一条无碰撞的路径。然而，一旦在规划过程中检测到机器人与环境发生碰撞，这种方法需要重新规划路径，在复杂场景中算法效率反而会降低。另一种方法是基于障碍物信息进行优化，将临近障碍物信息纳入节点并且使用这些信息来避免将一些节点添加到扩展树中，不仅减少了很多无效的采样点，而且减少了相关的碰撞检测和最近邻搜索等耗时操作。

近年来，基于机器学习的方法也被应用于RRT算法以提高算法的效率。例如，基于概率的碰撞检测算法，引入支持向量机(SVM)算法应对环境中的不确定性；基于学习的运动规划模型，在RRT算法中引入高斯过程，通过预测障碍物轨迹动态地规划路径，在动态约束环境中找到一条安全无碰撞的路径。此外，基于高斯混合模型(GMM,Gauss MixtureModels)的高维构型空间碰撞检测算法也可以应用到RRT*算法之中，使用增量期望最大化算法学习基于环境的高斯混合模型，代替传统方法进行碰撞检测，从而有效地减少碰撞检测的次数，提高RRT*算法的效率。然而，由于高斯混合模型是针对具体的环境训练获得，当环境发生变化时需要对新的环境重新开始训练，即使遇到重复场景也需要重新训练，时间成本较高。

人类能在一定的时间内记住所遇到的环境，这些环境信息则是做出相应决策的依据。根据现代认知学理论，记忆机制可以分为感觉记忆、短期记忆和长期记忆三个阶段，也称为长短时记忆机制。长时记忆是一个庞大而复杂的信息数据库，存储着主体过去学习的经验和知识；其优点是存储海量信息，让记忆主体可以适应各种各样复杂的环境。

发明内容

本发明针对基于GMM模型的RRT*算法仅适用于单一固定场景的局限性，引入记忆机制，提出了一种融合长短时记忆机制的快速运动规划算法用来解决多场景下的快速运动规划问题。通过模拟人类的记忆机制，在建立高斯混合模型的过程中将当前场景及对应的GMM模型通过感觉记忆、短期记忆和长期记忆三个阶段存储到记忆空间，让机器人能够记住出现过的环境。当机器人遇到类似环境时，便能够根据记忆适应当前场景，快速完成运动规划。

具体技术方案如下：

一种融合记忆机制的机械臂多场景快速运动规划方法，包括步骤如下：

(1)在机器人初始化过程中，通过RGB-D相机获取当前环境信息，建立基于机器人坐标系的环境描述E。E包含彩色图、深度图以及点云。如果使用rgb表示彩色图，depth表示深度图，pcl表示点云数据。那么E定义为：

E＝{rgb,depth,pcl} (6)

(2)在场景匹配步骤中，将环境信息E依次与短时记忆空间和长时记忆空间中的场景进行匹配并计算相似度。首先使用词袋法在记忆空间中搜索匹配场景，然后对当前场景与匹配场景进行ICP配准，计算两个场景之间的位姿变换。最后，将位姿变换与预先设定好的阈值T进行比较，若变换大于阈值，则认为不匹配，否则认为是匹配场景。

(3)提取长时记忆空间到短时记忆空间的过程如图2(a)所示，先从长时记忆空间中提取匹配模型T_i到短时记忆空间，然后将短时记忆空间中最后一个环境模型T_K存储到长时记忆空间中。图中灰色圆点表示可记忆，黑色五角星表示记忆权重，五角星不同的灰度表示权重的大小，颜色越淡，代表记忆权重越小。

(4)如果短时和长时记忆空间中都不存在与环境相匹配的模型，说明机器人在过去的时间里没有遇到类似的场景。采用GMM-RRT*算法对感觉记忆空间中新环境进行学习与训练，得到新环境的模型T_i，同时使用新环境训练并更新BOW词典。

(5)当模型T_K是不可记忆模型时，提取匹配模型T_i的同时直接遗忘T_K。如图2(b)所示。

(6)模型T_K存储到长时记忆空间中的流程如图2(c)所示。从短时记忆空间中提取出模型T_K，如果长时记忆空间未满，将T_K存储直接存储至模型队列。如果空间已满，就使用竞争机制，通过比较模型T_K与长时记忆空间中模型的记忆权重决定是否存储T_K。设T_j表示长时记忆空间中权重最小的模型，α_jmin表示T_j的权重，α_K表示模型T_K的权重。如果α_K>α_jmin,就保留模型T_K，遗忘模型T_j。否则，直接遗忘模型T_K。

GMM-RRT*算法是一种结合了高斯混合模型的RRT*算法，它以模型检测的方式代替了传统的FCL碰撞检测算法。算法的基本流程如下：

首先，通过传统RRT*算法获取机械臂n维关节空间的碰撞区域与非碰撞区域样本，通过增量期望最大值聚类(EM)算法学习如公式(1)所示的高斯混合模型的参数，其中(μ_k,S_k,π_k)表示GMM模型中第k个组元的均值、协方差矩阵以及权重。

其次，在RRT算法执行过程中，当产生新的采样点时，通过公式(2)计算采样点与高斯混合模型各个组元之间的Mahalanobis距离判断采样点是否与周围环境中的障碍物发生碰撞。

最后，由于基于概率的方法具有不确定性，对规划后的路径进行完整碰撞检测，并根据检测结果更新高斯混合模型。相应的伪代码如算法1所示，其中X_col,X_free是规划过程中采集的样本，G_col,G_free则是使用样本训练得到的高斯混合模型，T是随机扩展树，q_rand、q_near、q_new分别是随机采样点、最近邻点和新扩展点，d_col和d_free分别是新扩展点到碰撞区域和非碰撞区域的的距离，X_pathset是生成的路径。

算法1.GMM-RRT*算法

本发明是改进的GMM-RRT*算法，GMM-RRT*算法的优势是针对固定的场景，可以使用GMM模型替代FCL方法进行碰撞检测，然而训练得到的高斯混合模型仅能适应当前环境，一旦环境发生了变化就需要重新训练，会降低算法的效率。若机器人能够记忆遇到的环境及对应的GMM模型，当其遇到类似环境时只需要从记忆中提取与环境相匹配的模型，不需要重新训练便能适应新的环境，算法效率将得到提升。为了能够存储和调用基于场景的高斯混合模型，本发明引入长短时记忆机制，通过模拟人类的记忆机制对模型进行记忆、匹配和提取，在存储模型的同时使用词袋模型算法构建词典，用于场景图像检索与匹配。将存储场景及对应模型的空间分为三部分，即：感觉记忆空间，短时记忆空间和长时记忆空间。

感觉记忆空间(USTMS)存储机器人对于当前环境的感知信息，包括在仿真环境中障碍物的大小位置或者通过视觉传感器获取的图像和点云信息等。

将存储在USTMS中的感知信息经过训练得到的高斯混合模型存储在短时记忆空间(STMS)中，用STMS模拟人类的短时记忆。对于高斯混合模型，STMS中存储环境信息及对应GMM的K个组元的集合。用E_j表示第j个环境信息，N_ji表示记忆空间中用于描述环境E_j对应的GMM第i个组元，M表示STMS空间存储的最大容量，则STMS可以表示为：

STMS＝{T_j＝(E_j,N_ji,i＝1,2...K)|j＝1,2...M} (3)

短时记忆空间中的模型是RRT*算法做出决策与规划的主要依据。短时记忆空间中的模型不仅包括由感觉记忆空间中的场景学习得到的新的高斯混合模型，还包括短时记忆空间中已有且被更新的高斯混合模型和从长时记忆空间中调用的高斯混合模型。

在短时记忆空间中的模型设置参数β，作为模型可记忆性的衡量，在记忆过程中通过β的大小确定该模型是否需要被记忆。参数β与模型的质量有关，在一般情况下，RRT*算法迭代次数越多，训练得到的模型越精确，碰撞检测出错的几率越低，模型被存储的可能性也越大。具体地，对于

j＝1,...M，设置初始β_j＝0，β_j的更新公式为

β_j＝s_t/t_t*100 (4)

s_t和t_t分别为算法在当前场景中规划成功的次数和规划的总次数。若β_j大于阈值β_t，则模型T_j就是可记忆模型。根据经验，阈值β_t＝85％时可以取得较好的实验效果。

长时记忆空间(LTMS)存储机器人在过去时间中遇到的所有的可记忆模型及对应的环境信息，并通过一些策略维护和更新记忆。LTMS空间集合表示为

LTMS＝{T_j＝(E_j,N_ji,i＝1,2...K)|j＝1,2...Q} (5)

其中，Q表示LTMS空间的最大容量。

图1描述了场景模型的存储和提取过程。感觉记忆空间中存储环境信息，使用算法训练得到的高斯混合模型存储于短时记忆空间，当使用模型规划路径用时短且成功率较高时，将模型存储于长时记忆空间。在遇到相似环境时提取记忆中的匹配模型，通过短暂的学习阶段快速的适应环境，提高算法在不同环境中的适应能力。

本发明在高斯混合模型中引入了记忆机制，提出了一种基于记忆高斯混合模型的RRT*算法(MGMM-RRT*)。算法在运动规划过程中基于环境训练高斯混合模型，使用模型进行引导机械臂在关节空间采样并代替传统的FCL方法进行碰撞检测，与RRT*算法相比，有效提高了算法的规划成功率，减少了算法的运行时间。机器人能够通过记忆机制记住所遇到的场景及其对应模型，并在遇到相似场景时通过提取对应的场景模型快速的适应环境，不需要重新训练学习，提高了规划算法的适应能力。

附图说明

图1三段式记忆存储机制示意图。

图2模型存储、提取和遗忘的示意图；(a)模型存储和提取示意图；(b)模型提取和遗忘示意图；(c)模型竞争存储示意图。

图3Matlab实验场景设置。

图4可记忆权重阈值β_t。

图5运动规划实验结果。

图6实验结果对比。

具体实施方式

为了验证算法的有效性，本发明分别在Matlab和ROS环境下针对机械臂运动规划开展了研究。为了比较算法在不同障碍物场景中的性能，分别按照障碍物形状、障碍物数量的多少在Matlab环境中设置了40组不同的实验场景，进行平面机械臂的运动规划实验。图3为4种典型的场景。在每组实验场景中，1代表机械臂，2代表目标物体，其他黑色代表障碍物。将本发明MGMM-RRT*算法应用到所有的Matlab实验场景中进行运动规划，并分别与RRT*算法和GMM-RRT*算法的规划结果进行比较。最后，在ROS环境中进行了Kinova六自由度机械臂仿真实验，进一步评估算法的性能。

(1)MGMM参数

对于长短时记忆机制，可记忆权重β忆是非常重要的参数。通过实验分析的方法确定最佳的权重，在实验过程中分别设置不同的权重，并对比在不同权重下的场景模型的可存储数量和算法平均运行时间，可存储数量指在当前记忆权重阈值β_t的情况下，短时记忆空间中可以被存储到长时记忆空间中的数量。平均运行时间指算法在每种场景下规划时间的平均值。在Matlab环境下的实验结果如表1和图4所示。

表1可记忆权重阈值β_t

通过对比不难发现，随着阈值β_t的减少，场景模型的可存储数量越多，但是算法运行时间所需收敛次数越多。β_t小于85％时，收敛次数发生较大变化。因此，选择85％作为权重阈值β_t。

(2)Matlab仿真实验

本发明将MGMM-RRT*算法分别与RRT*和GMM-RRT*算法进行了对比。图5展示了分别在四种场景下使用MGMM-RRT*运动规划算法得到的结果，可以看出，在每个场景中算法都能规划出正确的路径，使机械臂绕过障碍到达目标点。然后在同样场景下分别使用RRT*和GMM-RRT*进行运动规划，并对规划结果进行统计。

由于RRT类算法具有随机性，本发明分别在每种场景下进行了1000次重复试验，然后给出平均结果。表2统计了本发明算法与其它两种算法在40种实验场景下的平均数据对比，图6展示了其中四种场景的实验数据。从图6(a)～(c)中可以看出，本发明算法比其他两个算法运行时间短、所需的迭代次数少，图6(d)展示了MGMM-RRT*与GMM-RRT*实验对比结果。从表2与图6可以看出，MGMM-RRT*的优势在于当遇到类似场景时不需要训练模型，可以通过提取记忆中的模型快速的适应环境，减少了算法运行时间，提高了算法的成功率，增强了算法适应能力。

表2 40组实验场景数据对比

(3)ROS仿真实验

本发明在ROS环境下，以Kinova机械臂为研究对象进行了三维仿真实验。针对现有算法在狭窄通道中路路径规划成功率较低的情况，设计了100组不同的实验场景。

由于在三维环境下运动规划的复杂程度更高，基于随机采样的运动规划算法有一定的失败几率，算法规划成功率更有参考意义。本发明对比了RRT*，GMM-RRT*以及MGMM-RRT*在四种实验场景下的平均规划时间和规划成功率。在匹配过程中，先通过RGB图像进行词典匹配，再利用点云对匹配场景进行ICP配准计算3D位姿变换。其中，耗时较长的ICP配准过程仅需执行一次。实验数据表明，100组场景所需匹配时间小于100ms。

表3展示了三种算法在各个场景下的规划成功率。RRT*、GMM-RTT*和MGMM-RRT*这三种算法的平均运行时间分别是23.3s、16.7s和9.8s，相应的规划成功率依次为74.7％、75.3％和87％。可以看出，由于本发明算法使用GMM模型并加入了记忆机制，相比RRT*和GMM-RRT*算法，规划成功率提高了16％，规划时间减少了41％。

表3算法规划成功率

Claims (3)

1.一种融合记忆机制的机械臂多场景快速运动规划方法，其特征在于，包括如下步骤：

(1)在机器人初始化过程中，通过RGB-D相机获取当前环境信息，存储于感觉记忆空间，建立基于机器人坐标系的环境描述E；E包含彩色图、深度图以及点云；使用rgb表示彩色图，depth表示深度图，pcl表示点云数据，E定义为：

E＝{rgb,depth,pcl} (6)

(2)场景匹配，将环境信息E依次与短时记忆空间和长时记忆空间中的场景进行匹配并计算相似度；首先使用词袋法在记忆空间中搜索匹配场景，然后对当前场景与匹配场景进行ICP配准，计算两个场景之间的位姿变换；最后，将位姿变换与预先设定好的阈值T进行比较，若变换大于阈值，则认为不匹配，否则认为是匹配场景；

(3)提取长时记忆空间到短时记忆空间的过程：先从长时记忆空间中提取匹配模型T_i到短时记忆空间，然后将短时记忆空间中最后一个环境模型T_K存储到长时记忆空间中；

(4)如果短时和长时记忆空间中都不存在与环境相匹配的模型，说明机器人在过去的时间里没有遇到类似的场景；采用GMM-RRT*算法对感觉记忆空间中新环境进行学习与训练，得到新环境的模型T_i，同时使用新环境训练并更新BOW词典；

(5)当模型T_K是不可记忆模型时，提取匹配模型T_i的同时直接遗忘T_K；(6)模型T_K存储到长时记忆空间中的流程：从短时记忆空间中提取出模型T_K，如果长时记忆空间未满，将T_K存储直接存储至模型队列；如果空间已满，使用竞争机制，通过比较模型T_K与长时记忆空间中模型的记忆权重决定是否存储T_K；设T_j表示长时记忆空间中权重最小的模型，α_jmin表示T_j的权重，α_K表示模型T_K的权重；如果α_K>α_jmin，保留模型T_K，遗忘模型T_j；否则，直接遗忘模型T_K。

2.根据权利要求1所述的融合记忆机制的机械臂多场景快速运动规划方法，其特征在于，短时记忆空间(STMS)中的模型包括由感觉记忆空间中的场景学习得到的新的高斯混合模型、短时记忆空间中已有且被更新的高斯混合模型和从长时记忆空间中调用的高斯混合模型；

对于高斯混合模型，STMS中存储环境信息及对应GMM的K个组元的集合；E_j表示第j个环境信息，N_ji表示记忆空间中用于描述环境E_j对应的GMM第i个组元，M表示STMS空间存储的最大容量，则STMS表示为：

STMS＝{T_j＝(E_j,N_ji,i＝1,2...K)|j＝1,2...M} (3)

在短时记忆空间中的模型设置参数β，作为模型可记忆性的衡量，在记忆过程中通过β的大小确定该模型是否需要被记忆；参数β与模型的质量有关，对于

j＝1,...M，设置初始β_j＝0，β_j的更新公式为

β_j＝s_t/t_t*100 (4)

s_t和t_t分别为算法在当前场景中规划成功的次数和规划的总次数；若β_j大于阈值β_t，则模型T_j就是可记忆模型。

3.根据权利要求2所述的融合记忆机制的机械臂多场景快速运动规划方法，其特征在于，长时记忆空间(LTMS)存储机器人在过去时间中遇到的所有的可记忆模型及对应的环境信息，并通过策略维护和更新记忆；LTMS空间集合表示为

LTMS＝{T_j＝(E_j,N_ji,i＝1,2...K)|j＝1,2...Q} (5)

其中，Q表示LTMS空间的最大容量。

Images