CN113110521B - 移动机器人路径规划控制方法及其控制装置、存储介质 - Google Patents

Abstract

本发明提供的一种移动机器人路径规划控制方法及其控制装置、存储介质，包括如下步骤：获取激光雷达的数据，根据激光雷达的数据提取行人位姿，并实时更新静态点集；根据更新后的静态点集，并从中提取环境关键点；根据环境关键点和行人位姿建立局部环境拓扑图，在局部环境拓扑图中搜索若干个初始轨迹，并选择一个初始轨迹作为待优化初始轨迹；根据TEB算法思路构建目标函数，然后使用优化算法使待优化初始轨迹收敛以形成移动轨迹，并控制机器人沿移动轨迹移动。本发明提供的规划控制方法重新定义初始轨迹的确定方式，以降低目标函数非凸性带来的影响。同时，在目标函数中引入描述行人舒适性的代价，以减少机器人在移动过程中对行人的影响。

Description

移动机器人路径规划控制方法及其控制装置、存储介质

技术领域

本发明涉及机器人技术领域，特别涉及一种移动机器人路径规划控制方法及其控制装置、存储介质。

背景技术

自主导航是移动机器人所需具备的基本能力之一，而路径规划是实现自主导航的一项关键技术。路径规划算法用于为移动机器人提供一条可安全通行的路径，其主要分为全局路径规划和局部路径规划。其中，全局路径规划算法基于环境地图规划一条从起点到终点的全局参考路径，而局部路径规划算法则需要根据机器人周围障碍的实时变化动态地调整运动方向，以确保沿全局路径安全地到达指定终点。

现有的技术中主要通过优化迭代的方式确定最优轨迹，其结果的好坏很大程度上取决于轨迹的初始状态。此外，传统的路径规划方法由于没有考虑机器人在移动过程中对周围行人产生的影响而容易使行人产生不舒适的情绪，这严重限制了机器人融入人类日常生活的进程。

发明内容

本发明的主要目的是提供一种移动机器人路径规划控制方法及其控制装置、存储介质，旨在改善现有技术中，机器人的移动路径并不是最优路径、以及在移动过程中会令周围行人不舒适的技术问题。

为实现上述目的，本发明提供一种移动机器人路径规划控制方法，机器人上安装有激光雷达，移动机器人路径规划控制方法包括如下步骤：

获取激光雷达的数据，根据激光雷达的数据提取行人位姿，并实时更新静态点集；

根据更新后的静态点集，从中提取环境关键点；

根据环境关键点和行人位姿建立局部环境拓扑图，在局部环境拓扑图中搜索若干个初始轨迹，并选择一个初始轨迹作为待优化初始轨迹；

根据TEB算法思路构建目标函数，然后使用优化算法使待优化初始轨迹收敛以形成移动轨迹，并控制机器人沿移动轨迹移动。

可选地，获取激光雷达的数据，根据激光雷达的数据提取行人位姿，并实时更新静态点集的步骤包括：

将扫描点集记为P＝{p_i＝(ρ_i，θ_i)|i∈[0，N_s-1]}；

将扫描点集P中属于行人的障碍点的测量距离更新为传感器探测距离R_max；

其中，p_i＝(ρ_i，θ_i)表示第i个扫描点的极坐标，ρ_i表示扫描点距离，θ_i表示扫描点角度，N_s表示扫描点总数。

可选地，根据更新后的静态点集，从中提取环境关键点的步骤包括：

从扫描点p₀开始，沿下标递增的顺序遍历扫描点集P；

若在p_i处检测到了第一类上升沿，则继续向后扫描，直到在p_j(j∈[i+1，i+N_s])处检测到第一类下降沿；

若在p_i处检测到了第二类上升沿或者第二类下降沿，则将两点下标以数组a＝{i，i+1}的形式保存至集合S，然后从p_i+1开始继续扫描，直至检测完最后一个扫描点

可选地，若在p_i处检测到了第一类上升沿，则继续向后扫描，直到在p_j(j∈[i+1，i+N_s])处检测到第一类下降沿的步骤包括：

