CN113238247A - 基于激光雷达的机器人定位导航方法、装置及设备 - Google Patents

Abstract

本申请公开了基于激光雷达的机器人定位导航方法、装置及设备，属于机器人导航领域。该方法包括根据机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据；将有效激光数据与原始环境地图进行匹配融合，构建出更新环境地图；利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置；计算机器人在优化地图中从当前位置到目标位置的规划路径，将规划路径进行离散化和优化，得到路径离散点集合，并计算其执行速度，机器人按照路径离散点集合和执行速度行驶。本申请通采用图优化算法以及规划路径，极大的提高了地图更新的求解效率，使得机器人可以遇到障碍物时可以大尺度的绕行，避免机器人失去控制。

Description

基于激光雷达的机器人定位导航方法、装置及设备

技术领域

本申请涉及机器人导航领域，特别涉及一种基于激光雷达的机器人定位导航方法、装置及设备。

背景技术

目前市场上现有的机器人定位导航，大都按照反光板或者预设磁条等固定路线行驶。近来出现的自主定位导航，大部分也都是严格按照预设的固定轨迹行驶，当在前进路线上出现了随机障碍时，则处理比较困难，比较稳妥的方式是采用停车方式，等到随机干扰消失则继续沿原来预设的路线继续行驶。这种方法最大的问题在于，不能随机应变，不能保证运行效率。

另外，在构建的地图中，如果出现随机障碍，地图更新也会变成了一种计算困难。把障碍理解成噪声还是地图更新，会影响机器人的行进路线。当机器人需要暂停或者进行较大弧度的绕行时，机器人很容易发生路径的偏离计算或者地图的更新计算而导致机器人最终失去控制的情况。

发明内容

针对现有技术存在的机器人地图更新困难，机器人导航时容易失去控制的问题，本申请主要提供一种基于激光雷达的机器人定位导航方法、装置及设备。

为了实现上述目的，本申请采用的一个技术方案是：提供一种基于激光雷达的机器人定位导航方法，其包括：根据预先确定的机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据；

将有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行修复检测，构建出更新环境地图；

利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置；

计算机器人在优化地图中从当前位置到目标位置的规划路径，将规划路径进行离散化和优化，得到路径离散点集合，并计算路径离散点集合的执行速度，机器人按照路径离散点集合和执行速度进行行驶。

本申请采用的另一个技术方案是：提供一种基于激光雷达的机器人定位导航装置，其包括：本体噪声滤除模块，其用于根据预先确定的机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据；

地图构建模块，其用于将有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行修复检测，构建出更新环境地图；

地图优化模块，其用于利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置；

导航模块，其用于计算机器人在优化地图中从当前位置到目标位置的规划路径，将规划路径进行离散化和优化，得到路径离散点集合，并计算路径离散点集合的执行速度，机器人按照路径离散点集合和执行速度进行行驶。

本申请采用的另一个技术方案是：提供一种电子设备，其包括处理器和存储器，存储器存储有计算机指令，该计算机指令被操作以执行方案一中的基于激光雷达的机器人定位导航方法。

本申请的技术方案可以达到的有益效果是：本申请设计了一种基于激光雷达的机器人定位导航的方法、装置及设备。该方法通过机器人的本体结构，从而消除本体结构噪声，保障了数据的真实性；采用图优化算法，极大的提高了地图更新的求解效率；路径规划导航，遇到障碍物可以大尺度的绕行，避免机器人失去控制。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本申请一种基于激光雷达的机器人定位导航方法的一个具体实施方式的示意图；

图2是本申请一种基于激光雷达的机器人定位导航方法的一个具体实施例的示意图；

图3是本申请一种基于激光雷达的机器人定位导航装置的一个具体实施方式的示意图；

图4是本申请一种基于激光雷达的机器人定位导航装置的一个具体实施例的示意图。

通过上述附图，已示出本申请确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本公开构思的范围，而是通过参考特定实施例为本领域技术人员说明本申请的概念。

具体实施方式

下面结合附图对本申请的较佳实施例进行详细阐述，以使本申请的优点和特征能更易于被本领域技术人员理解，从而对本申请的保护范围做出更为清楚明确的界定。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

