CN114740849B - 基于行人步行决策规则的移动机器人自主导航方法及装置 - Google Patents

Abstract

本发明涉及基于行人步行决策规则的移动机器人自主导航方法及装置，所述方法包括以下步骤：基于给定的目的地位置和机器人初始位置，获取无障碍物的全局路径，将全局路径分隔成多个路点，其中相邻的路点之间具有多个路点间距；将所述代谢能量值输入所述闭环控制模型，使达到下一个路点的轨迹数据输送到机器人的运动控制器，然后控制机器人移动到下一个路点。其中的装置包括存储器和处理器，该处理器在执行储存在存储器的指令时实施所述方法。本发明公开的移动机器人动态环境中自主导航方法及装置，机器人移动便于周围行人的理解，提高了机器人的导航安全性和效率，提高行人对机器人的接受度。

Description

基于行人步行决策规则的移动机器人自主导航方法及装置

技术领域

本发明涉及一种移动机器人动态环境中自主导航方法及装置，尤其涉及一种稠密行人环境中基于行人步行决策规则的移动机器人自主导航方法及装置。

背景技术

在服务机器人自主智能技术中，机器人自主导航技术是需要关注的重点。近年来，随着我国城镇化的发展，城市的数量与人口显著增加，服务机器人存在许多重要的应用场景，如快件与外卖收取、楼宇清洁、文件配送、迎宾接待等，而这些应用都需要机器人能够在稠密行人环境中自主导航。

为了实现移动机器人在稠密行人环境中实现自主导航，就必须要求机器人能够以类似行人的决策方式生成自身的导航行为，以便于周围行人理解机器人的行动意图。在传统的导航框架中，导航方法针对静态环境设计，并未考虑机器人的行为对周围行人的影响，对公共交通秩序的堵塞，故不适用于稠密行人环境导航。

发明内容

本发明提供一种移动机器人动态环境中自主导航方法及装置，旨在至少解决现有技术中存在的技术问题之一。

本发明的技术方案涉及一种基于行人步行决策规则的移动机器人自主导航方法，所述方法包括以下步骤：

S100、基于给定的目的地位置和机器人初始位置，获取无障碍物的全局路径，将全局路径分隔成多个路点，其中相邻的路点之间具有多个路点间距；

S200、对于每个路点通过检测模块获取多个行人信息，通过线性速度模型输入所述行人信息，得到预测行人轨迹位置信息，其中，所述行人信息包括行人位置数据、行人速度数据及行人占地面积半径数据；

S300、计算当前时间点的路点周围的每个行人轨迹位置信息，根据预测的行人轨迹位置信息，寻找与预测的行人位置信息直线距离最短的下一个无碰撞行人的路点，同时根据人际距离模型，在机器人移动过程中与行人保持人际距离；

S400、提供机器人的代谢能量目标函数及闭环控制模型，将机器人的线速度及角速度输入所述代谢能量目标函数，以输出机器人的代谢能量值，再将所述代谢能量值输入所述闭环控制模型，使达到下一个路点的轨迹数据输送到机器人的运动控制器，然后控制机器人移动到下一个路点。

进一步，所述步骤S100包括：设置所述机器人的最大速度，所述路点间距通过以下公式计算：

Δd＝3×v_max

其中，v_max是机器人的最大移动速度上限，Δd是路点间距。

进一步，所述步骤S100包括：将当前路点作为中间目标点，当机器人距离当前路点小于3m时，将下一个路点设置为下一个中间目标点，直到机器人移动到达给定的目的地位置。

进一步，所述步骤S200包括：构建所述线性速度模型公式，计算预测行人轨迹信息，

其中，p_x、p_y是当前所述行人位置数据的坐标值，p_v是当前所述行人速度数据的速度值，p_θ是当前所述行人占地面积半径数据的面积值，

是所述预测行人轨迹信息的坐标值。

进一步，在所述步骤S300中，通过以下方式构建所述机器人与行人的无碰撞距离：

d＝p_b+r_b+d_b

其中，p_b是行人的半径，r_b是机器人的半径，d_b是机器人与行人保持的距离，所述人际距离包括社交空间距离及私人空间距离，所述社交空间距离不超过1.2m，所述私人空间距离不超过0.5m，所述机器人随着行人密度的增加，机器人与行人之间的保持的人际距离线性递减，不超过与行人的私人空间距离。

进一步，在所述步骤S400中，通过以下方式构建机器人的代谢能量目标函数，计算机器人的代谢能量值：

f＝a₀+a₁v²+a₂ω²

其中，a₀、a₁、a₂是机器人的代谢能量目标函数的权重参数，v是机器人的线速度，ω是机器人的角速度，f是机器人的代谢能量值。

