CN111735451B - 一种基于多源先验信息的点云匹配高精度定位方法 - Google Patents
一种基于多源先验信息的点云匹配高精度定位方法 Download PDFInfo
- 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
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时刻的三维先验体素子地图i=1,2,3…,当i=1时,将三维先验体素子地图S1的坐标系的伪全局位姿设为(0,0,0,0,0,0),并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图的GNSS全局位姿;当i≠1时,以三维先验体素子地图中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图坐标系的伪全局位姿和GNSS全局位姿;
式中,n=0,1,…N,Tt为t时刻的智能平台与三维先验体素子地图之间的相对位姿,Cn为点云观测数据中的任一点,为距离TtCn最近的特征体素中点云分布均值,为距离TtCn最近的特征体素中点云分布特征向量矩阵,为的转置矩阵,为距离TtCn最近的特征体素中点云分布特征值矩阵的逆矩阵,对于线特征,令中的1/λ1=0,对于面特征,令中的1/λ1=0,1/λ2=0;为点云观测数据中任一点的特征观测误差;e为点云特征观测误差,eT为e的转置矩阵,为点云特征观测误差协方差矩阵的逆矩阵,为智能平台在三维先验体素子地图坐标系中的局部位姿,即t时刻点云观测数据相对于三维先验体素子地图的最优相对位姿;
步骤S205,对智能平台在三维先验体素子地图坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿,将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为第一个先验路标将先验路标mt记录到三维先验体素子地图中;
步骤S207,重复步骤S201~S206,完成智能平台运行区域内各个三维先验体素子地图Si的创建,i=1,2,3…。
S2021,记录每一体素中来自不同时刻或者不同线束编号的激光雷达的观测点的数量Nf,当Nf增大时,采用贝叶斯概率更新公式对在t-1时刻第l个特征体素中特征可信度进行更新,得到特征可信度当Nf不变时,无需更新;
所述贝叶斯概率更新公式如下:
式中,为在t-1时刻之前基于所有历史观测数据的特征可信度,为t-1时刻三维先验体素子地图中的第l个特征体素,χ1:t-1为1到t-1时刻的智能平台的历史位姿,C1:t-1为智能平台的1到t-1时刻的点云观测数据,为t-1时刻观测到的第l个特征体素中特征可信度,χt-1为t-1时刻智能平台的位姿,Ct-1为智能平台在t-1时刻的点云观测数据,为更新前的特征可信度;
当协方差矩阵的特征值中的λ1≥10λ2且λ1≥10λ3时,该特征体素中的点云呈线特征分布;当特征值中的λ1≥15λ3且λ2≥15λ3时,该特征体素中的点云呈面特征分布;当协方差矩阵的λ1、λ2及λ3在同一数量级时,该特征体素中的点云呈团状特征分布。
进一步的,所述步骤S4的具体步骤如下:
S401,根据GNSS数据获取智能平台所在位置的范围,根据全局拓扑关系检索与当前智能平台距离最近的三维先验体素子地图Sj,将三维先验体素子地图Sj的数据读取到计算平台内存当中;
S403,利用先验路标mk的伪全局位姿与该三维先验体素子地图Sj坐标系的伪全局位姿计算先验路标mk的局部位姿T0,并将该局部位姿T0作为初始数据匹配的启发值;
S404,根据公式(5)和S403中的启发值计算解空间W;
式中,Wx、Wy、Wz分别为三维先验体素子地图Sj坐标系的x,y,z轴的搜索窗口大小,Wγ为航向角搜索窗口、Wθ为横滚角和俯仰角的搜索窗口,r为位置搜索步长,δγ为角度搜索步长;为离散化的总搜索窗口,hx为{-wx,…,wx}中的任一值,hy为{-wy,…,wy}中的任一值,hz为{-wz,…,wz}中的任一值,hα为{-wα,…,wα}中的任一值,hβ为{-wβ,…,wβ}中的任一值,hγ为{-wγ,…,wγ}中的任一值;
S405.使用分支定界法在解空间W中进行位姿求解,得到准确的启发值将准确的启发值替换公式(4)中的Tt,进行三维先验体素子地图Sj与点云观测数据的数据匹配,得到智能平台在三维先验体素子地图Sj坐标系中的局部位姿,即智能平台在三维先验体素子地图Sj中的最优相对位姿。
进一步的,所述步骤S5的具体步骤如下:
S502,利用当前智能平台的局部位姿和所在三维先验体素子地图Sj坐标系的伪全局位姿求取智能平台的伪全局位姿检索与三维先验体素子地图Sj距离最近的先验路标并利用该先验路标校正智能平台的位姿,校正公式如下:
S503,随着智能平台的不断运动,智能平台会从三维先验体素子地图Sj逐渐运动到三维先验体素子地图Sj+1,此时需要进行子地图切换,为了避免智能平台处于子地图边界时产生子地图频繁切换,当智能平台的全局位姿Aj与三维先验体素子地图Sj的GNSS全局位姿的距离大于设定值Tshift,且与三维先验体素子地图Sj+1的GNSS全局位姿的距离小于等于设定值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时刻的三维先验体素子地图i=1,2,3…,当i=1时,将三维先验体素子地图的坐标系的伪全局位姿设为(0,0,0,0,0,0),并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图的GNSS全局位姿;当i≠1时,以三维先验体素子地图中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图坐标系的伪全局位姿和GNSS全局位姿;
S301,记录每一体素中来自不同时刻(即1到t-1时刻)或者不同线束编号的激光雷达的观测点的数量Nf,当Nf增大时,采用贝叶斯概率更新公式对在t-1时刻第l个特征体素中特征可信度进行更新,得到特征可信度当Nf不变时,无需更新;
所述贝叶斯概率更新公式如下:
式中,为在t-1时刻之前(包括t-1时刻)基于所有历史观测数据的特征可信度,为t-1时刻三维先验体素子地图中的第l个特征体素,χ1:t-1为1到t-1时刻的智能平台的历史位姿,C1:t-1为智能平台的1到t-1时刻的点云观测数据,为t-1时刻观测到的第l个特征体素中特征可信度,χt-1为t-1时刻智能平台的位姿,Ct-1为智能平台在t-1时刻的点云观测数据,为更新前的特征可信度;
当协方差矩阵的特征值中的λ1远大于λ2及λ3(即λ1≥10λ2且λ1≥10λ3)时,该特征体素中的点云呈线特征分布;当特征值中的λ1、λ2远大于λ3(即λ1≥15λ3且λ2≥15λ3)时,该特征体素中的点云呈面特征分布;当协方差矩阵的三个特征值无明显差别(即λ1、λ2及λ3在同一数量级)时,该特征体素中的点云呈团状特征分布;
式中,n=0,1,…N,Tt为t时刻的智能平台与三维先验体素子地图之间的相对位姿,Cn为点云观测数据中的任一点,为距离TtCn最近的特征体素中点云分布均值,为距离TtCn最近的特征体素中点云分布特征向量矩阵,为的转置矩阵,为距离TtCn最近的特征体素中点云分布特征值矩阵的逆矩阵,根据步骤S303计算得出,且在步骤S303的计算结果的逆矩阵中,对于线特征,令中的1/λ1=0,对于面特征,令中的1/λ1=0,1/λ2=0后,得到 为点云观测数据中任一点的特征观测误差;e为点云特征观测误差,eT为e的转置矩阵,为点云特征观测误差协方差矩阵的逆矩阵,为点云观测数据匹配的结果,即t时刻点云观测数据相对于三维先验体素子地图的最优(最优即为概率值最大的位姿)相对位姿,即智能平台在三维先验体素子地图坐标系中的局部位姿;
步骤S5,根据数据匹配结果和t时刻的点云观测数据更新三维先验体素子地图得到t时刻的三维先验体素子地图并按照步骤S3的方法统计三维先验体素子地图中每一体素的点云分布特征及特征可信度后,进行存储,以便用于计算t+1时刻的三维先验体素子地图
步骤S6,对智能平台在三维先验体素子地图坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿,将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为一个先验路标将先验路标mt记录到三维先验体素子地图中;
步骤S8,重复步骤S2~S7,完成智能平台运行区域内各个三维先验体素子地图Si的创建,i=1,2,3…,创建结果参见附图2和图3,图2为实际场景图,图3为根据实际场景图创建三维先验体素子地图,从图3中可以看出三维先验体素子地图良好的反映了实际场景图中存在的平面状、线状以及团状物体的特征;
步骤S9,根据步骤S8创建的各个三维先验体素子地图的拓扑连通关系建立全局拓扑关系,得到智能平台运行区域的全局先验地图,该全局先验地图保存有各个三维先验体素子地图的N帧多源先验信息。
创建完包含多源先验信息的全局先验地图后,根据全局先验地图对智能平台进行定位,即根据多源先验信息进行定位,所述定位的具体步骤如下:
步骤S10,将步骤S1的GNSS数据与全局先验地图的多源先验信息进行初始数据匹配,得到智能平台在某一三维先验体素子地图Sj中的相对位姿,具体步骤如下:
S1001,根据GNSS数据获取智能平台所在位置的范围,根据全局拓扑关系检索与当前智能平台距离最近的三维先验体素子地图Sj,将三维先验体素子地图Sj的数据读取到计算平台内存当中,即调用多源先验信息;
S1003,利用先验路标mk的伪全局位姿与该三维先验体素子地图Sj坐标系的伪全局位姿计算先验路标mk的局部位姿T0,并将该局部位姿T0作为初始数据匹配的启发值;
S1004,根据公式(5)和S1003中的启发值(该启发值为局部位姿T0)计算解空间W;
式中,Wx、Wy、Wz分别为三维先验体素子地图Sj坐标系的x,y,z轴的搜索窗口大小,Wγ为航向角搜索窗口、Wθ为横滚角和俯仰角的搜索窗口,r为位置搜索步长,δγ为角度搜索步长;为离散化的总搜索窗口,hx为{-wx,…,wx}中的任一值,hy为{-wy,…,wy}中的任一值,hz为{-wz,…,wz}中的任一值,hα为{-wα,…,wα}中的任一值,hβ为{-wβ,…,wβ}中的任一值,hγ为{-wγ,…,wγ}中的任一值;
S1005.使用分支定界法在解空间W中进行位姿求解,得到准确的启发值将准确的启发值代入公式(4)中(即替换公式(4)中的Tt),进行三维先验体素子地图Sj与点云观测数据的数据匹配,得到初始数据匹配的结果,即智能平台在三维先验体素子地图Sj中的最优相对位姿,也即智能平台在三维先验体素子地图Sj坐标系中的局部位姿;
步骤S11,智能平台运动过程中不断利用当前点云观测数据与三维先验体素子地图Sj进行数据匹配(即进行位姿初匹配),得到智能平台的局部位姿,然后利用距离智能平台最近的先验路标mk校正智能平台的位姿,最终得到精确稳定的智能平台位姿,此过程不再需要GNSS数据,具体步骤如下:
S1102,利用当前智能平台的局部位姿和所在三维先验体素子地图Sj坐标系的伪全局位姿求取智能平台的伪全局位姿检索与三维先验体素子地图Sj距离最近的先验路标并利用该先验路标校正智能平台的位姿,校正公式如下:
S1103,随着智能平台的不断运动,智能平台会从三维先验体素子地图Sj逐渐运动到三维先验体素子地图Sj+1,此时需要进行子地图切换,为了避免智能平台处于子地图边界时产生子地图频繁切换,当智能平台的全局位姿Aj与三维先验体素子地图Sj的GNSS全局位姿的距离大于设定值Tshift,且与三维先验体素子地图Sj+1的GNSS全局位姿的距离小于等于设定值Tshift时,进行子地图的切换,即将三维先验体素子地图Sj切换为三维先验体素子地图Sj+1;完成子地图切换后,重复S1101~S1102,即继续利用点云观测数据与智能平台所在三维先验体素子地图Sj+1进行匹配定位,最终得到精确稳定的智能平台的位姿;
最终,对采用多源先验信息定位智能平台的位姿进行误差分析,令智能平台开始运动处为原点,令在水平面上的经度方向(即东西向)为X轴方向,令在水平面上的纬度方向(即南北向)为Y轴方向,参见附图4可知,智能平台在X轴方向的误差,最大误差为3.55m,参见附图5可知,智能平台在Y轴方向的误差,最大误差为4.62m;且表1是本实施例定位方法的定位精度统计表,从表1中可以看出,多源先验信息定位X轴位置以及Y轴位置的定位精度均在内,可以满足智能平台常规定位的需求。
表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时刻的三维先验体素子地图当i=1时,将三维先验体素子地图S1的坐标系的伪全局位姿设为(0,0,0,0,0,0),并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图的GNSS全局位姿;当i≠1时,以三维先验体素子地图中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图坐标系的伪全局位姿和GNSS全局位姿;
式中,n=0,1,…N,Tt为t时刻的智能平台与三维先验体素子地图之间的相对位姿,Cn为点云观测数据中的任一点,为距离TtCn最近的特征体素中点云分布均值,为距离TtCn最近的特征体素中点云分布特征向量矩阵,为的转置矩阵,为距离TtCn最近的特征体素中点云分布特征值矩阵的逆矩阵,对于线特征,令中的1/λ1=0,对于面特征,令中的1/λ1=0,1/λ2=0;为点云观测数据中任一点的特征观测误差;e为点云特征观测误差,eT为e的转置矩阵,为点云特征观测误差协方差矩阵的逆矩阵,为智能平台在三维先验体素子地图坐标系中的局部位姿,即t时刻点云观测数据相对于三维先验体素子地图的最优相对位姿;
步骤S205,对智能平台在三维先验体素子地图坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿,将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为第一个先验路标将先验路标mt记录到三维先验体素子地图中;
步骤S207,重复步骤S201~S206,完成智能平台运行区域内各个三维先验体素子地图Si的创建,i=1,2,3...。
S2021,记录每一体素中来自不同时刻或者不同线束编号的激光雷达的观测点的数量Nf,当Nf增大时,采用贝叶斯概率更新公式对在t-1时刻第l个特征体素中特征可信度进行更新,得到特征可信度当Nf不变时,无需更新;
所述贝叶斯概率更新公式如下:
式中,为在t-1时刻之前基于所有历史观测数据的特征可信度,为t-1时刻三维先验体素子地图中的第l个特征体素,χ1:t-1为1到t-1时刻的智能平台的历史位姿,C1:t-1为智能平台的1到t-1时刻的点云观测数据,为t-1时刻观测到的第l个特征体素中特征可信度,χt-1为t-1时刻智能平台的位姿,Ct-1为智能平台在t-1时刻的点云观测数据,为更新前的特征可信度;
当协方差矩阵的特征值中的λ1≥10λ2且λ1≥10λ3时,该特征体素中的点云呈线特征分布;当特征值中的λ1≥15λ3且λ2≥15λ3时,该特征体素中的点云呈面特征分布;当协方差矩阵的λ1、λ2及λ3在同一数量级时,该特征体素中的点云呈团状特征分布。
4.如权利要求2所述的一种基于多源先验信息的点云匹配高精度定位方法,其特征在于,所述步骤S4的具体步骤如下:
S401,根据GNSS数据获取智能平台所在位置的范围,根据全局拓扑关系检索与当前智能平台距离最近的三维先验体素子地图Sj,将三维先验体素子地图Sj的数据读取到计算平台内存当中;
S403,利用先验路标mk的伪全局位姿与该三维先验体素子地图Sj坐标系的伪全局位姿计算先验路标mk的局部位姿T0,并将该局部位姿T0作为初始数据匹配的启发值;
S404,根据公式(5)和S403中的启发值计算解空间W;
式中,Wx、Wy、Wz分别为三维先验体素子地图Sj坐标系的x,y,z轴的搜索窗口大小,Wγ为航向角搜索窗口、Wθ为横滚角和俯仰角的搜索窗口,r为位置搜索步长,δγ为角度搜索步长;为离散化的总搜索窗口,hx为{-wx,…,wx}中的任一值,hy为{-wy,…,wy}中的任一值,hz为{-wz,…,wz}中的任一值,hα为{-wα,…,wα}中的任一值,hβ为{-wβ,…,wβ}中的任一值,hγ为{-wγ,…,wγ}中的任一值;
5.如权利要求2所述的一种基于多源先验信息的点云匹配高精度定位方法,其特征在于,所述步骤S5的具体步骤如下:
S502,利用当前智能平台的局部位姿和所在三维先验体素子地图Sj坐标系的伪全局位姿求取智能平台的伪全局位姿检索与三维先验体素子地图Sj距离最近的先验路标并利用该先验路标校正智能平台的位姿,校正公式如下:
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)
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)
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)
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 |
-
2020
- 2020-04-16 CN CN202010298730.XA patent/CN111735451B/zh active Active
Patent Citations (15)
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)
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 |