CN110553652B - 机器人多传感器融合定位方法及其应用 - Google Patents

Abstract

本发明涉及一种机器人多传感器融合定位方法及其在智能清洁机器人中的应用，包括以下步骤：在机器人上安装激光扫描仪、IMU，在机器人的两个车轮轴上安装编码器，若在室外且需要使用GPS，则安装GPS；若在室内且需要使用UWB，则安装UWB，并可安装RFID、摄像头；机器人使用SLAM构建场所的环境地图；定位时，首先给定机器人一个初始位姿，在后续定位过程中，使用图优化方法计算得到机器人的移动距离，使得此移动距离最符合各个传感器观测到的机器人移动距离，结合上一时刻的位姿得到当前机器人精确的定位；本发明可准确识别出当前位置，大大提高定位的准确性，避免机器人处于地图质量较差环境中定位丢失的现象。

Description

机器人多传感器融合定位方法及其应用

[技术领域]

本申请涉及智能机器人领域，特别涉及一种机器人多传感器融合定位方法。

[背景技术]

定位与导航技术是机器人的关键技术之一，而定位是导航的前提。定位是指机器人通过读取传感器信息，与当前地图进行匹配，获取自身在地图中的位置。在SLAM问题中，应用传感器感知的信息实现可靠的定位是机器人最基本、最重要的一项功能。

然而，机器人应用场景复杂，不同场景需要相应地改变传感器，造成了各个场景中定位方法的独特性，使得定位方法的通用性较差。另外，由于当前大部分定位方法使用的是基于上一次定位信息进行推测当前定位信息，然后根据当前传感器观测数据进行矫正，从而不能充分利用历史定位信息，所以很容易出现定位丢失。

[发明内容]

本发明的目的就是要解决上述的不足而提供一种机器人多传感器融合定位方法，能够准确识别出当前位置，大大提高了定位的准确性，避免了当机器人处于地图质量较差环境时容易定位丢失的现象。

为实现上述目的设计一种机器人多传感器融合定位方法，包括以下步骤：

步骤s1，在机器人上安装激光雷达、编码器；

步骤s2，将机器人移动至实际环境中的各个位置来构建2D或3D激光栅格地图；

步骤s3，在定位时，先基于上一时刻的机器人定位，使用多个传感器测量机器人移动距离，再基于图优化方法计算得到最符合各个传感器观测的机器人移动距离，进而得到机器人当前时刻的定位数据。

进一步地，步骤s1中，机器人上还安装有里程计、IMU惯性测量单元、GPS模块、UWB定位模块、RFID射频定位模块、摄像头中的一种或多种。

进一步地，步骤s1中，当机器人在室外时，机器人上安装有GPS模块。

进一步地，步骤s1中，当机器人在室内时，机器人上安装有UWB定位模块，同时安装有RFID射频定位模块、摄像头。

进一步地，步骤s1中还包括以下步骤：步骤s11，安装编码器时，将编码器安装在机器人的车轮轴上；步骤s12，将各个传感器进行时间戳对齐。

进一步地，步骤s2中还包括以下步骤：步骤s21，使用激光SLAM算法构建激光栅格地图；步骤s22，若步骤s1中使用GPS，当接收到GPS定位数据时，同时为此时的栅格地图坐标数据关联此时的GPS坐标；若步骤s1中使用UWB和/或RFID，当接收到UWB和/或RFID定位数据时，同时为此时的栅格地图坐标数据关联此时的UWB坐标和/或RFID坐标。

进一步地，步骤s3中还包括以下步骤：

步骤s31，在栅格地图中给定机器人一个初始位姿，将此时机器人的定位信息记为节点一；

步骤s32，如果机器人不动，则忽略传感器数据；如果机器人行走，则读取里程计数据；

步骤s33，当使用并接收到GPS数据时，在建图时地图中存储的GPS坐标中寻找最近的数个GPS坐标对应的栅格地图坐标，然后进行插值，得到当前的栅格地图坐标；如果不使用GPS或者没接收到GPS信号，则跳过此步骤；

步骤s34，当使用UWB时，与步骤s33同理；

步骤s35，基于上一时刻的定位加上里程计测量的行驶距离，在此处使用激光数据和栅格地图进行匹配，得到当前的初始定位信息，并记为节点二；

步骤s36，使用上一时刻的节点一和此时刻的节点二作为图优化的两个节点，里程计测量到的行驶距离作为图优化中的约束之一；如果使用GPS或UWB，那么GPS或UWB测量得到的位姿和节点二的位姿作为另一种约束；通过图优化算法，得到当前节点二新的位姿，作为精确的定位结果。

