CN113819914A - 一种地图构建方法及装置 - Google Patents

一种地图构建方法及装置 Download PDF

Info

Publication number
CN113819914A
CN113819914A CN202010566851.8A CN202010566851A CN113819914A CN 113819914 A CN113819914 A CN 113819914A CN 202010566851 A CN202010566851 A CN 202010566851A CN 113819914 A CN113819914 A CN 113819914A
Authority
CN
China
Prior art keywords
pose
point cloud
map
frame
movable object
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
CN202010566851.8A
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.)
Beijing Tusen Weilai Technology Co Ltd
Original Assignee
Beijing Tusen Weilai 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 Beijing Tusen Weilai Technology Co Ltd filed Critical Beijing Tusen Weilai Technology Co Ltd
Priority to CN202010566851.8A priority Critical patent/CN113819914A/zh
Publication of CN113819914A publication Critical patent/CN113819914A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Abstract

本申请提供了一种地图构建方法及装置，涉及高精度地图技术领域。方法包括：获得各种传感器采集的传感器数据；基于多传感器融合的里程计方法，构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；进行基于帧的全局优化，确定各帧优化位姿；根据各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。

Description

一种地图构建方法及装置
技术领域
本申请涉及高精度地图技术领域，尤其涉及一种地图构建方法及装置。
背景技术
目前，随着自动驾驶技术、智能机器人技术的发展，如何保证自动驾驶车辆和智能机器人的精确行驶成为了一个热点问题。在自动驾驶技术中，一般会应用高精度地图，其不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使自动驾驶车辆等达到厘米级的精度，这对确保自动驾驶车辆的安全至关重要。
在自动驾驶领域，高精度地图的构建一般需要通过激光雷达来采集数据，因此在构建高精度地图时，首先形成的是高精度点云地图。高精度点云地图的形成需要依赖良好的定位环境和里程计算法，在一些卫星信号较差的路段(长隧道、偏远山区)，高精度点云地图的构建结果较差。本申请旨在提供一种针对于卫星信号较差的路段的高精度点云地图的构建方案。
发明内容
本申请的实施例提供一种地图构建方法及装置，能够实现卫星信号较差的路段的高精度点云地图的构建，进而保证自动驾驶车辆、智能机器人等的正常行驶。
为达到上述目的，本申请的实施例采用如下技术方案：
本申请实施例的第一方面，提供一种地图构建方法，包括：
获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据；
基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；
通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；
获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；
根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；
根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿；
根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
本申请实施例的第二方面，提供一种地图构建装置，包括：
数据获得单元，用于获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据；
多传感器融合单元，用于基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；
子地图位姿约束建立单元，用于通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；
位姿观测约束建立单元，用于获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；
基于子地图的全局优化单元，用于根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；
基于帧的全局优化单元，用于根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿；
全局地图形成单元，用于根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
本申请实施例的第三方面，提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述第一方面所述的方法。
本申请实施例的第四方面，提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如上述第一方面所述的方法。
本申请实施例的第五方面，提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现上述第一方面所述的方法。
本申请实施例提供的一种地图构建方法及装置，首先获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据；然后基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；之后通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；之后获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；之后根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；之后根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿；进而根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。可见，本申请通过构建独立的局部点云地图、基于子地图的全局优化、基于帧的全局优化，有效利用了里程计算法短时间内精度高、预设路段前后预设道路范围定位精度高的特点，同时避免了优化算法陷入局部优化的问题，能够实现卫星信号较差的路段的高精度点云地图的构建，进而保证自动驾驶车辆、智能机器人等的正常行驶。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种地图构建方法的流程图一；
图2为本申请实施例提供的一种地图构建方法的流程图二；
图3为本申请实施例中的隧道场景示意图；
图4为本申请实施例中在隧道场景内采用现有技术方法与本申请实施例提供的基于多传感器融合的里程计方法的结果对比示意图；
图5为本申请实施例中的地图构建方法所产生的地图的比较结果示意图；
图6为本申请实施例提供的一种地图构建装置的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了使本领域的技术人员更好的了解本申请，下面先对本申请实施例中出现的部分技术术语进行解释如下：
可移动物体：是指车辆、移动机器人、飞行器等可进行地图采集的物体，可移动物体上可以搭载各类型传感器，如激光雷达、相机等。
GNSS：Global Navigation Satellite System，全球导航卫星系统。
GPS：Global Positioning System，全球定位系统。
IMU：Inertial Measurement Unit，惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。
高精度地图：不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使车辆达到厘米级的精度，这对确保自动驾驶汽车的安全至关重要。
建图(Mapping)：根据估计得到的车辆或移动机器人实时位姿以及激光雷达等视觉传感器的采集数据，构建出描述当前场景的高精度地图。
位姿(Pose)：位置和朝向的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。朝向的3个自由度通常用俯仰角(Pitch)、翻滚角(Roll)、偏航角(Yaw)来表示。
帧(Frame)：传感器完成一次观测所接收到的测量数据，如相机的一帧数据为一张图片，激光雷达的一帧数据为一组激光点云。
子地图(Submap)：全局地图由若干个子地图组成，每个子地图包含连续多帧的观测结果。
配准(Registration)：对同一区域在不同时刻、不同位置的观测结果进行匹配，得到两个观测时刻间的相对位姿关系。
NDT：Normal Distributions Transform，即正态分布变换算法，是一种配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配。
NovAtel：精密全球导航卫星系统(GNSS)及其子系统领域中，处于领先地位的产品与技术供应商。本申请实施例中表示NovAtel的组合导航系统。
在本申请的一些实施例中，术语“车辆”广泛地解释为包括任何移动物体，包括例如飞行器、船只、航天器、汽车、卡车、厢式货车、半挂车、摩托车、高尔夫球车、越野车辆、仓库运输车辆或农用车以及行驶在轨道上的运输工具，例如电车或火车以及其它有轨车辆。本申请中的“车辆”通常可以包括：动力系统、传感器系统、控制系统、外围设备和计算机系统。在其它实施例中，车辆可以包括更多、更少或者不同的系统。
其中，动力系统是为车辆提供动力运动的系统，包括：引擎/马达、变速器和车轮/轮胎、能源单元。
控制系统可以包括控制车辆及其组件的装置的组合，例如转向单元、节气门、制动单元。
外围设备可以是允许车辆与外部传感器、其它车辆、外部计算设备和/或用户进行交互的设备，例如无线通信系统、触摸屏、麦克风和/或扬声器。
基于上述描述的车辆，自动驾驶车辆中还配置有传感器系统和自动驾驶控制装置。
传感器系统可以包括用于感测车辆所处环境的信息的多个传感器，以及改变传感器的位置和/或方向的一个或多个致动器。传感器系统可以包括全球定位系统传感器、惯性测量单元、无线电检测和测距(RADAR)单元、相机、激光测距仪、光检测和测距(LIDAR)单元和/或声学传感器等传感器的任何组合；传感器系统还可以包括监视车辆内部系统的传感器(例如O2监视器、燃油表、引擎温度计等)。
自动驾驶控制装置可以包括一个处理器和存储器，存储器中存储有至少一条机器可执行指令，处理器执行至少一条机器可执行指令实现包括地图引擎、定位模块、感知模块、导航或路径模块、以及自动控制模块等的功能。地图引擎和定位模块用于提供地图信息和定位信息。感知模块用于根据传感器系统获取到的信息和地图引擎提供的地图信息感知车辆所处环境中的事物。导航或路径模块用于根据地图引擎、定位模块和感知模块的处理结果，为车辆规划行驶路径。自动控制模块将导航或路径模块等模块的决策信息输入解析转换成对车辆控制系统的控制命令输出，并通过车载网(例如通过CAN总线、局域互联网络、多媒体定向系统传输等方式实现的车辆内部电子网络系统)将控制命令发送给车辆控制系统中的对应部件，实现对车辆的自动控制；自动控制模块还可以通过车载网来获取车辆中各部件的信息。
目前，在自动驾驶领域，常用的高精度点云地图构建算法主要有两种：一种是使用高精度组合导航系统获取车辆位姿，同时结合多线激光雷达观测数据构建全局高精度点云地图，如文献[1]([1]Haala,Norbert,et al."Mobile LiDAR mapping for 3D pointcloud collection in urban areas—A performance test."Int.Arch.Photogramm.Remote Sens.Spat.Inf.Sci 37(2008):1119-1127.)所述使用多台激光雷达设备实时扫描周围场景的三维数据，同时使用由高精度GPS和IMU组成的组合导航系统测量地图车的实时位姿，并根据组合导航系统与激光雷达之间的外参，计算出激光雷达的实时位姿，之后将激光点云按照位姿进行叠加，得到全局地图；另一种是使用如文献[2]([2]Zhang,Ji,and Sanjiv Singh."Low-drift and real-time lidar odometry andmapping."Autonomous Robots 41.2(2017):401-416.)所述的里程计算法，即不断的将当前帧所观测到的激光雷达点云与前一时刻或前几个时刻的激光点云进行配准，从而增量式估计当前车辆的实时位姿，并构建全局地图。
在隧道等场景下，由于卫星信号较差，组合导航系统可能无法接收到卫星信号，位姿估计误差极大，因此依赖于高精度组合导航系统的建图方式在卫星信号较差的区域，建图结果并不准确；
另外，依赖里程计算法的方案不可避免的存在累计误差，在长距离隧道等场景下，里程计算法在隧道出口处的累计误差过大，通常在数百米以上，无法在隧道内外建立平滑、高一致性的点云地图，无法满足无人驾驶中高精度定位、路径规划等算法模块的实际需求。
本申请实施例旨在提出一种地图构建方法，以克服现有技术中在长隧道等卫星信号较差、距离较长的环境下进行高精度点云地图的建图较为困难、准确性较差的问题。
如图1所示，本申请实施例提供一种地图构建方法，包括：
步骤101、获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据。
步骤102、基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束。
步骤103、通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束。
步骤104、获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束。
步骤105、根据子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿。
步骤106、根据帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿。
步骤107、根据各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
为了使本领域的技术人员能够更清楚的理解本申请的实施例，下面结合具体步骤、实例和附图来对本申请的实施例进一步进行说明。如图2所示，本申请实施例提供一种地图构建方法，该方法应用于一种搭载有多种传感器的可移动物体上，可移动物体可以是指自动驾驶车辆、智能机器人、无人机等，但不仅局限于此。预设路段可以是指待进行地图构建的路段，如隧道、卫星信号较差的高速公路、国道等，但不仅局限于此。另外，可移动物体上搭载的各种传感器可以包括惯性测量单元IMU、轮速计、激光雷达和气压计；其中，IMU包括加速度计和陀螺仪。
该方法包括：
步骤201、获得可移动物体在预设路段行驶时加速度计测量的三轴加速度数据、陀螺仪测量的三轴角速度数据、轮速计测量的可移动物体轮速数据、激光雷达测量的点云数据和气压计测量的高度观测数据。
在步骤201之后，继续执行步骤202至步骤205。
步骤202、根据加速度计测量的三轴加速度数据进行建模，建立可移动物体的横滚角约束和俯仰角约束。
IMU中的加速度计可以实时测量IMU坐标系下的三轴加速度数据，测量得到的三轴加速度数据一般由重力加速度和可移动物体自身加速度两部分组成，但由于可移动物体自身的加速度通常远小于重力加速度，因此可以忽略可移动物体自身加速度的影响。
具体的，此处步骤202可以采用如下方式实现：
根据加速度计测量的三轴加速度数据进行建模。
其中，所建立的数学模型有如下关系：
Figure BDA0002547921220000081
在以上数学模型中，ax、ay、az表示加速度计测量的三轴加速度数据；
Figure BDA0002547921220000082
为IMU坐标系到世界坐标系的旋转矩阵；g表示归一化后的重力加速度；ar表示车体加速度。
通过化简以上数学模型，可以确定IMU在世界坐标系下的横滚角估计值θroll和俯仰角估计值θpitch；其中，
Figure BDA0002547921220000091
ax、ay、az表示加速度计测量的三轴加速度数据。
为降低后续步骤中联合优化的自由度，避免里程计方法在隧道、跨海大桥等场景下因特征稀疏而快速退化，本申请提出将上述横滚角估计值θroll和俯仰角估计值θpitch作为固定约束加入到后续的联合优化过程中。此外，由于联合优化中姿态的状态变量需要用四元数表示，所以需先将四元数转换为旋转矩阵，之后再将旋转矩阵转为欧拉角形式，从而可以根据横滚角估计值θroll和俯仰角估计值θpitch，建立可移动物体的横滚角约束rRoll(X)和俯仰角约束rPitch(X)；其中，rRoll(X)＝θroll-arcsin(-R13)；rPitch(X)＝θpitch-arctan2(R23，R33)；X表示IMU在世界坐标系下的位姿，X为待优化的状态变量，X包括位置p和姿态q；R为待优化的状态变量X中的姿态q的旋转矩阵形式，R23、R33、R13分别为旋转矩阵R中对应行列的元素。
步骤203、根据陀螺仪测量的三轴角速度数据和轮速计测量的可移动物体轮速数据，采用阿克曼模型进行运动学建模，建立可移动物体的水平位置和偏航角的阿克曼模型约束。
具体的，此处步骤203可以采用如下方式实现：
本申请可以基于阿克曼模型进行可移动物体的运动学建模。其中，为便于计算，在阿克曼运动学模型中，一般以可移动物体的后轴(例如车辆后轴)的中心为原点建立车体坐标系。
一般情况下，阿克曼运动学模型的默认输入为可移动物体速度和方向盘转角，但在实际应用中，发明人发现方向盘转角的精度通常难以保证，为提高整个里程计方法的精度和鲁棒性，本申请应用世界坐标系下可移动物体前进方向与y轴夹角的角度积分值来替换方向盘转角。因此，此处需要根据陀螺仪测量的三轴角速度数据，确定世界坐标系下可移动物体前进方向与y轴夹角的角度积分值：
Figure BDA0002547921220000092
Figure BDA0002547921220000093
其中，θi表示第i时刻的可移动物体前进方向与y轴夹角的角度积分值；t表示第t时刻；
Figure BDA0002547921220000094
为预先获知的车体坐标系到IMU坐标系的旋转变换关系；
Figure BDA0002547921220000095
为第t时刻陀螺仪测量的三轴角速度数据中的偏航角。
之后，在阿克曼运动学模型中，可以根据轮速计测量的第i时刻的可移动物体左后轮在车体坐标系下的速度
Figure BDA0002547921220000096
和右后轮在车体坐标系下的速度
Figure BDA0002547921220000097
确定可移动物体后轴中心在车体坐标系下的速度vi；其中，
Figure BDA0002547921220000098
为预先获知的速度噪声。
之后，采用阿克曼模型的运动学建模，可以确定世界坐标系下可移动物体位姿传递方程：
xi+1＝xi+vi·Δt·sinθi
yi+1＝yi+vi·Δt·cosθi
其中，Δt为轮速计两个相邻测量时刻的时间差；xi、yi表示可移动物体在世界坐标系下的水平位置。
由于IMU、轮速计的测量频率通常高于激光雷达频率，因此可以根据激光雷达的测量频率，对相邻两个激光雷达第k时刻和第k+1时刻之间的xi、yi、θi进行积分，确定在世界坐标系下xi、yi、θi各自的改变量δxk(k+1)、δyk(k+1)、δθk(k+1)。
之后，可以根据车体坐标系和IMU坐标系之间的外参确定IMU坐标系到车体坐标系的位姿变换关系
Figure BDA0002547921220000101
并确定第k时刻到第k+1时刻之间的IMU在世界坐标系下的位姿变换关系
Figure BDA0002547921220000102
其中：
Figure BDA0002547921220000103
从而，可以建立可移动物体的阿克曼模型约束rAkerman(X)；其中：
Figure BDA0002547921220000104
X表示IMU在世界坐标系下的位姿，X为待优化的状态变量。例如式中，Xk、Xk+1分别为IMU在世界坐标系下在第k、第k+1时刻的位姿。
步骤204、根据激光雷达测量的点云数据进行建模，建立可移动物体的激光雷达位姿约束。
此处，该步骤204可以采用如下方式实现，例如包括如下步骤：
步骤2041、将激光雷达测量的各帧点云数据进行运动补偿，确定各帧点云数据中的点的运动补偿后的位置。
需要进行运动补偿的原因为：激光雷达一般为机械式结构，完成一帧扫描需要一定时间(通常为0.1s或0.05s)，而该段时间内由于可移动物体(例如车辆)高速运动，采集得到的激光雷达原始数据会受运动影响，使得测量值与真实值存在偏差。为减小可移动运动的影响，本申请可以根据上述阿克曼模型估计得到的IMU在世界坐标系下的位姿变换关系
Figure BDA0002547921220000105
对激光雷达测量的原始数据进行运动补偿。由于两次扫描之间的时间间隔很短，所以可假设两帧之间的运动为线性运动，通过时间戳插值即可得到一帧内的激光雷达所采集的点相对于该帧起始时刻的位姿，从而将一帧内的所有激光雷达所采集的点都转换到该帧的起始时刻，确定各点运动补偿后的位置。
步骤2042、对进行运动补偿后的各帧点云数据进行特征提取，根据各帧点云数据中的点的曲率信息，将各帧点云数据中的点划分为线特征点和平面特征点。
该步骤2042具体可以通过如下方式实现：
在进行运动补偿后的一帧点云数据中获得一个线束上的任一点及该线束上的任一点预设范围内的若干点。此处，由于激光雷达测量得到的激光点是按照线束排列的，所以可以按照线束为每一个激光点找到其预设范围内的若干点，如在线束上其左右两侧的若干激光点(如左右两侧各取5个激光点，但不仅局限于此)。
根据该任一点在激光雷达坐标系下的坐标和该线束上的任一点预设范围内的若干点在激光雷达坐标系下的坐标，确定该任一点处的曲率。例如，可以采用如下曲率计算公式来确定该任一点处的曲率：
Figure BDA0002547921220000111
其中，c表示
Figure BDA0002547921220000112
点处的曲率；
Figure BDA0002547921220000113
分别表示当前帧中第k条线上的第i、j个点在激光雷达坐标系下的坐标，S表示第i个点左右两侧若干个点组成的点集，S表示该点集所包含点的个数。
根据预先设置的曲率阈值，在一点的曲率大于曲率阈值时，将该一点作为线特征点，在一点的曲率小于曲率阈值时，将该一点作为平面特征点。
步骤2043、将当前帧点云数据之前的预设帧点云数据按照已经进行位姿估计得到的位姿进行叠加，确定当前帧点云数据对应的局部线特征地图和局部面特征地图。
具体的，位姿估计是增量式进行的，因此当前帧之前的各帧点云的线特征点、面特征点及对应的位姿均为已知量，所以可以将当前帧点云数据之前的预设帧点云数据(如15帧点云数据)按照已经进行位姿估计得到的位姿进行叠加，即可得到对应的局部线特征地图(由线特征点组成)和局部面特征地图(由平面特征点组成)。
步骤2044、根据激光雷达和IMU之间的外参，得到当前帧激光雷达在世界坐标系下的初始位姿：
Figure BDA0002547921220000114
Figure BDA0002547921220000115
其中，pLiDAR为当前时刻激光雷达在世界坐标系下的初始位置，RLiDAR为当前时刻激光雷达在世界坐标系下的初始姿态，RIMU、tIMU分别为当前时刻IMU在世界坐标系下的姿态和位置，
Figure BDA0002547921220000121
和
Figure BDA0002547921220000122
分别为预先通过激光雷达和IMU之间的外参标定得到姿态变换关系和位置变换关系。
步骤2045、根据预先采用KD-Tree算法为每个点建立的数据索引，在局部线特征地图中搜索得到当前帧点云数据中每个线特征点对应的若干近邻点，在局部面特征地图中搜索得到当前帧点云数据中每个平面特征点对应的若干近邻点。
步骤2046、根据当前帧点云数据中的线特征点xl对应的若干近邻点(例如5个)拟合得到一条直线，将线特征点xl与该直线的距离函数作为线特征点误差函数；
所述线特征点误差函数为：
Figure BDA0002547921220000123
其中，
Figure BDA0002547921220000124
和
Figure BDA0002547921220000125
为该直线上的任意两点。
步骤2047、根据当前帧点云数据中的平面特征点xp对应的若干近邻点(例如5个)拟合(例如通过SVD分解)得到一个平面Ax+By+Cz+D＝0，将面特征点xp与该平面的距离函数作为面特征点误差函数。
其中，A、B、C和D表示拟合得到的平面的参数。
所述面特征点误差函数为：
Figure BDA0002547921220000126
其中，n表示矩阵：n＝(A,B,C)。
步骤2048、根据线特征点误差函数和面特征点误差函数，建立可移动物体的激光雷达位姿约束rLiDAR(X)。
其中：
Figure BDA0002547921220000127
X表示IMU在世界坐标系下的位姿，X为待优化的状态变量；nline表示当前帧点云数据中的线特征点数目，nplane表示当前帧点云数据中的平面特征点数目。
步骤205、根据气压计测量的高度观测数据进行建模，建立可移动物体的高度位置的气压计约束。
具体的，气压计可以通过测量大气压得到当前的海拔高度。虽然温度突变、气流冲击等因素会影响气压计高度测量的绝对精度，但气压计观测的相对精度通常较高。而高度估计精度低一直是目前主流里程计算法的一个突出问题，所以为提高里程计在高度方向的估计精度，降低系统累计误差，本申请实施例中可以采用如下方式：
根据气压计测量的当前时刻高度观测数据Zk+1、气压计预先测量的初始时刻高度观测数据Z0、IMU测量的当前时刻在世界坐标系下的高度估计值
Figure BDA0002547921220000131
以及IMU预先测量的初始时刻在世界坐标系下的高度估计值
Figure BDA0002547921220000132
进行建模，建立可移动物体的高度位置的气压计约束rAltimeter(X)；其中：
Figure BDA0002547921220000133
X表示IMU在世界坐标系下的位姿，X为待优化的状态变量；
Figure BDA0002547921220000134
分别为预先获知的当前时刻气压计坐标系到世界坐标系的旋转数据和平移数据。
步骤206、对所述横滚角约束、俯仰角约束、阿克曼模型约束、激光雷达位姿约束和气压计约束采用非线性优化方法进行联合优化求解，确定可移动物体的位姿结果。
具体的，此处可以对横滚角约束rRoll(X)、俯仰角约束rPitch(X)、阿克曼模型约束rAkerman(X)、激光雷达位姿约束rLiDAR(X)和气压计约束rAltimeter(X)，采用优化算法对联合优化代价函数求解非线性最小二乘问题，确定可移动物体的IMU在世界坐标系下的位姿结果(即当前待优化的状态变量X的最大后验概率估计)。其中，所采用的优化算法可以为高斯牛顿算法或者Levenberg-Marquardt算法(L-M算法，列文伯格-马夸尔特法)等，但不仅局限于此。
其中，联合优化代价函数为：
Figure BDA0002547921220000136
其中，
Figure BDA0002547921220000135
分别为各约束项所对应的预先设置的信息矩阵。
这样，通过上述步骤201至步骤206实现的基于多传感器(包括激光雷达、IMU、轮速计和气压计)融合的里程计方法，可以得到相对于激光雷达采集的各帧间准确的相对位姿，能够满足例如隧道、跨海大桥等特征稀疏、GPS信号较差场景下的实时位姿估计，得到的位姿结果准确性和鲁棒性较好。
在本申请的一实施例中，发明人对上述本申请所实现的基于多传感器融合的里程计方法进行了实验验证，过程如下：
为验证本申请所实现的基于多传感器融合的里程计方法的准确性和鲁棒性，本申请实施例中使用装配了激光雷达、IMU、轮速计、气压计等传感器的数据采集车，采集了一段特长隧道数据进行实验验证，该隧道全长约9.2Km，如图3所示，隧道内场景特征稀疏、两侧墙壁均为光滑平面。
在上述图3所示的场景中采用本申请实施例的基于多传感器融合的里程计方法后，结合现有技术中最具代表性的激光里程计算法LOAM以及激光惯导里程计算法LIO-Mapping在相同数据上进行了对比实验。实验结果如图4所示，其中图4的横纵坐标用于表示IMU在世界坐标系下的位姿中的位置信息，Ground-Truth表示位姿的真实值，Sensor-Fusion-Odometry表示本申请实施例的基于多传感器融合的里程计方法。可见，在该实验场景下，LOAM、LIO-Mapping两种算法均发生严重退化，无法跑完全程，数据采集车搭载的IMU在世界坐标系下的位姿丢失，完全没有达到隧道建图的要求；而同样条件下，本申请实施例所提出的基于多传感器融合的里程计方法可以跑完全程，最终得到的位姿估计结果虽然不可避免的存在累计误差，但得到了隧道内各帧间准确的相对位姿，为后续的地图构建奠定了基础。
步骤207、根据可移动物体的位姿结果，增量式构建预设路段的多个局部点云地图。
例如获得到实时的可移动物体的位姿结果后，可确定每帧的可移动物体的位姿结果，从而可增量式构建预设路段的多个局部点云地图，例如预设路段为10000m，则可以每100m建立一个局部点云地图，共计构建100个局部点云地图，但不仅局限于此。
步骤208、建立同一局部点云地图中相邻两帧的帧间位姿约束。
由于在里程计算法中，初始几帧的位姿估计精度通常不高，为保证系统鲁棒性，本申请实施例将同一局部点云地图中各帧对应的可移动物体的位姿结果按照帧采集顺序进行排序；将排序后的各帧对应的可移动物体的位姿结果中的前n帧丢弃(例如前5帧，但不仅局限于此)；其中，n为预先设置的帧阈值，例如n＝5，但不仅局限于此。
这样，可以根据丢弃前n帧后的排序后的各帧对应的可移动物体的位姿结果，建立同一局部点云地图中相邻两帧的帧间位姿约束
Figure BDA0002547921220000141
其中：
Figure BDA0002547921220000142
xi，xi+1分别为局部点云地图中第i帧，第i+1帧对应的可移动物体的位姿；
Figure BDA0002547921220000151
为预先计算得到的相邻两帧的相对位姿关系，例如可以通过前述基于多传感器融合的里程计方法来确定该相邻两帧的相对位姿关系
Figure BDA0002547921220000152
步骤209、通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束。
此处，该步骤209可以采用如下方式实现，例如：
通过NDT算法对相邻局部点云地图进行配准，确定相邻局部点云地图之间的位姿变换关系TNDT；此处，通过NDT算法确定相邻局部点云地图之间的位姿变换关系TNDT的方式为：首先选择一个局部点云地图的点云进行三维栅格划分，之后为每一个栅格计算一个正态分布，然后对另一个局部点云地图中的激光点做降采样，并将所有降采样后的点按照组合导航系统提供的初始位姿变换投影到三维栅格中，同时计算出每个点的概率密度函数，并计算所有概率密度函数乘积的最大似然，从而得到相邻局部点云地图之间的位姿变换关系TNDT。
建立相邻局部点云地图的子地图位姿约束
Figure BDA0002547921220000153
其中：
Figure BDA0002547921220000154
Xi，Xi+1分别表示第i个局部点云地图的位姿和第i+1个局部点云地图的位姿；
Figure BDA0002547921220000155
表示第i个局部点云地图和第i+1个局部点云地图之间的位姿变换关系。
步骤210、获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束。
其中，在步骤210中，获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束，可以采用如下方式：
获得可移动物体在预设路段前后预设道路范围的局部点云地图对应的观测位姿
Figure BDA0002547921220000156
(即可采用组合导航系统来测量每个局部点云地图对应的观测位姿，如可将每个局部点云地图第一帧的位姿作为该局部点云地图对应的观测位姿，但不仅局限于此)；其中i表示第i个局部点云地图；
建立局部点云地图位姿观测约束
Figure BDA0002547921220000157
其中，
Figure BDA0002547921220000158
Xi用于表示第i个局部点云地图的位姿。
其中，在步骤210中，获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立帧位姿观测约束，可以采用如下方式：
获得可移动物体在预设路段前后预设道路范围的帧对应的观测位姿
Figure BDA0002547921220000161
(即可采用组合导航系统来测量每个帧对应的观测位姿，但不仅局限于此)；其中i表示全局第i个帧。
建立帧位姿观测约束
Figure BDA0002547921220000162
其中，
Figure BDA0002547921220000163
xi用于表示全局第i个帧的位姿。
步骤211、根据子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿。
此处，该步骤211可以采用如下方式：
根据子地图位姿约束
Figure BDA0002547921220000164
和局部点云地图位姿观测约束
Figure BDA0002547921220000165
确定子地图联合优化约束：
Figure BDA0002547921220000166
其中，nSubmap表示全局子地图的数量，例如上述预设路段为10000m，则可以每100m建立一个局部点云地图，共计构建100个局部点云地图，则全局子地图的数量为100，但不仅局限于此；nGPS-Submap表示可移动物体在预设路段前后预设道路范围的局部点云地图对应的观测位姿数量，例如上述100个局部点云地图中，前2个局部点云地图和后2个局部点云地图对应的观测位姿是可信且准确的，则可以应用该4个局部点云地图的观测位姿，nGPS-Submap即等于4，但不仅局限于此；Xi，Xi+1分别表示第i个局部点云地图的位姿和第i+1个局部点云地图的位姿；CSubmap、CGPS-Submap为预先设置的信息矩阵，均是对角线元素为固定数值的对角阵，用于赋予两个约束的权重。
对所述子地图联合优化约束进行计算，求解得到各局部点云地图优化位姿Xi。这是一个非线性最小二乘问题，使用高斯牛顿算法等可以进行求解，即可得到当前状态量Xi的最大后验概率估计，作为各局部点云地图优化位姿。
根据同一局部点云地图中相邻两帧的帧间位姿约束，确定局部点云地图内的各帧位姿。在获知得到各局部点云地图优化位姿后，一般相当于获得了各局部点云地图第一帧的优化位姿，从而根据相邻两帧的帧间位姿约束，可以得到每一帧的位姿，作为初始值，后续需要对该初始值继续进行优化。这是由于优化得到各局部点云地图的优化位姿后，已在较大程度上消除了里程计累积误差对建图精度的影响，但各个局部点云地图的内部依然存在一定的累积误差，对于高精度地图而言，该误差是不可忽视的。为减小局部点云地图内累积误差对建图精度的影响，本申请后续在局部点云地图优化的基础上，进行针对全局所有帧的全局优化。
步骤212、根据帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿。
此处，该步骤212可以采用如下方式：
根据帧间位姿约束
Figure BDA0002547921220000171
和帧位姿观测约束
Figure BDA0002547921220000172
确定全局各帧位姿的帧全局优化约束：
Figure BDA0002547921220000173
其中，nFrame表示全局帧的数量，例如上述100个局部点云地图中，每个局部点云地图由100帧组成，则全局帧的数量即为10000帧，但不仅局限于此；nGPS-Frame表示可移动物体在预设路段前后预设道路范围的帧对应的观测位姿数量，例如上述100个局部点云地图中，前2个局部点云地图和后2个局部点云地图对应的观测位姿是可信且准确的，每个局部点云地图由100帧组成，则nGPS-Frame为400，但不仅局限于此；xi，xi+1分别表示全局第i个帧的位姿和第i+1个帧的位姿；CFrame、CGPS-Frame为预先设置的信息矩阵，均是对角线元素为固定数值的对角阵，用于赋予两个约束的权重；
对帧全局优化约束进行计算，求解得到各帧优化位姿xi。此处同样可以使用高斯牛顿算法对非线性最小二乘问题进行求解，即可得到当前状态量xi的最大后验概率估计，作为各帧优化位姿。
步骤213、根据各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
其中，传感器数据主要是指激光雷达测量的各帧激光点云中的点在激光雷达坐标系下的坐标。则该步骤213可以采用如下方式：
根据帧优化位姿和激光雷达测量的各帧激光点云中的点在激光雷达坐标系下的坐标，将各帧激光点云中的点映射到世界坐标系下。
将映射到世界坐标系下的激光点云中的点进行叠加，形成全局地图。
为充分验证本申请实施例的上述步骤的精度以及鲁棒性，本申请实施例使用装配了LiDAR、IMU、轮速计、气压计等传感器的数据采集车，采集了某高速特长隧道数据进行实验验证，隧道全长约9.2Km，实验结果见图5所示，其中图5的横纵坐标用于表示IMU在世界坐标系下的位姿中的位置信息。从实验结果可以看出，在隧道内行驶时，组合导航由于无法接收到卫星信号，所以位姿估计误差极大(见曲线Integrated-Navigation)；基于多传感器融合的里程计算法虽然可以较准确的估计相邻帧间的相对位姿，但在隧道出口处的累计误差较大，无法直接用于建图(见曲线Sensor-Fusion-Odometry)。而本申请实施例在基于多传感器融合的里程计算法的基础上，继续进行了基于帧的全局优化，确定了各帧优化位姿，从而形成与真值十分相近的全局地图(见曲线Tunnel-Mapping)。
另外，如图6所示，本申请实施例还提供一种地图构建装置，包括：
数据获得单元31，用于获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据。
多传感器融合单元32，用于基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束。
子地图位姿约束建立单元33，用于通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束。
位姿观测约束建立单元34，用于获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束。
基于子地图的全局优化单元35，用于根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿。
基于帧的全局优化单元36，用于根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿。
全局地图形成单元37，用于根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
值得说明的是，本申请实施例提供的一种地图构建装置的具体实现方式可以参见上述图1至图5所对应的方法实施例，此处不再赘述。
另外，本申请实施例还提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述图1至图5所述的方法。
另外，本申请实施例还提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如上述图1至图5所述的方法。
另外，本申请实施例还提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现上述图1至图5所述的方法。
本申请实施例提供的一种地图构建方法及装置，首先获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据；然后基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；之后通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；之后获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；之后根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；之后根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿；进而根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。可见，本申请通过构建独立的局部点云地图、基于子地图的全局优化、基于帧的全局优化，有效利用了里程计算法短时间内精度高、预设路段前后预设道路范围定位精度高的特点，同时避免了优化算法陷入局部优化的问题，能够实现卫星信号较差的路段的高精度点云地图的构建，进而保证自动驾驶车辆、智能机器人等的正常行驶。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本申请中应用了具体实施例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (22)

