一种基于因子图优化的定位方法及装置和系统

技术领域

本发明涉及一种基于因子图优化的定位方法及装置和系统，属于车辆定位技术领域。

背景技术

目前，所有的定位技术都是以直接定位和航位推算这两类基本方法为基础实现的。直接定位采用可识别的外部信息直接确定位置，航位推算通过运动目标的一些信息来计算出目标距离初始位置的距离和方向,从而得到目标的当前位置。精确的定位数据为物体的定位、运行等提供了重要的依据，特别是在汽车领域，为了实现车辆的安全行驶，需要具有一个高精度的位置定位功能，最简单的定位方式为GPS定位，但传统的GPS定位导航在GPS信号容易丢失的场景下，如停车场、交错高架路以及室内等环境下无法正常使用，因此又出现了开关融合定位，开关切换式融合定位使用激光雷达对GPS缺失的场景进行补足，但开关切换式融合定位方案在使用中存在着不可忽视的问题，如在隧道场景中，侧向立面墙体可保证激光点云匹配的横向精度，但由于车辆前进方向上没有特征物，导致激光匹配在纵向定位上产生跳变，同时隧道中GPS定位也是无效的。

发明内容

本发明的目的是提供一种基于因子图优化的定位方法及装置和系统，用以解决在GPS信号和激光雷达特征缺失的场景下，无法实现精准定位的问题。

为实现上述目的，本发明的方案包括：

本发明的一种基于因子图优化的定位方法，包括如下过程：

1)对采集到的激光雷达数据、惯导数据、GPS数据和底盘轮速计数据进行处理，得到激光雷达数据队列、惯导数据队列、GPS投影数据队列和运动速度队列；利用激光雷达数据队列进行点云匹配，依据匹配结果得到激光里程计定位结果；

2)依据激光里程计定位结果、GPS投影数据队列、惯导数据队列和运动速度队列分别构建用于构建因子图优化模型的激光里程计因子、GPS因子、IMU预积分因子和轮速预积分因子，以融合定位结果对应的状态变量为变量节点，以构建的因子为因子节点，利用因子节点对变量节点进行约束，从而构建因子图优化模型，并对构建的因子图优化模型进行求解，得到车辆优化后的融合定位结果；

其中，当运动速度队列表征车辆的相对位移或者旋转超过一定阈值时，在因子图优化模型中添加新的变量节点、因子节点以及因子节点对变量节点的约束。

上述技术方案的有益效果为：考虑到一些特殊场景(例如隧道)会导致激光雷达的定位在纵向定位上发生跳变或者隧道中GPS定位无效的情况，本发明在多传感器融合定位中增加有底盘轮速计，并对激光雷达、惯导、GPS和底盘轮速计四种传感器的定位结果进行融合定位。而且，将因子图优化应用于多传感器融合定位中，在应用因子图优化过程中，将速度队列计算出的相对位移或者旋转作为关键帧的触发条件，当相对位移超过一定阈值时，才在因子图中添加新的状态变量和约束因子，而不使用GPS或者激光雷达作为关键帧触发条件，实现在激光雷达或GPS无法使用时，导致定位异常的问题。

作为方法的进一步改进，所述融合定位结果对应的状态变量包括位置、姿态和速度，所述因子节点对变量节点的约束包括：

所述激光里程计因子对状态变量中的位置和姿态进行约束；

所述GPS因子对状态变量中的位置进行约束；

所述IMU预积分因子和轮速预积分因子对状态变量中的位置、姿态和速度进行约束。

上述技术方案的有益效果为：利用IMU预积分因子和轮速预积分因子对位置、姿态和速度进行约束，解决之前使用惯性积分对因子图优化时，每次迭代时都需要花大量计算资源用来对数据重新积分的问题；同时采用因子图优化模型执行融合定位优化，车辆在无GPS信号和激光点云特征缺失环境下的精确定位。

作为方法的进一步改进，在进行点云匹配过程中需要利用航迹推算结果对点云匹配结果进行矫正处理；