进一步地，步骤s36之后还包括步骤s37，步骤s37按步骤s31至步骤s36类推，维持多个节点，节点数量不少于五个，使用图优化方法计算得到当前定位。

进一步地，当机器人上安装有里程计、GPS模块、UWB定位模块三种传感器，机器人行走一段距离后，得到里程计记录的行走距离、GPS探测的当前时刻位置和行走之初时的位置差、UWB探测的当前时刻位置和行走之初时的位置差，三种传感器得到的行走距离不同，使用优化的方法即最大似然法，使当前的定位和上一时刻的定位之间的距离与各个传感器记录的行走距离的差的平方和最小；继续行驶后，得到另一组观测数据，同样使用优化的方法计算得到最符合所有观测数据的第二个节点及以后所有节点的位姿，即作为定位结果。

进一步地，本发明提供了一种安装多种传感器的智能清洁机器人，包括：通信接口；存储器，用于存储计算机程序；一个或多个处理器，用于执行所述存储器中存储的计算机程序，且所述计算机程序被执行时，实现上述方法。

进一步地，本发明提供了一种计算机存储介质，所述存储介质中存储计算机程序，所述计算机程序在执行时实现上述方法。

本发明同现有技术相比，具有如下优点：

(1)本发明所提供的机器人多传感器融合定位方法采用多种传感器的信息进行定位，且不受传感器数量的限制，即使在相似度很高的不同区域也可以准确识别出当前位置，大大提高了定位的准确性。

(2)本发明所提供的机器人多传感器融合定位方法采用多种传感器定位，避免了使用单一传感器在特殊定位区域无法提供有效信息的缺陷，使得机器人在人群拥挤的环境中仍然能实现定位，提高了定位的鲁棒性；

(3)本发明所提供的机器人多传感器融合定位方法在定位时，不仅接收当前的传感器数据，而且还考虑了历史的定位信息，使得当机器人处于地图质量较差的环境中时不容易定位丢失。

本申请的实施方式的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实施方式的实践了解到。

[附图说明]

图1是本发明实施方式的流程示意图；

图2是本发明实施方式所提供的基于图优化的移动机器人多传感器的安装示意图,图中，1、激光扫描仪2、GPS模块3、编码器；

图3是本发明实施方式所提供的图优化方法示意图；

图4是本发明实施方式根据多组传感器观测数据优化各个定位节点的示意图；

图5是本申请某些实施方式的智能清洁机器人的模块示意图，图中，20、处理器30、存储器40、通信接口100、智能清洁机器人。

[具体实施方式]

下面详细描述本申请的实施方式，所述实施方式的示例在附图中示出，其中，相同或类似的标号自始至终表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本申请的实施方式，而不能理解为对本申请的实施方式的限制。

SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写，主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。SLAM通常包括如下几个部分，特征提取，数据关联，状态估计，状态更新以及特征更新等。对于其中每个部分，均存在多种方法。

图优化，就是把一个常规的优化问题，以图(Graph)的形式来表述。图是由顶点(Vertex)和边(Edge)组成的结构，而图论则是研究图的理论。我们记一个图为G＝{V,E}，其中V为顶点集，E为边集。边一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点(Unary Edge，一元边)、两个顶点(Binary Edge，二元边)或多个顶点(Hyper Edge，多元边)。最常见的边连接两个顶点。

下面结合具体实施例对本发明作以下进一步说明：

如附图1所示，本实施例公开了一种机器人多传感器融合定位方法，该定位方法主要包括以下步骤：

步骤s1，在机器人上安装激光扫描仪1、编码器3，如附图2所示，可选地安装IMU(惯性测量元件)、GPS(2)、UWB、RFID、摄像头；具体地，如果在室外且需要使用GPS(全球定位***)，则安装GPS模块；如果在室内且需要使用UWB(超宽频谱)，则安装UWB定位模块，还可安装RFID(射频定位模块)、摄像头；

该步骤s1中还包括以下步骤：

步骤s11，安装编码器时，需安装在机器人车轮轴上，用于航迹推演，计算机器人行走的距离；

步骤s12，将各个传感器进行时间戳对齐；

步骤s2，将机器人移动到环境中的各个位置来构建2D或3D激光栅格地图；具体为，在定位场所移动机器人，使用SLAM(同时定位与建图)算法构建场所的环境地图；

该步骤s2中还包括以下步骤：

步骤s21，使用激光SLAM算法构建环境地图(激光栅格地图)；