若i和j之间的间隔大于预设值时，则每隔第一间距取一点p作为部分环境关键点，并将两点下标以数组a＝{i，j+1}的形式保存至集合S，然后从p_j+1开始继续扫描。

可选地，根据更新后的静态点集，从中提取环境关键点的步骤还包括：

依次遍历集合S中的下标数组，对于任意两相邻数组a_i和a_i+1，将a_i的尾元素f＝a_i[1]和a_i+1的首元素b＝a_i+1[0]构成区间[f，b]，以确定一个连续点集合{p_k|k∈[f，b]}；

遍历完集合S，以确定所有的连续点集合；

从各连续点集合中提取其余环境关键点。

可选地，根据环境关键点和行人位姿建立局部环境拓扑图，在局部环境拓扑图中搜索若干个初始轨迹，并选择一个初始轨迹作为待优化初始轨迹的步骤包括：

将所有的环境关键点和行人位置点作为第一节点，进行三角剖分，以形成局部环境拓扑图；

判断局部拓扑图各边所连的两个第一节点的性质；

若第一节点A和第一节点B均为环境关键点并且第一节点A和第一节点B的连线不是静态障碍边缘，则判断第一节点A和第一节点B的间距L1，当L1≥2R_s，则将第一节点A和第一节点B的中点作为该边的路径点C；

若第一节点A和第一节点B均为行人位置点，则判断第一节点A和第一节点B的间距L2，当L2≥2(R_s+R_p)，则将该边的路径点C满足如下关系：

若第一节点A为行人位置，第一节点B为环境关键点，则判断第一节点A和第一节点B的间距L3，当L3≥2R_s+R_p，则该边的路径C满足如下关系：

将机器人的实时位置、局部终点位置以及路径点C作为第二节点构建无向图G，进而生成若干个初始轨迹；

其中，R_p表示行人半径，R_s表示安全半径，O表示机器人的实时位置，w_A，B和w_B，A均为各向异性系数。

可选地，根据环境关键点和行人位姿建立局部环境拓扑图，在局部环境拓扑图中搜索若干个初始轨迹，并选择一个初始轨迹作为待优化初始轨迹的步骤包括：

根据评价函数对若干个初始轨迹进行评价，从中选择评分最高的初始轨迹作为所述待优化初始轨迹，所述评价函数满足如下关系：

其中，α，β，γ，δ∈[0，1]表示权重系数。

可选地，根据TEB算法思路构建目标函数，然后使用优化算法使待优化初始轨迹收敛以形成移动轨迹，并控制机器人沿移动轨迹移动的步骤包括：

根据导航要求和机器人约束构建目标函数；

使用Levenberg-Marquardt优化算法对待优化初始轨迹进行优化，以形成移动轨迹。

此外，为实现上述目的，本发明还提出一种移动机器人路径规划控制装置，包括储存器、处理器、以及仓储在所述储存器上并可在所述处理器上运行的移动机器人路径规划控制程序，移动机器人路径规划控制程序配置为实现如上文所述的移动机器人路径规划控制方法的步骤。

此外，为实现上述目的，本发明还提出一种存储介质，存储介质上存储有移动机器人路径规划控制程序，移动机器人路径规划控制程序被处理器执行时实现如上文所述的移动机器人路径规划控制方法的步骤。

在本发明提供的技术方案中，关注于局部路径规划算法，通过将机器人的运动学约束和动力学约束、避免碰撞、轨迹终点接近局部终点以及减少移动时间等要求表示为一个有约束的非线性优化问题，然后利用Levenberg-Marquardt(莱文贝格-马夸特方法)优化算法找出最优轨迹，并控制机器人沿该轨迹移动。重新定义初始轨迹的确定方式，以降低目标函数非凸性带来的影响。同时，在目标函数中引入描述行人舒适性的代价，以减少机器人在移动过程中对行人的影响。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明实施例方案涉及的硬件运行环境的移动机器人路径规划控制装置的结构示意图；

图2为本发明提供的移动机器人路径规划控制方法的一实施例的流程示意图；

图3为图2中机器人周围行人和静态障碍信息示意图；

图4为图2中机器人进行环境关键点提取示意图；