进一步，所述步骤S400中：通过以下方式构建闭环控制模型，计算从当前路点到达下一个路点的轨迹数据：

其中，当前路点的坐标为

下一个路点的坐标为/>

其中，/>

分别表示ρ、α、φ的闭环误差向量，ρ是从当前路点的坐标/>

到下一个路点的坐标/>

之间的距离，α＝θ-θ_g，θ是当前路点的偏转角度值，θ_g是下一个路点的偏转角度值，K_ρ、K_α、K_φ是权重参数。

进一步，所述步骤S400中：使K_ρ取一预设值，通过公式

优化计算找寻一组K_α、K_φ的值，使得生成一条从当前位置到点/>

的轨迹；根据机器人的代谢能量目标函数计算得到代谢能量值，多个路点之间移动的所述轨迹数据及所述代谢能量值依次迭代直到机器人的代谢能量值最小。

进一步，所述步骤S200包括：机器人预测行人从当前位置至少两秒后的所述预测行人轨迹位置信息。

本发明的技术方案还涉及一种计算机可读存储介质，其上储存有计算机程序指令，所述计算机程序指令被处理器执行时实施上述的方法。

本发明的有益效果如下。

机器人在稠密行人环境中导航的过程中，基于本发明提供的自主导航方法，机器人能够自主生成类人的导航行为，便于周围行人的理解，提高了机器人的导航安全性和效率，提高行人对机器人的接受度。

附图说明

图1是根据本发明的方法的机器人最优选择路径的示意图。

图2是根据本发明实施例中的系列路点划分的示意图。

图3是根据本发明实施例中的机器人与行人的距离-行人密度关系图。

图4是根据本发明实施例中的机器人轨迹优化过程示意图。

具体实施方式

以下将结合实施例和附图对本发明的构思、具体结构及产生的技术效果进行清楚、完整的描述，以充分地理解本发明的目的、方案和效果。

参照图1至图4，在一些实施例中，本发明公开了一种基于行人步行决策规则的移动机器人自主导航方法，所述方法包括以下步骤：

S100、基于给定的目的地位置和机器人初始位置，获取无障碍物的全局路径，将全局路径分隔成多个路点，其中相邻的路点之间具有多个路点间距。参照图1所示，机器人在x、y坐标轴的初始位置，机器人通过自身的拍摄设备采集，采用A-Star算法获取全局路径。具体地，将图1中的静态环境表示为二维占用网格地图，其中有行人或其他障碍物的网格为1，没有障碍物的网格为0。

S200、对于每个路点通过检测模块获取多个行人信息，通过线性速度模型输入所述行人信息，得到预测行人轨迹位置信息，其中，所述行人信息包括行人位置数据、行人速度数据及行人占地面积半径数据。该处机器人的检测模块可以包括拍摄设备、雷达等设备，用于收集上述的行人位置数据、行人速度数据及行人占地面积半径数据。

S300、计算当前时间点的路点周围的每个行人轨迹位置信息，根据预测的行人轨迹位置信息，寻找与预测的行人位置信息直线距离最短的下一个无碰撞行人的路点，同时根据人际距离模型，在机器人移动过程中与行人保持人际距离。

S400、提供机器人的代谢能量目标函数及闭环控制模型，将机器人的线速度及角速度输入所述代谢能量目标函数，以输出机器人的代谢能量值，再将所述代谢能量值输入所述闭环控制模型，使达到下一个路点的轨迹数据输送到机器人的运动控制器，然后控制机器人移动到下一个路点。

对于步骤S100的具体实施方式

基于将全局路径离散为一些列路点，步骤S100还包括：设置所述机器人的最大速度，所述路点间距通过以下公式计算：

Δd＝3×v_max

其中，v_max是机器人的最大移动速度上限，Δd是路点间距。在全局路径上，譬如机器人的最大速度上限v_max＝1m/s，计算得到Δd＝3m。将全局路径上，距离机器人初始位置3m处的路点设置为第一路点，6m处的路点设置为第二路点，如此类推，依次设置直到倒数第二个路点距离机器人目的地位置小于3m。最后，将机器人目的地位置设置为最后一个路点。

进一步地，步骤S100可以包括：将当前路点作为中间目标点，当机器人距离当前路点小于3m时，将下一个路点设置为下一个中间目标点，直到机器人移动到达给定的目的地位置。具体地，参照图2，机器人将第一个路点作为机器人的中间目标点，当机器人距离当前路点小于1m，可以将下一个路点设置为机器人的中间目标点，直至机器人到达最后一个路点，并最终抵达目的地位置。

对于步骤S200的具体实施方式

进一步地，所述步骤S200包括：构建所述线性速度模型公式，计算预测行人轨迹信息，

