CN113237482B - 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 - Google Patents

一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 Download PDF

Info

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
Application number
CN202110519891.1A
Other languages
English (en)
Other versions
CN113237482A (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.)
Innovation Research Institute Of Jiangbei New District Southeast University
Southeast University
Original Assignee
Innovation Research Institute Of Jiangbei New District Southeast University
Southeast University
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 Innovation Research Institute Of Jiangbei New District Southeast University, Southeast University filed Critical Innovation Research Institute Of Jiangbei New District Southeast University
Priority to CN202110519891.1A priority Critical patent/CN113237482B/zh
Publication of CN113237482A publication Critical patent/CN113237482A/zh
Application granted granted Critical
Publication of CN113237482B publication Critical patent/CN113237482B/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/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

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时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为
Figure BDA0003063495900000021
通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1)：
Figure BDA0003063495900000031
上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径。记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz)，则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk)：
Figure BDA0003063495900000032
再通过下式将位置由地心空间直角坐标系转向地心大地坐标系：
Figure BDA0003063495900000033
式中，
Figure BDA0003063495900000034
分别表示k时刻激光里程计得到的自动驾驶汽车的经纬高数值，即LiDAR传感器的观测量；
步骤二：构建因子图动态融合框架
因子图是概率图的一种，包括变量节点和因子节点两种类型的节点。有关因子图的详细原理可见参考文献(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观测因子节点，代价函数为：
Figure BDA0003063495900000041
上式中的下标皆表示时刻，
Figure BDA0003063495900000042
代表k时刻惯导测量值，即陀螺仪和加速度计的数据，
Figure BDA0003063495900000043
为k+1时刻的先验状态，h()为系统状态转移函数；其次，构建用于连接惯导误差参数状态变量节点的INS偏置因子节点，其代价函数：
fbias(Ck+1,Ck)＝d(Ck+1-g(Ck)) (19)
g()为误差状态更新函数；接着，构建GNSS因子节点，其代价函数为：
Figure BDA0003063495900000044
其中
Figure BDA0003063495900000045
表示k时刻GNSS的经纬高等观测量，hGNSS()表示GNSS观测方程，并设置其协方差矩阵QGNSS，为GNSS所测经纬高的标称协方差；
最后，构建LiDAR因子节点，其代价函数为：
Figure BDA0003063495900000046
其中
Figure BDA0003063495900000047
表示k时刻雷达里程计获取的经纬高观测量，hLiDAR()为LiDAR的观测方程，并设置其协方差矩阵QLiDAR，初值为LiDAR所测值的标称协方差；
步骤三：依据动态融合规则选择观测量
本发明提出一种既考虑因子节点可用性及可靠性，又符合特定选用策略的动态融合规则。当因子节点更新时，首先对其可用性及可靠性进行判定。本发明中，可用性是指以观测值当前质量是否能够接入因子图；可靠性则是以观测值当前质量将其接入因子图中的可信程度。基于因子图“即插即用”的特性，通过判断可用性，移除不可用的观测信息；通过判断可靠性，调节观测值协方差，保障融合算法及定位结果的自适应和鲁棒性，具体步骤如下：
子步骤一：LiDAR因子节点可用性及可靠性判定
本发明依据每一时刻匹配点数来判定LiDAR的可用性及可靠性。记k时刻匹配点数为nk，定义滑动窗口点数为
Figure BDA0003063495900000048
通过下式计算：
Figure BDA0003063495900000049
当出现：
Figure BDA0003063495900000051
则认为LiDAR失效，其观测值不可用，不接入因子图；
此外，当LiDAR可用时，判断其可靠性，有：
Figure BDA0003063495900000052
Figure BDA0003063495900000053
是此刻的协方差调节系数，gLiDAR()是通过当前匹配点数和滑动窗口匹配点数计算调节系数的函数，此时更新LiDAR因子节点协方差矩阵为：
Figure BDA0003063495900000054
子步骤二：GNSS因子节点可用性及可靠性判定
GNSS因子节点的可用性及可靠性根据k时刻GNSS的卫星数和GDOP数值来进行判定。记k时刻GNSS的卫星数为
Figure BDA00030634959000000510
，GDOP数值为GDOPk。当卫星数小于4或者GDOP数值大于5时，认为此刻的GNSS观测值不可用。当GNSS观测值可用时，有：
Figure BDA0003063495900000055
其中，
Figure BDA0003063495900000056
是此刻的协方差调节系数，gGNSS()是通过卫星数和GDOP数值计算调节系数的函数，此时更新GNSS因子节点的协方差矩阵为：
Figure BDA0003063495900000057
子步骤三：因子节点选用策略
当因子节点都可用时，为提高融合效率并保证融合质量，本发明提出如下的选用策略：当LiDAR因子节点满足可用性要求时，评估完可靠性后即可接入因子图；而GNSS因子节点可用并评估完可靠性后，仅在下面两种情况接入因子图：
1.当前时刻GNSS因子节点协方差小于上一时刻系统定位结果的协方差；
2.当前时刻GNSS因子节点协方差小于最近时刻LiDAR因子节点协方差，
其他情况下，都不接入GNSS因子节点。
步骤四：更新定位结果
根据上述因子图架构与动态融合规则，利用最大后验估计对状态量进行实时优化递推，确定最终位置输出：
Figure BDA0003063495900000058
式中，
Figure BDA0003063495900000059
为k时刻自动驾驶汽车的位置结果，P(Xk|Xk-1)对应INS观测因子节点，P(Zk|Xk)对应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时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为
Figure BDA0003063495900000071
通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1)：
Figure BDA0003063495900000081
上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径。记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz)，则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk)：
Figure BDA0003063495900000082
再通过下式将位置由地心空间直角坐标系转向地心大地坐标系：
Figure BDA0003063495900000083
式中，
Figure BDA0003063495900000084
分别表示k时刻激光里程计得到的自动驾驶汽车的经纬高数值，即LiDAR传感器的观测量；
步骤二：构建因子图动态融合框架
因子图是概率图的一种，包括变量节点和因子节点两种类型的节点。有关因子图的详细原理可见参考文献(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观测因子节点，代价函数为：
Figure BDA0003063495900000091
上式中的下标皆表示时刻，
Figure BDA0003063495900000092
代表k时刻惯导测量值，即陀螺仪和加速度计的数据，
Figure BDA0003063495900000093
为k+1时刻的先验状态，h()为系统状态转移函数；其次，构建用于连接惯导误差参数状态变量节点的INS偏置因子节点，其代价函数：
fbias(Ck+1,Ck)＝d(Ck+1-g(Ck)) (33)
g()为误差状态更新函数；接着，构建GNSS因子节点，其代价函数为：
Figure BDA0003063495900000094
其中
Figure BDA0003063495900000095
表示k时刻GNSS的经纬高等观测量，hGNSS()表示GNSS观测方程，并设置其协方差矩阵QGNSS，为GNSS所测经纬高的标称协方差；
最后，构建LiDAR因子节点，其代价函数为：
Figure BDA0003063495900000096
其中
Figure BDA0003063495900000097
表示k时刻雷达里程计获取的经纬高观测量，hLiDAR()为LiDAR的观测方程，并设置其协方差矩阵QLiDAR，初值为LiDAR所测值的标称协方差；
步骤三：依据动态融合规则选择观测量
本发明提出一种既考虑因子节点可用性及可靠性，又符合特定选用策略的动态融合规则。当因子节点更新时，首先对其可用性及可靠性进行判定。本发明中，可用性是指以观测值当前质量是否能够接入因子图；可靠性则是以观测值当前质量将其接入因子图中的可信程度。基于因子图“即插即用”的特性，通过判断可用性，移除不可用的观测信息；通过判断可靠性，调节观测值协方差，保障融合算法及定位结果的自适应和鲁棒性，具体步骤如下：
子步骤一：LiDAR因子节点可用性及可靠性判定
本发明依据每一时刻匹配点数来判定LiDAR的可用性及可靠性。记k时刻匹配点数为nk，定义滑动窗口点数为
Figure BDA0003063495900000098
通过下式计算：
Figure BDA0003063495900000099
当出现：
Figure BDA0003063495900000101
则认为LiDAR失效，其观测值不可用，不接入因子图；
此外，当LiDAR可用时，判断其可靠性，有：
Figure BDA0003063495900000102
Figure BDA0003063495900000103
是此刻的协方差调节系数，gLiDAR()是通过当前匹配点数和滑动窗口匹配点数计算调节系数的函数，此时更新LiDAR因子节点协方差矩阵为：
Figure BDA0003063495900000104
子步骤二：GNSS因子节点可用性及可靠性判定
GNSS因子节点的可用性及可靠性根据k时刻GNSS的卫星数和GDOP数值来进行判定。记k时刻GNSS的卫星数为
Figure BDA0003063495900000105
GDOP数值为GDOPk。当卫星数小于4或者GDOP数值大于5时，认为此刻的GNSS观测值不可用。当GNSS观测值可用时，有：
Figure BDA0003063495900000106
其中，
Figure BDA0003063495900000107
是此刻的协方差调节系数，gGNSS()是通过卫星数和GDOP数值计算调节系数的函数，此时更新GNSS因子节点的协方差矩阵为：
Figure BDA0003063495900000108
子步骤三：因子节点选用策略
当因子节点都可用时，为提高融合效率并保证融合质量，本发明提出如下的选用策略：当LiDAR因子节点满足可用性要求时，评估完可靠性后即可接入因子图；而GNSS因子节点可用并评估完可靠性后，仅在下面两种情况接入因子图：
1.当前时刻GNSS因子节点协方差小于上一时刻系统定位结果的协方差；
2.当前时刻GNSS因子节点协方差小于最近时刻LiDAR因子节点协方差，
其他情况下，都不接入GNSS因子节点。
步骤四：更新定位结果
根据上述因子图架构与动态融合规则，利用最大后验估计对状态量进行实时优化递推，确定最终位置输出：
Figure BDA0003063495900000109
式中，
Figure BDA00030634959000001010
为k时刻自动驾驶汽车的位置结果，P(Xk|Xk-1)对应INS观测因子节点，P(Zk|Xk)对应LiDAR因子节点或GNSS因子节点。

Claims (1)

1.一种基于因子图的城市峡谷环境下车辆鲁棒定位方法，其特征在于，在现有GNSS/INS定位的基础上，引入激光雷达观测量，并构建基于因子图的动态融合模型，提出一种既考虑观测量可用性及可靠性，又遵循特定选用策略的动态融合规则，依据规则利用因子图更新定位结果，具体包括以下步骤：
步骤一：转换激光雷达坐标系到全局坐标系
记k-1时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为
Figure FDA0003503737380000011
Figure FDA0003503737380000012
通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1)：
Figure FDA0003503737380000013
上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径，记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz)，则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk)：
Figure FDA0003503737380000014
再通过下式将位置由地心空间直角坐标系转向地心大地坐标系：
Figure FDA0003503737380000015
式中，
Figure FDA0003503737380000016
分别表示k时刻激光里程计得到的自动驾驶汽车的经纬高数值，即LiDAR传感器的观测量；
步骤二：构建因子图动态融合框架
基于因子图构建融合框架，具体步骤如下：
子步骤一：构建变量节点
首先构建表示自动驾驶汽车状态的变量节点Xk，主要包含k时刻自动驾驶汽车的经纬高；其次构建表示INS误差参数状态的变量节点Ck，主要包含k时刻惯导的常值漂移及随机游走；
子步骤二：构建因子节点
首先，构建用于连接自动驾驶汽车状态变量节点、INS误差参数状态变量节点的INS观测因子节点，代价函数为：
Figure FDA0003503737380000021
上式中的下标皆表示时刻，
Figure FDA0003503737380000022
代表k时刻惯导测量值，即陀螺仪和加速度计的数据，
Figure FDA0003503737380000023
为k+1时刻的先验状态，h()为系统状态转移函数；其次，构建用于连接惯导误差参数状态变量节点的INS偏置因子节点，其代价函数：
fbias(Ck+1,Ck)＝d(Ck+1-g(Ck)) (5)
g()为误差状态更新函数；接着，构建GNSS因子节点，其代价函数为：
Figure FDA0003503737380000024
其中
Figure FDA0003503737380000025
表示k时刻GNSS的经纬高观测量，hGNSS()表示GNSS观测方程，并设置其协方差矩阵QGNSS，为GNSS所测经纬高的标称协方差；
最后，构建LiDAR因子节点，其代价函数为：
Figure FDA0003503737380000026
其中
Figure FDA0003503737380000027
表示k时刻雷达里程计获取的经纬高观测量，hLiDAR()为LiDAR的观测方程，并设置其协方差矩阵QLiDAR，初值为LiDAR所测值的标称协方差；
步骤三：依据动态融合规则选择观测量
提出一种既考虑因子节点可用性及可靠性，又符合特定选用策略的动态融合规则；当因子节点更新时，首先对其可用性及可靠性进行判定，其中，可用性是指以观测值当前质量是否能够接入因子图；可靠性则是以观测值当前质量将其接入因子图中的可信程度，基于因子图“即插即用”的特性，通过判断可用性，移除不可用的观测信息；通过判断可靠性，调节观测值协方差，保障融合算法及定位结果的自适应和鲁棒性，具体步骤如下：
子步骤一：LiDAR因子节点可用性及可靠性判定
依据每一时刻匹配点数来判定LiDAR的可用性及可靠性，记k时刻匹配点数为nk，定义滑动窗口点数为
Figure FDA0003503737380000031
通过下式计算：
Figure FDA0003503737380000032
当出现：
Figure FDA0003503737380000033
则认为LiDAR失效，其观测值不可用，不接入因子图；
此外，当LiDAR可用时，判断其可靠性，有：
Figure FDA0003503737380000034
Figure FDA0003503737380000035
是此刻的协方差调节系数，gLiDAR()是通过当前匹配点数和滑动窗口匹配点数计算调节系数的函数，此时更新LiDAR因子节点协方差矩阵为：
Figure FDA0003503737380000036
子步骤二：GNSS因子节点可用性及可靠性判定
GNSS因子节点的可用性及可靠性根据k时刻GNSS的卫星数和GDOP数值来进行判定；记k时刻GNSS的卫星数为
Figure FDA0003503737380000037
GDOP数值为GDOPk，当卫星数小于4或者GDOP数值大于5时，认为此刻的GNSS观测值不可用；当GNSS观测值可用时，有：
Figure FDA0003503737380000038
其中，
Figure FDA0003503737380000039
是此刻的协方差调节系数，gGNSS()是通过卫星数和GDOP数值计算调节系数的函数，此时更新GNSS因子节点的协方差矩阵为：
Figure FDA00035037373800000310
子步骤三：因子节点选用策略
当因子节点都可用时，为提高融合效率并保证融合质量，提出如下的选用策略：当LiDAR因子节点满足可用性要求时，评估完可靠性后即可接入因子图；而GNSS因子节点可用并评估完可靠性后，当满足下面两种情况之一或者都满足时将其接入因子图：
1.当前时刻GNSS因子节点协方差小于上一时刻系统定位结果的协方差，
2.当前时刻GNSS因子节点协方差小于最近时刻LiDAR因子节点协方差，
其他情况下，都不接入GNSS因子节点；
步骤四：更新定位结果
根据上述因子图架构与动态融合规则，利用最大后验估计对状态量进行实时优化递推，确定最终位置输出：
Figure FDA0003503737380000041
式中，
Figure FDA0003503737380000042
为k时刻自动驾驶汽车的位置结果，P(Xk|Xk-1)对应INS观测因子节点，P(Zk|Xk)对应LiDAR因子节点或GNSS因子节点。
CN202110519891.1A 2021-05-13 2021-05-13 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 Active CN113237482B (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117437552B (zh) * 2023-12-21 2024-03-26 绘见科技(深圳)有限公司 视觉定位地图的构建方法及装置、设备及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
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 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN113029137B (zh) 一种多源信息自适应融合定位方法及系统
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN110411462B (zh) 一种gnss/惯导/车道线约束/里程计多源融合方法
JP5162849B2 (ja) 不動点位置記録装置
CN109883426B (zh) 基于因子图的动态分配与校正多源信息融合方法
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN110307836B (zh) 一种用于无人清扫车辆贴边清扫的精确定位方法
CN111429716A (zh) 用于确定自车的位置的方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN112147651B (zh) 一种异步多车协同目标状态鲁棒估计方法
CN111157984B (zh) 基于毫米波雷达及惯性测量单元的一种行人自主导航方法
CN114689047B (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
CN112577496B (zh) 一种基于自适应选权的多源融合定位方法
WO2024041156A1 (zh) 车辆定位校准方法、装置、计算机设备及存储介质
CN113237482B (zh) 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
CN116719037A (zh) 一种用于智能割草机器人的环境感知方法及系统
CN115900708A (zh) 基于gps引导式粒子滤波的机器人多传感器融合定位方法
CN114415224A (zh) 一种复杂受限环境中车辆融合定位系统及方法
CN115639585A (zh) 一种gps/imu和激光雷达的多传感器融合定位方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
CN115046546A (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