一种机器人室内导航系统自适应狭窄环境的方法

本发明涉及机器人室内导航的，尤其是指一种机器人室内导航系统自适应狭窄环境的方法。

背景技术：

1、室内环境的复杂性和障碍物的存在对机器人的导航技术提出了挑战。目前，机器人室内完全自主可靠地执行导航任务仍存在一些问题，需要解决的实际问题之一是，当在工作场景中遇到略宽于机身宽度的狭窄环境，如走廊、角落或门口时，机器人无法安全和稳定地通过。理想情况下，如果存在一条可行的全局路径，局部轨迹规划器能够找到合适的轨迹使机器人通过这些狭窄环境。然而，根据实际部署经验，机器人往往在本可通行的狭窄环境前因无法生成可行的轨迹而出现振荡、停止行为或在狭窄环境中陷入致命区域导致导航任务失败。

2、对此问题，探测和穿越门的任务被机器人界作为机器人应该能够通过的狭窄环境的一个特殊例子进行了大量的研究，但存在特定场景的局限性，即强制存在门或走廊(需要检测和识别特定的特征)，我们希望机器人应该安全地通过任务场景中的任何狭窄环境；通过调整运动控制中的避障权重等相关参数的方法能够使机器人进入狭窄环境，但该方法也增加了机器人的碰撞风险，同时会使其在其他环境下的导航效果变差；也有方法提出在狭窄通道两端分别设置辅助导航点，帮助机器人以更好的姿态进入通道以提高通行概率，然而辅助导航点需要提前在地图中手工标定，无法实现机器人完全自主地通过狭窄环境。

技术实现思路

1、本发明的目的在于克服现有技术的不足，提供了一种机器人室内导航系统自适应狭窄环境的方法，使得机器人在无人为干涉的情况下通过自动切换导航策略以适应狭窄环境，实现提高机器人自主通过宽度略大于机身宽度的狭窄环境中的成功率与流畅程度的效果。

2、为实现上述目的，本发明所提供的技术方案为：一种机器人室内导航系统自适应狭窄环境的方法，包括：

3、导航策略信息生成环节：在机器人执行室内导航任务的过程中，收集由环境狭窄导致的导航失败位姿nfp，生成与nfp对应的合适通行位姿对stps，形成导航策略信息以供导航策略切换环节使用；

4、导航策略切换环节：在机器人执行室内导航任务的过程中，根据导航策略信息生成环节提供的导航策略信息判断切换狭窄环境导航策略的时机并执行该策略，包括切换狭窄环境导航参数配置与执行多点导航任务以辅助机器人在狭窄环境中安全通行，在通过狭窄环境后切换为正常导航策略，包括还原正常的导航参数配置与恢复原导航任务。

5、进一步，该方法所辅助的室内导航系统为ros机器人操作系统下的navigationstack导航系统，由地图服务模块、环境感知模块、自主定位模块、路径规划模块和运动控制模块这五个模块构成；所述室内导航系统的工作流程为：所述地图服务模块加载全局成本地图，所述环境感知模块通过机器人的传感器获取周围环境的数据，所述自主定位模块根据环境感知模块获取的数据按设定频率计算机器人当前的位姿，当接收到目标导航点时，开始执行导航任务：所述路径规划模块中的全局路径规划器按设定频率根据全局成本地图计算由当前位姿到目标导航点的最优全局路径；所述运动控制模块根据环境感知模块获取的数据更新局部成本地图，其中的局部轨迹规划器则根据局部成本地图按设定频率计算尽可能符合全局路径的轨迹并发布相应速度控制指令以控制机器人按轨迹行动；上述五个模块联合工作直到机器人到达目标导航点。

6、进一步，所述导航策略信息是由n个nfp及根据其生成的stps构成的集合，表示为setstrategy＝{(nfpi,stpsi)}i＝1,2,3,...,n，其中，nfpi表示找到的第i个导航失败位姿，stpsi表示根据nfpi生成的合适通行位姿对，stps由合适通行起点位姿stpstart和合适通行终点位姿stpend构成，表示为stps＝(stpstart,stpend)，其中位姿(x,y,θ)由机器人在地图中的横坐标x、纵坐标y和方向θ构成。

7、进一步，所述导航策略信息生成环节通过导航恢复行为实现，具体情况如下：