近年来出现的自主定位导航，大部分是严格按照预设的固定轨迹行驶，当在构建的地图中的前进路线上出现了随机障碍时，地图更新会变成一种计算困难。例如，把障碍物理解成噪声，并进行地图更新，这会影响机器人的行进路线。当机器人需要暂停或者进行较大弧度的绕行时，机器人很容易发生路径的偏离计算或者地图的更新计算而导致机器人最终失去控制的情况。

本申请提供的基于激光雷达的机器人定位导航方法适用场景为：在地图建立过程中，如果没有对图进行优化，则机器人按照预设的轨迹行驶过程中遇到随机出现的障碍物，会变得难以处理，因为激光雷达得到的只是局部环境的点云数据，所以此时如果暂停或者采用大弧度的绕行，会出现导致机器人最终失去控制。因此在建图的过程中应对地图进行优化，让机器人能轻易绕过障碍。

本申请的发明构思是：检测出机器人的本体结构，根据本体结构可以从数据层面上消除掉激光雷达检测到的数据中的噪声，为后续建图、图优化和导航奠定了真实数据基础；采用图优化，可以将无关边的雅可比矩阵视作零矩阵，海森矩阵视作稀疏矩阵，因此可以极大的提高求解效率；路径规划导航，使得机器人遇到障碍物可以大尺度的绕行，避免机器人失去控制。

在本申请提出的基于激光雷达的机器人定位导航方法包括，接收激光雷达检测到的激光数据，根据当前时段接收的激光数据与上一时段优化后的可用数据，确定机器人的本体结构；当在一个新的场景中，选择机器人执行任务时，即开始启动，这时机器人本体进行旋转、位移操作时，接收激光数据帧。根据接收的可用激光帧，判别机器人的本体结构。根据机器人的本体结构，将激光雷达采集到的激光数据的噪声数据进行滤除，得到有效激光数据；这消除了本体结构噪声，保障了数据真实性。将有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行回环检测处理，构建出更新环境地图；选择机器人进行移动，在移动进程中，对环境地图进行构建。激光雷达所采集的激光数据是点云数据，它只能反映机器人所在环境的一部分，所以要对环境原始数据进行优化，剔除一些有问题的数据或者进行滤波。利用点云匹配算法，把局部环境的点云数据在已经建立的地图上找到相应的位置，匹配之后，将激光雷达的新数据拼接到原始地图中，完成地图更新，即地图融合，为避免环境中存在的干扰对机器人的地图出现断开，进行回环处理，此时构建出了新环境地图。利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置；避免在每步迭代中都需要求解雅可比矩阵和海森矩阵，这样优化过的地图，在运行过程中能大大提高计算效率。计算机器人在优化地图中从当前位置到目标位置的规划路径，将规划路径进行离散化和优化，得到路径离散点集合，并计算路径离散点集合的执行速度，机器人按照路径离散点集合和执行速度进行导航；路径规划导航，使得机器人在遇到障碍物时，可以不停止进行大尺度的绕行，避免机器人失去控制。

图1示出了本申请一种基于激光雷达的机器人定位导航方法的一个具体实施方式。

在图1所示的具体实施方式中，基于激光雷达的机器人定位导航方法主要包括步骤S101，根据预先确定的机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据。消除了本体结构噪声，保障了数据真实性。

在该具体实施方式中，滤除掉机器人的本体结构从数据层面带来的数据噪声，可以让激光雷达探测到的数据尽可能的接近真实数据，这些数据就可以为作为机器人定位导航所依靠的基础数据。本体结构在后续数据处理中，可以有效的滤除掉因为本体结构影响而返回的无效数据，所以在数据处理层面上更高效。无论是在地图构建还是路径导航过程中，都可以过滤掉本体噪声数据。

需要说明的是，为了保证激光雷达的探测范围，激光雷达安装区域尽保持空间开阔，尽可能的清空遮挡部分。如果180度的激光雷达或者270度的激光雷达可以尽量保证区域没有遮挡，那么360度的激光雷达就带来了很大的问题，因为运动底盘上层需要支撑以及为了机器人之间便于识别会做一些特殊标志，因此会对激光雷达造成一定的遮挡，这部分数据既影响地图构建过程，也影响机器人定位导航过程。所以在定位构图伊始，需要滤除因为机器人本体结构返回的无效数据，从而保证激光雷达对环境的有效感知，提高了机器人的探测精准度。

图2示出了本申请一种基于激光雷达的机器人定位导航方法的一个具体实施例。

在图2所示的具体实施例中，基于激光雷达的机器人定位导航方法还包括预先确定机器人的本体结构，其具体过程包括，

步骤S201，机器人进行移动操作时，将当前时段接收到的激光数据与上一时段优化后的可用数据进行匹配，得到数据差值；若数据差值小于第一预设阈值，则判定当前时段接收到的激光数据为可用激光数据。

步骤S202，获取预定数量的可用激光数据，确定可用激光数据的稳定性参数，若稳定性参数小于第二预设阈值，则将可用激光数据作为本体结构数据。

步骤S203，对本体结构数据进行滤波优化，确定机器人的本体结构。

在该具体实施例中，机器人的移动操作包括旋转和位移。确定可用激光数据的稳定性参数可以是方差，也可以是标准差。只有激光雷达检测到的激光数据超出正常变化范围、数据稳定且离散度低时，才可以去确定机器人的本体结构数据，从而确定机器人的本体结构。

在本申请的一个具体实例中，当机器人进行旋转和位移操作时，接收激光雷达检测到的激光数据，将当前时段接收到的激光数据与上一次检测到的进行滤波优化后的可用数据进行匹配，假定此时激光数据与可用数据发生了变化，两者所产生的数据差值小于第一预设阈值，则认为，当前时段最新一轮检测的激光数据有效，判定为可用激光数据，其中，第一预设阈值根据具体的机器人导航情况或者数据测量情况进行确定。

在该具体实例中，获取预定数量的可用激光数据，确定可用激光数据的稳定性参数，例如，获取6000个可用激光数据帧，计算此6000个可用激光数据的方差，根据此方差结果，若方差小于第二预设阈值，则将此6000个可用激光数据用作进行本体结构计算的本体结构可用数据；对所获得的本体结构数据进行逐步处理，进行滤波优化计算后，可以确定机器人本体结构，其中第二预设阈值根据可用激光数据的离散度规律进行确定。

在图1所示的具体实施方式中，基于激光雷达的机器人定位导航方法，还包括步骤S102，将有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行修复检测，构建出更新环境地图。

在该具体实施方式中，在机器人移动过程中，需要对环境地图进行构建。激光雷达所采集的数据是点云数据，它只能反映机器人所在真实环境的一部分，所以要对真实环境中的原始数据进行优化，剔除一些有问题的点云数据或者对点云数据进行滤波。利用点云匹配算法，把局部真实环境的点云数据在已经建立的原始环境地图上找到相应的位置，并进行匹配之后，将激光雷达检测到的新点云数据拼接到原始环境地图中，完成地图更新，即地图融合，为避免因为真实环境中存在的干扰，机器人构建的地图出现断开的情况，可以对融合后的环境地图进行回环处理，进而构建出更新环境地图。

在图1所示的具体实施方式中，基于激光雷达的机器人定位导航方法，还包括步骤S103，利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置。

在该具体实施方式中，在匹配-融合-回环后，得到更新环境地图，采用图优化的方法对此时更新环境地图进行进一步的优化，根据激光雷达检测到的已有观测数据，对机器人的当前位置和更新环境地图进行优化。

在本申请的一个具体实施例中，利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置的具体过程包括，根据激光雷达检测到的有效激光数据，得到机器人在对应时刻的观测位姿参数；将观测位姿参数作为当前估计值，利用当前估计值得到机器人在时刻的当前机器人位姿参数；将当前机器人位姿参数与当前估计值进行对比，得到迭代参数，对迭代参数进行判断；若迭代参数大于第三预设阈值，则将当前机器人位姿参数作为当前估计值，重新计算当前机器人位姿参数，并进行对比；若迭代参数不大于第三预设阈值，则将当前机器人位姿参数作为机器人在优化地图中的当前位置。

在该具体实施例中，因为激光雷达探测到的有效激光数据不一定是真实值，所以需要使用图优化算法尽量保证探测到的有效激光数据接近真实值。观测位姿参数可以选择在t₀时刻激光雷达探测到的有效激光数据，也可以直接取值为0，在优化过程中通过迭代去对当前机器人位姿参数的优化。