图5为图2中机器人初始轨迹搜索示意图；

图6为图2中机器人轨迹优化示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明，若本发明实施例中有涉及方向性指示，则该方向性指示仅用于解释在某一特定姿态下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，全文中出现的“和/或”的含义，包括三个并列的方案。还有就是，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

参照图1，图1为本发明实施例方案涉及的硬件运行环境的移动机器人路径规划控制装置的结构示意图。如图1所示，该移动机器人路径规划控制装置可以包括：处理器1001，例如中央处理器(Central Processing Unit，CPU)，通信总线1002、用户接口1003，网络接口1004，存储器1005。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)，可选用户接口1003还可以包括标准的有线接口、无线接口，对于用户接口1003的有线接口在本发明中可为USB接口。网络接口1004可选的可以包括标准的有线接口、无线接口(如无线保真(WIreless-FIdelity，WI-FI)接口)。存储器1005可以是高速的随机存取存储器(Random Access Memory，RAM)存储器，也可以是稳定的存储器(Non-volatile Memory，NVM)，例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。

本领域技术人员可以理解，图1中示出的结构并不构成对移动机器人路径规划控制装置的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。

如图1所示，认定为一种计算机存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及移动机器人路径控制装置的控制程序。

在图1所示的移动机器人路径规划控制装置中，网络接口1004主要用于连接后台服务器，与后台服务器进行数据通信；用户接口1003主要用于连接用户设备；移动机器人路径规划控制装置通过处理器1001调用存储器1005中存储的移动机器人路径规划控制程序，并执行本发明实施例提供的移动机器人路径规划的控制步骤。

整个系统主要包括装有360度2D激光雷达传感器的地面移动机器人、室内静态障碍和动态行人。面向室内场景，移动机器人进行局部路径规划，在能够在安全地到达局部终点的前提下，降低其对行人的负面影响。

基于上述硬件结构，提出本发明移动机器人路径规划控制方法的实施例。

参见图2，图2为本发明移动机器人路径规划控制方法一实施例的流程示意图，包括如下步骤：

步骤S10：获取激光雷达的数据，根据激光雷达的数据提取行人位姿，并实时更新静态点集。

应理解的是，本实施例的执行主体是移动机器人路径规划控制装置，其中，移动机器人路径规划控制装置可为个人电脑或服务器等等电子设备。

步骤S20：根据更新后的静态点集，从中提取环境关键点。

步骤S30：根据环境关键点和行人位姿建立局部环境拓扑图，在局部环境拓扑图中搜索若干个初始轨迹，并选择一个初始轨迹作为待优化初始轨迹。

步骤S40：根据TEB算法思路构建目标函数，然后使用优化算法使待优化初始轨迹收敛以形成移动轨迹，并控制机器人沿移动轨迹移动。

需要说明的是，Timed-Elastic-Bands(TEB)是一种先进的轨迹优化算法，其主要思想是将机器人的动力学约束、避免碰撞、以及减少移动时间等要求表示为一个有约束的非线性优化问题。现有的TEB算法通过优化迭代的方式确定最优轨迹，即将机器人当前位置和局部终点的连线作为初始轨迹，然后根据机器人的动力学约束、避免碰撞、以及减少移动时间等要求构建目标函数以定量地计算轨迹代价，最后利用优化算法找出使代价最小的轨迹作为最终结果，并控制机器人沿该轨迹移动。但是目标函数通常是非凸的，其表现为目标函数总是存在多个局部最优解。这导致最终收敛到的结果轨迹并非全局最优轨迹，其结果的好坏很大程度上取决于轨迹的初始状态。

本发明将局部环境抽象为拓扑图，然后从拓扑图中搜索一定数量的候选初始轨迹，然后根据各轨迹的长度、与局部终点的接近程度、与障碍的距离以及对行人的舒适性等方面的评估选择最优轨迹作为初始轨迹，然后基于这条较优的初始轨迹进行迭代优化，从而获得最终的结果轨迹。相比于直接将机器人位置和局部终点的连线作为初始轨迹的方式，这种初始轨迹的确定方式有助于轨迹收敛至更优的结果。

