CN113485373B - 一种基于高斯混合模型的机器人实时运动规划方法 - Google Patents

Abstract

本发明公开了一种基于高斯混合模型的机器人实时运动规划方法，包括：S1、提供一张环境地图，使用高斯混合模型获取环境地图中的环境结构特征，并实时提取机器人的节点位姿信息；S2、使用角速度通道和距离融合对机器人节点进行特征初步提取，获得初始特征点集，并更新特征地图；S3、再次使用距离融合对初始特征点集进行进一步提取，获得最终特征点集；S4、使用欧式距离对环境地图更新，得到最终特征地图，完成实时特征提取；S5、在所述最终特征地图中找到机器人起点和终点对应的特征编号，根据所述最终特征点集得到特征矩阵和行人矩阵，并形成特征点的特征搜索树，输出启发式路径。本发明具有导航成功率高、效率高、占用资源少的优点。

Description

一种基于高斯混合模型的机器人实时运动规划方法

技术领域

本发明涉及机器人运动规划技术领域，特别涉及一种基于高斯混合模型的机器人实时运动规划方法。

背景技术

人工智能技术的突破给移动型服务机器人研究带来了巨大的机遇，目前，引导机器人、扫地机器人、导购机器人、货物搬运机器人等移动型服务机器人已经成功应用到了机场、超市、博物馆、家庭等多种环境。移动型机器人路径规划是指在无人干预的条件下，在给定的初始和目标点之间找到一条无碰撞且满足规定的各种约束的路径。当前，主要的路径规划算法可以大致分为四种：基于栅格的路径规划算法、基于人工势场的路径规划算法、基于奖赏的路径规划算法和基于随机采样的运动规划算法。在这四类算法中，基于随机采样的路径规划算法计算量小、避障灵活并且其未对环境建模，极大地减少了规划时间和内存成本，保障了路径规划的实时性，更适用于解决动态环境的路径规划问题。然而，在动态人机共融环境，如包含狭窄的走廊、人群密集等场景中，随机采样的路径规划算法忽略对环境结构信息以及人群密度的考量，仅仅把起点到终点的路径长度作为规划成本，这就不可避免机器人会选择在路径较短但可行空间较小或行人密度较大的区域内运动规划，由于可行区域较小，人群运动的不确定性，机器人路径规划的成功率大大降低，导致路径的规划时间变长甚至规划失败。如何在动态人机共融环境中高效快速地找到最优运动轨迹是移动机器人要研究的重要问题之一。同时，机器人没有被赋予“大脑记忆”的概念，同一场景机器人走过无数次，下次再走还是要重新规划路径，没能实现采样节点的复用，浪费计算资源。因此，研究快速找出可行区域的候选节点是优化基于随机采样的路径规划算法的关键，对于提高移动机器人的智能化有着重要意义。

发明内容

本发明要解决的技术问题是提供一种导航成功率高、效率高、占用资源少的基于高斯混合模型的机器人实时运动规划方法。

为了解决上述问题，本发明提供了一种基于高斯混合模型的机器人实时运动规划方法，其包括以下步骤：

S1、提供一张环境地图，使用高斯混合模型获取环境地图中的环境结构特征，并实时提取机器人的节点位姿信息；其中，所述环境地图为二维栅格地图；

S2、使用角速度通道和距离融合对机器人节点进行特征初步提取，获得初始特征点集，并更新特征地图；

S3、再次使用距离融合对初始特征点集进行进一步提取，获得最终特征点集；

S4、使用欧式距离对环境地图更新，得到最终特征地图，完成实时特征提取；

S5、在所述最终特征地图中找到机器人起点和终点对应的特征编号，根据所述最终特征点集得到特征矩阵和行人矩阵，并形成特征点的特征搜索树，输出启发式路径。

作为本发明的进一步改进，步骤S1包括：

S11、提供一张环境地图，在环境地图的无障碍物区域内随机撒点，得到训练集；

S12、设置协方差矩阵类型为“full”，高斯组件个数为k，对所述训练集进行高斯混合模型聚类处理，并不断迭代得到环境地图的k个聚类结果，其中，中心均值点x、y坐标存入mean.txt文档中，其相对应的协方差矩阵存入covariance.txt文档中，将k个组件的中心点作为特征点，存进特征点集G_FP中，在此基础上去实时提取机器人的位姿信息；

S13、在ROS***下，每次打开终端时均判断存放特征点位置信息的fp.txt文件是否存在；

若不存在fp.txt文件，则判断mean.txt及covariance.txt是否存在，若存在，将mean.txt中的k个组件的中心点作为原始特征点，存进特征点集G_FP中，在此基础上再去实时提取机器人的位姿信息，并将covariance.txt中k行4列的协方差矩阵存进covariance数组中，并令标志位update_fp_covariance等于1，执行步骤S15；若不存在mean.txt及covariance.txt，执行步骤S12；

若存在fp.txt文件，则将fp.txt文件里存储的节点G_new＝(x_n，y_n)存进特征点集中，并将fp_covariance.txt文件里存储的k行4列的协方差矩阵存进covariance数组中，令标志位update_fp_covariance等于0，执行步骤S14；

S14、每次打开终端时判断feature_map.txt及feature_matrix.txt文件是否存在；若不存在，说明表征度不足，则令representative等于0；若存在，说明表征度完备，则令representative等于1，执行步骤S4；

S15、给定导航目标，机器人基于Risk-RRT算法开始路径规划，实时获取机器人在路径规划过程中的节点位姿信息。

作为本发明的进一步改进，步骤S15包括：

通过订阅/amcl_pose和/robot_0/odom话题实时获取机器人在路径规划过程中的节点位姿信息，记作G_m＝(x_m，y_m，ω_m)，其中，x、y表示节点的位置，ω表示节点的角速度大小，提取角速度大于0.2rad/s(人为设定的角速度阈值)的节点，每隔一定间隔提取节点，记作G_new＝(x_n，y_n)并作为/pose_velocity话题发布。