在本申请的一个具体实施例中，将观测位姿参数作为当前估计值，利用当前估计值得到机器人在时刻的当前机器人位姿参数具体过程包括，将观测位姿参数作为当前估计值，计算当前估计值的当前雅克比矩阵和当前海森矩阵；根据当前雅克比矩阵和当前海森矩阵，确定梯度方向；将梯度方向代入目标函数，得到机器人在时刻的当前机器人位姿。

在该具体实施例中，在迭代过程初始时，将位姿观测参数作为初始值开始进行第一次迭代，将第一次迭代出的当前机器人位姿参数与观测位姿参数进行比较，当两者差值过大时，需要将当前机器人位姿参数作为当前估计值进行第二次迭代，将第二次迭代出的当前机器人位姿参数与第一次迭代出的当前机器人位姿参数进行比较，一直到两者差值小于第三预设阈值时，迭代才会结束。

在本申请的一个具体实例中，根据实际问题抽象出问题的顶点和边，以机器人各个时刻的观测位姿作为顶点，通过激光雷达匹配获得的各时刻机器人的观测位姿变换矩阵为边；假设在时刻t的机器人位姿为y(t)，激光雷达检测到的观测位姿为z(t)，误差为e(t)，激光雷达的观测方程为：

z_t＝h(y_t)+e_t

在更新环境地图中，可以把观测方程写成更广义的形式：

Z_t＝H(y_t1，y_t2，y_t3…)

在更新环境地图中，目标函数可以写成如下形式：

这里的目标函数是一个关于误差e_t的最小二乘表达式。其中，Ω_t是信息矩阵，是协方差矩阵的逆，表示各分量的不一致性。由于z_t是已知的，为了数学上的简洁，再把目标函数写成e_t(y_t)的形式，于是目标函数的总体优化问题变为n条边加和的形式：

为了求解优化，需要知道一个初始值和一个迭代方向。为了数学上的方便，首先考虑第t条边e_t(y_t)。假设初始值为

并且给初始值一个Δy的增量，那么边的估计值就变为

而误差值则从

变为了

首先对误差项进行一阶展开：

J_t是关于y_t的导数，矩阵形式下为雅可比阵。这里在估计点附近作了一次线性假设，函数值用一阶导数来逼近，于是有：

故F_t的增量可表示为：

[ΔF]_t＝2[e_t]^TΩ_tJ_tΔy+[Δy]^TH_tΔy

要求解[ΔF]_t的极小值，令它对于Δy的导数为零，有：

[dΔF]_t/dΔy＝2[e_t]^TΩ_tJ_t+2H_tΔy＝0

最终，问题变为了一个一阶线性方程组的求解：

H_tΔy＝-[e_t]^TΩ_tJ_t

如果考虑所有的顶点和边，则系数矩阵方程可变为:

HΔy＝-e^TΩJ

需要说明的是，对于一个实际的SLAM实例，用于求解的顶点和边可能有成千上万条，在每一步迭代中都需要求解一个雅可比矩阵和海森矩阵，会有几十万个待估参数，这在以前被认为是无法求解的。不过我们认识到，SLAM问题构建的图，并非是全连通图，它往往是很稀疏的。如一个地图里大部分路标点，只会在很少的时刻被机器人看见，从而建立起一些边，大多数时候它们是看不见的。体现在数学公式中，虽然总体目标函数F(y)有很多项，但某个顶点yt就只会出现在和它有关的边里面。这样求解的时候，无关边的雅可比矩阵就是个零矩阵了，只有少数相连的边才会有非零值。同理，海森矩阵也会是稀疏矩阵。

在本申请的一个具体实例中，每步迭代中计算对于当前估计值的雅可比矩阵和海森矩阵，将雅可比矩阵和海森矩阵代入系数矩阵方程中，得到梯度方向Δy，将梯度方向Δy的结果放进目标函数中求解优化变量机器人位姿参数。当两次迭代之间优化变量的差值小于第三预设阈值时，则迭代结束。理想的第三预设阈值为0，但阈值和步长之间有一些关联，所以第三预设阈值可以取0.1或者0.05，其在实际过程中均可调整。优化变量的值就是迭代过程计算得到的对机器人位姿参数的估计值，如果满足迭代结束条件，则把最新得到的结果做为优化值返回。

