CN111735451B - 一种基于多源先验信息的点云匹配高精度定位方法 - Google Patents

一种基于多源先验信息的点云匹配高精度定位方法 Download PDF

Info

Publication number
CN111735451B
CN111735451B CN202010298730.XA CN202010298730A CN111735451B CN 111735451 B CN111735451 B CN 111735451B CN 202010298730 A CN202010298730 A CN 202010298730A CN 111735451 B CN111735451 B CN 111735451B
Authority
CN
China
Prior art keywords
map
point cloud
intelligent platform
pose
prior
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
Application number
CN202010298730.XA
Other languages
English (en)
Other versions
CN111735451A (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.)
China North Vehicle Research Institute
Original Assignee
China North Vehicle Research Institute
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 China North Vehicle Research Institute filed Critical China North Vehicle Research Institute
Priority to CN202010298730.XA priority Critical patent/CN111735451B/zh
Publication of CN111735451A publication Critical patent/CN111735451A/zh
Application granted granted Critical
Publication of CN111735451B publication Critical patent/CN111735451B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/88—Lidar systems specially adapted for specific applications
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本发明公开了一种基于多源先验信息的点云匹配高精度定位方法，智能平台通过IMU数据和GNSS数据获得智能平台t时刻的GNSS全局位姿，通过点云数据获得点云观测数据；根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图；根据各个三维先验体素子地图的拓扑连通关系建立全局拓扑关系，得到智能平台运行区域的全局先验地图；将GNSS数据与全局先验地图的多源先验信息进行初始数据匹配，得到智能平台在某一三维先验体素子地图中的相对位姿；智能平台运动过程中不断利用当前点云观测数据与三维先验体素子地图进行数据匹配，得到智能平台的局部位姿，然后利用距离智能平台最近的先验路标校正智能平台的位姿，最终得到智能平台的位姿。

Description