作为本发明的进一步改进，步骤S2包括：

S21、订阅/pose_velocity话题，获取角速度大于0.2rad/s的节点G_new＝(x_n，y_n)；

S22、判断表征度representative是否等于0，如果是，执行步骤S23；如果不是，则根据最优平均距离替换原有特征点，执行步骤S24；

S23、将节点G_new与特征点集G_FP中的所有节点G_i＝(x_i,y_i)(i＝1，2，3，……n，其中n为特征点集G_FP的个数)依次进行欧氏距离计算，记作d_once，判断d_once是否小于等于人为设置的第一次距离融合阈值，如果是，返回步骤S21；如果节点G_new与特征点集G_FP中的所有节点的距离都大于人为设置的第一次距离融合阈值，则将节点G_new添加进特征点集G_FP，并将G_FP带入步骤S3并执行；

S24、将节点G_new与特征点集G_FP中的k+1个节点G_i＝(x_i,y_i)(i＝k+1，k+2，k+3，……n)依次进行欧氏距离计算，记作d_once，判断d_once是否小于等于预设的第一次距离融合阈值，如果是，执行步骤S25，如果不是，执行步骤S5；

S25、将特征地图M_F中栅格值为i的栅格点与节点G_new依次进行碰撞检测，如果碰撞检测全部都通过且特征地图M_F中栅格值为i的栅格点与节点G_new的距离之和小于特征地图M_F中栅格值为i的栅格点与原先的特征节点的距离之和，则节点G_new替换原有特征节点G_i＝(x_i,y_i)，更新特征地图，将G_FP带入步骤S4并执行；否则，执行步骤S5。

作为本发明的进一步改进，步骤S3包括：

S31、初始化计数c为0，设置一个要保存的特征点集为G_R，将特征点集G_FP中的前k个特征点添加进特征点集G_R中；提取特征点集G_FP中的每个节点G_i＝(x_i,y_i)(i＝1，2，3，……n，其中n为特征点集G_FP的个数)的相邻特征节点集合G_NX(i)；

S32、初始化i为1；

S33、判断i是否小于等于特征点集G_FP的个数n，若是，则执行步骤S34，否则，则执行步骤S36；

S34、判断第i个节点的相邻特征节点集合G_NX(i)的个数是否小于等于预设值3，若是，则进行步骤S35；否则，则令i＝i+1并返回步骤S33；

S35、将节点G_i＝(x_i，y_i)本身及其相邻特征节点集合G_NX(i)里的节点添加进点集G_R中；判断相邻特征节点集合G_NX(i)的个数是否小于等于预设值1，若是，则计数c+1；如果i不等于特征点集G_FP的个数n则令i＝i+1并返回步骤S33，否则执行步骤S36；

S36、对点集G_R进行删重处理，删重过后如果特征点集G_FP的个数n大于等于点集G_R的个数N-1(即n≥N-1)，则更新特征地图，将特征点集G_FP带入步骤S4并执行，否则执行步骤S37；

S37、将特征点集G_FP除去点集G_R的其余所有节点添加到点集G_D中，进行第二次距离融合删除特征点，取G_D的第x个节点G_Dx(x＝1，2，……n₁-1，其中n₁为点集G_D的个数)，依次与G_D中的节点G_Dx1(x1＝x+1，x+2，……n₁)连线在地图上做碰撞检测以及欧氏距离计算，如果碰撞检测通过且距离小于人为设定的二次融合距离阈值，则从特征点集G_FP中删除节点G_Dx1；如果点集G_D中的所有点都已经检测完毕，则更新特征地图，将特征点集G_FP带入步骤S4并执行。

作为本发明的进一步改进，步骤S4包括：

S41、初始化标志位为0，用于表示地图上无障碍物区域的点是否都找到了它所对应的特征点编号，将特征点集G_FP中的所有节点坐标转换为栅格坐标，记作G_i1＝(x_i1，y_i1)(i1＝1，2，……n，其中n为特征点集G_FP的个数)；

S42、遍历二维栅格地图M_p(p＝0，1，……p1，其中p1为二维栅格地图的大小)中无障碍物区域的所有栅格点(i_k，j_k)，初始化c1为0，初始化min_dist为地图长度×地图宽度；将栅格点(i_k，j_k)依次与特征点集G_FP中的特征节点G_i1连线在地图上做碰撞检测，如果碰撞检测通过，则计算两点之间的欧氏距离dist₁，如果dist₁小于min_dist，则在特征地图M_F中，更新栅格点(i_k，j_k)的对应值为特征节点的编号i并且min_dist更新为dist₁的值；如果碰撞检测未通过，则计数c1+1；直至特征点集G_FP中的所有节点遍历结束，如果c1＝特征点集G_FP的个数n，则令标志位为1，并退出循环，返回步骤S2；如果c1≠特征点集G_FP的个数n，则重复步骤S42直至二维栅格地图M_p中无障碍物区域的所有栅格点都遍历结束，得到特征地图M_F；

S43、如果标志位等于0，则表征度完备，令representative等于1，将特征点集G_FP带入步骤S5，否则令representative等于0，返回步骤S2。

作为本发明的进一步改进，步骤S5包括：

S51、选定特征点集G_FP中的某一节点G_FPi(i＝1，2，……n，其中n为特征点集G_FP的个数)，将其余节点G_FPx(x＝1，2，……n且x≠i)与节点G_FPi的连线在地图上做碰撞检测，如果碰撞检测通过，则将两点之间的欧氏距离存入特征矩阵P_F(i，x)，否则将0存入P_F(i，x)；