在本申请的一个具体实例中，优化指的是通过改变某个函数中的自变量，来得到最小值或者最大值。优化有三个最重要的因素：目标函数，优化变量，优化约束，优化问题可以简单的描述如下：

其中y为优化变量，而F(y)为优化函数。此问题称为无约束优化问题，因为我们没有给出任何约束形式。机器人定位导航中的slam中，优化问题多为无约束优化。

在该实例中，函数的导数f′(y)的几何意义是f(y)在y处的斜率，如果在y₀处f(y0)′＝0>0，则表示函数f(y)在自变量y₀处存在一个(y₀-Δy，y₀+Δy)区间，在这个区间内f(y)是单调递增的。当f(y)在y₀处的导数f′(y₀)＝0时，表示f(y)可能在y₀处取得极大值、极小值或者鞍点，遍历比较这些值就可以获得最小值。当目标函数是凸函数时，沿着梯度下降方向更新y，对f(y)求得的最小值是全局最小值。在迭代过程中选择梯度方向为迭代方向，可以提高计算效率，同时也能保证我们要求的最小值为全局最小值。

在图1所示的具体实施方式中，基于激光雷达的机器人定位导航方法，还包括步骤S104，计算机器人在优化地图中从当前位置到目标位置的规划路径，将规划路径进行离散化和优化，得到路径离散点集合，并计算路径离散点集合的执行速度，机器人按照路径离散点集合和执行速度进行行驶。

在该具体实施方式中，按照规划路径进行行驶可以在遇到障碍物时，机器人不停止然后大尺度的绕行，避免机器人失去控制。

在本申请的一个具体实施例中，将规划路径进行离散化和优化，得到路径离散点集合，并计算路径离散点集合的执行速度，机器人按照路径离散点集合和执行速度进行导航的具体过程包括，将规划路径中进行离散化，得到初始点集合；根据初始点集合中的机器人的当前位置信息和设定的目标位置信息，对机器人的导航模式进行判断；若当前位置信息与目标位置信息不相同，则机器人处于导航模式，根据映射到优化地图中的初始点集合，绘制规划路径的道路边界，得到路径离散点集合；根据路径离散点集合确定具有道路边界的规划道路，并将规划道路切割成第二预设数量的线段路径，得到线段信息，并根据线段信息确定机器人的执行速度，其中第二预设数量根据规划道路的长度确定；机器人按照线段信息和执行速度进行行驶。

在该具体实施例中，机器人最开始收到信号后，在不考虑行进过程中的扰动时，会规划出一条从当前位置到目标位置的规划路径，并将规划路径离散化得到初始点集合是；在初始点集合的基础上，加入道路边界，成为路径离散点集合，其中道路边界一般在规划路径的两侧。

在本申请的一个具体实施例中，机器人按照线段信息和执行速度进行行驶的具体过程包括，根据线段信息和激光雷达检测到的有效激光数据，对机器人实际行驶路径中的位姿方向以及行驶路径上的障碍物进行判断；若位姿方向与线段信息的方向不符合和、或行驶路径上存在障碍物，则重新计算规划路径，并进行离散化，得到更新线段信息和更新执行速度，并按照更新线段信息和更新执行速度进行行驶；若行驶路径中的位姿方向与线段信息的方向相符合以及行驶路径上不存在障碍物，则机器人按照线段信息与执行速度进行行驶。

在该具体实施例中，当位姿方向与线段信息的方向不符合时，说明机器人已偏离行驶方向，此时重新计算规划路径，对规划路径进行离散化和优化后，得到更新线段信息，并计算更新执行速度，当机器人按照更新线段信息行驶时，继续判断位姿方向与线段信息的方向是否符合。并对规划路径上是否存在障碍物进行判断，若存在障碍物，则需要重新计算规划路径。

