CN112880674A - 一种行驶设备的定位方法、装置、设备及存储介质 - Google Patents

一种行驶设备的定位方法、装置、设备及存储介质 Download PDF

Info

Publication number
CN112880674A
CN112880674A CN202110084188.2A CN202110084188A CN112880674A CN 112880674 A CN112880674 A CN 112880674A CN 202110084188 A CN202110084188 A CN 202110084188A CN 112880674 A CN112880674 A CN 112880674A
Authority
CN
China
Prior art keywords
information
space vector
state space
error
determining
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
CN202110084188.2A
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.)
LeiShen Intelligent System Co Ltd
Original Assignee
LeiShen Intelligent System 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 LeiShen Intelligent System Co Ltd filed Critical LeiShen Intelligent System Co Ltd
Priority to CN202110084188.2A priority Critical patent/CN112880674A/zh
Publication of CN112880674A publication Critical patent/CN112880674A/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/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
    • 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

Abstract

本申请实施例公开了一种行驶设备的定位方法、装置、设备及存储介质。所述方法包括：根据行驶设备的状态数据和初始滤波器参数，确定预测状态空间向量和预测误差协方差矩阵；根据预测状态空间向量的一般状态空间向量，对先验地图数据和点云数据进行迭代匹配，并确定第一位姿信息和观测噪声矩阵；根据轮速计采集的脉冲数据，确定第二位姿信息和测量速度信息，并确定轮速计估计误差；根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置误差状态空间向量和当前误差协方差矩阵。本申请能够为行驶设备提供稳定准确的定位信息。

Description