8、1.1)预先设定导航恢复行为的触发时机，有以下三种情况：全局路径规划连续失败、局部轨迹规划连续失败和机器人发生在两个位置之间来回移动的振荡现象；

9、1.2)当机器人在室内导航任务过程中出现步骤1.1)中所述三种情况之一时暂停导航任务，执行导航恢复行为：执行合适通行位姿对生成方法，当成功生成stps时则认为当前位姿为由于环境狭窄导致的导航失败位姿，并将nfp与stps写入导航策略信息，并将更新后的导航策略信息加载至导航策略切换环节中，随后继续尝试执行原导航任务；当生成stps失败时则认为机器人发生步骤1.1)中所述三种情况并非由狭窄环境导致的，导航任务失败。

10、进一步，在步骤1.2)中，所述合适通行位姿对生成方法参考当前的全局成本地图与位姿，尝试在前方可能存在的狭窄环境的入口处生成stpstart，出口处生成stpend，其中stpstart和stpend靠近道路中间，具体步骤如下：

11、1.2.1)导航参数中的全局成本地图的膨胀半径修改为预设定的狭窄阈值γnarrow，并调用全局路径规划器重新规划一条由当前位姿到目标导航点的含有m个路径点的全局路径path＝{p′j}j＝1,2,3,...,a,...,b,...,m，其中p′j表示全局路径中第j个路径点的位姿，重新规划出的全局路径path将贴近狭窄环境相对两侧障碍靠中的区域；

12、1.2.2)使用b样条曲线拟合方法优化path，得到更平滑的全局路径pathopt＝{pj}j＝1,2,3,...,a,...,b,...,m，其中pj表示优化后的全局路径中第j个路径点的位姿；

13、1.2.3)计算setwidth＝{widthj}j＝1,2,3,...,a,...,b,...,m，其中setwidth为pathopt中路径点的宽度的集合，widthj是pathopt中的pj所处环境的宽度，计算方式为寻找widthj的第一定义点oj1与第二定义点oj2，并计算该两点之间的距离：在全局成本地图中，以地图分辨率rmap为半径步长和初始半径，γnarrow为最大搜索半径，pj为圆心向外一步步扩展圆形范围来搜索障碍点，搜索到第一个障碍点后停止搜索，并将该点作为widthj的第一定义点oj1，若最大搜索半径的圆形范围内搜索不到障碍点，则将widthj置为最大值max；搜索到第一定义点oj1后，沿向量方向以rmap为步长搜索第一个遇到的障碍点，并将该点作为widthj的第二定义点oj2；计算其中和分别表示oj1的x坐标和y坐标，和分别表示点oj2的x坐标和y坐标；

14、1.2.4)setwidth计算完毕后，按序搜索setwidth：当搜索到第a个宽度widtha首次满足widtha＜γnarrow时，将widtha的第一定义点oa1和第二定义点oa2位置的中点作为stpstart的位置，垂直于向量且朝向向量的方向作为stpstart的方向；找到widtha后从setwidth的第a+1项开始搜索，当搜索到第b个宽度widthb首次满足widthb＞γnarrow时，将pb的位置作为stpend的位置，垂直于向量且朝向向量的方向作为stpend的方向，其中ob1是widthb的第一定义点；若setwidth搜索完毕后未能找到满足条件的widtha或widthb，则认为合适通行位姿对stps生成失败；

15、1.2.5)若stps生成成功，则根据全局成本地图对其位置进行局部优化：以stpstart为圆心，向量所在直线为分界，stpstart的反方向为朝向，机器人底盘半径rrobot为半径形成半圆形搜索范围，在该范围中搜索成本最小的点，搜索完毕后找到n′个满足条件的点ci′形成点集setcost＝{ci′}i′＝1,2,3,...,n′，依次计算向量的方向与的角度差，其中是stpstart的方向，将使该角度差最小的点的位置作为stpstart优化后的位置；以stpend为圆心，向量所在直线为分界，stpend的方向为朝向，rrobot为半径形成半圆形搜索范围，在该范围中搜索成本最小的点，搜索完毕后找到n”个满足条件的点ci”形成点集setcost′＝{ci”}i”＝1,2,3,...,n”，依次计算向量的方向与的角度差，其中是stpend的方向，将使角度差最小的点的位置作为stpend优化后的位置。

16、进一步，所述导航策略切换环节执行的具体步骤如下：