其中，结合运动速度队列、利用激光里程计定位结果和所述融合定位结果对惯导数据队列对应的定位结果进行矫正，以得到航迹推算结果。

上述技术方案的有益效果为：通过优化后的融合定位结果对惯导数据队列和运动速度队列中的定位数据进行优化处理，得到航迹推算定位结果，利用航迹推算定位结果对激光里程计算结果进行矫正，实现在激光里程计定位无法使用场景下的精确定位；同时利用激光里程计定位结果对航迹推算定位结果进行优化矫正，防止惯性里程计发散对系统稳定性造成影响。

作为方法的进一步改进，采用NDT匹配算法实现所述点云匹配。

作为方法的进一步改进，得到激光雷达数据队列的处理手段为：对激光雷达数据进行点云畸变校正处理，所述点云畸变校正步骤包括：

a.依据某一帧激光雷达采集数据时的时间戳与某一时刻惯导里程计对应的IMU时间戳相减得到时间差，判定时间差与同步阈的大小，以实现激光雷达和IMU的时间戳同步，同时将时间差小于同步阈时对应的IMU数据作为同步数据输出；

b.对单帧点云进行分线束，并记录每个点所属线束和每个点在此帧点云内的相对扫描时间，以实现激光无序点云的有序转换；

c.依据IMU在世界坐标系下的加速度，得到每一帧IMU数据在世界坐标系下对应的位移和速度，并对其进行插值后计算每一个激光点相对于起始点扫描时刻激光点的补偿变换矩阵，依据补偿变换矩阵对激光雷达数据进行修正。

上述技术方案的有益效果为：通过使用惯导中提供的车辆惯性运动信息，对由于运动引起的激光雷达单帧数据畸变进行矫正。

作为方法的进一步改进，得到GPS投影数据队列的处理手段为：对GPS数据中的经度、纬度、高程进行投影映射处理，以映射至局部笛卡尔坐标系。

作为方法的进一步改进，所述得到运动速度队列的处理手段为：所述得到运动速度队列的处理手段为：对底盘轮速计数据进行车辆运动模型解算处理，将底盘反馈的运动信息转变为车辆刚体运动状态。

上述技术方案的有益效果为：通过运动模型解算将底盘反馈的运动信息转变为车辆刚体运动状态，为车辆定位提供速度计算变量。

本发明的一种基于因子图优化的定位装置，包括存储器和处理器，所述处理器用于执行存储在存储器中的计算机程序指令以实现上述所述的基于因子图优化的定位方法。

本发明的一种基于因子图优化的定位系统，包括激光雷达、惯导和GPS，还包括底盘轮速计和上述所述的基于因子图优化的定位装置。

上述技术方案的有益效果为：利用激光雷达、惯导、GPS和底盘轮速计传感器采集车辆定位计算时的位置、姿态和速度，并采用本发明中的种基于因子图优化的定位装置进行控制，实现在激光雷达或GPS无法使用时，导致定位异常的问题。

附图说明

图1是本发明的整体结构框架图；

图2是本发明的整体工作流程图；

图3是本发明的数据处理层框架构图；

图4是本发明的数据处理层工作流程图；

图5是本发明的前端处理层框架构图；

图6是本发明的前端处理层工作流程构图；

图7是本发明的后端优化层框架构图；

图8是本发明的后端优化层工作流程图；

图9是本发明的因子图模型示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明了，下面结合附图及实施例，对本发明作进一步的详细说明。

本发明的目的是提供一种基于因子图优化的定位方法、一种基于因子图优化的定位装置、一种基于因子图优化的定位系统，通过构建因子图优化模型，得到优化后的融合定位结果，当运动速度计算出的相对位移超过一定阈值时，输出一组约束因子对激光里程计定位结果及获得航迹推算定位结果的惯导数据队列和运动速度队列的定位数据值进行矫正优化，实现在GPS信号和激光雷达特征缺失的场景下精确定位，提高了融合定位的准确性和实时性。

一种基于因子图优化的定位方法实施例：

如图1和图2所示，本实施例通过数据处理层、前端处理层和后端优化层实现在无GPS信号和激光点云特征缺失环境下的精确定位。具体实现包括如下过程：