S52、在机器人运动规划的全过程中，ROS始终订阅并接收行人位姿话题/robot_h/base_pose_ground_truth(其中h＝1,2……n_p，n_p为行人个数)，计算机器人实时位姿(x_r,y_r)与第h个行人实时位姿(x_ph,y_ph)的欧式距离d_h，如果d_h小于预设值3，就将此时刻的行人坐标(x_ph,y_ph)存入数组ped_xy中，如果ped_xy数组有值，则执行步骤S53，否则执行步骤S52；

S53、选取特征点集G_FP中的某一节点G_FPi(i＝1，2，……n，其中n为特征点集G_FP的个数)，将其余节点G_FPx(x＝1，2，……n且x≠i)与节点G_FPi的连线在地图上做碰撞检测，如果碰撞检测通过，则令ped_num等于0，将ped_xy数组中存储的行人坐标依次取出计算其与G_FPi及G_FPx两点连线的垂直距离d_t，如果d_t小于预设值2，则令ped_num+1，当ped_xy数组遍历完全时，将ped_num的值存入临时行人矩阵P_{temp_ped}(i，x)，否则将0存入P_{temp_ped}(i，x)，循环执行步骤S53直至遍历G_FP中的所有节点；清空ped_xy数组中的值，P_ped＝P_ped+P_{temp_ped}得到行人矩阵P_ped，执行步骤S54；

S54、初始化数组tree[i]＝n+1以及dist[i]＝M*N(其中i＝0,1,2……n，n为特征点个数，M为地图的长，N为地图的宽)，设置三个权重：ω₁，ω₂，ω₃，其中，ω₁用于两个特征点之间的欧氏距离，ω₂用于高斯混合模型的某组件的近似矩形的面积，ω₃用于两特征点之间的行人个数；

S55、在特征地图M_F中找到机器人起点位置对应的特征节点编号robot_index，并将其加入到多模态信息特征树T中，完成多模态信息特征树T的初始化，初始化开放节点集合为T_open＝T，令数组tree[robot_index-1]＝0及数组dist[robot_index-1]＝0；

S56、判断开放节点的个数是否大于零，如果是，则进行步骤S57；否则，则进行步骤S5-10；

S57、提取第一个开放节点t_open，将t_open从开放节点集合T_open中移除，在特征矩阵P_F中找到t_open与其余特征节点的欧氏距离为value1＝P_F(t_open-1，i)，如果value1>0则执行下列步骤；

A、判断开放节点t_open是否为高斯混合模型的某个组件中心点，若是，计算该组件面积为lenda1，计算协方差矩阵中第一个元素和第四个元素中的最小值为width1，判断width1是否小于等于两个成年人的舒适社交距离，如果是，则value3等于50*行人矩阵P_ped(t_open-1，i)，否则value3等于10*行人矩阵P_ped(t_open-1，i)；如果开放节点t_open不是高斯混合模型的某个组件中心点，则令lenda1＝1，value3等于10*行人矩阵P_ped(t_open-1，i)；

B、判断t_open的邻居点i是否为高斯混合模型的某个组件中心点，若是，计算该组件面积为lenda2，计算协方差矩阵中第一个元素和第四个元素中的最小值为width2，判断width2是否小于等于两个成年人的舒适社交距离，如果是，则value3等于50*行人矩阵P_ped(t_open-1，i)，否则value3等于10*行人矩阵P_ped(t_open-1，i)；如果t_open的邻居点i不是高斯混合模型的某个组件中心点，则令lenda2＝1，value3等于10*行人矩阵P_ped(t_open-1，i)；

S58、计算lenda1和lenda2中的最小值为lenda，value2设为1/lenda；

sum_value1＝100*(ω₁*value1+ω₂*value2+ω₃*value3)；

sum_value＝dist[t_open-1]+sum_value1；

S59、如果tree[i]>n，则将编号i+1加入到多模态信息特征树T和开放节点集合T_open中，更新tree[i]＝t_open，dist[i]＝sum_value，否则如果dist[i]>sum_value，则更新tree[i]＝t_open，dist[i]＝sum_value，返回步骤S56；

S5-10、在特征地图M_F中找到终点位置对应的特征节点，通过反向搜索特征树T，找到从起点到终点的路径；

S5-11、返回步骤S2。

作为本发明的进一步改进，ω₁＝0.175，ω₂＝0.175，ω₃＝0.65。

作为本发明的进一步改进，所述步骤A和B中的组件面积为协方差矩阵中第一个元素和第四个元素的乘积。

作为本发明的进一步改进，所述两个成年人的舒适社交距离为1.944。

本发明的有益效果：

本发明基于高斯混合模型的机器人实时运动规划方法通过使用高斯混合模型训练出聚类结果，将其作为特征点，该特征点不仅包含了环境的结构信息和行人密度信息，其提取过程所需的时间成本和计算资源也大大减少。同时，在路径规划的搜索过程中，通过在成本函数中加入对环境结构信息以及人群密度的考量，使机器人选择走路径较远但人群密度较小的区域，提高了导航成功率。此外，高斯混合模型的训练以及特征点的提取是实时进行的，通过txt文件保存特征点的位置信息以及对应的协方差矩阵信息，占用内存小，解决了节点复用和节约计算资源问题，提取和创建完成后，在不同起/终点的路径规划中可以重复利用，相当于赋予了机器人“大脑记忆”的功能，越用越灵。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。

附图说明

图1是本发明优选实施例中基于高斯混合模型的机器人实时运动规划方法的流程图；

图2是本发明优选实施例中高斯混合模型训练得到的特征点示意图；

图3是本发明优选实施例中通过角速度通道和距离融合提取特征点的示意图；

图4是本发明优选实施例中优化过后的启发式路径示意图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

如图1所示，为本发明优选实施例中的基于高斯混合模型的机器人实时运动规划方法，包括以下步骤：

