机器人路径规划方法及规划路线的机器人与流程

本发明涉及机器人领域，具体而言，涉及一种机器人路径规划方法及规划路线的机器人。

背景技术：

随着计算机技术、传感器技术和人工智能等技术的快速发展，机器人技术也变得日趋成熟，而其中的移动机器人类型应用最为广泛，在家用服务、航天、工业等众多的行业中扮演着越来越重要的角色，这些各种各样的机器人能够在特定环境下很好地完成工作。

但是目前的机器人还是存在很多不足之处。因为大多数情况下机器人的工作环境是未知或者不确定的，但是目前的机器人还不能很好地完成在陌生的环境中完成指定路线。

技术实现要素：

有鉴于此，本发明实施例的目的在于提供一种机器人路径规划方法及规划路线的机器人。

本发明实施例提供的一种机器人路径规划方法，该方法包括：根据机器人的位姿和里程计的控制信息获取所述机器人的初始位姿；

根据所述初始位姿及传感器观测到的数据创建先验地图；

根据自适应调整的快速扩展树算法在所述先验地图计算出控制所述机器人行走的全局路径；

获取机器人的传感器的实时探测数据，判断在机器人的行走过程中，传感器是否探测到所述全局路径中的障碍物；

当传感器探测到所述全局路径中的障碍物时，建立动态窗口模型规划该机器人的局部避障路线，以控制机器人根据该局部避障路线避开障碍物。

本发明实施例还提供一种规划路线的机器人，所述机器人包括：存储器、处理器及存储在所述存储器中，由处理器处理的路线规划装置；

所述路线规划装置包括：

初始位姿获取模块，用于根据机器人的位姿和里程计的控制信息获取所述机器人的初始位姿；

先验地图创建模块，用于根据所述初始位姿及传感器观测到的数据创建先验地图；

全局路线计算模块，用于根据自适应调整的快速扩展树算法在所述先验地图计算出控制所述机器人行走的全局路径；

障碍物探测模块，用于获取机器人的传感器的实时探测数据，判断在机器人的行走过程中，传感器是否探测到所述全局路径中的障碍物；

避障路线规划模块，用于当传感器探测到所述全局路径中的障碍物时，建立动态窗口模型规划该机器人的局部避障路线，以控制机器人根据该局部避障路线避开障碍物。

与现有技术相比，本发明的机器人路径规划方法及规划路线的机器人。在较复杂环境下所述机器人在基于自适应调整的快速扩展树算法搜索出全局最优路径，从而保证了全局路径最优，同时对于复杂环境和动态障碍物环境结合动态窗口模型，使所述机器人的方向明确，搜索高效，可以有效提高机器人自主避障能力，安全到达目标点。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明较佳实施例提供的机器人的方框示意图。

图2是本发明提供的机器人的路线规划装置的功能模块示意图。

图3本发明较佳实施例提供的机器人路径规划方法的流程图。

图4是图3中的步骤S105的详细流程图。

图5为传感器获取周围环境信息并建立局部环境模型示意图。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

如图1所示，是机器人100的方框示意图。所述机器人100包括路线规划装置200、存储器102、存储控制器104、处理器106、外设接口108、定位模块110及传感器112。

所述存储器102、存储控制器104、处理器106、外设接口108、定位模块110及传感器112各元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。所述路线规划装置200包括至少一个可以软件或固件(firmware)的形式存储于所述存储器中或固化在所述机器人100的操作系统(operating system，OS)中的软件功能模块。所述处理器106用于执行存储器中存储的可执行模块，例如所述路线规划装置200包括的软件功能模块或计算机程序。

其中，存储器102可以是，但不限于，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-Only Memory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，存储器102用于存储程序，所述处理器106在接收到执行指令后，执行所述程序，本发明实施例任一实施例揭示的流过程定义的机器人100所执行的方法可以应用于处理器106中，或者由处理器106实现。

处理器106可能是一种集成电路芯片，具有信号的处理能力。上述的处理器106可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(DSP)、专用集成电路(ASIC)、现场编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述外设接口108将各种输入/输入装置耦合至处理器以及存储器102。在一些实施例中，外设接口108，处理器106以及存储控制器104可以在单个芯片中实现。在其他一些实例中，他们可以分别由独立的芯片实现。