一、数据处理层：如图3和图4所示，对来自硬件平台的激光雷达数据、GPS数据和线控底盘轮速数据分别进行点云畸变校正、GPS投影映射和车辆运动模型解算处理后再结合惯导原始数据，得到标准的定位传感器数据队列：激光雷达数据队列、GPS投影数据队列、运动数据队列和惯导数据队列)；

具体地，对激光雷达原始数据进行点云畸变矫正的过程为：使用惯导中提供的车辆惯性运动信息，对由于运动引起的激光雷达单帧数据畸变进行矫正(激光点云畸变矫正)。具体分为以下几个步骤：

1)激光雷达和IMU的时间戳同步。根据激光雷达和IMU自带的时间戳，获取距离某一帧点云时间戳最近的IMU数据，时间戳相减求出差值，如果时间差小于同步阈值则作为同步数据输出；如果时间差大于同步阈值则丢弃此数据，拿下一帧激光雷达采样数据循环上述过程。

2)无序点云的有序转换。激光点云数据是无序的。对单帧点云进行分线束，并记录每个点所属线束和每个点在此帧点云内的相对扫描时间。

3)计算载体运动信息。获取IMU坐标系下三个轴相对于世界坐标系的欧拉角和三个轴上的加速度，然后去除重力影响获得IMU在世界坐标系下的加速度，得到每一帧IMU数据在世界坐标系下对应的位移和速度。最后使用得到的运动信息进行插值，计算每一个激光点相对于起始点扫描时刻激光点的补偿变换矩阵,通过补偿变换矩阵对激光雷达数据进行修正，得到去畸变后的激光雷达点云数据队列。

对GPS原始数据的投影映射过程为：对GPS原始数据中的经度、纬度、高程进行映射，形成局部的笛卡尔坐标系坐标。局部笛卡尔坐标的生成过程如下：

a.设置局部笛卡尔坐标系原点。原点通常选择在场景的中心位置以减小整体变换畸变；

b.使用WGS84椭球参数，通过地心坐标系对经度、纬度、高程(大地坐标系)进行变换。其中局部笛卡尔坐标系Y坐标轴对应正北方向，Z坐标轴垂直于椭球体。

对轮速计原始数据的车辆模型结算过程为：通过运动模型解算将底盘反馈的运动信息(四轮轮速信息、车速、转向角等数据)转变为车辆刚体运动状态。本实例车辆模型可简化为自行车模型，通过底盘控制器输出前轮轴中心位置的转向以及后轮轴中心位置的速度以及运动学模型，可以计算出以车辆后轮轴中心位置为坐标系原点的刚体运动速度：

二、前端处理层：如图5和图6所示，前端处理层主要包括点云匹配模块和航迹推算模块。在点云匹配模块中对激光里程计的实现进行设计，在航迹推算模块中，对基于惯导和底盘刚体运动线速度/角速度的里程计方法、激光里程计的优化以及惯性数据队列的数据进行优化。

具体地，点云匹配：采用NDT匹配，NDT匹配的建模过程中，把点云按体素分块，然后用三维正态分布参数描述体素块内的点云分布。匹配的点云采用某组变换参数后，使得新的点云和目标点云的正态分布参数匹配，那么变换点在参考系中的概率密度将会很大。使用优化的方法求出使得概率密度之和最大的变换参数，即为激光里程计的输出结果。

航迹推算：通过惯导队列以及运动速度队列的数据，获得车辆刚体运动的角速度(3元素向量)、线加速度(3元素向量)以及线速度(3元素向量)，通过积分的方式测得车辆的相对运动里程计。

由于惯性数据的测量会出现测量误差、零位偏置等噪声误差，不经过优化的惯性里程计会迅速发散，因此可从前端和后端两个层面对惯性里程计的结果进行优化：

前端优化矫正：通过前端激光里程计的输出定位结果，对航迹推算结果进行优化矫正。此优化的更新频率受点云匹配速率影响，属于中频校正，目的是防止惯性里程计发散对系统稳定性造成影响。