1.一种地图构建方法，其特征在于，包括：
获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据；
基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；
通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；
获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；
根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；
根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿；
根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
2.根据权利要求1所述的方法，其特征在于，所述各种传感器包括惯性测量单元IMU、轮速计、激光雷达和气压计；其中，所述IMU包括加速度计和陀螺仪。
3.根据权利要求2所述的方法，其特征在于，所述获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据，包括：
获得可移动物体在预设路段行驶时加速度计测量的三轴加速度数据、陀螺仪测量的三轴角速度数据、轮速计测量的可移动物体轮速数据、激光雷达测量的点云数据和气压计测量的高度观测数据。
4.根据权利要求3所述的方法，其特征在于，所述基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，包括：
对各种传感器采集的传感器数据分别进行建模，建立可移动物体位姿的约束关系；
对可移动物体位姿的约束关系进行联合优化求解，确定可移动物体的位姿结果；
根据可移动物体的位姿结果，增量式构建预设路段的多个局部点云地图。
5.根据权利要求4所述的方法，其特征在于，所述对各种传感器采集的传感器数据分别进行建模，建立可移动物体位姿的约束关系，包括：
根据加速度计测量的三轴加速度数据进行建模，建立可移动物体的横滚角约束和俯仰角约束；
根据陀螺仪测量的三轴角速度数据和轮速计测量的可移动物体轮速数据，采用阿克曼模型进行运动学建模，建立可移动物体的水平位置和偏航角的阿克曼模型约束；
根据激光雷达测量的点云数据进行建模，建立可移动物体的激光雷达位姿约束；
根据气压计测量的高度观测数据进行建模，建立可移动物体的高度位置的气压计约束。
6.根据权利要求5所述的方法，其特征在于，所述对可移动物体位姿的约束关系进行联合优化求解，确定可移动物体的位姿结果，包括：
对所述横滚角约束、俯仰角约束、阿克曼模型约束、激光雷达位姿约束和气压计约束采用非线性优化方法进行联合优化求解，确定可移动物体的位姿结果。
7.根据权利要求6所述的方法，其特征在于，所述根据加速度计测量的三轴加速度数据进行建模，建立可移动物体的横滚角约束和俯仰角约束，包括：
根据加速度计测量的三轴加速度数据进行建模，确定IMU在世界坐标系下的横滚角估计值θroll和俯仰角估计值θpitch；其中，
Figure FDA0002547921210000021
ax、ay、az表示加速度计测量的三轴加速度数据；
根据所述横滚角估计值θroll和俯仰角估计值θpitch，建立可移动物体的横滚角约束rRoll(X)和俯仰角约束rPitch(X)；其中，rRoll(X)＝θroll-arcsin(-R13)；rPitch(X)＝θpitch-arctan2(R23，R33)；X表示IMU在世界坐标系下的位姿，X为待优化的状态变量，X包括位置p和姿态q；R为待优化的状态变量X中的姿态q的旋转矩阵形式，R23、R33、R13分别为旋转矩阵R中对应行列的元素。
8.根据权利要求6所述的方法，其特征在于，根据陀螺仪测量的三轴角速度数据和轮速计测量的可移动物体轮速数据，采用阿克曼模型进行运动学建模，建立可移动物体的水平位置和偏航角的阿克曼模型约束，包括：
根据陀螺仪测量的三轴角速度数据，确定世界坐标系下可移动物体前进方向与y轴夹角的角度积分值：
Figure FDA0002547921210000022
其中，θi表示第i时刻的可移动物体前进方向与y轴夹角的角度积分值；t表示第t时刻；
Figure FDA0002547921210000023
为预先获知的车体坐标系到IMU坐标系的旋转变换关系；
Figure FDA0002547921210000031
为第t时刻陀螺仪测量的三轴角速度数据中的偏航角；
根据轮速计测量的第i时刻的可移动物体左后轮在车体坐标系下的速度
Figure FDA0002547921210000032
和右后轮在车体坐标系下的速度
Figure FDA0002547921210000033
确定可移动物体后轴中心在车体坐标系下的速度vi；其中，
Figure FDA0002547921210000034
Figure FDA0002547921210000035
为预先获知的速度噪声；
采用阿克曼模型进行运动学建模，确定世界坐标系下可移动物体位姿传递方程：
χi+1＝χi+vi·Δt·sinθi
yi+1＝yi+vi·Δt·cosθi
其中，Δt为轮速计两个相邻测量时刻的时间差；xi、yi表示可移动物体在世界坐标系下的水平位置；
根据激光雷达的测量频率，对相邻两个激光雷达第k时刻和第k+1时刻之间的xi、yi、θi进行积分，确定在世界坐标系下xi、yi、θi各自的改变量δχk(k+1)、δyk(k+1)、δθk(k+1)；
根据车体坐标系和IMU坐标系之间的外参确定IMU坐标系到车体坐标系的位姿变换关系
Figure FDA0002547921210000036
并确定第k时刻到第k+1时刻之间的IMU在世界坐标系下的位姿变换关系
Figure FDA0002547921210000037
其中：
Figure FDA0002547921210000038
建立可移动物体的阿克曼模型约束rAkerman(X)；其中：
Figure FDA0002547921210000039
X表示IMU在世界坐标系下的位姿，X为待优化的状态变量。
9.根据权利要求6所述的方法，其特征在于，根据激光雷达测量的点云数据进行建模，建立可移动物体的激光雷达位姿约束，包括：
将激光雷达测量的各帧点云数据进行运动补偿，确定各帧点云数据中的点的运动补偿后的位置；
对进行运动补偿后的各帧点云数据进行特征提取，根据各帧点云数据中的点的曲率信息，将各帧点云数据中的点划分为线特征点和平面特征点；
将当前帧点云数据之前的预设帧点云数据按照已经进行位姿估计得到的位姿进行叠加，确定当前帧点云数据对应的局部线特征地图和局部面特征地图；
根据激光雷达和IMU之间的外参，得到当前帧激光雷达在世界坐标系下的初始位姿：
Figure FDA0002547921210000041
Figure FDA0002547921210000042
其中，pLiDAR为当前时刻激光雷达在世界坐标系下的初始位置，RLiDAR为当前时刻激光雷达在世界坐标系下的初始姿态，RIMU、tIMU分别为当前时刻IMU在世界坐标系下的姿态和位置，
Figure FDA0002547921210000043
和
Figure FDA0002547921210000044
分别为预先通过激光雷达和IMU之间的外参标定得到姿态变换关系和位置变换关系；
根据预先采用KD-Tree算法为每个点建立的数据索引，在局部线特征地图中搜索得到当前帧点云数据中每个线特征点对应的若干近邻点，在局部面特征地图中搜索得到当前帧点云数据中每个平面特征点对应的若干近邻点；
根据当前帧点云数据中的线特征点xl对应的若干近邻点拟合得到一条直线，将线特征点xl与该直线的距离函数作为线特征点误差函数；
所述线特征点误差函数为：
Figure FDA0002547921210000045
其中，
Figure FDA0002547921210000046
和
Figure FDA0002547921210000047
为该直线上的任意两点；
根据当前帧点云数据中的平面特征点xp对应的若干近邻点拟合得到一个平面Ax+By+Cz+D＝0，将面特征点xp与该平面的距离函数作为面特征点误差函数；其中，A、B、C和D表示拟合得到的平面的参数；
所述面特征点误差函数为：
Figure FDA0002547921210000048
其中，n表示矩阵：n＝(A,B,C)；
根据所述线特征点误差函数和面特征点误差函数，建立可移动物体的激光雷达位姿约束rLiDAR(X)；其中：
Figure FDA0002547921210000049
X表示IMU在世界坐标系下的位姿，X为待优化的状态变量；nline表示当前帧点云数据中的线特征点数目，nplane表示当前帧点云数据中的平面特征点数目。
10.根据权利要求6所述的方法，其特征在于，根据气压计测量的高度观测数据进行建模，建立可移动物体的高度位置的气压计约束，包括：
根据气压计测量的当前时刻高度观测数据Zk+1、气压计预先测量的初始时刻高度观测数据Z0、IMU测量的当前时刻在世界坐标系下的高度估计值
Figure FDA0002547921210000051
以及IMU预先测量的初始时刻在世界坐标系下的高度估计值
Figure FDA0002547921210000052
进行建模，建立可移动物体的高度位置的气压计约束rAltimeter(X)；其中：
Figure FDA0002547921210000053
X表示IMU在世界坐标系下的位姿，X为待优化的状态变量；
Figure FDA0002547921210000054
分别为预先获知的当前时刻气压计坐标系到世界坐标系的旋转数据和平移数据。
11.根据权利要求6所述的方法，其特征在于，对所述横滚角约束、俯仰角约束、阿克曼模型约束、激光雷达位姿约束和气压计约束采用非线性优化方法进行联合优化求解，确定可移动物体的位姿结果，包括：
对所述横滚角约束rRoll(X)、俯仰角约束rPitch(X)、阿克曼模型约束rAkerman(X)、激光雷达位姿约束rLiDAR(X)和气压计约束rAltimeter(X)，采用优化算法对联合优化代价函数求解非线性最小二乘问题，确定可移动物体的IMU在世界坐标系下的位姿结果；
其中，联合优化代价函数为：
Figure FDA0002547921210000055
其中，
Figure FDA0002547921210000056
分别为各约束项所对应的预先设置的信息矩阵；X表示IMU在世界坐标系下的位姿，X为待优化的状态变量。
12.根据权利要求6所述的方法，其特征在于，所述建立同一局部点云地图中相邻两帧的帧间位姿约束，包括：
将同一局部点云地图中各帧对应的可移动物体的位姿结果按照帧采集顺序进行排序；
将排序后的各帧对应的可移动物体的位姿结果中的前n帧丢弃；其中，n为预先设置的帧阈值；
根据丢弃前n帧后的排序后的各帧对应的可移动物体的位姿结果，建立同一局部点云地图中相邻两帧的帧间位姿约束
Figure FDA0002547921210000057
其中：
Figure FDA0002547921210000061
xi，xi+1分别为局部点云地图中第i帧，第i+1帧对应的可移动物体的位姿；
Figure FDA0002547921210000062
为预先计算得到的相邻两帧的相对位姿关系。
13.根据权利要求6所述的方法，其特征在于，所述通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束，包括：
通过NDT算法对相邻局部点云地图进行配准，确定相邻局部点云地图之间的位姿变换关系TNDT；
建立相邻局部点云地图的子地图位姿约束
Figure FDA0002547921210000063
其中：
Figure FDA0002547921210000064
Xi，Xi+1分别表示第i个局部点云地图的位姿和第i+1个局部点云地图的位姿；
Figure FDA0002547921210000065
表示第i个局部点云地图和第i+1个局部点云地图之间的位姿变换关系。
14.根据权利要求6所述的方法，其特征在于，获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束，包括：
获得可移动物体在预设路段前后预设道路范围的局部点云地图对应的观测位姿
Figure FDA0002547921210000066
其中i表示第i个局部点云地图；
建立局部点云地图位姿观测约束
Figure FDA0002547921210000067
其中，
Figure FDA0002547921210000068
Xi用于表示第i个局部点云地图的位姿。
15.根据权利要求6所述的方法，其特征在于，获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立帧位姿观测约束，包括：
获得可移动物体在预设路段前后预设道路范围的帧对应的观测位姿
Figure FDA0002547921210000069
其中i表示全局第i个帧；
建立帧位姿观测约束
Figure FDA00025479212100000610
其中，
Figure FDA00025479212100000611
xi用于表示全局第i个帧的位姿。
16.根据权利要求6所述的方法，其特征在于，根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿，包括：
根据子地图位姿约束
Figure FDA0002547921210000071
和局部点云地图位姿观测约束
Figure FDA0002547921210000072
确定子地图联合优化约束：
Figure FDA0002547921210000073
其中，nSubmap表示全局子地图的数量；nGPS-Submap表示可移动物体在预设路段前后预设道路范围的局部点云地图对应的观测位姿数量；Xi，Xi+1分别表示第i个局部点云地图的位姿和第i+1个局部点云地图的位姿；CSubmap、CGPS-Submap为预先设置的信息矩阵；
对所述子地图联合优化约束进行计算，求解得到各局部点云地图优化位姿Xi；
根据同一局部点云地图中相邻两帧的帧间位姿约束，确定局部点云地图内的各帧位姿。
17.根据权利要求6所述的方法，其特征在于，根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿，包括：
根据帧间位姿约束
Figure FDA0002547921210000074
和帧位姿观测约束
Figure FDA0002547921210000075
确定全局各帧位姿的帧全局优化约束：
Figure FDA0002547921210000076
其中，nFrame表示全局帧的数量；nGPS-Frame表示可移动物体在预设路段前后预设道路范围的帧对应的观测位姿数量；xi，xi+1分别表示全局第i个帧的位姿和第i+1个帧的位姿；CFrame、CGPS-Frame为预先设置的信息矩阵；
对所述帧全局优化约束进行计算，求解得到各帧优化位姿xi。
18.根据权利要求6所述的方法，其特征在于，根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图，包括：
根据所述帧优化位姿和激光雷达测量的各帧激光点云中的点在激光雷达坐标系下的坐标，将各帧激光点云中的点映射到世界坐标系下；
将映射到世界坐标系下的激光点云中的点进行叠加，形成全局地图。
19.一种地图构建装置，其特征在于，包括：
数据获得单元，用于获得可移动物体在预设路段行驶时可移动物体上搭载的各种传感器采集的传感器数据；
多传感器融合单元，用于基于多传感器融合的里程计方法，增量式构建预设路段的多个局部点云地图，并建立同一局部点云地图中相邻两帧的帧间位姿约束；
子地图位姿约束建立单元，用于通过点云配准计算相邻局部点云地图间的位姿关系，并建立相邻局部点云地图的子地图位姿约束；
位姿观测约束建立单元，用于获得可移动物体在预设路段前后预设道路范围的位姿观测数据，并建立局部点云地图位姿观测约束和帧位姿观测约束；
基于子地图的全局优化单元，用于根据所述子地图位姿约束和局部点云地图位姿观测约束进行基于子地图全局优化，确定各局部点云地图优化位姿，并确定基于子地图全局优化后各局部点云地图内的各帧位姿；
基于帧的全局优化单元，用于根据所述帧间位姿约束和帧位姿观测约束对全局各帧位姿进行基于帧的全局优化，确定各帧优化位姿；
全局地图形成单元，用于根据所述各帧优化位姿和传感器数据，进行地图数据融合，形成全局地图。
20.一种计算机可读存储介质，其特征在于，包括程序或指令，当所述程序或指令在计算机上运行时，实现权利要求1至18任一项所述的方法。
21.一种包含指令的计算机程序产品，其特征在于，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如权利要求1至18任一项所述的方法。
22.一种计算机服务器，其特征在于，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如权利要求1至18任一项所述的方法。
CN202010566851.8A 2020-06-19 2020-06-19 一种地图构建方法及装置 Pending CN113819914A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010566851.8A CN113819914A (zh) 2020-06-19 2020-06-19 一种地图构建方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010566851.8A CN113819914A (zh) 2020-06-19 2020-06-19 一种地图构建方法及装置