一种行驶设备的定位方法、装置、设备及存储介质
技术领域
本发明实施例涉及计算机技术领域，尤其涉及一种行驶设备的定位方法、装置、设备及存储介质。
背景技术
随着人工智能技术的发展，智能化的行驶设备(如智能机器人、自动行驶车辆等)在生产和生活中的使用越来越广泛。在行驶设备运行的过程中，对其进行实时定位是监管行驶设备的关键环节。
目前，现有技术在对行驶设备进行定位时，通常是在环境地图已知的情况下，采用行驶设备上安装的单一传感器采集数据进行行驶设备的定位，存在定位结果准确性低的问题，亟需改进。
发明内容
本申请实施例提供了一种行驶设备的定位方法、装置、设备及存储介质，能够使行驶设备实现精确的自主定位。
第一方面，本申请实施例提供了一种行驶设备的定位方法，该方法包括：
根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定所述行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，所述预测状态空间向量包括一般状态空间向量和误差状态空间向量；
根据所述预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第一位姿信息和观测噪声矩阵；
根据轮速计采集的脉冲数据，确定所述行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；
根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵、所述轮速计估计误差和所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，并重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵。
第二方面，本申请实施例提供了一种行驶设备的定位装置，该装置包括：
预测模块，用于根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定所述行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，所述预测状态空间向量包括一般状态空间向量和误差状态空间向量；
匹配模块，用于根据所述预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第一位姿信息和观测噪声矩阵；
第一确定模块，用于根据轮速计采集的脉冲数据，确定所述行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；
第二确定模块，用于根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵、所述轮速计估计误差和所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，并重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵。
第三方面，本申请实施例提供了一种电子设备，该电子设备包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本申请任意实施例所述的行驶设备的定位方法。
第四方面，本申请实施例提供了一种非易失性可读存储介质，所述非易失性可读存储介质存储有计算机可执行指令，其中，该计算机可执行指令用于实现本申请任意实施例所述的行驶设备的定位方法。
本申请实施例提供了一种行驶设备的定位方法、装置、设备及存储介质，根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵；根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵；根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定行驶设备的当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵。执行本申请方案，能够为行驶设备提供稳定准确的定位信息。
应当理解，本部分所描述的内容并非旨在标识本申请的实施例的关键或重要特征，也不用于限制本申请的范围。本申请的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本申请的限定。其中：
图1A为本申请实施例一提供的一种行驶设备的定位方法的第一流程示意图；
图1B为本申请实施例一提供的行驶设备的运动模型示意图；
图2为本申请实施例二提供的一种行驶设备的定位方法的第二流程示意图；
图3A为本申请实施例三提供的一种行驶设备的定位方法的第三流程示意图；
图3B为本申请实施例三提供的一种行驶设备的定位方法的局部地图更新示意图；
图3C为本申请实施例二提供的一种行驶设备的定位方法的原理示意图；
图4为本申请实施例四提供的一种行驶设备的定位装置的结构示意图；
图5是用来实现本申请实施例的一种行驶设备的定位方法的电子设备的框图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
实施例一
图1A为本申请实施例一提供的一种行驶设备的定位方法的第一流程示意图；图1B为本申请实施例一提供的行驶设备的运动模型示意图。本实施例可适用于行驶设备运行过程中，对其进行实时定位的情况。本实施例提供的一种行驶设备的定位方法可以由本申请实施例提供的行驶设备的定位装置来执行，该装置可以通过软件和/或硬件的方式实现，并集成在执行本方法的电子设备中。在本实施例中，该电子设备可以是服务器设备，还可以是行驶设备，若为行驶设备时，可以是智能机器人、无人驾驶车辆等。
参见图1A，本实施例的方法包括但不限于如下步骤：
S110、根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵。
其中，组合惯导(Inertial Measurement Unit，IMU)又称惯性测量单元，其至少包括加速度计和陀螺仪。状态数据包括加速度信息和角速度信息，其中，加速度信息是通过加速度计采集的，角速度信息是通过陀螺仪采集的。本申请实施例的滤波器可以是卡尔曼滤波器，可选的，卡尔曼滤波器可以是传统的卡尔曼滤波器，也可以是误差状态卡尔曼滤波器。优选的，卡尔曼滤波器采用误差状态卡尔曼滤波器，误差状态卡尔曼滤波器能够有效避免卡尔曼滤波中参数冗余及协方差矩阵存在奇异性的缺点。初始滤波器参数是指为卡尔曼滤波器设置的初始参数，包括初始定位信息(即行驶设备的初始位姿信息)、初始误差协方差矩阵、加速度计的白噪声方差、陀螺仪的白噪声方差、速度随机游走方差和角度随机游走方差。
在本申请实施例中，预测状态空间向量包含一般状态空间向量X和误差状态空间向量δX两部分。其中，一般状态空间向量X中包含位置P、速度v、姿态(即角度信息)q、加速度偏差ab及角速度偏差wb；误差状态空间向量δX包含位置误差δP、速度误差δv、姿态误差δq、加速度偏差误差δab及角速度偏差误差δwb。如下公式(1)-(2)所示：
X＝[P,v,q,ab,wb]T (1)；
δX＝[δP,δv,δq,δab,δwb]T (2)；
具体的，公式(1)可由下述公式(3)表示，可基于下述公式(3)来预测行驶设备的预测状态空间向量中的一般状态空间向量X，即(Pt，vt，qt，ab(t)，wb(t))。
Figure BDA0002910328460000031
其中，Pt是当前时刻的位置；Pt-1是上一时刻的位置；vt-1是上一时刻的速度；Δt是相邻两次采集数据的时间间隔；R是上一时刻到当前时刻的当前姿态旋转矩阵；am是组合惯导中的加速度计采集的当前加速度；ab(t-1)是上一时刻的加速度偏差；g重力加速度；vt是当前时刻的速度；qt是当前时刻的姿态；qt-1是上一时刻的姿态；wm是组合惯导中的陀螺仪采集的当前角速度；wb(t)是当前时刻的角速度偏差；wb(t-1)是上一时刻的角速度偏差；ab(t)是当前时刻的加速度偏差。
具体的，公式(2)可由下述公式(4)-(5)表示，可基于下述公式(4)-(5)来预测行驶设备的预测状态空间向量中的误差状态空间向量δX，即(δP，δv，δq，δab(t)，δwb(t))。
Figure BDA0002910328460000041
Figure BDA0002910328460000042
其中，δPt是当前时刻的位置误差；δPt-1是上一时刻的位置误差；δvt-1是上一时刻的速度误差；Δt是相邻两次采集数据的时间间隔；δvt是当前时刻的速度误差；R是上一时刻到当前时刻的当前姿态旋转矩阵；am是组合惯导中的加速度计采集的当前加速度；ab(t-1)是上一时刻的加速度偏差；δqt-1是上一时刻的姿态误差；δqt是当前时刻的姿态误差；wm是组合惯导中的陀螺仪采集的当前角速度；wb(t-1)是上一时刻的角速度偏差；δab(t)为当前时刻的加速度偏差误差；δab(t-1)为上一时刻的加速度偏差误差；δwb(t)为当前时刻的角速度偏差误差；δwb(t-1)为上一时刻的角速度偏差误差；Vi为加速度计白噪声；θi陀螺仪白噪声；Ai为加速度计随机游走噪声；Ωi为陀螺仪随机游走噪声；I是单位矩阵；σ2an为加速度计的白噪声方差、σ2wn为陀螺仪的白噪声方差、σ2aw为速度随机游走方差和σ2ww为角度随机游走方差。
在本申请实施例中，预测状态空间向量对应的预测误差协方差矩阵可由下述(6)-(9)计算而得：
Figure BDA0002910328460000043
Figure BDA0002910328460000051
Figure BDA0002910328460000052
Figure BDA0002910328460000053
其中，Fx是状态转移矩阵；I3是3×3的单位矩阵；Δt是相邻两次采集数据的时间间隔；R是上一时刻到当前时刻的当前姿态旋转矩阵；am是组合惯导中的加速度计采集的当前加速度；ab是加速度偏差；wm是组合惯导中的陀螺仪采集的当前角速度；wb是角速度偏差；Fi是扰动矩阵；Qi是噪声矩阵；σ2an为加速度计的白噪声方差、σ2wn为陀螺仪的白噪声方差、σ2aw为速度随机游走方差和σ2ww为角度随机游走方差；S为当前预测误差协方差矩阵；S上是上一次的预测误差协方差矩阵。
S120、根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵。
其中，本申请实施例的行驶设备上还配置有激光雷达，该激光雷达用于在行驶设备行驶过程中，扫描周围环境的点云数据。所谓先验地图数据为已经构建好的高精度的环境地图，该先验地图数据可以预先存储在电子设备(如行驶设备)中。
可选的，本申请实施例采用正态分布变换(Normal Distance Transform，NDT)算法进行迭代匹配。在一些实施例中，还可以采用其他迭代匹配算法，对此本申请实施例不进行限定。本步骤根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵的具体过程可以通过以下子步骤实现：
S1201、将预测状态空间向量的一般状态空间向量中的位姿信息从惯导坐标系转换到雷达坐标系，并将转换到雷达坐标系下的预测状态空间向量的一般状态空间向量中的位姿信息作为初始位姿矩阵。
具体的，本子步骤在对先验地图数据和激光雷达采集的点云数据进行迭代匹配之前，需要将预测状态空间向量的一般状态空间向量中的位姿信息从惯导坐标系转换到雷达坐标系。坐标系转换公式(10)如下所示：
Figure BDA0002910328460000054
其中，FI为惯导坐标系下预测状态空间向量的一般状态空间向量中的位姿信息，包含位置信息和姿态信息；FL为雷达坐标系下预测状态空间向量的一般状态空间向量中的位姿信息，包含位置信息和姿态信息；
Figure BDA0002910328460000061
为惯导坐标系到激光雷达坐标系的转换矩阵。
进一步地，将转换到雷达坐标系下的预测状态空间向量的一般状态空间向量中的位姿信息FL作为初始位姿矩阵，需要说明的是，该初始位姿矩阵即为首次迭代匹配的预测值(即初值)。
S1202、基于正态分布变换算法和初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及匹配分数关联的目标位姿矩阵。
其中，本申请实施例可以将每次对先验地图数据和激光雷达采集的点云数据中的各位置点进行匹配后得到的各位置点的概率密度匹配之和作为本次迭代匹配的匹配分数。
在本申请的具体实施例中，将初始位姿矩阵带入下述概率密度之和计算公式(11)，基于激光雷达采集的点云数据和先验地图数据，计算先验地图数据和点云数据的匹配情况。本实施例需要进行预设次数的迭代匹配计算，在多次迭代匹配的过程中，如果位姿矩阵能使得点云数据与先验地图数据匹配的很好，那么基于公式(11)计算的匹配分数将会很大。
具体的，假设激光雷达采集的点云数据为A＝{a0，...，an}，其中，A为包含n个点的三维点云数据；先验地图数据为B＝{b0，...，bn}，其中，B为包含n个点的位置坐标。首先，利用NDT算法将先验地图数据B构建成多维变量的正态分布。其次，通过概率密度之和计算公式，基于初始位姿矩阵(即迭代匹配的预测值)和预先设置的迭代次数和迭代步长，对先验地图数据和点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的迭代匹配结果，即匹配分数以及匹配分数关联的目标位姿矩阵。可选的，NDT算法可以采用牛顿优化算法、最小二乘法等对先验地图数据和点云数据进行预设次数的迭代匹配。匹配分数的计算方式如下：
Figure BDA0002910328460000062
Figure BDA0002910328460000063
Figure BDA0002910328460000064
ai′＝T(ai,FL) (14)；
其中，score为匹配分数，即概率密度之和；ai′为点云数据中第i个点云在地图坐标系下的坐标值；d为先验地图数据的均值；n为点云数据中的点云数量(即先验地图数据中的位置点数量)；∑为先验地图数据的方差；bi为先验地图数据中第i个点的坐标值；ai为在激光雷达坐标系下的点云数据中第i个点云的坐标值；FL为当前迭代匹配的目标位姿矩阵(即下一次迭代匹配的预测值)，其中，首次迭代匹配时FL为初始位姿矩阵；T为雷达坐标系到地图坐标系的转换算子。需要说明的是，本子步骤每次基于公式(11)进行迭代匹配，都可计算出一组概率密度之和(即匹配分数score)以及匹配分数score关联的目标位姿矩阵FL。
S1203、将迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为行驶设备的第一位姿信息。
具体的，上述步骤基于每次迭代匹配，都可计算出一组匹配分数score以及该匹配分数score关联的目标位姿矩阵FL。本子步骤从多次匹配迭代得到的匹配分数score中，确定匹配分数最高的score。此时，先验地图数据和点云数据的匹配情况是最好的。将最高匹配分数score所对应的目标位姿矩阵FL作为第一位姿信息。
S1204、根据迭代匹配结果中的最高匹配分数，确定匹配置信度。
在本申请的具体实施例中，由于雷达采集的点云数据与先验地图数据进行迭代匹配时，可能会存在一定的匹配误差。在确定迭代匹配得到的第一位姿信息对应的观测噪声矩阵时，需要先基于下述公式(15)计算匹配置信度，其中，该匹配置信度代表当前时刻采用激光雷达进行定位的可信度。
Pbelief＝1/score高×I6×6 (15)；
其中，Pbelief表示匹配置信度；I6×6表示6×6的单位矩阵；score高为上述公式(11)进行预设次数的迭代匹配得到的匹配分数中的最高匹配分数。
S1205、根据匹配置信度和旋转矩阵，确定观测噪声矩阵。
在本申请实施例中，可基于下述公式(16)-公式(17)计算第一位姿信息的观测噪声矩阵。
Figure BDA0002910328460000071
Figure BDA0002910328460000072
其中，R3×3表示旋转矩阵；Rcur表示第一位姿信息的旋转变换矩阵；Vlidar表示第一位姿信息对应的观测噪声矩阵。
S130、根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差。
其中，本申请实施例中的行驶设备上配置有轮速计，该轮速计在行驶设备运行过程中，实时或定时进行数据采集。轮速计采集的脉冲数据指的是单位时间内脉冲数目。行驶设备的运动模型是基于行驶设备的运动原理为该行驶设备搭建的模型，例如，图1B示出的行驶设备的运动模型，其中的Px和Py分别为行驶设备在水平方向X轴和Y轴的位置坐标。第二位姿信息指的是基于轮速计采集的数据确定的行驶设备当前的位置信息以及姿态信息，测量速度信息指的是基于轮速计采集的数据确定的行驶设备在X轴和Y轴方向的速度。
可选的，本步骤根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息并确定轮速计估计误差的具体过程可以通过以下子步骤实现：
S1301、根据轮速计采集的脉冲数据，确定行驶设备当前时刻相对于上一时刻的位置增量和当前时刻的轮速信息。
其中，轮速计作为轮速传感器的一种，轮速计包括轮速计编码器，轮速计用于测量行驶设备的目标车轮的轮速。具体的，轮速计用于感应行驶设备的目标车轮的转动，输出电压信号，以使行驶设备的处理模块对电压信号进行数据处理，得到对应的脉冲数据。
在本实施例中，行驶设备设置有角度传感器，角度传感器安装在驱动轮上，用于采集行驶设备的中轴线与X轴方向的夹角。通过轮速计采集的脉冲数据确定当前时刻行驶设备的轮速信息，其中，轮速信息包括左轮的轮速和右轮的轮速。在一些实施例中，假设行驶设备的所有左轮、所有右轮的半径分别一致，则行驶设备的轮速信息的确定步骤包括：根据轮速计采集的脉冲数据和左轮半径，计算左轮的轮速；根据轮速计采集的脉冲数据和右轮半径，计算右轮的轮速。
在本实施例中，行驶设备当前时刻相对于上一时刻的位置增量的确定过程为：根据轮速计的工作原理，结合行驶设备的运动模型，采用如下公式(18)，根据轮速计采集的脉冲数据，计算行驶设备当前时刻相对于上一时刻的位置增量。
Figure BDA0002910328460000081
其中，ΔD是行驶设备当前时刻相对于上一时刻(即相邻时间间隔内)的位置增量；Q是行驶设备的车轮半径；Re是行驶设备的轮速计编码器的分辨率；ΔN为轮速计采集的脉冲数据，即单位时间内脉冲数目；α是行驶设备的中轴线与X轴方向的夹角。
S1302、根据当前时刻的轮速信息，确定行驶设备当前时刻相对于上一时刻的角度增量。
在本申请实施例中，经上述步骤，通过轮速计采集的脉冲数据确定当前时刻行驶设备的轮速信息，其中，轮速信息包括右轮的轮速VR和左轮的轮速VL。然后，根据当前时刻的轮速信息，采用如下公式(19)计算行驶设备当前时刻相对于上一时刻的角度增量。
Figure BDA0002910328460000082
其中，Δq表示行驶设备当前时刻相对于上一时刻的角度增量；VR表示行驶设备的右轮轮速；VL表示行驶设备的左轮轮速；E表示左轮与右轮之间的轮距；Δt表示相邻两次采集数据的时间间隔。
S1303、基于行驶设备的运动模型，根据行驶设备在上一时刻的位姿信息、位置增量以及角度增量，确定行驶设备在当前时刻的第二位姿信息。
在本申请实施例中，基于行驶设备的运动模型，可以采用如下公式(20)来行驶设备当前时刻在二维水平面上的位姿信息。
Figure BDA0002910328460000091
其中，(xt，yt，qt)为行驶设备当前时刻在二维水平面上的位姿信息；(xt-1，yt-1，qt-1)为行驶设备上一时刻在二维水平面上的位姿信息；Δq为行驶设备当前时刻相对于上一时刻(即相邻时间间隔内)的角度增量；ΔD是行驶设备当前时刻相对于上一时刻(即相邻时间间隔内)的位置增量。
需要说明的是，通过上述公式(20)计算出来的是行驶设备当前时刻在二维水平面内的位姿信息，即没有Z轴方向的位姿信息。但是，由于第二位姿信息应当包含X轴、Y轴和Z轴三个方向的位姿信息。所以，本申请实施例需要在上述公式(20)计算结果的基础上，添加Z轴方向的位姿信息，方可得到行驶设备在当前时刻的第二位姿信息。可选的，由于行驶设备行驶过程中的高度信息变化不大，所以行驶设备当前时刻在Z轴方向的位姿信息可以沿用该行驶设备上一时刻在Z轴方向的位姿信息，即在上述公式(20)计算结果的基础上，添加该行驶设备上一时刻在Z轴方向的位姿信息，即可得到行驶设备在当前时刻的第二位姿信息。
S1304、根据当前时刻的轮速信息和角度增量，确定行驶设备在当前时刻的测量速度信息。
经上述步骤，根据行驶设备当前时刻的轮速信息和行驶设备当前时刻相对于上一时刻的角度增量，确定行驶设备在当前时刻的测量速度信息。具体的，行驶设备当前时刻的测量速度信息的确定过程为：在行驶设备运行过程中，通过轮速计实时或定时采集行驶设备的轮速信息，即行驶设备的左轮与右轮的轮速信息。根据采集的行驶设备的左轮与右轮的轮速信息，计算当前行驶设备在X轴方向和Y轴方向的测量速度信息，测量速度信息的计算公式(21)-(22)如下所示：
Figure BDA0002910328460000092
Figure BDA0002910328460000093
其中，VR是行驶设备当前时刻右轮的轮速信息；VL是行驶设备当前时刻左轮的轮速信息；Δq是行驶设备当前时刻相对于上一时刻的角度增量；Vx是行驶设备当前时刻在X轴方向的测量速度信息；Vy是行驶设备当前时刻在Y轴方向的测量速度信息。
需要说明的是，通过上述公式(21)和(22)计算出的是行驶设备当前时刻在二维水平面内的测量速度信息，即没有Z轴方向的测量速度信息，此时与添加Z轴方向位姿信息类似，可以是将该行驶设备上一时刻在Z轴方向的测量速度信息作为当前时刻在Z轴方向的测量速度信息。即，将上述公式(21)和(22)计算出的Vx、Vy，以及行驶设备上一时刻在Z轴方向的测量速度信息合并称为行驶设备的测量速度信息。
S1305、根据行驶设备的上一时刻的观测噪声矩阵和预设噪声值，确定轮速计估计误差。
在本申请实施例中，第二位姿信息对应的轮速计估计误差的计算方式可以是：当行驶设备是第一次定位时，轮速计估计误差P2可以为第一次地图匹配时，基于上述公式(17)计算出的观测噪声矩阵Vlidar，即P2＝Vlidar；当行驶设备不是第一次定位(如第二次、第三次等)时，可在上一次定位时确定的观测噪声矩阵Vlidar的基础上迭代预设噪声值，如预设的高斯白噪声，得到本次定位的轮速计估计误差P2。例如当行驶设备第二次定位时，可在上一次定位时确定的观测噪声矩阵Vlidar的基础上迭代预设的高斯白噪声，得到本次定位的轮速计估计误差P2。
S140、根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定行驶设备的当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵。
在本申请实施例中，本步骤可以分为四个子步骤进行实现：第一，对第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵以及轮速计估计误差进行预处理，得到行驶设备的观测信息和观测误差。第二，根据观测信息、观测误差以及预测误差协方差矩阵，确定行驶设备的当前状态空间向量。第三、根据观测误差和预测误差协方差矩阵，确定当前误差协方差矩阵。第四，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵。需要注意的，这四个子步骤的具体实现过程将在下述实施例中进行具体的解释说明。
本实施例提供的技术方案，根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，预测状态空间向量包括一般状态空间向量和误差状态空间向量；根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵；根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定行驶设备的当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵。本申请方案利用组合惯导和雷达多种传感器之间的不同特点进行优势互补，使用多传感器融合技术进行组合定位，解决了现有技术中使用单一传感器的数据存在不确定性以及可靠性较低的问题。执行本申请方案，能够为行驶设备提供稳定准确的定位信息。
实施例二
图2为本申请实施例二提供的一种行驶设备的定位方法的第二流程示意图。本申请实施例是在上述实施例的基础上进行优化，具体优化为：增加了对行驶设备的当前状态空间向量和当前误差协方差矩阵的确定过程，以及当前状态空间向量的误差状态空间向量和当前误差协方差矩阵的重置过程进行详细的解释说明。
参见图2，本实施例的方法包括但不限于如下步骤：
S201、根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵。
S202、根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵。
S203、根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差。
S204、判断观测噪声矩阵与第一预设阈值和第二预设阈值的关系，若观测噪声矩阵大于或等于第一预设阈值，则执行S205，若观测噪声矩阵小于或等于第二预设阈值，则执行S206，若观测噪声矩阵大于第二预设阈值且小于第一预设阈值，则执行S207。
其中，第一预设阈值大于第二预设阈值。
在本申请实施例中，由于雷达采集的点云数据与先验地图数据进行迭代匹配(即地图匹配)时，可能出现错误匹配。若出现匹配错误，即观测噪声矩阵Vlidar较大，则不能基于第一位姿信息进行融合处理。因此，需要判断观测噪声矩阵Vlidar与第一预设阈值和第二预设阈值之间的关系。
S205、将第二位姿信息和测量速度信息作为观测信息，将轮速计估计误差作为观测误差。
其中，观测信息包含行驶设备的位姿信息(即位置信息和姿态信息)以及测量速度信息。观测误差是指观测信息所对应的误差信息。
在本申请实施例中，若观测噪声矩阵满足如下公式(23)时(即观测噪声矩阵大于或等于第一预设阈值)，表明通过迭代匹配得到的第一位姿信息不准确，则将根据轮速计采集的脉冲数据确定的第二位姿信息和测量速度信息作为行驶设备的观测信息，并且将轮速计估计误差作为观测误差。
Vlidar≥Vthre1 (23)；
其中，Vlidar为观测噪声矩阵；Vthre1为第一预设阈值。
S206、将第一位姿信息和测量速度信息作为观测信息，将观测噪声矩阵作为观测误差。
在本申请实施例中，若观测噪声矩阵满足如下公式(24)时(即观测噪声矩阵小于或等于第二预设阈值)，表明此时的迭代匹配结果较为正确，则将根据对先验地图数据和激光雷达采集的点云数据进行迭代匹配的结果所确定的第一位姿信息和根据轮速计采集的脉冲数据确定的测量速度信息，作为行驶设备的观测信息；将根据对先验地图数据和激光雷达采集的点云数据进行迭代匹配的结果所确定的观测噪声矩阵，作为行驶设备的观测误差。
Vlidar≤Vthre2 (24)；
其中，Vlidar为观测噪声矩阵；Vthre2为第二预设阈值。
S207、根据观测噪声矩阵和轮速计估计误差，确定观测误差，以及根据第一位姿信息、第二位姿信息、测量速度信息、观测误差、观测噪声矩阵和轮速计估计误差，确定观测信息。
在本申请实施例中，若观测噪声矩阵满足如下公式(25)时(观测噪声矩阵大于第二预设阈值且小于第一预设阈值，其中，第一预设阈值大于第二预设阈值),则需要通过融合处理的方式来确定行驶设备的观测信息和观测误差。
Vthre2＜Vlidar＜Vthre1 (25)；
其中，Vlidar为观测噪声矩阵；Vthre1为第一预设阈值；Vthre2为第二预设阈值。
可选的，可以根据观测噪声矩阵和轮速计估计误差，利用如下公式(26)确定行驶设备的观测误差。
Pg＝(Vlidar -1+P2 -1)-1 (26)；
其中，Pg是观测误差；Vlidar是第一位姿信息对应的观测噪声矩阵；P2表示第二位姿信息对应的轮速计估计误差。
可选的，本步骤根据第一位姿信息、第二位姿信息、测量速度信息、观测误差、观测噪声矩阵和轮速计估计误差确定观测信息的具体过程可以通过以下子步骤实现：
S2071、根据观测误差、观测噪声矩阵和轮速计估计误差，对第一位姿信息和第二位姿信息进行加权计算，得到观测位姿。
在本申请实施例中，可以利用如下公式(27)确定行驶设备的观测位姿。
Xg＝Pg*(Vlidar -1*X1+P2 -1*X2) (27)；
其中，X1表示第一位姿信息；X2表示第二位姿信息；Xg表示观测位姿；P2表示第二位姿信息对应的轮速计估计误差；Vlidar是第一位姿信息对应的观测噪声矩阵；Pg是观测误差。
S2072、根据观测位姿和测量速度信息，确定所述观测信息。
在本申请实施例中，经S2071步骤，得到行驶设备的观测位姿Xg，其中包括位置信息和姿态信息。将轮速计采集的脉冲数据确定的测量速度信息和该观测位姿Xg合并作为行驶设备的观测信息。
S208、根据观测信息、观测误差以及预测误差协方差矩阵，确定当前状态空间向量的误差状态空间向量。
在本申请实施例中，可以将观测信息、观测误差以及预测误差协方差矩阵和观测噪声矩阵，输入误差状态卡尔曼滤波器中，基于误差状态卡尔曼滤波器的工作原理，结合下述公式(28)-(30)即可求得误差状态空间向量。
y＝h(Xt)+Pg (28)；
K＝SHT(HSHT+Pg)-1 (29)；
δX＝K(y-h(Xt))+Pg (30)；
其中，y是观测信息(包含位置信息、姿态信息以及测量速度信息)；Pg是观测误差；h(Xt)是状态量相关的观测方程；K是增益矩阵；S是预测误差协方差矩阵；H是雅可比矩阵的观测方程；δX是误差状态空间向量，其中，δX包含位置误差、速度误差、姿态角度误差、加速度偏差误差以及角速度偏差误差。
S209、根据当前状态空间向量的误差状态空间向量和行驶设备的上一时刻状态空间向量的一般状态空间向量，确定并输出行驶设备的当前状态空间向量的一般状态空间向量。
其中，所谓上一时刻状态空间向量的一般状态空间向量为行驶设备在上一次定位时，确定出的一般状态空间向量(包含位置信息、速度信息、姿态信息、加速度偏差信息以及角速度偏差信息)。本子步骤是基于当前状态空间向量的误差状态空间向量和行驶设备的上一时刻状态空间向量的一般状态空间向量，确定本次定位时的当前状态空间向量的一般状态空间向量。
在本申请实施例中，当前状态空间向量的一般状态空间向量可由公式(31)计算可到：
Figure BDA0002910328460000131
其中，(Pt，vt，qt，ab(t)，wb(t))为当前状态空间向量的一般状态空间向量；(Pt-1，vt-1，qt-1，ab(t-1)，wb(t-1))为上一时刻状态空间向量的一般状态空间向量；δP是位置误差；δv是速度误差；δq是姿态角度误差；δab是加速度偏差误差；δwb是角速度偏差误差。
S210、根据预测误差协方差矩阵和观测噪声矩阵，确定当前误差协方差矩阵。
在本申请实施例中，经上述步骤，得到当前状态空间向量的一般状态空间向量之后，可以通过公式(32)计算当前误差协方差矩阵。公式(32)如下所示：
S＝(I-KH)S上(I-KH)T+KPgKT (32)；
其中，S为当前预测误差协方差矩阵；S上是上一次的当前预测误差协方差矩阵；I是单位矩阵；K是增益矩阵；H是雅可比矩阵的观测方程；Pg是观测误差。
S211、将当前状态空间向量的误差状态空间向量重置为零。
在本申请实施例中，确定了行驶设备的当前状态空间向量和当前误差协方差矩阵之后，需要将当前状态空间向量的误差状态空间向量重置为零。这样设置的好处在于可以用于计算下一时刻行驶设备的定位，使得行驶设备的状态空间向量更加准确。
S212、根据单位矩阵和预测状态空间向量的误差状态空间向量中的姿态角度误差，确定重置矩阵。
在本申请实施例中，根据单位矩阵和预测状态空间向量的误差状态空间向量中的姿态角度误差，通过公式(33)确定重置矩阵。
Figure BDA0002910328460000141
其中，δq是预测状态空间向量的误差状态空间向量中的姿态角度误差；I6是6×6的单位矩阵；I是单位矩阵；G是经公式(33)计算后的重置矩阵。
S213、根据重置矩阵，重置当前误差协方差矩阵。
在本申请实施例中，根据公式(33)得到的重置矩阵，可以通过公式(34)重置当前误差协方差矩阵。
S′＝GSGT (34)；
其中，S是当前误差协方差矩阵；S'是重置后的误差协方差矩阵。
本实施例提供的技术方案，通过判断观测噪声矩阵与第一预设阈值和第二预设阈值之间的关系，细分类型分别得出观测信息和观测误差；根据观测信息、观测误差以及预测误差协方差矩阵，确定误差状态空间向量；根据误差状态空间向量和行驶设备的上一时刻状态空间向量的一般状态空间向量，确定并输出行驶设备的一般状态空间向量；根据预测误差协方差矩阵和观测噪声矩阵，确定当前误差协方差矩阵，并将误差状态空间向量重置为零；根据单位矩阵和误差状态空间向量中的姿态角度误差，重置当前误差协方差矩阵。本申请实施例的方案通过观测信息和观测误差确定行驶设备的当前状态空间向量和当前误差协方差矩阵，并重置误差状态空间向量和当前误差协方差矩阵，通过融合算法得到行驶设备的精准定位数据，进一步提高了行驶设备自主定位的精准性。
实施例三
图3A为本申请实施例三提供的一种行驶设备的定位方法的第三流程示意图；图3B为本申请实施例三提供的一种行驶设备的定位方法的局部地图更新示意图；图3C为本申请实施例二提供的一种行驶设备的定位方法的原理示意图。本申请实施例是在上述实施例的基础上进行优化，具体优化为：增加了对行驶设备进行初始化过程，以及对先验地图的更新过程详细解释说明。
参考图3A，本实施例的方法包括但不限于如下步骤：
S310、根据先验地图数据和激光雷达采集的点云数据，设置行驶设备的初始位姿信息。
其中，所谓初始位姿信息是在行驶设备的初始化阶段，为行驶设备设置的位姿初始值。可选的，初始位姿信息作为对行驶设备进行首次定位时的位姿信息。
可选的，确定行驶设备的初始位姿信息的方式有很多，对此本实施例不进行限定。例如，方式一：将先验地图数据与激光雷达采集的点云数据进行比较，解析点云数据在当前地图中的位置，并将该位置设置为行驶设备的初始位姿信息。方式二：在上述方式一对行驶设备的初始位姿信息进行粗定位的基础上，基于先验地图数据和激光雷达数据，采用迭代匹配算法(如NDT算法)，设置精准的行驶设备的初始位姿信息。需要说明的是，本步骤采用迭代匹配算法，计算行驶设备的初始位姿信息的过程和上述实施例中的使用迭代匹配算法确定行驶设备的第一位姿信息的实现过程类似。具体的迭代匹配过程上述实施例已进行了详细介绍，在此不做叙述。
S320、根据初始位姿信息和惯导参数进行初始化，得到初始滤波器参数。
其中，惯导参数可以是惯导参数手册记录的组合惯导的相关噪声参数值，如可以包括加速度计的白噪声方差σ2an、陀螺仪的白噪声方差σ2wn、速度随机游走方差σ2aw以及角度随机游走方差σ2ww。在本申请实施例中，初始滤波器参数可以包括但不限于：初始定位参数、初始误差协方差矩阵和惯导参数等。具体的，可以将S310设置的行驶设备的初始位姿信息作为误差状态卡尔曼滤波器的初始定位参数；将误差状态卡尔曼滤波器的初始误差协方差矩阵设置为预设数值，优选的，该预设数值可以为数值较大的矩阵。同时还需要根据组合惯导参数手册设置惯导参数，即加速度计的白噪声方差σ2an、陀螺仪的白噪声方差σ2wn、速度随机游走方差σ2aw以及角度随机游走方差σ2ww。
较佳的，在设置行驶设备的初始位姿信息之前，还可以增加行驶设备是否能够具有接收数据功能的判断过程。具体的，如果行驶设备能够接收到所有定位所需数据(即组合惯导采集的状态数据、激光雷达采集的点云数据以及轮速计采集的轮速信息)，则执行上述S310-S320的操作，否则生成检修请求。
S330、根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵。
S340、根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵。
S350、根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差。
S360、根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定行驶设备的当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵。
S370、若监测到行驶设备当前时刻相对于上一次构建局部地图时刻的行驶距离达到预设距离，则根据激光雷达采集的点云数据，构建当前时刻对应的局部地图。
在本申请实施例中，当行驶设备在动态环境下进行作业时，由于部分环境发生变化可能造成雷达地图匹配发生错误。另外，行驶设备在进行作业之前，需要将先验地图数据输入至行驶设备中。当行驶设备由于工作需要可能会进入一些先验地图中没有的场景，此时也无法进行地图匹配。为了解决这些问题，本申请实施例可以在每次定位确定出当前位置信息后，判断其当前时刻相对于上一次构建局部地图时刻的行驶距离是否达到预设距离，若达到预设距离时，需要对先验地图数据进行一次更新。在对先验地图数据进行更新之前，需要通过雷达扫描的点云数据和对应的定位信息，利用滑动窗口维护固定大小的局部地图更新尺寸，即构建一个实时的局部地图，即构建得到的所有局部地图尺寸一致。可选的，预设距离可以是两米。如图3B所示，为每间隔两米对当前环境下的地图数据进行更新。
S380、根据当前时刻对应的局部地图，更新先验地图数据。
在本申请实施例中，根据当前的定位信息和局部地图，对先验地图进行一次更新。具体的，以当前时刻对应的局部地图为标准，对先验地图中与当前时刻对应的局部地图数据不符的地方进行替换，或者对先验地图中没有的区域依据当前时刻对应的局部地图进行增添，得到更新后的先验地图数据。
需要说明的是，本申请实施例将更新后的先验地图数据作为当前环境下的地图数据，在后续步骤对先验地图数据和激光雷达采集的点云数据进行迭代匹配时使用，以实现行驶设备在动态或部分未知环境下的精确定位。
示例性的，如图3C所示，假设行驶设备为移动机器人，该移动机器人上配置有组合惯导、激光雷达和轮速计，先将组合惯导采集的移动机器人的状态数据输入到状态预测模块，该状态预测模块可以基于误差状态卡尔曼滤波器的初始滤波器参数，预测移动机器人的预测状态空间向量及其对应的预测误差协方差矩阵；再将状态预测模块预测的预测状态空间向量的一般状态空间向量中的位姿信息输入到迭代匹配模块，迭代匹配模块会基于该位姿信息对先验地图数据和激光雷达采集的点云数据进行迭代匹配，确定出移动机器人的第一位姿信息和观测噪声矩阵；再将轮速计采集的脉冲数据输入至速度测量模块和航位推算模块，确定行驶设备的第二位姿信息、测量速度信息以及轮速计估计误差；接下来将迭代匹配模块、状态预测模块、速度测量模块以及航位推算模块的输出结果(即第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵)传输至更新融合模块，由更新融合模块计算移动机器人的当前状态空间向量和当前误差协方差矩阵，并将计算结果反馈给状态预测模块，以使状态预测模块重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵，以用于下一次定位时精准确定预测状态空间向量及其对应的预测误差协方差矩阵。将当前状态空间向量中的一般状态空间向量作为状态输出。本申请实施例还可根据更新融合模块计算出的移动机器人当前状态空间向量中的位置信息来判断当前时刻相对于上次构建局部地图时刻，该移动机器人的行驶距离是否达到预设距离，若达到预设距离时，需要通过地图更新模块更新先验地图数据，下一次使用迭代匹配模块时会基于更新后的先验地图与激光雷达采集的点云数据进行迭代匹配。此外，本申请实施例还将更新融合模块计算出的移动机器人当前状态空间向量中当前时刻Z轴方向的测量速度信息作为下一时刻Z轴方向的测量速度信息。
本实施例提供的技术方案，根据先验地图数据和激光雷达采集的点云数据，设置行驶设备的初始位姿信息，并对滤波器参数进行初始化设置，进而基于初始化的初始滤波器参和组合惯导采集的行驶设备的状态数据，确定行驶设备的预测状态空间向量及其对应的预测误差协方差矩阵；当行驶设备行驶距离达到预设距离时，根据激光雷达采集的点云数据构建局部地图，对先验地图建行更新；根据预测状态空间向量的一般状态空间向量中的位姿信息，对更新后的先验地图数据和激光雷达采集的点云数据进行迭代匹配，确定行驶设备的第一位姿信息和观测噪声矩阵；根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息、测量速度信息和确定轮速计估计误差。最后根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定行驶设备的当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵，用于下一次定位。通过对滤波器参数和初始定位参数进行精准的初始化，以及对先验地图数据进行更新，极大的提高了行驶设备自主定位的精准性。
实施例四
图4为本申请实施例提供的一种行驶设备的定位装置的结构示意图，如图4所示，该装置400可以包括：
预测模块410，用于根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，所述预测状态空间向量包括一般状态空间向量和误差状态空间向量。
匹配模块420，用于根据所述预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第一位姿信息和观测噪声矩阵。
第一确定模块430，用于根据轮速计采集的脉冲数据，确定所述行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差。
第二确定模块440，用于根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵、所述轮速计估计误差和所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，并重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵。
进一步的，上述匹配模块420，可以具体用于：将所述预测状态空间向量的一般状态空间向量中的位姿信息从惯导坐标系转换到雷达坐标系，并将转换到所述雷达坐标系下的预测状态空间向量的一般状态空间向量中的位姿信息作为初始位姿矩阵；基于正态分布变换算法和所述初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵。
进一步的，上述匹配模块420，还可以具体用于：将所述迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为所述行驶设备的第一位姿信息；根据所述迭代匹配结果中的最高匹配分数，确定匹配置信度；根据所述匹配置信度和旋转矩阵，确定所述观测噪声矩阵。
进一步的，上述第一确定模块430，可以具体用于：根据轮速计采集的脉冲数据，确定所述行驶设备当前时刻相对于上一时刻的位置增量和当前时刻的轮速信息；根据所述当前时刻的轮速信息，确定所述行驶设备当前时刻相对于上一时刻的角度增量；基于所述行驶设备的运动模型，根据所述行驶设备在上一时刻的位姿信息、所述位置增量以及所述角度增量，确定所述行驶设备在当前时刻的第二位姿信息；根据所述当前时刻的轮速信息和所述角度增量，确定所述行驶设备在当前时刻的测量速度信息；根据所述行驶设备的上一时刻的观测噪声矩阵和预设噪声值，确定所述轮速计估计误差。
进一步的，上述第二确定模块440，可以包括：第一融合单元、第二融合单元和误差确定单元；
所述第一融合单元，用于对所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵以及所述轮速计估计误差进行预处理，得到观测信息和观测误差。
所述第二融合单元，用于根据所述观测信息、所述观测误差以及所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量。
所述误差确定单元，用于根据所述观测误差和所述预测误差协方差矩阵，确定所述当前误差协方差矩阵。
进一步的，上述第一融合单元，可以具体用于：若所述观测噪声矩阵大于或等于第一预设阈值，则将所述第二位姿信息和所述测量速度信息作为所述观测信息，将所述轮速计估计误差作为所述观测误差；若所述观测噪声矩阵小于或等于第二预设阈值，则将所述第一位姿信息和所述测量速度信息作为所述观测信息，将所述观测噪声矩阵作为所述观测误差；若所述观测噪声矩阵大于第二预设阈值且小于第一预设阈值，则根据所述观测噪声矩阵和所述轮速计估计误差，确定所述观测误差，以及根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测误差、所述观测噪声矩阵和所述轮速计估计误差，确定所述观测信息；其中，所述第一预设阈值大于第二预设阈值。
进一步的，上述第一融合单元，还可以具体用于：根据所述观测误差、所述观测噪声矩阵和所述轮速计估计误差，对所述第一位姿信息和所述第二位姿信息进行加权计算，得到观测位姿；根据所述观测位姿和所述测量速度信息，确定所述观测信息。
进一步的，上述第二融合单元，可以具体用于：根据所述观测信息、所述观测误差以及所述预测误差协方差矩阵，确定所述当前状态空间向量的误差状态空间向量；根据所述当前状态空间向量的误差状态空间向量和所述行驶设备的上一时刻状态空间向量的一般状态空间向量，确定并输出所述行驶设备的当前状态空间向量的一般状态空间向量。
进一步的，上述第二确定模块440，还可以具体用于：将所述当前状态空间向量的误差状态空间向量重置为零；根据单位矩阵和所述预测状态空间向量的误差状态空间向量中的姿态角度误差，确定重置矩阵；根据所述重置矩阵，重置所述当前误差协方差矩阵。
进一步的，上述行驶设备的定位装置，还可以包括：地图更新模块；
所述地图更新模块模块，用于若监测到所述行驶设备当前时刻相对于上一次构建局部地图时刻的行驶距离达到预设距离，则根据激光雷达采集的点云数据，构建当前时刻对应的局部地图；根据当前时刻对应的局部地图，更新所述先验地图数据。
进一步的，上述行驶设备的定位装置，还可以包括：初始信息设置模块；
根据所述先验地图数据和所述激光雷达采集的点云数据，设置所述行驶设备的初始位姿信息；根据所述初始位姿信息和惯导参数进行初始化，得到所述初始滤波器参数。
本实施例提供的行驶设备的定位装置可适用于上述任意实施例提供的行驶设备的定位方法，具备相应的功能和有益效果。
实施例五
根据本申请的实施例，本申请还提供了一种电子设备和一种可读存储介质。
如图5所示，是根据本申请实施例的行驶设备的定位方法的电子设备的框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。可选的，本申请实施例的电子设备可以是服务器设备，还可以是行驶设备，若为行驶设备时，可以是智能机器人、无人驾驶车辆等。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本申请的实现。
如图5所示，该电子设备包括：一个或多个处理器510、存储器520，以及用于连接各部件的接口，包括高速接口和低速接口。各个部件利用不同的总线互相连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。处理器可以对在电子设备内执行的指令进行处理，包括存储在存储器中或者存储器上以在外部输入/输出装置(诸如，耦合至接口的显示设备)上显示GUI的图形信息的指令。在其它实施方式中，若需要，可以将多个处理器和/或多条总线与多个存储器和多个存储器一起使用。同样，可以连接多个电子设备，各个设备提供部分必要的操作(例如，作为服务器阵列、一组刀片式服务器、或者多处理器系统)。图5中以一个处理器510为例。
存储器520即为本申请所提供的非瞬时计算机可读存储介质。其中，所述存储器存储有可由至少一个处理器执行的指令，以使所述至少一个处理器执行本申请所提供的行驶设备的定位方法。本申请的非瞬时计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行本申请所提供的行驶设备的定位方法。
存储器520作为一种非瞬时计算机可读存储介质，可用于存储非瞬时软件程序、非瞬时计算机可执行程序以及模块，如本申请实施例中的行驶设备的定位方法对应的程序指令/模块(例如，附图4所示的预测模块410、匹配模块420、第一确定模块430和第二确定模块440)。处理器510通过运行存储在存储器520中的非瞬时软件程序、指令以及模块，从而执行服务器的各种功能应用以及数据处理，即实现上述方法实施例中的行驶设备的定位方法。
存储器520可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据行驶设备的定位的电子设备的使用所创建的数据等。此外，存储器520可以包括高速随机存取存储器，还可以包括非瞬时存储器，例如至少一个磁盘存储器件、闪存器件、或其他非瞬时固态存储器件。在一些实施例中，存储器520可选包括相对于处理器510远程设置的存储器，这些远程存储器可以通过网络连接至行驶设备的定位的电子设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
行驶设备的定位方法的电子设备还可以包括：输入装置530和输出装置540。处理器510、存储器520、输入装置530和输出装置540可以通过总线或者其他方式连接，图5中以通过总线连接为例。
输入装置530可接收输入的数字或字符信息，以及产生与行驶设备的定位的电子设备的用户设置以及功能控制有关的键信号输入，例如触摸屏、小键盘、鼠标、轨迹板、触摸板、指示杆、一个或者多个鼠标按钮、轨迹球、操纵杆等输入装置。输出装置540可以包括显示设备、辅助照明装置(例如，LED)和触觉反馈装置(例如，振动电机)等。该显示设备可以包括但不限于，液晶显示器(LCD)、发光二极管(LED)显示器和等离子体显示器。在一些实施方式中，显示设备可以是触摸屏。
此处描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、专用ASIC(专用集成电路)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置和该至少一个输出装置。
这些计算程序(也称作程序、软件、软件应用、或者代码)包括可编程处理器的机器指令，并且可以利用高级过程和/或面向对象的编程语言、和/或汇编/机器语言来实施这些计算程序。如本文使用的，术语“机器可读介质”和“计算机可读介质”指的是用于将机器指令和/或数据提供给可编程处理器的任何计算机程序产品、设备、和/或装置(例如，磁盘、光盘、存储器、可编程逻辑装置(PLD))，包括，接收作为机器可读信号的机器指令的机器可读介质。术语“机器可读信号”指的是用于将机器指令和/或数据提供给可编程处理器的任何信号。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)、互联网和区块链网络。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，又称为云计算服务器或云主机，是云计算服务体系中的一项主机产品，以解决了传统物理主机与VPS服务中，存在的管理难度大，业务扩展性弱的缺陷。
根据本申请实施例的技术方案，根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，预测状态空间向量包括一般状态空间向量和误差状态空间向量；根据预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第一位姿信息和观测噪声矩阵；根据轮速计采集的脉冲数据，确定行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；根据第一位姿信息、第二位姿信息、测量速度信息、观测噪声矩阵、轮速计估计误差和预测误差协方差矩阵，确定行驶设备的当前状态空间向量和当前误差协方差矩阵，输出当前状态空间向量，并重置当前状态空间向量的误差状态空间向量和当前误差协方差矩阵。本申请方案利用组合惯导和雷达多种传感器之间的不同特点进行优势互补，使用多传感器融合技术进行组合定位，解决了现有技术中使用单一传感器的数据存在不确定性以及可靠性较低的问题。执行本申请方案，能够为行驶设备提供稳定准确的定位信息。
值得注意的是，上述行驶设备的定位装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本申请中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本申请公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本申请保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本申请的精神和原则之内所作的修改、等同替换和改进等，均应包含在本申请保护范围之内。

