CN113945206A - 一种基于多传感器融合的定位方法及装置 - Google Patents

一种基于多传感器融合的定位方法及装置 Download PDF

Info

Publication number
CN113945206A
CN113945206A CN202010686900.1A CN202010686900A CN113945206A CN 113945206 A CN113945206 A CN 113945206A CN 202010686900 A CN202010686900 A CN 202010686900A CN 113945206 A CN113945206 A CN 113945206A
Authority
CN
China
Prior art keywords
data
time
sliding window
imu
current
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.)
Granted
Application number
CN202010686900.1A
Other languages
English (en)
Other versions
CN113945206B (zh
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 CN202010686900.1A priority Critical patent/CN113945206B/zh
Priority to EP21184662.1A priority patent/EP3940421A1/en
Priority to JP2021116216A priority patent/JP2022019642A/ja
Priority to US17/378,484 priority patent/US12092740B2/en
Publication of CN113945206A publication Critical patent/CN113945206A/zh
Application granted granted Critical
Publication of CN113945206B publication Critical patent/CN113945206B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/66—Tracking systems using electromagnetic waves other than radio waves
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Traffic Control Systems (AREA)

Abstract

本申请提供了一种基于多传感器融合的定位方法及装置，涉及物体定位技术领域。方法包括：实时获得可移动物体上搭载的各种传感器采集的传感器数据；对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。本申请能够在GPS信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。

Description

一种基于多传感器融合的定位方法及装置
技术领域
本申请涉及物体定位技术领域，尤其涉及一种基于多传感器融合的定位方法及装置。
背景技术
目前，随着自动驾驶技术、智能机器人和无人机技术的发展，如何保证自动驾驶车辆、智能机器人和无人机的精确定位成为了一个热点问题。在自动驾驶车辆、智能机器人、无人机等可移动物体上，一般可以搭载各类传感器，以便于对周围环境进行感知，并依靠这些感知信息进行定位。
然而，当前自动驾驶车辆、智能机器人和无人机的定位大多需要依赖于良好的GPS信号以及良好的激光雷达感知来实现精准的定位。在GPS信号丢失、跳变的场景(隧道，被高楼遮挡卫星信号的街道等)，在激光雷达观测严重退化的场景(空旷的跨海大桥,高速公路，以及隧道等)，如何对自动驾驶车辆、智能机器人以及无人机等进行准确的定位，成为了一个亟待解决的问题。
发明内容
本申请的实施例提供一种基于多传感器融合的定位方法及装置，能够在GPS信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。
为达到上述目的，本申请的实施例采用如下技术方案：
本申请实施例的第一方面，提供一种基于多传感器融合的定位方法，应用于一种搭载有多种传感器的可移动物体上，所述方法包括：
实时获得可移动物体上搭载的各种传感器采集的传感器数据；
对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；
将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；
获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；
根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
另外，本申请实施例的第二方面，提供一种基于多传感器融合的定位装置，应用于一种搭载有多种传感器的可移动物体上，所述装置，包括：
传感器数据获得单元，实时获得可移动物体上搭载的各种传感器采集的传感器数据；
时间空间同步单元，用于对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；
数据预处理单元，用于将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；
状态信息获得单元，用于获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；
联合优化单元，用于根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
另外，本申请实施例的第三方面，提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述第一方面所述的基于多传感器融合的定位方法。
另外，本申请实施例的第四方面，提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如上述第一方面所述的基于多传感器融合的定位方法。
另外，本申请实施例的第五方面，提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如上述第一方面所述的基于多传感器融合的定位方法。
本申请实施例提供的一种基于多传感器融合的定位方法及装置，首先实时获得可移动物体上搭载的各种传感器采集的传感器数据；对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；之后，获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；从而根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。可见，本申请能够结合多种传感器数据，并且通过滑动窗口处理，实时更新当前时刻的状态信息，能够在GPS信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种基于多传感器融合的定位方法的流程图一；
图2为本申请实施例提供的一种基于多传感器融合的定位方法的流程图二；
图3为本申请实施例中的一子步骤示意图；
图4为本申请实施例中的联合优化的滑动窗口示意图；
图5为本申请实施例提供的一种基于多传感器融合的定位装置的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了使本领域的技术人员更好的了解本申请，下面先对本申请实施例中出现的部分技术术语进行解释如下：
可移动物体：是指车辆、移动机器人、飞行器等可进行地图采集的物体，可移动物体上可以搭载各类型传感器，如激光雷达、相机等。
GNSS：Global Navigation Satellite System，全球导航卫星系统。
GPS：Global Positioning System，全球定位系统。
IMU：Inertial Measurement Unit，惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。
高精度地图：不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使车辆达到厘米级的精度，这对确保自动驾驶汽车的安全至关重要。
位姿(Pose)：位置和朝向的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。朝向的3个自由度通常用俯仰角(Pitch)、翻滚角(Roll)、偏航角(Yaw)来表示。
帧(Frame)：传感器完成一次观测所接收到的测量数据，如相机的一帧数据为一张图片，激光雷达的一帧数据为一组激光点云。
配准(Registration)：对同一区域在不同时刻、不同位置的观测结果进行匹配，得到两个观测时刻间的相对位姿关系。
SVD：Singular Value Decomposition，奇异值分解，是线性代数中一种重要的矩阵分解。
RTK：Real-Time Kinematic，实时动态载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。
MSCKF：Multi-State Constraint Kalman Filter，即多状态约束下的卡尔曼滤波器。
在本申请的一些实施例中，术语“车辆”广泛地解释为包括任何移动物体，包括例如飞行器、船只、航天器、汽车、卡车、厢式货车、半挂车、摩托车、高尔夫球车、越野车辆、仓库运输车辆或农用车以及行驶在轨道上的运输工具，例如电车或火车以及其它有轨车辆。本申请中的“车辆”通常可以包括：动力系统、传感器系统、控制系统、外围设备和计算机系统。在其它实施例中，车辆可以包括更多、更少或者不同的系统。
其中，动力系统是为车辆提供动力运动的系统，包括：引擎/马达、变速器和车轮/轮胎、能源单元。
控制系统可以包括控制车辆及其组件的装置的组合，例如转向单元、节气门、制动单元。
外围设备可以是允许车辆与外部传感器、其它车辆、外部计算设备和/或用户进行交互的设备，例如无线通信系统、触摸屏、麦克风和/或扬声器。
基于上述描述的车辆，自动驾驶车辆中还配置有传感器系统和自动驾驶控制装置。
传感器系统可以包括用于感测车辆所处环境的信息的多个传感器，以及改变传感器的位置和/或方向的一个或多个致动器。传感器系统可以包括全球定位系统传感器、惯性测量单元、无线电检测和测距(RADAR)单元、相机、激光测距仪、光检测和测距(LIDAR)单元和/或声学传感器等传感器的任何组合；传感器系统还可以包括监视车辆内部系统的传感器(例如O2监视器、燃油表、引擎温度计等)。
自动驾驶控制装置可以包括一个处理器和存储器，存储器中存储有至少一条机器可执行指令，处理器执行至少一条机器可执行指令实现包括地图引擎、定位模块、感知模块、导航或路径模块、以及自动控制模块等的功能。地图引擎和定位模块用于提供地图信息和定位信息。感知模块用于根据传感器系统获取到的信息和地图引擎提供的地图信息感知车辆所处环境中的事物。导航或路径模块用于根据地图引擎、定位模块和感知模块的处理结果，为车辆规划行驶路径。自动控制模块将导航或路径模块等模块的决策信息输入解析转换成对车辆控制系统的控制命令输出，并通过车载网(例如通过CAN总线、局域互联网络、多媒体定向系统传输等方式实现的车辆内部电子网络系统)将控制命令发送给车辆控制系统中的对应部件，实现对车辆的自动控制；自动控制模块还可以通过车载网来获取车辆中各部件的信息。
目前，可移动物体的定位方案大致有以下几种，这些定位方案大多对所定位的场景有较大依赖性。
例如，第①种：四旋翼类的无人机主要以相机和IMU为主，来进行传感器融合定位；又例如，第②种：室内轮速机器人主要使用相机和轮速计为主，来进行传感器组合方式定位。然而，上述两种(①、②)方法对视觉观测的稳定数据关联要求很高，一旦遇到弱纹理以及大量重复纹理的场景，IMU的状态变量将不能获得准确的视觉约束的更新，会导致状态变量估计不准，发生漂移。另外，在室外场景下，上述两种(①、②)方法还会受到太阳光线，曝光、遮挡等影响。并且对于大部分匀速或匀加速运动的情况下，IMU得不到充分的激励，会导致原始观测(加速度计的加速度和陀螺仪角速度)噪声很大，这种情况会对Visual-IMU(视觉-惯性测量)紧耦合系统的可观性产生影响。
又例如，第③种：以机器视觉为主的无人车定位系统：通过图片上的语义信息和语义地图进行特征关联，并进行车辆定位，也有融合GPS，以及多相机和IMU的传感器融合方案进行车辆定位。对于该第③种方案，以视觉为主依赖语义信息进行状态估计和定位的传感器组合，在非结构化(没有车道线等语义路标的)场景下，长时间得不到稳定准确的数据关联，将会导致状态估计的准确性和鲁棒性受到影响。
又例如，第④种：以激光雷达为主的无人车定位系统：通过激光雷达的观测与全局激光雷达的反射率地图或者特征地图进行特征关联，融合RTK等传感器进行联合状态估计，还有一些方案是融合激光雷达和IMU进行无人车状态估计，也有用MSCKF的滤波框架融合Visual-LiDAR-IMU(视觉-激光雷达-惯性测量)的方案进行车辆状态估计。对于上述第④种方案，在空旷场景下，激光雷达的观测会发生严重退化，导致激光雷达和IMU的数据更新不及时，传感器很难鲁棒的输出稳定的状态。
又例如，第⑤种：也有以GPS为主的室外无人车定位系统。然而该第⑤种方案在大部分室外高动态环境，尤其是城市高楼林立的街道等场景，又或者是遮挡情况严重的隧道，GPS的多路径反射的问题非常明显，这时候的GPS会出现少则几米，多则几十米的定位误差，难以满足无人驾驶场景的定位需求。
本申请实施例旨在提出一种基于激光雷达点云地图的紧耦合的多传感器融合定位方案。旨在提高自动驾驶定位系统在复杂场景下的定位鲁棒性和准确性。使其能够适用于GPS信号丢失、跳变的场景(隧道，被高楼遮挡卫星信号的街道等)，以及激光雷达观测严重退化的场景(空旷的跨海大桥,高速公路，以及隧道等)。
如图1所示，本申请实施例提供一种基于多传感器融合的定位方法，该方法应用于一种搭载有多种传感器的可移动物体上，该方法包括：
步骤101、实时获得可移动物体上搭载的各种传感器采集的传感器数据。
步骤102、对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据。
步骤103、将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据。
步骤104、获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息。
步骤105、根据用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
为了使本领域的技术人员更好的了解本发明，下面结合具体的实施例来对本申请实施例进行阐述，如图2所示，本申请实施例提供一种基于多传感器融合的定位方法，该方法应用于一种搭载有多种传感器的可移动物体上，该多种传感器可以包括惯性测量单元IMU、轮速计、激光雷达等传感器，但不仅局限于此，例如一般情况下还可以包括相机、GPS传感器等；其中，该IMU包括加速度计和陀螺仪。
该方法包括：
步骤201、实时获得IMU测量的IMU数据、轮速计测量的可移动物体轮速数据和激光雷达测量的点云数据。
步骤202、根据IMU、轮速计和激光雷达之间预先标定的外参，将可移动物体轮速数据和点云数据对齐到IMU坐标系下，并根据IMU、轮速计和激光雷达各自的数据采集帧率，将可移动物体轮速数据和点云数据的采集时的每帧与IMU的时间戳对齐，形成IMU坐标系下的，时间戳对齐的IMU数据、可移动物体轮速数据和点云数据。
步骤203、将进行时间空间同步后的IMU数据中处于连续两帧激光雷达时间戳之间的IMU数据使用预先设置的积分算法进行传播，并通过预先设置的预积分方法进行处理，得到位移预积分量、速度预积分量和旋转增量预积分量。
值得说明的是，IMU的原始数据一般为包含偏置(bias)和噪声(noise)的加速度和角速度数据。因此，需要先进行上述步骤202的时间空间同步，然后将进行时间空间同步后的IMU数据中处于连续两帧激光雷达时间戳(如第k帧和第k+1帧)之间的IMU数据使用如RK4(4阶龙格库塔)积分算法、欧拉积分算法或者中值积分算法等积分算法进行传播，并通过预先设置的预积分方法(例如论文C.Forster,L.Carlone,F.Dellaert,and D.Scaramuzza,"IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,"Georgia Institute of Technology,2015.中的方法)进行处理，得到不依赖于绝对位姿的位移预积分量Δp、速度预积分量Δv和旋转增量预积分量Δq。
步骤204、根据当前时刻的上一时刻在预先设置的滑动窗口中联合优化后的状态变量，以及进行时间空间同步后的IMU数据进行下一时刻的状态变量预测，得到下一时刻的状态变量预测数据。
为了给后续的激光雷达的数据关联和后续的联合优化提供准确的初值，本申请实施例需要如步骤204这样的过程来进行下一时刻的状态变量预测。
具体过程是：通过滑动窗口中当前时刻的上一时刻的联合优化后的状态变量X1:m＝[X1,X2,..,Xm]，
Figure BDA0002587897450000081
以及当前时刻IMU的观测输入
Figure BDA0002587897450000082
作为输入。其中，X1:m为状态变量数组，Xk为第k个状态变量；此处的T表示矩阵的转置；该状态变量包括平移向量pk、速度向量vk、旋转矩阵qk、IMU中加速度计偏置bak和IMU中陀螺仪偏置bgk；
Figure BDA0002587897450000083
为IMU观测的第k个加速度，
Figure BDA0002587897450000084
为IMU观测的第k个角速度。
这样，已知IMU的观测噪声模型为如下公式(1)、(2)：
Figure BDA0002587897450000085
Figure BDA0002587897450000086
其中，Rk表示第k时刻IMU相对于世界坐标系
Figure BDA0002587897450000087
的旋转矩阵。
从而，通过上述的输入，可以有效进行下一时刻(下一帧)的状态变量预测，公式如公式(3)、(4)所示：
Figure BDA0002587897450000088
Figure BDA0002587897450000089
其中，
Figure BDA00025878974500000810
为第k+1帧的平移向量预测值，
Figure BDA00025878974500000811
为第k+1帧的旋转矩阵预测值，
Figure BDA00025878974500000812
表示四元数的乘法运算，
Figure BDA00025878974500000813
Figure BDA00025878974500000814
分别为滑动窗口前m帧状态变量中的两种偏置的平均值，因为陀螺仪和加速度计离散时刻的偏置是零均值随机游走的高斯白噪声，使用多帧偏置的均值能够减少异常偏置量的影响，使得预测的状态变量更加平滑。
步骤205、将进行时间空间同步后的点云数据，采用下一时刻的状态变量预测数据进行去畸变处理。
即通过下一时刻的状态变量预测数据，可以对点云数据产生约束，从而能够对进行时间空间同步后的点云数据进行去畸变处理。
步骤206、对去畸变处理后的点云数据进行线面特征提取，获得线特征数据和面特征数据。
具体的对点云数据进行线面特征提取的过程，可见文章：J.Zhang and S.Singh,"LOAM:Lidar Odometry and Mapping in Real-time,"in Robotics:Science andSystems,2014,vol.2.，此处不再赘述。
步骤207、将线特征数据和面特征数据与预先生成的特征地图上的线特征和面特征分别进行配准，得到线特征约束关系数据和面特征约束关系数据。
其中，线特征约束关系数据中包含线特征数据中的点及其对应的特征地图中的线特征中的点，面特征约束关系数据中包含面特征数据中的点及其对应的特征地图中的面特征中的点。
步骤208、将进行时间空间同步后的可移动物体轮速数据，通过预先设置的车辆动力学模型，确定在IMU坐标系下前进方向线速度、水平方向线速度和偏航角速度。
例如，预先设置的车辆动力学模型可以采用阿克曼运动学模型，但不仅局限于此。
步骤209、获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息。
其中，状态信息可以为状态变量，该状态变量包括平移向量、速度向量、旋转矩阵、IMU中加速度计偏置和IMU中陀螺仪偏置。
值得说明的是，在开始进行联合优化之前，可移动物体可能处于静止状态，也可能处于移动状态，当可移动物体处于静止状态，才开始移动时，联合优化之前需要进行初始化。具体的初始化过程主要针对初始速度，重力矢量g、陀螺仪偏置bg和加速度计偏置ba；考虑到重力加速度和加速度计偏置耦合在一起，短时间的初始化过程很难进行完全的解耦，所以此处的初始化方案，侧重估计初始速度、重力矢量g以及陀螺仪偏置bg。
则整个步骤209其过程可以如图3所示，包括：
步骤2091、在用于联合优化的传感器数据未经过初始化时，实时确定预先设置的滑动窗口中的数据是否满足初始化条件。
其中，初始化条件包括所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励。
此处的IMU数据充分激励是指判断一预设时间段的IMU的观测数据的读数的方差，若方差大于等于预设阈值，则表示充分激励，否则若方差小于预设阈值，则表示未充分激励。
步骤2092、在所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励时，对状态变量进行初始化，形成初始化状态变量。
另外，在滑动窗口中的数据观测时刻数小于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励时，则需要对当前滑动窗口中的点云数据和可移动物体轮速数据进行紧耦合帧间优化，得到当前滑动窗口中的当前时刻的初始化激光雷达位姿。所述初始化激光雷达位姿包括激光雷达旋转矩阵和激光雷达平移向量。其过程为：首先，基于轮速计结合匀速运动模型的假设，提供一个初始的帧间运动预测
Figure BDA0002587897450000101
(轮速计结合匀速模型提供粗略的线速度，IMU提供带偏置的角速度)，对相邻帧的激光雷达线、面特征进行帧间配准和位姿优化。帧间位姿的优化目标包含基于线面特征的几何误差，以及基于轮速计的观测数据产生的位移约束，如下式(5)：
Figure BDA0002587897450000102
其中，
Figure BDA0002587897450000103
表示滑动窗口中第k帧中，激光雷达线特征配准的几何残差；
Figure BDA0002587897450000104
表示滑动窗口中第k帧中，激光雷达面特征配准的几何残差；rodom(Vj，Xk)表示基于轮速计的观测数据产生的位移约束，如公式(6)所示：
Figure BDA0002587897450000105
其中，
Figure BDA0002587897450000106
表示相邻帧的相对位移，(Rk，pk)表示第k帧位姿的旋转矩阵和平移向量，[Vk]4∶6＝[vx vy 0]T表示增广速度向量Vk的4到6列元素组成的子向量。
这样，在通过上述公式(5)得到每帧的相对位姿
Figure BDA0002587897450000107
后，转到滑动窗口的第一帧激光雷达坐标系
Figure BDA0002587897450000108
之后，得到一系列的初始激光雷达位姿
Figure BDA0002587897450000109
之后，该步骤2092可以采用如下方式实现：
步骤(1)、根据当前时刻和当前时刻之前各时刻的初始化激光雷达位姿，采用激光雷达相邻帧间的旋转改变量以及激光雷达相邻帧间旋转增量预积分量的一致性约束，将连续多帧的一致性约束进行累加，求解得到初始化陀螺仪偏置。
此处，可以采用激光雷达相邻帧间的旋转改变量Δql以及激光雷达相邻帧间旋转增量预积分量Δq的一致性约束，构建优化问题，连续多帧的一致性约束累加在一起，即可求解初始化陀螺仪偏置。这样，使用求得的初始化陀螺仪偏置重新进行预积分的传递，可以减少由陀螺仪偏置引起的累计误差。求解得到初始化陀螺仪偏置δbg可通过如下公式(7)实现：
Figure BDA0002587897450000111
其中：
Figure BDA0002587897450000112
表示滑动窗口中的帧数；
Figure BDA0002587897450000113
为预积分中旋转增量
Figure BDA0002587897450000114
关于陀螺仪偏置bg的雅克比(Jacobian)矩阵，
Figure BDA0002587897450000115
是IMU坐标系
Figure BDA0002587897450000116
到激光雷达坐标系
Figure BDA0002587897450000117
的外参矩阵的旋转分量四元数形式，
Figure BDA0002587897450000118
表示第k帧激光雷达的旋转矩阵四元数形式。
步骤(2)、根据当前滑动窗口中的初始化激光雷达位姿以及所述位移预积分量的一致性约束，求解得到激光雷达坐标系下的重力加速度，将所述激光雷达坐标系下的重力加速度转换到世界坐标系下，得到初始化重力加速度。
以下结合公式和具体推导过程，来详细阐述得到初始化重力加速度的过程：
重力加速度的估计，主要利用激光雷达配准得到的激光雷达位姿与预积分的位移增量Δp的尺度一致性构建约束；预积分量有如下公式(8)的关系：
Figure BDA0002587897450000119
为了推导过程更清晰，使用Δtk表示k,k+1帧间的时间差，
Figure BDA00025878974500001110
分别简记为pk，Rk，vk，Δvk以及Δpk，则上述公式(8)可以记为公式(9)：
Figure BDA00025878974500001111
则连续三帧(下标分别使用1，2，3表示连续的三帧)有以下关系，如公式(10)所示：
Figure BDA00025878974500001112
则通过上述公式(9)、(10)可以得到如公式(11)的关系：
Figure BDA0002587897450000121
将上述公式(10)的上下两公式相减，结合上述公式(11)，可以得到下面公式的等价关系，如公式(12)：
Figure BDA0002587897450000122
其中，公式(12)中有如下公式(13)和公式(14)的关系：
Figure BDA0002587897450000123
Figure BDA0002587897450000124
可见，上述公式(12)中，出了重力加速度
Figure BDA0002587897450000125
其他变量均为已知量，Rk,pk是通过上述已经求得的初始激光雷达位姿进行外参转换得到；结合公式(12)至(14)的等式关系，在连续m(m≥3)帧构成的滑动窗口中，可以构建出m-2组等式(12)的约束关系(k＝1,…,m-2)，将上述m-2组约束关系式，构建非线性方程(例如，方程形式为：A3(m-2)×3·X3×1＝b3(m-2)×1)，使用SVD分解，得到激光雷达坐标系
Figure BDA0002587897450000126
下的重力加速度
Figure BDA0002587897450000127
在得到激光雷达坐标系
Figure BDA0002587897450000128
下的重力加速度
Figure BDA0002587897450000129
之后，要将其转换到世界坐标系
Figure BDA00025878974500001210
下。考虑到重力加速度g的模长G＝9.81约束，参考现有的重力参数化方式，可以令上述估计的激光雷达坐标系
Figure BDA00025878974500001211
下的重力加速度
Figure BDA00025878974500001212
进行模长归一化：
Figure BDA00025878974500001213
定义IMU坐标系(惯性系)
Figure BDA00025878974500001214
下的归一化向量：
Figure BDA00025878974500001215
分解重力矢量
Figure BDA00025878974500001216
到惯性系，得到最终的重力加速度：
Figure BDA00025878974500001217
作为初始化重力加速度；其中：
Figure BDA00025878974500001218
Exp表示指数映射。
步骤(3)、根据初始化陀螺仪偏置和初始化重力加速度，进行初始速度向量估计，确定初始化速度。
此处，在得到初始化陀螺仪偏置以及初始化重力加速度之后，可以继续求解得到初始化速度。其具体过程可以如下公式(15)所示：
Figure BDA00025878974500001219
即当k＜m时，可以由上述公式(15)得到初始化速度；当k＝m时，由于没有tk+1时刻的观测，只能通过上一帧的速度结合加速度模型进行两次积分来进行求解。
步骤2093、在对状态变量进行初始化完成后，且当前时刻之前的上一时刻进行联合优化求解结束后，获得当前时刻之前的各时刻的联合优化后的当前状态变量。
即通过上述步骤209，能够得到初始化状态变量或者滑动窗口内当前时刻之前的各时刻的联合优化后的当前状态变量。两者的区别在于，前者的滑动窗口内当前时刻之前的各时刻还没有进行联合优化，只能通过初始化来提供初始化状态变量；而后者的滑动窗口内当前时刻之前的各时刻已经进行联合优化，可以提供各时刻的联合优化后的当前状态变量。从而基于上述两者对应的情况，以下步骤210可以有两种方式。
步骤210、根据用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
如上述所述，该步骤210可以通过如下两种方式实现：
例如：在滑动窗口中的当前时刻之前的各时刻未进行联合优化时，根据当前时刻之前的各时刻的初始化状态变量和所述用于联合优化的传感器数据，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量；其中滑动窗口中当前时刻的状态变量用于表示可移动物体的当前位姿状态。
又例如，在滑动窗口中的当前时刻之前的各时刻已进行联合优化时，根据当前时刻之前的各时刻的联合优化后的当前状态变量和所述用于联合优化的传感器数据，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量；其中滑动窗口中当前时刻的状态变量用于表示可移动物体的当前位姿状态。
此处的联合优化采用的优化框架是基于固定滞后平滑(fixed-lagsmoother)的滑动窗口结构。固定滞后平滑是一种迭代算法，交替进行新增观测值或状态变量的更新和边缘化操作，递归地维护对最近m个状态变量全概率密度的估计，是一种基于图模型的增量状态估计方法。从因子图的角度看，可以理解成将一个最大后验概率的求解，分解成对多个条件概率和似然概率的因子相乘，并且能够有效地进行增量求解，在维护更多观测信息的同时，降低增量目标优化的计算复杂度。
如图4所示：此处以滑动窗口的尺寸为5为例(但不仅局限于此)，系统处于稳定状态时，在任何给定时间，5个最近时刻的状态变量的联合密度都可以随时获得；当添加了下一时刻的状态变量Xi+6，可以将滑动窗口中滞后最久的变量(状态变量Xi+1)边缘化，仅剩4个变量Xi+2,Xi+3,Xi+4,Xi+5同新的状态变量Xi+6一起进行联合状态估计。其中HDMap表示预先设置的特征地图，此处为高清点云特征地图。
其中，上述步骤210中的两种实现方式中，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量，可以采用如下方式实现：
根据联合概率密度估计函数：
Figure BDA0002587897450000141
确定滑动窗口中的各时刻的状态变量Xk；其中，k表示第k时刻及第k帧，rp(X)表示边缘化的先验项；
Figure BDA0002587897450000142
表示滑动窗口中的总帧数；i表示第i个线特征；j表示第j个面特征；
Figure BDA0002587897450000143
表示线特征约束关系数据的线特征对总数；
Figure BDA0002587897450000144
表示面特征约束关系数据的面特征对总数；
Figure BDA0002587897450000145
表示滑动窗口中第k帧中，激光雷达线特征配准的几何残差；Zi表示第i个线特征观测；
Figure BDA0002587897450000146
表示当前激光雷达线特征的观测协方差；
Figure BDA0002587897450000147
表示滑动窗口中第k帧中，激光雷达面特征配准的几何残差；Zj表示第j个面特征观测；
Figure BDA0002587897450000148
表示当前激光雷达面特征的观测协方差；
Figure BDA0002587897450000149
表示IMU的残差；
Figure BDA00025878974500001410
表示IMU坐标系下第k帧到第k+1帧的IMU观测；
Figure BDA00025878974500001411
表示IMU对应的预积分量的协方差矩阵；
Figure BDA00025878974500001412
表示轮速计对部分状态变量约束的残差；
Figure BDA00025878974500001413
表示轮速计坐标系下第k帧到第k+1帧的轮速计观测；
Figure BDA00025878974500001414
表示轮速计观测的协方差；
其中，
Figure BDA00025878974500001415
表示为
Figure BDA00025878974500001416
Figure BDA00025878974500001417
表示为
Figure BDA00025878974500001418
Figure BDA00025878974500001419
表示为
Figure BDA00025878974500001420
Figure BDA00025878974500001421
表示为rV＝(vi-Rk·[Vi]4：6)；
其中，pi表示待配准的线特征点；p1和p2表示特征地图上的线特征上的两个点；Rk,pk分别表示第k帧状态变量Xk中的旋转矩阵和平移向量；pj表示待配准的面特征点；n表示特征地图上的平面法向量；vk表示第k帧状态变量Xk中的初始化速度；Δt表示相邻两帧的时间间隔；gW表示初始化重力加速度；Δp表示位移预积分量；Δv表示速度预积分量；Δq表示旋转增量预积分量；qk表示第k帧状态变量Xk中的旋转矩阵Rk的四元数表示形式；[·]xyz表示四元数的实部分量；bak表示第k帧状态变量Xk中的IMU中加速度计偏置；bgk表示第k帧状态变量Xk中的陀螺仪偏置；vi表示关于IMU坐标系下前进方向线速度vx、水平方向线速度vy和偏航角速度θ的矩阵，vi＝[0 0 0 vx vy 0 0 0 θ 0 0 0 0 0 0]T。
本申请实施例提供的基于多传感器融合的定位方法，能够最终确定可移动物体的当前位姿状态。而在研究本申请的过程中，发明人考虑到如下情况：尽管视觉传感器(相机)成本相对较低，但是在大部分自动驾驶的室外场景下，受曝光、光线影响，数据的观测误差以及数据关联的准确性很难保证，并且单目相机缺乏尺度信息。激光雷达传感器，受光线变化等影响较少，有限范围内的数据精度高(200米以内的观测误差趋于厘米级别)，且包含三维结构信息，在自动驾驶领域的定位和建图上有很强的实用价值。IMU是测量物体三轴角速率以及加速度的传感器，能够完整提供六自由度的位姿观测数据，其本身是一种主动传感器，不受限于场景的差异，在机器人、无人机以及无人车的应用上都是不可或缺的组成部分。但是，IMU本身在一些特殊运动的情况下(匀速或者匀加速运动)会导致系统可观性受到影响的缺点，尤其在自动驾驶应用上，车辆的运动状态近似趋于平面上的匀速运动，这时候IMU的可观性往往会受到影响。轮速计可以在低频下提供帧间的平面速度矢量，以及帧间的距离尺度测量。因而，本申请实施例采用轮速计与IMU的融合，能够弥补IMU在近似平面的匀速运动时，可观性受影响的缺陷。
本申请实施例充分融合激光雷达，IMU和轮速计，结合预生成的点云特征地图，能够准确、鲁棒地进行可移动物体(车辆、智能机器人等)的定位和状态估计。并且，整个联合优化框架能够很好的兼容新增传感器观测源，包括GPS/RTK这些绝对的观测数据，以及相机提供的帧间运动和观测约束，即在增加新增传感器观测源后，本申请实施例同样可以进行联合优化。
另外，如图5所示，本申请实施例提供一种基于多传感器融合的定位装置，应用于一种搭载有多种传感器的可移动物体上，该基于多传感器融合的定位装置，包括：
传感器数据获得单元31，用于实时获得可移动物体上搭载的各种传感器采集的传感器数据。
时间空间同步单元32，用于对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据。
数据预处理单元33，用于将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据。
状态信息获得单元34，用于获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息。
联合优化单元35，用于根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
另外，本申请实施例还提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述图1至图3所述的基于多传感器融合的定位方法。其具体实现过程请见上述方法实施例，此处不再赘述。
另外，本申请实施例还提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行上述图1至图3所述的基于多传感器融合的定位方法。其具体实现过程请见上述方法实施例，此处不再赘述。
另外，本申请实施例还提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现上述图1至图3所述的基于多传感器融合的定位方法。其具体实现过程请见上述方法实施例，此处不再赘述。
本申请实施例提供的一种基于多传感器融合的定位方法及装置，首先实时获得可移动物体上搭载的各种传感器采集的传感器数据；对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；之后，获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；从而根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。可见，本申请能够结合多种传感器数据，并且通过滑动窗口处理，实时更新当前时刻的状态信息，能够在GPS信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
本申请中应用了具体实施例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (13)

1.一种基于多传感器融合的定位方法，其特征在于，应用于一种搭载有多种传感器的可移动物体上，所述方法，包括：
实时获得可移动物体上搭载的各种传感器采集的传感器数据；
对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；
将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；
获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；
根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
2.根据权利要求1所述的方法，其特征在于，所述各种传感器包括惯性测量单元IMU、轮速计和激光雷达；其中，所述IMU包括加速度计和陀螺仪。
3.根据权利要求2所述的方法，其特征在于，所述状态信息为状态变量，所述状态变量包括平移向量、速度向量、旋转矩阵、IMU中加速度计偏置和IMU中陀螺仪偏置；
所述获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息，包括：
在用于联合优化的传感器数据未经过初始化时，实时确定预先设置的滑动窗口中的数据是否满足初始化条件；所述初始化条件包括所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励；
在所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励时，对状态变量进行初始化，形成初始化状态变量；
在对状态变量进行初始化完成后，且当前时刻之前的上一时刻进行联合优化求解结束后，获得当前时刻之前的各时刻的联合优化后的当前状态变量。
4.根据权利要求3所述的方法，其特征在于，所述实时获得可移动物体上搭载的各种传感器采集的传感器数据，包括：
实时获得IMU测量的IMU数据、轮速计测量的可移动物体轮速数据和激光雷达测量的点云数据。
5.根据权利要求4所述的方法，其特征在于，对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据，包括：
根据IMU、轮速计和激光雷达之间预先标定的外参，将可移动物体轮速数据和点云数据对齐到IMU坐标系下，并根据IMU、轮速计和激光雷达各自的数据采集帧率，将可移动物体轮速数据和点云数据的采集时的每帧与IMU的时间戳对齐，形成IMU坐标系下的，时间戳对齐的IMU数据、可移动物体轮速数据和点云数据。
6.根据权利要求4所述的方法，其特征在于，将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据，包括：
将进行时间空间同步后的IMU数据中处于连续两帧激光雷达时间戳之间的IMU数据使用预先设置的积分算法进行传播，并通过预先设置的预积分方法进行处理，得到位移预积分量、速度预积分量和旋转增量预积分量；
根据当前时刻的上一时刻在预先设置的滑动窗口中联合优化后的状态变量，以及进行时间空间同步后的IMU数据进行下一时刻的状态变量预测，得到下一时刻的状态变量预测数据；
将进行时间空间同步后的点云数据，采用下一时刻的状态变量预测数据进行去畸变处理；
对去畸变处理后的点云数据进行线面特征提取，获得线特征数据和面特征数据；
将所述线特征数据和面特征数据与预先生成的特征地图上的线特征和面特征分别进行配准，得到线特征约束关系数据和面特征约束关系数据；
将进行时间空间同步后的可移动物体轮速数据，通过预先设置的车辆动力学模型，确定在IMU坐标系下前进方向线速度、水平方向线速度和偏航角速度。
7.根据权利要求6所述的方法，其特征在于，所述方法还包括：
在所述滑动窗口中的数据观测时刻数小于预先设置的数量阈值，且所述滑动窗口中的IMU数据充分激励时，对当前滑动窗口中的点云数据和可移动物体轮速数据进行紧耦合帧间优化，得到当前滑动窗口中的当前时刻的初始化激光雷达位姿，所述初始化激光雷达位姿包括激光雷达旋转矩阵和激光雷达平移向量；
所述对状态变量进行初始化，形成初始化状态变量，包括：
根据当前时刻和当前时刻之前各时刻的初始化激光雷达位姿，采用激光雷达相邻帧间的旋转改变量以及激光雷达相邻帧间旋转增量预积分量的一致性约束，将连续多帧的一致性约束进行累加，求解得到初始化陀螺仪偏置；
根据当前滑动窗口中的初始化激光雷达位姿以及所述位移预积分量的一致性约束，求解得到激光雷达坐标系下的重力加速度，将所述激光雷达坐标系下的重力加速度转换到世界坐标系下，得到初始化重力加速度；
根据初始化陀螺仪偏置和初始化重力加速度，进行初始速度向量估计，确定初始化速度。
8.根据权利要求7所述的方法，其特征在于，根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态，包括：
在滑动窗口中的当前时刻之前的各时刻未进行联合优化时，根据当前时刻之前的各时刻的初始化状态变量和所述用于联合优化的传感器数据，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量；其中滑动窗口中当前时刻的状态变量用于表示可移动物体的当前位姿状态；
在滑动窗口中的当前时刻之前的各时刻已进行联合优化时，根据当前时刻之前的各时刻的联合优化后的当前状态变量和所述用于联合优化的传感器数据，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量；其中滑动窗口中当前时刻的状态变量用于表示可移动物体的当前位姿状态。
9.根据权利要求8所述的方法，其特征在于，所述采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量，包括：
根据联合概率密度估计函数：
Figure FDA0002587897440000031
确定滑动窗口中的各时刻的状态变量Xk；其中，k表示第k时刻及第k帧，rp(X)表示边缘化的先验项；
Figure FDA0002587897440000033
表示滑动窗口中的总帧数；i表示第i个线特征；j表示第j个面特征；
Figure FDA0002587897440000032
表示线特征约束关系数据的线特征对总数；
Figure FDA0002587897440000034
表示面特征约束关系数据的面特征对总数；
Figure FDA0002587897440000035
表示滑动窗口中第k帧中，激光雷达线特征配准的几何残差；Zi表示第i个线特征观测；
Figure FDA0002587897440000036
表示当前激光雷达线特征的观测协方差；
Figure FDA0002587897440000041
表示滑动窗口中第k帧中，激光雷达面特征配准的几何残差；Zj表示第j个面特征观测；
Figure FDA0002587897440000042
表示当前激光雷达面特征的观测协方差；
Figure FDA00025878974400000412
表示IMU的残差；
Figure FDA0002587897440000043
表示IMU坐标系下第k帧到第k+1帧的IMU观测；
Figure FDA00025878974400000411
表示IMU对应的预积分量的协方差矩阵；
Figure FDA0002587897440000045
表示轮速计对部分状态变量约束的残差；
Figure FDA0002587897440000044
表示轮速计坐标系下第k帧到第k+1帧的轮速计观测；
Figure FDA00025878974400000410
表示轮速计观测的协方差；
其中，
Figure FDA0002587897440000046
表示为
Figure FDA00025878974400000413
Figure FDA0002587897440000047
表示为
Figure FDA0002587897440000048
Figure FDA0002587897440000049
表示为
Figure FDA00025878974400000415
Figure FDA00025878974400000414
表示为rV＝(vi-Rk·[Vi]4∶6)；
其中，pi表示待配准的线特征点；p1和p2表示特征地图上的线特征上的两个点；Rk,pk分别表示第k帧状态变量Xk中的旋转矩阵和平移向量；pj表示待配准的面特征点；n表示特征地图上的平面法向量；vk表示第k帧状态变量Xk中的初始化速度；Δt表示相邻两帧的时间间隔；gW表示初始化重力加速度；Δp表示位移预积分量；Δv表示速度预积分量；Δq表示旋转增量预积分量；qk表示第k帧状态变量Xk中的旋转矩阵Rk的四元数表示形式；[·]xyz表示四元数的实部分量；bak表示第k帧状态变量Xk中的IMU中加速度计偏置；bgk表示第k帧状态变量Xk中的陀螺仪偏置；vi表示关于IMU坐标系下前进方向线速度vx、水平方向线速度vy和偏航角速度θ的矩阵，vi＝[0 0 0 vx vy 0 0 0 θ 0 0 0 0 0 0]T。
10.一种基于多传感器融合的定位装置，其特征在于，应用于一种搭载有多种传感器的可移动物体上，所述装置，包括：
传感器数据获得单元，实时获得可移动物体上搭载的各种传感器采集的传感器数据；
时间空间同步单元，用于对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；
数据预处理单元，用于将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；
状态信息获得单元，用于获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；
联合优化单元，用于根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
11.一种计算机可读存储介质，其特征在于，包括程序或指令，当所述程序或指令在计算机上运行时，实现权利要求1至9任一项所述的基于多传感器融合的定位方法。
12.一种包含指令的计算机程序产品，其特征在于，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如权利要求1至9任一项所述的基于多传感器融合的定位方法。
13.一种计算机服务器，其特征在于，包括存储器，以及与所述存储器通信连接的一个或多个处理器；所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如权利要求1至9任一项所述的基于多传感器融合的定位方法。
CN202010686900.1A 2020-07-16 2020-07-16 一种基于多传感器融合的定位方法及装置 Active CN113945206B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN202010686900.1A CN113945206B (zh) 2020-07-16 2020-07-16 一种基于多传感器融合的定位方法及装置
EP21184662.1A EP3940421A1 (en) 2020-07-16 2021-07-09 Positioning method and device based on multi-sensor fusion
JP2021116216A JP2022019642A (ja) 2020-07-16 2021-07-14 マルチセンサ融合に基づく測位方法及び装置
US17/378,484 US12092740B2 (en) 2020-07-16 2021-07-16 Positioning method and device based on multi-sensor fusion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010686900.1A CN113945206B (zh) 2020-07-16 2020-07-16 一种基于多传感器融合的定位方法及装置

Publications (2)

Publication Number Publication Date
CN113945206A true CN113945206A (zh) 2022-01-18
CN113945206B CN113945206B (zh) 2024-04-19

Family

ID=77447673

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010686900.1A Active CN113945206B (zh) 2020-07-16 2020-07-16 一种基于多传感器融合的定位方法及装置

Country Status (4)

Country Link
US (1) US12092740B2 (zh)
EP (1) EP3940421A1 (zh)
JP (1) JP2022019642A (zh)
CN (1) CN113945206B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114593735A (zh) * 2022-01-26 2022-06-07 奥比中光科技集团股份有限公司 一种位姿预测方法及装置
CN114608569A (zh) * 2022-02-22 2022-06-10 杭州国辰机器人科技有限公司 三维位姿估计方法、系统、计算机设备及存储介质
CN114818836A (zh) * 2022-06-29 2022-07-29 电科疆泰(深圳)科技发展有限公司 一种射击计数方法、装置、电子设备及存储介质
CN114820800A (zh) * 2022-06-29 2022-07-29 山东信通电子股份有限公司 一种用于输电线路的实时巡检方法及设备
CN114964175A (zh) * 2022-03-30 2022-08-30 华南理工大学 多传感器数据同步采集装置及采集方法
CN115655287A (zh) * 2022-10-12 2023-01-31 中汽创智科技有限公司 一种车辆定位方法、装置、设备及存储介质
CN116736322A (zh) * 2023-08-15 2023-09-12 山东科技大学 融合相机图像与机载激光雷达点云数据的速度预测方法
CN116774195A (zh) * 2023-08-22 2023-09-19 国网天津市电力公司滨海供电分公司 多传感器联合标定的激励判断与参数自调节方法及系统
CN116817928A (zh) * 2023-08-28 2023-09-29 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法
WO2023226375A1 (zh) * 2022-05-22 2023-11-30 远也科技（苏州）有限公司 一种确定运动参数的方法、装置及系统
WO2024104225A1 (zh) * 2022-11-17 2024-05-23 中移(成都)信息通信科技有限公司 地图的构建方法、定位方法及装置、设备、存储介质

Families Citing this family (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114018274B (zh) * 2021-11-18 2024-03-26 阿波罗智能技术(北京)有限公司 车辆定位方法、装置及电子设备
CN114593724B (zh) * 2022-01-21 2024-06-14 北京邮电大学 集群融合定位方法和装置
CN114155495B (zh) * 2022-02-10 2022-05-06 西南交通大学 跨海桥梁中车辆运行的安全监控方法、装置、设备及介质
CN114608568B (zh) * 2022-02-22 2024-05-03 北京理工大学 一种基于多传感器信息即时融合定位方法
CN114527481A (zh) * 2022-02-24 2022-05-24 东南大学 一种面向遮挡测评环境的多传感高精度融合测量方法
CN114739355B (zh) * 2022-04-12 2024-05-17 深圳市德传技术有限公司 基于gnss与三轴加速计的滑坡监测方法、系统及介质
CN114964270B (zh) * 2022-05-17 2024-04-26 驭势科技（北京）有限公司 融合定位方法、装置、车辆及存储介质
CN115265581B (zh) * 2022-05-18 2024-08-16 广州文远知行科技有限公司 激光雷达与惯性测量单元的标定参数确定方法及相关设备
US20230400306A1 (en) * 2022-06-14 2023-12-14 Volvo Car Corporation Localization for autonomous movement using vehicle sensors
CN114998436A (zh) * 2022-06-27 2022-09-02 北京易航远智科技有限公司 对象标注方法、装置、电子设备及存储介质
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN114858226B (zh) * 2022-07-05 2022-10-25 武汉大水云科技有限公司 一种无人机山洪流量测量方法、装置及设备
CN115407357B (zh) * 2022-07-05 2024-05-31 东南大学 基于大场景的低线束激光雷达-imu-rtk定位建图算法
KR102480565B1 (ko) * 2022-09-05 2022-12-22 한화시스템(주) 무인 함정의 자율항해를 위한 빅데이터 수집 시스템 및 방법
CN115235454B (zh) * 2022-09-15 2022-12-30 中国人民解放军国防科技大学 行人运动约束的视觉惯性融合定位与建图方法和装置
TWI831440B (zh) * 2022-10-26 2024-02-01 財團法人車輛研究測試中心 電腦視覺式車輛定位融合系統及方法
CN115655302B (zh) * 2022-12-08 2023-03-21 安徽蔚来智驾科技有限公司 激光里程计的实现方法、计算机设备、存储介质及车辆
CN116228188B (zh) * 2022-12-19 2023-11-24 西南交通大学 铁路客运站设备管理方法、装置、设备及可读存储介质
CN115801046B (zh) * 2023-02-08 2023-05-30 深圳市思创捷物联科技有限公司 飞行状态自动识别方法、系统、设备及计算机存储介质
CN116125507B (zh) * 2023-02-16 2024-08-09 腾讯科技（深圳）有限公司 移动终端的定位方法、装置、设备及存储介质
CN115965756B (zh) * 2023-03-13 2023-06-06 安徽蔚来智驾科技有限公司 地图构建方法、设备、驾驶设备和介质
CN116429094B (zh) * 2023-06-15 2023-09-26 小米汽车科技有限公司 定位方法、装置、电子设备及存储介质
CN116460653A (zh) * 2023-06-20 2023-07-21 山东硕德博数控机械有限公司 基于传感器的数控机床数据处理方法
CN116775750B (zh) * 2023-08-15 2023-10-31 中钢集团武汉安全环保研究院有限公司 一种基于冶金行业的数据管理方法和装置
CN116929338B (zh) * 2023-09-15 2024-01-09 深圳市智绘科技有限公司 地图构建方法、设备及存储介质
CN117031442B (zh) * 2023-10-08 2024-01-02 中国地质大学（武汉） 融合多通道波形数据的激光雷达水域地形测量方法及装置
CN117268373B (zh) * 2023-11-21 2024-02-13 武汉大学 一种多传感器信息融合的自主导航方法及系统
CN118392168B (zh) * 2024-06-24 2024-09-27 广东工业大学 一种应用于边防的视巡一体化机器人的巡检方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110726413A (zh) * 2019-10-25 2020-01-24 中国人民解放军国防科技大学 面向大规模slam的多传感器融合与数据管理机制
CN110837854A (zh) * 2019-10-30 2020-02-25 东南大学 一种基于因子图的auv多源信息融合方法和设备
CN111258313A (zh) * 2020-01-20 2020-06-09 深圳市普渡科技有限公司 多传感器融合slam系统及机器人

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016187760A1 (en) * 2015-05-23 2016-12-01 SZ DJI Technology Co., Ltd. Sensor fusion using inertial and image sensors
US11348269B1 (en) * 2017-07-27 2022-05-31 AI Incorporated Method and apparatus for combining data to construct a floor plan
US11274929B1 (en) * 2017-10-17 2022-03-15 AI Incorporated Method for constructing a map while performing work
US11241791B1 (en) * 2018-04-17 2022-02-08 AI Incorporated Method for tracking movement of a mobile robotic device
US11153503B1 (en) * 2018-04-26 2021-10-19 AI Incorporated Method and apparatus for overexposing images captured by drones
US11940277B2 (en) * 2018-05-29 2024-03-26 Regents Of The University Of Minnesota Vision-aided inertial navigation system for ground vehicle localization
US11181379B2 (en) * 2019-09-12 2021-11-23 Robert Bosch Gmbh System and method for enhancing non-inertial tracking system with inertial constraints
US11852751B2 (en) * 2020-03-02 2023-12-26 Beijing Baidu Netcom Science And Technology Co., Ltd. Method, apparatus, computing device and computer-readable storage medium for positioning

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110726413A (zh) * 2019-10-25 2020-01-24 中国人民解放军国防科技大学 面向大规模slam的多传感器融合与数据管理机制
CN110837854A (zh) * 2019-10-30 2020-02-25 东南大学 一种基于因子图的auv多源信息融合方法和设备
CN111258313A (zh) * 2020-01-20 2020-06-09 深圳市普渡科技有限公司 多传感器融合slam系统及机器人

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HAOYANG YE1 , ET AL: "Tightly Coupled 3D Lidar Inertial Odometry and Mapping", 《2019 INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》, pages 3144 - 3150 *
TONG QIN， ET AL: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimat", 《IEEE TRANSACTIONS ON ROBOTICS》, vol. 34, no. 4, pages 1004 - 1020, XP011688854, DOI: 10.1109/TRO.2018.2853729 *

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114593735A (zh) * 2022-01-26 2022-06-07 奥比中光科技集团股份有限公司 一种位姿预测方法及装置
CN114593735B (zh) * 2022-01-26 2024-05-31 奥比中光科技集团股份有限公司 一种位姿预测方法及装置
CN114608569A (zh) * 2022-02-22 2022-06-10 杭州国辰机器人科技有限公司 三维位姿估计方法、系统、计算机设备及存储介质
CN114608569B (zh) * 2022-02-22 2024-03-01 杭州国辰机器人科技有限公司 三维位姿估计方法、系统、计算机设备及存储介质
CN114964175A (zh) * 2022-03-30 2022-08-30 华南理工大学 多传感器数据同步采集装置及采集方法
WO2023226375A1 (zh) * 2022-05-22 2023-11-30 远也科技（苏州）有限公司 一种确定运动参数的方法、装置及系统
CN114818836A (zh) * 2022-06-29 2022-07-29 电科疆泰(深圳)科技发展有限公司 一种射击计数方法、装置、电子设备及存储介质
CN114820800A (zh) * 2022-06-29 2022-07-29 山东信通电子股份有限公司 一种用于输电线路的实时巡检方法及设备
CN115655287A (zh) * 2022-10-12 2023-01-31 中汽创智科技有限公司 一种车辆定位方法、装置、设备及存储介质
WO2024104225A1 (zh) * 2022-11-17 2024-05-23 中移(成都)信息通信科技有限公司 地图的构建方法、定位方法及装置、设备、存储介质
CN116736322A (zh) * 2023-08-15 2023-09-12 山东科技大学 融合相机图像与机载激光雷达点云数据的速度预测方法
CN116736322B (zh) * 2023-08-15 2023-10-20 山东科技大学 融合相机图像与机载激光雷达点云数据的速度预测方法
CN116774195B (zh) * 2023-08-22 2023-12-08 国网天津市电力公司滨海供电分公司 多传感器联合标定的激励判断与参数自调节方法及系统
CN116774195A (zh) * 2023-08-22 2023-09-19 国网天津市电力公司滨海供电分公司 多传感器联合标定的激励判断与参数自调节方法及系统
CN116817928B (zh) * 2023-08-28 2023-12-01 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法
CN116817928A (zh) * 2023-08-28 2023-09-29 北京交通大学 基于因子图优化的卫导/惯导列车多源融合定位的方法

Also Published As

Publication number Publication date
US12092740B2 (en) 2024-09-17
CN113945206B (zh) 2024-04-19
JP2022019642A (ja) 2022-01-27
US20220018962A1 (en) 2022-01-20
EP3940421A1 (en) 2022-01-19

Similar Documents

Publication Publication Date Title
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN113819914B (zh) 一种地图构建方法及装置
CN109341706B (zh) 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN110160542B (zh) 车道线的定位方法和装置、存储介质、电子装置
CN109885080B (zh) 自主控制系统及自主控制方法
Zhou et al. Deep learning-enabled fusion to bridge GPS outages for INS/GPS integrated navigation
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
CN111142091B (zh) 一种融合车载信息的自动驾驶系统激光雷达在线标定方法
CN113819905B (zh) 一种基于多传感器融合的里程计方法及装置
CN115082549A (zh) 位姿估计方法及装置、以及相关设备和存储介质
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN113252022A (zh) 一种地图数据处理方法及装置
CN113252051A (zh) 一种地图构建方法及装置
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及系统
Noureldin et al. a Framework for Multi-Sensor Positioning and Mapping for Autonomous Vehicles
Chen et al. An integrated GNSS/INS/DR positioning strategy considering nonholonomic constraints for intelligent vehicle
AU2021204783A1 (en) Positioning method and device based on multi-sensor fusion
Wu et al. Integrated navigation algorithm based on vision-inertial extended Kalman filter for low-cost unmanned aerial vehicle

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
GR01 Patent grant
GR01 Patent grant