CN112902951B - 一种行驶设备的定位方法、装置、设备及存储介质 - Google Patents
一种行驶设备的定位方法、装置、设备及存储介质 Download PDFInfo
- Publication number
- CN112902951B CN112902951B CN202110084180.6A CN202110084180A CN112902951B CN 112902951 B CN112902951 B CN 112902951B CN 202110084180 A CN202110084180 A CN 202110084180A CN 112902951 B CN112902951 B CN 112902951B
- Authority
- CN
- China
- Prior art keywords
- pose information
- estimation error
- final
- matching
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 70
- 238000003860 storage Methods 0.000 title claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 35
- 238000007499 fusion processing Methods 0.000 claims description 23
- 230000015654 memory Effects 0.000 claims description 19
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 230000004927 fusion Effects 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 6
- 238000009826 distribution Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 description 15
- 238000004364 calculation method Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 8
- 230000008901 benefit Effects 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 4
- 238000004590 computer program Methods 0.000 description 3
- 230000007774 longterm Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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
-
- 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种行驶设备的定位方法、装置、设备及存储介质。根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和行驶设备的运动模型，确定行驶设备的第一位姿信息；根据第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息和第二位姿信息对应的第二估计误差；确定第一位姿信息对应的第一估计误差；若第二估计误差小于预设阈值，则分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差，并更新第一位姿信息和第一估计误差。执行本申请方案，能够为行驶设备提供稳定准确的定位信息。
Description
技术领域
本发明实施例涉及计算机技术领域，尤其涉及一种行驶设备的定位方法、装置、设备及存储介质。
背景技术
随着人工智能技术的发展，智能化的行驶设备(如智能机器人、自动行驶车辆等)在生产和生活中的使用越来越广泛。在行驶设备运行的过程中，对其进行实时定位是监管行驶设备的关键环节。
目前，现有技术在对行驶设备进行定位时，通常是在环境地图已知的情况下，采用行驶设备上安装的单一传感器采集数据进行行驶设备的定位，存在定位结果准确性低的问题，亟需改进。
发明内容
本申请实施例提供了一种行驶设备的定位方法、装置、设备及存储介质，能够为行驶设备提供稳定准确的定位信息。
第一方面，本申请实施例提供了一种行驶设备的定位方法，该方法包括：
根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和所述行驶设备的运动模型，确定所述行驶设备的第一位姿信息；
根据所述第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第二位姿信息和所述第二位姿信息对应的第二估计误差；
确定所述第一位姿信息对应的第一估计误差；
若所述第二估计误差小于预设阈值，则分别对所述第一估计误差和所述第二估计误差，以及所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息和所述最终位姿信息对应的最终估计误差，并更新所述第一位姿信息和所述第一估计误差。
第二方面，本申请实施例提供了一种行驶设备的定位装置，该装置包括：
第一确定模块，用于根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和所述行驶设备的运动模型，确定所述行驶设备的第一位姿信息；
第二确定模块，用于根据所述第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第二位姿信息和所述第二位姿信息对应的第二估计误差；
第三确定模块，确定所述第一位姿信息对应的第一估计误差；
融合更新模块，用于若所述第二估计误差小于预设阈值，则分别对所述第一估计误差和所述第二估计误差，以及所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息和所述最终位姿信息对应的最终估计误差，并更新所述第一位姿信息和所述第一估计误差。
第三方面，本申请实施例提供了一种电子设备，该电子设备包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本申请任意实施例所述的行驶设备的定位方法。
第四方面，本申请实施例提供了一种非易失性可读存储介质，所述非易失性可读存储介质存储有计算机可执行指令，其中，该计算机可执行指令用于使电子设备执行本申请任意实施例所述的行驶设备的定位方法。
本申请实施例提供了一种行驶设备的定位方法、装置、设备及存储介质，根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和行驶设备的运动模型，确定行驶设备的第一位姿信息；根据第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息和第二位姿信息对应的第二估计误差；确定第一位姿信息对应的第一估计误差；若第二估计误差小于预设阈值，则分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差，并更新第一位姿信息和第一估计误差。本申请实施例的方案基于轮速计、陀螺仪和激光雷达三种传感器采集的数据融合得到行驶设备的最终位姿信息，能够为行驶设备提供稳定准确的定位信息。
应当理解，本部分所描述的内容并非旨在标识本申请的实施例的关键或重要特征，也不用于限制本申请的范围。本申请的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本申请的限定。其中：
图1A为本申请实施例一提供的一种行驶设备的定位方法的第一流程示意图；
图1B为本申请实施例一提供的行驶设备的运动模型示意图；
图1C为本申请实施例一提供的一种行驶设备的定位方法的原理示意图；
图2为本申请实施例二提供的一种行驶设备的定位方法的第二流程示意图；
图3为本申请实施例三提供的一种行驶设备的定位装置的结构示意图；
图4是用来实现本申请实施例的行驶设备的定位方法的电子设备的框图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
为了便于清楚描述本申请实施例的技术方案，在本申请的实施例中，采用了“第一”、“第二”等字样对功能或作用基本相同的相同项或相似项进行区分，本领域技术人员可以理解“第一”、“第二”等字样并不对数量和执行次序进行限定。
实施例一
图1A为本申请实施例一提供的一种行驶设备的定位方法的第一流程示意图；图1B为本申请实施例一提供的行驶设备的运动模型示意图；图1C为本申请实施例一提供的一种行驶设备的定位方法的原理示意图。本实施例可适用于行驶设备运行过程中，对其进行实时定位的情况，尤其适用于在三维地图已知的情况下，基于行驶设备上配置的轮速计、陀螺仪和激光雷达获取的数据，对行驶设备进行定位的情况。本实施例提供的一种行驶设备的定位方法可以由本申请实施例提供的行驶设备的定位装置来执行，该装置可以通过软件和/或硬件的方式实现，并集成在执行本方法的电子设备中。在本实施例中，该电子设备可以是服务器设备，还可以是行驶设备，若为行驶设备时，可以是智能机器人、无人驾驶车辆(如无人叉车)等。
参见图1A-1C，本实施例的方法包括但不限于如下步骤：
S110、根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和行驶设备的运动模型，确定行驶设备的第一位姿信息。
其中，本申请实施例中的行驶设备上配置有轮速计和陀螺仪，该轮速计和陀螺仪在行驶设备运行过程中，实时或定时进行数据采集。轮速计采集的脉冲数据指的是单位时间内脉冲数目。陀螺仪采集的角速度数据指的是陀螺仪的角速度，本申请优选使用陀螺仪在Z轴的角速度。行驶设备的运动模型是基于行驶设备的运动原理为该行驶设备搭建的模型，例如，图1B示出的行驶设备的运动模型，其中的Px和Py分别为行驶设备在水平方向X轴和Y轴的位置坐标，行驶设备包括至少一个驱动轮和至少一个从动轮，通过至少一个驱动轮控制所述行驶设备运动，以至少一个驱动轮的数量等于1为例，α表示行驶设备的驱动轮相对行驶设备的中轴线的偏角。第一位姿信息指的是基于轮速计和陀螺仪采集的数据确定的行驶设备当前的位置信息以及姿态信息，其中，位置信息指的是行驶设备的坐标信息(即X轴、Y轴和Z轴上的位置坐标)；姿态信息指的是行驶设备的旋转量(即X轴、Y轴和Z轴上的旋转量)。
可选的，本步骤根据轮速计和陀螺仪采集的数据，基于行驶设备的运动模型，确定第一位姿信息的具体过程可以通过以下两个子步骤实现：
S1101、根据轮速计采集的脉冲数据和行驶设备的运动模型，确定行驶设备当前时刻相对于上一时刻的位置增量。
轮速计作为轮速传感器的一种，轮速计包括轮速计编码器，轮速计用于测量行驶设备的目标车轮的轮速。具体的，轮速计用于感应行驶设备的目标车轮的转动，输出电压信号，以使行驶设备的处理模块对电压信号进行数据处理，得到对应的脉冲数据，并根据脉冲数据的频率变化测量目标车轮的轮速。在本实施例中，行驶设备还设置有角度传感器，角度传感器安装在驱动轮上，用于采集行驶设备的驱动轮相对行驶设备的中轴线的偏角。
具体的，本申请实施例可以根据轮速计的工作原理，结合行驶设备的运动模型，采用如下公式(1)，根据轮速计采集的脉冲数据，计算行驶设备当前时刻相对于上一时刻的位置增量。
其中，ΔD是行驶设备当前时刻相对于上一时刻(即相邻时间间隔内)的位置增量；Q是行驶设备的驱动轮的车轮半径；Re是行驶设备的轮速计编码器的分辨率；ΔN为轮速计采集的脉冲数据，即单位时间内脉冲数目；α是行驶设备的驱动轮相对行驶设备的中轴线的偏角。
S1102、根据行驶设备在上一时刻的第一位姿信息、位置增量和陀螺仪采集的角速度数据，确定行驶设备在当前时刻的第一位姿信息。
具体的，本申请实施例可以基于S1101确定的行驶设备的位置增量ΔD，再结合行驶设备的运动模型和行驶设备上一时刻的第一位姿信息，采用如下公式(2)-公式(3)计算行驶设备当前时刻在二维水平面上的位姿信息。
Δθ＝ωZ*Δt (3)；
其中，(xt，yt，θt)为行驶设备当前时刻在二维水平面上的位姿信息；(xt-1，yt-1，θt-1)为行驶设备上一时刻在二维水平面上的位姿信息；Δθ为行驶设备当前时刻相对于上一时刻(即相邻时间间隔内)的角度增量；ωZ为陀螺仪在Z轴的角速度；Δt为单位时间间隔。
需要说明的是，通过上述公式(2)-(3)计算出来的是行驶设备当前时刻在二维水平面内的位姿信息，即没有Z轴方向的位姿信息。但是，由于第一位姿信息应当包含X轴、Y轴和Z轴三个方向的位姿信息。所以，本申请实施例需要在上述公式(2)-(3)计算结果的基础上，添加Z轴方向的位姿信息，方可得到行驶设备在当前时刻的第一位姿信息。可选的，由于行驶设备行驶过程中的高度信息变化不大，所以行驶设备当前时刻在Z轴方向的位姿信息可以由该行驶设备上一时刻在Z轴方向的位姿信息来替代，即在上述公式(2)-(3)计算结果的基础上，添加该行驶设备上一时刻在Z轴方向的位姿信息，即可得到行驶设备在当前时刻的第一位姿信息。
需要说明的是，若行驶设备首次进行位姿定位，则将初始化阶段设置的初始位姿信息作为行驶设备的第一位姿信息；若行驶设备非首次(如第二次、第三次等)进行定位，则利用每一次按照本申请实施例的方法计算得到的最终位姿信息更新行驶设备的第一位姿信息，更新后的第一位姿信息作为行驶设备下一次定位时地图配的预测值。其中，初始化阶段配置初始位姿信息的相关内容将在下述实施例中详细解释说明。
S120、根据第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息和第二位姿信息对应的第二估计误差。
其中，本申请实施例的行驶设备上不但配置有轮速计和陀螺仪，还配置有激光雷达，该激光雷达用于在行驶设备行驶过程中，扫描周围环境的点云数据。所谓先验地图数据为已经构建好的高精度的环境地图，该先验地图数据可以预先存储在电子设备(如行驶设备)中。第二位姿信息和第一位姿信息包含的内容相同，即都包含行驶设备当前的位置信息以及姿态信息，但两者的确定方式不同，第二位姿信息是基于先验地图数据和激光雷达采集的点云数据，采用迭代匹配的方式确定。
可选的，本申请实施例采用正态分布变换(Normal Distance Transform，NDT)算法进行迭代匹配。在一些实施例中，还可以采用其他迭代匹配算法，对此本申请实施例不进行限定。本步骤通过迭代匹配确定第二位姿信息和第二估计误差的过程具体可以通过以下子步骤实现：
S1201、将第一位姿信息从轮速计坐标系转换到雷达坐标系，并将转换到雷达坐标系下的第一位姿信息作为初始位姿矩阵。
具体的，本子步骤在对先验地图数据和激光雷达采集的点云数据进行迭代匹配之前，需要将第一位姿信息从轮速计坐标系转换到雷达坐标系。坐标系转换公式(4)如下所示：
其中，FB为轮速计坐标系下的第一位姿信息，包含位置信息和姿态信息；FL为雷达坐标系下的第一位姿信息，包含位置信息和姿态信息；为轮速计坐标系到激光雷达坐标系的转换矩阵。
进一步地，将转换到雷达坐标系下的第一位姿信息FL作为初始位姿矩阵，需要说明的是，该初始位姿矩阵即为首次迭代匹配的预测值(即初值)。
S1202、基于正态分布变换算法和初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及匹配分数关联的目标位姿矩阵。
其中，本申请实施例将每次对先验地图数据和激光雷达采集的点云数据中的各位置点进行匹配后得到的各位置点的概率密度之和作为本次迭代匹配的匹配分数。
在本申请的具体实施例中，将初始位姿矩阵带入下述概率密度之和计算公式(5)，基于激光雷达采集的点云数据和先验地图数据，计算先验地图数据和点云数据的匹配情况，本实施例需要进行预设次数的迭代匹配计算，在多次迭代匹配的过程中，如果位姿矩阵能使得点云数据与先验地图数据匹配的很好，那么基于公式(5)计算的匹配分数将会很大。
具体的，假设激光雷达采集的点云数据为A＝{a0，...，an}，其中，A为包含n个点的三维点云数据；先验地图数据为B＝{b0，...，bn}，其中，B为包含n个点的位置坐标。首先，利用NDT算法将先验地图数据B构建成多维变量的正态分布。其次，通过概率密度之和计算公式，基于初始位姿矩阵(即迭代匹配的预测值)和预先设置的迭代次数和迭代步长，对先验地图数据和点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的迭代匹配结果，即匹配分数以及匹配分数关联的目标位姿矩阵。可选的，NDT算法可以采用牛顿优化算法、最小二乘法等对先验地图数据和点云数据进行预设次数的迭代匹配。匹配分数的计算方式如下：
ai′＝T(ai,FL) (8)；
其中，score为匹配分数，即概率密度之和；ai'为点云数据中第i个点云在地图坐标系下的坐标值；d为先验地图数据的均值；n为点云数据中的点云数量(即先验地图数据中的位置点数量)；∑为先验地图数据的方差；bi为先验地图数据中第i个点的坐标值；ai为在激光雷达坐标系下的点云数据中第i个点云的坐标值；FL为当前迭代匹配的目标位姿矩阵(即下一次迭代匹配的预测值)，其中，首次迭代匹配时FL为初始位姿矩阵；T为雷达坐标系到地图坐标系的转换算子。
S1203、将迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为行驶设备的第二位姿信息。
具体的，上述步骤每进行一次迭代匹配，得到一组匹配分数score以及该匹配分数score关联的目标位姿矩阵FL。本子步骤从多次匹配迭代得到的匹配分数score中，确定最高的匹配分数score。此时，先验地图数据和点云数据的匹配情况是最好的，并将最高匹配分数score所对应的目标位姿矩阵FL作为第二位姿信息。
S1204、根据迭代匹配结果中的最高匹配分数，确定匹配置信度。
在本申请的具体实施例中，由于雷达采集的点云数据与先验地图数据进行迭代匹配时，可能会存在一定的匹配误差，该匹配误差主要用于对第一位姿信息和第二位姿信息进行数据融合。在确定迭代匹配得到的第二位姿信息对应的第二估计误差时，需要先基于下述公式(9)计算匹配置信度，其中，该匹配置信度代表当前时刻采用激光雷达进行定位的可信度。
Pbelief＝1/score高×I6×6 (9)；
其中，Pbelief表示匹配置信度；I6×6表示6×6的单位矩阵；score高为上述公式(5)进行预设次数的迭代匹配得到的匹配分数中的最高匹配分数。
S1205、根据匹配置信度和旋转矩阵，确定第二位姿信息对应的第二估计误差。
在本申请实施例中，可基于下述公式(10)-公式(11)计算第二位姿信息的第二估计误差。
其中，R3×3表示旋转矩阵；Rcur表示第二位姿信息的旋转变换矩阵；P2第二位姿信息对应的第二估计误差。
S130、确定第一位姿信息对应的第一估计误差。
在本申请实施例中，第一位姿信息对应的第一估计误差的计算方式可以是：当行驶设备是第一次定位时，第一估计误差P1可以为第一次地图匹配时，基于上述公式(11)计算出的第二估计误差P2，即P1＝P2；当行驶设备不是第一次定位(如第二次、第三次等)时，可在第二估计误差P2的基础上依次迭代白噪声，得到本次定位的第一估计误差P1。如当行驶设备第二次定位时，可在第二估计误差P2的基础上迭代第二次定位时对应的白噪声，得到本次定位的第一估计误差P1；如当行驶设备第三次定位时，可在第二估计误差P2的基础上迭代第二次定位与第三次定位时对应的白噪声，得到本次定位的第一估计误差P1。
S140、若第二估计误差小于预设阈值，则分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差，并更新第一位姿信息和第一估计误差。
可选的，在本申请实施例中，第一位姿信息是通过利用轮速计和陀螺仪传感器而得到的，使用第一位姿信息进行定位的方法虽然成本较低，但是由于仅使用轮速计和陀螺仪确定行驶设备的第一位姿信息的定位方法，会随着时间的累积导致行驶设备的定位误差逐步增大，存在可靠性较低的问题。第二位姿信息是通过利用雷达传感器而得到的，使用第二位姿信息进行定位的方法虽然精度高，但是会存在不确定性。因此，本实施例可以利用轮速计、陀螺仪和雷达三种传感器之间的不同特点进行优势互补，即将第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息。
可选的，在本申请实施例中，由于雷达采集的点云数据与先验地图数据进行迭代匹配(即地图匹配)时，可能出现错误匹配。若出现匹配错误，即第二估计误差P2较大，则不能基于第二位姿信息进行融合处理。因此，需要判断第二估计误差P2是否小于预设阈值。判断第二估计误差P2和与预设阈值之间的关系如公式(12)所示：
P2＜Vthre1 (12)；
其中，Vthre1为预设阈值；P2为第二估计误差。
在本申请实施例中，若第二估计误差小于预设阈值(即满足上述公式(12))，表示此时的迭代匹配结果正确，可以分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差。具体可以通过以下四个子步骤实现：
S1401、对第一估计误差和第二估计误差进行融合处理，得到最终位姿信息对应的最终估计误差。
在本申请实施例中，可以基于下述公式(13)，根据第一估计误差和第二估计误差，确定最终估计误差。
Pg＝(P1 -1+P2 -1)-1 (13)；
其中，P1为第一位姿信息对应的第一估计误差；P2为第二位姿信息对应的第二估计误差；Pg表示最终位姿信息对应的最终估计误差。
S1402、基于第一估计误差、第二估计误差以及最终估计误差，对第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息。
在本申请实施例中，可以基于下述公式(14)，根据第一估计误差、第二估计误差以及最终估计误差，对第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息。
Xg＝Pg*(P1 -1*X1+P2 -1*X2) (14)；
其中，X1为第一位姿信息；X2为第二位姿信息；Xg为最终位姿信息；P1为第一位姿信息对应的第一估计误差；P2为第二位姿信息对应的第二估计误差；Pg表示最终位姿信息对应的最终估计误差。
S1403、根据最终位姿信息，更新第一位姿信息。
在本申请实施例中，根据最终位姿信息Xg，更新第一位姿信息X1。这样设置使得行驶设备通过轮速计和陀螺仪获得的第一位姿信息X1更加准确，进而使得第一位姿信息X1与第二位姿信息X2进行融合之后所得到的最终位姿信息Xg更加精准。更新第一位姿信息X1的计算公式(15)如下所示：
X1'＝Xg (15)；
其中，X1'为更新后的第一位姿信息；Xg为行驶设备本次定位的终位姿信息。
S1404、根据第一估计误差、第二估计误差以及最终估计误差，更新第一估计误差。
在本申请实施例中，根据第一估计误差P1、第二估计误差P2和最终估计误差Pg，更新第一估计误差P1。这样设置的好处在于可以一直更新第一估计误差P1，以便后续使用更新后的第一估计误差P1计算最终估计误差Pg，使得行驶设备获得的定位信息更加精准。具体的，可以先基于下述公式(16)计算第一估计误差P1更新的权重系数β1，再根据第一估计误差P1更新的权重系数β1和基于公式(13)计算得到的最终估计误差Pg对第一估计误差P1进行更新，得到更新后的第一估计误差值P1′。
P1′＝(β1 -1Pg) (17)；
其中，P1为第一估计误差；P2为第二估计误差；Pg为最终估计误差；β1表示权重系数；tr(·)表示矩阵迹运算；P1'为更新后的第一估计误差值。
示例性的，如图1C所示，假设行驶设备为无人叉车，该无人叉车上配置有轮速计、陀螺仪和激光雷达，先根据轮速计采集的脉冲数据和陀螺仪采集的角速度数据，结合图1B所示的无人叉车的运动模型，采用位姿预测模块(如可以是卡尔曼滤波器)预测无人叉车的第一位姿信息；接下来位姿预测模块将第一位姿信息输入到迭代匹配模块，迭代匹配模块会基于该第一位姿信息对先验地图数据和激光雷达采集的点云数据进行迭代匹配，确定出无人叉车的第二位姿信息和第二位姿信息对应的第二估计误差；并根据迭代匹配得到的第二估计误差，确定第一位姿信息对应的第一估计误差，最后将第一估计误差、第二估计误差、第一位姿信息和第二位姿信息输入到估计融合模块，估计融合模块会对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，确定无人叉车的最终位姿信息和最终位姿信息对应的最终估计误差。估计融合模块在输出最终位姿信息的同时，还会将最终位姿信息反馈给位姿预测模块，以使位姿预测模块基于最终位姿信息更新第一位姿信息，以用于下一次定位时精准预测第一位姿信息。
本实施例提供的技术方案，通过利用轮速计和陀螺仪传感器而得到的第一位姿信息；根据该第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息和第二位姿信息对应的第二估计误差；确定第一位姿信息对应的第一估计误差；进而基于迭代匹配结果与预设阈值的比较结果，若第二估计误差小于预设阈值，则分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差，并更新第一位姿信息和第一估计误差。本申请通过将利用轮速计和陀螺仪传感器而得到的第一位姿信息和利用雷达传感器而得到的第二位姿信息进行融合处理，解决了现有技术中使用单一传感器的数据存在不确定性以及可靠性较低的问题。执行本申请方案，可以利用轮速计、陀螺仪和雷达三种传感器之间的不同特点进行优势互补，能够获得长期、稳定、精确的高频定位输出，具有很强的工程应用价值。
实施例二
图2为本申请实施例二提供的一种行驶设备的定位方法的第二流程示意图。本申请实施例是在上述实施例的基础上进行优化，具体优化为：增加了确定行驶设备初始位姿信息的过程以及第二估计误差大于或等于预设阈值的情况进行详细的解释说明。
参见图2，本实施例的方法包括但不限于如下步骤：
S210、根据先验地图数据和激光雷达采集的点云数据，设置行驶设备的初始位姿信息。
其中，初始位姿信息是指在行驶设备的初始化阶段为行驶设备设置的位姿初始值，作为对行驶设备进行首次定位时的位姿信息。
可选的，确定行驶设备的初始位姿信息的方式有很多，对此本实施例不进行限定。例如，方式一、将先验地图数据与激光雷达采集的点云数据进行比较，解析点云数据在当前地图中的位置，并将该位置设置为行驶设备的初始位姿信息。方式二：在上述方式一对行驶设备的初始位姿信息进行粗定位的基础上，基于先验地图数据和激光雷达数据，采用迭代匹配算法(如NDT算法)，设置精准的行驶设备的初始位姿信息。需要说明的是，本步骤采用迭代匹配算法，计算行驶设备的初始位姿信息的过程和上述实施例中的使用迭代匹配算法确定行驶设备的第二位姿信息的实现过程类似，具体的迭代匹配过程上述实施例已进行了详细介绍，在此不做叙述。
较佳的，在设置行驶设备的初始位姿信息之前，还可以增加行驶设备是否能够具有接收数据功能的判断过程。具体的，如果行驶设备能够接收到所有定位所需数据(即轮速计采集的脉冲数据、陀螺仪采集的角速度数据以及激光雷达采集的点云数据)，则对行驶设备进行初始位姿信息设置，否则生成检修请求。
S220、根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和行驶设备的运动模型，确定行驶设备的第一位姿信息。
S230、根据第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息和第二位姿信息对应的第二估计误差。
S240、确定第一位姿信息对应的第一估计误差。
S250、判断第二估计误差是否小于预设阈值，若否，则执行S260，若是，则执行S270。
S260、若第二估计误差大于或等于预设阈值，则将第一位姿信息作为行驶设备的最终位姿信息，将第一估计误差作为最终位姿信息对应的最终估计误差。
具体的，由于雷达采集的点云数据与先验地图数据进行迭代匹配(即地图匹配)时，可能出现错误匹配。若出现匹配错误，即第二估计误差P2较大，则不能基于第二位姿信息进行融合处理。因此，需要判断第二估计误差P2是否大于或等于预设阈值。判断第二估计误差P2和与预设阈值之间的关系如公式(18)所示：
P2≥Vthre1 (18)；
其中，Vthre1为预设阈值。
在本申请实施例中，若第二估计误差P2大于或等于预设阈值Vthre1，则说明通过迭代匹配得到的第二位姿信息不准确，此时可以直接将第一位姿信息作为行驶设备的最终位姿信息，将第一估计误差作为最终位姿信息对应的最终估计误差。
S270、若第二估计误差小于预设阈值，则分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差。
S280、更新第一位姿信息和第一估计误差。
本实施例提供的技术方案，在获取行驶设备的定位信息之前，根据激光雷达采集的点云数据设置行驶设备的初始位姿信息；通过利用轮速计和陀螺仪传感器而得到的第一位姿信息；根据第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息；对第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息。本申请在初始化阶段设置行驶设备的初始位姿信息提供了多种实现方式，提高了行驶设备初始化过程的灵活性和准确性。后续利用轮速计、陀螺仪和雷达三种传感器之间的不同特点进行优势互补，对初始化后的行驶设进行定位，能够获得长期、稳定、精确的高频定位输出，具有很强的工程应用价值。
应该理解，上述各个方法实施例所示的各种形式的流程可以进行重新排序、增加或删除步骤。例如，本申请中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本申请公开的技术方案所期望的结果，本文在此不进行限制。
实施例三
图3为本申请实施例提供的一种行驶设备的定位装置的结构示意图，如图3所示，该装置300可以包括：
第一确定模块310，用于根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和所述行驶设备的运动模型，确定所述行驶设备的第一位姿信息。
第二确定模块320，用于根据所述第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第二位姿信息和所述第二位姿信息对应的第二估计误差。
第三确定模块330，确定所述第一位姿信息对应的第一估计误差。
融合更新模块340，用于若所述第二估计误差小于预设阈值，则分别对所述第一估计误差和所述第二估计误差，以及所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息和所述最终位姿信息对应的最终估计误差，并更新所述第一位姿信息和所述第一估计误差。
进一步的，上述第一确定模块310，可以具体用于：根据轮速计采集的脉冲数据和所述行驶设备的运动模型，确定所述行驶设备当前时刻相对于上一时刻的位置增量；根据所述行驶设备在上一时刻的第一位姿信息、所述位置增量和陀螺仪采集的角速度数据，确定所述行驶设备在当前时刻的第一位姿信息。
进一步的，上述第二确定模块320，可以具体用于：将所述第一位姿信息从轮速计坐标系转换到雷达坐标系，并将转换到所述雷达坐标系下的第一位姿信息作为初始位姿矩阵；基于正态分布变换算法和所述初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵。
进一步的，上述第二确定模块320，还可以具体用于：将所述迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为所述行驶设备的第二位姿信息；根据所述迭代匹配结果中的最高匹配分数，确定匹配置信度；根据所述匹配置信度和旋转矩阵，确定所述第二位姿信息对应的第二估计误差。
进一步的，上述融合更新模块340，可以具体用于：对所述第一估计误差和所述第二估计误差进行融合处理，得到所述最终位姿信息对应的最终估计误差；基于所述第一估计误差、所述第二估计误差以及所述最终估计误差，对所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息。
进一步的，上述融合更新模块340，还可以具体用于：根据所述最终位姿信息，更新所述第一位姿信息；
根据所述第一估计误差、所述第二估计误差以及所述最终估计误差，更新所述第一估计误差。
进一步的，上述行驶设备的定位装置，还可以包括：初始信息确定模块；
初始信息确定模块，用于根据所述先验地图数据和所述激光雷达采集的点云数据，设置所述行驶设备的初始位姿信息；
融合更新模块340还用于：若所述第二估计误差大于或等于所述预设阈值，则将所述第一位姿信息作为所述行驶设备的最终位姿信息，将所述第一估计误差作为所述最终位姿信息对应的最终估计误差。
本实施例提供的行驶设备的定位装置可适用于上述任意实施例提供的行驶设备的定位方法，具备相应的功能和有益效果。
值得注意的是，上述行驶设备的定位装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。
实施例四
根据本申请的实施例，本申请还提供了一种电子设备和一种可读存储介质。
如图4所示，是根据本申请实施例的行驶设备的定位方法的电子设备的框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本申请的实现。可选的，若该电子设备为服务器设备，则服务器可以获取行驶设备上的各个传感器(如轮速计、陀螺仪和激光雷达)采集的数据执行本申请实施例的行驶设备的定位方法。若该电子设备为行驶设备，如智能机器人、无人驾驶车辆等，则该行驶设备处理中处理包括下述内容介绍的器件(如处理器410和存储器420等)外，还包括：用于采集脉冲数据的轮速计；用于采集角速度数据的陀螺仪；以及用于采集所述行驶设备周围环境的点云数据的激光雷达。
如图4所示，该电子设备包括：一个或多个处理器410、存储器420，以及用于连接各部件的接口，包括高速接口和低速接口。各个部件利用不同的总线互相连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。处理器410可以对在电子设备内执行的指令进行处理，包括存储在存储器420中或者存储器420上以在外部输入/输出装置(诸如，耦合至接口的显示设备)上显示GUI的图形信息的指令。在其它实施方式中，若需要，可以将多个处理器410和/或多条总线与多个存储器420和多个存储器420一起使用。同样，可以连接多个电子设备，各个设备提供部分必要的操作(例如，作为服务器阵列、一组刀片式服务器、或者多处理器系统)。图4中以一个处理器410为例。
其中，所述存储器420存储有可由至少一个处理器执行的指令，以使所述至少一个处理器410执行本申请所提供的行驶设备的定位方法。本申请的非瞬时计算机可读存储介质存储计算机指令，该计算机指令用于使电子设备执行本申请所提供的行驶设备的定位方法。
存储器420作为一种非瞬时计算机可读存储介质，可用于存储非瞬时软件程序、非瞬时计算机可执行程序以及模块，如本申请实施例中的行驶设备的定位方法对应的程序指令/模块(例如，附图3所示的第一确定模块310、第二确定模块320、第一确定模块330和融合更新模块340)。处理器410通过运行存储在存储器420中的非瞬时软件程序、指令以及模块，从而执行电子设备的各种功能应用以及数据处理，即实现上述方法实施例中的行驶设备的定位方法。
存储器420可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据行驶设备的定位的电子设备的使用所创建的数据等。此外，存储器420可以包括高速随机存取存储器，还可以包括非瞬时存储器，例如至少一个磁盘存储器件、闪存器件、或其他非瞬时固态存储器件。在一些实施例中，存储器420可选包括相对于处理器410远程设置的存储器，这些远程存储器可以通过网络连接至执行行驶设备的定位方法的电子设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
行驶设备的定位方法的电子设备还可以包括：输入装置430和输出装置440。处理器410、存储器420、输入装置430和输出装置440可以通过总线或者其他方式连接，图4中以通过总线连接为例。
输入装置430可接收输入的数字或字符信息，以及产生与行驶设备的定位的电子设备的用户设置以及功能控制有关的键信号输入，例如触摸屏、小键盘、鼠标、轨迹板、触摸板、指示杆、一个或者多个鼠标按钮、轨迹球、操纵杆等输入装置。输出装置440可以包括显示设备、辅助照明装置(例如，LED)和触觉反馈装置(例如，振动电机)等。该显示设备可以包括但不限于，液晶显示器(LCD)、发光二极管(LED)显示器和等离子体显示器。在一些实施方式中，显示设备可以是触摸屏。
此处描述的方法和装置的各种实施方式可以在数字电子电路系统、集成电路系统、专用ASIC(专用集成电路)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以通过一个或者多个计算机程序实施，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储器420、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储器420、该至少一个输入装置和该至少一个输出装置。
这些计算程序(也称作程序、软件、软件应用、或者代码)包括可编程处理器的机器指令，并且可以利用高级过程和/或面向对象的编程语言、和/或汇编/机器语言来实施这些计算程序。如本文使用的，术语“机器可读介质”和“计算机可读介质”指的是用于将机器指令和/或数据提供给可编程处理器的任何计算机程序产品、设备、和/或装置(例如，磁盘、光盘、存储器、可编程逻辑装置(PLD))，包括，接收作为机器可读信号的机器指令的机器可读介质。术语“机器可读信号”指的是用于将机器指令和/或数据提供给可编程处理器的任何信号。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)、互联网和区块链网络。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，又称为云计算服务器或云主机，是云计算服务体系中的一项主机产品，以解决了传统物理主机与VPS服务中，存在的管理难度大，业务扩展性弱的缺陷。
根据本申请实施例的技术方案，通过利用轮速计和陀螺仪传感器而得到的第一位姿信息；根据该第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定行驶设备的第二位姿信息和第二位姿信息对应的第二估计误差；确定第一位姿信息对应的第一估计误差；进而基于迭代匹配结果与预设阈值的比较结果，若第二估计误差小于预设阈值，则分别对第一估计误差和第二估计误差，以及第一位姿信息和第二位姿信息进行融合处理，得到行驶设备的最终位姿信息和最终位姿信息对应的最终估计误差，并更新第一位姿信息和第一估计误差。本申请通过将利用轮速计和陀螺仪传感器而得到的第一位姿信息和利用雷达传感器而得到的第二位姿信息进行融合处理，解决了现有技术中使用单一传感器的数据存在不确定性以及可靠性较低的问题。执行本申请方案，可以利用轮速计、陀螺仪和雷达三种传感器之间的不同特点进行优势互补，能够获得长期、稳定、精确的高频定位输出，具有很强的工程应用价值。
上述具体实施方式，并不构成对本申请保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本申请的精神和原则之内所作的修改、等同替换和改进等，均应包含在本申请保护范围之内。
Claims (9)
1.一种行驶设备的定位方法，其特征在于，所述方法包括：
根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和所述行驶设备的运动模型，确定所述行驶设备的第一位姿信息；
根据所述第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第二位姿信息和所述第二位姿信息对应的第二估计误差，其中，迭代匹配结果包括每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵；
确定所述第一位姿信息对应的第一估计误差；
若所述第二估计误差小于预设阈值，则分别对所述第一估计误差和所述第二估计误差进行融合处理，并对所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息和所述最终位姿信息对应的最终估计误差，并更新所述第一位姿信息和所述第一估计误差；
所述根据迭代匹配结果，确定所述行驶设备的第二位姿信息和所述第二位姿信息对应的第二估计误差，包括：
将所述迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为所述行驶设备的第二位姿信息；
根据所述迭代匹配结果中的最高匹配分数，确定匹配置信度；
根据所述匹配置信度和旋转矩阵，确定所述第二位姿信息对应的第二估计误差。
2.根据权利要求1所述的方法，其特征在于，所述根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和所述行驶设备的运动模型，确定所述行驶设备的第一位姿信息，包括：
根据轮速计采集的脉冲数据和所述行驶设备的运动模型，确定所述行驶设备当前时刻相对于上一时刻的位置增量；
根据所述行驶设备在上一时刻的第一位姿信息、所述位置增量和陀螺仪采集的角速度数据，确定所述行驶设备在当前时刻的第一位姿信息。
3.根据权利要求1所述的方法，其特征在于，所述根据所述第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，包括：
将所述第一位姿信息从轮速计坐标系转换到雷达坐标系，并将转换到所述雷达坐标系下的第一位姿信息作为初始位姿矩阵；
基于正态分布变换算法和所述初始位姿矩阵，对先验地图数据和激光雷达采集的点云数据进行预设次数的迭代匹配，得到每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵。
4.根据权利要求1所述的方法，其特征在于，所述分别对所述第一估计误差和所述第二估计误差，以及所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息和所述最终位姿信息对应的最终估计误差，包括：
对所述第一估计误差和所述第二估计误差进行融合处理，得到所述最终位姿信息对应的最终估计误差；
基于所述第一估计误差、所述第二估计误差以及所述最终估计误差，对所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息。
5.根据权利要求4所述的方法，其特征在于，所述更新所述第一位姿信息和所述第一估计误差，包括：
根据所述最终位姿信息，更新所述第一位姿信息；
根据所述第一估计误差、所述第二估计误差以及所述最终估计误差，更新所述第一估计误差。
6.根据权利要求1-5任一项所述的方法，其特征在于，所述方法还包括：
根据所述先验地图数据和所述激光雷达采集的点云数据，设置所述行驶设备的初始位姿信息；和
若所述第二估计误差大于或等于所述预设阈值，则将所述第一位姿信息作为所述行驶设备的最终位姿信息，将所述第一估计误差作为所述最终位姿信息对应的最终估计误差。
7.一种行驶设备的定位装置，其特征在于，所述装置包括:
第一确定模块，用于根据轮速计采集的脉冲数据、陀螺仪采集的角速度数据和所述行驶设备的运动模型，确定所述行驶设备的第一位姿信息；
第二确定模块，用于根据所述第一位姿信息，对先验地图数据和激光雷达采集的点云数据进行迭代匹配，并根据迭代匹配结果，确定所述行驶设备的第二位姿信息和所述第二位姿信息对应的第二估计误差，其中，迭代匹配结果包括每次迭代匹配对应的匹配分数，以及所述匹配分数关联的目标位姿矩阵；
第三确定模块，用于基于所述第二估计误差，确定所述第一位姿信息对应的第一估计误差；
融合更新模块，用于若所述第二估计误差小于预设阈值，则分别对所述第一估计误差和所述第二估计误差进行融合处理，并对所述第一位姿信息和所述第二位姿信息进行融合处理，得到所述行驶设备的最终位姿信息和所述最终位姿信息对应的最终估计误差，并更新所述第一位姿信息和所述第一估计误差；
第二确定模块，被配置为具体用于：
将所述迭代匹配结果中的最高匹配分数关联的目标位姿矩阵作为所述行驶设备的第二位姿信息；
根据所述迭代匹配结果中的最高匹配分数，确定匹配置信度；
根据所述匹配置信度和旋转矩阵，确定所述第二位姿信息对应的第二估计误差。
8.一种电子设备，其特征在于，所述电子设备包括：
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器能够执行如权利要求1-6中任一项所述的行驶设备的定位方法。
9.一种非易失性可读存储介质，其特征在于，所述非易失性可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使电子设备执行如权利要求1-6任一项所述的行驶设备的定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110084180.6A CN112902951B (zh) | 2021-01-21 | 2021-01-21 | 一种行驶设备的定位方法、装置、设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110084180.6A CN112902951B (zh) | 2021-01-21 | 2021-01-21 | 一种行驶设备的定位方法、装置、设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112902951A CN112902951A (zh) | 2021-06-04 |
| CN112902951B true CN112902951B (zh) | 2024-07-26 |
Family
ID=76118598
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110084180.6A Active CN112902951B (zh) | 2021-01-21 | 2021-01-21 | 一种行驶设备的定位方法、装置、设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112902951B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113739819B (zh) * | 2021-08-05 | 2024-04-16 | 上海高仙自动化科技发展有限公司 | 校验方法、装置、电子设备、存储介质及芯片 |
| CN113838129B (zh) * | 2021-08-12 | 2024-03-15 | 高德软件有限公司 | 一种获得位姿信息的方法、装置以及系统 |
| CN115388886A (zh) * | 2022-08-30 | 2022-11-25 | 北京四维万兴科技有限公司 | 一种车辆定位方法、装置及设备 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110307836A (zh) * | 2019-07-10 | 2019-10-08 | 北京智行者科技有限公司 | 一种用于无人清扫车辆贴边清扫的精确定位方法 |
| CN110967011A (zh) * | 2019-12-25 | 2020-04-07 | 苏州智加科技有限公司 | 一种定位方法、装置、设备及存储介质 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2019040167A (ja) * | 2017-08-23 | 2019-03-14 | 安田 知弘 | カラオケ装置およびその制御方法 |
| CN109870157B (zh) * | 2019-02-20 | 2021-11-02 | 苏州风图智能科技有限公司 | 确定车体位姿的方法及装置、制图方法 |
| CN112214009B (zh) * | 2019-06-25 | 2022-07-26 | 上海商汤临港智能科技有限公司 | 传感器数据处理方法、装置、电子设备及系统 |
| CN111102978B (zh) * | 2019-12-05 | 2022-03-29 | 深兰科技（上海）有限公司 | 一种车辆运动状态确定的方法、装置及电子设备 |
| CN116106927A (zh) * | 2020-03-27 | 2023-05-12 | 深圳市镭神智能系统有限公司 | 一种基于激光雷达的二维栅格地图构建方法、介质和系统 |
| CN111649739B (zh) * | 2020-06-02 | 2023-09-01 | 阿波罗智能技术(北京)有限公司 | 定位方法和装置、自动驾驶车辆、电子设备和存储介质 |
-
2021
- 2021-01-21 CN CN202110084180.6A patent/CN112902951B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110307836A (zh) * | 2019-07-10 | 2019-10-08 | 北京智行者科技有限公司 | 一种用于无人清扫车辆贴边清扫的精确定位方法 |
| CN110967011A (zh) * | 2019-12-25 | 2020-04-07 | 苏州智加科技有限公司 | 一种定位方法、装置、设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112902951A (zh) | 2021-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112902951B (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
| KR102498439B1 (ko) | 차량용 카메라의 외부 파라미터 캘리브레이션 방법, 장치, 시스템 및 저장매체 | |
| US11698262B2 (en) | Method and apparatus for generating route planning model, and storage medium | |
| CN111666891B (zh) | 用于估计障碍物运动状态的方法和装置 | |
| CN112880674A (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
| CN111310840B (zh) | 数据融合处理方法、装置、设备和存储介质 | |
| CN111368760B (zh) | 一种障碍物检测方法、装置、电子设备及存储介质 | |
| CN110879395B (zh) | 障碍物位置预测方法、装置和电子设备 | |
| CN111563450B (zh) | 数据处理方法、装置、设备及存储介质 | |
| CN113934205B (zh) | 用于控制引导机器人的方法、装置、设备以及存储介质 | |
| KR20210052409A (ko) | 차선의 확정방법, 포지셔닝 정밀도의 평가방법, 장치, 기기 및 컴퓨터 프로그램 | |
| KR102566603B1 (ko) | 자율 주행을 위한 속도 계획 방법, 장치, 설비, 매체 및 차량 | |
| CN111079079B (zh) | 数据修正方法、装置、电子设备及计算机可读存储介质 | |
| KR102568948B1 (ko) | 장애물 속도를 결정하는 방법, 장치, 전자 기기, 저장 매체 및 프로그램 | |
| CN111693059B (zh) | 环岛的导航方法、装置、设备及存储介质 | |
| CN111652113A (zh) | 障碍物检测方法、装置、设备以及存储介质 | |
| CN111708033A (zh) | 坐标系标定方法、装置、电子设备及存储介质 | |
| CN111814651A (zh) | 车道线的生成方法、装置和设备 | |
| CN114528941A (zh) | 传感器数据融合方法、装置、电子设备及存储介质 | |
| CN113177980B (zh) | 用于自动驾驶的目标对象速度确定方法、装置及电子设备 | |
| CN111260722B (zh) | 车辆定位方法、设备及存储介质 | |
| CN112880664A (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
| CN116929343A (zh) | 位姿估计方法、相关设备及存储介质 | |
| CN111336984A (zh) | 障碍物测距方法、装置、设备和介质 | |
| CN112180910A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |