CN115616641B - 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法 - Google Patents
一种基于粒子滤波的城市峡谷中组合导航高精度定位方法 Download PDFInfo
- Publication number
- CN115616641B CN115616641B CN202211620822.0A CN202211620822A CN115616641B CN 115616641 B CN115616641 B CN 115616641B CN 202211620822 A CN202211620822 A CN 202211620822A CN 115616641 B CN115616641 B CN 115616641B
- Authority
- CN
- China
- Prior art keywords
- grid
- data
- gnss
- correction model
- imu
- 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 69
- 239000002245 particle Substances 0.000 title claims abstract description 63
- 238000012937 correction Methods 0.000 claims abstract description 91
- 238000001914 filtration Methods 0.000 claims abstract description 45
- 238000012549 training Methods 0.000 claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 32
- 238000004422 calculation algorithm Methods 0.000 claims description 14
- 238000005070 sampling Methods 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 11
- 238000012952 Resampling Methods 0.000 claims description 9
- 238000010586 diagram Methods 0.000 claims description 9
- 230000000694 effects Effects 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 7
- 238000013519 translation Methods 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 241000287196 Asthenes Species 0.000 claims description 3
- 238000010801 machine learning Methods 0.000 claims description 3
- 238000012986 modification Methods 0.000 claims description 3
- 230000004048 modification Effects 0.000 claims description 3
- 238000007670 refining Methods 0.000 claims description 3
- 206010034719 Personality change Diseases 0.000 claims description 2
- 230000001186 cumulative effect Effects 0.000 claims 1
- 238000010606 normalization Methods 0.000 claims 1
- 238000001514 detection method Methods 0.000 abstract description 4
- 238000012545 processing Methods 0.000 description 7
- 239000000243 solution Substances 0.000 description 7
- 230000008901 benefit Effects 0.000 description 4
- 238000004590 computer program Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 238000009499 grossing Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000012088 reference solution Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- 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
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本发明提出了一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,包括:构建并训练格网修正模型用于伪距修正;获取GNSS接收机的输出数据,计算待定位车辆位置,确定格网修正模型的类型;利用LIDAR获取点云数据,进行实时地图构建,确定待定位车辆周围的环境,确定格网修正模型的类型;对GNSS接收机测得的位置进行伪距修正;对GNSS位置数据和IMU位姿数据组合计算后得到GNSS与IMU组合导航位姿数据;利用点云数据,求解位姿;对LIDAR位姿数据和GNSS与IMU组合导航位姿数据进行融合,完成基于粒子滤波的城市峡谷中组合导航高精度定位。
Description
技术领域
本发明涉及一种组合导航高精度定位方法,特别是一种基于粒子滤波的城市峡谷中组合导航高精度定位方法。
背景技术
随着无人驾驶研究的火热进行,对高精度定位结果的需求愈发迫切。由于城市环境下对卫星信号的遮挡,单一的卫星导航无法获得满意的定位结果。GNSS/IMU是一种组合导航设备,通过观测GNSS(全球导航卫星***,Global Navigation Satellite System,GNSS)卫星信号,并与惯性导航***输出的比力和角速度相组合,为各种载体提供精确、可靠的位置信息。很多学者对其进行研究,提出很多方法。如使用基于模糊逻辑的技术来提高集成***RTK/MEMS-INS(INS,Inertial Navigation System,惯性导航***)在陆地车辆导航中的准确性;采用RTS(实时差分定位,real-time kinematic,RTS)(Rauch-Tung-Striebel)平滑或者双向平滑技术,减少GNSS信号缺失下导航误差;将卡尔曼滤波用于码跟踪环路,根据信号噪声、INS误差等信息,自适应的改变码环跟踪特性,提高***抗干扰性。这些算法在一定程度上提高了定位的精度,但是当处于城市环境时,卫星信号会被遮挡,同时这些算法无法处理动态障碍物对车辆定位的影响,GNSS/IMU组合导航定位(IMU,惯性测量单元,Inertial Measurement Unit))精度退化至分米甚至米级精度,无法满足需求。
LIDAR(激光探测及测距,Light Detection and Ranging,LIDAR)是激光探测与测距技术,通过在两帧点云数据间进行基于特征的匹配来获得两个时刻之间位姿的变化量。城市环境中有丰富的点特征和面特征,可以保证LIDAR的匹配正确率。LIDAR和GNSS/IMU多传感器进行融合,优劣互补,可获得更高精度的定位结果。有学者提出了将GNSS/INS的导航结果与激光雷达特征观测值融合在EKF-SLAM框架中的方法,相比紧组合模型,改善了短时相对定位精度。或者以激光雷达里程计状态信息为待优化量,通过GNSS/IMU组合导航输出值和二者之间观测方程,对里程计状态量进行优化的图优化模型。或者为了有效补偿标定过程中的LiDAR运动失真,利用IMU预积分的LiDAR/IMU外参标定算法。对GNSS/IMU/LIDAR组合导航定位算法进行研究,对于城市环境无人移动平台的智能化具有重要意义。
多路径效应是影响城市GNSS定位精度的主要因素,对其进行3D建模是一个解决思路。可以使用建筑的三维表面模型的多路径误差模拟值,计算的点定位解和使用粒子滤波假设的多个用户粒子位置,来估计用户的位置,以纠正多路径误差;或利用三维地图和光线追踪法估计行人位置的方法。在一个参考位置周围分配候选位置的数量,根据模拟的伪距和观测到的伪距之间的相似性来评估候选位置的权重。格网模型是解决城市多路径效应对定位影响的思路,利用卫星-城市环境下的时空重复性,通过GNSS观测数据的多日采集与城市路段网格划分,构建城市区域环境热力图与GNSS伪距误差修正格网模型,优化城市复杂环境下的GNSS定位性能,但无法处理城市中高频出现的动态障碍物带来的影响。使用LIDAR可以实时判断当前车辆所处的环境,将格网模型再细分为空旷型格网模型和包围型格网模型,来减弱动态障碍物,即车辆周围其他车辆造成的影响。
为了实现多源融合,需要使用合适的数据处理算法。有学者提出了基于无迹卡尔曼滤波器(Unscented KalmanFilter,UKF)的RTK与低成本IMU融合的方法;或将卡尔曼滤波中状态量分为可观测与不可观测状态量,滤波过程不对不可观测部分进行更新,提高了组合导航***的适用性;有学者分析了集中、分数和联邦卡尔曼滤波各自的优缺点,提出了一种松组合、紧组合相结合的联邦滤波,降低***运算量。通过集中式卡尔曼滤波可以实现将三个传感器的数据集中在一个卡尔曼滤波器进行处理,但容错能力差,滤波器结构可扩展性差,不能发挥多传感器融合的优势。而且,GNSS/IMU组合导航作为商业导航***配有成熟的数据处理软件,如果在GNSS/IMU数据处理的过程中融合激光雷达观测数据,需要对***进行二次开发,增加实现难度和成本。使用卡尔曼滤波可以融合GNSS/IMU组合导航的位姿与LIDAR的位姿,但是卡尔曼滤波只适用于线性***,对非线性***的滤波效果较差。扩展卡尔曼通过取泰勒级数展开后的一次项拟合实际曲线,对强非线性情况适用性不好。
综上所述,现有技术存在以下缺陷:
a)现有的城市定位下的格网模型只是单独使用GNSS数据的算法,所以只能根据城市和卫星的时空同步性对卫星伪距进行修正。在城市环境对伪距产生的多路径影响中,一方面是静态的建筑物的影响,一方面是车辆动态实时环境的影响。单独的GNSS导航方法只能修正建筑物的影响,无法修正动态障碍物对GNSS定位的影响。
b)集中式卡尔曼滤波处理GNSS/IMU/LIDAR多传感融合数据,将三个传感器数据集中到一个卡尔曼滤波器解算,状态方程复杂,容错性差。且现有的GNSS/IMU组合导航***已发展较为成熟,如完全构造一个新的滤波器,成本会大幅增加。对于将GNSS/IMU组合导航获得的位姿与LIDAR获得的位姿进行组合的方法,受到卡尔曼滤波和拓展卡尔曼滤波原理的制约,当***为强非线性情况时,将产生较大的误差,适用性不好,会降低最终定位精度。
发明内容
发明目的:本发明所要解决的技术问题是针对现有技术的不足,提供一种基于粒子滤波的城市峡谷中组合导航高精度定位方法。
为了解决上述技术问题,本发明公开了一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,包括以下步骤:
步骤1,构建格网修正模型用于伪距修正,并训练所述格网修正模型;所述格网修正模型包括:普通格网修正模型和精细化格网修正模型;
步骤2,获取GNSS接收机的输出数据,计算待定位车辆位置,根据所述位置确定格网修正模型的类型;
步骤3,利用LIDAR实时测量获取点云数据,进行实时地图构建,确定待定位车辆周围的环境,根据所述环境确定格网修正模型的类型;
步骤4,根据步骤2和步骤3所确定的格网修正模型的类型对GNSS接收机测得的位置进行伪距修正,得到GNSS位置数据;对从IMU获取的数据进行力学编排后求解得到IMU位姿数据;对GNSS位置数据和IMU位姿数据组合计算后得到GNSS与IMU组合导航位姿数据;
步骤5,利用LIDAR获得的点云数据,求解待定位车辆的位姿,得到LIDAR位姿数据;
步骤6,对LIDAR位姿数据和GNSS与IMU组合导航位姿数据进行融合,得到最终定位结果,完成基于粒子滤波的城市峡谷中组合导航高精度定位。
有益效果:
a)针对传统的格网模型只能修正,建筑物等静态障碍物对GNSS卫星伪距造成的多路径效应误差,无法修正动态障碍物带来的影响,本发明引入LIDAR作为辅助传感器,能够实时检测车辆所处的动态环境,调用更细致的空旷型格网模型或包围型格网模型,部分修正动态障碍物的影响。
b)针对使用集中式卡尔曼滤波处理GNSS/IMU/LIDAR组合导航存在数学公式复杂,容错性差的问题,本发明采用利用已有的GNSS/IMU组合导航的位姿和LIDAR得到的载***姿再进行滤波的方式。为了解决使用卡尔曼滤波和拓展卡尔曼滤波融合GNSS/IMU组合导航结果和LIDAR获得的位姿存在的,***为强非线性***,拟合结果不理想的问题,本发明使用粒子集表示概率的粒子滤波,可以用在任何形式的状态空间模型上,解决了非线性问题。
附图说明
下面结合附图和具体实施方式对本发明做更进一步的具体说明,本发明的上述和/或其他方面的优点将会变得更加清楚。
图1为本发明流程示意图。
图2为网格确定方法示意图
图3为空旷型与包围型示意图。
具体实施方式
本发明核心内容为一种基于粒子滤波的城市峡谷GNSS/IMU/LIDAR组合导航高精度定位方法。其中,待定位车辆中选择的传感器为GNSS(全球导航卫星***,GlobalNavigation Satellite System,GNSS)接收机(可接收北斗和GPS信号),IMU(惯性测量单元,Inertial Measurement Unit,IMU)和LIDAR(激光探测及测距,Light Detection andRanging,LIDAR)。一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,如图1所示,包括以下步骤:
步骤1,构建格网修正模型用于伪距修正,并训练所述格网修正模型;所述格网修正模型包括:普通格网修正模型和精细化格网修正模型;
所述的构建格网修正模型,具体方法包括:
根据卫星和城市环境的重复性利用机器学习算法学习GNSS原始观测量与伪距误差间的规则,建立伪距误差修正模型即格网修正模型;通过输入新观测到的GNSS原始观测量数据修正当前时刻卫星伪距;建立区域环境热力图,将城市路网以六边形网格进行划分,调整区域环境热力图的空间分辨率;根据设置的网格大小,网格修正模型分为普通格网修正模型和精细化格网修正模型;确定网格后,按照已划分的网格确定每个训练样本的所属网格,最后按照网格对应的数据逐个训练对应的格网修正模型;
训练格网修正模型前,根据车辆周围环境判断方法,将训练数据划分为包围型车辆数据和空旷型车辆数据,即所有的数据分为普通格网包围型数据,普通格网空旷型数据,精细格网包围型数据以及精细格网空旷型数据;使用普通格网包围型数据和普通格网空旷型数据训练普通格网修正模型,得到普通格网下的包围型修正模型和普通格网下的空旷型修正模型;使用精细格网包围型数据和精细格网空旷型数据训练精细化格网修正模型,得到精细格网下的包围型修正模型和精细格网下的空旷型修正模型。
所述的车辆周围环境判断方法,即判断车辆周围环境,是包围型还是空旷型,具体方法包括:
当目标车辆处于以自身的尺度构建的九宫格的中心时,测量其他车辆最靠近目标车辆的距离S,若S<r,则周围车辆数量+1,表示其他车辆对目标车辆产生多路径效应影响,其中,r为预设的划定范围的半径;当周围车辆数量小于等于3时将车辆周围环境定义为空旷型,大于等于4时定义为包围型。
步骤2,获取GNSS接收机的输出数据,计算待定位车辆位置,根据所述位置确定格网修正模型的类型;所述的GNSS接收机的输出数据,包括:卫星星历,伪距观测值以及多普勒观测值;
所述确定格网修正模型的类型,具体方法如下:
设六边形网格的中心点为O,参考解位置点即待定位车辆位置为K,判定K是否位于以o为中心点的六边形内,判断步骤如下:x轴与y轴分别对应ENU坐标系下的E轴与N轴;选取待定位车辆位置周围的三个六边形网格,所述六边形网格的中心点坐标分别为(x1,y1)、(x2,y2)和(x3,y3),K点的坐标为(xk,yk),计算待定位车辆位置到三个六边形网格中心点的距离di,方法如下:
其中,l为六边形网格编号;选取最小的距离,确定待定位车辆位置所属的网格;
确定所属的网格后,即根据网格类型确定格网修正模型的类型。
步骤3,利用LIDAR实时测量获取点云数据,进行实时地图构建,确定待定位车辆周围的环境,根据所述环境确定格网修正模型的类型;所述的确定待定位车辆周围的环境的方法与步骤1中所述的车辆周围环境判断方法相同;根据包围类型选择采用普通格网下的包围型修正模型、普通格网下的空旷型修正模型、精细格网下的包围型修正模型或精细格网下的空旷型修正模型对伪距进行修正。
步骤4,根据步骤2和步骤3所确定的格网修正模型的类型对GNSS接收机测得的位置进行伪距修正,得到GNSS位置数据;对从IMU获取的数据进行力学编排后求解得到IMU位姿数据;对GNSS位置数据和IMU位姿数据组合计算后得到GNSS与IMU组合导航位姿数据,具体方法包括:
使用卡尔曼滤波对GNSS位置数据和IMU位姿数据进行松组合滤波,构建输入滤波器的21维***误差状态如下:
其中,及分别表示地心地固系下的惯导三维位置误差向量、速度误差向量和姿态误差向量;bg、ba、sg及sa分别表示陀螺仪三轴零偏、加速度计三轴零偏、陀螺仪三轴比例因子及加速度计三轴比例因子误差向量;零偏及比例因子误差建模为一阶马尔科夫过程;上标T表示对向量或矩阵进行转置;
GNSS与IMU离散时间卡尔曼滤波***误差状态方程:
xk+1=Φk+1,kxk+wk
观测方程为用惯性导航***INS推算的GNSS天线三维位置和GNSS解算得到的三维位置
作差,构建三维位置误差观测向量zp如下:
Xk=Xk|k-1+K(Zk-HXk|k-1)
其中,K为卡尔曼增益矩阵;H为观测矩阵;
经过上述步骤,用误差修正惯导预测值后,得到GNSS与IMU组合导航位姿数据。
步骤5,利用LIDAR获得的点云数据,求解待定位车辆的位姿,得到LIDAR位姿数据,具体方法如下:
利用LIDAR获得的点云数据,采用迭代最近点匹配算法进行点云配准,求解两帧点云间的位姿变化;设两帧点云M和N中各包含m和n个坐标点,将M和N中的点进行匹配,找到所有满足欧式距离最小的相邻点对(ma,na),计算使误差函数f(R,T)最小的旋转矩阵R和平移向量T:
使用奇异值分解求解R和T:
利用第一次求出的R和T对点云N进行旋转和平移变换,得到二次点云N1,将N1再次与M点云进行匹配,求解二次旋转矩阵R1和平移向量T1;上述过程反复执行迭代,直到满足阈值或者完成预设迭代次数,则求得最优旋转矩阵R和平移向量T;已知上一时刻的位置xt和姿态φt,得到当前的LIDAR位姿数据即当前时刻的位置xt+1和姿态φt+1如下:
其中,Δx表示位置变化量,Δφ表示姿态变化量。
步骤6,对LIDAR位姿数据和GNSS与IMU组合导航位姿数据进行融合,得到最终定位结果,完成基于粒子滤波的城市峡谷中组合导航高精度定位。
所述的对LIDAR位姿数据和GNSS与IMU组合导航位姿数据进行融合,采用粒子滤波进行融合,具体方法包括:
构建输入滤波器的6维状态向量为
x=[(δr)T (δφ)T]
其中,δr为GNSS与IMU组合导航位姿数据的位置误差,δφ为GNSS与MU组合导航位姿数据经过修正后的IMU姿态误差;
构建观测方程为:
其中,rLIDAR为LIDAR测得的位置,rZHUHE为GNSS与IMU组合导航位姿数据的位置,φLIDAR为LIDAR测得的姿态,φZHUHE为GNSS与IMU组合导航位姿数据的姿态;
状态方程和观测方程如下:
X(k+1)=ΦX(k)+ΓW(k)
Z(k)=X(k)+v(k)
其中,X(k+1)表示下一时刻状态,Φ表示状态转移矩阵,X(k)表示当前状态,Γ表示误差系数矩阵,W(k)表示状态误差,Z(k)表示当前观测值,v(k)表示观测误差;
所述粒子滤波包括:初始化、重要性采样和重采样3步,具体为:
第一步:初始化:
第二步:重要性采样:
式中:ρ为观测误差;归一化权值:
第三步:重采样:
在t时刻产生[0,1]区间内均匀分布的随机数序列:Ui=U[0,1],i=1,2,...,N;按照下式:
计算此时所有状态向量粒子的平均值,得到位置误差修正值Δpst和姿态误差修正值Δlat;
再用GNSS与IMU组合导航得到的位置姿态数据分别减去Δpst和Δlat,得到最终定位结果。
步骤1中所述的格网修正模型中样本的空间分辨率设定在25m以内;所述的精细化格网六边形边长为10m。
步骤6中所述的粒子滤波中采用的粒子个数N为100。
实施例:
一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,具体步骤如下:
1)首先,离线部分,需要提前训练用于GNSS伪距修正的格网修正模型,在进行数据采集后,根据步骤3)中的分类方法,判断车辆属于包围型还是空旷型。在训练格网修正模型之前,就将数据先划分为包围型车辆数据,和空旷型车辆数据。即所有的数据分为普通格网包围型数据,普通格网空旷型数据,精细格网包围型数据,精细格网空旷型数据,再进行修正模型的构建。
伪距修正模型的构建是根据卫星-城市环境的重复性利用机器学习算法学习GNSS原始观测量与伪距误差间潜在规则并建立对应的伪距误差修正模型,通过输入新观测到的GNSS原始观测量数据修正当前时刻卫星伪距。区域环境热力图将城市路网以六边形网格进行划分,网格大小可按照环境和需求更改,调整区域环境热力图的空间分辨率,本发明建模样本的空间分辨率设定在25m以内。在多路径效应严重区域,即各卫星的伪距误差均方根值较大的区域,对区域环境热力图进行细化,即减小格网大小。本发明选定的精细化格网六边形边长为10m。确定网格后,按照已划分的网格确定每个训练样本的所属网格,最后按照网格对应的数据逐个训练对应的伪距误差修正模型。
2)在线部分,利用GNSS接收机输出的卫星星历,伪距观测值,多普勒观测值,计算车辆的位置,设六边形网格的中心点为O,参考解位置点为K,判定K是否位于以o为中心点的六边形。
判断步骤如下,x轴与y轴分别对应ENU坐标系下的E轴与N轴
网格中心点的坐标在离线建模过程中已知,即(x1,y1),(x2,y2),(x3,y3)已知。又有K点的坐标,通过计算
选取最小的距离d,确定位置点所属的网格。确定所属网格后,即可确定调用普通格网修正模型还是精细化格网修正模型。
3)下一步利用LIDAR实时测量的点云数据,进行实时地图构建,确定车辆周围的环境,是包围型还是空旷型。当车辆处于以车辆自身的尺度构建的九宫格的中心时,测量其他车辆最靠近目标车辆的一端的距离S,如图3所示,若s<r,则统计为1,视为对目标车辆产生多路径效应影响,r为划定范围的半径。阈值根据实际情况选取,本发明选择1.5倍车长。当周围车辆数量小于等于3时为空旷型,大于等于4时为包围型。之后根据包围类型选择普通格网或精细格网下的空旷型修正模型或包围型修正模型对伪距进行修正。
4)下一步,使用标准单点定位法利用修正后的伪距得到GNSS测得的位置,对IMU进行力学编排后求解得到惯性导航测得的位置和姿态,本发明使用卡尔曼滤波对GNSS和IMU的数据进行松组合滤波,构建输入滤波器的21维***误差状态如下:
其中,及分别表示e系下的惯导三维位置误差向量、速度误差向量和姿态误差向量:bg、ba、sg及sa分别表示陀螺仪三轴零偏、加速度计三轴零偏、陀螺仪三轴比例因子及加速度计三轴比例因子误差向量。零偏及比例因子误差建模为一阶马尔科夫过程。上标T表示对向量或矩阵进行转置。
GNSS/IMU离散时间卡尔曼滤波***误差状态方程:
xk+1=Φk+1,kxk+wk
观测方程为用INS推算的GNSS天线三维位置和GNSS解算得到的三维位置
作差,构建三维位置误差观测向量如下:
可以得到执行测量更新后k历元状态向量的估计值
Xk=Xk|k-1+K(Zk-HXk|k-1)
其中,K为卡尔曼增益矩阵;H为观测矩阵。用误差修正惯导预测值后,得到GNSS/IMU组合导航输出的位置和姿态。
5)下一步利用LIDAR获得的点云数据,求解车辆的位姿。本发明采用迭代最近点匹配算法(Iterative Closest Point,ICP)来进行点云配准,求解两帧点云间的位姿变化。当初始值质量较好时,其解算精度较高。两帧点云M和N中各包含m和n个坐标点,将M和N中的点进行匹配,找到所有满足欧式距离最小的相邻点对(mi,ni),计算使其误差函数最小的旋转矩阵R和平移向量T:
使用SVD分解(SVD,Singular Value Decomposition,奇异值分解)求解R和T。
第一步:将点集m、n中的点均减去自身的质心,两组点集的质心分别为
两组点集去掉质心后的坐标为:
第二步:求去中心后两点集的协方差矩阵:
第三步:计算旋转矩阵R:
对W进行SVD分解
W=U∑VT
当W满秩的时候,有唯一解
R=UVT
第四步:计算平移矢量:
T=um-Run
之后利用第一次求出的R和T对点云N进行旋转和平移变换,得到二次点云N1,将N1再次与M点云进行匹配,求解二次旋转矩阵R1和平移向量T1。整个过程多次迭代,直到满足一定阈值或者完成一定迭代次数,则求得最优旋转矩阵R和平移向量T。本发明迭代数取5次。已知上一时刻的位置姿态xt,φt,可得此时LIDAR输出的位置姿态:
6)接下来对LIDAR的位姿和GNSS/IMU组合导航位姿进行融合,本发明采用粒子滤波进行融合。
构建输入滤波器的6维状态向量为
x=[(δr)T (δφ)T]
其中δr为GNSS和IMU组合后的位置误差,δφ为GNSS/IMU组合修正后的IMU姿态误差。
构建观测方程为
其中rLIDAR为LIDAR测得的位置,rZHUHE为GNSS/IMU组合导航测得的位置,φLIDAR为LIDAR测得的姿态,φZHUHE为GNSS/IMU组合导航测得的姿态。
状态方程和观测方程如下:
X(k+1)=ΦX(k)+ΓW(k)
Z(k)=X(k)+v(k)
粒子滤波主要包括初始化、重要性采样、重采样3步。具体算法为:
第一步:初始化
第二步:重要性采样
对于i=1,2,...,N,采用高斯概率密度函数计算粒子的权值:
归一化权值:
第三步:重采样
在t时刻产生[0,1]区间内均匀分布的随机数序列:Ui=U[0,1],i=1,2,...,N;按照下式
计算此时所有状态向量粒子的平均值,得到位置误差修正值Δpst和姿态误差修正值Δlat;在用GNSS与IMU组合导航得到的位置姿态数据分别减去Δpst和Δlat,得到最终定位结果。
本发明采用的N为100。使用粒子滤波输出的位置和姿态的修正量对GNSS/IMU组合导航输出的位置和姿态进行修正后,得到最终的城市峡谷下GNSS/IMU/LIDAR组合导航高精度定位结果。
具体实现中,本申请提供计算机存储介质以及对应的数据处理单元,其中,该计算机存储介质能够存储计算机程序,所述计算机程序通过数据处理单元执行时可运行本发明提供的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法的发明内容以及各实施例中的部分或全部步骤。所述的存储介质可为磁碟、光盘、只读存储记忆体(read-onlymemory,ROM)或随机存储记忆体(random access memory,RAM)等。
本领域的技术人员可以清楚地了解到本发明实施例中的技术方案可借助计算机程序以及其对应的通用硬件平台的方式来实现。基于这样的理解,本发明实施例中的技术方案本质上或者说对现有技术做出贡献的部分可以以计算机程序即软件产品的形式体现出来,该计算机程序软件产品可以存储在存储介质中,包括若干指令用以使得一台包含数据处理单元的设备(可以是个人计算机,服务器,单片机,MUU或者网络设备等)执行本发明各个实施例或者实施例的某些部分所述的方法。
本发明提供了一种基于粒子滤波的城市峡谷中组合导航高精度定位方法的思路及方法,具体实现该技术方案的方法和途径很多,以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。
Claims (9)
1.一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,包括以下步骤:
步骤1,构建格网修正模型用于伪距修正,并训练所述格网修正模型;所述格网修正模型包括:普通格网修正模型和精细化格网修正模型;
步骤2,获取GNSS接收机的输出数据,计算待定位车辆位置,根据所述位置确定格网修正模型的类型;
步骤3,利用LIDAR实时测量获取点云数据,进行实时地图构建,确定待定位车辆周围的环境,根据所述环境确定格网修正模型的类型;
步骤4,根据步骤2和步骤3所确定的格网修正模型的类型对GNSS接收机测得的位置进行伪距修正,得到GNSS位置数据;对从IMU获取的数据进行力学编排后求解得到IMU位姿数据;对GNSS位置数据和IMU位姿数据组合计算后得到GNSS与IMU组合导航位姿数据;
步骤5,利用LIDAR获得的点云数据,求解待定位车辆的位姿,得到LIDAR位姿数据;
步骤6,对LIDAR位姿数据和GNSS与IMU组合导航位姿数据进行融合,得到最终定位结果,完成基于粒子滤波的城市峡谷中组合导航高精度定位;
步骤1中所述的构建格网修正模型,具体方法包括:
根据卫星和城市环境的重复性利用机器学习算法学习GNSS原始观测量与伪距误差间的规则,建立伪距误差修正模型即格网修正模型;通过输入新观测到的GNSS原始观测量数据修正当前时刻卫星伪距;建立区域环境热力图,将城市路网以六边形网格进行划分,调整区域环境热力图的空间分辨率;根据设置的网格大小,网格修正模型分为普通格网修正模型和精细化格网修正模型;确定网格后,按照已划分的网格确定每个训练样本的所属网格,最后按照网格对应的数据逐个训练对应的格网修正模型;
训练格网修正模型前,根据车辆周围环境判断方法,将训练数据划分为包围型车辆数据和空旷型车辆数据,即所有的数据分为普通格网包围型数据,普通格网空旷型数据,精细格网包围型数据以及精细格网空旷型数据;使用普通格网包围型数据和普通格网空旷型数据训练普通格网修正模型,得到普通格网下的包围型修正模型和普通格网下的空旷型修正模型;使用精细格网包围型数据和精细格网空旷型数据训练精细化格网修正模型,得到精细格网下的包围型修正模型和精细格网下的空旷型修正模型。
2.根据权利要求1所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤1中所述的车辆周围环境判断方法,即判断车辆周围环境,是包围型还是空旷型,具体方法包括:
当目标车辆处于以自身的尺度构建的九宫格的中心时,测量其他车辆最靠近目标车辆的距离S,若S<r,则周围车辆数量+1,表示其他车辆对目标车辆产生多路径效应影响,其中,r为预设的划定范围的半径;当周围车辆数量小于等于3时将车辆周围环境定义为空旷型,大于等于4时定义为包围型。
3.根据权利要求2所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤2中所述的GNSS接收机的输出数据,包括:卫星星历,伪距观测值以及多普勒观测值;
所述确定格网修正模型的类型,具体方法如下:
设六边形网格的中心点为O,参考解位置点即待定位车辆位置为K,判定K是否位于以o为中心点的六边形内,判断步骤如下:x轴与y轴分别对应ENU坐标系下的E轴与N轴;选取待定位车辆位置周围的三个六边形网格,所述六边形网格的中心点坐标分别为(x1,y1)、(x2,y2)和(x3,y3),K点的坐标为(xk,yk),计算待定位车辆位置到三个六边形网格中心点的距离di,方法如下:
其中,l为六边形网格编号;选取最小的距离,确定待定位车辆位置所属的网格;
确定所属的网格后,即根据网格类型确定格网修正模型的类型。
4.根据权利要求3所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤3中所述的确定待定位车辆周围的环境的方法与步骤1中所述的车辆周围环境判断方法相同;根据包围类型选择采用普通格网下的包围型修正模型、普通格网下的空旷型修正模型、精细格网下的包围型修正模型或精细格网下的空旷型修正模型对伪距进行修正。
5.根据权利要求4所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤4中所述的对GNSS位置数据和IMU位姿数据组合计算后得到GNSS与IMU组合导航位姿数据,具体方法包括:
使用卡尔曼滤波对GNSS位置数据和IMU位姿数据进行松组合滤波,构建输入滤波器的21维***误差状态如下:
其中,及分别表示地心地固系下的惯导三维位置误差向量、速度误差向量和姿态误差向量;bg、ba、sg及sa分别表示陀螺仪三轴零偏、加速度计三轴零偏、陀螺仪三轴比例因子及加速度计三轴比例因子误差向量;零偏及比例因子误差建模为一阶马尔科夫过程;上标T表示对向量或矩阵进行转置;
GNSS与IMU离散时间卡尔曼滤波***误差状态方程:
xk+1=Φk+1,kxk+wk
观测方程为用惯性导航***INS推算的GNSS天线三维位置和GNSS解算得到的三维位置
作差,构建三维位置误差观测向量zp如下:
Xk=Xk|k-1+K(Zk-HXk|k-1)
其中,K为卡尔曼增益矩阵;H为观测矩阵;
经过上述步骤,用误差修正惯导预测值后,得到GNSS与IMU组合导航位姿数据。
6.根据权利要求5所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤5中所述的得到LIDAR位姿数据的方法如下:
利用LIDAR获得的点云数据,采用迭代最近点匹配算法进行点云配准,求解两帧点云间的位姿变化;设两帧点云M和N中各包含m和n个坐标点,将M和N中的点进行匹配,找到所有满足欧式距离最小的相邻点对(ma,na),计算使误差函数f(R,T)最小的旋转矩阵R和平移向量T:
使用奇异值分解求解R和T:
利用第一次求出的R和T对点云N进行旋转和平移变换,得到二次点云N1,将N1再次与M点云进行匹配,求解二次旋转矩阵R1和平移向量T1;上述过程反复执行迭代,直到满足阈值或者完成预设迭代次数,则求得最优旋转矩阵R和平移向量T;已知上一时刻的位置xt和姿态φt,得到当前的LIDAR位姿数据即当前时刻的位置xt+1和姿态φt+1如下:
其中,Δx表示位置变化量,Δφ表示姿态变化量。
7.根据权利要求6所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤6中所述的对LIDAR位姿数据和GNSS与IMU组合导航位姿数据进行融合,采用粒子滤波进行融合,具体方法包括:
构建输入滤波器的6维状态向量为
x=[(δr)T (δφ)T]
其中,δr为GNSS与IMU组合导航位姿数据的位置误差,δφ为GNSS与MU组合导航位姿数据经过修正后的IMU姿态误差;
构建观测方程为:
其中,rLIDAR为LIDAR测得的位置,rZHUHE为GNSS与IMU组合导航位姿数据的位置,φLIDAR为LIDAR测得的姿态,φZHUHE为GNSS与IMU组合导航位姿数据的姿态;
状态方程和观测方程如下:
X(k+1)=ΦX(k)+ΓW(k)
Z(k)=X(k)+v(k)
其中,X(k+1)表示下一时刻状态,Φ表示状态转移矩阵,X(k)表示当前状态,Γ表示误差系数矩阵,W(k)表示状态误差,Z(k)表示当前观测值,v(k)表示观测误差;
所述粒子滤波包括:初始化、重要性采样和重采样3步,具体为:
第一步:初始化:
第二步:重要性采样:
式中ρ为观测误差:归一化权值:
第三步:重采样:
在t时刻产生[0,1]区间内均匀分布的随机数序列:Ui=U[0,1],i=1,2,...,N;按照下式:
计算此时所有状态向量粒子的平均值,得到位置误差修正值Δpst和姿态误差修正值Δlat;
再用GNSS与IMU组合导航得到的位置姿态数据分别减去Δpst和Δlat,得到最终定位结果。
8.根据权利要求7所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤1中所述的格网修正模型中样本的空间分辨率设定在25m以内;所述的精细化格网六边形边长为10m。
9.根据权利要求8所述的一种基于粒子滤波的城市峡谷中组合导航高精度定位方法,其特征在于,步骤6中所述的粒子滤波中采用的粒子个数N为100。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211620822.0A CN115616641B (zh) | 2022-12-16 | 2022-12-16 | 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211620822.0A CN115616641B (zh) | 2022-12-16 | 2022-12-16 | 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115616641A CN115616641A (zh) | 2023-01-17 |
CN115616641B true CN115616641B (zh) | 2023-03-17 |
Family
ID=84880097
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211620822.0A Active CN115616641B (zh) | 2022-12-16 | 2022-12-16 | 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115616641B (zh) |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106291645B (zh) * | 2016-07-19 | 2018-08-21 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
CN109991640B (zh) * | 2017-12-29 | 2022-04-01 | 上海司南卫星导航技术股份有限公司 | 一种组合导航***及其定位方法 |
CN109376785B (zh) * | 2018-10-31 | 2021-09-24 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN111380521B (zh) * | 2020-03-31 | 2021-10-29 | 苏州芯智谷智能科技有限公司 | 一种gnss/mems惯性组合芯片定位算法中的多路径滤波方法 |
CN114545475A (zh) * | 2022-01-14 | 2022-05-27 | 南京航空航天大学 | 一种复杂环境下的多源组合导航定位方法 |
CN114562992B (zh) * | 2022-02-09 | 2024-07-09 | 南京航空航天大学 | 一种基于因子图及场景约束的多径环境组合导航方法 |
-
2022
- 2022-12-16 CN CN202211620822.0A patent/CN115616641B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN115616641A (zh) | 2023-01-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
CN101825717B (zh) | 一种基于载波平滑码伪距技术的动态定姿方法 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN110567454B (zh) | 一种复杂环境下sins/dvl紧组合导航方法 | |
CN109507706B (zh) | 一种gps信号丢失的预测定位方法 | |
CN112129300A (zh) | 位置间动力学约束的低轨卫星星载gnss精密定轨方法及*** | |
CN110412596A (zh) | 一种基于图像信息和激光点云的机器人定位方法 | |
CN115616643B (zh) | 一种城市区域建模辅助的定位方法 | |
CN116819580B (zh) | 一种惯性辅助的双天线gnss海上船舶姿态确定方法 | |
CN113325452A (zh) | 一种三星无源融合定位体制机动目标跟踪方法 | |
CN115900708A (zh) | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 | |
CN114047766B (zh) | 面向室内外场景长期应用的移动机器人数据采集***及方法 | |
CN114608568A (zh) | 一种基于多传感器信息即时融合定位方法 | |
CN113900069A (zh) | 一种基于干涉成像高度计的垂线偏差计算方法及其*** | |
CN111307136B (zh) | 一种双智能水下机器人水下航行地形匹配导航方法 | |
CN115616641B (zh) | 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法 | |
CN116953744A (zh) | 一种导航卫星非视距信号识别方法和导航卫星定位方法 | |
CN117031513A (zh) | 一种道路及附属物的实时监测定位方法、***及装置 | |
Park et al. | Evenly weighted particle filter for terrain-referenced navigation using gaussian mixture proposal distribution | |
CN110849349A (zh) | 一种基于磁传感器与轮式里程计融合定位方法 | |
CN115616637A (zh) | 一种基于三维格网多径建模的城市复杂环境导航定位方法 | |
CN112230249A (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 |