一种基于多源先验信息的点云匹配高精度定位方法
技术领域
本发明属于定位技术领域，具体涉及一种基于多源先验信息的点云匹配高精度定位方法。
背景技术
定位技术是智能平台领域的关键技术之一，对于智能平台路径规划、运动控制和规避障碍有着重要的意义。
智能平台常用的定位技术有航迹推算定位技术、GNSS(GNSS为Global NavigationSatellite System的缩写，译为：全球卫星导航***或全球导航卫星***)定位技术以及多传感器融合定位技术。航迹推算定位技术在智能平台运动过程中会产生累积误差，导致智能平台无法获得准确的自身位姿。GNSS定位技术受天气、遮挡、信号干扰等因素的影响较大，不利于智能平台获取稳定的高精度定位信息。多传感器融合定位技术结合了GNSS定位技术和航迹推算定位技术的优点，但当卫星信号丢失时，该技术就会退化为航迹推算定位技术，因此该定位技术也无法为智能平台提供稳定的高精度定位信息。
发明内容
有鉴于此，针对常用的航迹推算定位技术、GNSS定位技术、多传感器融合定位技术无法为智能平台提供稳定的高精度定位信息的问题，本发明提供了一种基于多源先验信息的点云匹配高精度定位方法，智能平台通过融合IMU数据(IMU为Inertial MeasurementUnit的缩写，译为：惯性测量单元)、GNSS数据、激光雷达的点云数据等多源先验信息构建全局先验地图，然后通过点云观测数据匹配获得持续稳定的高精度定位信息。
本发明是通过下述技术方案实现的：
一种基于多源先验信息的点云匹配高精度定位方法，具体步骤如下：
步骤S1，通过安装于智能平台上的IMU、RTK-GPS设备以及激光雷达，在t时刻同步采集IMU数据、GNSS数据和点云数据，通过IMU数据和GNSS数据获得智能平台t时刻的GNSS全局位姿，通过点云数据获得点云观测数据；
步骤S2，根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图Si，i＝1,2,3…；
步骤S3，根据步骤S2创建的各个三维先验体素子地图的拓扑连通关系建立全局拓扑关系，得到智能平台运行区域的全局先验地图，该全局先验地图保存有各个三维先验体素子地图的N帧多源先验信息；
步骤S4，将步骤S1的GNSS数据与全局先验地图的多源先验信息进行初始数据匹配，得到智能平台在某一三维先验体素子地图Sj中的相对位姿；
步骤S5，智能平台运动过程中不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配，得到智能平台的局部位姿，然后利用距离智能平台最近的先验路标mk校正智能平台的位姿，最终得到智能平台的位姿。
进一步的，在步骤S2中，根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图Si的具体步骤如下：
步骤S201，根据点云观测数据创建t-1时刻的三维先验体素子地图
Figure BDA0002453191340000021
i＝1,2,3…，当i＝1时，将三维先验体素子地图S1的坐标系的伪全局位姿设为(0,0,0,0,0,0),并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图
Figure BDA0002453191340000022
的GNSS全局位姿；当i≠1时，以三维先验体素子地图
Figure BDA0002453191340000023
中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图
Figure BDA0002453191340000024
坐标系的伪全局位姿和GNSS全局位姿；
步骤S202，统计三维先验体素子地图
Figure BDA0002453191340000025
的每一体素中的点云分布特征及特征可信度，所述点云分布特征包括：线特征、面特征和团状特征；
步骤S203，采用最大似然估计法将t时刻的点云观测数据与三维先验体素子地图
Figure BDA0002453191340000026
进行数据匹配，得到智能平台在三维先验体素子地图
Figure BDA0002453191340000027
坐标系中的局部位姿
Figure BDA0002453191340000028
其中，点云观测数据与三维先验体素子地图
Figure BDA0002453191340000029
进行数据匹配的公式如下：
Figure BDA00024531913400000210
式中，n＝0,1,…N,Tt为t时刻的智能平台与三维先验体素子地图
Figure BDA00024531913400000211
之间的相对位姿，Cn为点云观测数据中的任一点，
Figure BDA00024531913400000212
为距离TtCn最近的特征体素中点云分布均值，
Figure BDA00024531913400000213
为距离TtCn最近的特征体素中点云分布特征向量矩阵，
Figure BDA00024531913400000214
为
Figure BDA00024531913400000215
的转置矩阵，
Figure BDA00024531913400000216
为距离TtCn最近的特征体素中点云分布特征值矩阵的逆矩阵,对于线特征，令
Figure BDA00024531913400000217
中的1/λ1＝0，对于面特征，令
Figure BDA00024531913400000218
中的1/λ1＝0,1/λ2＝0；
Figure BDA00024531913400000219
为点云观测数据中任一点的特征观测误差；e为点云特征观测误差，eT为e的转置矩阵，
Figure BDA00024531913400000220
为点云特征观测误差协方差矩阵的逆矩阵，
Figure BDA00024531913400000221
为智能平台在三维先验体素子地图
Figure BDA00024531913400000222
坐标系中的局部位姿，即t时刻点云观测数据相对于三维先验体素子地图
Figure BDA00024531913400000223
的最优相对位姿；
步骤S204，根据数据匹配结果
Figure BDA00024531913400000224
和t时刻的点云观测数据更新三维先验体素子地图
Figure BDA00024531913400000225
得到t时刻的三维先验体素子地图
Figure BDA00024531913400000226
并按照步骤S202的方法统计三维先验体素子地图
Figure BDA00024531913400000227
中每一体素的点云分布特征及特征可信度；
步骤S205，对智能平台在三维先验体素子地图
Figure BDA0002453191340000031
坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿，将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为第一个先验路标
Figure BDA0002453191340000032
将先验路标mt记录到三维先验体素子地图
Figure BDA0002453191340000033
中；
步骤S206，在三维先验体素子地图
Figure BDA0002453191340000034
中保存N帧多源先验信息后，将t时刻的三维先验体素子地图Si保存到计算平台外存中；
步骤S207，重复步骤S201～S206，完成智能平台运行区域内各个三维先验体素子地图Si的创建，i＝1,2,3…。
进一步的，在步骤S202中，统计三维先验体素子地图
Figure BDA00024531913400000322
的每一体素中的特征可信度
Figure BDA0002453191340000035
的具体步骤如下：
S2021，记录每一体素中来自不同时刻或者不同线束编号的激光雷达的观测点的数量Nf，当Nf增大时，采用贝叶斯概率更新公式对在t-1时刻第l个特征体素中特征可信度
Figure BDA0002453191340000036
进行更新，得到特征可信度
Figure BDA0002453191340000037
当Nf不变时，无需更新；
所述贝叶斯概率更新公式如下：
Figure BDA0002453191340000038
式中，
Figure BDA0002453191340000039
为在t-1时刻之前基于所有历史观测数据的特征可信度，
Figure BDA00024531913400000310
为t-1时刻三维先验体素子地图中的第l个特征体素，χ1:t-1为1到t-1时刻的智能平台的历史位姿，C1:t-1为智能平台的1到t-1时刻的点云观测数据，
Figure BDA00024531913400000311
为t-1时刻观测到的第l个特征体素中特征可信度，χt-1为t-1时刻智能平台的位姿，Ct-1为智能平台在t-1时刻的点云观测数据,
Figure BDA00024531913400000312
为更新前的特征可信度；
S2022，对于特征可信度
Figure BDA00024531913400000313
大于阈值Pthresh_f的体素(对于小于阈值Pthresh_f的体素不进行处理)，利用三维高斯分布统计该体素中的点云分布
Figure BDA00024531913400000314
即该体素中的点云分布
Figure BDA00024531913400000315
服从三维高斯分布，
即
Figure BDA00024531913400000316
式中，
Figure BDA00024531913400000317
为体素中点云分布的均值，
Figure BDA00024531913400000318
为体素中点云分布的协方差；
S2023，根据公式(2)计算体素中点云分布
Figure BDA00024531913400000319
的协方差矩阵的特征值与特征向量；
Figure BDA00024531913400000320
式中，
Figure BDA00024531913400000321
为一个对角矩阵，其对角元素λ1、λ2及λ3分别为协方差矩阵的三个从大到小排列的特征值，
Figure BDA0002453191340000041
的每一列依次为该特征值对应的特征向量；
当协方差矩阵的特征值中的λ1≥10λ2且λ1≥10λ3时，该特征体素中的点云呈线特征分布；当特征值中的λ1≥15λ3且λ2≥15λ3时，该特征体素中的点云呈面特征分布；当协方差矩阵的λ1、λ2及λ3在同一数量级时，该特征体素中的点云呈团状特征分布。
进一步的，所述步骤S4的具体步骤如下：
S401，根据GNSS数据获取智能平台所在位置的范围，根据全局拓扑关系检索与当前智能平台距离最近的三维先验体素子地图Sj，将三维先验体素子地图Sj的数据读取到计算平台内存当中；
S402，检索所述三维先验体素子地图Sj的GNSS全局位姿中与智能平台距离最近的先验路标
Figure BDA0002453191340000042
S403，利用先验路标mk的伪全局位姿与该三维先验体素子地图Sj坐标系的伪全局位姿计算先验路标mk的局部位姿T0，并将该局部位姿T0作为初始数据匹配的启发值；
S404，根据公式(5)和S403中的启发值计算解空间W；
Figure BDA0002453191340000043
式中，Wx、Wy、Wz分别为三维先验体素子地图Sj坐标系的x，y，z轴的搜索窗口大小，Wγ为航向角搜索窗口、Wθ为横滚角和俯仰角的搜索窗口，r为位置搜索步长，δγ为角度搜索步长；
Figure BDA0002453191340000046
为离散化的总搜索窗口，hx为{-wx,…,wx}中的任一值，hy为{-wy,…,wy}中的任一值，hz为{-wz,…,wz}中的任一值，hα为{-wα,…,wα}中的任一值，hβ为{-wβ,…,wβ}中的任一值，hγ为{-wγ,…,wγ}中的任一值；
S405.使用分支定界法在解空间W中进行位姿求解，得到准确的启发值
Figure BDA0002453191340000047
将准确的启发值
Figure BDA0002453191340000048
替换公式(4)中的Tt，进行三维先验体素子地图Sj与点云观测数据的数据匹配，得到智能平台在三维先验体素子地图Sj坐标系中的局部位姿，即智能平台在三维先验体素子地图Sj中的最优相对位姿。
进一步的，所述步骤S5的具体步骤如下：
S501，随着智能平台的运动，不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配，得到当前智能平台的局部位姿
Figure BDA0002453191340000044
S502，利用当前智能平台的局部位姿
Figure BDA0002453191340000045
和所在三维先验体素子地图Sj坐标系的伪全局位姿求取智能平台的伪全局位姿
Figure BDA0002453191340000051
检索与三维先验体素子地图Sj距离最近的先验路标
Figure BDA0002453191340000052
并利用该先验路标校正智能平台的位姿，校正公式如下：
Figure BDA0002453191340000053
式中，
Figure BDA0002453191340000054
为当前智能平台的伪全局位姿，
Figure BDA0002453191340000055
为先验路标mj的伪全局位姿
Figure BDA0002453191340000056
的逆矩阵，
Figure BDA0002453191340000057
为先验路标mj的GNSS全局位姿，Aj为当前智能平台的全局位姿，即先验信息数据匹配结果；
S503，随着智能平台的不断运动，智能平台会从三维先验体素子地图Sj逐渐运动到三维先验体素子地图Sj+1，此时需要进行子地图切换，为了避免智能平台处于子地图边界时产生子地图频繁切换，当智能平台的全局位姿Aj与三维先验体素子地图Sj的GNSS全局位姿
Figure BDA0002453191340000058
的距离大于设定值Tshift，且与三维先验体素子地图Sj+1的GNSS全局位姿
Figure BDA0002453191340000059
的距离小于等于设定值Tshift时，进行子地图的切换，即将三维先验体素子地图Sj切换为三维先验体素子地图Sj+1；完成子地图切换后，重复S501～S502，即继续利用点云观测数据与智能平台所在三维先验体素子地图Sj+1进行匹配定位，最终得到智能平台的位姿。
有益效果：(1)本发明的智能平台基于IMU数据、GNSS数据、激光雷达的点云数据等多源先验信息构建全局先验地图，然后在不依赖GNSS数据的情况下，基于全局先验地图通过激光雷达的点云观测数据匹配实现对智能平台的高精度定位，避免了航迹推算定位技术中的累积误差问题以及GNSS定位技术中的信号干扰问题，在GNSS信号缺失的地方仍可正常使用。
(2)本发明在智能平台采集完运行区域的先验数据后，就可立即完成全局先验地图的创建并投入使用，无需进行全局先验地图的离线优化，全局先验地图构建方法简便快捷。
附图说明
图1为本发明的方法流程图；
图2为实际场景图；
图3为图2对应的三维先验体素子地图；
图4为多源先验信息定位误差分析中X轴误差图；
图5为多源先验信息定位误差分析中Y轴误差图。
具体实施方式
下面结合附图并举实施例，对本发明进行详细描述。
本实施例提供了一种基于多源先验信息的点云匹配高精度定位方法，参见附图1，该方法包括两部分，第一部分为创建多源先验信息，第二部分为多源先验信息定位；
创建多源先验信息的具体步骤如下：
步骤S1，通过安装于智能平台上的IMU、RTK-GPS设备(RTK为Real-Time Kinematic的缩写，译为实时动态，IMU和RTK-GPS设备组成惯性导航***)以及激光雷达，在t时刻同步采集IMU数据(该IMU数据为智能平台的姿态信息，该信息通过IMU采集得到)、GNSS数据(该GNSS数据为智能平台的位置信息，该信息通过RTK-GPS设备采集得到)和点云数据(该数据通过激光雷达采集得到)，通过IMU数据和GNSS数据获得智能平台t时刻的GNSS全局位姿(位姿包括位置和姿态)，通过点云数据进行点云预处理获得点云观测数据；
步骤S2，根据点云观测数据估计智能平台的位姿，并根据估计的智能平台位姿创建t-1时刻的三维先验体素子地图
Figure BDA0002453191340000061
i＝1,2,3…，当i＝1时，将三维先验体素子地图
Figure BDA0002453191340000062
的坐标系的伪全局位姿设为(0,0,0,0,0,0),并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图
Figure BDA0002453191340000063
的GNSS全局位姿；当i≠1时，以三维先验体素子地图
Figure BDA0002453191340000064
中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图
Figure BDA0002453191340000065
坐标系的伪全局位姿和GNSS全局位姿；
步骤S3，统计三维先验体素子地图
Figure BDA0002453191340000066
的每一体素中的点云分布特征及特征可信度，所述点云分布特征包括：线特征、面特征和团状特征；
其中，统计三维先验体素子地图
Figure BDA0002453191340000067
的每一体素中的特征可信度
Figure BDA0002453191340000068
的具体步骤如下：
S301，记录每一体素中来自不同时刻(即1到t-1时刻)或者不同线束编号的激光雷达的观测点的数量Nf，当Nf增大时，采用贝叶斯概率更新公式对在t-1时刻第l个特征体素中特征可信度
Figure BDA0002453191340000069
进行更新，得到特征可信度
Figure BDA00024531913400000610
当Nf不变时，无需更新；
所述贝叶斯概率更新公式如下：
Figure BDA00024531913400000611
式中，
Figure BDA00024531913400000612
为在t-1时刻之前(包括t-1时刻)基于所有历史观测数据的特征可信度，
Figure BDA00024531913400000613
为t-1时刻三维先验体素子地图中的第l个特征体素，χ1:t-1为1到t-1时刻的智能平台的历史位姿，C1:t-1为智能平台的1到t-1时刻的点云观测数据，
Figure BDA00024531913400000614
为t-1时刻观测到的第l个特征体素中特征可信度，χt-1为t-1时刻智能平台的位姿，Ct-1为智能平台在t-1时刻的点云观测数据,
Figure BDA00024531913400000615
为更新前的特征可信度；
S302，对于特征可信度
Figure BDA0002453191340000071
大于阈值Pthresh_f的体素(对于小于阈值Pthresh_f的体素不进行处理)，利用三维高斯分布统计该体素中的点云分布
Figure BDA0002453191340000072
即该体素中的点云分布
Figure BDA0002453191340000073
服从三维高斯分布，
即
Figure BDA0002453191340000074
式中，
Figure BDA0002453191340000075
为体素中点云分布的均值，
Figure BDA0002453191340000076
为体素中点云分布的协方差；
S303，根据公式(2)计算体素中点云分布
Figure BDA0002453191340000077
的协方差矩阵的特征值与特征向量；
Figure BDA0002453191340000078
式中，
Figure BDA0002453191340000079
为一个对角矩阵，其对角元素λ1、λ2及λ3分别为协方差矩阵的三个从大到小排列的特征值，
Figure BDA00024531913400000710
的每一列依次为该特征值对应的特征向量；
当协方差矩阵的特征值中的λ1远大于λ2及λ3(即λ1≥10λ2且λ1≥10λ3)时，该特征体素中的点云呈线特征分布；当特征值中的λ1、λ2远大于λ3(即λ1≥15λ3且λ2≥15λ3)时，该特征体素中的点云呈面特征分布；当协方差矩阵的三个特征值无明显差别(即λ1、λ2及λ3在同一数量级)时，该特征体素中的点云呈团状特征分布；
步骤S4，采用最大似然估计法将t时刻的点云观测数据与三维先验体素子地图
Figure BDA00024531913400000711
进行数据匹配，得到智能平台在三维先验体素子地图
Figure BDA00024531913400000712
坐标系中的局部位姿
Figure BDA00024531913400000713
其中，点云观测数据与三维先验体素子地图
Figure BDA00024531913400000714
进行数据匹配的公式如下：
Figure BDA00024531913400000715
式中，n＝0,1,…N,Tt为t时刻的智能平台与三维先验体素子地图
Figure BDA00024531913400000716
之间的相对位姿，Cn为点云观测数据中的任一点，
Figure BDA00024531913400000717
为距离TtCn最近的特征体素中点云分布均值，
Figure BDA00024531913400000718
为距离TtCn最近的特征体素中点云分布特征向量矩阵，
Figure BDA00024531913400000719
为
Figure BDA00024531913400000720
的转置矩阵，
Figure BDA00024531913400000721
为距离TtCn最近的特征体素中点云分布特征值矩阵的逆矩阵，
Figure BDA00024531913400000722
根据步骤S303计算得出，且在步骤S303的计算结果
Figure BDA00024531913400000723
的逆矩阵中，对于线特征，令
Figure BDA00024531913400000724
中的1/λ1＝0，对于面特征，令
Figure BDA00024531913400000725
中的1/λ1＝0,1/λ2＝0后，得到
Figure BDA00024531913400000726
Figure BDA00024531913400000727
为点云观测数据中任一点的特征观测误差；e为点云特征观测误差，eT为e的转置矩阵，
Figure BDA00024531913400000728
为点云特征观测误差协方差矩阵的逆矩阵，
Figure BDA00024531913400000729
为点云观测数据匹配的结果，即t时刻点云观测数据相对于三维先验体素子地图
Figure BDA00024531913400000730
的最优(最优即为概率值最大的位姿)相对位姿,即智能平台在三维先验体素子地图
Figure BDA00024531913400000731
坐标系中的局部位姿；
步骤S5，根据数据匹配结果
Figure BDA00024531913400000732
和t时刻的点云观测数据更新三维先验体素子地图
Figure BDA00024531913400000733
得到t时刻的三维先验体素子地图
Figure BDA00024531913400000734
并按照步骤S3的方法统计三维先验体素子地图
Figure BDA00024531913400000735
中每一体素的点云分布特征及特征可信度后，进行存储，以便用于计算t+1时刻的三维先验体素子地图
Figure BDA0002453191340000081
步骤S6，对智能平台在三维先验体素子地图
Figure BDA0002453191340000082
坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿，将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为一个先验路标
Figure BDA0002453191340000083
将先验路标mt记录到三维先验体素子地图
Figure BDA0002453191340000084
中；
步骤S7，为了简化多源先验信息创建的计算量，在当前(当前即为t时刻)三维先验体素子地图
Figure BDA0002453191340000085
中保存N帧多源先验信息后，将t时刻的三维先验体素子地图
Figure BDA0002453191340000086
保存到计算平台外存中供日后调用；
步骤S8，重复步骤S2～S7，完成智能平台运行区域内各个三维先验体素子地图Si的创建，i＝1,2,3…，创建结果参见附图2和图3，图2为实际场景图，图3为根据实际场景图创建三维先验体素子地图，从图3中可以看出三维先验体素子地图良好的反映了实际场景图中存在的平面状、线状以及团状物体的特征；
步骤S9，根据步骤S8创建的各个三维先验体素子地图的拓扑连通关系建立全局拓扑关系，得到智能平台运行区域的全局先验地图，该全局先验地图保存有各个三维先验体素子地图的N帧多源先验信息。
创建完包含多源先验信息的全局先验地图后，根据全局先验地图对智能平台进行定位，即根据多源先验信息进行定位，所述定位的具体步骤如下：
步骤S10，将步骤S1的GNSS数据与全局先验地图的多源先验信息进行初始数据匹配，得到智能平台在某一三维先验体素子地图Sj中的相对位姿，具体步骤如下：
S1001，根据GNSS数据获取智能平台所在位置的范围，根据全局拓扑关系检索与当前智能平台距离最近的三维先验体素子地图Sj，将三维先验体素子地图Sj的数据读取到计算平台内存当中,即调用多源先验信息；
S1002，检索所述三维先验体素子地图Sj的GNSS全局位姿中与智能平台距离最近的先验路标
Figure BDA0002453191340000087
S1003，利用先验路标mk的伪全局位姿与该三维先验体素子地图Sj坐标系的伪全局位姿计算先验路标mk的局部位姿T0，并将该局部位姿T0作为初始数据匹配的启发值；
S1004，根据公式(5)和S1003中的启发值(该启发值为局部位姿T0)计算解空间W；
Figure BDA0002453191340000088
式中，Wx、Wy、Wz分别为三维先验体素子地图Sj坐标系的x，y，z轴的搜索窗口大小，Wγ为航向角搜索窗口、Wθ为横滚角和俯仰角的搜索窗口，r为位置搜索步长，δγ为角度搜索步长；
Figure BDA00024531913400000914
为离散化的总搜索窗口，hx为{-wx,…,wx}中的任一值，hy为{-wy,…,wy}中的任一值，hz为{-wz,…,wz}中的任一值，hα为{-wα,…,wα}中的任一值，hβ为{-wβ,…,wβ}中的任一值，hγ为{-wγ,…,wγ}中的任一值；
S1005.使用分支定界法在解空间W中进行位姿求解，得到准确的启发值
Figure BDA0002453191340000091
将准确的启发值
Figure BDA0002453191340000092
代入公式(4)中(即替换公式(4)中的Tt)，进行三维先验体素子地图Sj与点云观测数据的数据匹配，得到初始数据匹配的结果，即智能平台在三维先验体素子地图Sj中的最优相对位姿，也即智能平台在三维先验体素子地图Sj坐标系中的局部位姿；
步骤S11，智能平台运动过程中不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配(即进行位姿初匹配)，得到智能平台的局部位姿，然后利用距离智能平台最近的先验路标mk校正智能平台的位姿，最终得到精确稳定的智能平台位姿，此过程不再需要GNSS数据，具体步骤如下：
S1101，随着智能平台的运动，不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配，得到当前智能平台的局部位姿
Figure BDA0002453191340000093
S1102，利用当前智能平台的局部位姿
Figure BDA0002453191340000094
和所在三维先验体素子地图Sj坐标系的伪全局位姿求取智能平台的伪全局位姿
Figure BDA0002453191340000095
检索与三维先验体素子地图Sj距离最近的先验路标
Figure BDA0002453191340000096
并利用该先验路标校正智能平台的位姿，校正公式如下：
Figure BDA0002453191340000097
式中，
Figure BDA0002453191340000098
为当前智能平台的伪全局位姿，
Figure BDA0002453191340000099
为先验路标mj的伪全局位姿
Figure BDA00024531913400000910
的逆矩阵，
Figure BDA00024531913400000911
为先验路标mj的GNSS全局位姿，Aj为当前智能平台的全局位姿，即先验信息数据匹配结果；
S1103，随着智能平台的不断运动，智能平台会从三维先验体素子地图Sj逐渐运动到三维先验体素子地图Sj+1，此时需要进行子地图切换，为了避免智能平台处于子地图边界时产生子地图频繁切换，当智能平台的全局位姿Aj与三维先验体素子地图Sj的GNSS全局位姿
Figure BDA00024531913400000912
的距离大于设定值Tshift，且与三维先验体素子地图Sj+1的GNSS全局位姿
Figure BDA00024531913400000913
的距离小于等于设定值Tshift时，进行子地图的切换，即将三维先验体素子地图Sj切换为三维先验体素子地图Sj+1；完成子地图切换后，重复S1101～S1102，即继续利用点云观测数据与智能平台所在三维先验体素子地图Sj+1进行匹配定位，最终得到精确稳定的智能平台的位姿；
最终，对采用多源先验信息定位智能平台的位姿进行误差分析，令智能平台开始运动处为原点，令在水平面上的经度方向(即东西向)为X轴方向，令在水平面上的纬度方向(即南北向)为Y轴方向，参见附图4可知，智能平台在X轴方向的误差，最大误差为3.55m，参见附图5可知，智能平台在Y轴方向的误差，最大误差为4.62m；且表1是本实施例定位方法的定位精度统计表，从表1中可以看出，多源先验信息定位X轴位置以及Y轴位置的定位精度均在
Figure BDA0002453191340000101
内，可以满足智能平台常规定位的需求。
Figure BDA0002453191340000102
表1
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，具体步骤如下：
步骤S1，通过安装于智能平台上的IMU、RTK-GPS设备以及激光雷达，在t时刻同步采集IMU数据、GNSS数据和点云数据，通过IMU数据和GNSS数据获得智能平台t时刻的GNSS全局位姿，通过点云数据获得点云观测数据；
步骤S2，根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图Si，i＝1，2，3...；
步骤S3，根据步骤S2创建的各个三维先验体素子地图的拓扑连通关系建立全局拓扑关系，得到智能平台运行区域的全局先验地图，该全局先验地图保存有各个三维先验体素子地图的N帧多源先验信息；
步骤S4，将步骤S1的GNSS数据与全局先验地图的多源先验信息进行初始数据匹配，得到智能平台在某一三维先验体素子地图Sj中的相对位姿；
步骤S5，智能平台运动过程中不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配，得到智能平台的局部位姿，所述局部位姿为智能平台在三维先验体素子地图Sj中的最优相对位姿，然后利用距离智能平台最近的先验路标mk校正智能平台的位姿，最终得到智能平台的位姿。
2.如权利要求1所述的一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，在步骤S2中，根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图Si的具体步骤如下：
步骤S201，根据点云观测数据创建t-1时刻的三维先验体素子地图
Figure FDA0003503440440000011
当i＝1时，将三维先验体素子地图S1的坐标系的伪全局位姿设为(0，0，0，0，0，0)，并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图
Figure FDA0003503440440000012
的GNSS全局位姿；当i≠1时，以三维先验体素子地图
Figure FDA0003503440440000013
中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图
Figure FDA0003503440440000014
坐标系的伪全局位姿和GNSS全局位姿；
步骤S202，统计三维先验体素子地图
Figure FDA0003503440440000015
的每一体素中的点云分布特征及特征可信度，所述点云分布特征包括：线特征、面特征和团状特征；
步骤S203，采用最大似然估计法将t时刻的点云观测数据与三维先验体素子地图
Figure FDA0003503440440000016
进行数据匹配，得到智能平台在三维先验体素子地图
Figure FDA0003503440440000017
坐标系中的局部位姿
Figure FDA0003503440440000018
其中，点云观测数据与三维先验体素子地图
Figure FDA0003503440440000019
进行数据匹配的公式如下：
Figure FDA00035034404400000110
式中，n＝0，1，…N，Tt为t时刻的智能平台与三维先验体素子地图
Figure FDA0003503440440000021
之间的相对位姿，Cn为点云观测数据中的任一点，
Figure FDA0003503440440000022
为距离TtCn最近的特征体素中点云分布均值，
Figure FDA0003503440440000023
为距离TtCn最近的特征体素中点云分布特征向量矩阵，
Figure FDA0003503440440000024
为
Figure FDA0003503440440000025
的转置矩阵，
Figure FDA0003503440440000026
为距离TtCn最近的特征体素中点云分布特征值矩阵的逆矩阵，对于线特征，令
Figure FDA0003503440440000027
中的1/λ1＝0，对于面特征，令
Figure FDA0003503440440000028
中的1/λ1＝0，1/λ2＝0；
Figure FDA0003503440440000029
为点云观测数据中任一点的特征观测误差；e为点云特征观测误差，eT为e的转置矩阵，
Figure FDA00035034404400000210
为点云特征观测误差协方差矩阵的逆矩阵，
Figure FDA00035034404400000211
为智能平台在三维先验体素子地图
Figure FDA00035034404400000212
坐标系中的局部位姿，即t时刻点云观测数据相对于三维先验体素子地图
Figure FDA00035034404400000213
的最优相对位姿；
步骤S204，根据数据匹配结果
Figure FDA00035034404400000214
和t时刻的点云观测数据更新三维先验体素子地图
Figure FDA00035034404400000230
得到t时刻的三维先验体素子地图
Figure FDA00035034404400000216
并按照步骤S202的方法统计三维先验体素子地图
Figure FDA00035034404400000217
中每一体素的点云分布特征及特征可信度；
步骤S205，对智能平台在三维先验体素子地图
Figure FDA00035034404400000228
坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿，将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为第一个先验路标
Figure FDA00035034404400000218
将先验路标mt记录到三维先验体素子地图
Figure FDA00035034404400000219
中；
步骤S206，在三维先验体素子地图
Figure FDA00035034404400000220
中保存N帧多源先验信息后，将t时刻的三维先验体素子地图Si保存到计算平台外存中；
步骤S207，重复步骤S201～S206，完成智能平台运行区域内各个三维先验体素子地图Si的创建，i＝1，2，3...。
3.如权利要求2所述的一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，在步骤S202中，统计三维先验体素子地图
Figure FDA00035034404400000221
的每一体素中的特征可信度
Figure FDA00035034404400000229
的具体步骤如下：
S2021，记录每一体素中来自不同时刻或者不同线束编号的激光雷达的观测点的数量Nf，当Nf增大时，采用贝叶斯概率更新公式对在t-1时刻第l个特征体素中特征可信度
Figure FDA00035034404400000223
进行更新，得到特征可信度
Figure FDA00035034404400000224
当Nf不变时，无需更新；
所述贝叶斯概率更新公式如下：
Figure FDA00035034404400000225
式中，
Figure FDA00035034404400000226
为在t-1时刻之前基于所有历史观测数据的特征可信度，
Figure FDA00035034404400000227
为t-1时刻三维先验体素子地图中的第l个特征体素，χ1：t-1为1到t-1时刻的智能平台的历史位姿，C1：t-1为智能平台的1到t-1时刻的点云观测数据，
Figure FDA0003503440440000031
为t-1时刻观测到的第l个特征体素中特征可信度，χt-1为t-1时刻智能平台的位姿，Ct-1为智能平台在t-1时刻的点云观测数据，
Figure FDA0003503440440000032
为更新前的特征可信度；
S2022，对于特征可信度
Figure FDA0003503440440000033
大于阈值Pthresh_f的体素(对于小于阈值Pthresh_f的体素不进行处理)，利用三维高斯分布统计该体素中的点云分布
Figure FDA0003503440440000034
即该体素中的点云分布
Figure FDA0003503440440000035
服从三维高斯分布，
即
Figure FDA0003503440440000036
式中，
Figure FDA0003503440440000037
为体素中点云分布的均值，
Figure FDA0003503440440000038
为体素中点云分布的协方差；
S2023，根据公式(2)计算体素中点云分布
Figure FDA0003503440440000039
的协方差矩阵的特征值与特征向量；
Figure FDA00035034404400000310
式中，
Figure FDA00035034404400000311
为一个对角矩阵，其对角元素λ1、λ2及λ3分别为协方差矩阵的三个从大到小排列的特征值，
Figure FDA00035034404400000312
的每一列依次为该特征值对应的特征向量；
当协方差矩阵的特征值中的λ1≥10λ2且λ1≥10λ3时，该特征体素中的点云呈线特征分布；当特征值中的λ1≥15λ3且λ2≥15λ3时，该特征体素中的点云呈面特征分布；当协方差矩阵的λ1、λ2及λ3在同一数量级时，该特征体素中的点云呈团状特征分布。
4.如权利要求2所述的一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，所述步骤S4的具体步骤如下：
S401，根据GNSS数据获取智能平台所在位置的范围，根据全局拓扑关系检索与当前智能平台距离最近的三维先验体素子地图Sj，将三维先验体素子地图Sj的数据读取到计算平台内存当中；
S402，检索所述三维先验体素子地图Sj的GNSS全局位姿中与智能平台距离最近的先验路标
Figure FDA00035034404400000313
S403，利用先验路标mk的伪全局位姿与该三维先验体素子地图Sj坐标系的伪全局位姿计算先验路标mk的局部位姿T0，并将该局部位姿T0作为初始数据匹配的启发值；
S404，根据公式(5)和S403中的启发值计算解空间W；
Figure FDA00035034404400000314
式中，Wx、Wy、Wz分别为三维先验体素子地图Sj坐标系的x，y，z轴的搜索窗口大小，Wγ为航向角搜索窗口、Wθ为横滚角和俯仰角的搜索窗口，r为位置搜索步长，δγ为角度搜索步长；
Figure FDA0003503440440000041
为离散化的总搜索窗口，hx为{-wx，…，wx}中的任一值，hy为{-wy，…，wy}中的任一值，hz为{-wz，…，wz}中的任一值，hα为{-wα，…，wα}中的任一值，hβ为{-wβ，…，wβ}中的任一值，hγ为{-wγ，…，wγ}中的任一值；
S405.使用分支定界法在解空间W中进行位姿求解，得到准确的启发值
Figure FDA0003503440440000042
将准确的启发值
Figure FDA0003503440440000043
替换公式(4)中的Tt，进行三维先验体素子地图Sj与点云观测数据的数据匹配，得到智能平台在三维先验体素子地图Sj坐标系中的局部位姿，即智能平台在三维先验体素子地图Sj中的最优相对位姿。
5.如权利要求2所述的一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，所述步骤S5的具体步骤如下：
S501，随着智能平台的运动，不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配，得到当前智能平台的局部位姿
Figure FDA0003503440440000044
S502，利用当前智能平台的局部位姿
Figure FDA0003503440440000045
和所在三维先验体素子地图Sj坐标系的伪全局位姿求取智能平台的伪全局位姿
Figure FDA0003503440440000046
检索与三维先验体素子地图Sj距离最近的先验路标
Figure FDA0003503440440000047
并利用该先验路标校正智能平台的位姿，校正公式如下：
Figure FDA0003503440440000048
式中，
Figure FDA0003503440440000049
为当前智能平台的伪全局位姿，
Figure FDA00035034404400000410
为先验路标mj的伪全局位姿
Figure FDA00035034404400000411
的逆矩阵，
Figure FDA00035034404400000412
为先验路标mj的GNSS全局位姿，Aj为当前智能平台的全局位姿，即先验信息数据匹配结果；
S503，随着智能平台的不断运动，智能平台会从三维先验体素子地图Sj逐渐运动到三维先验体素子地图Sj+1，此时需要进行子地图切换，为了避免智能平台处于子地图边界时产生子地图频繁切换，当智能平台的全局位姿Aj与三维先验体素子地图Sj的GNSS全局位姿
Figure FDA00035034404400000413
的距离大于设定值Tshift，且与三维先验体素子地图Sj+1的GNSS全局位姿
Figure FDA00035034404400000414
的距离小于等于设定值Tshift时，进行子地图的切换，即将三维先验体素子地图Sj切换为三维先验体素子地图Sj+1；完成子地图切换后，重复S501～S502，即继续利用点云观测数据与智能平台所在三维先验体素子地图Sj+1进行匹配定位，最终得到智能平台的位姿。
CN202010298730.XA 2020-04-16 2020-04-16 一种基于多源先验信息的点云匹配高精度定位方法 Active CN111735451B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010298730.XA CN111735451B (zh) 2020-04-16 2020-04-16 一种基于多源先验信息的点云匹配高精度定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010298730.XA CN111735451B (zh) 2020-04-16 2020-04-16 一种基于多源先验信息的点云匹配高精度定位方法