此外，现有的传统的路径规划方法仅考虑以最短的移动距离或者时间到达终点，并未考虑机器人在移动过程中对周围行人产生的影响(如阻挡他人前进)，而容易使行人产生不舒适的情绪，这严重限制了机器人融入人类日常生活的进程。

本发明使用社会力的概念将机器人的轨迹对行人的影响进行量化，并将其引入目标函数。因此，在根据目标函数寻找最优轨迹时，可以将减少机器人对行人的影响考虑在内，从而表现出导航的舒适性。

进一步地，步骤S10，包括：

步骤S101：将扫描点集记为P＝{p_i＝(p_i，θ_i)|i∈[0，N_s-1]}。

步骤S102：将扫描点集P中属于行人的障碍点的测量距离更新为传感器探测距离R_max。

其中，p_i＝(p_i，θ_i)表示第i个扫描点的极坐标，p_i表示扫描点距离，θ_i表示扫描点角度，N_S表示扫描点总数。

需要说明的是，此时的扫描点集P为静态点集。利用行人检测与追踪算法从激光雷达数据中提取行人的位姿信息。将扫描点集P中属于行人的障碍点的测量距离更新为最大传感器探测距离R_max，此时的点集P中仅包含静态障碍点和非障碍点(非障碍点即测量距离为R_max的点，表示该方向无障碍)。

进一步地，步骤S20，包括：

步骤S201：从扫描点p₀开始，沿下标递增的顺序遍历扫描点集P。

步骤S202：若在p_i处检测到了第一类上升沿，则继续向后扫描，直到在p_j(j∈[i+1，i+N_s])处检测到第一类下降沿。

步骤S203：若在p_i处检测到了第二类上升沿或者第二类下降沿，则将两点下标以数组a＝{i，i+1}的形式保存至集合S，然后从P_i+1开始继续扫描。

需要说明的是，遍历点集P，获取所有连续点集合以及部分环境关键点。1)设两个相邻扫描点p_i为和p_j，则可以定义两类不连续边沿：

a)第一类不连续边沿：ρ_i＜R_max&&ρ_j＝R_max。

b)第二类不连续边沿：ρ_j-ρ_i＞2R_s。

其中，ρ_i和ρ_j分别是扫描点p_i和p_j的距离，R_max是传感器可感知的最大距离，R_s是安全半径(略大于机器人半径)。

2)基于上述不连续边沿的定义再做进一步规定：若j＞i，则称在p_i处检测到了上升沿，反之，则称在p_j处检测到了下降沿。

从扫描点p₀开始，沿下标递增的顺序遍历扫描点集P，若检测到两类不连续，则采取不同的处理方式：若在p_i处检测到了第一类上升沿，则继续向后扫描，直到在p_j(j∈[i+1，i+N_s])处检测到第一类下降沿。若在p_i处检测到第二类上升沿或者下降沿，则以数组a＝{i，i+1}的形式保存至集合S，然后从p_i+1开始继续扫描。上述过程不断重复，直至检测完最后一个扫描点。可以理解的是，第一类上升沿就是指同时满足a)第一类不连续边沿：ρ_i＜R_max&&ρ_j＝R_max、j＞i这两个条件；第一类下降沿就是指同时满足a)第一类不连续边沿：ρ_i＜R_max&&ρ_j＝R_max、j≤i这两个条件；依次类推就可以知道第二类上升沿和第二类下降沿的具体含义，在此不再赘述。

进一步地，步骤S202，包括：

步骤S2021：若i和j之间的间隔大于预设值时，则每隔第一间距取一点p作为部分所述环境关键点，并将两点下标以数组a＝{i，j+1}的形式保存至所述集合S，然后从p_j+1开始继续扫描。

需要说明的是，上文中的两点指代的是相邻扫描点ρ_i为和p_j。第一间距指的是每隔N_s/10个扫描点取一点p作为部分所述环境关键点。

进一步地，步骤S20，还包括：

步骤S204：依次遍历所述集合S中的下标数组，对于任意两相邻数组a_i和a_i+1，将a_i的尾元素f＝a_i[1]和a_i+1的首元素b＝a_i+1[0]构成区间[f，b]，以确定一个连续点集合{p_k|k∈[f，b]}。