步骤s22，如果使用GPS，当接收到GPS定位数据时，同时为此时的栅格坐标数据关联此时的GPS坐标。当使用UWB、RFID时同理，即为此时的栅格坐标数据关联此时的UWB坐标或RFID坐标。具体为：同时为此时的栅格地图坐标数据关联此时的GPS坐标，本发明实施例使用结构体的形式表达机器人的定位数据，关联即表现为在此时定位数据的结构体中填充GPS坐标。

步骤s3，在定位时，基于上一时刻的机器人定位，使用各个传感器测量机器人移动的距离，基于图优化算法计算得到最符合各个传感器观测的移动距离，得到机器人当前时刻的定位数据。即，定位时，首先给定机器人一个初始位姿，在后续定位过程中，使用图优化算法计算得到机器人的移动距离，使得此移动距离最符合各个传感器观测到的机器人移动距离，结合上一时刻的位姿得到当前机器人精确的定位。

该步骤s3中还包括如下步骤：

步骤s31，如附图3所示，在栅格地图中给定机器人一个初始位姿，将此时机器人的定位信息记为节点一x1；

步骤s32，如果机器人不动，则忽略传感器数据。如果机器人行走，则读取里程计数据△₁₂；

步骤s33，当使用并接收到GPS数据时，在建图时地图中存储的GPS坐标中寻找最近的数个GPS坐标对应的栅格坐标数据，然后进行插值，得到当前的栅格地图坐标。如果不使用GPS或者没接收到GPS信号，则跳过；

步骤s34，当使用UWB时，与步骤s33同理；

步骤s35，基于上一时刻的定位加上里程计测量的行驶距离，在此处使用激光数据和栅格地图进行匹配，得到当前的初始定位信息，并记为节点二x2；

步骤s36，使用上一时刻的节点一和此时刻的节点二作为图优化的两个节点，里程计测量到的行驶距离作为图优化中的约束之一，如果使用GPS，那么GPS测量得到的位姿和节点一的GPS位姿之差R12作为另一种约束，UWB与GPS同理。通过图优化算法，得到当前节点二新的位姿，作为精确的定位结果。

步骤s37，以此类推，为了更好地使用历史定位信息，可以维持多个节点，如维持5个节点，使用图优化方法计算得到当前定位。

如附图4所示，机器人使用了里程计、gps和uwb三种传感器进行定位，当机器人行走一段距离后，得到了里程计记录的行走距离、gps探测的当前时刻位置和行走之初时的位置差、uwb探测的当前时刻位置和行走之初的位置差，三种传感器得到的行走距离不同，使用优化的方法即最大似然法，使当前的定位和上一时刻的定位之间的距离与各个传感器记录的行走距离的差的平方和最小。继续行驶后得到另一组观测数据，同样使用优化的方法，即可计算得到最符合所有观测数据的第二个个节点及以后所有节点的位姿。

图5是根据本申请一个实施例的智能清洁机器人100的模块示意图。该智能清洁机器人100包括存储器30、处理器20及存储在存储器30上并可在处理器20上运行的计算机程序。

处理器20执行程序时实现上述实施例中提供的智能清洁机器人100以及机器人多传感器融合定位方法。

存储器30用于存放可在处理器20上运行的计算机程序。存储器30可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。

进一步地，智能清洁机器人100还可包括通信接口40，通信接口40用于存储器30和处理器20之间的通信。

