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

本发明专利技术公开了一种基于因子图的城市峡谷环境下车辆鲁棒定位方法。在现有方法基础上，本方法首先引入激光雷达观测量，其次，构建基于因子图的动态融合模型，接着，提出一种既考虑观测量可用性及可靠性，又遵循特定选用策略的动态融合规则，根据规则决定是否将观测量引入因子图，最后，利用因子图实现定位数据更新。本发明专利技术公开的车辆定位方法，克服了现有方法观测量不够冗余、融合模型不够灵活等难题，保障了城市峡谷环境下自动驾驶汽车高精鲁棒定位。

【技术实现步骤摘要】
一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
本专利技术涉及车辆导航定位领域，尤其涉及一种面向城市峡谷环境的自动驾驶汽车鲁棒定位方法。
技术介绍
随着汽车工业和社会经济的不断发展，我国汽车保有量不断增长。然而，车辆数量的迅速增加带来的交通拥堵、环境污染和道路事故等一系列问题，给国民经济发展和人民群众生活带来一定负面影响。依托传感器、信息融合、人工智能等前沿技术的自动驾驶汽车，可以提供更为安全高效的车辆控制及驾驶策略，降低了疲劳驾驶、违反交规等情况的发生，进而提高了交通系统的安全和效率，被认为是解决上述问题的有效手段。精准且鲁棒的定位系统是保证自动驾驶汽车安全高效的关键。当前主流的自动驾驶汽车定位系统采用卡尔曼滤波(KalmanFilter,KF)融合惯性导航系统(InertialNavigationSystem,INS)和全球导航卫星系统(GlobalNavigationSatelliteSystem,GNSS)的信息来提供可靠的全局位置。INS以较高的频率(典型值100Hz)提供加速度和角速度数据，不受外界干扰，但存在随时间增长的累计误差；GNSS信号作为观测量，以较低的频率(典型值1Hz)提供更精确的全局位置。两者通过卡尔曼滤波融合，最终输出高频准确鲁棒的全局位置，为后续的驾驶策略提供可靠位置信息。尽管该系统已能够在大多数场景下有效运行，但在建筑稠密的城市峡谷等环境中表现仍不如人意。更重要的是，城市峡谷不仅是自动驾驶汽车最主要的应用场景之一，还有着极为密集的人/车流。如果在此场景下出现定位误差甚至错误，影响自动驾驶汽车后续的控制判断，会给人民群众生命财产安全带来严重威胁，同时也会给行业发展前景蒙上阴影。现有定位方案在城市峡谷环境下不够鲁棒，主要因为：(1)观测信号不够冗余。一方面，城市峡谷里林立的高楼带来了诸如信号阻塞、多径效应和非视距等误差因素，大大降低了GNSS信号的可靠性甚至可用性；另一方面，当GNSS信号失效时，仅依靠惯导来确定位置会带来随时间增长的累计误差。(2)融合算法不够灵活。一方面，GNSS信号在城市峡谷环境下经常会不可用，此时仍将其引入融合滤波反而会带来严重错误；另一方面，卡尔曼滤波依赖于目标系统的预定数学模型和统计特性，滤波过程中目标运动状态的变化和外界环境的干扰将影响滤波精度。城市峡谷环境下即便GNSS信号可用，其统计特性也可能与初始设置不同，导致融合结果不够准确。
技术实现思路
针对上述问题，本专利技术提出了一种基于因子图的城市峡谷环境下车辆鲁棒定位方法，具体特征如下：(1)更丰富的传感体系。激光雷达(LiDAR)是近年来用于自动驾驶汽车定位和感知的重要传感器，不易受环境、光照、天气等影响。引入激光雷达传感器与原有的GNSS和INS形成互补冗余的传感体系，利用雷达里程计方法添加新的观测量，在GNSS信号不可用或可信度不高时弥补观测量缺失的问题，校正惯导累计误差；(2)基于因子图的动态融合。基于因子图“即插即用”的特性，充分考虑城市峡谷环境下观测量质量多变的特性，提出一种既考虑观测量可用性及可靠性，又遵循特定选用策略的动态融合规则，依据规则对多传感信息进行动态融合。本专利技术提出的车辆定位方法解决了目前城市峡谷环境下自动驾驶汽车定位不够精准可靠的难题，提高了现有定位系统的容错性和鲁棒性，充分保障了该环境下自动驾驶汽车的高精鲁棒定位，对行业发展有积极重要的意义。下面对本专利技术的思路做进一步说明：步骤一：转换激光雷达坐标系到全局坐标系激光雷达测距精度高，受环境等因素影响小，在自动驾驶汽车的定位和感知等方面已有广泛应用。激光雷达通过对前后两帧点云的匹配获取两帧间的旋转和平移矩阵，得出增量信息。这种功能与汽车里程计相似，也叫激光里程计，其详细原理可见参考文献(JiZhang,SanjivSingh.Low-driftandreal-timelidarodometryandmapping[J].AutonomousRobots,2017.)。激光里程计获得的增量数值是在欧式空间下，而全局定位需要车辆的经纬高数值，同时由于GNSS/INS已统一到全局坐标系，因此只需对激光雷达坐标系进行转换。记本专利技术k-1时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1)：上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径。记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz)，则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk)：再通过下式将位置由地心空间直角坐标系转向地心大地坐标系：式中，分别表示k时刻激光里程计得到的自动驾驶汽车的经纬高数值，即LiDAR传感器的观测量；步骤二：构建因子图动态融合框架因子图是概率图的一种，包括变量节点和因子节点两种类型的节点。有关因子图的详细原理可见参考文献(M.Kaess,H.Johannsson,R.Roberts,V.Ila,J.Leonard,andF.Dellaert,iSAM2:IncrementalsmoothingandmappingusingtheBayestree[J]Intl.J.ofRoboticsResearch.31:217-236)。基于因子图构建融合框架，具体步骤如下：子步骤一：构建变量节点首先构建表示自动驾驶汽车状态的变量节点Xk，主要包含k时刻自动驾驶汽车的经纬高；其次构建表示INS误差参数状态的变量节点Ck，主要包含k时刻惯导的常值漂移及随机游走。子步骤二：构建因子节点首先，构建用于连接自动驾驶汽车状态变量节点、INS误差参数状态变量节点的INS观测因子节点，代价函数为：上式中的下标皆表示时刻，代表k时刻惯导测量值，即陀螺仪和加速度计的数据，为k+1时刻的先验状态，h()为系统状态转移函数；其次，构建用于连接惯导误差参数状态变量节点的INS偏置因子节点，其代价函数：fbias(Ck+1,Ck)＝d(Ck+1-g(Ck))(19)g()为误差状态更新函数；接着，构建GNSS因子节点，其代价函数为：其中表示k时刻GNSS的经纬高等观测量，hGNSS()表示GNSS观测方程，并设置其协方差矩阵QGNSS，为GNSS所测经纬高的标称协方差；最后，构建LiDAR因子节点，其代价函数为：其中表示k时刻雷达里程计获取的经纬高观测量，hLiDAR()为LiDAR的观测方程，并设置其协方差矩阵QLiDAR，初值为LiDAR所测值的标称协方差；步骤三：依据动态融合规则选择观测量本专利技术提出一种既考虑因子节点可用性及可靠性，又符合特定选用策略的动态融合规则。当因子节点更新时，首先对其可用性及可靠性进行判定。本专利技术中，可用性是指以观测值当前质量是否能够接入因子图；可靠性则是以观测值当前质量将其接入因子图中的可信程度。基于因子图“即插即用”的特性，本文档来自技高网...