步骤S205：遍历完集合S，以确定所有的连续点集合。

步骤S206：从各所述连续点集合Q中提取其余所述环境关键点。

需要说明的是，对任意连续点集合Q，以增量的方式确定属于同一条直线的点。一个区间[f，b]只能确定一个连续点集合Q，当遍历完集合S，才能确定所有的连续点集合。

a)取点集Q中的前两个点确定一条直线，直线模型采用如下形式：

x·cosα+y·sinα＝r

式中，α∈(-π，π]是传感器坐标系的x轴与直线法线的夹角，r∈[0，+∞)是坐标系原点与直线的距离，(x，y)表示直线上点的笛卡尔坐标。

b)计算下一个点p_k＝(x_k，y_k)到直线l的距离d_k：

d_k＝|x_k·cosα+y_k·sinα-r|

i.若距离d_k大于预设的最大允许误差E_max，则认为点p_k不在当前的直线l上，此时保存该条直线及其对应的点集。然后根据p_k和p_k+1两点重新确定直线l，并返回步骤b)继续执行，直至遍历结束。

ii.若距离d_k不大于预设的最大允许误差E_max，则认为点p_k在当前的直线l上，此时将p_k添加至直线l对应的点集中，并利用最小二乘法重新拟合直线l。具体地，若该直线上的点的数量为N，则定义误差函数：

令

可解得使L(α，r)最小的参数α^*和r^*满足：

式中，

和

分别表示所有点的横坐标均值和纵坐标均值。当更新直线l的参数后，返回步骤b)继续执行，直至遍历结束。

2)尝试对各相邻直线进行合并，即利用最小二乘法对两相邻直线上的所有点进行直线拟合，仅对合并后的拟合误差E小于最大允许误差E_max的两直线进行合并。这里的拟合误差E计算为：

式中，α′和r′是拟合直线的参数，L是合并后的点集。

3)将各段直线的两个端点作为环境关键点。

进一步地，步骤S30，包括：

步骤S301：将所有的环境关键点和行人位置点作为第一节点，进行三角剖分，以形成局部环境拓扑图。

步骤S302：判断局部拓扑图各边所连的两个第一节点的性质。

步骤S303：若第一节点A和第一节点B均为环境关键点并且第一节点A和第一节点B的连线不是静态障碍边缘，则判断第一节点A和第一节点B的间距L1，当L1≥2R_s，则将第一节点A和第一节点B的中点作为该边的路径点C。

步骤S302：若第一节点A和第一节点B均为行人位置点，则判断第一节点A和第一节点B的间距L2，当L2≥2(R_s+R_p)，则将该边的路径点C满足如下关系：

步骤S304：若第一节点A为所述行人位置，第一节点B为环境关键点，则判断第一节点A和第一节点B的间距L3，当L3≥2R_s+R_p，则将该边的路径C满足如下关系：

步骤S304：将机器人的实时位置、局部终点位置以及路径点C作为第二节点构建无向图

进而生成若干个初始轨迹。

需要说明的是，根据环境关键点和行人位姿信息建立局部环境拓扑图，并搜索最优初始轨迹。

(1)将所有环境关键点和行人位置点作为节点，进行Delaunay三角剖分，剖分结果即为局部环境拓扑图。

(2)在局部环境拓扑图中搜索给定数量的候选初始轨迹。

1)根据拓扑图各边所连的两节点的性质确定各边所对应的路径点。

a)若两节点A和B均为环境关键点而且两点连线不是静态障碍边缘，则判断两点的间距。若间距小于2R_s，则认为机器人无法通行，因此该边无路径点。否则，将二者的中点作为该边的路径点。

b)若两节点A和B均为行人位置点，则判断两点的间距。若间距小于2(R_s+R_p)，(R_p表示行人半径)，则认为机器人无法通行，因此该边无路径点。否则，将该边的路径点C取为：

式中，W_A，B和w_B，A均为各向异性系数，其定义为：

式中，λ∈[0，1]是各项异性强度，

