CN112731337A - 地图构建方法、装置和设备 - Google Patents

地图构建方法、装置和设备 Download PDF

Info

Publication number
CN112731337A
CN112731337A CN202011604615.7A CN202011604615A CN112731337A CN 112731337 A CN112731337 A CN 112731337A CN 202011604615 A CN202011604615 A CN 202011604615A CN 112731337 A CN112731337 A CN 112731337A
Authority
CN
China
Prior art keywords
pose
laser
data
calculating
estimation value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202011604615.7A
Other languages
English (en)
Inventor
魏青铜
秦野
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Hikrobot Technology Co Ltd
Original Assignee
Hangzhou Hikrobot Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Hikrobot Technology Co Ltd filed Critical Hangzhou Hikrobot Technology Co Ltd
Priority to CN202011604615.7A priority Critical patent/CN112731337A/zh
Publication of CN112731337A publication Critical patent/CN112731337A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (AREA)

Abstract

本申请实施例提供一种地图构建方法、装置和设备，方法包括：获取激光测距数据和激光强度数据，确定激光强度数据中记录的反光器件的数量不为0，计算第一位姿，根据激光测距数据、激光强度数据以及第一位姿计算目标位姿，根据激光测距数据和目标位姿更新栅格地图，根据激光强度数据和目标位姿更新路标地图。本申请实施例结合激光测距数据和激光强度数据进行地图构建，可以适用于更多的应用场景。

Description