S1、提供一张环境地图，使用高斯混合模型获取环境地图中的环境结构特征，并实时提取机器人的节点位姿信息；其中，所述环境地图为二维栅格地图；

S2、使用角速度通道和距离融合对机器人节点进行特征初步提取，获得初始特征点集，并更新特征地图；

S3、再次使用距离融合对初始特征点集进行进一步提取，获得最终特征点集；

S4、使用欧式距离对环境地图更新，得到最终特征地图，完成实时特征提取；

S5、在所述最终特征地图中找到机器人起点和终点对应的特征编号，根据所述最终特征点集得到特征矩阵和行人矩阵，并形成特征点的特征搜索树，输出启发式路径。

在一些实施例中，步骤S1具体包括：

S11、提供一张环境地图，在环境地图的无障碍物区域内随机撒点，得到训练集；

S12、设置协方差矩阵类型为“full”，高斯组件个数为k，对所述训练集进行高斯混合模型聚类处理，并不断迭代得到环境地图的k个聚类结果，其中，中心均值点x、y坐标存入mean.txt文档中，其相对应的协方差矩阵存入covariance.txt文档中，将k个组件的中心点作为特征点，存进特征点集G_FP中，在此基础上去实时提取机器人的位姿信息；(此处注意高斯混合模型的k个特征点在后续步骤中永远不会被删除、修改、调整位置)。如图2所示，图中大正方形表示高斯混合模型训练出来的聚类中心点，小正方形为机器人通过欧氏距离通道以及第一次距离融合实时得到的特征点。

S13、在ROS(Robot Operating System)***下，每次打开终端时均判断存放特征点x、y位置信息的fp.txt文件是否存在；

若不存在fp.txt文件，则判断mean.txt(存放高斯混合模型训练出来的k个聚类中心的x、y坐标信息)及covariance.txt(存放高斯混合模型训练出来的k个聚类中心的协方差矩阵)是否存在，若存在，将mean.txt中的k个组件的中心点作为原始特征点，存进特征点集中，在此基础上再去实时提取机器人的位姿信息，并将covariance.txt中k行4列的协方差矩阵存进covariance数组中，并令标志位update_fp_covariance等于1(该标志位是为了保证对于一幅地图，它的k个中心点和其对应的协方差矩阵不变)，执行步骤S15；若不存在mean.txt及covariance.txt，执行步骤S12；

若存在fp.txt文件，则将fp.txt文件里存储的节点G_new＝(x_n，y_n)存进特征点集G_FP中，并将fp_covariance.txt文件里存储的k行4列的协方差矩阵存进covariance数组中，令标志位update_fp_covariance等于0，执行步骤S14；

S14、每次打开终端时判断feature_map.txt及feature_matrix.txt文件是否存在；若不存在，说明表征度不足，则令representative等于0；若存在，说明表征度完备，则令representative等于1，执行步骤S4；

其中，representative等于地图上无障碍区域的点能找到它所对应的特征点编号的个数与无障碍区域的所有点的比值，用于表示表征度，即：

为了简便，我们把representative<1的所有情况都设置为representative＝0，只有当representative＝1说明表征度完备了，此时设置representative＝1。

S15、给定导航目标，机器人基于Risk-RRT算法开始路径规划，实时获取机器人在路径规划过程中的节点位姿信息。因为由步骤S12提取到的高斯混合模型的k个特征点还不足以完成对环境结构特征的表征，所以还需实时提取机器人的节点位姿信息加以补充。

进一步的，步骤S15包括：

通过订阅/amcl_pose和/robot_0/odom话题实时获取机器人在路径规划过程中的节点位姿信息，记作G_m＝(x_m，y_m，ω_m)，其中，x、y表示节点的位置，ω表示节点的角速度大小，提取角速度大于0.2rad/s(人为设定的角速度阈值)的节点，每隔一定间隔提取节点，记作G_new＝(x_n，y_n)并作为/pose_velocity话题发布。

在一些实施例中，步骤S2具体包括：

S21、订阅/pose_velocity话题，获取角速度大于0.2rad/s的节点G_new＝(x_n，y_n)；

S22、判断表征度representative是否等于0，如果是，执行步骤S23；如果不是，则根据最优平均距离替换原有特征点，执行步骤S24；

S23、将节点G_new与特征点集G_FP中的所有节点G_i＝(x_i,y_i)(i＝1，2，3，……n，其中n为特征点集G_FP的个数)依次进行欧氏距离计算，记作d_once，判断d_once是否小于等于人为设置的第一次距离融合阈值，如果是，返回步骤S21；如果节点G_new与特征点集G_FP中的所有节点的距离都大于人为设置的第一次距离融合阈值，则将节点G_new添加进特征点集G_FP，并将G_FP带入步骤S3并执行；

S24、将节点G_new与特征点集G_FP中的k+1个节点G_i＝(x_i,y_i)(i＝k+1，k+2，k+3，……n)依次进行欧氏距离计算，记作d_once，判断d_once是否小于等于预设的第一次距离融合阈值，如果是，执行步骤S25，如果不是，执行步骤S5；

S25、将特征地图M_F中栅格值为i的栅格点与节点G_new依次进行碰撞检测(两点之间的连线如果有障碍物就是碰撞检测不通过，否则就是通过)，公式如下：

如果碰撞检测全部都通过且特征地图M_F中栅格值为i的栅格点与节点G_new的距离之和小于特征地图M_F中栅格值为i的栅格点与原先的特征节点的距离之和，则节点G_new替换原有特征节点G_i＝(x_i,y_i)，更新特征地图，将G_FP带入步骤S4并执行；否则，执行步骤S5。

在一些实施例中，步骤S3包括：