是点j相对于行人i的方向与行人i的朝向之间的夹角。

c)若节点A为行人位置，B为环境关键点，则判断两点的间距。若间距小于2R_s+R_p，则认为机器人无法通行，因此该边无路径点。否则，将该边的路径点C取为：

2)根据机器人当前位置O、局部终点位置以及所有路径点进行初始轨迹搜索。

a)将机器人当前位置O、局部终点位置以及所有路径点作为节点构建无向图

其中，将路径点所在的Delaunay三角形的各边上的路径点作为该点的相邻点，并形成无向边。考虑到机器人当前位置或局部终点位置不属于任何Delaunay三角形，因此，若有包含该点的Delaunay三角形，则将该Delaunay三角形上的路径点作为该点的相邻点。否则，将与该点最近的路径点作为其相邻点。

b)以机器人当前位置O为起点、局部终点位置为终点，在无向图中进行深度优先搜索，从而生成若干候选初始轨迹。在搜索过程中，优先选择与局部终点最近的节点。同时，为了避免生成过多或过长的候选路径影响实时性，可设置搜索的路径数量上限和路径长度上限。

(3)根据评价函数对所有候选初始轨迹进行评价，从中选择评分最高的轨迹作为最优初始轨迹。评价函数具体定义为：

式中，α，β，γ，δ∈[0，1]是权重系数。

函数

用于评价轨迹

与局部终点的接近程度，其定义为：

其中，

是轨迹

的终点位置与局部终点的距离，D_max是所有轨迹的终点与局部终点的最大距离。

函数

用于评价轨迹

的长度，其定义为：

其中，

是轨迹

的长度，L_max是所有轨迹中的最长轨迹的长度。

函数

用于评价轨迹

的安全性，其定义为：

其中，D_s是安全距离，d_i，o是轨迹

上某点i与静态点集

中某点o之间的距离。

函数

用于评价轨迹

的舒适性，其定义为：

其中，

是当机器人在轨迹

上某点i时对行人j的社会力大小，M和N是待调参数，d_j，i表示两点的距离。F_max是机器人沿轨迹

移动时对所有行人产生的最大社会力(这里的社会力引自社会力模型，可以定量地描述机器人对行人的作用强弱)。

进一步地，步骤S40，包括：

步骤S401：根据导航要求和机器人约束构建目标函数。

步骤S402：使用Levenberg-Marquardt优化算法对待优化初始轨迹进行优化，以形成移动轨迹。

需要说明的是，根据TEB算法思路构建目标函数，然后使用Levenberg-Marquardt(莱文贝格-马夸特方法)优化算法使初始轨迹收敛至最优轨迹，并控制机器人沿最优轨迹移动。

(1)根据导航要求和机器人约束构建目标函数。具体地，设机器人的状态s＝[x，y，θ]^T，(x，y)表示机器人的坐标，θ表示机器人的朝向，一条轨迹上的离散状态可记为(s_k)_{k＝1，2，...，n}。令(ΔT_k)_{k＝1，2，...，n-1}表示一组时间间隔

的序列，ΔT_k表示从状态s_k转移至s_k+1的时间间隔。因此，优化问题的参数集

可定义为：

目标函数定义为：

式中，σ₁，σ₂，σ₃，σ₄，σ₅，σ₆均为权重系数，F_k(s_k)表示行人舒适性约束，其定义为机器人处于状态s_k时对所有行人的社会力总和，h_k(s_k+1，s_k)表示曲率不变约束，其定义为：

其中，d_k＝[x_k+1-x_k，y_k+1-y_k，0]^T。o_k(s_k)表示静态障碍约束，其定义为：

其中，δ(s_k，o)，o∈T表示机器人处于状态s_k时与障碍o之间的距离，δ_min表示机器人与障碍之间的可接受的最小距离。v_k(s_k+1，s_k，ΔT_k)表示机器人速度约束，其定义为：

其中，v_max和ω_max分别表示机器人的最大线速度和最大角速度，v_k和ω_k分别表示线速度和角速度，γ(·)表示机器人线速度的方向，其定义为：