后端优化矫正：后端优化体现在对前端及前端激光里程计的矫正，以及对惯导队列的偏置进行矫正。此优化的更新频率受后端优化速率影响，属于低频校正，目的是对航迹推算的结果进行精确矫正。

三、后端优化层：如图7和图8所示，后端优化层对激光里程计定位结果、GPS投影数据队列、惯导数据队列以及运动速度队列进行处理，形成构建因子图的约束因子，即激光里程计因子、GPS因子以及IMU预积分因子和轮速预积分因子。对因子图执行融合定位优化后，输出优化结果给前端处理层的航迹推算模块，为惯性里程计提供优化输入。定位优化变量包含位置(x,y,z),姿态(roll,pitch,yaw),速度(Vx,Vy,Vz)共9个优化变量。

具体地，激光里程计因子构建有两种形式：当采用点云地图匹配时，若点云地图坐标系与世界坐标系对齐，则构建单边约束；当采用帧间点云匹配时，测量结果与两帧中第一帧的坐标系对齐，属于双边约束。激光里程计因子对车辆定位优化状态变量中的位置(x,y,z)、姿态(roll,pitch,yaw)进行约束。

方案框架中世界坐标系与GPS投影形成的局部笛卡尔坐标系对齐，GPS投影数据及为车辆在世界坐标系下的位置测量，属于单边约束测量。GPS因子对车辆定位优化状态变量中的位置(x,y,z)进行约束。

对惯导数据以及车辆运动速度进行预积分处理，解决因子图优化每次迭代时都需要花大量计算资源用来对惯导数据重新积分的问题。预积分因子测量结果与两帧中第一帧的坐标系对齐，属于双边约束，对车辆定位优化状态变量中的位置(x,y,z)、姿态(roll,pitch,yaw)、速度(Vx,Vy,Vz)进行约束。

如图9所示，因子图优化是结合多传感器数据，执行融合定位优化的核心过程。具体流程如下：

1、使用运动速度计算出的相对位移作为关键帧触发条件，当车辆位移超过一定阈值时，才在因子图中添加新的状态变量和约束因子。

2、构建定位状态变量，并添加约束因子，定位状态变量为惯导数据队列和运动速度队列组合得到的预积分因子。

3、将构建的因子图转换为优化方程，并配置解算器参数。

4、对优化方式进行求解，输出后端优化结果。

基于因子图优化的定位装置实施例：

基于因子图优化的定位系统，包括存储器和处理器，存储器和处理器之间通过内部总线进行数据交互。处理器用于执行存储在存储器中的计算机程序指令，以实现本发明的基于因子图优化的定位方法，关于该方法的具体内容已在基于因子图优化的定位方法实施例中做了详细介绍，本实施例不再赘述。其中，存储器可以选择半导体存储器、磁表面存储器、只读存储器、顺序存储器等，处理器可以选择英特尔处理器、MD处理器、IBM处理器、联发科处理器等。

基于因子图优化的定位系统实施例：

基于因子图优化的定位系统，利用激光雷达传感器、惯导传感器、GPS传感器、底盘轮速计传感器采集车辆定位中涉及的位置、姿态和速度状态对应的数据，依据采集到的数据以实现本发明的基于因子图优化的定位装置，关于该装置的具体内容已在基于因子图优化的定位装置实施例中做了详细介绍，本实施例不再赘述。

本发明通过数据处理层对激光里程计定位结果、GPS投影数据队列、惯导数据队列以及运动速度队列进行处理；在前端处理层完成里程计的实现，以及后端优化结果对前端里程计的优化过程；然后后端优化层形成构建因子图的激光里程计因子、GPS因子以及预积分因子；最后进行因子图优化的过程，添加约束因子，执行优化，实现无人车在无GPS信号和激光点云特征缺失环境下的精确定位。

以上实施例仅用以说明本发明的技术方案而非对其限制，所属领域的技术人员阅读本申请后，参照上述实施例对本发明进行的各种修改或变更的行为，均在本发明专利的权利申请要求保护范围之内。