Publications (2)

Publication Number Publication Date
CN111735451A CN111735451A (zh) 2020-10-02
CN111735451B true CN111735451B (zh) 2022-06-07

Family

ID=72646744

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010298730.XA Active CN111735451B (zh) 2020-04-16 2020-04-16 一种基于多源先验信息的点云匹配高精度定位方法

Country Status (1)

Country Link
CN (1) CN111735451B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114440901A (zh) * 2020-11-05 2022-05-06 北理慧动（常熟）车辆科技有限公司 一种全局混合地图创建方法
CN113639749A (zh) * 2021-07-02 2021-11-12 天津大学 一种基于不确定性的多波束测深数据匹配检测方法
CN117739954B (zh) * 2024-02-07 2024-06-04 未来机器人（深圳）有限公司 地图局部更新方法、装置及电子设备

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107144285A (zh) * 2017-05-08 2017-09-08 深圳地平线机器人科技有限公司 位姿信息确定方法、装置和可移动设备
CN107144292A (zh) * 2017-06-08 2017-09-08 杭州南江机器人股份有限公司 一种运动设备的里程计方法以及里程计装置
CN107340522A (zh) * 2017-07-10 2017-11-10 浙江国自机器人技术有限公司 一种激光雷达定位的方法、装置及***
CN108225302A (zh) * 2017-12-27 2018-06-29 中国矿业大学 一种石化工厂巡检机器人定位***和方法
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
CN108375976A (zh) * 2018-01-22 2018-08-07 中国民用航空飞行学院 一种服务机器人导航方法及***
CN108732584A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于更新地图的方法和装置
CN108759833A (zh) * 2018-04-25 2018-11-06 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109425348A (zh) * 2017-08-23 2019-03-05 北京图森未来科技有限公司 一种同时定位与建图的方法和装置
CN110208802A (zh) * 2019-05-16 2019-09-06 四川省客车制造有限责任公司 融合多视图模糊推理赋值的障碍物检测方法
CN110412635A (zh) * 2019-07-22 2019-11-05 武汉大学 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法
CN110849374A (zh) * 2019-12-03 2020-02-28 中南大学 地下环境定位方法、装置、设备及存储介质
CN110988894A (zh) * 2019-12-25 2020-04-10 畅加风行(苏州)智能科技有限公司 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110222757A1 (en) * 2010-03-10 2011-09-15 Gbo 3D Technology Pte. Ltd. Systems and methods for 2D image and spatial data capture for 3D stereo imaging

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108732584A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于更新地图的方法和装置
CN107144285A (zh) * 2017-05-08 2017-09-08 深圳地平线机器人科技有限公司 位姿信息确定方法、装置和可移动设备
CN107144292A (zh) * 2017-06-08 2017-09-08 杭州南江机器人股份有限公司 一种运动设备的里程计方法以及里程计装置
CN107340522A (zh) * 2017-07-10 2017-11-10 浙江国自机器人技术有限公司 一种激光雷达定位的方法、装置及***
CN109425348A (zh) * 2017-08-23 2019-03-05 北京图森未来科技有限公司 一种同时定位与建图的方法和装置
CN108225302A (zh) * 2017-12-27 2018-06-29 中国矿业大学 一种石化工厂巡检机器人定位***和方法
CN108375976A (zh) * 2018-01-22 2018-08-07 中国民用航空飞行学院 一种服务机器人导航方法及***
CN108320329A (zh) * 2018-02-02 2018-07-24 维坤智能科技（上海）有限公司 一种基于3d激光的3d地图创建方法
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN108759833A (zh) * 2018-04-25 2018-11-06 中国科学院合肥物质科学研究院 一种基于先验地图的智能车辆定位方法
CN110208802A (zh) * 2019-05-16 2019-09-06 四川省客车制造有限责任公司 融合多视图模糊推理赋值的障碍物检测方法
CN110412635A (zh) * 2019-07-22 2019-11-05 武汉大学 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN110501017A (zh) * 2019-08-12 2019-11-26 华南理工大学 一种基于orb_slam2的移动机器人导航地图生成方法
CN110849374A (zh) * 2019-12-03 2020-02-28 中南大学 地下环境定位方法、装置、设备及存储介质
CN110988894A (zh) * 2019-12-25 2020-04-10 畅加风行(苏州)智能科技有限公司 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种基于多传感融合的室内建图和定位算法;纪嘉文等;《成都信息工程大学学报》;20180831;第33卷(第4期);400-407 *