所述定位模块110用于接收GPS卫星播报的定位信号，并根据定位信号计算出自身的位置。上述位置例如可用经度、纬度以及海拔高度来表示。可以理解，实现定位的方式并不限于GPS系统。例如，其他可用的卫星定们系统还包括北斗卫星定位系统(Compass Navigation Satellite System，CNSS)或者格洛纳斯系统(Global Navigation Satellite System，GLONASS)。此外，定位也并不限于采用卫星定位技术，例如，还可采用无线定位技术，例如基于无线基站的定位技术或者无线热点的定位技术。此时，定位模块110可被替换成相应的模块，或者直接经由处理器106执行特定的定位程序来实现。

所述传感器112是一种检测装置，能感受到被测量的信息，并能将感受到的信息，按一定规律变换成为电信号或其他所需形式的信息输出，以满足信息的传输、处理、存储、显示、记录和控制等要求。

如图2所示，图2是本发明提供的机器人100的路线规划装置200的功能模块示意图。所述路线规划装置200包括：初始位姿获取模块210、先验地图创建模块220、全局路线计算模块230、障碍物探测模块240及避障路线规划模块250。其中，所述避障路线规划模块250包括：环境模型建立单元251、局部路径计算单元252及环境模型更新单元253。本实施例中的所述路线规划装置200中的各个模块或单元用于执行机器人100路径规划方法中的各个步骤，下面将结合图3及机器人100路径规划方法实施例详细描述各个模块及单元的功能。

请参阅图3，是本发明较佳实施例提供的应用于图1所示的机器人100的机器人100路径规划方法的流程图。下面将对图3所示的具体流程进行详细阐述。

步骤S101，根据机器人100的位姿和里程计的控制信息获取所述机器人100的初始位姿。步骤S101可由上述的初始位姿获取模块210执行。

步骤S102，根据所述初始位姿及传感器观测到的数据创建先验地图。步骤S102可由上述的先验地图创建模块220执行。

步骤S103，根据自适应调整的快速扩展树算法在所述先验地图计算出控制所述机器人100行走的全局路径。步骤S103可由上述的全局路线计算模块230执行。

快速扩展树(Rapidly-exploring Random Tree，RRT)算法为通过引入一个目标引力函数，促使扩散树随机朝目标点方向生长的算法。

全局路线计算模块230可通过所述快速扩展树算法构建全局路径对应的路径生长树T，所述路径生长树T中含有n个节点，T＝{x_i},i＝1,2,...,n，选取x_i为生长点，其中定义ε为探索步长，f_i为节点x_i探索失败的次数，即如果节点探索成功，记f_i为1；如果节点探索失败一次，则f_i＝f_i+1，

定义δ_j为节点x_j的影响因子为：

其中，r_ij＝x_j-x_i表示当前节点x_i与其余节点x_j(x_j∈T)的距离，

w_j＝δ_j/D_j表示节点x_j的权值，

D_j＝x_j-x_rand表示节点x_j与随机节点x_rand间的距离；

其中，权值w_j的大小与影响因子δ_j成正比，与其余节点x_j与随机节点x_rand间的距离D_j成反比，所以为使搜索树朝最有利方向进行，根据权值大小选取生长点。

在规定步长范围下以所述机器人100最大转角θ_max为限制条件计算探索点公式如下：

其中，表示未知区域随机产生的随机目标点；

表示所述机器人100当前位置；

θ_k表示所述当前位置到的运动方向改变量；

θ_max表示步长范围内机器人100最大转角；

由计算探索点公式可知：当机器人100当前位置到的距离小于步长ε时，如果运动方向改变量θ_k小于等于最大转角时θ_max，探索点即可直接设置为如果航向改变量θ_k大于最大转角时θ_max时，探索点即以最大转角限制探索点范围。如果当前位置到的距离大于步长ε时，且航向改变量θ_k大于最大转角时，即此时以最大转角限制探索点范围，即

以当前探索点距目标的距离为启发信息，计算每个探索点到目标点的直线距离J_k：选择距离目标点最近的探索点为最优节点加入路径生长树。

首先对标准快速扩展树算法流程生长阶段和探索过程进行改进。在生长阶段，选取生长点时引入衡量节点探索失败次数的影响因子，影响因子的大小由于考虑了随机方向点和树节点之间的距离和探索失败次数，故可实现自适应调整生长阶段节点的生长权值，从而保证搜索树朝着最有利的方向生长。与此同时，在探索过程中，结合机器人100自身运动约束，以机器人100自身最大转角限制探索方向，保证规划路径实际安全性，最后以当前节点到目标点的距离作为启发因子，使得搜索出的路径分支节点少、路径更平滑。

步骤S104，获取机器人100的传感器的实时探测数据，判断在机器人100的行走过程中，传感器是否探测到所述全局路径中的障碍物。步骤S104由所述障碍物探测模块240执行。