Publications (1)

Publication Number Publication Date
CN113819914A true CN113819914A (zh) 2021-12-21

Family

ID=78911586

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010566851.8A Pending CN113819914A (zh) 2020-06-19 2020-06-19 一种地图构建方法及装置

Country Status (1)

Country Link
CN (1) CN113819914A (zh)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114295138A (zh) * 2021-12-31 2022-04-08 深圳市普渡科技有限公司 机器人、地图扩建方法、装置和可读存储介质
CN114322994A (zh) * 2022-03-10 2022-04-12 之江实验室 一种基于离线全局优化的多点云地图融合方法和装置
CN114608569A (zh) * 2022-02-22 2022-06-10 杭州国辰机器人科技有限公司 三维位姿估计方法、系统、计算机设备及存储介质
CN114842084A (zh) * 2022-07-04 2022-08-02 矿冶科技集团有限公司 一种地图构建方法、装置及移动探测设备
CN115493603A (zh) * 2022-11-17 2022-12-20 安徽蔚来智驾科技有限公司 地图对齐方法、计算机设备及计算机可读存储介质
CN115561731A (zh) * 2022-12-05 2023-01-03 安徽蔚来智驾科技有限公司 位姿优化方法、点云地图建立方法、计算机设备及介质
CN115880690A (zh) * 2022-11-23 2023-03-31 郑州大学 一种在三维重建辅助下进行点云中物体快速标注的方法
CN115979248A (zh) * 2023-03-17 2023-04-18 上海仙工智能科技有限公司 基于定位位姿为约束的地图更新方法及系统、存储介质
CN116255976A (zh) * 2023-05-15 2023-06-13 长沙智能驾驶研究院有限公司 地图融合方法、装置、设备及介质
CN116878488A (zh) * 2023-09-07 2023-10-13 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN117590371A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波成像雷达实现全局车位状态检测的方法
WO2024050961A1 (zh) * 2022-09-09 2024-03-14 广东汇天航空航天科技有限公司 建图方法、装置、设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107515891A (zh) * 2017-07-06 2017-12-26 杭州南江机器人股份有限公司 一种机器人地图制作方法、装置和存储介质
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109425348A (zh) * 2017-08-23 2019-03-05 北京图森未来科技有限公司 一种同时定位与建图的方法和装置
CN109934920A (zh) * 2019-05-20 2019-06-25 奥特酷智能科技(南京)有限公司 基于低成本设备的高精度三维点云地图构建方法
CN109974712A (zh) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 一种基于图优化的变电站巡检机器人建图方法
US20190323843A1 (en) * 2018-07-04 2019-10-24 Baidu Online Network Technology (Beijing) Co., Ltd. Method for generating a high precision map, apparatus and storage medium
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107515891A (zh) * 2017-07-06 2017-12-26 杭州南江机器人股份有限公司 一种机器人地图制作方法、装置和存储介质
CN109425348A (zh) * 2017-08-23 2019-03-05 北京图森未来科技有限公司 一种同时定位与建图的方法和装置
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
US20190323843A1 (en) * 2018-07-04 2019-10-24 Baidu Online Network Technology (Beijing) Co., Ltd. Method for generating a high precision map, apparatus and storage medium
CN109974712A (zh) * 2019-04-22 2019-07-05 广东亿嘉和科技有限公司 一种基于图优化的变电站巡检机器人建图方法
CN109934920A (zh) * 2019-05-20 2019-06-25 奥特酷智能科技(南京)有限公司 基于低成本设备的高精度三维点云地图构建方法
CN110706279A (zh) * 2019-09-27 2020-01-17 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
柯艳国;王雄奇;魏新;吴贤斌;朱仲贤;董翔宇;黄杰;董二宝;: "一种基于图优化的实时3D激光SLAM算法", 机电一体化, no. 1 *
纪嘉文;杨明欣;: "一种基于多传感融合的室内建图和定位算法", 成都信息工程大学学报, no. 04, 15 August 2018 (2018-08-15) *