Also Published As

Publication number Publication date
CN111735451A (zh) 2020-10-02

Similar Documents

Publication Publication Date Title
CN111735451B (zh) 一种基于多源先验信息的点云匹配高精度定位方法
Sim et al. Integrated position estimation using aerial image sequences
CN111486845B (zh) 基于海底地形匹配的auv多策略导航方法
US4495580A (en) Navigation system
US20090125223A1 (en) Video navigation
EP2246763A2 (en) System and method for simultaneous localization and map building
US11158065B2 (en) Localization of a mobile unit by means of a multi hypothesis kalman filter method
EP4143507B1 (en) Navigation apparatus and method in which measurement quantization errors are modeled as states
CN115861860B (zh) 一种无人机的目标跟踪定位方法和***
CN111578959A (zh) 一种基于改进Hector SLAM算法的未知环境自主定位方法
CN113447949A (zh) 一种基于激光雷达和先验地图的实时定位***及方法
CN112504261A (zh) 一种基于视觉锚点的无人机降落位姿滤波估计方法及***
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集***及方法
Venable et al. Large scale image aided navigation
US10677881B2 (en) Map assisted inertial navigation
CN114723811A (zh) 一种非结构化环境四足机器人立体视觉定位建图方法
Keivan et al. Constant-time monocular self-calibration
US10330769B1 (en) Method and apparatus for geolocating emitters in a multi-emitter environment
CN113781567A (zh) 基于三维地图生成的航拍图像目标地理定位方法
CN113379915A (zh) 一种基于点云融合的行车场景构建方法
Stoven-Dubois et al. Graph optimization methods for large-scale crowdsourced mapping
Escourrou et al. Ndt localization with 2d vector maps and filtered lidar scans
Navard et al. A Probabilistic-based Drift Correction Module for Visual Inertial SLAMs
CN116989763B (zh) 一种面向水陆两用无人***的融合定位与建图方法
Garcia et al. Fast and Reliable GNSS Attitude Estimation Using a Constrained Bayesian Ambiguity Resolution Technique (C-BART)

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