17、2.1)机器人执行室内导航任务时按预设定频率ratejudg执行狭窄环境进入判断方法；

18、2.2)当狭窄环境进入判断方法的判断结果为真时，暂停狭窄环境进入判断方法，导航策略切换为狭窄环境导航策略，该策略包括：导航参数切换为狭窄环境导航参数配置，原导航任务更改为按序导航至对应于判断结果的导航失败位姿的合适通行起点位姿、合适通行终点位姿的多点导航任务；

19、2.3)到达合适通行位姿对终点后，导航策略切换为正常导航策略，该策略包括：导航参数切换为正常导航参数配置，恢复原导航任务，即从当前位姿导航至原目标导航点，并继续按ratejudg频率执行狭窄环境进入判断方法。

20、进一步，在步骤2.1)中，所述狭窄环境进入判断方法是根据当前位姿与导航失败位姿的距离以及当前全局路径与导航失败位姿对应的合适通行位姿对的匹配程度来判断机器人是否进入狭窄环境，具体步骤如下：

21、2.1.1)依次计算导航策略信息setstrategy中所有导航失败位姿nfp与当前位姿的距离，并将该距离小于预设定阈值γdist这一条件的nfp加入候选点集setcandidate＝{nfpk}k＝1,2,3,...,q，其中nfpk表示满足上述条件的第k个nfp，q表示一共存在q个满足上述条件的nfp；

22、2.1.2)依次计算setcandidate中每个nfpk对应的合适通行位姿对stpsk与含有m′个路径点的当前全局路径的匹配度dk，其中表示pathcur中的第个路径点，计算方法为：i)按序在pathcur中寻找首个满足与距离小于且角度差小于的路径点分别记该距离和角度差为和其中表示stpsk对应的合适通行起点位姿；若pathcur中的路径点均不满足i)中描述的条件，则匹配度dk＝0；ii)若存在则从开始继续寻找首个满足与距离小于且角度差小于的路径点其中表示stpsk对应的合适通行终点位姿，分别记该距离和角度差为和若pathcur中的路径点均不满足ii)中描述的条件，则匹配度dk＝0；iii)若与均存在，则

23、2.1.3)找到最大的匹配度de＝max{dk}k＝1,2,3,...,q，若de＞0，则判断结果为真，并返回(nfpe,stpse)，其中stpse是用于计算最大匹配度de的合适通行位姿对，nfpe是stpse对应的导航失败位姿，其它情况判断结果均为假。

24、进一步，在步骤2.2)中，所述狭窄环境导航参数配置的具体情况如下：

25、狭窄环境导航参数配置包括局部轨迹规划器参数配置以及全局成本地图参数配置；狭窄环境导航参数配置将最小障碍距离减小为机器人底盘半径rrobot、减小轨迹可行性检查的采样点密度使局部轨迹规划器的碰撞检测更宽松，将全局成本地图的膨胀半径设置为γnarrow、考虑全局路径子集最大长度减小至1米以内使轨迹生成更靠道路中间，将线速度及线加速度减小，角速度及角加速度增大使机器人在狭窄环境中调整角度更灵敏。

26、本发明与现有技术相比，具有如下优点与有益效果：

27、1、本发明不依赖于特定特征的检测来识别门或走廊等特殊狭窄环境，适用于场景中的任何狭窄环境，具备更好的泛用性。

28、2、本发明通过导航策略信息生成环节实现了机器人根据环境信息自动生成狭窄环境出入口对应的合适通行位姿对，相比于手工标记的用于辅助机器人通行狭窄环境的额外导航航路点具备更优秀的精确性与可行性，且省去了手工标记的步骤。

29、3、本发明通过导航策略切换环节实现了机器人在导航过程中通过改变导航策略自动适应狭窄环境的功能，且能够通过更窄的环境。

30、4、本发明方法使用更宽松的碰撞检测标准的狭窄环境导航参数配置，使得机器人在遇到狭窄环境时能够忽略地图精度误差、传感器噪声等误差源导致的碰撞误判，且行驶得更顺畅，而该参数配置带来的碰撞概率上升的风险则由机器人经过狭窄环境时生成靠道路中间的局部轨迹以及遵循合适通行位姿对进行分段导航来规避；真实环境下的实验结果表明，机器人使用本发明方法导航通过狭窄环境的成功率相比于单纯的参数调整与手工标记辅助导航点等方法有了显著提高。