Cited By (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114295138A (zh) * 2021-12-31 2022-04-08 深圳市普渡科技有限公司 机器人、地图扩建方法、装置和可读存储介质
CN114295138B (zh) * 2021-12-31 2024-01-12 深圳市普渡科技有限公司 机器人、地图扩建方法、装置和可读存储介质
CN114608569A (zh) * 2022-02-22 2022-06-10 杭州国辰机器人科技有限公司 三维位姿估计方法、系统、计算机设备及存储介质
CN114608569B (zh) * 2022-02-22 2024-03-01 杭州国辰机器人科技有限公司 三维位姿估计方法、系统、计算机设备及存储介质
CN114322994A (zh) * 2022-03-10 2022-04-12 之江实验室 一种基于离线全局优化的多点云地图融合方法和装置
CN114322994B (zh) * 2022-03-10 2022-07-01 之江实验室 一种基于离线全局优化的多点云地图融合方法和装置
CN114842084A (zh) * 2022-07-04 2022-08-02 矿冶科技集团有限公司 一种地图构建方法、装置及移动探测设备
WO2024050961A1 (zh) * 2022-09-09 2024-03-14 广东汇天航空航天科技有限公司 建图方法、装置、设备及存储介质
CN115493603A (zh) * 2022-11-17 2022-12-20 安徽蔚来智驾科技有限公司 地图对齐方法、计算机设备及计算机可读存储介质
CN115493603B (zh) * 2022-11-17 2023-03-10 安徽蔚来智驾科技有限公司 地图对齐方法、计算机设备及计算机可读存储介质
CN115880690A (zh) * 2022-11-23 2023-03-31 郑州大学 一种在三维重建辅助下进行点云中物体快速标注的方法
CN115880690B (zh) * 2022-11-23 2023-08-11 郑州大学 一种在三维重建辅助下进行点云中物体快速标注的方法
CN115561731B (zh) * 2022-12-05 2023-03-10 安徽蔚来智驾科技有限公司 位姿优化方法、点云地图建立方法、计算机设备及介质
CN115561731A (zh) * 2022-12-05 2023-01-03 安徽蔚来智驾科技有限公司 位姿优化方法、点云地图建立方法、计算机设备及介质
CN115979248A (zh) * 2023-03-17 2023-04-18 上海仙工智能科技有限公司 基于定位位姿为约束的地图更新方法及系统、存储介质
CN115979248B (zh) * 2023-03-17 2023-08-18 上海仙工智能科技有限公司 基于定位位姿为约束的地图更新方法及系统、存储介质
CN116255976B (zh) * 2023-05-15 2023-10-31 长沙智能驾驶研究院有限公司 地图融合方法、装置、设备及介质
CN116255976A (zh) * 2023-05-15 2023-06-13 长沙智能驾驶研究院有限公司 地图融合方法、装置、设备及介质
CN116878488B (zh) * 2023-09-07 2023-11-28 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN116878488A (zh) * 2023-09-07 2023-10-13 湘江实验室 一种建图方法、装置、存储介质及电子装置
CN117590371A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波成像雷达实现全局车位状态检测的方法
CN117590371B (zh) * 2024-01-18 2024-03-29 上海几何伙伴智能驾驶有限公司 基于4d毫米波成像雷达实现全局车位状态检测的方法

Similar Documents

Publication Publication Date Title
CN113819914A (zh) 一种地图构建方法及装置
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位系统
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN107246876B (zh) 一种无人驾驶汽车自主定位与地图构建的方法及系统
CN111142091B (zh) 一种融合车载信息的自动驾驶系统激光雷达在线标定方法
CN113819905A (zh) 一种基于多传感器融合的里程计方法及装置
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN110745140B (zh) 一种基于连续图像约束位姿估计的车辆换道预警方法
CN107615201A (zh) 自身位置估计装置及自身位置估计方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN104061899A (zh) 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法
CN104180818A (zh) 一种单目视觉里程计算装置
EP4124829B1 (en) Map construction method, apparatus, device and storage medium
CN113252051A (zh) 一种地图构建方法及装置
CN113252022A (zh) 一种地图数据处理方法及装置
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
Liu et al. Vehicle sideslip angle estimation: a review
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
CN110489807B (zh) 一种摇臂悬架结构巡视器的局部精确定位方法
CN111708010B (zh) 一种可移动设备的定位方法、装置、系统及可移动设备
CN117234203A (zh) 一种多源里程融合slam井下导航方法
CN113048987A (zh) 一种车载导航系统定位方法
CN113777589A (zh) 一种基于点特征的lidar与gps/imu联合标定方法
CN114370872B (zh) 车辆姿态确定方法和车辆

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