其中，p_x、p_y是当前所述行人位置数据的坐标值，p_v是当前所述行人速度数据的速度值，p_θ是当前所述行人占地面积半径数据的面积值，

是所述预测行人轨迹信息的坐标值。

机器人预测行人从当前位置至少两秒后的所述预测行人轨迹位置信息。机器人预测行人从当前位置至少两秒后的所述预测行人轨迹位置信息。参照图1，假设行人#1所在的位置为(p_x，p_y)，然后预测某一时间(比如2秒)后朝箭头方向移动，通过上述的线性速度模型公式计算出行人#1的预测坐标位置为

对于步骤S300的具体实施方式

参照图1，需要计算k时刻机器人当前路点与周围行人#1及行人#2的某一时间(比如2秒)的预测位置直线上的无碰撞的路点

使得从到当前的路点/>

到下个路点/>

的距离最小。机器人通过获取行人#1及行人#2两者的当前路点的行人位置数据的坐标值、行人速度数据的速度值及行人占地面积半径数据的面积值，输入线性速度模型公式，得出行人#1及行人#2两秒后预测的坐标位置。图1中机器人找寻到下一个无行人障碍的路点/>

机器人在前进过程中与行人#1及行人#2的实时位置保持人际距离。

具体地，在所述步骤S300中，通过以下方式构建所述机器人与行人的无碰撞距离：

d＝p_b+r_b+d_b

其中，p_b是行人的半径，r_b是机器人的半径，d_b是机器人与行人保持的距离，

优选地，所述人际距离包括社交空间距离及私人空间距离，所述社交空间距离不超过1.2m，所述私人空间距离不超过0.5m，

根据图3，当机器人行人密度逐渐增加的多个路点中移动时，所述机器人随着行人密度的增加(从0.2至0.8)，机器人与行人之间的保持的距离线性递减，不超过与行人的私人空间距离。图3中显示，行人的密度较小时，机器人与行人遵循距离私人空间距离，即不超过1.2m；行人的密度较大时，机器人与行人遵循距离私人空间距离，即不超过0.5m。

对于步骤S400的具体实施方式

由于行人步行行为总是最小化代谢能量。具体的，在社会行为学研究中表明，当人类在行走足够长的距离时，通常会以单位距离代谢成本最小的速度行走，它能够使得在代谢能量值固定的情况下行走距离最长，在社会行为学中，该代谢函数表示为，

E＝λ₀+λ₁p_v ²

其中，p_v是行人速度，λ₀和λ₁为权重参数，两个权重参数从行人行为统计数据集中估计得到。

同样地，机器人在移动足够长的距离时，为了让机器人移动行走模仿行人步行行为，通过以下方式构建机器人的代谢能量目标函数，计算机器人的代谢能量值：

f＝a₀+a₁v²+a₂ω²

其中，为了让机器人移动行走模仿行人步行行为，α₀、a₁、α₂是机器人的代谢能量目标函数的权重参数，v是机器人的线速度，ω是机器人的角速度，f是机器人的代谢能量值。

进一步地，所述步骤S400中：通过以下方式构建闭环控制模型，计算从当前路点到达下一个路点的轨迹数据：参照图1所示机器人要移动的两个路点，当前路点的坐标为

下一个路点的坐标为/>

其中，

分别表示ρ、α、φ的闭环误差向量，ρ是从当前路点的坐标/>

到下一个路点的坐标/>

之间的距离，α＝θ-θ_g，θ是当前路点的偏转角度值，θ_g是下一个路点的偏转角度值，K_ρ、K_α、K_φ是权重参数，参见图1，φ＝Φ-θ，φ表示检测模块的摇头角度，Φ表示检测模块的可视范围与X轴的夹角，θ也表示机器人的运动方向v与X轴的夹角。

如图1所示，最后的大黑点为机器人当前的目标路点，虚线黑色圆为机器人距离目标路点最近的无碰撞的位置。因此，在附图1示例中，选择Δ_g为当前最优方向。

具体地，所述步骤S400中：机器人在多个路点之间移动，对于移动过程中的多个路点，按照以下的方式进行轨迹优化：首先使K_ρ取一预设值，譬如K_ρ取3，通过公式

进行输入，通过优化计算找寻一组K_α、K_φ的值，使得生成一条从当前位置到点/>

的轨迹，然后根据机器人的代谢能量目标函数计算得到代谢能量值，多个路点之间移动的所述轨迹数据及所述代谢能量值依次迭代直到机器人的代谢能量值最小。参照图4，经过上述步骤过程，优化计算出图中具有多个离散点的最优轨迹及相应的K_α、K_φ的值。