在本申请的一个具体实施例中，根据线段信息和激光雷达检测到的有效激光数据，对机器人实际行驶路径中的位姿方向以及行驶路径上的障碍物进行判断的过程包括，根据激光雷达检测到的有效激光数据，得到机器人的位姿方向信息和行驶路径信息，行驶路径信息包括行驶路径上的障碍物信息；根据位姿方向信息与线段信息中的方向信息，得到第一信息数值，若第一信息数值超出第一预设阈值范围，则确定行驶路径中的位姿方向偏离线段信息的方向；根据行驶路径信息和线段信息中机器人所处线段路径的方向信息，确定线段路径的方向上行驶路径信息是否存在超出第二预设阈值范围的障碍物信息，若存在，则确定行驶路径上存在障碍物。

在该具体实施例中，线段路径上一条线上的每一个点都有一个方向信息，通过激光雷达可以时时获得机器人的在行驶过程中的位姿方向信息，若两者的差值在第一预设阈值范围内，则可以判定机器人没有偏离方向，继续行驶，否则停止行驶，重新计算规划路径；当行驶路径信息处于第二预设阈值范围内，则表明行驶路径上的障碍物太小，可以通过，不需要停止或绕行，否则重新计算规划路径。

在本申请的一个具体实例中，每次将从初始位置到设定的目标位置的规划路径离散成初始点集合后，都需要将目标位置信息和当前位置信息比对，判断机器人是否已经完成导航，如果是，则可以结束导航，输出执行速度为0的信息给执行机构；如果不是，则返回导航模式；继续将初始点集合映射到机器人所在的优化地图中，绘制规划路径的道路边界；并将此道路边界规划出的机器人的运动路径切割成若干条线段路径，将线段信息发送给执行机构，使机器人在此些线段路径上进行行驶，然后判断机器人是否在分割出的每条线段路径上进行行驶，如果是，则继续行驶，如果不是，则停止前进，重新计算新的规划路径，并进行离散化。判断在当前位置和目标位置之间是否存在障碍，即规划路径是否可达，如果存在障碍物，则重新计算新的规划路径，并进行离散化，否则继续沿当前的线段路径行驶。

图3示出了本申请一种基于激光雷达的机器人定位导航装置的具体实施方式。

在该具体实施方式中，基于激光雷达的机器人定位导航装置主要包括：

模块301，本体噪声滤除模块，其用于根据预先确定的机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据。

模块302，地图构建模块，其用于将有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行修复检测，构建出更新环境地图。

模块303，地图优化模块，其用于利用图优化算法对更新环境地图进行优化，得到优化地图中机器人的当前位置。

模块304，导航模块，其用于计算机器人在优化地图中从当前位置到目标位置的规划路径，将规划路径进行离散化和优化，得到路径离散点集合，并计算路径离散点集合的执行速度，机器人按照路径离散点集合和执行速度进行行驶。

图4示出了本申请一种基于激光雷达的机器人定位导航装置的具体实施例。

在图4所示的具体实施例中，基于激光雷达的机器人定位导航装置还包括：

模块401，本体结构检测模块，其用于所述机器人进行移动操作时，将当前时段接收到的激光数据与上一时段优化后的可用数据进行匹配，得到数据差值；用于若所述数据差值超出第一预设阈值范围，则判定所述当前时段接收到的所述激光数据为可用激光数据；用于获取预定数量的所述可用激光数据，确定所述可用激光数据的稳定性参数，若所述稳定性参数在第二预设阈值范围内，则将所述可用激光数据作为本体结构数据；用于对所述本体结构数据进行滤波优化，确定所述机器人的所述本体结构。

本实施例中，模块402～模块405的具体实现过程和技术原理请参见图3所示的基于激光雷达的机器人定位导航装置中模块S301～模块304中的相关描述，此处不再赘述。

本申请提供的基于激光雷达的机器人定位导航装置，可用于执行上述任一实施例描述的基于激光雷达的机器人定位导航方法，其实现原理和技术效果类似，在此不再赘述。

在本申请的一个具体实施例中，本申请一种基于激光雷达的机器人定位导航装置中各功能模块可直接在硬件中、在由处理器执行的软件模块中或在两者的组合中。

软件模块可驻留在RAM存储器、快闪存储器、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、可装卸盘、CD-ROM或此项技术中已知的任何其它形式的存储介质中。示范性存储介质耦合到处理器，使得处理器可从存储介质读取信息和向存储介质写入信息。