【技术保护点】
1.一种基于因子图的城市峡谷环境下车辆鲁棒定位方法，其特征在于，在现有GNSS/INS定位的基础上，引入激光雷达观测量，并构建基于因子图的动态融合模型，提出一种既考虑观测量可用性及可靠性，又遵循特定选用策略的动态融合规则，依据规则利用因子图更新定位结果，具体包括以下步骤：/n步骤一：转换激光雷达坐标系到全局坐标系/n记k-1时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为

【技术特征摘要】
1.一种基于因子图的城市峡谷环境下车辆鲁棒定位方法，其特征在于，在现有GNSS/INS定位的基础上，引入激光雷达观测量，并构建基于因子图的动态融合模型，提出一种既考虑观测量可用性及可靠性，又遵循特定选用策略的动态融合规则，依据规则利用因子图更新定位结果，具体包括以下步骤：
步骤一：转换激光雷达坐标系到全局坐标系
记k-1时刻在地心大地坐标下自动驾驶汽车的纬度、经度和高度为通过下式将其转换至地心空间直角坐标系下为(xk-1,yk-1,zk-1)：



上式中，e为地球椭球偏心率，N为地球基准椭球体曲率半径，记k时刻激光里程计在地心空间直角坐标系下得到的自动驾驶汽车位置增量为(Δxk,Δyk,Δz)，则k时刻自动驾驶汽车在地心空间直角坐标系下的位置(xk,yk,zk)：



再通过下式将位置由地心空间直角坐标系转向地心大地坐标系：



式中，分别表示k时刻激光里程计得到的自动驾驶汽车的经纬高数值，即LiDAR传感器的观测量；
步骤二：构建因子图动态融合框架
基于因子图构建融合框架，具体步骤如下：
子步骤一：构建变量节点
首先构建表示自动驾驶汽车状态的变量节点Xk，主要包含k时刻自动驾驶汽车的经纬高；其次构建表示INS误差参数状态的变量节点Ck，主要包含k时刻惯导的常值漂移及随机游走；
子步骤二：构建因子节点
首先，构建用于连接自动驾驶汽车状态变量节点、INS误差参数状态变量节点的INS观测因子节点，代价函数为：



上式中的下标皆表示时刻，代表k时刻惯导测量值，即陀螺仪和加速度计的数据，为k+1时刻的先验状态，h()为系统状态转移函数；其次，构建用于连接惯导误差参数状态变量节点的INS偏置因子节点，其代价函数：
fbias(Ck+1,Ck)＝d(Ck+1-g(Ck))(5)
g()为误差状态更新函数；接着，构建GNSS因子节点，其代价函数为：



其中表示k时刻GNSS的经纬高等观测量，hGNSS()表示GNSS观测方程，并设置其协方差矩阵QGNSS，为GNSS所测经纬高的标称协方差；
最后，构建LiDAR因子节点，其代价函数为：



其中表示k时刻雷达里程计获取的经纬高观测量，hLiDAR()为LiDAR的观测方程，并设置其协方差矩阵QLiDAR，初值为LiDAR所测值的标称协方差；
步骤三：依据动态融合规则选择观测...

【专利技术属性】
技术研发人员：李旭，胡悦，孔栋，朱建潇，
申请(专利权)人：东南大学，东南大学江北新区创新研究院，
类型：发明
国别省市：江苏;32