其中，q_k＝[cosθ_k，sinθ_k，0]^T。α_k(s_k+2，s_k+1，s_k，ΔT_k+1，ΔT_k)和α₁(s₂，s₁，ΔT₁)表示加速度约束，其定义为：

其中，a_max表示机器人的最大线加速度。

(2)使用Levenberg-Marquardt优化算法寻找最优轨迹，即求解：

(3)控制机器人沿最优轨迹移动。本发明采用差分驱动移动机器人，其运动学模型为：

式中，u(t)＝[v(t)，ω(t)]^T是控制输入。当获取最优参数

后，最优控制输入u^*(t)可计算为：

如图3所示，利用行人检测与追踪算法可提取周围行人的位置和朝向。静态障碍点是激光雷达数据中探测距离小于R_max、且不在行人上的点。如图4所示，共检测到5个连续点集(用虚线框标出)。然后，从各连续点集合中可提取到环境关键点。如图5所示，连线表示Delaunay三角剖分结果，即局部环境拓扑图。然后，在局部环境拓扑图中搜索给定数量的候选初始轨迹，搜索时的各路径点在图中标记，同时，部分路径也用箭头表示。最后，根据评价函数对所有初始轨迹进行评价，从中选择评分最高的轨迹作为移动轨迹。如图6所示，箭头表示的轨迹时上一步选出的待优化初始轨迹，通过优化算法可在待优化初始轨迹的基础上进行调整与优化，最终收敛至移动轨迹。不断重复上述步骤，直至机器人到达终点。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如只读存储器镜像(Read Only Memory image，ROM)/随机存取存储器(Random AccessMemory，RAM)、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种移动机器人路径规划控制方法，机器人上安装有激光雷达，其特征在于，所述机器人移动路径的的控制方法包括如下步骤：

获取所述激光雷达的数据，根据所述激光雷达的数据提取行人位姿，并实时更新静态点集；

根据更新后的所述静态点集，从中提取环境关键点；

根据所述环境关键点和所述行人位姿建立局部环境拓扑图，在所述局部环境拓扑图中搜索若干个初始轨迹，并选择一个所述初始轨迹作为待优化初始轨迹；

根据TEB算法思路构建目标函数，然后使用优化算法使所述待优化初始轨迹收敛以形成移动轨迹，并控制所述机器人沿所述移动轨迹进行移动；

其中，所述根据所述环境关键点和所述行人位姿建立局部环境拓扑图，在所述局部环境拓扑图中搜索若干个初始轨迹，并选择一个所述初始轨迹作为待优化初始轨迹的步骤包括：

将所有的所述环境关键点和行人位置点作为第一节点，进行三角剖分，以形成所述局部环境拓扑图；

判断所述局部拓扑图各边所连的两个所述第一节点的性质；

若第一节点A和第一节点B均为所述环境关键点并且所述第一节点A和所述第一节点B的连线不是静态障碍边缘，则判断所述第一节点A和所述第一节点B的间距L1，当L1≥2R_S，则将所述第一节点A和所述第一节点B的中点作为该边的路径点C；

若所述第一节点A和所述第一节点B均为所述行人位置点，则判断所述第一节点A和所述第一节点B的间距L2，当L2≥2(R_s+R_p)，则将该边的路径点C满足如下关系：

若所述第一节点A为所述行人位置，所述第一节点B为所述环境关键点，则判断所述第一节点A和所述第一节点B的间距L3，当L3≥2R_s+R_p，则该边的路径C满足如下关系：

将所述机器人的实时位置、局部终点位置以及所述路径点C作为第二节点构建无向图G，进而生成若干个所述初始轨迹；

其中，R_p表示行人半径，R_s表示安全半径，O表示所述机器人的实时位置，w_A，B和w_B，A均为各向异性系数。

2.如权利要求1所述的移动机器人路径规划控制方法，其特征在于，所述获取所述激光雷达的数据，根据所述激光雷达的数据提取行人位姿，并实时更新静态点集的步骤包括：

将扫描点集记为P＝{p_i＝(ρ_i，θ_i)|i∈[0，N_s-1]}；