S31、初始化计数c为0，设置一个要保存的特征点集为G_R，将特征点集G_FP中的前k个特征点添加进特征点集G_R中；提取特征点集G_FP中的每个节点G_i＝(x_i,y_i)(i＝1，2，3，……n，其中n为特征点集G_FP的个数)的相邻特征节点集合G_NX(i)；

S32、初始化i为1；

S33、判断i是否小于等于特征点集G_FP的个数n，若是，则执行步骤S34，否则，则执行步骤S36；

S34、判断第i个节点的相邻特征节点集合G_NX(i)的个数是否小于等于预设值3，若是，则进行步骤S35；否则，则令i＝i+1并返回步骤S33；

S35、将节点G_i＝(x_i，y_i)本身及其相邻特征节点集合G_NX(i)里的节点添加进点集G_R中；判断相邻特征节点集合G_NX(i)的个数是否小于等于预设值1，若是，则计数c+1；如果i不等于特征点集G_FP的个数n则令i＝i+1并返回步骤S33，否则执行步骤S36；

S36、对点集G_R进行删重处理，删重过后如果特征点集G_FP的个数n大于等于点集G_R的个数N-1(即n≥N-1)，则更新特征地图，将特征点集G_FP带入步骤S4并执行，否则执行步骤S37；

S37、将特征点集G_FP除去点集G_R的其余所有节点添加到点集G_D中，进行第二次距离融合删除特征点，取G_D的第x个节点G_Dx(x＝1，2，……n₁-1，其中n₁为点集G_D的个数)，依次与G_D中的节点G_Dx1(x1＝x+1，x+2，……n₁)连线在地图上做碰撞检测以及欧氏距离计算，如果碰撞检测通过且距离小于人为设定的二次融合距离阈值，则从特征点集G_FP中删除节点G_Dx1；如果点集G_D中的所有点都已经检测完毕，则更新特征地图，将特征点集G_FP带入步骤S4并执行。如图3所示，r1表示第一次距离融合的阈值(50)，r2表示第二次距离融合的阈值(80)，叉号表示被二次距离融合所删除的冗余特征点，实心圆表示最终提取的特征点，矩形表示障碍物区域。

在一些实施例中，步骤S4具体包括：

S41、初始化标志位为0，用于表示地图上无障碍物区域的点是否都找到了它所对应的特征点编号，将特征点集G_FP中的所有节点坐标转换为栅格坐标，记作G_i1＝(x_i1，y_i1)(i1＝1，2，……n，其中n为特征点集G_FP的个数)；转化公式如下：

即：栅格坐标I＝(x坐标-地图的原点x坐标)/地图分辨率

即：栅格坐标J＝(y坐标-地图的原点y坐标)/地图分辨率

S42、遍历二维栅格地图M_p(p＝0，1，……p1，其中p1为二维栅格地图的大小)中无障碍物区域的所有栅格点(i_k，j_k)，初始化c1为0，初始化min_dist为地图长度×地图宽度；将栅格点(i_k，j_k)依次与特征点集G_FP中的特征节点G_i1连线在地图上做碰撞检测，如果碰撞检测通过，则计算两点之间的欧氏距离dist₁，如果dist₁小于min_dist，则在特征地图M_F中，更新栅格点(i_k，j_k)的对应值为特征节点的编号i并且min_dist更新为dist₁的值；如果碰撞检测未通过，则计数c1+1；直至特征点集G_FP中的所有节点遍历结束，如果c1＝特征点集G_FP的个数n，则令标志位为1，并退出循环，返回步骤S2；如果c1≠特征点集G_FP的个数n，则重复步骤S42直至二维栅格地图M_p中无障碍物区域的所有栅格点都遍历结束，得到特征地图M_F；

S43、如果标志位等于0，则表征度完备，令representative等于1，将特征点集G_FP带入步骤S5，否则令representative等于0，返回步骤S2。

在一些实施例中，步骤S5具体包括：

S51、选定特征点集G_FP中的某一节点G_FPi(i＝1，2，……n，其中n为特征点集G_FP的个数)，将其余节点G_FPx(x＝1，2，……n且x≠i)与节点G_FPi的连线在地图上做碰撞检测，如果碰撞检测通过，则将两点之间的欧氏距离存入特征矩阵P_F(i，x)，否则将0存入P_F(i，x)；

S52、在机器人运动规划的全过程中，ROS始终订阅并接收行人位姿话题/robot_h/base_pose_ground_truth(其中h＝1,2……n_p，n_p为行人个数)，计算机器人实时位姿(x_r,y_r)与第h个行人实时位姿(x_ph,y_ph)的欧式距离d_h，如果d_h小于预设值3，就将此时刻的行人坐标(x_ph,y_ph)存入数组ped_xy中，如果ped_xy数组有值，则执行步骤S53，否则执行步骤S52；

S53、选取特征点集G_FP中的某一节点G_FPi(i＝1，2，……n，其中n为特征点集G_FP的个数)，将其余节点G_FPx(x＝1，2，……n且x≠i)与节点G_FPi的连线在地图上做碰撞检测，如果碰撞检测通过，则令ped_num等于0，将ped_xy数组中存储的行人坐标依次取出计算其与G_FPi及G_FPx两点连线的垂直距离d_t，如果d_t小于预设值2，则令ped_num+1，当ped_xy数组遍历完全时，将ped_num的值存入临时行人矩阵P_{temp_ped}(i，x)，否则将0存入P_{temp_ped}(i，x)，循环执行步骤S53直至遍历G_FP中的所有节点；清空ped_xy数组中的值，P_ped＝P_ped+P_{temp_ped}得到行人矩阵P_ped，执行步骤S54；