如果存储器30、处理器20和通信接口40独立实现，则通信接口40、存储器30和处理器20可以通过总线相互连接并完成相互间的通信。总线可以是工业标准体系结构(Industry Standard Architecture，简称为ISA)总线、外部设备互连(PeripheralComponent，简称为PCI)总线或扩展工业标准体系结构(Extended Industry StandardArchitecture，简称为EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。

可选的，在具体实现上，如果存储器30、处理器20及通信接口40，集成在一块芯片上实现，则存储器30、处理器20及通信接口40可以通过内部接口完成相互间的通信。

处理器20可能是一个中央处理器(Central Processing Unit，简称为CPU)，或者是特定集成电路(Application Specific Integrated Circuit，简称为ASIC)，或者是被配置成实施本申请实施例的一个或多个集成电路。

本实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器20执行时实现如上的智能清洁机器人100的多传感器融合定位方法。

综上，本申请实施方式的智能清洁机器人100以及机器人多传感器融合定位方法，其采用多种传感器的信息进行定位，且不受传感器数量的限制，即使在相似度很高的不同区域也可以准确识别出当前位置，大大提高了定位的准确性；避免了使用单一传感器在特殊定位区域无法提供有效信息的缺陷，使得机器人在人群拥挤的环境中仍然能实现定位，提高了定位的鲁棒性；不仅接收当前的传感器数据，而且还考虑了历史的定位信息，使得当机器人处于地图质量较差的环境中时不容易定位丢失。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用，或结合这些指令执行***、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本申请各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。上述提到的存储介质可以是只读存储器，磁盘或光盘等。

在本说明书的描述中，参考术语“某些实施方式”、“一个实施方式”、“一些实施方式”、“示意性实施方式”、“示例”、“具体示例”、或“一些示例”的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个所述特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个，除非另有明确具体的限定。

尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型，本申请的范围由权利要求及其等同物限定。

Claims (9)

1.一种机器人多传感器融合定位方法，其特征在于，所述多传感器融合定位方法包括：

步骤s1，在机器人上安装激光雷达、编码器；

步骤s2，将机器人移动至实际环境中的各个位置来构建2D或3D激光栅格地图；

步骤s3，在定位时，先基于上一时刻的机器人定位，使用多个传感器测量机器人移动距离，再基于图优化方法计算得到最符合各个传感器观测的机器人移动距离，进而得到机器人当前时刻的定位数据；

步骤s3中还包括以下步骤：

步骤s31，在栅格地图中给定机器人一个初始位姿，将此时机器人的定位信息记为节点一；

步骤s32，如果机器人不动，则忽略多组传感器数据；如果机器人行走，则读取里程计数据；

步骤s33，当使用并接收到GPS数据，建图时在地图中存储的GPS坐标中寻找最近的数个GPS坐标对应的栅格地图坐标，然后进行插值，得到当前的栅格地图坐标；如果不使用GPS或者没接收到GPS信号，则跳过此步骤；

步骤s34，当使用UWB时，与步骤s33同理；

步骤s35，基于上一时刻的定位加上里程计测量的行驶距离，在此处使用激光数据和栅格地图进行匹配，得到当前的初始定位信息，并记为节点二；

步骤s36，使用上一时刻的节点一和此时刻的节点二作为图优化的两个节点，里程计测量到的行驶距离作为图优化中的约束之一；和/或使用GPS或UWB，那么GPS或UWB测量得到的位姿和节点二的位姿作为另一种约束；通过图优化方法，得到当前节点二新的位姿，作为精确的定位结果。

2.根据权利要求1所述的方法，其特征在于：步骤s1中，编码器用于航迹推演，用于计算机器人行走的距离，在机器人上安装里程计、IMU惯性测量单元、GPS模块、UWB定位模块、RFID射频定位模块、摄像头中的一种或多种。

3.根据权利要求2所述的方法，其特征在于：步骤s1中，当机器人在室外时，机器人上安装有GPS模块；当机器人在室内时，机器人上安装有UWB定位模块，和/或安装有RFID射频定位模块、摄像头。

4.根据权利要求2所述的方法，其特征在于，步骤s1中还包括以下步骤：

步骤s11，安装编码器时，将编码器安装在机器人的车轮轴上；

步骤s12，将多个传感器进行时间戳对齐。

5.根据权利要求1所述的方法，其特征在于，步骤s2中还包括以下步骤：

步骤s21，使用激光SLAM算法构建激光栅格地图；

步骤s22，若步骤s1中使用GPS，当接收到GPS定位数据时，同时为此时的栅格地图坐标数据关联此时的GPS坐标；若步骤s1中使用UWB和/或RFID，当接收到UWB和/或RFID定位数据时，同时为此时的栅格地图坐标数据关联此时的UWB坐标和/或RFID坐标。

6.根据权利要求1所述的方法，其特征在于：步骤s36之后还包括步骤s37，步骤s37按步骤s31至步骤s36类推，维持多个节点，节点数量不少于五个，使用图优化方法计算得到当前定位。

7.根据权利要求1所述的方法，其特征在于：当机器人上安装有里程计、GPS模块、UWB定位模块三种传感器时，机器人行走一段距离后，得到里程计记录的行走距离、GPS探测的当前时刻位置和行走之初时的位置差、UWB探测的当前时刻位置和行走之初时的位置差，所述三种传感器得到的行走距离不同，使用优化的方法即最大似然法，使当前的定位和上一时刻的定位之间的距离与各个传感器记录的行走距离的差的平方和最小；继续行驶后，得到另一组观测数据，同样使用优化的方法计算得到最符合所有观测数据的第二个节点及以后所有节点的位姿，即作为定位结果。

8.一种安装多种传感器的智能清洁机器人，其特征在于，包括：通信接口；存储器，用于存储计算机程序；一个或多个处理器，用于执行所述存储器中存储的计算机程序，且所述计算机程序被执行时，实现上述权利要求1-7任一所述的方法。

9.一种计算机存储介质，其特征在于，所述存储介质中存储计算机程序，所述计算机程序在执行时实现如权利要求1-7中任一项所述的方法。

Images