地图构建方法、装置和设备
技术领域
本申请涉及机器人同步定位与地图构建(Simultaneous Localization andMapping,SLAM)领域，特别涉及一种地图构建方法、装置和设备。
背景技术
随着智能机器人技术的迅速发展，智能机器人的应用场景越来越广泛。从室内外搬运机器人，到自动驾驶汽车，再到空中无人机、水下探测机器人等，智能机器人的运用都得到了巨大突破。
在智能机器人研究中一个最关键的技术就是定位和地图构建，也就是SLAM技术。激光SLAM是用激光雷达作为智能机器人的外部传感器来完成智能机器人的定位和地图构建。激光SLAM技术可以包括基于反光材料的激光SLAM技术和基于轮廓的激光SLAM技术。
两种激光SLAM技术各有优缺点，均具有自身适用场景的局限性。
发明内容
本申请提供了一种地图构建方法、装置和设备，能够结合两种激光SLAM技术进行地图构建，相对于单一激光SLAM技术具有更广泛的应用场景。
第一方面，本申请实施例提供一种地图构建方法，包括：
获取激光测距数据和激光强度数据，所述激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息；所述激光强度数据用于记录所述激光雷达检测到的反光器件的位置信息；所述激光雷达设置于所述智能机器人设备中；
确定所述激光强度数据中记录的反光器件的数量不为0，计算第一位姿，所述第一位姿是所述激光雷达在世界坐标系中位姿的粗略估计值；
根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿；所述目标位姿是所述激光雷达在世界坐标系中位姿的精确估计值；
根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿更新所述路标地图。
在一种可能的实现方式中，所述根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿，包括：
根据所述激光测距数据、所述激光强度数据和所述第一位姿计算所述第一位姿的误差估计值；所述第一位姿的误差估计值是所述第一位姿相对于激光雷达的实际位姿的误差估计值；
根据所述第一位姿和所述第一位姿的误差估计值计算第二位姿；
判断所述第一位姿的误差估计值是否满足预设结束条件；
如果所述第一位姿的误差估计值满足所述预设结束条件，将所述第二位姿作为所述目标位姿；
如果所述第一位姿的误差估计值不满足所述预设结束条件，使用所述第二位姿替换所述第一位姿，返回计算所述第二位姿的误差估计值、计算第三位姿、判断所述第二位姿的误差估计值是否满足所述预设结束条件，循环至第K位姿的误差估计值满足所述预设结束条件，将所述第K+1位姿作为所述目标位姿，K是大于1的自然数。
在一种可能的实现方式中，所述激光测距数据包括：激光数据点的雷达坐标，所述激光强度数据包括：所述反光器件的雷达坐标；所述雷达坐标是雷达坐标系中的坐标；
所述根据所述激光测距数据、所述激光强度数据和所述第一位姿计算所述第一位姿的误差估计值，包括：
根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点基于所述第一位姿的位置误差估计值；所述激光数据点基于第一位姿的位置误差估计值用于表示所述激光数据点基于第一位姿计算得到的世界坐标与所述激光数据点的实际世界坐标之间的误差估计值；所述世界坐标是世界坐标系中的坐标；
根据所述反光器件的雷达坐标以及所述第一位姿计算所述反光器件基于所述第一位姿的位置误差估计值；所述反光器件基于第一位姿的位置误差估计值用于表示所述反光器件基于第一位姿计算得到的世界坐标与所述反光器件的实际世界坐标之间的误差估计值；
根据所述激光数据点基于所述第一位姿的位置误差估计值、以及所述反光器件基于所述第一位姿的位置误差估计值计算所述第一位姿的误差估计值。
在一种可能的实现方式中，所述根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点基于所述第一位姿的位置误差估计值，包括：
根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点的世界坐标；
根据所述激光数据点的世界坐标确定所述激光数据点在所述栅格地图中对应的栅格；
获取所述激光数据点对应栅格的概率；
根据所述激光数据点对应栅格的概率计算所述激光数据点基于所述第一位姿的位置误差估计值。
在一种可能的实现方式中，所述根据所述反光器件的雷达坐标以及所述第一位姿计算所述反光器件基于所述第一位姿的位置误差估计值，包括：
根据所述反光器件的雷达坐标以及所述第一位姿，计算所述反光器件的世界坐标；
计算所述反光器件的世界坐标与所述路标地图中记录的该反光器件的世界坐标之间的欧式距离，将计算得到的所述欧氏距离作为所述反光器件基于所述第一位姿的位置误差估计值。
在一种可能的实现方式中，所述根据所述激光数据点基于所述第一位姿的位置误差估计值、以及所述反光器件基于所述第一位姿的位置误差估计值计算所述第一位姿的误差估计值，包括：
根据以下公式计算所述第一位姿的误差估计值ΔX：
Figure BDA0002871613520000021
Figure BDA0002871613520000031
Figure BDA0002871613520000032
其中，pi(X)用于表示第i个激光数据点基于第一位姿计算得到的世界坐标；qj(X)用于表示第j个反光器件基于第一位姿计算得到的世界坐标；MCt-1(pi(X))用于表示栅格地图中第i个激光数据点基于第一位姿计算得到的世界坐标所对应的栅格的概率；X表示第一位姿；MFt-1用于表示第j个反光器件对应的路标地图中的反光器件的世界坐标；
Figure BDA0002871613520000033
用于表示第i个激光数据点第一位姿计算得到的世界坐标所对应的栅格梯度；Ωp是第i个激光数据点基于第一位姿的位置误差估计值构成矩阵的信息矩阵；Ωq是第j个反光器件基于第一位姿的位置误差估计值构成矩阵的信息矩阵；H是所述激光数据点的雅可比矩阵；J是所述反光器件的雅可比矩阵。
在一种可能的实现方式中，判断所述第K位姿的误差估计值满足所述预设结束条件，包括：
K达到预设第一数值；和/或，
基于第K位姿计算得到的总误差估计值与基于第K-1位姿计算得到的总误差估计值之间的差值小于预设第二数值；
所述总误差估计值
Figure BDA0002871613520000034
其中，A是第i个激光数据点的位置误差估计值构成的矩阵，B是第j个反光器件的位置误差估计值构成的矩阵，Ωp是第i个激光数据点基于第N位姿的位置误差估计值构成矩阵的信息矩阵，N是K或者K-1，K是大于等于1的自然数，Ωq是第j个反光器件基于第N位姿的位置误差估计值构成矩阵的信息矩阵。
在一种可能的实现方式中，还包括：
确定所述激光强度数据中记录的反光器件的数量为0，计算所述第一位姿；
根据所述激光测距数据和所述第一位姿计算所述目标位姿；
根据所述激光测距数据和所述目标位姿更新所述栅格地图。
在一种可能的实现方式中，所述确定所述激光强度数据中记录的反光器件的数量不为0之后，还包括：
确定未建立所述路标地图，根据所述激光测距数据和所述第一位姿计算所述目标位姿；
根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿建立所述路标地图。
第二方面，本申请实施例提供一种地图构建装置，包括：
获取单元，用于获取激光测距数据和激光强度数据，所述激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息；所述激光强度数据用于记录所述激光雷达检测到的反光器件的位置信息；所述激光雷达设置于所述装置中；
第一计算单元，用于确定所述激光强度数据中记录的反光器件的数量不为0，计算第一位姿，所述第一位姿是所述激光雷达在世界坐标系中位姿的粗略估计值；
第二计算单元，用于根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿；所述目标位姿是所述激光雷达在世界坐标系中位姿的精确估计值；
地图更新单元，用于根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿更新所述路标地图。
第三方面，本申请实施例提供一种智能机器人设备，包括：
激光雷达，一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述设备执行时，使得所述设备执行第一方面任一项所述的方法。
第四方面，本申请实施例提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，当其在计算机上运行时，使得计算机执行第一方面任一项所述的方法。
第五方面，本申请提供一种计算机程序，当所述计算机程序被计算机执行时，用于执行第一方面所述的方法。
在一种可能的设计中，第五方面中的程序可以全部或者部分存储在与处理器封装在一起的存储介质上，也可以部分或者全部存储在不与处理器封装在一起的存储器上。
本申请实施例提供的地图构建方法中，获取激光测距数据和激光强度数据，激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息，激光强度数据用于记录激光雷达检测到的反光器件的位置信息，激光雷达设置于智能机器人设备中，确定激光强度数据中记录的反光器件的数量不为0，计算第一位姿，第一位姿是激光雷达在世界坐标系中位姿的粗略估计值，根据激光测距数据、激光强度数据以及第一位姿计算目标位姿，目标位姿是激光雷达在世界坐标系中位姿的精确估计值，根据激光测距数据和目标位姿更新栅格地图，根据激光强度数据和目标位姿更新路标地图，从而能够结合两种激光SLAM技术进行地图构建，降低了单一激光SLAM技术适用场景的局限性，相对于单一激光SLAM技术具有更广泛的应用场景。
附图说明
图1为现有技术长走廊环境下栅格地图无法构建的原理说明示例图；
图2为现有技术路标地图的构建原理说明示例图；
图3为本申请实施例智能机器人设备的结构示例图；
图4为本申请地图构建方法一个实施例的流程图；
图5为本申请地图构建方法另一个实施例的流程图；
图6为本申请栅格坐标示例图；
图7为本申请地图构建装置一个实施例的结构示意图。
具体实施方式
本申请的实施方式部分使用的术语仅用于对本申请的具体实施例进行解释，而非旨在限定本申请。
首先对本申请实施例中涉及的名词进行示例性而非限定性的说明。
SLAM：智能机器人设备在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上构建增量式地图。
激光SLAM：使用激光雷达作为智能机器人设备的外部传感器完成智能机器人设备的定位和地图构建。
反光器件：使用具有强反光的材料制成的、辅助激光雷达进行反光测量的器件，包括但不限于：反光板、反光柱等。激光雷达向反光器件发射激光信号时，基于反光器件的强反光特性，反光器件将返回具有很高强度的回波，从而激光雷达根据激光的回波强度，可以从激光数据中提取出反光器件的位置信息，位置信息具体可以为：反光器件在激光雷达坐标系中的坐标。
激光SLAM的过程中建立的地图可以包括栅格地图和/或路标地图。
栅格地图：将激光雷达探测到的区域划分为固定尺寸的栅格，为每个栅格设置一个概率值，概率越大表示该栅格存在障碍物的可能性越高，栅格地图可被用来进行智能机器人设备的定位和导航。
路标地图：基于环境中设置的反光器件构建的地图，记录有环境中反光器件的坐标，路标地图可被用来进行智能机器人设备的定位和导航。
激光雷达坐标系：基于激光雷达建立的坐标系。激光雷达可以以该坐标系为依据记录检测到的障碍物轮廓和反光器件的位置信息。本申请实施例中激光雷达坐标系的原点是激光雷达的物理中心点，在一种方式中，可以定义x轴正方向是激光雷达的水平正前方向，y轴正方向是激光雷达的水平左前方向。
世界坐标系：本申请实施例中构建栅格地图和路标地图所使用的坐标系。本申请实施例中世界坐标系的原点是智能机器人初始进入一个未知环境、首次使用激光雷达进行测量时激光雷达的物理中心点所在位置，在一种方式中，可以定义x轴正方向是智能机器人初始进入一个未知环境、首次使用激光雷达进行测量时激光雷达的水平正前方向，y轴正方向是智能机器人初始进入一个未知环境、首次使用激光雷达进行测量时激光雷达的水平左前方向。
设备的位姿：设备在所处环境中的位置和朝向。例如，智能机器人设备的位姿：智能机器人设备在所处环境中的位置和朝向。激光雷达的位姿是激光雷达在所处环境中的位置和朝向。对于设置有激光雷达的智能机器人设备，智能机器人设备的位姿可以通过激光雷达的位姿来表示。
本申请实施例中通过激光雷达的物理中心点在世界坐标系中的坐标和夹角来表示激光雷达的位姿。这里的夹角是指激光雷达的正方向与世界坐标系中的x轴正方向之间的夹角。
现有技术中，激光SLAM技术可以包括基于反光材料的激光SLAM技术和基于轮廓的激光SLAM技术。
基于轮廓的激光SLAM技术中，智能机器人设备使用激光雷达检测环境中障碍物轮廓的位置信息，基于轮廓的位置信息建立栅格地图，栅格地图包括：若干个大小相同的栅格以及每个栅格对应的概率，栅格对应的概率用于描述栅格对应的物理位置存在障碍物的可能性。
基于反光材料的激光SLAM技术中，在智能机器人设备所处环境中预先布置一定数量的反光器件，例如反光板或者反光柱，智能机器人根据激光雷达检测到的反光器件的位置信息建立路标地图，路标地图中包括：检测到的反光器件在世界坐标系中的坐标。
在某些结构性较差、长走廊等环境中，由于原理所限，栅格地图通常难以构建成功。典型的场景就是长走廊环境，如图1所示，斜条纹填充的部分120是走廊墙壁，圆形虚线表示智能机器人设备110处于中间位置时激光雷达的检测区域范围，此时激光测量数据为两条平行的直线，参见图中的2条虚线140，因此在与环境轮廓也即走廊墙壁匹配时，只能判断智能机器人设备110的位置位于图中虚线160上，无法确定具体位置。因此，在长走廊环境下，智能机器人设备110无法正常进行栅格地图构建。
基于反光材料的激光SLAM技术中，要求在环境中布置足够数量的反光器件，例如反光板或者反光柱等，并且基于激光雷达对于反光材料的测量特性，获取反光器件在环境中的位置，并且基于三点定位法，实现智能机器人设备的实时位姿估计，如图2所示，在长走廊的墙壁上设置反光器件210，最终可构建出环境中的路标地图，该地图可直接用于导航。但是，基于反光材料的激光SLAM技术基于三点定位法，也即激光雷达需要每次至少检测到3个反光板的位置才能实现对于智能机器人设备的定位，从而基于反光材料的激光SLAM技术对反光板的布置数量和位置有较为严格的要求：必须满足智能机器人设备处于工作路径上的每个点时智能机器人设备中的激光雷达至少能够观测到3个反光板。基于反光材料的激光SLAM技术对于反光板的上述数量要求，使得基于反光材料的激光SLAM技术更多应用于体型较高、且激光雷达安装位置较高的智能机器人设备例如叉车AGV等，如果智能机器人设备中激光雷达的安装位置较低，从而反光板的安装位置低，反光板很容易被环境中的障碍物或者行人遮挡，导致智能机器人设备无法实现定位，从而限制了基于反光材料的激光SLAM技术的应用场景。
为此，本申请提出一种地图构建方法，能够结合两种激光SLAM技术进行地图构建，相对于两种激光SLAM技术具有更广泛的应用场景。
以下对本申请实施例智能机器人的结构进行示例性说明。
如图3所示，智能机器人设备300包括：处理器310、存储器320、激光雷达330以及运动结构340；其中，处理器310、存储器320、激光雷达330以及运动结构340之间可以通过内部连接通路互相通信，传递控制和/或数据信号，该存储器320用于存储计算机程序，该处理器310用于从该存储器320中调用并运行该计算机程序。
上述存储器320可以是只读存储器(read-only memory，ROM)、可存储静态信息和指令的其它类型的静态存储设备、随机存取存储器(random access memory，RAM)或可存储信息和指令的其它类型的动态存储设备，也可以是电可擦可编程只读存储器(electrically erasable programmable read-only memory，EEPROM)、只读光盘(compactdisc read-only memory，CD-ROM)或其他光盘存储、光碟存储(包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等)、磁盘存储介质或者其它磁存储设备，或者还可以是能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其它介质等。
上述处理器310可以和存储器320可以合成一个处理装置，更常见的是彼此独立的部件，处理器310用于执行存储器320中存储的程序代码来实现上述功能。具体实现时，该存储器320也可以集成在处理器310中，或者，独立于处理器310。
激光雷达330，用于进行激光扫描，获得环境中的轮廓数据和/或反光器件数据，将上述数据发送至处理器310。
运动结构340，用于在处理器310的控制下，通过自身的运动使得智能机器人设备发生移动。
应理解，图3所示的智能机器人设备300中的处理器310可以是片上系统SOC，该处理器310中可以包括中央处理器(Central Processing Unit；以下简称：CPU)，还可以进一步包括其他类型的处理器，例如：图像处理器(Graphics Processing Unit；以下简称：GPU)等。
总之，处理器310内部的各部分处理器或处理单元可以共同配合实现之前的方法流程，且各部分处理器或处理单元相应的软件程序可存储在存储器330中。
应理解，图3所示的智能机器人设备300能够实现本申请实施例提供的方法的各个过程。智能机器人设备300中的各个模块的操作和/或功能，分别为了实现上述方法实施例中的相应流程。具体可参见本申请方法实施例中的描述，为避免重复，此处适当省略详细描述。
以下对本申请实施例地图构建方法进行示例性说明。
图4为本申请地图构建方法一个实施例的流程图，如图4所示，该方法可以包括：
步骤401：获取激光测距数据和激光强度数据，激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息；激光强度数据用于记录激光雷达检测到的反光器件的位置信息。
激光测距数据和激光强度数据可以从设置于智能机器人设备中的激光雷达获取。
步骤402：确定激光强度数据中记录的反光器件的数量不为0，计算第一位姿，第一位姿是激光雷达在世界坐标系中位姿的粗略估计值。
世界坐标系是构建路标地图和栅格地图所使用的坐标系。
本申请实施例中，激光雷达的位姿X可以通过(x,y,Ω)来表示。Ω是激光雷达处于当前方位下，激光雷达坐标系的x轴正方向与世界坐标系x轴正方向之间的夹角。
步骤403：根据激光测距数据、激光强度数据以及第一位姿计算目标位姿；目标位姿是激光雷达在世界坐标系中位姿的精确估计值。
步骤404：根据激光测距数据和目标位姿更新栅格地图，根据激光强度数据和目标位姿更新路标地图。
该方法根据激光测距数据、激光强度数据和第一位姿计算目标位姿，根据激光测距数据和目标位姿更新栅格地图，根据激光强度数据和目标位姿更新路标地图，从而能够结合两种激光SLAM技术进行地图构建，降低了单一激光SLAM技术适用场景的局限性，相对于单一激光SLAM技术具有更广泛的应用场景。
其中，激光雷达对于周围环境的激光扫描，可以按照周期触发执行，智能机器人设备触发执行本申请实施例地图构建方法也可以按照周期触发执行。智能机器人设备触发执行本申请实施例的地图构建方法的周期可以与激光雷达进行激光扫描的周期相同，从而激光雷达在每个周期的触发时刻完成激光扫描，得到激光测距数据和激光强度数据后，智能机器人设备可以及时基于激光雷达扫描得到的上述数据完成地图构建，使得智能机器人设备构建的地图更为精确。以下实施例中以智能机器人设备触发执行本申请实施例的地图构建方法的周期可以与激光雷达进行激光扫描的周期相同为例。以下实施例中第一时刻是激光雷达最近一次进行激光扫描的时刻，而第二时刻是激光雷达在第一时刻之前一次进行激光扫描的时刻。
图5为本申请地图构建方法另一个实施例的流程图，如图5所示，该方法可以包括：
步骤501：智能机器人设备获取第一时刻的激光测距数据和激光强度数据。
可选地，激光测距数据可以包括：m个激光数据点在激光雷达坐标系中的坐标，m是大于等于0的整数，m个激光数据点的坐标集合为(p1，p2，…，pm)。激光数据点是激光雷达监测到的障碍物轮廓上的点。
以下，为了描述方便，将某一点在激光雷达坐标系中的坐标称为该点的雷达坐标，将某一点在世界坐标系中的坐标称为该点的世界坐标。则上述p1～pm可以称为m个激光数据点的雷达坐标。
激光强度数据包括：n个反光器件在激光雷达坐标系中的坐标，n是大于等于0的整数，n个反光材料的坐标集合为(q1，q2，…，qn)。反光器件具有一定的表面积，本申请实施例中反光器件的雷达坐标可以通过反光器件的指定位置点例如中心点的雷达坐标来表示。
步骤502：智能机器人设备判断是否存在栅格地图，如果不存在，执行步骤503，如果存在，执行步骤504。
智能机器人设备初始进入一个未知环境、开始工作时，一般智能机器人设备中不存在栅格地图和路标地图，由智能机器人设备开始工作后，基于激光雷达进行激光扫描得到的数据相应建立栅格地图和路标地图，如步骤503和步骤509所示。
步骤503：智能机器人设备建立世界坐标系，根据第一时刻的激光测距数据建立栅格地图，本分支流程结束。
可选地，可以以第一时刻时激光雷达的物理中心点作为原点，以第一时刻时激光雷达的水平正前方向作为x轴正方向，第一时刻时激光雷达的水平左前方向作为y轴正方向，建立世界坐标系。也即世界坐标系和第一时刻的激光雷达坐标系可以重合，此时，两个坐标系中任一点的雷达坐标与世界坐标相同，激光雷达在第一时刻的位姿为(0，0，0°)。
栅格地图的建立方法可以包括：
确定栅格地图的覆盖区域，将覆盖区域划分为栅格，将每个栅格的概率设置为初始概率；
根据激光数据点的雷达坐标，计算激光数据点的世界坐标；
根据激光数据点的世界坐标确定激光数据点在栅格地图中对应的栅格，更新激光数据点对应的栅格的概率，更新后的概率大于更新前的概率。
其中，如果世界坐标系与第一时刻的激光雷达坐标系重合，则激光数据点的世界坐标与雷达坐标相同。
可选地，栅格地图的覆盖区域可以根据激光雷达可探测距离、和角度等确定，例如，激光雷达可探测距离为30米，可探测角度为360度，则栅格地图的覆盖区域可以是世界坐标系原点为中心点、半径为30米的圆形。
其中，栅格地图的覆盖区域划分栅格的方法本申请实施例不作限定，优选的，不同栅格的大小相同。可选地，为每个栅格设置的初始概率可以为0。概率的更新幅度本申请实施例不作限定，例如可以是预设的一个固定数值，该数值小于1。
智能机器人设备在栅格地图中可以记录每个栅格的指定位置点例如中心点的世界坐标，从而实现栅格地图中栅格的记录。
每个栅格的概率是大于等于0小于等于1的数值。
步骤504：智能机器人设备判断激光强度数据中记录的反光器件的数量是否为0，如果否，执行步骤505，如果是，执行步骤508。
步骤505：智能机器人设备判断是否存在路标地图，如果存在，执行步骤506；否则，执行步骤509。
步骤506：智能机器人设备计算第一时刻时激光雷达在世界坐标系中的第一位姿(以下简称为第一时刻的第一位姿)，根据激光测距数据、激光强度数据和第一位姿计算第一时刻时激光雷达在世界坐标系中的目标位姿(以下简称为第一时刻的目标位姿)；执行步骤507。
其中，第一时刻的第一位姿是对第一时刻时激光雷达在世界坐标系中的位姿的粗略估计值。第一时刻的目标位姿是对第一时刻的第一位姿进行修正后得到的、激光雷达在世界坐标系中位姿的相对更为精确的估计值。可选地，智能机器人设备可以根据第二时刻的目标位姿，以及在第二时刻到第一时刻之间智能机器人设备的移动里程估计第一时刻的第一位姿。其中，第二时刻是第一时刻之前一次激光雷达触发激光扫描的时刻。
假设第二时刻的目标位姿X2为(x2,y2,θ2)，第二时刻到第一时刻的里程估计ΔX1为(Δx1,Δy1,Δθ1),那么第一时刻的第一位姿X1为(x1,y1,θ1)计算如下：
Figure BDA0002871613520000091
其中，第二时刻到第一时刻的里程估计ΔX1可以由智能机器人设备从智能机器人设备中设置的里程计获取。
其中，上述根据激光测距数据、激光强度数据和第一位姿计算第一时刻的目标位姿，可以包括：
根据激光测距数据、激光强度数据和第一位姿计算第一位姿的误差估计值；第一位姿的误差估计值是第一位姿相对于激光雷达的实际位姿的误差估计值；
根据第一位姿和第一位姿的误差估计值计算第二位姿；
判断第一位姿的误差估计值是否满足预设结束条件；
如果第一位姿的误差估计值满足预设结束条件，将第二位姿作为目标位姿；
如果第一位姿的误差估计值不满足预设结束条件，使用第二位姿替换第一位姿，返回计算第二位姿的误差估计值、计算第三位姿、判断第二位姿的误差估计值是否满足预设结束条件，循环至第K位姿的误差估计值满足预设结束条件，将第K+1位姿作为目标位姿，K是大于1的自然数。
其中，上述根据激光测距数据、激光强度数据和第一位姿计算第一位姿的误差估计值，可以包括：
根据激光数据点的雷达坐标以及第一位姿计算激光数据点基于第一位姿的位置误差估计值；激光数据点基于第一位姿的位置误差估计值用于表示激光数据点基于第一位姿计算得到的世界坐标与激光数据点的实际世界坐标之间的误差估计值；
根据反光器件的雷达坐标以及第一位姿计算反光器件基于第一位姿的位置误差估计值；反光器件基于第一位姿的位置误差估计值用于表示反光器件基于第一位姿计算得到的世界坐标与反光器件的实际世界坐标之间的误差估计值；
根据激光数据点基于第一位姿的位置误差估计值、反光器件基于第一位姿的位置误差估计值、以及第一位姿计算第一位姿的误差估计值；第一位姿的误差估计值是第一位姿相对于激光雷达的实际位姿的误差估计值。
其中，上述计算激光数据点基于第一位姿的位置误差估计值，可以包括：
对于每个激光数据点，根据激光数据点的雷达坐标以及第一时刻的第一位姿，计算激光数据点基于第一位姿的世界坐标，根据激光数据点基于第一位姿的世界坐标确定激光数据点在第二时刻的栅格地图中对应的栅格，获取激光数据点对应栅格的概率，根据激光数据点对应栅格的概率计算激光数据点基于第一位姿的位置误差估计值。
由于智能机器人设备在每个触发时刻执行本申请实施例的地图构建方法后，会更新栅格地图、或者更新栅格地图和路标地图，因此，每个触发时刻的栅格地图不同，在第一时刻所使用的地图是基于第二时刻激光扫描结果更新后的地图。
其中，激光数据点基于第一位姿的位置误差估计值epi的计算公式可以为：
Figure BDA0002871613520000101
Figure BDA0002871613520000102
X表示第一时刻的第一位姿，pi是第一时刻的激光测距数据中第i个激光数据点的雷达坐标，
Figure BDA0002871613520000103
表示第一时刻的激光测距数据中第i个激光数据点对应栅格的概率，
Figure BDA0002871613520000104
表示第i个激光数据点基于第一位姿计算得到的世界坐标。
根据第一时刻的激光数据点的世界坐标确定激光数据点在栅格地图中对应栅格的概率时，由于智能机器人设备的移动，激光雷达随之移动，从而激光雷达可能扫描到了之前未扫描到的新区域，第一时刻的激光测距数据中的某些激光数据点可能是新区域中的点，而第二时刻的栅格地图中未包括该新区域，从而可能存在第一时刻的激光数据点无法在第二时刻的栅格地图中查找到对应栅格的情况，此时，可以将该激光数据点的概率确定为栅格的初始概率。
其中，假设任一点p点的雷达坐标为p(xA1,yA1)，激光雷达在世界坐标系中的位姿X为(x,y,θ)，则p点基于位姿X计算得到的世界坐标(xA2,yA2)，计算公式可以如以下公式(1)所示：
Figure BDA0002871613520000105
其中，激光数据点基于位姿X的世界坐标和反光器件基于位姿X的世界坐标都可以根据公式1计算得到，例如，对于激光数据点1来说，将激光数据点1的雷达坐标、以及第一位姿代入上述公式1，即可以计算得到激光数据点1基于第一位姿的世界坐标。其他激光数据点和反光器件的世界坐标的计算方法不再赘述。
其中，反光器件基于第一位姿的位置误差估计值可以是：激光强度数据中反光器件基于第一位姿的世界坐标与路标地图中该反光器件的世界坐标之间的欧式距离，则，
计算反光器件基于第一位姿的位置误差估计值，可以包括：
对于每个反光器件，根据反光器件的雷达坐标以及第一位姿，计算反光器件基于第一位姿的世界坐标；确定在路标地图中记录的该反光器件的世界坐标，计算两个世界坐标之间的欧式距离，作为反光器件基于第一位姿的位置误差估计值。
举例来说，假设反光器件1基于第一位姿的世界坐标为W1，路标地图中记录的该反光器件的世界坐标为W2，则计算W1和W2之间的欧式距离，可以作为反光器件1基于第一位姿的位置误差估计值。
其中，本申请实施例中可以根据激光强度数据中一反光器件B基于第一位姿的世界坐标与路标地图中已记录的反光器件的世界坐标之间的距离来确定路标地图中记录的哪个反光器件是反光器件B。例如，如果路标地图中已记录反光器件1～5的世界坐标，而反光器件B基于第一位姿的世界坐标与路标地图中反光器件2的世界坐标之间的距离最小、且距离小于预设阈值，则反光器件B就是路标地图中记录的反光器件2，如果路标地图中已记录的反光器件1～5中不存在满足上述条件的反光器件，可以认为反光器件B是路标地图中未记录的反光器件，可以将该反光器件B从激光强度数据中剔除出去，不作为计算第一时刻的目标位姿的依据。
其中，上述根据激光数据点基于第一位姿的位置误差估计值、反光器件基于第一位姿的位置误差估计值、以及第一位姿计算第一位姿的误差估计值，可以包括：
根据以下公式计算第一位姿的误差估计值ΔX：
Figure BDA0002871613520000111
Figure BDA0002871613520000112
Figure BDA0002871613520000113
其中，pi(X)用于表示第i个激光数据点基于第一位姿计算得到的世界坐标系中的坐标；qj(X)用于表示第j个反光器件基于第一位姿计算得到的世界坐标系中的坐标；MCt-1(pi(X))用于表示栅格地图中第i个激光数据点基于第一位姿计算得到的世界坐标所对应的栅格的概率；X表示第一位姿；MFt-1用于表示第j个反光器件对应的路标地图中的反光器件的世界坐标；
Figure BDA0002871613520000114
用于表示第i个激光数据点基于第一位姿计算得到的世界坐标所对应的栅格梯度；Ωp是第i个激光数据点基于第一位姿的位置误差估计值构成矩阵的信息矩阵；Ωq是第j个反光器件基于第一位姿的位置误差估计值构成矩阵的信息矩阵；H是用于和栅格地图匹配的激光数据点的雅可比矩阵；J是用于和路标地图匹配的反光器件的雅可比矩阵。
以上说明中，对与第一位姿相关的计算进行了举例说明，分别将第一位姿替换为第二位姿，第三位姿，…，或者第K位姿，从而可以对应计算与第二位姿相关的计算～与第K位姿相关的计算，这里不赘述。
以下，对本步骤的实现原理进行说明。
仍以第一位姿为例。其中，第一位姿对应的位置误差总估计值S的计算公式可以为：S＝S1+S2；其中，S1是激光数据点基于第一位姿的位置误差总估计值，S2是反光器件基于第一位姿的位置误差总估计值。
设
Figure BDA0002871613520000121
其中，A是由激光数据点i的位置误差估计值epi构成的矩阵，Ωp是矩阵A的信息矩阵，也即矩阵A的协方差矩阵的逆矩阵；
设
Figure BDA0002871613520000122
其中，B是由反光器件j对应的位置误差估计值eqj构成的矩阵，Ωq是矩阵B的信息矩阵，也即矩阵B的协方差矩阵的逆矩阵。
从而，对于栅格地图来说，基于前述描述，第i个激光数据点的位置误差估计值epi计算公式如下式(2)所示：
Figure BDA0002871613520000123
其中，MCt-1(·)用于表示第二时刻的栅格地图中栅格的概率。X是第一时刻的第一位姿；ΔX是第一位姿的误差估计值。
以下为了描述方便，将
Figure BDA0002871613520000124
简写为MCt-1(pi(X))，那么激光数据点基于第一位姿的位置误差总估计值计算公式如下：
Figure BDA0002871613520000125
其中，假设Ωp＝I，I是单位矩阵。
使用一阶泰勒展开对S1进行近似：
Figure BDA0002871613520000126
其中，
Figure BDA0002871613520000127
表示栅格梯度。
那么，问题转化为寻找合适的第一位姿的误差估计值ΔX，使得激光数据点基于第一位姿的位置误差总估计值S1最小，直接令上式对ΔX的求导结果为0，即可得到令S1最小的ΔX，求导结果和ΔX计算如公式(5)-公式(8)所示：
Figure BDA0002871613520000128
根据公式(5)求解ΔX，得到：
Figure BDA0002871613520000129
Figure BDA00028716135200001210
根据公式(1)可计算：
Figure BDA00028716135200001211
公式(7)-公式(8)代入公式(6)即可计算得到第一位姿的误差估计值ΔX。
为了使得计算得到的目标位姿更为准确，可以令X＝X+ΔX，从而得到第一位姿的修正值，本申请实施例中称为第二位姿，将第一位姿迭代为第二位姿基于前述过程进行计算，直到位置误差总估计值S1收敛，也即相邻两次计算得到的位姿代入公式(3)，计算得到的S1之间的差值小于预设阈值。为了防止迭代次数过多导致的智能机器人设备计算目标位姿的处理速度慢和数据处理量过大的问题，可以为迭代次数设置阈值，如果迭代次数达到阈值，即便S1未收敛，仍然停止迭代处理，将最终计算得到的位姿作为目标位姿。
以下说明栅格概率MCt-1(·)另一种计算方法以及栅格梯度
Figure BDA00028716135200001212
的计算方法：
参见图6，假设已有栅格坐标P00(x0，y0)，P01(x0，y1)，P10(x1，y0)，P11(x1，y1)，每个栅格对应的概率为M(P00)，M(P01)，M(P10)，M(P11)，则基于已有栅格坐标可以计算一指定坐标Pm(x，y)的栅格概率的坐标MCt-1(Pm)以及栅格梯度
Figure BDA00028716135200001213
具体公式如下：
Figure BDA0002871613520000131
Figure BDA0002871613520000132
Figure BDA0002871613520000133
对于公式(5)～(7)中的激光数据点i的坐标pi对应的栅格概率以及栅格梯度可以基于以上公式计算得到。
其中，对于路标地图来说，基于前述描述，第j个反光器件的位置误差估计值eqj计算公式如下公式(9)所示：
Figure BDA0002871613520000134
其中，dis(a，b)表示a与b之间的欧式距离，MFt-1表示第j个反光器件对应的第二时刻的路标地图中的反光器件在世界坐标系中的坐标；
Figure BDA0002871613520000135
表示第j个反光器件在激光雷达坐标系中的qj基于位姿(X+ΔX)转换得到的世界坐标系中的坐标。
以下为了描述方便，将
Figure BDA0002871613520000136
简写为(qj(X))，那么反光器件基于第一位姿的位置误差总估计值S2计算公式如下：
Figure BDA0002871613520000137
其中，假设Ωq＝I，I是单位矩阵；
使用一阶泰勒展开对S2进行近似：
Figure BDA0002871613520000138
那么，问题转化为寻找合适的第一位姿的误差估计值ΔX，使得反光器件基于第一位姿的位置误差总估计值S2最小，直接令上式对ΔX的求导结果为0，即可得到令S2最小的ΔX，求导结果和ΔX计算如公式(12)-公式(14)所示：
Figure BDA0002871613520000139
根据公式(12)求解ΔX，得到：
Figure BDA00028716135200001310
Figure BDA00028716135200001311
根据公式(1)可计算：
Figure BDA00028716135200001312
公式(14)-公式(15)代入式(13)即可计算得到ΔX。
为了使得计算得到的目标位姿更为准确，可以令X＝X+ΔX，从而得到第一位姿的修正值，本申请实施例中称为第二位姿，将第一位姿迭代为第二位姿基于前述过程进行计算，直到位置误差总估计值S2收敛，也即相邻两次计算得到的位姿代入公式(11)，计算得到的S2之间的差值小于预设阈值。为了防止迭代次数过多导致的智能机器人设备计算目标位姿的处理速度慢和数据处理量过大的问题，可以为迭代次数设置阈值，如果迭代次数达到阈值，即便S2未收敛，仍然停止迭代处理，将最终计算得到的位姿作为目标位姿。.
基于同样的处理原则，如果需要计算使得位置误差总估计值S＝S1+S2最小的ΔX，可以将公式(6)与公式(13)联立，可得：
Figure BDA0002871613520000141
Figure BDA0002871613520000142
Figure BDA0002871613520000143
公式(16)中Ωp和Ωq分别表示栅格点和反光器件误差权重，权重越大表示该项误差占比越高，在优化时也会重点优化该部分误差。
步骤507：智能机器人设备根据第一时刻的激光测距数据和目标位姿更新栅格地图，根据第一时刻的激光强度数据和目标位姿更新路标地图，当前流程结束。
其中，根据激光测距数据和目标位姿更新栅格地图，可以包括：
根据目标位姿扩展栅格地图的覆盖区域，将新扩展的覆盖区域划分为栅格；
根据每个激光数据点的雷达坐标以及目标位姿，计算激光数据点基于目标位姿的世界坐标，根据世界坐标从栅格地图中查找到对应的栅格，更新该栅格的概率。
其中，根据激光强度数据和目标位姿更新路标地图，可以包括：
根据每个反光器件的雷达坐标以及目标位姿，计算反光器件基于目标位姿的世界坐标，根据世界坐标查找路标地图中是否记录有该反光器件的世界坐标；
如果是，根据计算得到的世界坐标更新路标地图中该反光器件的世界坐标；
如果否，在路标地图中记录该反光器件的世界坐标。
其中，根据计算得到的坐标更新路标地图中该反光器件的坐标时，可以根据已经检测到的该反光器件的坐标取平均值或者中位数，作为路标地图中该反光器件更新后的坐标。
步骤508：智能机器人设备计算第一时刻的第一位姿，根据激光测距数据和第一位姿计算第一时刻的目标位姿，根据激光测距数据和目标位姿更新栅格地图，当前流程结束。
本步骤的处理，可以参考步骤506中的对应描述，区别主要在于，本步骤中将计算第一位姿的误差估计值ΔX的公式从公式16替换为公式6，这里不赘述。
步骤509：智能机器人设备计算第一时刻时激光雷达在世界坐标系中的第一位姿，根据第一时刻的激光测距数据和第一位姿计算第一时刻时激光雷达在世界坐标系中的目标位姿，根据激光测距数据和目标位姿更新栅格地图，并且，根据激光强度数据和目标位姿建立路标地图，当前流程结束。
本步骤的处理，可以参考步骤506中的对应描述，区别主要在于，本步骤中将计算第一位姿的误差估计值ΔX的公式从公式16替换为公式6，这里不赘述。
根据激光强度数据和目标位姿建立路标地图，可以包括：
对于激光强度数据中的每个反光器件，根据反光器件的雷达坐标和目标位姿计算反光器件基于目标位姿的世界坐标；
记录每个反光器件基于目标位姿的世界坐标，得到路标地图。
本申请实施例的地图构建方法，如果激光雷达仅检测到激光测距数据，则根据激光测距数据计算目标位姿，进行地图更新，如果同时检测到激光测距数据和激光强度数据，则根据激光测距数据和激光强度数据计算目标位姿，进行地图更新，从而能够结合两种激光SLAM技术进行地图构建，降低了单一激光SLAM技术适用场景的局限性，相对于单一激光SLAM技术具有更广泛的应用场景。
可以理解的是，上述实施例中的部分或全部步骤或操作仅是示例，本申请实施例还可以执行其它操作或者各种操作的变形。此外，各个步骤可以按照上述实施例呈现的不同的顺序来执行，并且有可能并非要执行上述实施例中的全部操作。
图7为本申请地图构建装置一个实施例的结构图，该装置可以设置于智能机器人设备中，如图7所示，该装置700可以包括：
获取单元710，用于获取激光测距数据和激光强度数据，所述激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息；所述激光强度数据用于记录所述激光雷达检测到的反光器件的位置信息；所述激光雷达设置于所述装置中；
第一计算单元720，用于确定所述激光强度数据中记录的反光器件的数量不为0，计算第一位姿，所述第一位姿是所述激光雷达在世界坐标系中位姿的粗略估计值；
第二计算单元730，用于根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿；所述目标位姿是所述激光雷达在世界坐标系中位姿的精确估计值；
地图更新单元740，用于根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿更新所述路标地图。
可选地，第二计算单元730具体可以用于：根据所述激光测距数据、所述激光强度数据和所述第一位姿计算所述第一位姿的误差估计值；所述第一位姿的误差估计值是所述第一位姿相对于激光雷达的实际位姿的误差估计值；根据所述第一位姿和所述第一位姿的误差估计值计算第二位姿；判断所述第一位姿的误差估计值是否满足预设结束条件；如果所述第一位姿的误差估计值满足所述预设结束条件，将所述第二位姿作为所述目标位姿；如果所述第一位姿的误差估计值不满足所述预设结束条件，使用所述第二位姿替换所述第一位姿，返回计算所述第二位姿的误差估计值、计算第三位姿、判断所述第二位姿的误差估计值是否满足所述预设结束条件，循环至第K位姿的误差估计值满足所述预设结束条件，将所述第K+1位姿作为所述目标位姿，K是大于1的自然数。
可选地，所述激光测距数据包括：激光数据点的雷达坐标，所述激光强度数据包括：所述反光器件的雷达坐标；所述雷达坐标是雷达坐标系中的坐标；第二计算单元730具体可以用于：
根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点基于所述第一位姿的位置误差估计值；所述激光数据点基于第一位姿的位置误差估计值用于表示所述激光数据点基于第一位姿计算得到的世界坐标与所述激光数据点的实际世界坐标之间的误差估计值；所述世界坐标是世界坐标系中的坐标；
根据所述反光器件的雷达坐标以及所述第一位姿计算所述反光器件基于所述第一位姿的位置误差估计值；所述反光器件基于第一位姿的位置误差估计值用于表示所述反光器件基于第一位姿计算得到的世界坐标与所述反光器件的实际世界坐标之间的误差估计值；
根据所述激光数据点基于所述第一位姿的位置误差估计值、以及所述反光器件基于所述第一位姿的位置误差估计值计算所述第一位姿的误差估计值。
可选地，第二计算单元730具体可以用于：根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点的世界坐标；根据所述激光数据点的世界坐标确定所述激光数据点在所述栅格地图中对应的栅格；获取所述激光数据点对应栅格的概率；根据所述激光数据点对应栅格的概率计算所述激光数据点基于所述第一位姿的位置误差估计值。
可选地，第二计算单元730具体可以用于：根据所述反光器件的雷达坐标以及所述第一位姿，计算所述反光器件的世界坐标；计算所述反光器件的世界坐标与所述路标地图中记录的该反光器件的世界坐标之间的欧式距离，将计算得到的所述欧氏距离作为所述反光器件基于所述第一位姿的位置误差估计值。
可选地，第二计算单元730具体可以用于：根据以下公式计算所述第一位姿的误差估计值ΔX：
Figure BDA0002871613520000161
Figure BDA0002871613520000162
Figure BDA0002871613520000163
其中，pi(X)用于表示第i个激光数据点基于第一位姿计算得到的世界坐标；qj(X)用于表示第j个反光器件基于第一位姿计算得到的世界坐标；MCt-1(pi(X))用于表示栅格地图中第i个激光数据点基于第一位姿计算得到的世界坐标所对应的栅格的概率；X表示第一位姿；MFt-1用于表示第j个反光器件对应的路标地图中的反光器件的世界坐标；
Figure BDA0002871613520000164
用于表示第i个激光数据点第一位姿计算得到的世界坐标所对应的栅格梯度；Ωp是第i个激光数据点基于第一位姿的位置误差估计值构成矩阵的信息矩阵；Ωq是第j个反光器件基于第一位姿的位置误差估计值构成矩阵的信息矩阵；H是所述激光数据点的雅可比矩阵；J是所述反光器件的雅可比矩阵。
可选地，第二计算单元730具体可以用于：K达到预设第一数值；和/或，
基于第K位姿计算得到的总误差估计值与基于第K-1位姿计算得到的总误差估计值之间的差值小于预设第二数值；
所述总误差估计值
Figure BDA0002871613520000165
其中，A是第i个激光数据点的位置误差估计值构成的矩阵，B是第j个反光器件的位置误差估计值构成的矩阵，Ωp是第i个激光数据点基于第N位姿的位置误差估计值构成矩阵的信息矩阵，N是K或者K-1，K是大于等于1的自然数，Ωq是第j个反光器件基于第N位姿的位置误差估计值构成矩阵的信息矩阵。
可选地，第一计算单元720还可以用于：确定所述激光强度数据中记录的反光器件的数量为0，计算所述第一位姿；
第二计算单元730还可以用于：根据所述激光测距数据和所述第一位姿计算所述目标位姿；
地图更新单元740还可以用于：根据所述激光测距数据和所述目标位姿更新所述栅格地图。
可选地，第二计算单元730还可以用于：确定未建立所述路标地图，根据所述激光测距数据和所述第一位姿计算所述目标位姿；
地图更新单元740还可以用于：根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿建立所述路标地图。
图7所示实施例提供的装置可用于执行本申请图4～图5所示方法实施例的技术方案，其实现原理和技术效果可以进一步参考方法实施例中的相关描述。
应理解以上图7所示的装置的各个单元的划分仅仅是一种逻辑功能的划分，实际实现时可以全部或部分集成到一个物理实体上，也可以物理上分开。且这些单元可以全部以软件通过处理元件调用的形式实现；也可以全部以硬件的形式实现；还可以部分单元以软件通过处理元件调用的形式实现，部分单元通过硬件的形式实现。例如，地图更新单元单元可以为单独设立的处理元件，也可以集成在电子设备的某一个芯片中实现。其它单元的实现与之类似。此外这些单元全部或部分可以集成在一起，也可以独立实现。在实现过程中，上述方法的各步骤或以上各个单元可以通过处理器元件中的硬件的集成逻辑电路或者软件形式的指令完成。
例如，以上这些单元可以是被配置成实施以上方法的一个或多个集成电路，例如：一个或多个特定集成电路(Application Specific Integrated Circuit；以下简称：ASIC)，或，一个或多个微处理器(Digital Singnal Processor；以下简称：DSP)，或，一个或者多个现场可编程门阵列(Field Programmable Gate Array；以下简称：FPGA)等。再如，这些单元可以集成在一起，以片上系统(System-On-a-Chip；以下简称：SOC)的形式实现。
本申请实施例还提供一种智能机器人设备，包括：激光雷达，一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述设备执行时，使得所述设备执行本申请图4～图5所示实施例提供的方法。
本申请还提供一种智能机器人设备，所述设备包括存储介质和中央处理器，所述存储介质可以是非易失性存储介质，所述存储介质中存储有计算机可执行程序，所述中央处理器与所述非易失性存储介质连接，并执行所述计算机可执行程序以实现本申请图4～图5所示实施例提供的方法。
本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，当其在计算机上运行时，使得计算机执行本申请图4～图5所示实施例提供的方法。
本申请实施例还提供一种计算机程序产品，该计算机程序产品包括计算机程序，当其在计算机上运行时，使得计算机执行本申请图4～图5所示实施例提供的方法。
本申请实施例中，“至少一个”是指一个或者多个，“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示单独存在A、同时存在A和B、单独存在B的情况。其中A，B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项”及其类似表达，是指的这些项中的任意组合，包括单项或复数项的任意组合。例如，a，b和c中的至少一项可以表示：a，b，c，a和b，a和c，b和c或a和b和c，其中a，b，c可以是单个，也可以是多个。
本领域普通技术人员可以意识到，本文中公开的实施例中描述的各单元及算法步骤，能够以电子硬件、计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，任一功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory；以下简称：ROM)、随机存取存储器(Random Access Memory；以下简称：RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。本申请的保护范围应以所述权利要求的保护范围为准。

Claims (12)

1.一种地图构建方法，应用于智能机器人设备，其特征在于，包括：
获取激光测距数据和激光强度数据，所述激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息；所述激光强度数据用于记录所述激光雷达检测到的反光器件的位置信息；所述激光雷达设置于所述智能机器人设备中；
确定所述激光强度数据中记录的反光器件的数量不为0，计算第一位姿，所述第一位姿是所述激光雷达在世界坐标系中位姿的粗略估计值；
根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿；所述目标位姿是所述激光雷达在世界坐标系中位姿的精确估计值；
根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿更新所述路标地图。
2.根据权利要求1所述的方法，其特征在于，所述根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿，包括：
根据所述激光测距数据、所述激光强度数据和所述第一位姿计算所述第一位姿的误差估计值；所述第一位姿的误差估计值是所述第一位姿相对于激光雷达的实际位姿的误差估计值；
根据所述第一位姿和所述第一位姿的误差估计值计算第二位姿；
判断所述第一位姿的误差估计值是否满足预设结束条件；
如果所述第一位姿的误差估计值满足所述预设结束条件，将所述第二位姿作为所述目标位姿；
如果所述第一位姿的误差估计值不满足所述预设结束条件，使用所述第二位姿替换所述第一位姿，返回计算所述第二位姿的误差估计值、计算第三位姿、判断所述第二位姿的误差估计值是否满足所述预设结束条件，循环至第K位姿的误差估计值满足所述预设结束条件，将所述第K+1位姿作为所述目标位姿，K是大于1的自然数。
3.根据权利要求2所述的方法，其特征在于，所述激光测距数据包括：激光数据点的雷达坐标，所述激光强度数据包括：所述反光器件的雷达坐标；所述雷达坐标是雷达坐标系中的坐标；
所述根据所述激光测距数据、所述激光强度数据和所述第一位姿计算所述第一位姿的误差估计值，包括：
根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点基于所述第一位姿的位置误差估计值；所述激光数据点基于第一位姿的位置误差估计值用于表示所述激光数据点基于第一位姿计算得到的世界坐标与所述激光数据点的实际世界坐标之间的误差估计值；所述世界坐标是世界坐标系中的坐标；
根据所述反光器件的雷达坐标以及所述第一位姿计算所述反光器件基于所述第一位姿的位置误差估计值；所述反光器件基于第一位姿的位置误差估计值用于表示所述反光器件基于第一位姿计算得到的世界坐标与所述反光器件的实际世界坐标之间的误差估计值；
根据所述激光数据点基于所述第一位姿的位置误差估计值、以及所述反光器件基于所述第一位姿的位置误差估计值计算所述第一位姿的误差估计值。
4.根据权利要求3所述的方法，其特征在于，所述根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点基于所述第一位姿的位置误差估计值，包括：
根据所述激光数据点的雷达坐标以及所述第一位姿计算所述激光数据点的世界坐标；
根据所述激光数据点的世界坐标确定所述激光数据点在所述栅格地图中对应的栅格；
获取所述激光数据点对应栅格的概率；
根据所述激光数据点对应栅格的概率计算所述激光数据点基于所述第一位姿的位置误差估计值。
5.根据权利要求3所述的方法，其特征在于，所述根据所述反光器件的雷达坐标以及所述第一位姿计算所述反光器件基于所述第一位姿的位置误差估计值，包括：
根据所述反光器件的雷达坐标以及所述第一位姿，计算所述反光器件的世界坐标；
计算所述反光器件的世界坐标与所述路标地图中记录的该反光器件的世界坐标之间的欧式距离，将计算得到的所述欧氏距离作为所述反光器件基于所述第一位姿的位置误差估计值。
6.根据权利要求3任一项所述的方法，其特征在于，所述根据所述激光数据点基于所述第一位姿的位置误差估计值、以及所述反光器件基于所述第一位姿的位置误差估计值计算所述第一位姿的误差估计值，包括：
根据以下公式计算所述第一位姿的误差估计值ΔX：
Figure FDA0002871613510000021
Figure FDA0002871613510000022
Figure FDA0002871613510000023
其中，pi(X)用于表示第i个激光数据点基于第一位姿计算得到的世界坐标；qj(X)用于表示第j个反光器件基于第一位姿计算得到的世界坐标；MCt-1(pi(X))用于表示栅格地图中第i个激光数据点基于第一位姿计算得到的世界坐标所对应的栅格的概率；X表示第一位姿；MFt-1用于表示第j个反光器件对应的路标地图中的反光器件的世界坐标；
Figure FDA0002871613510000024
用于表示第i个激光数据点第一位姿计算得到的世界坐标所对应的栅格梯度；Ωp是第i个激光数据点基于第一位姿的位置误差估计值构成矩阵的信息矩阵；Ωq是第j个反光器件基于第一位姿的位置误差估计值构成矩阵的信息矩阵；H是所述激光数据点的雅可比矩阵；J是所述反光器件的雅可比矩阵。
7.根据权利要求2至6任一项所述的方法，其特征在于，判断所述第K位姿的误差估计值满足所述预设结束条件，包括：
K达到预设第一数值；和/或，
基于第K位姿计算得到的总误差估计值与基于第K-1位姿计算得到的总误差估计值之间的差值小于预设第二数值；
所述总误差估计值
Figure FDA0002871613510000031
其中，A是第i个激光数据点的位置误差估计值构成的矩阵，B是第j个反光器件的位置误差估计值构成的矩阵，Ωp是第i个激光数据点基于第N位姿的位置误差估计值构成矩阵的信息矩阵，N是K或者K-1，K是大于等于1的自然数，Ωq是第j个反光器件基于第N位姿的位置误差估计值构成矩阵的信息矩阵。
8.根据权利要求1至7任一项所述的方法，其特征在于，还包括：
确定所述激光强度数据中记录的反光器件的数量为0，计算所述第一位姿；
根据所述激光测距数据和所述第一位姿计算所述目标位姿；
根据所述激光测距数据和所述目标位姿更新所述栅格地图。
9.根据权利要求1至8任一项所述的方法，其特征在于，所述确定所述激光强度数据中记录的反光器件的数量不为0之后，还包括：
确定未建立所述路标地图，根据所述激光测距数据和所述第一位姿计算所述目标位姿；
根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿建立所述路标地图。
10.一种地图构建装置，其特征在于，包括：
获取单元，用于获取激光测距数据和激光强度数据，所述激光测距数据用于记录激光雷达检测到的障碍物轮廓的位置信息；所述激光强度数据用于记录所述激光雷达检测到的反光器件的位置信息；所述激光雷达设置于所述装置中；
第一计算单元，用于确定所述激光强度数据中记录的反光器件的数量不为0，计算第一位姿，所述第一位姿是所述激光雷达在世界坐标系中位姿的粗略估计值；
第二计算单元，用于根据所述激光测距数据、所述激光强度数据以及所述第一位姿计算目标位姿；所述目标位姿是所述激光雷达在世界坐标系中位姿的精确估计值；
地图更新单元，用于根据所述激光测距数据和所述目标位姿更新所述栅格地图，根据所述激光强度数据和所述目标位姿更新所述路标地图。
11.一种智能机器人设备，其特征在于，包括：
激光雷达，一个或多个处理器；存储器；以及一个或多个计算机程序，其中所述一个或多个计算机程序被存储在所述存储器中，所述一个或多个计算机程序包括指令，当所述指令被所述设备执行时，使得所述设备执行权利要求1至9任一项所述的方法。
12.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序，当其在计算机上运行时，使得计算机执行权利要求1至9任一项所述的方法。
CN202011604615.7A 2020-12-30 2020-12-30 地图构建方法、装置和设备 Pending CN112731337A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011604615.7A CN112731337A (zh) 2020-12-30 2020-12-30 地图构建方法、装置和设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011604615.7A CN112731337A (zh) 2020-12-30 2020-12-30 地图构建方法、装置和设备

Publications (1)

Publication Number Publication Date
CN112731337A true CN112731337A (zh) 2021-04-30

Family

ID=75610802

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011604615.7A Pending CN112731337A (zh) 2020-12-30 2020-12-30 地图构建方法、装置和设备

Country Status (1)

Country Link
CN (1) CN112731337A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113390427A (zh) * 2021-06-09 2021-09-14 深圳市优必选科技股份有限公司 机器人建图方法、装置、机器人及计算机可读存储介质
CN116068564A (zh) * 2023-02-14 2023-05-05 南京天创电子技术有限公司 一种长距离隧道场景下的机器人激光定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109613547A (zh) * 2018-12-28 2019-04-12 芜湖哈特机器人产业技术研究院有限公司 一种基于反光板的占据栅格地图构建方法
CN110389590A (zh) * 2019-08-19 2019-10-29 杭州电子科技大学 一种融合2d环境地图和稀疏人工地标的agv定位系统及方法
CN111895989A (zh) * 2020-06-24 2020-11-06 浙江大华技术股份有限公司 一种机器人的定位方法、装置和电子设备

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN109613547A (zh) * 2018-12-28 2019-04-12 芜湖哈特机器人产业技术研究院有限公司 一种基于反光板的占据栅格地图构建方法
CN110389590A (zh) * 2019-08-19 2019-10-29 杭州电子科技大学 一种融合2d环境地图和稀疏人工地标的agv定位系统及方法
CN111895989A (zh) * 2020-06-24 2020-11-06 浙江大华技术股份有限公司 一种机器人的定位方法、装置和电子设备

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
周凯月 等: "融合反光柱的2D激光SLAM和高精度定位系统", 《现代计算机 研究与开发》, pages 3 - 7 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113390427A (zh) * 2021-06-09 2021-09-14 深圳市优必选科技股份有限公司 机器人建图方法、装置、机器人及计算机可读存储介质
WO2022257332A1 (zh) * 2021-06-09 2022-12-15 深圳市优必选科技股份有限公司 机器人建图方法、装置、机器人及计算机可读存储介质
CN116068564A (zh) * 2023-02-14 2023-05-05 南京天创电子技术有限公司 一种长距离隧道场景下的机器人激光定位方法

Similar Documents

Publication Publication Date Title
CN111060113B (zh) 一种地图更新方法及装置
Wen et al. Correcting NLOS by 3D LiDAR and building height to improve GNSS single point positioning
Lenac et al. Fast planar surface 3D SLAM using LIDAR
US6442476B1 (en) Method of tracking and sensing position of objects
US8558679B2 (en) Method of analyzing the surroundings of a vehicle
Rapp et al. Probabilistic ego-motion estimation using multiple automotive radar sensors
Barjenbruch et al. Joint spatial-and Doppler-based ego-motion estimation for automotive radars
Han et al. Precise localization and mapping in indoor parking structures via parameterized SLAM
Leonard et al. Application of multi-target tracking to sonar-based mobile robot navigation
US11506511B2 (en) Method for determining the position of a vehicle
CN112731337A (zh) 地图构建方法、装置和设备
US20210223776A1 (en) Autonomous vehicle with on-board navigation
CN113743171A (zh) 目标检测方法及装置
CN107153422A (zh) 飞机着陆系统和方法
Li et al. Hybrid filtering framework based robust localization for industrial vehicles
Ghallabi et al. LIDAR-based high reflective landmarks (HRL) s for vehicle localization in an HD map
Schütz et al. Simultaneous tracking and shape estimation with laser scanners
Schütz et al. Multiple extended objects tracking with object-local occupancy grid maps
Braillon et al. Occupancy grids from stereo and optical flow data
CN110794434B (zh) 一种位姿的确定方法、装置、设备及存储介质
CN116202509A (zh) 一种面向室内多层建筑的可通行地图生成方法
Piasecki Global localization for mobile robots by multiple hypothesis tracking
CN114459467B (zh) 一种未知救援环境中基于vi-slam的目标定位方法
Selkäinaho Adaptive autonomous navigation of mobile robots in unknown environments
Guivant et al. Simultaneous localization and map building: Test case for outdoor applications

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant after: Hangzhou Hikvision Robot Co.,Ltd.

Address before: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd.