【技术实现步骤摘要】
本专利技术涉及一种基于因子图优化的定位方法及装置和系统，属于车辆定位。
技术介绍
1、目前，所有的定位技术都是以直接定位和航位推算这两类基本方法为基础实现的。直接定位采用可识别的外部信息直接确定位置，航位推算通过运动目标的一些信息来计算出目标距离初始位置的距离和方向,从而得到目标的当前位置。精确的定位数据为物体的定位、运行等提供了重要的依据，特别是在汽车领域，为了实现车辆的安全行驶，需要具有一个高精度的位置定位功能，最简单的定位方式为gps定位，但传统的gps定位导航在gps信号容易丢失的场景下，如停车场、交错高架路以及室内等环境下无法正常使用，因此又出现了开关融合定位，开关切换式融合定位使用激光雷达对gps缺失的场景进行补足，但开关切换式融合定位方案在使用中存在着不可忽视的问题，如在隧道场景中，侧向立面墙体可保证激光点云匹配的横向精度，但由于车辆前进方向上没有特征物，导致激光匹配在纵向定位上产生跳变，同时隧道中gps定位也是无效的。
技术实现思路
1、本专利技术的目的是提供一种基于因子图优化的定位方法及装置和系统，用以解决在gps信号和激光雷达特征缺失的场景下，无法实现精准定位的问题。
2、为实现上述目的，本专利技术的方案包括：
3、本专利技术的一种基于因子图优化的定位方法，包括如下过程：
4、1)对采集到的激光雷达数据、惯导数据、gps数据和底盘轮速计数据进行处理，得到激光雷达数据队列、惯导数据队列、gps投影数据队列和运动速度队列；利用激光
5、2)依据激光里程计定位结果、gps投影数据队列、惯导数据队列和运动速度队列分别构建用于构建因子图优化模型的激光里程计因子、gps因子、imu预积分因子和轮速预积分因子，以融合定位结果对应的状态变量为变量节点，以构建的因子为因子节点，利用因子节点对变量节点进行约束，从而构建因子图优化模型，并对构建的因子图优化模型进行求解，得到车辆优化后的融合定位结果；
6、其中，当运动速度队列表征车辆的相对位移或者旋转超过一定阈值时，在因子图优化模型中添加新的变量节点、因子节点以及因子节点对变量节点的约束。
7、上述技术方案的有益效果为：考虑到一些特殊场景(例如隧道)会导致激光雷达的定位在纵向定位上发生跳变或者隧道中gps定位无效的情况，本专利技术在多传感器融合定位中增加有底盘轮速计，并对激光雷达、惯导、gps和底盘轮速计四种传感器的定位结果进行融合定位。而且，将因子图优化应用于多传感器融合定位中，在应用因子图优化过程中，将速度队列计算出的相对位移或者旋转作为关键帧的触发条件，当相对位移超过一定阈值时，才在因子图中添加新的状态变量和约束因子，而不使用gps或者激光雷达作为关键帧触发条件，实现在激光雷达或gps无法使用时，导致定位异常的问题。
8、作为方法的进一步改进，所述融合定位结果对应的状态变量包括位置、姿态和速度，所述因子节点对变量节点的约束包括：
9、所述激光里程计因子对状态变量中的位置和姿态进行约束；
10、所述gps因子对状态变量中的位置进行约束；
11、所述imu预积分因子和轮速预积分因子对状态变量中的位置、姿态和速度进行约束。
12、上述技术方案的有益效果为：利用imu预积分因子和轮速预积分因子对位置、姿态和速度进行约束，解决之前使用惯性积分对因子图优化时，每次迭代时都需要花大量计算资源用来对数据重新积分的问题；同时采用因子图优化模型执行融合定位优化，车辆在无gps信号和激光点云特征缺失环境下的精确定位。
13、作为方法的进一步改进，在进行点云匹配过程中需要利用航迹推算结果对点云匹配结果进行矫正处理；
14、其中，结合运动速度队列、利用激光里程计定位结果和所述融合定位结果对惯导数据队列对应的定位结果进行矫正，以得到航迹推算结果。
15、上述技术方案的有益效果为：通过优化后的融合定位结果对惯导数据队列和运动速度队列中的定位数据进行优化处理，得到航迹推算定位结果，利用航迹推算定位结果对激光里程计算结果进行矫正，实现在激光里程计定位无法使用场景下的精确定位；同时利用激光里程计定位结果对航迹推算定位结果进行优化矫正，防止惯性里程计发散对系统稳定性造成影响。
16、作为方法的进一步改进，采用ndt匹配算法实现所述点云匹配。
17、作为方法的进一步改进，得到激光雷达数据队列的处理手段为：对激光雷达数据进行点云畸变校正处理，所述点云畸变校正步骤包括：
18、a.依据某一帧激光雷达采集数据时的时间戳与某一时刻惯导里程计对应的imu时间戳相减得到时间差，判定时间差与同步阈的大小，以实现激光雷达和imu的时间戳同步，同时将时间差小于同步阈时对应的imu数据作为同步数据输出；
19、b.对单帧点云进行分线束，并记录每个点所属线束和每个点在此帧点云内的相对扫描时间，以实现激光无序点云的有序转换；
20、c.依据imu在世界坐标系下的加速度，得到每一帧imu数据在世界坐标系下对应的位移和速度，并对其进行插值后计算每一个激光点相对于起始点扫描时刻激光点的补偿变换矩阵，依据补偿变换矩阵对激光雷达数据进行修正。
21、上述技术方案的有益效果为：通过使用惯导中提供的车辆惯性运动信息，对由于运动引起的激光雷达单帧数据畸变进行矫正。
22、作为方法的进一步改进，得到gps投影数据队列的处理手段为：对gps数据中的经度、纬度、高程进行投影映射处理，以映射至局部笛卡尔坐标系。
23、作为方法的进一步改进，所述得到运动速度队列的处理手段为：所述得到运动速度队列的处理手段为：对底盘轮速计数据进行车辆运动模型解算处理，将底盘反馈的运动信息转变为车辆刚体运动状态。
24、上述技术方案的有益效果为：通过运动模型解算将底盘反馈的运动信息转变为车辆刚体运动状态，为车辆定位提供速度计算变量。
25、本专利技术的一种基于因子图优化的定位装置，包括存储器和处理器，所述处理器用于执行存储在存储器中的计算机程序指令以实现上述所述的基于因子图优化的定位方法。
26、本专利技术的一种基于因子图优化的定位系统，包括激光雷达、惯导和gps，还包括底盘轮速计和上述所述的基于因子图优化的定位装置。
27、上述技术方案的有益效果为：利用激光雷达、惯导、gps和底盘轮速计传感器采集车辆定位计算时的位置、姿态和速度，并采用本专利技术中的种基于因子图优化的定位装置进行控制，实现在激光雷达或gps无法使用时，导致定位异常的问题。
本文档来自技高网...【技术保护点】
1.一种基于因子图优化的定位方法，其特征在于，包括如下过程：
2.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，所述融合定位结果对应的状态变量包括位置、姿态和速度，所述因子节点对变量节点的约束包括：
3.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，在进行点云匹配过程中需要利用航迹推算结果对点云匹配结果进行矫正处理；
4.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，采用NDT匹配算法实现所述点云匹配。
5.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，得到激光雷达数据队列的处理手段为：对激光雷达数据进行点云畸变校正处理，所述点云畸变校正步骤包括：
6.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，所述得到GPS投影数据队列的处理手段为：对GPS数据中的经度、纬度、高程进行投影映射处理，以映射至局部笛卡尔坐标系。
7.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，所述得到运动速度队列的处理手段为：对底盘轮速计数据进行车辆运动模型解算处理，
8.一种基于因子图优化的定位装置，其特征在于，包括存储器和处理器，所述处理器用于执行存储在存储器中的计算机程序指令以实现如权利要求1-7任一项所述的基于因子图优化的定位方法。
9.一种基于因子图优化的定位系统，包括激光雷达、惯导和GPS，其特征在于，还包括底盘轮速计和如权利要求8所述的基于因子图优化的定位装置。
...【技术特征摘要】
1.一种基于因子图优化的定位方法，其特征在于，包括如下过程：
2.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，所述融合定位结果对应的状态变量包括位置、姿态和速度，所述因子节点对变量节点的约束包括：
3.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，在进行点云匹配过程中需要利用航迹推算结果对点云匹配结果进行矫正处理；
4.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，采用ndt匹配算法实现所述点云匹配。
5.根据权利要求1所述的基于因子图优化的定位方法，其特征在于，得到激光雷达数据队列的处理手段为：对激光雷达数据进行点云畸变校正处理，所述点云畸变校正步骤包括：
6.根据权利要求1所述的...
【专利技术属性】
技术研发人员：司马浩东，鲁靖文，郭晨策，翟燕，楚坤棚，刘建伟，
申请(专利权)人：中船海为高科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。