步骤S105，当传感器探测到所述全局路径中的障碍物时，建立动态窗口模型规划该机器人100的局部避障路线，以控制机器人100根据该局部避障路线避开障碍物。步骤S105由所述避障路线规划模块250执行。

如图4所示，所述步骤S105具体包括以下步骤S1051、S1052以及S1053。

步骤S1051，通过所述传感器探测环境信息，建立感知环境模型。步骤S1051由上述的环境模型建立单元251执行。

所述环境信息可由所述传感器探测得到，并且每一个障碍物300到所述机器人100相对距离可由所述传感器计算得到，图5为传感器获取周围环境信息并建立局部环境模型示意图。在图5的环境模型示意图中，假设原点处为机器人100的位置坐标(x_R,y_R)，机器人100前进方向设为θ_R，r_R为机器人100自身大小半径。假设激光传感器扫描范围为180°，频率为1Hz，每秒扫描间隔为1°，故可得到181个扫描点。

将所述传感器扫描的扫描点以逆时针标记，从x轴正向开始，分别为l₁,l₂,…,l_i,…,l₁₈₁，其中扫描点l_i用极坐标形式表示，即以扫描角和对应的测量距离d_i来表示，所述扫描点l_i的公式为：

其中，i＝1,2,…,181，传感器测量的最大距离为d_max，当d_i<d_max时，即被判定为一个障碍物，为l_i相对于x轴的角度，当时，l_i与机器人100同向；

基于上述对激光传感器环境建模分析知，利用传感器探测到的局部环境信息，设置一个半径大小为r_min，可根据环境信息动态调整(r_min≤r_win≤d_max，r_min大于安全半径r_saf)的动态自适应规划窗口，因此可在半径为r_win的动态半圆窗口中将t时刻测距范围内探测到的局部环境信息中表示出来，其中窗口大小为win(r_win(t))＝{l₁,l₂,…,l_i,…l₁₈₁}，其中扫描点l_i被重新定义为：

其中，ρ表示重新定义后的测量距离。

步骤S1052，根据所述全局路径中的目标点位置及预设的局部路径算法计算得到局部优化路径。步骤S1052由上述的局部路径计算单元252执行。

局部路径计算单元252可根据所述机器人100可实现的速度集及探测到的障碍物300计算出所述机器人100的局部优化速度。

其中，所述机器人100的运动轨迹的每一步都包含一个速度向量(v,ω)，v表示所述机器人100平移速度，ω表示机器人100旋转速度，探测出的局部窗口中的环境障碍物300将限制平移速度和旋转速度。

所述机器人100最大的许可速度由机器人100到下一障碍物的距离dist(v,ω)可以确定，所述许可速度为所述机器人100以该许可速度运行能够及时停止防止碰撞的速度；其中，和表示刹车用加速度和角加速度，许可速度集V_a定义如下：

以t表示时间间隔，和表示平移加速度和角加速度，(v_a,ω_a)表示真实速度，局部滚动优化速度集V_d可定义为：

步骤S1053，根据所述局部优化路径更新所述感知环境模型。步骤S1053由上述的环境模型更新单元253执行。

确定所述局部滚动优化速度集V_d后，环境模型更新单元253可从中选取一个速度，通过目标函数进行评价，实时进行反馈校正；所述目标函数定义如下：

G(v,ω)＝σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω))；

其中，所述heading(v,ω)为用于测量机器人100的运动方向与目标点的一致性的变量，其中所述heading(v,ω)的值为1-θ/π，θ为目标点与机器人100运动方向的夹角，所述θ通过所述机器人100的预测位置进行计算，预测位置为所述机器人100以当前速度继续运行一个时间间隔，并以最大减速度停下的位置；

所述dist(v,ω)表示圆弧到最近障碍物300的距离，如果没有障碍物300在圆弧上，则设为一个较大的常数；

所述velocity(v,ω)＝v/v_max表示相应轨迹上机器人100的前进程度。

所述α、β和γ用来对所述heading(v,ω)、dist(v,ω)及velocity(v,ω)＝v/v_max进行加权。其中可以通过改变所述α、β和γ的值实现修正机器人100的运动轨迹的修正。

根据本实施例中的方法，在较复杂环境下所述机器人100在基于自适应调整的快速扩展树算法搜索出全局最优路径，从而保证了全局路径最优，同时对于复杂环境和动态障碍物环境结合动态窗口模型，使所述机器人100的方向明确，搜索高效，可以有效提高机器人100自主避障能力，安全到达目标点。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。