Claims (13)

1.一种行驶设备的定位方法，其特征在于，所述方法包括:
根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定所述行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，所述预测状态空间向量包括一般状态空间向量和误差状态空间向量；
根据所述预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第一位姿信息和观测噪声矩阵；
根据轮速计采集的脉冲数据，确定所述行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；
根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵、所述轮速计估计误差和所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，并重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵。
2.根据权利要求1所述的方法，其特征在于，所述根据所述预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，包括：
将所述预测状态空间向量的一般状态空间向量中的位姿信息从惯导坐标系转换到雷达坐标系，并将转换到所述雷达坐标系下的预测状态空间向量的一般状态空间向量中的位姿信息作为初始位姿矩阵；
基于正态分布变换算法和所述初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵。
3.根据权利要求2所述的方法，其特征在于，所述根据迭代匹配结果，确定所述行驶设备的第一位姿信息和观测噪声矩阵，包括：
将所述迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为所述行驶设备的第一位姿信息；
根据所述迭代匹配结果中的最高匹配分数，确定匹配置信度；
根据所述匹配置信度和旋转矩阵，确定所述观测噪声矩阵。
4.根据权利要求1所述的方法，其特征在于，所述根据轮速计采集的脉冲数据，确定所述行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差，包括：
根据轮速计采集的脉冲数据，确定所述行驶设备当前时刻相对于上一时刻的位置增量和当前时刻的轮速信息；
根据所述当前时刻的轮速信息，确定所述行驶设备当前时刻相对于上一时刻的角度增量；
基于所述行驶设备的运动模型，根据所述行驶设备在上一时刻的位姿信息、所述位置增量以及所述角度增量，确定所述行驶设备在当前时刻的第二位姿信息；
根据所述当前时刻的轮速信息和所述角度增量，确定所述行驶设备在当前时刻的测量速度信息；
根据所述行驶设备的上一时刻的观测噪声矩阵和预设噪声值，确定所述轮速计估计误差。
5.根据权利要求1所述的方法，其特征在于，所述根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵、所述轮速计估计误差和所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，包括：
对所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵以及所述轮速计估计误差进行预处理，得到观测信息和观测误差；
根据所述观测信息、所述观测误差以及所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量；
根据所述观测误差和所述预测误差协方差矩阵，确定所述当前误差协方差矩阵。
6.根据权利要求5所述的方法，其特征在于，所述对所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵以及所述轮速计估计误差进行预处理，得到观测信息和观测误差，包括：
若所述观测噪声矩阵大于或等于第一预设阈值，则将所述第二位姿信息和所述测量速度信息作为所述观测信息，将所述轮速计估计误差作为所述观测误差；
若所述观测噪声矩阵小于或等于第二预设阈值，则将所述第一位姿信息和所述测量速度信息作为所述观测信息，将所述观测噪声矩阵作为所述观测误差；
若所述观测噪声矩阵大于第二预设阈值且小于第一预设阈值，则根据所述观测噪声矩阵和所述轮速计估计误差，确定所述观测误差，以及根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测误差、所述观测噪声矩阵和所述轮速计估计误差，确定所述观测信息；其中，所述第一预设阈值大于所述第二预设阈值。
7.根据权利要求6所述的方法，其特征在于，所述根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测误差、所述观测噪声矩阵和所述轮速计估计误差，确定所述观测信息，包括：
根据所述观测误差、所述观测噪声矩阵和所述轮速计估计误差，对所述第一位姿信息和所述第二位姿信息进行加权计算，得到观测位姿；
根据所述观测位姿和所述测量速度信息，确定所述观测信息。
8.根据权利要求5所述的方法，其特征在于，所述根据所述观测信息、所述观测误差以及所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量，包括：
根据所述观测信息、所述观测误差以及所述预测误差协方差矩阵，确定所述当前状态空间向量的误差状态空间向量；
根据所述当前状态空间向量的误差状态空间向量和所述行驶设备的上一时刻状态空间向量的一般状态空间向量，确定并输出所述行驶设备的当前状态空间向量的一般状态空间向量。
9.根据权利要求1所述的方法，其特征在于，所述重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵，包括：
将所述当前状态空间向量的误差状态空间向量重置为零；
根据单位矩阵和所述预测状态空间向量的误差状态空间向量中的姿态角度误差，确定重置矩阵；
根据所述重置矩阵，重置所述当前误差协方差矩阵。
10.根据权利要求1-9任一项所述的方法，其特征在于，所述方法还包括：
若监测到所述行驶设备当前时刻相对于上一次构建局部地图时刻的行驶距离达到预设距离，则根据激光雷达采集的点云数据，构建当前时刻对应的局部地图；
根据当前时刻对应的局部地图，更新所述先验地图数据。
11.一种行驶设备的定位装置，其特征在于，所述装置包括:
预测模块，用于根据组合惯导采集的行驶设备的状态数据和初始滤波器参数，确定所述行驶设备的预测状态空间向量和预测误差协方差矩阵；其中，所述预测状态空间向量包括一般状态空间向量和误差状态空间向量；
匹配模块，用于根据所述预测状态空间向量的一般状态空间向量，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第一位姿信息和观测噪声矩阵；
第一确定模块，用于根据轮速计采集的脉冲数据，确定所述行驶设备的第二位姿信息和测量速度信息，并确定轮速计估计误差；
第二确定模块，用于根据所述第一位姿信息、所述第二位姿信息、所述测量速度信息、所述观测噪声矩阵、所述轮速计估计误差和所述预测误差协方差矩阵，确定所述行驶设备的当前状态空间向量和当前误差协方差矩阵，输出所述当前状态空间向量，并重置所述当前状态空间向量的误差状态空间向量和所述当前误差协方差矩阵。
12.一种电子设备，其特征在于，所述电子设备包括：
一个或多个处理器；
存储器，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器能够执行如权利要求1-10中任一项所述的行驶设备的定位方法。
13.一种非易失性可读存储介质，其特征在于，所述非易失性可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使电子设备执行如权利要求1-10中任一项所述的行驶设备的定位方法。
CN202110084188.2A 2021-01-21 2021-01-21 一种行驶设备的定位方法、装置、设备及存储介质 Pending CN112880674A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110084188.2A CN112880674A (zh) 2021-01-21 2021-01-21 一种行驶设备的定位方法、装置、设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110084188.2A CN112880674A (zh) 2021-01-21 2021-01-21 一种行驶设备的定位方法、装置、设备及存储介质