上述的机器人基于行人行为决策规则的机器人局部轨迹规划方法，使移动机器人能够根据周围静态/动态场景特征，自主生成类人导航行为，便于周围行人理解和接受该移动机器人，在实现导航任务的同时，减少对交通效率和对周围行人的影响。

应当认识到，本发明实施例中的方法步骤可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机***通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合)可在配置有可执行指令的一个或多个计算机***的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还可以包括计算机本身。

计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

以上所述，只是本发明的较佳实施例而已，本发明并不局限于上述实施方式，只要其以相同的手段达到本发明的技术效果，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。在本发明的保护范围内其技术方案和/或实施方式可以有各种不同的修改和变化。

Claims (7)

1.一种基于行人步行决策规则的移动机器人自主导航方法，其特征在于，所述方法包括以下步骤：

S100、基于给定的目的地位置和机器人初始位置，获取无障碍物的全局路径，将全局路径分隔成多个路点，其中相邻的路点之间具有多个路点间距；

S200、对于每个路点通过检测模块获取多个行人信息，通过线性速度模型输入所述行人信息，得到预测行人轨迹位置信息，其中，所述行人信息包括行人位置数据、行人速度数据及行人占地面积半径数据；

S300、计算当前时间点的路点周围的每个行人轨迹位置信息，根据预测的行人轨迹位置信息，寻找与预测的行人位置信息直线距离最短的下一个无碰撞行人的路点，同时根据人际距离模型，在机器人移动过程中与行人保持人际距离；

S400、提供机器人的代谢能量目标函数及闭环控制模型，将机器人的线速度及角速度输入所述代谢能量目标函数，以输出机器人的代谢能量值，再将所述代谢能量值输入所述闭环控制模型，使达到下一个路点的轨迹数据输送到机器人的运动控制器，然后控制机器人移动到下一个路点；

其中，在所述步骤S400中，

通过以下方式构建机器人的代谢能量目标函数，计算机器人的代谢能量值：

f＝a₀+a₁v²+a₂ω²

其中，a₀、a₁、a₂是机器人的代谢能量目标函数的权重参数，v是机器人的线速度，ω是机器人的角速度，f是机器人的代谢能量值；

其中，在所述步骤S400中，

通过以下方式构建闭环控制模型，计算从当前路点到达下一个路点的轨迹数据：

其中，当前路点的坐标为

下一个路点的坐标为/>

其中，

分别表示ρ、α、φ的闭环误差向量，ρ是从当前路点的坐标/>

到下一个路点的坐标/>

之间的距离，α＝θ-θ_g，θ是当前路点的偏转角度值，θ_g是下一个路点的偏转角度值，K_ρ、K_α、K_φ是权重参数；

其中，在所述步骤S400中，

使K_ρ取一预设值，通过

优化计算找寻一组K_α、K_φ的值，使得生成从当前位置到点

的轨迹；

根据机器人的代谢能量目标函数计算得到代谢能量值，多个路点之间移动的所述轨迹数据及所述代谢能量值依次迭代直到机器人的代谢能量值最小。

2.根据权利要求1所述的方法，其中，所述步骤S100包括：

设置所述机器人的最大速度，所述路点间距通过以下公式计算：

Δd＝3×v_max

其中，v_max是机器人的最大移动速度上限，Δd是路点间距。

3.根据权利要求2所述的方法，其中，所述步骤S100包括：

将当前路点作为中间目标点，当机器人距离当前路点小于3m时，将下一个路点设置为下一个中间目标点，直到机器人移动到达给定的目的地位置。

4.根据权利要求1所述的方法，其中，所述步骤S200包括：

构建所述线性速度模型公式，计算预测行人轨迹信息，

其中，p_x、p_y是当前所述行人位置数据的坐标值，p_v是当前所述行人速度数据的速度值，p_θ是当前所述行人占地面积半径数据的面积值，

是所述预测行人轨迹信息的坐标值。

5.根据权利要求1所述的方法，其中，在所述步骤S300中，

通过以下方式构建所述机器人与行人的无碰撞距离：

d＝p_b+r_b+d_b

其中，p_b是行人的半径，r_b是机器人的半径，d_b是机器人与行人保持的距离，

所述人际距离包括社交空间距离及私人空间距离，所述社交空间距离不超过1.2m，所述私人空间距离不超过0.5m，

所述机器人随着行人密度的增加，机器人与行人之间的保持的人际距离线性递减，不超过与行人的私人空间距离。

6.根据权利要求1所述的方法，其中，所述步骤S200包括：

机器人预测行人从当前位置至少两秒后的所述预测行人轨迹位置信息。

7.一种计算机可读存储介质，其上储存有程序指令，所述程序指令被处理器执行时实施如权利要求1至6中任一项所述的方法。

Images