处理器可以是中央处理单元(英文：Central Processing Unit，简称：CPU)，还可以是其他通用处理器、数字信号处理器(英文：Digital Signal Processor，简称：DSP)、专用集成电路(英文：Application Specific Integrated Circuit，简称：ASIC)、现场可编程门阵列(英文：Field Programmable Gate Array，简称：FPGA)或其它可编程逻辑装置、离散门或晶体管逻辑、离散硬件组件或其任何组合等。通用处理器可以是微处理器，但在替代方案中，处理器可以是任何常规处理器、控制器、微控制器或状态机。处理器还可实施为计算装置的组合，例如DSP与微处理器的组合、多个微处理器、结合DSP核心的一个或一个以上微处理器或任何其它此类配置。在替代方案中，存储介质可与处理器成一体式。处理器和存储介质可驻留在ASIC中。ASIC可驻留在用户终端中。在替代方案中，处理器和存储介质可作为离散组件驻留在用户终端中。

在本申请的另一个具体实施方式中，一种电子设备，其包括处理器和存储器，存储器存储有计算机指令，该计算机指令被操作以执行任一实施例中的基于激光雷达的机器人定位导航方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

以上所述仅为本申请的实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种基于激光雷达的机器人定位导航方法，其特征在于，包括：

根据预先确定的机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据；

将所述有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行修复检测，构建出更新环境地图；

利用图优化算法对所述更新环境地图进行优化，得到优化地图中所述机器人的当前位置；

计算所述机器人在所述优化地图中从所述当前位置到目标位置的规划路径，将所述规划路径进行离散化和优化，得到路径离散点集合，并计算所述路径离散点集合的执行速度，所述机器人按照所述路径离散点集合和所述执行速度进行行驶。

2.如权利要求1所述的基于激光雷达的机器人定位导航方法，其特征在于，所述预先确定的机器人本体结构，包括：

所述机器人进行移动操作时，将当前时段接收到的激光数据与上一时段优化后的可用数据进行匹配，得到数据差值，若所述数据差值小于第一预设阈值，则判定所述当前时段接收到的所述激光数据为可用激光数据；

获取预定数量的所述可用激光数据，确定所述可用激光数据的稳定性参数，若所述稳定性参数小于第二预设阈值，则将所述可用激光数据作为本体结构数据；

对所述本体结构数据进行滤波优化，确定所述机器人的所述本体结构。

3.如权利要求1所述的基于激光雷达的机器人定位导航方法，其特征在于，所述利用图优化算法对所述更新环境地图进行优化，得到优化地图中所述机器人的当前位置，包括：

根据所述激光雷达检测到的所述有效激光数据，得到所述机器人在对应时刻的观测位姿参数；

将所述观测位姿参数作为当前估计值，利用所述当前估计值得到所述机器人在所述时刻的当前机器人位姿参数；

将所述当前机器人位姿参数与所述当前估计值进行对比，得到迭代参数，对所述迭代参数进行判断；

若所述迭代参数大于第三预设阈值，则将所述当前机器人位姿参数作为所述当前估计值，重新计算所述当前机器人位姿参数，并进行所述对比；

若所述迭代参数不大于所述第三预设阈值，则将所述当前机器人位姿参数作为所述机器人在所述优化地图中的所述当前位置。

4.如权利要求3所述的基于激光雷达的机器人定位导航方法，其特征在于，所述将所述观测位姿参数作为当前估计值，利用所述当前估计值得到所述机器人在所述时刻的当前机器人位姿参数，包括：

将所述观测位姿参数作为所述当前估计值，计算所述当前估计值的当前雅克比矩阵和当前海森矩阵；

根据所述当前雅克比矩阵和所述当前海森矩阵，确定梯度方向；

将所述梯度方向代入目标函数，得到所述机器人在所述时刻的所述当前机器人位姿。

5.如权利要求1所述的基于激光雷达的机器人定位导航方法，其特征在于，所述将所述规划路径进行离散化和优化，得到路径离散点集合，并计算所述路径离散点集合的执行速度，所述机器人按照所述路径离散点集合和所述执行速度进行导航，包括：