S54、初始化数组tree[i]＝n+1以及dist[i]＝M*N(其中i＝0,1,2……n，n为特征点个数，M为地图的长，N为地图的宽)，设置三个权重：ω₁，ω₂，ω₃，其中，ω₁用于两个特征点之间的欧氏距离，ω₂用于高斯混合模型的某组件的近似矩形的面积，ω₃用于两特征点之间的行人个数；可选的，ω₁＝0.175，ω₂＝0.175，ω₃＝0.65。

S55、在特征地图M_F中找到机器人起点位置对应的特征节点编号robot_index，并将其加入到多模态信息特征树T中，完成多模态信息特征树T的初始化，初始化开放节点集合为T_open＝T，令数组tree[robot_index-1]＝0及数组dist[robot_index-1]＝0；

S56、判断开放节点的个数是否大于零，如果是，则进行步骤S57；否则，则进行步骤S5-10；

S57、提取第一个开放节点t_open，将t_open从开放节点集合T_open中移除，在特征矩阵P_F中找到t_open与其余特征节点的欧氏距离为value1＝P_F(t_open-1，i)，如果value1>0则执行下列步骤；

A、判断开放节点t_open是否为高斯混合模型的某个组件中心点，若是，计算该组件面积为lenda1，计算协方差矩阵中第一个元素和第四个元素中的最小值为width1，判断width1是否小于等于两个成年人的舒适社交距离1.944，如果是，则value3等于50*行人矩阵P_ped(t_open-1，i)，否则value3等于10*行人矩阵P_ped(t_open-1，i)；如果开放节点t_open不是高斯混合模型的某个组件中心点，则令lenda1＝1，value3等于10*行人矩阵P_ped(t_open-1，i)；

B、判断t_open的邻居点i是否为高斯混合模型的某个组件中心点，若是，计算该组件面积为lenda2，计算协方差矩阵中第一个元素和第四个元素中的最小值为width2，判断width2是否小于等于两个成年人的舒适社交距离1.944，如果是，则value3等于50*行人矩阵P_ped(t_open-1，i)，否则value3等于10*行人矩阵P_ped(t_open-1，i)；如果t_open的邻居点i不是高斯混合模型的某个组件中心点，则令lenda2＝1，value3等于10*行人矩阵P_ped(t_open-1，i)；其中，所述步骤A和B中的组件面积为协方差矩阵中第一个元素和第四个元素的乘积。

S58、计算lenda1和lenda2中的最小值为lenda，value2设为1/lenda；

sum_value1＝100*(ω₁*value1+ω₂*value2+ω₃*value3)；

sum_value＝dist[t_open-1]+sum_value1；

S59、如果tree[i]>n，则将编号i+1加入到多模态信息特征树T和开放节点集合T_open中，更新tree[i]＝t_open，dist[i]＝sum_value，否则如果dist[i]>sum_value，则更新tree[i]＝t_open，dist[i]＝sum_value，返回步骤S56；

S5-10、在特征地图MF中找到终点位置对应的特征节点，通过反向搜索特征树T，找到从起点到终点的路径；

S5-11、返回步骤S2。

如图4所示，给定终点，以机器人实时位置作为起点，通过碰撞检测模块，不断地把候选节点加入到特征搜索树上，完成特征搜索树的创建，得到虚线所示的搜索树，进而得到实线所示的启发式路径，虚线所示的为原始的未考虑行人及环境结构信息的启发式路径，走了人少却非常狭窄的区域，容易导航失败、堵塞。

在本发明中，每次关闭终端时，须将特征点集G_FP中的所有节点写进fp.txt中；若update_fp_covariance等于1，则将covariance数组中的k行4列协方差矩阵写进fp_covariance.txt中。

本发明基于高斯混合模型的机器人实时运动规划方法通过使用高斯混合模型训练出聚类结果，将其作为特征点，该特征点不仅包含了环境的结构信息和行人密度信息，其提取过程所需的时间成本和计算资源也大大减少。同时，在路径规划的搜索过程中，通过在成本函数中加入对环境结构信息以及人群密度的考量，使机器人选择走路径较远但人群密度较小的区域，提高了导航成功率。此外，高斯混合模型的训练以及特征点的提取是实时进行的，通过txt文件保存特征点的位置信息以及对应的协方差矩阵信息，占用内存小，解决了节点复用和节约计算资源问题，提取和创建完成后，在不同起/终点的路径规划中可以重复利用，相当于赋予了机器人“大脑记忆”的功能，越用越灵。

以上实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (9)

1.一种基于高斯混合模型的机器人实时运动规划方法，其特征在于，包括以下步骤：

S1、提供一张环境地图，使用高斯混合模型获取环境地图中的环境结构特征，并实时提取机器人的节点位姿信息；其中，所述环境地图为二维栅格地图；

S2、使用角速度通道和距离融合对机器人节点进行特征初步提取，获得初始特征点集，并更新特征地图；

S3、再次使用距离融合对初始特征点集进行进一步提取，获得最终特征点集；

S4、使用欧式距离对环境地图更新，得到最终特征地图，完成实时特征提取；

S5、在所述最终特征地图中找到机器人起点和终点对应的特征编号，根据所述最终特征点集得到特征矩阵和行人矩阵，并形成特征点的特征搜索树，输出启发式路径；

步骤S5包括：

S51、选定特征点集G_FP中的某一节点G_Fpi；i＝1，2，……n；其中n为特征点集G_FP的个数，将其余节点G_FPx与节点G_FPi的连线在地图上做碰撞检测；x＝1，2，……n且x≠i；如果碰撞检测通过，则将两点之间的欧氏距离存入特征矩阵P_F(i，x)，否则将0存入P_F(i，x)；

S52、在机器人运动规划的全过程中，ROS始终订阅并接收行人位姿话题/robot_h/base_pose_ground_truth，其中h＝1,2……，n_p，n_p为行人个数，计算机器人实时位姿(x_r,y_r)与第h个行人实时位姿(x_ph,y_ph)的欧式距离d_h，如果d_h小于预设值3，就将此时刻的行人坐标(x_ph,y_ph)存入数组ped_xy中，如果ped_xy数组有值，则执行步骤S53，否则执行步骤S52；

