一种移动机器人自主导航方法与流程

1.本发明涉及机器人领域，特别是涉及一种移动机器人自主导航方法。


背景技术：

2.在过去几十年中，移动机器人导航技术日渐发展，使得移动机器人可以完成一些基本的任务。随着自动化技术的不断提升，人们对于移动机器人的要求也在不断提高。例如在完成任务途中的精度要求以及任务的完成度等各个方面都在不断的提高。
3.机器人在完成这些任务的先决条件之一是对周围的环境具有准确的感知能力，并且将环境在一定时间内存储起来，用于下一步的任务决策。这个过程一般来说就是建立机器人可以理解的地图。建图的准确性直接决定了机器人对于给定任务执行的准确性。通常的建图方法利用inverse sensor model方法建立传感器数据融合模型，从而避免复杂的计算。当数据量较大时，一般而言用四叉树或者八叉树存储数据。但是如果对于实时性要求较高的场景，这样的存储方法不一定能胜任。
4.因此，需要一种移动机器人自主导航方法，以解决上述问题。


技术实现要素：

5.本发明的目的在于提供一种移动机器人自主导航方法，传感器接受到的信息处理后利用数组缓存建立机器人可以理解的局部地图，利用字节的比特操作从而简化查询地图上的对应位置的花销，提高查询效率。
6.为解决上述技术问题，本发明提供一种移动机器人自主导航方法，包括步骤：
7.基于位操作的地图检索查询，将所述地图保存为在缓存数组里；
8.应用多模概率函数，在可行区域内近似逼近目标函数，寻找最优路径。
9.进一步的，通过接受外部数据建立地图，并更新机器人距离每一个障碍物的距离。
10.进一步的，所述外部数据为传感器数据。
11.进一步的，所述缓存数组为一长度为n的连续数组。
12.进一步的，将空间点的位置栅格化，得到一个感知点的整数指数x，定义一补偿指数作为机器人局部坐标系的开始位置，建立全局坐标到机器人局部坐标的转化关系。
13.进一步的，通过公式(1)得到所述传感器数据是否在地图上：
14.是否在地图上(x)＝0《＝x-o《n
ꢀꢀꢀꢀꢀ
(1)
15.其中，x为正整数，n为正整数，o为坐标系原点。
16.进一步的，如果所述传感器数据位于地图的缓存数组上，则通过公式(2)判断此数据点在所述缓存数组上的具体位置：
17.数组地址(x)＝(x-o)mod n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
18.其中，x为正整数，n为正整数，o为坐标系原点，mod为取余操作。
19.进一步的，位操作判断判断所述传感器数据的具体位置，通过公式(3)和公式(4)判断：
20.是否在地图上(x)＝！((x-o)&(～(2
p-1)))
ꢀꢀꢀꢀꢀꢀ
(3)
21.数组地址(x)＝(x-0)&(2
p-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
22.其中，x为正整数，n＝2
p
为数组长度，o为坐标系原点，～为取反操作。
23.进一步的，根据bresenham算法将所述传感器数据到数据点之间的位置进行相应的射线跟踪操作，更新地图。
24.进一步的，在进行射线跟踪时，起点选为传感器位置；
25.将所述传感器的数据点用高斯概率进行建模，在采集到的位置为其初始概率最大的位置，根据其周围采集点的状态，更新其周围一定范围内的数据点为目标点的概率；
26.选择概率最大的点为终点，并进行射线跟踪。
27.进一步的，所述周围采集点的状态包括是否可见，是否被遮挡。
28.进一步的，对每一帧的所有传感器的数据点进行更新，绘制出整个地图。
29.进一步的，所述地图构建完成后，更新机器人距离每一个障碍物的距离，包括步骤：
30.采用同样长度的缓存数组保存每个数据点对于最近障碍物的距离；
31.在每一次地图状态更新后，保存所有被标记为障碍物的点，将其距离障碍物的距离标记为0，并保存所有被重新标记为空闲的点，将其在缓存数组的值标记为无穷大；
32.从每个障碍物点开始，更新其周围点的距离；
33.如果空间点距离障碍物的距离比此前距离缓存数组中所保存的距离短，将此新点加入一个队列；
34.如此等队列里的点为空时，说明地图上所有的点更新完距离，此时地图的状态为可用。
35.进一步的，应用多模概率函数，在可行区域内近似逼近目标函数，寻找最优路径，包括步骤：
36.假设轨迹服从如下概率分布：
[0037][0038]
其中f代表一个递减函数，其中，而c则代表代价值，z则代表分割函数，ε是指一条特定的轨迹；
[0039]
通过重采样和重要性采样方法，将其转换一易采样的函数，多模分布函数用高斯混合模型表示如下：
[0040][0041]
通过高斯混合模型(6)，可以得到dc(ε)的近似值，进而得到代表最小代价值的轨迹ε。
[0042]
进一步的，进行平滑处理，包括步骤：
[0043]
采用b样条曲线来表示得到的轨迹，如下式所示：
[0044][0045]
其中，pi指第i个在时刻ti的控制点，b
i,k
是b样条曲线的基函数；
[0046]
定义代价函数如式(8)所示：
[0047]etotal
＝ec+eq+e
l
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0048]
其中，e
total
为定义代价函数，ec代表碰撞代价，eq代表了平滑性，e
l
表示机器人状态各阶导数的约束；
[0049]
利用样条曲线对路径进行逼近，将路径转换为轨迹，通过查找机器人当前时刻距离轨迹最近的点，计算出下一时刻机器人的控制量，控制机器人朝向目标点移动。
[0050]
与现有技术相比，本发明的有益效果主要体现在：提出了一种移动机器人已知定位的导航方法，结合了建图和路径规划的方法，使得机器人可以顺利的完成导航任务。此方法不仅提高了机器人对于周围环境的感知速度，并且同时也提高了机器人完成指定任务的成功率。实现了导航任务的规模化和自动化部署。
附图说明
[0051]
图1为本发明移动机器人自主导航方法的流程图；
[0052]
图2为本发明构建地图方法的流程图。
具体实施方式
[0053]
下面将结合示意图对本发明的移动机器人自主导航方法进行更详细的描述，其中表示了本发明的优选实施例，应该理解本领域技术人员可以修改在此描述的本发明，而仍然实现本发明的有利效果。因此，下列描述应当被理解为对于本领域技术人员的广泛知道，而并不作为对本发明的限制。
[0054]
在下列段落中参照附图以举例方式更具体地描述本发明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。
[0055]
在本发明中提出了一种移动机器人自主导航方法，包括步骤：
[0056]
基于位操作的地图检索查询，将所述地图保存为在缓存数组里；
[0057]
应用多模概率函数，在可行区域内近似逼近目标函数，寻找最优路径。
[0058]
具体的，通过接受外部数据建立地图，并更新机器人距离每一个障碍物的距离。其中，通过接受外部数据建立地图，并更新机器人距离每一个障碍物的距离。所述缓存数组为一长度为n的连续数组。
[0059]
本技术的主要内容是在移动机器人已知其定位的情况下，提出了一种自主导航方法，具体的，如图1所示，本方法包含两个步骤：首先通过接受传感器的数据建立机器人可以理解的地图，并更新机器人距离每一个障碍物的距离。其次利用地图信息，为下一步的运动规划出安全的可行路径。
[0060]
首先，请参考图2，构建地图：
[0061]
不同于其他的利用四叉树或者八叉树的数据类型建立地图的方法，此方法利用数组缓存来保存地图数据。机器人将接受传感器数据，通过将空间点的位置栅格化，得到一个感知点的整数指数x。缓存数组是一个长度为n的连续数组，代表机器人周围一定范围内的环境。为了将此缓存数组映射到机器人周围的局部坐标系，需要定义一个补偿指数，代表此局部坐标系的开始位置，从而建立从全局坐标到机器人局部坐标的转换关系。
[0062]
接下来为每一个传感器数据转换到局部坐标上，因为缓存数组的长度已知，通过
(1)可以得到该传感器数据是否在地图上。
[0063]
是否在地图上(x)＝0《＝x-o《n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0064]
其中，x为正整数，n为正整数，o为坐标系原点。如果数据点在地图数组缓存上，同时也可以通过(2)来判断此数据点应该在数组的具体位置上：
[0065]
数组地址(x)＝(x-o)mod n
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0066]
其中，x为正整数，n为正整数，o为坐标系原点，mod为取余操作。或者用位操作来进行以上两步：
[0067]
是否在地图上(x)＝！((x-o)&(～(2
p-1)))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0068]
数组地址(x)＝(x-o)&(2
p-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0069]
其中，x为正整数，n＝2
p
为数组长度，o为坐标系原点，～为取反操作。
[0070]
这样通过位操作可以对每一个传感器数据点快速的判断是否在地图上，如果在，则找到其在数组上的对应位置，然后根据bresenham算法将传感器到数据点之间的位置都进行相应的射线跟踪操作，从而更新地图。
[0071]
在进行射线跟踪的步骤时，起点选为传感器位置，而终点不同于一般的射线跟踪，并不是传感器数据点。将传感器的数据点用高斯概率进行建模。在采集到的位置为其初始概率最大的位置，根据其周围采集点的状态，包括是否可见，是否被遮挡等因素，更新其周围一定范围内的点为目标点的概率。而后选择概率最大的点为终点，并进行射线跟踪。这样，对每一帧的所有传感器的数据点进行更新，整个地图就可以顺利的更新完成。
[0072]
在得到新的地图之后，需要更新地图上每个点距离障碍物的新的距离。类似地图的状态更新，采用一个同样长度的数组缓存来保存每个点对于最近障碍物的距离。在每一次地图状态更新后，只需要保存所有被标记为障碍物的点，将其距离障碍物的距离标记为0，并保存所有被重新标记为空闲的点，将其在数组的值标记为无穷大。
[0073]
之后，从每个障碍物点开始，更新它周围的点的距离。如果有空间点距离障碍物的距离比此前距离数组中所保存的距离要短，那么将此新点加入一个队列。如此等队列里的点为空时，说明地图上所有的点均已更新完距离，此时地图的状态为可用。这样每次更新地图都随着机器人一起移动，然而所有的数据都保存在一个固定尺寸的数组缓存中。
[0074]
上述建图方式极大的提高了机器人查找地图上某一个空间点的时间，并且所产生的花销也并不复杂，意味着机器人可以在实时任务中完成这一步骤。当每次地图更新完成后，代表着机器人可以将其传给规划模块进行下一步的任务。
[0075]
其次，轨迹规划模块：
[0076]
对于移动机器人而言，在得到新的地图之后，轨迹规划的主要目的一般是寻找一条从起点到终点的无碰撞的安全路径。为了快速找到这条路径，可以为每条路径都指定一个代价值。这样，最终的问题就变为找到一个代价值最小的路径，使得机器人可以安全的运动到终点。
[0077]
接着，通过考虑平滑性，避障，以及时间最优等条件设计代价函数以便可以表示所有的路径。一般来说考虑多种条件的情况下所设计的代价函数在大部分情况下非凸。这样就会导致找到最优值一般来说相对困难，因为代价函数呈现出一定的多模态性。
[0078]
本发明考虑将代价函数的多模性转换为轨迹的密度函数，通过估计密度函数分布来近似代价函数的多模性。这样，通过近似一个多模分布函数，可以得到多条轨迹。这些轨
迹代表着最优轨迹的候选，通过梯度下降等优化技术，就可以得到最优的轨迹，此轨迹可以同时满足一些预设的约束。
[0079]
主要的问题就转为如何寻找这样的多模密度函数，使其可以代表轨迹。假设轨迹服从如下概率分布：
[0080][0081]
其中f代表一个递减函数，而c则代表代价值。z则代表一个分割函数，ε是指一条特定的轨迹。一般而言，无法直接采样这样的概率函数，因此可以通过重采样和重要性采样方法，将其转换一个比较容易采样的函数，最后多模分布函数就可以用一个高斯混合模型来表示如下：
[0082][0083]
通过采样容易的高斯混合模型(6)，我们可以得到dc(ε)的近似值。这样，就可以得到代表最小代价值的轨迹ε。
[0084]
接下来，为了使得机器人的运动更加的平稳，需要对轨迹进行更进一步的平滑处理。这是因为尽管通过密度估计和重采样技术，可以得到一个代价值最小的轨迹，但是这样的轨迹在某些情况下不能满足一些特定的约束。因此平滑处理是为了机器人平稳运动而必须的。此处采用b样条曲线来表示得到的轨迹，如下式所示：
[0085][0086]
其中，pi指第i个在时刻ti的控制点，b
i,k
是b样条曲线的基函数。在这个基础上，可以定义代价函数如式(8)所示：
[0087]etotal
＝ec+eq+e
l
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0088]
其中，e
total
为定义代价函数，ec代表碰撞代价，用于惩罚机器人与障碍物的碰撞，eq代表了平滑性，表示机器人运动状态的各阶导数都需要尽可能小，从而保证了机器人移动过程中的平滑性。而e
l
则代表了软约束，表示机器人的运动状态的各阶导数不得超过设定量。否则对其进行惩罚。这样就保证了轨迹的可用性。利用样条曲线对路径进行逼近。可以将路径转换为轨迹。通过查找机器人当前时刻距离轨迹最近的点，计算出下一时刻机器人的控制量。从而不断的控制机器人朝向目标点走去。
[0089]
本发明在建图时候采用数组缓存来存储数据，每次查询时候将只花费o(1)的时间，与传统的四叉树和八叉树相比将会大大的提升时间上的花销，与此同时，本发明也并没有降低数据修改时候的时间花销。
[0090]
在地图的问题解决之后，机器人的轨迹规划是导航任务的另一大难点。传统的方法是事先有了全局路径，在全局路径上每次截取一段进行局部的路径规划。这样在静态环境中一般而言不会有大问题，但是对于动态环境，这样会导致机器人忽略其他更好的路径，从而陷入局部最优中。本发明通过设计一种新的局部路径规划方法将会避免这一问题。从而保证了导航的鲁棒性。提高机器人完成任务的成功率。
[0091]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。