将所述规划路径中进行离散化，得到初始点集合；

根据所述初始点集合中的所述机器人的当前位置信息和设定的目标位置信息，对所述机器人的导航模式进行判断；

若所述当前位置信息与所述目标位置信息不相同，则所述机器人处于导航模式，根据映射到所述优化地图中的所述初始点集合，绘制所述规划路径的道路边界，得到所述路径离散点集合；

根据所述路径离散点集合确定具有道路边界的规划道路，并将所述规划道路切割成第二预设数量的线段路径，得到线段信息，并根据所述线段信息确定所述机器人的执行速度，其中所述第二预设数量根据所述规划道路的长度确定；

所述机器人按照所述线段信息和所述执行速度进行行驶。

6.如权利要求5所述的基于激光雷达的机器人定位导航方法，其特征在于，所述机器人按照所述线段信息和所述执行速度进行行驶，包括：

根据所述线段信息和所述激光雷达检测到的所述有效激光数据，对所述机器人实际行驶路径中的位姿方向以及所述行驶路径上的障碍物进行判断；

若所述位姿方向与所述线段信息的方向不符合和/或所述行驶路径上存在所述障碍物，则重新计算所述规划路径，并进行所述离散化，得到更新线段信息和更新执行速度，并按照所述更新线段信息和所述更新执行速度进行行驶；

若所述行驶路径中的所述位姿方向与所述线段信息的方向相符合以及所述行驶路径上不存在所述障碍物，则所述机器人按照所述线段信息与所述执行速度进行行驶。

7.如权利要求6所述的基于激光雷达的机器人定位导航方法，其特征在于，所述根据所述线段信息和所述激光雷达检测到的所述有效激光数据，对所述机器人实际行驶路径中的位姿方向以及所述行驶路径上的障碍物进行判断的过程包括：

根据所述激光雷达检测到的所述有效激光数据，得到所述机器人的位姿方向信息和行驶路径信息，所述行驶路径信息包括行驶路径上的障碍物信息；

根据所述位姿方向信息与所述线段信息中的方向信息，得到第一信息数值，若所述第一信息数值超出第一预设阈值范围，则确定所述行驶路径中的所述位姿方向偏离所述线段信息的所述方向；

根据所述行驶路径信息和所述线段信息中所述机器人所处线段路径的方向信息，确定所述线段路径的方向上所述行驶路径信息是否存在超出第二预设阈值范围的所述障碍物信息，若存在，则确定所述行驶路径上存在所述障碍物。

8.一种基于激光雷达的机器人定位导航装置，其特征在于，包括：

本体噪声滤除模块，其用于根据预先确定的机器人本体结构，将激光雷达采集到的激光数据中的噪声数据进行滤除，得到有效激光数据；

地图构建模块，其用于将所述有效激光数据与原始环境地图进行匹配融合，并对融合后的环境地图进行修复检测，构建出更新环境地图；

地图优化模块，其用于利用图优化算法对所述更新环境地图进行优化，得到优化地图中所述机器人的当前位置；

导航模块，其用于计算所述机器人在所述优化地图中从当前位置到目标位置的规划路径，将所述规划路径进行离散化和优化，得到路径离散点集合，并计算所述路径离散点集合的执行速度，所述机器人按照所述路径离散点集合和所述执行速度进行行驶。

9.如权利要求8所述的基于激光雷达的机器人定位导航装置，其特征在于，还包括：

本体结构检测模块，其用于所述机器人进行移动操作时，将当前时段接收到的激光数据与上一时段优化后的可用数据进行匹配，得到数据差值；

用于若所述数据差值超出第一预设阈值范围，则判定所述当前时段接收到的所述激光数据为可用激光数据；

用于获取预定数量的所述可用激光数据，确定所述可用激光数据的稳定性参数，若所述稳定性参数在第二预设阈值范围内，则将所述可用激光数据作为本体结构数据；

用于对所述本体结构数据进行滤波优化，确定所述机器人的所述本体结构。

10.一种电子设备，其包括处理器和存储器，所述存储器存储有计算机指令，其中所述处理器操作所述计算机指令以执行权利要求1-7中任一项所述的基于激光雷达的机器人定位导航方法。

Images