S53、选取特征点集G_FP中的某一节点G_Fpi，i＝1，2，……，n，其中n为特征点集G_FP的个数，将其余节点G_FPx与节点G_FPi的连线在地图上做碰撞检测；x＝1，2，……n且x≠i；如果碰撞检测通过，则令ped_num等于0，将ped_xy数组中存储的行人坐标依次取出计算其与G_FPi及G_FPx两点连线的垂直距离d_t，如果d_t小于预设值2，则令ped_num+1，当ped_xy数组遍历完全时，将ped_num的值存入临时行人矩阵P_{temp_ped}(i，x)，否则将0存入P_{temp_ped}(i，x)，循环执行步骤S53直至遍历G_FP中的所有节点；清空ped_xy数组中的值，P_ped＝P_ped+P_{temp_ped}得到行人矩阵P_ped，执行步骤S54；

S54、初始化数组tree[i]＝n+1以及dist[i]＝M*N；其中i＝0,1,2……n，n为特征点个数，M为地图的长，N为地图的宽，设置三个权重：ω₁，ω₂，ω₃，其中，ω₁用于两个特征点之间的欧氏距离，ω₂用于高斯混合模型的某组件的近似矩形的面积，ω₃用于两特征点之间的行人个数；

S55、在特征地图M_F中找到机器人起点位置对应的特征节点编号robot_index，并将其加入到多模态信息特征树T中，完成多模态信息特征树T的初始化，初始化开放节点集合为T_open＝T，令数组tree[robot_index-1]＝0及数组dist[robot_index-1]＝0；

S56、判断开放节点的个数是否大于零，如果是，则进行步骤S57；否则，则进行步骤S5-10；

S57、提取第一个开放节点t_open，将t_open从开放节点集合T_open中移除，在特征矩阵P_F中找到t_open与其余特征节点的欧氏距离为value1＝P_F(t_open-1，i)，如果value1>0则执行下列步骤；

A、判断开放节点t_open是否为高斯混合模型的某个组件中心点，若是，计算该组件面积为lenda1，计算协方差矩阵中第一个元素和第四个元素中的最小值为width1，判断width1是否小于等于两个成年人的舒适社交距离，如果是，则value3等于50*行人矩阵P_ped(t_open-1，i)，否则value3等于10*行人矩阵P_ped(t_open-1，i)；如果开放节点t_open不是高斯混合模型的某个组件中心点，则令lenda1＝1，value3等于10*行人矩阵P_ped(t_open-1，i)；

B、判断t_open的邻居点i是否为高斯混合模型的某个组件中心点，若是，计算该组件面积为lenda2，计算协方差矩阵中第一个元素和第四个元素中的最小值为width2，判断width2是否小于等于两个成年人的舒适社交距离，如果是，则value3等于50*行人矩阵P_ped(t_open-1，i)，否则value3等于10*行人矩阵P_ped(t_open-1，i)；如果t_open的邻居点i不是高斯混合模型的某个组件中心点，则令lenda2＝1，value3等于10*行人矩阵P_ped(t_open-1，i)；

S58、计算lenda1和lenda2中的最小值为lenda，value2设为1/lenda；

sum_value1＝100*(ω₁*value1+ω₂*value2+ω₃*value3)；

sum_value＝dist[t_open-1]+sum_value1；

S59、如果tree[i]>n，则将编号i+1加入到多模态信息特征树T和开放节点集合T_open中，更新tree[i]＝t_open，dist[i]＝sum_value，否则如果dist[i]>sum_value，则更新tree[i]＝t_open，dist[i]＝sum_value，返回步骤S56；

S5-10、在特征地图M_F中找到终点位置对应的特征节点，通过反向搜索特征树T，找到从起点到终点的路径；

S5-11、返回步骤S2。

2.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，步骤S1包括：

S11、提供一张环境地图，在环境地图的无障碍物区域内随机撒点，得到训练集；

S12、设置协方差矩阵类型为“full”，高斯组件个数为k，对所述训练集进行高斯混合模型聚类处理，并不断迭代得到环境地图的k个聚类结果，其中，中心均值点x、y坐标存入mean.txt文档中，其相对应的协方差矩阵存入covariance.txt文档中，将k个组件的中心点作为特征点，存进特征点集G_FP中，在此基础上去实时提取机器人的位姿信息；

S13、在ROS***下，每次打开终端时均判断存放特征点位置信息的fp.txt文件是否存在；

若不存在fp.txt文件，则判断mean.txt及covariance.txt是否存在，若存在，将mean.txt中的k个组件的中心点作为原始特征点，存进特征点集G_FP中，在此基础上再去实时提取机器人的位姿信息，并将covariance.txt中k行4列的协方差矩阵存进covariance数组中，并令标志位update_fp_covariance等于1，执行步骤S15；若不存在mean.txt及covariance.txt，执行步骤S12；

若存在fp.txt文件，则将fp.txt文件里存储的节点G_new＝(x_n，y_n)存进特征点集中，并将fp_covariance.txt文件里存储的k行4列的协方差矩阵存进covariance数组中，令标志位update_fp_covariance等于0，执行步骤S14；

S14.每次打开终端时判断feature_map.txt及feature_matrix.txt文件是否存在；若不存在，说明表征度不足，则令representative等于0；若存在，说明表征度完备，则令representative等于1，执行步骤S4；

S15.给定导航目标，机器人基于Risk-RRT算法开始路径规划，实时获取机器人在路径规划过程中的节点位姿信息。

3.如权利要求2所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，步骤S15包括：

