本发明涉及定位导航技术领域，尤其涉及一种服务机器人导航方法及系统。
背景技术：
服务机器人是机器人家族中的一个年轻成员，到目前为止尚没有一个严格的定义。不同国家对服务机器人的认识不同。
可以分为专业领域服务机器人和个人/家庭服务机器人，服务机器人的应用范围很广，主要从事维护保养、修理、运输、清洗、保安、救援、监护等工作。
ros是一个机器人软件平台，能为异质计算机集群提供类似操作系统的功能。起源于2007年，是一种分布式的处理框架，一种开源的机器人后操作系统。例如，硬件抽象描述、底层驱动程序管理、程序间消息的传递等。
gmapping定位算法主要应用了粒子滤波中的rbpf方法，扫描匹配的过程是首先估计机器人位姿，利用梯度下降的方法，以当前构建的地图、激光点和机器人位姿作为初始估计值，为了解决pf中的粒子耗散问题，提出了自适应重采样的机制，在一定程度上可以减少为保证定位精度所需的粒子个数。但是缺点在于依赖里程计，无法适用无人机及地面不平情况，且由于pf遵循马尔科夫假设，当前的状态只与前一时刻有关，所以无法进行回环。
hector定位算法主要是将激光点与已有的地图进行匹配，利用已经获得的地图对激光束点阵进行优化，估计激光点在地图中的表示，和占据网格的概率。利用高斯牛顿解决scan-match(scan-map)的问题，找到激光点集映射到已有地图的刚体转换；为避免局部最小而非全剧最小，使用多分辨率地图。缺点在于没有地图的修正，一旦地图出错，之后的匹配也会出问题，同样该算法没有回环检测。
没有回环检测会产生误差累积，不能及时消除误差累积，则会导致定位不准确的问题。
技术实现要素：
本发明要解决的技术问题目的在于提供一种服务机器人导航方法及系统，用以解决现有的导航技术没有回环测试定位会产生误差的问题。
为了实现上述目的，本发明采用的技术方案为：
一种服务机器人导航方法，包括步骤：
利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；
利用快速遍历随机树算法对构建的地图进行全局路径规划；
利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
进一步地，所述利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差的步骤具体包括：
建立闭环检测的子地图并赋予所述子地图的概率网格的网格点的概率值；
利用ceres-based匹配扫描信息点相对于当前子地图的位姿以找到在所述子地图的概率最大的扫描信息点集的位姿；
利用spa优化所有扫描信息点和子地图的位姿并将所述相对位姿进行存储；
采用分支界限和dfs搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
进一步地，所述赋予所述子地图的概率网格的网格点的概率值的方法具体包括：
判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(phits)))；
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(pmiss)))。
进一步地，所述利用快速遍历随机树算法对构建的地图进行全局路径规划的步骤具体包括：
通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
进一步地，所述利用局部避障的动态窗口算法对构建的地图进行局部路径规划的步骤具体包括：
对服务机器人的当前状态进行采样；
针对每个采样速度计算所述服务机器人预设时间后的行驶路线；
利用预设评价标准对多条行驶路线评分；
根据评分结果选择路径。
一种服务机器人导航系统，包括：
误差消除模块，用于利用cartographer算法通过闭环检测消除地图构建过程中产生的累积误差；
全局路径规划模块，用于利用快速遍历随机树算法对构建的地图进行全局路径规划；
局部路径规划模块，用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
进一步地，所述误差消除模块具体包括：
子地图建立单元，用于建立闭环检测的子地图并赋予所述子地图的概率网格的网格点的概率值；
局部匹配扫描单元，用于利用ceres-based匹配扫描信息点相对于当前子地图的位姿以找到在所述子地图的概率最大的扫描信息点集的位姿；
全局闭环优化单元，用于利用spa优化所有扫描信息点和子地图的位姿并将所述相对位姿进行存储；
累积误差消除单元，用于采用分支界限和dfs搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
进一步地，子地图建立单元具体用于判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；若所述网格点已赋值，则根据以下公式更新概率值：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(phits)))；
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(pmiss)))。
进一步地，所述全局路径规划模块具体包括：
引入单元，用于通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
进一步地，所述局部路径规划模块具体包括：
采样单元，用于对服务机器人的当前状态进行采样；
计算单元，用于针对每个采样速度计算所述服务机器人预设时间后的行驶路线；
评分单元，用于利用预设评价标准对多条行驶路线评分；
选择单元，用于根据评分结果选择路径。
本发明与传统的技术相比，有如下优点：
本发明基于cartographer算法通过闭环检测消除构图过程中的误差，使定位更加精确，减小误差。
附图说明
图1是实施例一提供的一种服务机器人导航方法流程图；
图2是实施例二提供的一种服务机器人导航系统结构图。
具体实施方式
以下是本发明的具体实施例并结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。
实施例一
本实施例提供了一种服务机器人导航方法，如图1所示，包括步骤：
s11：利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差；
s12：利用快速遍历随机树算法对构建的地图进行全局路径规划；
s13：利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
本实施例的cartographer算法是通过闭环检测来消除地图构图过程中产生的累积误差。用于闭环检测的基本单元是子地图。一个子地图是由一定数量的激光扫描构成，将一个激光扫描插入其对应的子地图时，会基于子地图已有的激光扫描及其他传感器数据估计其在子地图中的最佳位置。子地图在段时间内的误差足够小，然而随着时间的推移，误差累积会越来越大。
本实施例中，步骤s11为利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差。
通过闭环检测适当地优化子地图的位姿进而消除累积误差，将问题转化为位姿优化的问题。位姿优化问题即求解非线性的最小二乘问题。当一个子地图的构建完成时，也就是不会再有新的激光扫描插入到该子地图时，该子地图会加入闭环检测中。闭环检测会考虑所有的已完成创建的子地图。当一个新的激光扫描加入到地图中时，如果该激光扫描的估计位姿与地图中某个子地图的某个激光扫描的位姿比较接近，那么通过对应的激光匹配策略就会找到该闭环。该激光匹配策略通过在新加入地图的激光扫描的估计位姿附近取一个窗口，进而在该窗口内寻找该激光扫描的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。
本实施例中，步骤s11具体包括：
建立闭环检测的子地图并赋予子地图的概率网络的网格点的概率值；
利用ceres-based匹配扫描信息点相对于当前子地图的位姿以找到子地图的概率最大的扫描信息点集的位姿；
利用spa优化所有扫描信息点和子地图的位姿并将相对位姿进行存储；
采用分支界限和dfs搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
其中，赋予子地图的概率网格的网格点的概率值的步骤具体包括：
判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(phits)))；
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(pmiss)))。
具体的，算法流程可分为以下几个步骤：
a.建立子地图：每当一个扫描点集被插入这个概率网格中，一个包含网格点的命中集合以及一个未命中集合都会被重新计算。对于每一次命中，将这个最近的网格点加入命中集合，对于每一次未命中，插入与扫描起点和每个扫描点间的一条连线相交的每个像素相关联的网格点，已经在命中集合中的网格点不需要插入。对于每一个之前没有观察到的网格点都会根据他们所在集合是命中集合还是未命中集合赋予一个概率值phit或pmiss，如果一个网格点x已经被观察到了，将会通过以下的方式更新这个概率值：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(phits)))；
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(pmiss)))。
b.局部扫描匹配：在将一个扫描信息点集插入子地图中之前，这个扫描信息点集的位姿使用ceres-based的扫描匹配方法相对于当前的局部子地图进行优化。扫描匹配过程的目的是为了找到一个在子地图中的概率最大的扫描信息点集的位姿，将这一过程作为非线性最小二乘问题。
c.全局闭环优化：cartographer通过创建许多小的子地图来处理较大的空间。由于扫描信息只和几个理它最近的扫描信息的子地图匹配，所以局部扫描匹配过程会缓慢地累积误差。这里使用spa来优化。除了这些相对位姿外，当子地图不再改变时，所有其他由扫描信息和其对应的自赌徒组成的对都被人为是闭环的。slam后端运行扫描匹配过程，当找到一个很好的扫描匹配时，相应的相对位姿就被添加到优化问题中。
d.消除累积误差：采用分支界限和dfs搜索的策略进行回环的检测和相对位姿求解，以消除累积误差。相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程。而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的选择通过对子地图的与计算来完成，使用子地图这样的中间结构使得回环过程可以实时完成，从而通过回环不断调整子地图以消除累积误差。
本实施例中，步骤s12为利用快速遍历随机树算法对构建的地图进行全局路径规划。
其中，步骤s12具体包括：
通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
快速遍历随机树rrt是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离，用于有效地搜索非凸的高维度的空间，特别适用于包含障碍物和非完整系统或反向动力学微分约束条件下的动作规划。
rrt的特点是算法建构简单，并且可以快速遍历空间的未探索区域，而通过手动设定找到实现这种效果的函数却是很难的。同时rrt可以在基本算法基础上，对空间的搜索加入带有输入参数的控制函数，使算法可以适用于不同约束条件下的路径搜索。
人工势场法路径规划是由khatib提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。
本实施例使用改进的快速搜索随机树算法，具体包括：
a*算法是一种静态路网中求解最短路径最有效的直接搜索方法；
d*算法是动态的a*算法。
为了解决随机性问题，将人工势场中引力的思想引入rrt算法，引导随机树朝着目标方向生长，无需对全局环境进行建模，减少了规划时间，可表示为：
应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。
本实施例中，步骤s13为利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
其中，步骤s13具体包括：
对服务机器人的当前状态进行采样；
针对每个采样速度计算服务机器人预设时间后的行驶路线；
利用预设评价标准对多条行驶路线评分；
根据评分结果选择路径。
具体的，局部路径规划采用的是局部避障的动态窗口dwa算法。先对服务机器人当前状态进行采样，针对每个采样速度，计算服务机器人以当前速度行驶一段时间后的状态，得出一条行驶路线。再利用评价标准对多条路线进行打分，根据打分，选择分数最高的最优路径。继续重复上述过程，进行下一个局部路径规划。这样弥补了全局路径规划的缺点。使路径规划更加科学全面。
本实施例采用cartographer算法对服务器机器人进行精确定位和导航，避免了累积误差以及局部最优点的问题，使定位更加精准。
实施例二
本实施例提供了一种服务机器人导航系统，如图2所示，包括：
误差消除模块21，用于利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差；
全局路径规划模块22，用于利用快速遍历随机树算法对构建的地图进行全局路径规划；
局部路径规划模块23，用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
本实施例的cartographer算法是通过闭环检测来消除地图构图过程中产生的累积误差。用于闭环检测的基本单元是子地图。一个子地图是由一定数量的激光扫描构成，将一个激光扫描插入其对应的子地图时，会基于子地图已有的激光扫描及其他传感器数据估计其在子地图中的最佳位置。子地图在段时间内的误差足够小，然而随着时间的推移，误差累积会越来越大。
本实施例中，误差消除模块21用于利用cartographer算法通过闭环检测消除地图构图过程中产生的累积误差。
通过闭环检测适当地优化子地图的位姿进而消除累积误差，将问题转化为位姿优化的问题。位姿优化问题即求解非线性的最小二乘问题。当一个子地图的构建完成时，也就是不会再有新的激光扫描插入到该子地图时，该子地图会加入闭环检测中。闭环检测会考虑所有的已完成创建的子地图。当一个新的激光扫描加入到地图中时，如果该激光扫描的估计位姿与地图中某个子地图的某个激光扫描的位姿比较接近，那么通过对应的激光匹配策略就会找到该闭环。该激光匹配策略通过在新加入地图的激光扫描的估计位姿附近取一个窗口，进而在该窗口内寻找该激光扫描的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。
本实施例中，误差消除模块21具体包括：
子地图建立单元，用于建立闭环检测的子地图并赋予子地图的概率网络的网格点的概率值；
局部匹配扫描单元，用于利用ceres-based匹配扫描信息点相对于当前子地图的位姿以找到子地图的概率最大的扫描信息点集的位姿；
全局闭环优化单元，用于利用spa优化所有扫描信息点和子地图的位姿并将相对位姿进行存储；
累积误差消除单元，用于采用分支界限和dfs搜索策略进行闭环检测和相对位姿的求解以消除累积误差。
其中，自突入建立单元具体用于判断所述网格点是否已赋值，若否，根据所述网格点是否命中赋予命中概率值或未命中概率值；
若所述网格点已赋值，则根据以下公式更新概率值：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(phits)))；
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(pmiss)))。
具体的，算法流程可分为以下几个步骤：
a.建立子地图：每当一个扫描点集被插入这个概率网格中，一个包含网格点的命中集合以及一个未命中集合都会被重新计算。对于每一次命中，将这个最近的网格点加入命中集合，对于每一次未命中，插入与扫描起点和每个扫描点间的一条连线相交的每个像素相关联的网格点，已经在命中集合中的网格点不需要插入。对于每一个之前没有观察到的网格点都会根据他们所在集合是命中集合还是未命中集合赋予一个概率值phit或pmiss，如果一个网格点x已经被观察到了，将会通过以下的方式更新这个概率值：
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(phits)))；
mnew(x)＝clamp(odds-1(odds(mold(x))·odds(pmiss)))。
b.局部扫描匹配：在将一个扫描信息点集插入子地图中之前，这个扫描信息点集的位姿使用ceres-based的扫描匹配方法相对于当前的局部子地图进行优化。扫描匹配过程的目的是为了找到一个在子地图中的概率最大的扫描信息点集的位姿，将这一过程作为非线性最小二乘问题。
c.全局闭环优化：cartographer通过创建许多小的子地图来处理较大的空间。由于扫描信息只和几个理它最近的扫描信息的子地图匹配，所以局部扫描匹配过程会缓慢地累积误差。这里使用spa来优化。除了这些相对位姿外，当子地图不再改变时，所有其他由扫描信息和其对应的自赌徒组成的对都被人为是闭环的。slam后端运行扫描匹配过程，当找到一个很好的扫描匹配时，相应的相对位姿就被添加到优化问题中。
d.消除累积误差：采用分支界限和dfs搜索的策略进行回环的检测和相对位姿求解，以消除累积误差。相比传统的先检测回环，在求解相对位姿来说，显得结构上更为统一，将建立回环的过程转化为一个查找过程。而另一方面，当完成对离散候选解空间的树形构造后，在树中对解的查找过程速度很快，剩下的对于建树过程中的节点的选择通过对子地图的与计算来完成，使用子地图这样的中间结构使得回环过程可以实时完成，从而通过回环不断调整子地图以消除累积误差。
本实施例中，全局路径规划模块22用于利用快速遍历随机树算法对构建的地图进行全局路径规划。
其中，全局路径规划模块22具体包括：
引入单元，用于通过在人工势场法路径规划引入快速遍历随机树算法引导随机树朝目标方向生长；所述引入快速遍历随机树算法的公式如下：
快速遍历随机树rrt是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离，用于有效地搜索非凸的高维度的空间，特别适用于包含障碍物和非完整系统或反向动力学微分约束条件下的动作规划。
人工势场法路径规划是由khatib提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。
本实施例使用改进的快速搜索随机树算法，具体包括：
a*算法是一种静态路网中求解最短路径最有效的直接搜索方法；
d*算法是动态的a*算法。
为了解决随机性问题，将人工势场中引力的思想引入rrt算法，引导随机树朝着目标方向生长，无需对全局环境进行建模，减少了规划时间，可表示为：
应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。
本实施例中，局部路径规划模块23用于利用局部避障的动态窗口算法对构建的地图进行局部路径规划。
其中，局部路径规划模块23具体包括：
采样单元，用于对服务机器人的当前状态进行采样；
计算单元，用于针对每个采样速度计算服务机器人预设时间后的行驶路线；
评分单元，用于利用预设评价标准对多条行驶路线评分；
选择单元，用于根据评分结果选择路径。
具体的，局部路径规划采用的是局部避障的动态窗口dwa算法。先对服务机器人当前状态进行采样，针对每个采样速度，计算服务机器人以当前速度行驶一段时间后的状态，得出一条行驶路线。再利用评价标准对多条路线进行打分，根据打分，选择分数最高的最优路径。继续重复上述过程，进行下一个局部路径规划。这样弥补了全局路径规划的缺点。使路径规划更加科学全面。
本实施例采用cartographer算法对服务器机器人进行精确定位和导航，避免了累积误差以及局部最优点的问题，使定位更加精准。
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。