CN113237482B - 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 - Google Patents
一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 Download PDFInfo
- Publication number
- CN113237482B CN113237482B CN202110519891.1A CN202110519891A CN113237482B CN 113237482 B CN113237482 B CN 113237482B CN 202110519891 A CN202110519891 A CN 202110519891A CN 113237482 B CN113237482 B CN 113237482B
- Authority
- CN
- China
- Prior art keywords
- factor
- gnss
- lidar
- node
- moment
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance 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/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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
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)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种基于因子图的城市峡谷环境下车辆鲁棒定位方法。在现有方法基础上,本方法首先引入激光雷达观测量,其次,构建基于因子图的动态融合模型,接着,提出一种既考虑观测量可用性及可靠性,又遵循特定选用策略的动态融合规则,根据规则决定是否将观测量引入因子图,最后,利用因子图实现定位数据更新。本发明公开的车辆定位方法,克服了现有方法观测量不够冗余、融合模型不够灵活等难题,保障了城市峡谷环境下自动驾驶汽车高精鲁棒定位。
Description
技术领域
本发明涉及车辆导航定位领域,尤其涉及一种面向城市峡谷环境的自动驾驶汽车鲁棒定位方法。
背景技术
随着汽车工业和社会经济的不断发展,我国汽车保有量不断增长。然而,车辆数量的迅速增加带来的交通拥堵、环境污染和道路事故等一系列问题,给国民经济发展和人民群众生活带来一定负面影响。依托传感器、信息融合、人工智能等前沿技术的自动驾驶汽车,可以提供更为安全高效的车辆控制及驾驶策略,降低了疲劳驾驶、违反交规等情况的发生,进而提高了交通系统的安全和效率,被认为是解决上述问题的有效手段。
精准且鲁棒的定位系统是保证自动驾驶汽车安全高效的关键。当前主流的自动驾驶汽车定位系统采用卡尔曼滤波(Kalman Filter,KF)融合惯性导航系统(InertialNavigation System,INS)和全球导航卫星系统(Global Navigation Satellite System,GNSS)的信息来提供可靠的全局位置。INS以较高的频率(典型值100Hz)提供加速度和角速度数据,不受外界干扰,但存在随时间增长的累计误差;GNSS信号作为观测量,以较低的频率(典型值1Hz)提供更精确的全局位置。两者通过卡尔曼滤波融合,最终输出高频准确鲁棒的全局位置,为后续的驾驶策略提供可靠位置信息。尽管该系统已能够在大多数场景下有效运行,但在建筑稠密的城市峡谷等环境中表现仍不如人意。更重要的是,城市峡谷不仅是自动驾驶汽车最主要的应用场景之一,还有着极为密集的人/车流。如果在此场景下出现定位误差甚至错误,影响自动驾驶汽车后续的控制判断,会给人民群众生命财产安全带来严重威胁,同时也会给行业发展前景蒙上阴影。现有定位方案在城市峡谷环境下不够鲁棒,主要因为:(1)观测信号不够冗余。一方面,城市峡谷里林立的高楼带来了诸如信号阻塞、多径效应和非视距等误差因素,大大降低了GNSS信号的可靠性甚至可用性;另一方面,当GNSS信号失效时,仅依靠惯导来确定位置会带来随时间增长的累计误差。(2)融合算法不够灵活。一方面,GNSS信号在城市峡谷环境下经常会不可用,此时仍将其引入融合滤波反而会带来严重错误;另一方面,卡尔曼滤波依赖于目标系统的预定数学模型和统计特性,滤波过程中目标运动状态的变化和外界环境的干扰将影响滤波精度。城市峡谷环境下即便GNSS信号可用,其统计特性也可能与初始设置不同,导致融合结果不够准确。
发明内容
针对上述问题,本发明提出了一种基于因子图的城市峡谷环境下车辆鲁棒定位方法,具体特征如下:(1)更丰富的传感体系。激光雷达(LiDAR)是近年来用于自动驾驶汽车定位和感知的重要传感器,不易受环境、光照、天气等影响。引入激光雷达传感器与原有的GNSS和INS形成互补冗余的传感体系,利用雷达里程计方法添加新的观测量,在GNSS信号不可用或可信度不高时弥补观测量缺失的问题,校正惯导累计误差;(2)基于因子图的动态融合。基于因子图“即插即用”的特性,充分考虑城市峡谷环境下观测量质量多变的特性,提出一种既考虑观测量可用性及可靠性,又遵循特定选用策略的动态融合规则,依据规则对多传感信息进行动态融合。
本发明提出的车辆定位方法解决了目前城市峡谷环境下自动驾驶汽车定位不够精准可靠的难题,提高了现有定位系统的容错性和鲁棒性,充分保障了该环境下自动驾驶汽车的高精鲁棒定位,对行业发展有积极重要的意义。
下面对本发明的思路做进一步说明:
步骤一:转换激光雷达坐标系到全局坐标系
激光雷达测距精度高,受环境等因素影响小,在自动驾驶汽车的定位和感知等方面已有广泛应用。激光雷达通过对前后两帧点云的匹配获取两帧间的旋转和平移矩阵,得出增量信息。这种功能与汽车里程计相似,也叫激光里程计,其详细原理可见参考文献(JiZhang,SanjivSingh.Low-drift and real-time lidar odometry and mapping[J].Autonomous Robots,2017.)。激光里程计获得的增量数值是在欧式空间下,而全局定位需要车辆的经纬高数值,同时由于GNSS/INS已统一到全局坐标系,因此只需对激光雷达坐标系进行转换。记本发明k-1时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1):
上式中,e为地球椭球偏心率,N为地球基准椭球体曲率半径。记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz),则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk):
再通过下式将位置由地心空间直角坐标系转向地心大地坐标系:
步骤二:构建因子图动态融合框架
因子图是概率图的一种,包括变量节点和因子节点两种类型的节点。有关因子图的详细原理可见参考文献(M.Kaess,H.Johannsson,R.Roberts,V.Ila,J.Leonard,andF.Dellaert,iSAM2:Incremental smoothing and mapping using theBayes tree[J]Intl.J.of Robotics Research.31:217-236)。基于因子图构建融合框架,具体步骤如下:
子步骤一:构建变量节点
首先构建表示自动驾驶汽车状态的变量节点Xk,主要包含k时刻自动驾驶汽车的经纬高;其次构建表示INS误差参数状态的变量节点Ck,主要包含k时刻惯导的常值漂移及随机游走。
子步骤二:构建因子节点
首先,构建用于连接自动驾驶汽车状态变量节点、INS误差参数状态变量节点的INS观测因子节点,代价函数为:
fbias(Ck+1,Ck)=d(Ck+1-g(Ck)) (19)
g()为误差状态更新函数;接着,构建GNSS因子节点,其代价函数为:
最后,构建LiDAR因子节点,其代价函数为:
步骤三:依据动态融合规则选择观测量
本发明提出一种既考虑因子节点可用性及可靠性,又符合特定选用策略的动态融合规则。当因子节点更新时,首先对其可用性及可靠性进行判定。本发明中,可用性是指以观测值当前质量是否能够接入因子图;可靠性则是以观测值当前质量将其接入因子图中的可信程度。基于因子图“即插即用”的特性,通过判断可用性,移除不可用的观测信息;通过判断可靠性,调节观测值协方差,保障融合算法及定位结果的自适应和鲁棒性,具体步骤如下:
子步骤一:LiDAR因子节点可用性及可靠性判定
当出现:
则认为LiDAR失效,其观测值不可用,不接入因子图;
此外,当LiDAR可用时,判断其可靠性,有:
子步骤二:GNSS因子节点可用性及可靠性判定
GNSS因子节点的可用性及可靠性根据k时刻GNSS的卫星数和GDOP数值来进行判定。记k时刻GNSS的卫星数为,GDOP数值为GDOPk。当卫星数小于4或者GDOP数值大于5时,认为此刻的GNSS观测值不可用。当GNSS观测值可用时,有:
子步骤三:因子节点选用策略
当因子节点都可用时,为提高融合效率并保证融合质量,本发明提出如下的选用策略:当LiDAR因子节点满足可用性要求时,评估完可靠性后即可接入因子图;而GNSS因子节点可用并评估完可靠性后,仅在下面两种情况接入因子图:
1.当前时刻GNSS因子节点协方差小于上一时刻系统定位结果的协方差;
2.当前时刻GNSS因子节点协方差小于最近时刻LiDAR因子节点协方差,
其他情况下,都不接入GNSS因子节点。
步骤四:更新定位结果
根据上述因子图架构与动态融合规则,利用最大后验估计对状态量进行实时优化递推,确定最终位置输出:
附图说明
图1是本发明本发明定位方法流程图;
图2是本发明基于因子图的融合框架图,其中,状态节点包括INS误差参数状态变量节点和车辆状态变量节点;因子节点包括LiDAR因子节点、INS观测因子节点、GNSS因子节点、INS偏置因子节点和先验因子节点。
具体实施方式
随着汽车工业和社会经济的不断发展,我国汽车保有量不断增长。然而,车辆数量的迅速增加带来的交通拥堵、环境污染和道路事故等一系列问题,给国民经济发展和人民群众生活带来一定负面影响。依托传感器、信息融合、人工智能等前沿技术的自动驾驶汽车,可以提供更为安全高效的车辆控制及驾驶策略,降低了疲劳驾驶、违反交规等情况的发生,进而提高了交通系统的安全和效率,被认为是解决上述问题的有效手段。
精准且鲁棒的定位系统是保证自动驾驶汽车安全高效的关键。当前主流的自动驾驶汽车定位系统采用卡尔曼滤波(Kalman Filter,KF)融合惯性导航系统(InertialNavigation System,INS)和全球导航卫星系统(Global Navigation Satellite System,GNSS)的信息来提供可靠的全局位置。INS以较高的频率(典型值100Hz)提供加速度和角速度数据,不受外界干扰,但存在随时间增长的累计误差;GNSS信号作为观测量,以较低的频率(典型值1Hz)提供更精确的全局位置。两者通过卡尔曼滤波融合,最终输出高频准确鲁棒的全局位置,为后续的驾驶策略提供可靠位置信息。尽管该系统已能够在大多数场景下有效运行,但在建筑稠密的城市峡谷等环境中表现仍不如人意。更重要的是,城市峡谷不仅是自动驾驶汽车最主要的应用场景之一,还有着极为密集的人/车流。如果在此场景下出现定位误差甚至错误,影响自动驾驶汽车后续的控制判断,会给人民群众生命财产安全带来严重威胁,同时也会给行业发展前景蒙上阴影。现有定位方案在城市峡谷环境下不够鲁棒,主要因为:(1)观测信号不够冗余。一方面,城市峡谷里林立的高楼带来了诸如信号阻塞、多径效应和非视距等误差因素,大大降低了GNSS信号的可靠性甚至可用性;另一方面,当GNSS信号失效时,仅依靠惯导来确定位置会带来随时间增长的累计误差。(2)融合算法不够灵活。一方面,GNSS信号在城市峡谷环境下经常会不可用,此时仍将其引入融合滤波反而会带来严重错误;另一方面,卡尔曼滤波依赖于目标系统的预定数学模型和统计特性,滤波过程中目标运动状态的变化和外界环境的干扰将影响滤波精度。城市峡谷环境下即便GNSS信号可用,其统计特性也可能与初始设置不同,导致融合结果不够准确。
针对上述问题,本发明提出了一种基于因子图的城市峡谷环境下车辆鲁棒定位方法,具体特征如下:(1)更丰富的传感体系。激光雷达(LiDAR)是近年来用于自动驾驶汽车定位和感知的重要传感器,不易受环境、光照、天气等影响。引入激光雷达传感器与原有的GNSS和INS形成互补冗余的传感体系,利用雷达里程计方法添加新的观测量,在GNSS信号不可用或可信度不高时弥补观测量缺失的问题,校正惯导累计误差;(2)基于因子图的动态融合。基于因子图“即插即用”的特性,充分考虑城市峡谷环境下观测量质量多变的特性,提出一种既考虑观测量可用性及可靠性,又遵循特定选用策略的动态融合规则,依据规则对多传感信息进行动态融合。
本发明提出的车辆定位方法解决了目前城市峡谷环境下自动驾驶汽车定位不够精准可靠的难题,提高了现有定位系统的容错性和鲁棒性,充分保障了该环境下自动驾驶汽车的高精鲁棒定位,对行业发展有积极重要的意义。
下面对本发明的思路做进一步说明:
步骤一:转换激光雷达坐标系到全局坐标系
激光雷达测距精度高,受环境等因素影响小,在自动驾驶汽车的定位和感知等方面已有广泛应用。激光雷达通过对前后两帧点云的匹配获取两帧间的旋转和平移矩阵,得出增量信息。这种功能与汽车里程计相似,也叫激光里程计,其详细原理可见参考文献(JiZhang,SanjivSingh.Low-drift and real-time lidar odometry and mapping[J].Autonomous Robots,2017.)。激光里程计获得的增量数值是在欧式空间下,而全局定位需要车辆的经纬高数值,同时由于GNSS/INS已统一到全局坐标系,因此只需对激光雷达坐标系进行转换。记本发明k-1时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1):
上式中,e为地球椭球偏心率,N为地球基准椭球体曲率半径。记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz),则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk):
再通过下式将位置由地心空间直角坐标系转向地心大地坐标系:
步骤二:构建因子图动态融合框架
因子图是概率图的一种,包括变量节点和因子节点两种类型的节点。有关因子图的详细原理可见参考文献(M.Kaess,H.Johannsson,R.Roberts,V.Ila,J.Leonard,andF.Dellaert,iSAM2:Incremental smoothing and mapping using theBayes tree[J]Intl.J.of Robotics Research.31:217-236)。基于因子图构建融合框架,具体步骤如下:
子步骤一:构建变量节点
首先构建表示自动驾驶汽车状态的变量节点Xk,主要包含k时刻自动驾驶汽车的经纬高;其次构建表示INS误差参数状态的变量节点Ck,主要包含k时刻惯导的常值漂移及随机游走。
子步骤二:构建因子节点
首先,构建用于连接自动驾驶汽车状态变量节点、INS误差参数状态变量节点的INS观测因子节点,代价函数为:
fbias(Ck+1,Ck)=d(Ck+1-g(Ck)) (33)
g()为误差状态更新函数;接着,构建GNSS因子节点,其代价函数为:
最后,构建LiDAR因子节点,其代价函数为:
步骤三:依据动态融合规则选择观测量
本发明提出一种既考虑因子节点可用性及可靠性,又符合特定选用策略的动态融合规则。当因子节点更新时,首先对其可用性及可靠性进行判定。本发明中,可用性是指以观测值当前质量是否能够接入因子图;可靠性则是以观测值当前质量将其接入因子图中的可信程度。基于因子图“即插即用”的特性,通过判断可用性,移除不可用的观测信息;通过判断可靠性,调节观测值协方差,保障融合算法及定位结果的自适应和鲁棒性,具体步骤如下:
子步骤一:LiDAR因子节点可用性及可靠性判定
当出现:
则认为LiDAR失效,其观测值不可用,不接入因子图;
此外,当LiDAR可用时,判断其可靠性,有:
子步骤二:GNSS因子节点可用性及可靠性判定
GNSS因子节点的可用性及可靠性根据k时刻GNSS的卫星数和GDOP数值来进行判定。记k时刻GNSS的卫星数为GDOP数值为GDOPk。当卫星数小于4或者GDOP数值大于5时,认为此刻的GNSS观测值不可用。当GNSS观测值可用时,有:
子步骤三:因子节点选用策略
当因子节点都可用时,为提高融合效率并保证融合质量,本发明提出如下的选用策略:当LiDAR因子节点满足可用性要求时,评估完可靠性后即可接入因子图;而GNSS因子节点可用并评估完可靠性后,仅在下面两种情况接入因子图:
1.当前时刻GNSS因子节点协方差小于上一时刻系统定位结果的协方差;
2.当前时刻GNSS因子节点协方差小于最近时刻LiDAR因子节点协方差,
其他情况下,都不接入GNSS因子节点。
步骤四:更新定位结果
根据上述因子图架构与动态融合规则,利用最大后验估计对状态量进行实时优化递推,确定最终位置输出:
Claims (1)
1.一种基于因子图的城市峡谷环境下车辆鲁棒定位方法,其特征在于,在现有GNSS/INS定位的基础上,引入激光雷达观测量,并构建基于因子图的动态融合模型,提出一种既考虑观测量可用性及可靠性,又遵循特定选用策略的动态融合规则,依据规则利用因子图更新定位结果,具体包括以下步骤:
步骤一:转换激光雷达坐标系到全局坐标系
上式中,e为地球椭球偏心率,N为地球基准椭球体曲率半径,记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz),则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk):
再通过下式将位置由地心空间直角坐标系转向地心大地坐标系:
步骤二:构建因子图动态融合框架
基于因子图构建融合框架,具体步骤如下:
子步骤一:构建变量节点
首先构建表示自动驾驶汽车状态的变量节点Xk,主要包含k时刻自动驾驶汽车的经纬高;其次构建表示INS误差参数状态的变量节点Ck,主要包含k时刻惯导的常值漂移及随机游走;
子步骤二:构建因子节点
首先,构建用于连接自动驾驶汽车状态变量节点、INS误差参数状态变量节点的INS观测因子节点,代价函数为:
fbias(Ck+1,Ck)=d(Ck+1-g(Ck)) (5)
g()为误差状态更新函数;接着,构建GNSS因子节点,其代价函数为:
最后,构建LiDAR因子节点,其代价函数为:
步骤三:依据动态融合规则选择观测量
提出一种既考虑因子节点可用性及可靠性,又符合特定选用策略的动态融合规则;当因子节点更新时,首先对其可用性及可靠性进行判定,其中,可用性是指以观测值当前质量是否能够接入因子图;可靠性则是以观测值当前质量将其接入因子图中的可信程度,基于因子图“即插即用”的特性,通过判断可用性,移除不可用的观测信息;通过判断可靠性,调节观测值协方差,保障融合算法及定位结果的自适应和鲁棒性,具体步骤如下:
子步骤一:LiDAR因子节点可用性及可靠性判定
当出现:
则认为LiDAR失效,其观测值不可用,不接入因子图;
此外,当LiDAR可用时,判断其可靠性,有:
子步骤二:GNSS因子节点可用性及可靠性判定
GNSS因子节点的可用性及可靠性根据k时刻GNSS的卫星数和GDOP数值来进行判定;记k时刻GNSS的卫星数为GDOP数值为GDOPk,当卫星数小于4或者GDOP数值大于5时,认为此刻的GNSS观测值不可用;当GNSS观测值可用时,有:
子步骤三:因子节点选用策略
当因子节点都可用时,为提高融合效率并保证融合质量,提出如下的选用策略:当LiDAR因子节点满足可用性要求时,评估完可靠性后即可接入因子图;而GNSS因子节点可用并评估完可靠性后,当满足下面两种情况之一或者都满足时将其接入因子图:
1.当前时刻GNSS因子节点协方差小于上一时刻系统定位结果的协方差,
2.当前时刻GNSS因子节点协方差小于最近时刻LiDAR因子节点协方差,
其他情况下,都不接入GNSS因子节点;
步骤四:更新定位结果
根据上述因子图架构与动态融合规则,利用最大后验估计对状态量进行实时优化递推,确定最终位置输出:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110519891.1A CN113237482B (zh) | 2021-05-13 | 2021-05-13 | 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110519891.1A CN113237482B (zh) | 2021-05-13 | 2021-05-13 | 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113237482A CN113237482A (zh) | 2021-08-10 |
CN113237482B true CN113237482B (zh) | 2022-05-13 |
Family
ID=77133843
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110519891.1A Active CN113237482B (zh) | 2021-05-13 | 2021-05-13 | 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113237482B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117437552B (zh) * | 2023-12-21 | 2024-03-26 | 绘见科技(深圳)有限公司 | 视觉定位地图的构建方法及装置、设备及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106197408A (zh) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | 一种基于因子图的多源导航信息融合方法 |
CN107421566A (zh) * | 2017-08-25 | 2017-12-01 | 北京理工大学 | 一种无人车多源传感器信息仿真平台 |
CN109737957A (zh) * | 2019-01-31 | 2019-05-10 | 济南大学 | 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统 |
CN111272165A (zh) * | 2020-02-27 | 2020-06-12 | 清华大学 | 一种基于特征点标定的智能车定位方法 |
CN111780755A (zh) * | 2020-06-30 | 2020-10-16 | 南京理工大学 | 一种基于因子图和可观测度分析的多源融合导航方法 |
-
2021
- 2021-05-13 CN CN202110519891.1A patent/CN113237482B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106197408A (zh) * | 2016-06-23 | 2016-12-07 | 南京航空航天大学 | 一种基于因子图的多源导航信息融合方法 |
CN107421566A (zh) * | 2017-08-25 | 2017-12-01 | 北京理工大学 | 一种无人车多源传感器信息仿真平台 |
CN109737957A (zh) * | 2019-01-31 | 2019-05-10 | 济南大学 | 一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统 |
CN111272165A (zh) * | 2020-02-27 | 2020-06-12 | 清华大学 | 一种基于特征点标定的智能车定位方法 |
CN111780755A (zh) * | 2020-06-30 | 2020-10-16 | 南京理工大学 | 一种基于因子图和可观测度分析的多源融合导航方法 |
Non-Patent Citations (1)
Title |
---|
基于可观测度分析和增量因子图的多源融合导航方法;左思琪等;《导航定位与授时》;20201130;第7卷(第6期);第22-29页 * |
Also Published As
Publication number | Publication date |
---|---|
CN113237482A (zh) | 2021-08-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11002859B1 (en) | Intelligent vehicle positioning method based on feature point calibration | |
CN113029137B (zh) | 一种多源信息自适应融合定位方法及系统 | |
CN108226980B (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN110411462B (zh) | 一种gnss/惯导/车道线约束/里程计多源融合方法 | |
CN110702091B (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
CN109000640B (zh) | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 | |
CN110307836B (zh) | 一种用于无人清扫车辆贴边清扫的精确定位方法 | |
CN111429716A (zh) | 用于确定自车的位置的方法 | |
CN112147651B (zh) | 一种异步多车协同目标状态鲁棒估计方法 | |
CN111157984B (zh) | 基于毫米波雷达及惯性测量单元的一种行人自主导航方法 | |
CN112505737A (zh) | 一种基于Elman神经网络在线学习辅助的GNSS/INS组合导航方法 | |
CN110940344B (zh) | 一种用于自动驾驶的低成本传感器组合定位方法 | |
CN112577496B (zh) | 一种基于自适应选权的多源融合定位方法 | |
CN114689047B (zh) | 基于深度学习的组合导航方法、装置、系统及存储介质 | |
WO2024041156A1 (zh) | 车辆定位校准方法、装置、计算机设备及存储介质 | |
CN113237482B (zh) | 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 | |
CN114915913A (zh) | 一种基于滑窗因子图的uwb-imu组合室内定位方法 | |
CN115900708A (zh) | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 | |
CN114415224A (zh) | 一种复杂受限环境中车辆融合定位系统及方法 | |
CN115639585A (zh) | 一种gps/imu和激光雷达的多传感器融合定位方法 | |
CN116105729A (zh) | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 | |
CN115046546A (zh) | 一种基于车道线识别的自动驾驶汽车定位系统及方法 | |
CN112904382B (zh) | 一种城市峡谷环境下激光里程计辅助的快速优化选星方法 | |
CN115046547A (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 |