将所述扫描点集P中属于行人的障碍点的测量距离更新为传感器探测距离R_max；

其中，p_i＝(ρ_i，θ_i)表示第i个扫描点的极坐标，ρ_i表示扫描点距离，θ_i表示扫描点角度，N_s表示扫描点总数，p₀是机器人的朝向方向上的扫描点，扫描点下标沿逆时针方向递增。

3.如权利要求2所述的移动机器人路径规划控制方法，其特征在于，所述根据更新后的所述静态点集，从中提取环境关键点的步骤包括：

从扫描点p₀开始，沿下标递增的顺序遍历所述扫描点集P；

若在p_i处检测到了第一类上升沿，则继续向后扫描，直到在p_j(j∈[i+1，i+N_s])处检测到第一类下降沿；

若在p_i处检测到了第二类上升沿或者第二类下降沿，则将两点下标以数组a＝{i，i+1}的形式保存至集合S，然后从p_i+1开始继续扫描，直至检测完最后一个扫描点

其中，设p_i和p_j为两个相邻扫描点，定义两类不连续边沿，其中，第一类不连续边沿满足ρ_i＜R_max&&ρ_j＝R_max，第二类不连续边沿满足ρ_j-ρ_i＞2R_s，R_s表示安全半径，通常略大于机器人半径，若j＞i，则在p_i处检测到的是上升沿，若j＜i，则在p_j处检测到的是下降沿。

4.如权利要求3所述的移动机器人路径规划控制方法，其特征在于，所述若在p_i处检测到了第一类上升沿，则继续向后扫描，直到在p_j(j∈[i+1，i+Ns])处检测到第一类下降沿的步骤包括：

若i和j之间的间隔大于预设值时，则每隔第一间距取一点p作为部分所述环境关键点，并将两点下标以数组a＝{i，j+1}的形式保存至所述集合S，然后从p_j+1开始继续扫描。

5.如权利要求4所述的移动机器人路径规划控制方法，其特征在于，所述根据更新后的所述静态点集，从中提取环境关键点的步骤还包括：

依次遍历所述集合S中的下标数组，对于任意两相邻数组a_i和a_i+1，将a_i的尾元素f＝a_i[1]和a_i+1的首元素b＝a_i+1[0]构成区间[f，b]，以确定一个连续点集合{p_k|k∈[f，b]}；

遍历完集合S，以确定所有的连续点集合；

从各所述连续点集合中提取其余所述环境关键点；

其中，所述集合S中的下标数组表示集合S中的元素，即集合S＝{a₁，a₂，...，a_n}中的每个元素a_i又是一个数组。

6.如权利要求1所述的移动机器人路径规划控制方法，其特征在于，所述根据所述环境关键点和所述行人位姿建立局部环境拓扑图，在所述局部环境拓扑图中搜索若干个初始轨迹，并选择一个所述初始轨迹作为待优化初始轨迹的步骤包括：

根据评价函数对若干个所述初始轨迹进行评价，从中选择评分最高的所述初始轨迹作为所述待优化初始轨迹，所述评价函数满足如下关系：

其中，α，β，γ，δ∈[0，1]表示权重系数。

7.如权利要求6所述的移动机器人路径规划控制方法，其特征在于，所述根据TEB算法思路构建目标函数，然后使用优化算法使所述待优化初始轨迹收敛以形成移动轨迹，并控制所述机器人沿所述移动轨迹移动的步骤包括：

根据导航要求和机器人约束构建所述目标函数；

使用Levenberg-Marquardt优化算法对所述待优化初始轨迹进行优化，以形成所述移动轨迹。

8.一种移动机器人路径规划控制装置，其特征在于，包括储存器、处理器、以及仓储在所述储存器上并可在所述处理器上运行的移动机器人路径规划控制程序，所述移动机器人路径规划控制程序配置为实现权利要求1-7中任一项所述的移动机器人路径规划控制方法的步骤。

9.一种存储介质，其特征在于，所述存储介质上存储有移动机器人路径规划控制程序，所述移动机器人路径规划控制程序被处理器执行时实现如权利要求1-7中任一项所述的移动机器人路径规划控制方法的步骤。

Images