Publications (1)

Publication Number Publication Date
CN112880674A true CN112880674A (zh) 2021-06-01

Family

ID=76051645

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110084188.2A Pending CN112880674A (zh) 2021-01-21 2021-01-21 一种行驶设备的定位方法、装置、设备及存储介质

Country Status (1)

Country Link
CN (1) CN112880674A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113470088A (zh) * 2021-06-24 2021-10-01 上海于万科技有限公司 一种车挂箱状态的判断方法和系统
CN113804194A (zh) * 2021-10-25 2021-12-17 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN115597595A (zh) * 2022-11-28 2023-01-13 上海仙工智能科技有限公司（Cn） 多线激光定位方法及定位装置、计算机设备、存储介质
CN116518986A (zh) * 2023-07-04 2023-08-01 蘑菇车联信息科技有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299557A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd Method and apparatus for updating maps
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置
CN110174104A (zh) * 2019-05-30 2019-08-27 北京邮电大学 一种组合导航方法、装置、电子设备及可读存储介质
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111427060A (zh) * 2020-03-27 2020-07-17 深圳市镭神智能系统有限公司 一种基于激光雷达的二维栅格地图构建方法和系统
CN111649739A (zh) * 2020-06-02 2020-09-11 北京百度网讯科技有限公司 定位方法和装置、自动驾驶车辆、电子设备和存储介质
CN111708047A (zh) * 2020-06-16 2020-09-25 浙江大华技术股份有限公司 机器人定位评估方法、机器人及计算机存储介质
CN111735439A (zh) * 2019-03-22 2020-10-02 北京京东尚科信息技术有限公司 地图构建方法、装置和计算机可读存储介质
WO2020258901A1 (zh) * 2019-06-25 2020-12-30 上海商汤智能科技有限公司 传感器数据处理方法、装置、电子设备及系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299557A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd Method and apparatus for updating maps
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111735439A (zh) * 2019-03-22 2020-10-02 北京京东尚科信息技术有限公司 地图构建方法、装置和计算机可读存储介质
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置
CN110174104A (zh) * 2019-05-30 2019-08-27 北京邮电大学 一种组合导航方法、装置、电子设备及可读存储介质
WO2020258901A1 (zh) * 2019-06-25 2020-12-30 上海商汤智能科技有限公司 传感器数据处理方法、装置、电子设备及系统
CN111427060A (zh) * 2020-03-27 2020-07-17 深圳市镭神智能系统有限公司 一种基于激光雷达的二维栅格地图构建方法和系统
CN111649739A (zh) * 2020-06-02 2020-09-11 北京百度网讯科技有限公司 定位方法和装置、自动驾驶车辆、电子设备和存储介质
CN111708047A (zh) * 2020-06-16 2020-09-25 浙江大华技术股份有限公司 机器人定位评估方法、机器人及计算机存储介质

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113470088A (zh) * 2021-06-24 2021-10-01 上海于万科技有限公司 一种车挂箱状态的判断方法和系统
CN113470088B (zh) * 2021-06-24 2022-09-20 上海于万科技有限公司 一种车挂箱状态的判断方法和系统
CN113804194A (zh) * 2021-10-25 2021-12-17 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN113804194B (zh) * 2021-10-25 2024-03-15 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN115597595A (zh) * 2022-11-28 2023-01-13 上海仙工智能科技有限公司（Cn） 多线激光定位方法及定位装置、计算机设备、存储介质
CN116518986A (zh) * 2023-07-04 2023-08-01 蘑菇车联信息科技有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质
CN116518986B (zh) * 2023-07-04 2023-10-03 蘑菇车联信息科技有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN111429574B (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
CN112880674A (zh) 一种行驶设备的定位方法、装置、设备及存储介质
CN109270545B (zh) 一种定位真值校验方法、装置、设备及存储介质
JP2019215853A (ja) 測位のための方法、測位のための装置、デバイス及びコンピュータ読み取り可能な記憶媒体
CN111666891B (zh) 用于估计障碍物运动状态的方法和装置
CN109522832B (zh) 基于点云片段匹配约束和轨迹漂移优化的回环检测方法
CN112639502A (zh) 机器人位姿估计
CN111310840B (zh) 数据融合处理方法、装置、设备和存储介质
KR102566603B1 (ko) 자율 주행을 위한 속도 계획 방법, 장치, 설비, 매체 및 차량
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
US20210402601A1 (en) Method, apparatus, device, and storage medium for controlling guide robot
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
KR20220153535A (ko) 자율 주행 차량에 대한 데이터 처리 방법 및 데이터 처리 장치, 전자장비, 저장매체, 컴퓨터 프로그램 및 자율 주행 차량
CN117392241B (zh) 自动驾驶中的传感器标定方法、装置及电子设备
CN112902951A (zh) 一种行驶设备的定位方法、装置、设备及存储介质
CN112880664A (zh) 一种行驶设备的定位方法、装置、设备及存储介质
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN116202509A (zh) 一种面向室内多层建筑的可通行地图生成方法
CN115147561A (zh) 位姿图生成方法、高精地图生成方法和装置
Yoshimura et al. Highlighted map for mobile robot localization and its generation based on reinforcement learning
Li et al. Multisensor Fusion SLAM Research Based on Improved RBPF-SLAM Algorithm
WO2022172831A1 (ja) 情報処理装置

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