通过订阅/amcl_pose和/robot_0/odom话题实时获取机器人在路径规划过程中的节点位姿信息，记作G_m＝(x_m，y_m，ω_m)，其中，x、y表示节点的位置，ω表示节点的角速度大小，提取角速度大于0.2rad/s的节点，每隔一定间隔提取节点，记作G_new＝(x_n，y_n)并作为/pose_velocity话题发布。

4.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，步骤S2包括：

S21、订阅/pose_velocity话题，获取角速度大于0.2rad/s的节点G_new＝(x_n，y_n)；

S22、判断表征度representative是否等于0，如果是，执行步骤S23；如果不是，则根据最优平均距离替换原有特征点，执行步骤S24；

S23、将节点G_new与特征点集G_FP中的所有节点G_i＝(x_i,y_i)，i＝1，2，3，……n，其中n为特征点集G_FP的个数，依次进行欧氏距离计算，记作d_once，判断d_once是否小于等于人为设置的第一次距离融合阈值，如果是，返回步骤S21；如果节点G_new与特征点集G_FP中的所有节点的距离都大于人为设置的第一次距离融合阈值，则将节点G_new添加进特征点集G_FP，并将G_FP带入步骤S3并执行；

S24、将节点G_new与特征点集G_FP中的k+1个节点G_i＝(x_i,y_i)，依次进行欧氏距离计算，记作d_once，i＝k+1，k+2，k+3，……n，判断d_once是否小于等于预设的第一次距离融合阈值，如果是，执行步骤S25，如果不是，执行步骤S5；

S25、将特征地图M_F中栅格值为i的栅格点与节点G_new依次进行碰撞检测，如果碰撞检测全部都通过且特征地图M_F中栅格值为i的栅格点与节点G_new的距离之和小于特征地图M_F中栅格值为i的栅格点与原先的特征节点的距离之和，则节点G_new替换原有特征节点G_i＝(x_i,y_i)，更新特征地图，将G_FP带入步骤S4并执行；否则，执行步骤S5。

5.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，步骤S3包括：

S31、初始化计数c为0，设置一个要保存的特征点集为G_R，将特征点集G_FP中的前k个特征点添加进特征点集G_R中；提取特征点集G_FP中的每个节点G_i＝(x_i,y_i)的相邻特征节点集合G_NX(i)，i＝1，2，3，……，n，其中n为特征点集G_FP的个数；

S32、初始化i为1；

S33、判断i是否小于等于特征点集G_FP的个数n，若是，则执行步骤S34，否则，则执行步骤S36；

S34、判断第i个节点的相邻特征节点集合G_NX(i)的个数是否小于等于预设值3，若是，则进行步骤S35；否则，则令i＝i+1并返回步骤S33；

S35、将节点G_i＝(x_i，y_i)本身及其相邻特征节点集合G_NX(i)里的节点添加进点集G_R中；判断相邻特征节点集合G_NX(i)的个数是否小于等于预设值1，若是，则计数c+1；如果i不等于特征点集G_FP的个数n则令i＝i+1并返回步骤S33，否则执行步骤S36；

S36、对点集G_R进行删重处理，删重过后如果特征点集G_FP的个数n大于等于点集G_R的个数N-1，即n≥N-1，则更新特征地图，将特征点集G_FP带入步骤S4并执行，否则执行步骤S37；

S37、将特征点集G_FP除去点集G_R的其余所有节点添加到点集G_D中，进行第二次距离融合删除特征点，取G_D的第x个节点G_Dx；x＝1，2，……，n₁-1；其中n₁为点集G_D的个数，依次与G_D中的节点G_Dx1连线在地图上做碰撞检测以及欧氏距离计算；x1＝x+1，x+2，……，n₁；如果碰撞检测通过且距离小于人为设定的二次融合距离阈值，则从特征点集G_FP中删除节点G_Dx1；如果点集G_D中的所有点都已经检测完毕，则更新特征地图，将特征点集G_FP带入步骤S4并执行。

6.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，步骤S4包括：

S41、初始化标志位为0，用于表示地图上无障碍物区域的点是否都找到了它所对应的特征点编号，将特征点集G_FP中的所有节点坐标转换为栅格坐标，记作G_i1＝(x_i1，y_i1)；i1＝1，2，……，n；其中n为特征点集G_FP的个数；

S42、遍历二维栅格地图M_p中无障碍物区域的所有栅格点(i_k，j_k)，p＝0，1，……p1，其中p1为二维栅格地图的大小，初始化c1为0，初始化min_dist为地图长度×地图宽度；将栅格点(i_k，j_k)依次与特征点集G_FP中的特征节点G_i1连线在地图上做碰撞检测，如果碰撞检测通过，则计算两点之间的欧氏距离dist₁，如果dist₁小于min_dist，则在特征地图M_F中，更新栅格点(i_k，j_k)的对应值为特征节点的编号i并且min_dist更新为dist₁的值；如果碰撞检测未通过，则计数c1+1；直至特征点集G_FP中的所有节点遍历结束，如果c1＝特征点集G_FP的个数n，则令标志位为1，并退出循环，返回步骤S2；如果c1≠特征点集G_FP的个数n，则重复步骤S42直至二维栅格地图M_p中无障碍物区域的所有栅格点都遍历结束，得到特征地图M_F；

S43、如果标志位等于0，则表征度完备，令representative等于1，将特征点集G_FP带入步骤S5，否则令representative等于0，返回步骤S2。

7.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，ω₁＝0.175，ω₂＝0.175，ω₃＝0.65。

8.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，所述步骤A和B中的组件面积为协方差矩阵中第一个元素和第四个元素的乘积。

9.如权利要求1所述的基于高斯混合模型的机器人实时运动规划方法，其特征在于，所述两个成年人的舒适社交距离为1.944。

Images