CN115728803A - 一种城市驾驶车辆连续定位系统及方法 - Google Patents

Abstract

本发明提出了一种城市驾驶车辆连续定位系统及方法。本发明方法构建城市环境三维地图；将相邻两个时刻的环境三维点云通过配准处理得到相对位姿变换以及当前时刻的环境三维点云中车辆位置；结合相对位姿进行坐标变换处理，得到当前时刻城市环境三维地图匹配位置；通过筛选得到当前时刻的环境三维点云的筛选后多个体素；计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重，并进一步计算配准得分；根据配准得分计算当前时刻的相对位姿权重矩阵；结合当前时刻的相对位姿权重矩阵、GNSS权重、环境三维地图匹配位置权重通过图优化方法得到融合后的位置；进一步进行修正得到修正后相对位姿。本发明提高了复杂城市环境中定位精度以及可用性。

Description

一种城市驾驶车辆连续定位系统及方法

技术领域

本发明属于多传感器融合定位技术领域，尤其涉及一种城市驾驶车辆连续定位系统及方法。

背景技术

全球卫星导航定位系统(Global Navigation Satellite System,GNSS) 具有诸多的优点，包括定位快速高效、省时且精度高等，在室外定位领域应用非常普遍。然而GNSS信号的传播很容易受到环境的阻碍，在城市环境中由于高楼、高架桥等人工建筑物遮挡的原因，很难保证GNSS信号的稳定性，在地下停车场等环境中更是难以接收到GNSS信号；同时，由于玻璃幕墙的反射，GNSS 信号会出现多路径效应，也会影响GNSS的定位精度。因此有关GNSS不可用环境下定位已成为一个迫切需要解决的科学问题，相关方法和技术成果不断涌现。

由于激光雷达生产制造技术的快速发展和在自动驾驶中的广泛应用，基于激光雷达的定位技术——激光里程计成为现在的研究热点，这种定位方法根据前后两帧激光雷达数据，计算载体的位置和姿态变化，从而实现连续定位。同时，这种方法还可以作为同时定位与制图技术的一部分，在实现定位的基础上构建周围环境的三维点云地图。但激光里程计是一种递推计算位置的方法，存在误差累积的问题。相比激光里程计只能提供相对位置和姿态，激光雷达全局定位方法可以提供载体在地理坐标系下的绝对位置和姿态。激光雷达全局定位方法通过将激光雷达扫描数据与预先构建的三维地图进行匹配，从而提供绝对位置和姿态。激光雷达扫描数据与三维地图之间的匹配是基于各种描述符算子来计算，这些算子可以人工定义或通过深度学习来构建。由于单一定位方法往往有很大的局限性，现代定位系统一般可融合多种定位技术，以实现精度和连续性的互补。但是这些融合方法一般只融合了激光里程计和GNSS，在GNSS长时间不能工作时，其定位精度仍会持续降低，一方面需要寻找另一种绝对定位方法在GNSS不可用时起到一定的替代作用，另一方面需要研究如何降低激光里程计的误差发散速度，以保证复杂场景中定位的高精度和高可用性。

发明内容

为了解决现有定位技术在复杂城市环境中的定位精度低和可用性差的问题，提出了一种城市驾驶车辆连续定位系统及方法。

本发明系统的技术方案为一种城市驾驶车辆连续定位系统，包括：三维激光雷达、上位机、GNSS定位模块、POS系统；

所述的三维激光雷达、GNSS定位模块、POS系统安装于车辆顶部，上位机安装于车辆内部；

所述上位机分别与所述的三维激光雷达、GNSS定位模块、POS系统依次连接；

所述三维激光雷达用于采集多个时刻的环境三维点云、多个时刻的扫描线字段，将多个时刻的环境三维点云传输至所述上位机；

所述GNSS定位模块用于采集多个时刻的车辆位置，将多个时刻的车辆位置传输至所述上位机；

所述POS系统用于采集多个时刻的车辆位姿，将多个时刻的车辆位姿传输至所述上位机；

所述上位机构建城市环境三维地图；将相邻两个时刻的环境三维点云通过配准处理，得到相邻两个时刻的姿态变换、相邻两个时刻的平移量，构建相邻两个时刻的相对位姿变换，进一步得到当前时刻的环境三维点云中车辆位置；结合相对位姿进行坐标变换处理得到当前时刻城市环境三维地图匹配位置；通过筛选得到当前时刻的环境三维点云的筛选后多个体素；计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重；进一步计算配准得分并根据配准得分计算当前时刻的相对位姿权重矩阵；结合当前时刻的相对位姿权重矩阵、当前时刻的GNSS权重、当前时刻的环境三维地图匹配位置权重通过图优化方法得到融合后的位置；进一步进行修正得到修正后相对位姿。

本发明方法的技术方案为一种城市驾驶车辆连续定位方法，包括以下步骤：

步骤1：上位机结合每个时刻的环境三维点云结合对应时刻的车辆位姿构建城市环境三维地图；

步骤2：上位机在多个时刻的环境三维点云中，将相邻两个时刻的环境三维点云通过配准处理，得到相邻两个时刻的姿态变换、相邻两个时刻的平移量，构建相邻两个时刻的相对位姿变换，进一步得到当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，将当前时刻的车辆位置结合相对位姿进行坐标变换处理，得到当前时刻的环境三维点云中车辆位置；

步骤3：将当前时刻的环境三维点云与城市环境三维地图通过地图匹配处理，得到当前时刻的城市环境三维地图中车辆位置，并结合当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿进行坐标变换处理，得到当前时刻城市环境三维地图匹配位置；

步骤4：将当前时刻的环境三维点云通过体素划分，得到当前时刻的环境三维点云的多个体素，将当前时刻的环境三维点云的多个体素结合体素点云阈值进行筛选，得到当前时刻的环境三维点云的筛选后多个体素；

步骤5：将当前时刻的环境三维点云的筛选后每个体素通过主成分分析方法，得到当前时刻的环境三维点云的筛选后每个体素的线性特征值、平面特征值、点状特征值，通过特征值判定方法得到当前时刻的环境三维点云的筛选后每个体素的权重，进一步得到当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重；

步骤6：将上一时刻的环境三维点云结合两个相邻时刻的相对位姿变换，得到上一时刻转换后的环境三维点云，结合上一时刻转换后的环境三维点云通过最近邻搜索方法得到当前时刻的环境三维点云筛选后每个体素中每个三维点在上一时刻转换后的环境三维点云中的对应三维点，计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的残差，进一步计算配准得分，根据配准得分计算当前时刻的相对位姿权重矩阵；

步骤7：将当前时刻的三维点云中车辆位置通过GNSS协方差矩阵方法计算当前时刻的GNSS权重，将当前时刻的城市环境三维地图匹配位置通过完备性确定方法计算当前时刻的环境三维地图匹配位置权重，结合当前时刻的相对位姿权重矩阵、当前时刻的GNSS权重、当前时刻的环境三维地图匹配位置权重通过图优化方法构建融合位置函数模型，以融合位置函数模型最小化作为优化目标，通过高斯牛顿方法进行优化求解，得到融合后的位置；

步骤8：将融合后的位置结合误差修正模型进行估计得到位姿变换累计误差，进一步修正当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，得到修正后相对位姿；

作为优选，步骤2所述相邻两个时刻的相对位姿变换，定义为：

i∈[2,K]

其中，T_i，i-1表示i时刻相对i-1时刻的相对位姿变换，R_i，i-1表示i时刻相对 i-1时刻的姿态变换，

表示i时刻相对i-1时刻的平移量，K表示时刻的数量；

步骤1所述得到当前时刻的环境三维点云与第一个时刻的环境三维点云之间的相对位姿变换，具体如下：

T_i＝T_i,i-1·T_i-1,i-2·...·T_2,1

其中，T_i表示当前时刻的环境三维点云与第一个时刻的环境三维点云之间的相对位姿变换；

步骤2所述当前时刻的环境三维点云中车辆位置，具体定义如下：

i∈[2,K]

其中，

表示第i个时刻的环境三维点云中车辆位置，x_i表示第i个时刻的环境三维点云中车辆位置的X轴坐标，y_i表示第i个时刻的环境三维点云中车辆位置的Y轴坐标，z_i表示第i个时刻的环境三维点云中车辆位置的Z轴坐标，K表示时刻的数量；

作为优选，步骤3所述当前时刻城市环境三维地图匹配位置，定义如下：

i∈[2，K]

其中，

表示第i个时刻的城市环境三维地图匹配位置，x_i表示第i个时刻的城市环境三维地图匹配位置的X轴坐标，y_i表示第i个时刻的城市环境三维地图匹配位置的Y轴坐标，z_i表示第i个时刻的城市环境三维地图匹配位置的Z轴坐标，K表示时刻的数量。

作为优选，步骤4所述将当前时刻的环境三维点云的多个体素结合体素点云阈值进行筛选，具体如下：

在当前时刻的环境三维点云的多个体素中筛选出体素中三维点的数量大于体素点云阈值的体素，以构建步骤4所述的当前时刻的环境三维点云的筛选后多个体素；

作为优选，步骤5所述通过特征值判定方法得到当前时刻的环境三维点云的筛选后每个体素的权重，具体如下：

若当前时刻环境三维点云的筛选后每个体素的平面特征值最大，则当前时刻的环境三维点云的筛选后每个体素的权重为0；

若当前时刻环境三维点云的筛选后每个体素的点状特征值最大，则当前时刻的环境三维点云的筛选后每个体素的权重为1；

若环境三维点云的筛选后每个体素的线性特征值最大：

根据当前时刻的扫描线字段判断体素中的点是否来自同一扫描线，若当前时刻的环境三维点云的筛选后每个体素的三维点都来自于同一扫描线，当前时刻的环境三维点云的筛选后每个体素的权重为0；

根据当前时刻的扫描线字段判断体素中的点是否来自同一扫描线，若当前时刻的环境三维点云的筛选后每个体素的三维点来自于不同的扫描线则为环境中的真实线特征，当前时刻的环境三维点云的筛选后每个体素的权重为1；

当前时刻的环境三维点云的筛选后每个体素的权重具体定义为：

i∈[2，K]

λ∈[1,V_i]

其中，V_i表示i时刻环境三维点云的筛选后体素的数量，

表示i时刻环境三维点云的第λ个筛选后体素的权重，K表示时刻的数量；

步骤5所述进一步得到当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重，具体如下：

当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重为对应的筛选后每个体素的权重，具体定义为：

i∈[2，K]，λ∈[1，V_i]

其中，V_i表示i时刻环境三维点云的筛选后体素的数量，

表示i时刻环境三维点云的第λ个筛选后的体素中第r个三维点的权重，K表示时刻的数量；

作为优选，步骤6所述计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的残差，具体公式如下：

i∈[2，K]

λ∈[1，V_i]

r∈[1，N_λ]

其中，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点的残差，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点在i-1时刻环境三维点云中的对应点，N_λ表示i时刻环境三维点云筛选后第λ个体素中三维点的数量，V_i表示 i时刻环境三维点云筛选后体素的数量，表示K表示时刻数量。

步骤6所述进一步计算配准得分，具体如下：

i∈[2，K]

λ∈[1，V_i]

r∈[1，N_λ]

其中，

表示i时刻环境三维点云的第λ个筛选后的体素中第r个三维点的权重，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点的残差， score_i表示i时刻相对位姿的得分，N_λ表示i时刻环境三维点云筛选后第λ个体素中三维点的数量，V_i表示i时刻环境三维点云筛选后体素的数量，表示K表示时刻数量。

步骤6所述根据配准得分计算当前时刻的相对位姿权重矩阵，具体如下：

i∈[2，K]

其中，Ω_i表示i时刻的相对位姿的权重矩阵，score_i是i时刻相对位姿配准得分，I是单位矩阵，K表示时刻数量。

作为优选，步骤7所述融合后的位置具体定义为：

i∈[2,Kl

其中，

表示第i个时刻的融合后位置，x_i表示第i个时刻的融合后位置的X轴坐标，y_i表示第i个时刻的融合后位置的Y轴坐标，z_i表示第i个时刻的融合后位置的Z轴坐标，K表示时刻的数量；

作为优选，步骤8所述将融合后的位置结合误差修正模型进行估计得到相对位姿变换累计误差，具体为：

具体地，误差定义为实时相对位姿变换与步骤7融合后位置的差值。漂移误差修正模型定义为下式：

i∈[2,K]

其中，Δe_i为i时刻的相对位姿变换误差模型，

和

分别为i时刻相对位姿变换位置和i时刻的融合后位置，

表示n+1时刻的融合后位置，

表示n 时刻的融合后位置，j表示上一次更新相对位姿变换误差模型的时刻，K表示时刻的数量。

当有三维点云中车辆位置或城市环境三维地图匹配位置时，可以使用上式更新相对位姿变换误差模型。

步骤8所述进一步修正当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，具体如下：

i∈[2，K]，h＜i

式中，

表示i时刻相对位姿变换的位置，t′_i表示i时刻经过误差校正后的位置，t_n+1表示n+1时刻相对位姿变换的位置，t_n表示n时刻相对位姿变换的位置，Δe_h为h时刻的相对位姿变换误差模型，实时输出校正后的位姿t′_i作为最终定位结果。

本发明通过融合GNSS位置、地图匹配定位结果、激光里程计位置，计算当前位置的最优估计；使用融合后的位置修正激光里程计累计误差，输出实时的定位结果。该方法有效克服了卫星定位系统在复杂城市环境中定位精度差、可用性低的问题，并使用地图匹配和传统卫星定位一起修正激光里程计误差，提高了系统的鲁棒性。

附图说明

图1：本发明实施例的方法流程示意图；

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。

本发明实施例系统的技术方案为一种城市驾驶车辆连续定位系统，包括：三维激光雷达、上位机、GNSS定位模块、POS系统；

所述的三维激光雷达、GNSS定位模块、POS系统安装于车辆顶部，上位机安装于车辆内部；

所述上位机分别与所述的三维激光雷达、GNSS定位模块、POS系统依次连接；

所述三维激光雷达用于采集多个时刻的环境三维点云、多个时刻的扫描线字段，将多个时刻的环境三维点云传输至所述上位机；

所述GNSS定位模块用于采集多个时刻的车辆位置，将多个时刻的车辆位置传输至所述上位机；

所述POS系统用于采集多个时刻的车辆位姿，将多个时刻的车辆位姿传输至所述上位机；

所述上位机根据多个时刻的环境三维点云、多个时刻的扫描线字段、多个时刻的车辆位姿、多个时刻的车辆位置通过城市驾驶车辆连续定位方法，实现车辆的精准定位；

所述三维激光雷达的型号为；禾赛XT32激光雷达

所述上位机的型号为；英特尔NUC 12

所述GNSS定位模块的型号为；Ublox F9P

所述POS系统的型号为；Applanix POS LV

下面结合图1介绍本发明实施例提供的一种城市驾驶车辆连续定位方法，包括以下步骤：

步骤1：上位机结合每个时刻的环境三维点云结合对应时刻的车辆位姿构建城市环境三维地图；

步骤2：上位机在多个时刻的环境三维点云中，将相邻两个时刻的环境三维点云通过配准处理，得到相邻两个时刻的姿态变换、相邻两个时刻的平移量，构建相邻两个时刻的相对位姿变换，进一步得到当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，将当前时刻的车辆位置结合相对位姿进行坐标变换处理，得到当前时刻的环境三维点云中车辆位置；

步骤2所述相邻两个时刻的相对位姿变换，定义为：

i∈[2,K]

其中，T_i，i-1表示i时刻相对i-1时刻的相对位姿变换，R_i，i-1表示i时刻相对 i-1时刻的姿态变换，

表示i时刻相对i-1时刻的平移量，K＝3000表示时刻的数量；

步骤1所述得到当前时刻的环境三维点云与第一个时刻的环境三维点云之间的相对位姿变换，具体如下：

T_i＝T_i,i-1·T_i-1,i-2·...·T_2,1

其中，T_i表示当前时刻的环境三维点云与第一个时刻的环境三维点云之间的相对位姿变换；

步骤2所述当前时刻的环境三维点云中车辆位置，具体定义如下：

i∈[2,K]

其中，

表示第i个时刻的环境三维点云中车辆位置，x_i表示第i个时刻的环境三维点云中车辆位置的X轴坐标，y_i表示第i个时刻的环境三维点云中车辆位置的Y轴坐标，z_i表示第i个时刻的环境三维点云中车辆位置的Z轴坐标，K＝3000表示时刻的数量；

步骤3：将当前时刻的环境三维点云与城市环境三维地图通过地图匹配处理，得到当前时刻的城市环境三维地图中车辆位置，并结合当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿进行坐标变换处理，得到当前时刻城市环境三维地图匹配位置；

步骤3所述当前时刻城市环境三维地图匹配位置，定义如下：

i∈[2,K]

其中，

表示第i个时刻的城市环境三维地图匹配位置，x_i表示第i个时刻的城市环境三维地图匹配位置的X轴坐标，y_i表示第i个时刻的城市环境三维地图匹配位置的Y轴坐标，z_i表示第i个时刻的城市环境三维地图匹配位置的Z轴坐标，K＝3000表示时刻的数量。

步骤4：将当前时刻的环境三维点云通过体素划分，得到当前时刻的环境三维点云的多个体素，将当前时刻的环境三维点云的多个体素结合体素点云阈值进行筛选，得到当前时刻的环境三维点云的筛选后多个体素；

步骤4所述将当前时刻的环境三维点云的多个体素结合体素点云阈值进行筛选，具体如下：

在当前时刻的环境三维点云的多个体素中筛选出体素中三维点的数量大于体素点云阈值的体素，以构建步骤4所述的当前时刻的环境三维点云的筛选后多个体素；

步骤5：将当前时刻的环境三维点云的筛选后每个体素通过主成分分析方法，得到当前时刻的环境三维点云的筛选后每个体素的线性特征值、平面特征值、点状特征值，通过特征值判定方法得到当前时刻的环境三维点云的筛选后每个体素的权重，进一步得到当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重；

步骤5所述通过特征值判定方法得到当前时刻的环境三维点云的筛选后每个体素的权重，具体如下：

若当前时刻环境三维点云的筛选后每个体素的平面特征值最大，则当前时刻的环境三维点云的筛选后每个体素的权重为0；

若当前时刻环境三维点云的筛选后每个体素的点状特征值最大，则当前时刻的环境三维点云的筛选后每个体素的权重为1；

若环境三维点云的筛选后每个体素的线性特征值最大：

根据当前时刻的扫描线字段判断体素中的点是否来自同一扫描线，若当前时刻的环境三维点云的筛选后每个体素的三维点都来自于同一扫描线，当前时刻的环境三维点云的筛选后每个体素的权重为0；

根据当前时刻的扫描线字段判断体素中的点是否来自同一扫描线，若当前时刻的环境三维点云的筛选后每个体素的三维点来自于不同的扫描线则为环境中的真实线特征，当前时刻的环境三维点云的筛选后每个体素的权重为1；

当前时刻的环境三维点云的筛选后每个体素的权重具体定义为：

i∈[2，K]

λ∈[1，V_i]

其中，V_i＝700表示i时刻环境三维点云的筛选后体素的数量，

表示i时刻环境三维点云的第λ个筛选后体素的权重，K＝3000表示时刻的数量；

步骤5所述进一步得到当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重，具体如下：

当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重为对应的筛选后每个体素的权重，具体定义为：

i∈[2，K]，λ∈[1，V_i]

其中，V_i＝700表示i时刻环境三维点云的筛选后体素的数量，

表示i时刻环境三维点云的第λ个筛选后的体素中第r个三维点的权重，K＝3000表示时刻的数量；

步骤6：将上一时刻的环境三维点云结合两个相邻时刻的相对位姿变换，得到上一时刻转换后的环境三维点云，结合上一时刻转换后的环境三维点云通过最近邻搜索方法得到当前时刻的环境三维点云筛选后每个体素中每个三维点在上一时刻转换后的环境三维点云中的对应三维点，计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的残差，进一步计算配准得分，根据配准得分计算当前时刻的相对位姿权重矩阵；

步骤6所述计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的残差，具体公式如下：

i∈[2，K]

λ∈[1，V_i]

r∈[1，N_λ]

其中，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点的残差，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点在i-1时刻环境三维点云中的对应点，N_λ表示i时刻环境三维点云筛选后第λ个体素中三维点的数量，V_i＝700 表示i时刻环境三维点云筛选后体素的数量，表示K＝3000表示时刻数量。

步骤6所述进一步计算配准得分，具体如下：

i∈[2，K]

λ∈[1，V_i]

r∈[1，N_λ]

其中，

表示i时刻环境三维点云的第λ个筛选后的体素中第r个三维点的权重，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点的残差， score_i表示i时刻相对位姿的得分，N_λ表示i时刻环境三维点云筛选后第λ个体素中三维点的数量，V_i＝700表示i时刻环境三维点云筛选后体素的数量，表示 K＝3000表示时刻数量。

步骤6所述根据配准得分计算当前时刻的相对位姿权重矩阵，具体如下：

i∈[2,K]

其中，Ω_i表示i时刻的相对位姿的权重矩阵，score_i是i时刻相对位姿配准得分，I是单位矩阵，K＝3000表示时刻数量。

步骤7：将当前时刻的三维点云中车辆位置通过GNSS协方差矩阵方法计算当前时刻的GNSS权重，将当前时刻的城市环境三维地图匹配位置通过完备性确定方法计算当前时刻的环境三维地图匹配位置权重，结合当前时刻的相对位姿权重矩阵、当前时刻的GNSS权重、当前时刻的环境三维地图匹配位置权重通过图优化方法构建融合位置函数模型，以融合位置函数模型最小化作为优化目标，通过高斯牛顿方法进行优化求解，得到融合后的位置；

步骤7所述融合后的位置具体定义为：

i∈[2，K]

其中，

表示第i个时刻的融合后位置，x_i表示第i个时刻的融合后位置的X轴坐标，y_i表示第i个时刻的融合后位置的Y轴坐标，z_i表示第i个时刻的融合后位置的Z轴坐标，K表示时刻的数量；

步骤8：将融合后的位置结合误差修正模型进行估计得到位姿变换累计误差，进一步修正当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，得到修正后相对位姿；

步骤8所述将融合后的位置结合误差修正模型进行估计得到相对位姿变换累计误差，具体为：

具体地，误差定义为实时相对位姿变换与步骤7融合后位置的差值。漂移误差修正模型定义为下式：

i∈[2,K]

其中，Δe_i为i时刻的相对位姿变换误差模型，

和

分别为i时刻相对位姿变换位置和i时刻的融合后位置，

表示n+1时刻的融合后位置，

表示n 时刻的融合后位置，j表示上一次更新相对位姿变换误差模型的时刻，K＝3000 表示时刻的数量。

当有三维点云中车辆位置或城市环境三维地图匹配位置时，可以使用上式更新相对位姿变换误差模型。

步骤8所述进一步修正当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，具体如下：

i∈[2，K]，h＜i

式中，

表示i时刻相对位姿变换的位置，t′_i表示i时刻经过误差校正后的位置，t_n+1表示n+1时刻相对位姿变换的位置，t_n表示n时刻相对位姿变换的位置，Δe_h为h时刻的相对位姿变换误差模型，实时输出校正后的位姿t′_i作为最终定位结果，K＝3000表示时刻的数量。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

尽管本文较多地使用了三维激光雷达、上位机、GNSS定位模块、POS系统等术语，但并不排除使用其他术语的可能性。使用这些术语仅仅是为了更方便的描述本发明的本质，把它们解释成任何一种附加的限制都是与本发明精神相违背的。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。

Claims (10)

1.一种城市驾驶车辆连续定位系统，其特征在于，包括：三维激光雷达、上位机、GNSS定位模块、POS系统；

所述的三维激光雷达、GNSS定位模块、POS系统安装于车辆顶部，上位机安装于车辆内部；

所述上位机分别与所述的三维激光雷达、GNSS定位模块、POS系统依次连接；

所述三维激光雷达用于采集多个时刻的环境三维点云、多个时刻的扫描线字段，将多个时刻的环境三维点云传输至所述上位机；

所述GNSS定位模块用于采集多个时刻的车辆位置，将多个时刻的车辆位置传输至所述上位机；

所述POS系统用于采集多个时刻的车辆位姿，将多个时刻的车辆位姿传输至所述上位机；

所述上位机构建城市环境三维地图；将相邻两个时刻的环境三维点云通过配准处理，得到相邻两个时刻的姿态变换、相邻两个时刻的平移量，构建相邻两个时刻的相对位姿变换，进一步得到当前时刻的环境三维点云中车辆位置；结合相对位姿进行坐标变换处理得到当前时刻城市环境三维地图匹配位置；通过筛选得到当前时刻的环境三维点云的筛选后多个体素；计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重；进一步计算配准得分并根据配准得分计算当前时刻的相对位姿权重矩阵；结合当前时刻的相对位姿权重矩阵、当前时刻的GNSS权重、当前时刻的环境三维地图匹配位置权重通过图优化方法得到融合后的位置；进一步进行修正得到修正后相对位姿。

2.一种利用权利要求1所述的城市驾驶车辆连续定位系统进行城市驾驶车辆连续定位方法，其特征在于，包括以下步骤：

步骤1：上位机结合每个时刻的环境三维点云结合对应时刻的车辆位姿构建城市环境三维地图；

步骤2：上位机在多个时刻的环境三维点云中，将相邻两个时刻的环境三维点云通过配准处理，得到相邻两个时刻的姿态变换、相邻两个时刻的平移量，构建相邻两个时刻的相对位姿变换，进一步得到当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，将当前时刻的车辆位置结合相对位姿进行坐标变换处理，得到当前时刻的环境三维点云中车辆位置；

步骤3：将当前时刻的环境三维点云与城市环境三维地图通过地图匹配处理，得到当前时刻的城市环境三维地图中车辆位置，并结合当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿进行坐标变换处理，得到当前时刻城市环境三维地图匹配位置；

步骤4：将当前时刻的环境三维点云通过体素划分，得到当前时刻的环境三维点云的多个体素，将当前时刻的环境三维点云的多个体素结合体素点云阈值进行筛选，得到当前时刻的环境三维点云的筛选后多个体素；

步骤5：将当前时刻的环境三维点云的筛选后每个体素通过主成分分析方法，得到当前时刻的环境三维点云的筛选后每个体素的线性特征值、平面特征值、点状特征值，通过特征值判定方法得到当前时刻的环境三维点云的筛选后每个体素的权重，进一步得到当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重；

步骤6：将上一时刻的环境三维点云结合两个相邻时刻的相对位姿变换，得到上一时刻转换后的环境三维点云，结合上一时刻转换后的环境三维点云通过最近邻搜索方法得到当前时刻的环境三维点云筛选后每个体素中每个三维点在上一时刻转换后的环境三维点云中的对应三维点，计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的残差，进一步计算配准得分，根据配准得分计算当前时刻的相对位姿权重矩阵；

步骤7：将当前时刻的三维点云中车辆位置通过GNSS协方差矩阵方法计算当前时刻的GNSS权重，将当前时刻的城市环境三维地图匹配位置通过完备性确定方法计算当前时刻的环境三维地图匹配位置权重，结合当前时刻的相对位姿权重矩阵、当前时刻的GNSS权重、当前时刻的环境三维地图匹配位置权重通过图优化方法构建融合位置函数模型，以融合位置函数模型最小化作为优化目标，通过高斯牛顿方法进行优化求解，得到融合后的位置；

步骤8：将融合后的位置结合误差修正模型进行估计得到位姿变换累计误差，进一步修正当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，得到修正后相对位姿。

3.根据权利要求2所述的城市驾驶车辆连续定位方法，其特征在于，

步骤2所述相邻两个时刻的相对位姿变换，定义为：

其中，T_i，i-1表示i时刻相对i-1时刻的相对位姿变换，R_i，i-1表示i时刻相对i-1时刻的姿态变换，

表示i时刻相对i-1时刻的平移量，K表示时刻的数量；

步骤2所述得到当前时刻的环境三维点云与第一个时刻的环境三维点云之间的相对位姿变换，具体如下：

T_i＝T_i,i-1·T_i-1,i-2·...·T_2,1

其中，T_i表示当前时刻的环境三维点云与第一个时刻的环境三维点云之间的相对位姿变换；

步骤2所述当前时刻的环境三维点云中车辆位置，具体定义如下：

其中，

表示第i个时刻的环境三维点云中车辆位置，x_i表示第i个时刻的环境三维点云中车辆位置的X轴坐标，y_i表示第i个时刻的环境三维点云中车辆位置的Y轴坐标，z_i表示第i个时刻的环境三维点云中车辆位置的Z轴坐标，K表示时刻的数量。

4.根据权利要求3所述的城市驾驶车辆连续定位方法，其特征在于，

步骤3所述当前时刻城市环境三维地图匹配位置，定义如下：

其中，

表示第i个时刻的城市环境三维地图匹配位置，x_i表示第i个时刻的城市环境三维地图匹配位置的X轴坐标，y_i表示第i个时刻的城市环境三维地图匹配位置的Y轴坐标，z_i表示第i个时刻的城市环境三维地图匹配位置的Z轴坐标，K表示时刻的数量。

5.根据权利要求4所述的城市驾驶车辆连续定位方法，其特征在于，

步骤4所述将当前时刻的环境三维点云的多个体素结合体素点云阈值进行筛选，具体如下：

在当前时刻的环境三维点云的多个体素中筛选出体素中三维点的数量大于体素点云阈值的体素，以构建步骤4所述的当前时刻的环境三维点云的筛选后多个体素。

6.根据权利要求5所述的城市驾驶车辆连续定位方法，其特征在于，

步骤5所述通过特征值判定方法得到当前时刻的环境三维点云的筛选后每个体素的权重，具体如下：

若当前时刻环境三维点云的筛选后每个体素的平面特征值最大，则当前时刻的环境三维点云的筛选后每个体素的权重为0；

若当前时刻环境三维点云的筛选后每个体素的点状特征值最大，则当前时刻的环境三维点云的筛选后每个体素的权重为1；

若环境三维点云的筛选后每个体素的线性特征值最大：

根据当前时刻的扫描线字段判断体素中的点是否来自同一扫描线，若当前时刻的环境三维点云的筛选后每个体素的三维点都来自于同一扫描线，当前时刻的环境三维点云的筛选后每个体素的权重为0；

根据当前时刻的扫描线字段判断体素中的点是否来自同一扫描线，若当前时刻的环境三维点云的筛选后每个体素的三维点来自于不同的扫描线则为环境中的真实线特征，当前时刻的环境三维点云的筛选后每个体素的权重为1；

当前时刻的环境三维点云的筛选后每个体素的权重具体定义为：

其中，V_i表示i时刻环境三维点云的筛选后体素的数量，

表示i时刻环境三维点云的第λ个筛选后体素的权重，K表示时刻的数量。

7.根据权利要求6所述的城市驾驶车辆连续定位方法，其特征在于，

步骤5所述进一步得到当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重，具体如下：

当前时刻的环境三维点云的筛选后每个体素中每个三维点的权重为对应的筛选后每个体素的权重，具体定义为：

其中，V_i表示i时刻环境三维点云的筛选后体素的数量，

表示i时刻环境三维点云的第λ个筛选后的体素中第r个三维点的权重，K表示时刻的数量。

8.根据权利要求7所述的城市驾驶车辆连续定位方法，其特征在于，

步骤6所述计算当前时刻的环境三维点云的筛选后每个体素中每个三维点的残差，具体公式如下：

其中，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点的残差，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点在i-1时刻环境三维点云中的对应点，N_λ表示i时刻环境三维点云筛选后第λ个体素中三维点的数量，V_i表示i时刻环境三维点云筛选后体素的数量，表示K表示时刻数量；

步骤6所述进一步计算配准得分，具体如下：

其中，

表示i时刻环境三维点云的第λ个筛选后的体素中第r个三维点的权重，

表示i时刻环境三维点云筛选后第λ个体素中第r个三维点的残差，score_i表示i时刻相对位姿的得分，N_λ表示i时刻环境三维点云筛选后第λ个体素中三维点的数量，V_i表示i时刻环境三维点云筛选后体素的数量，表示K表示时刻数量；

步骤6所述根据配准得分计算当前时刻的相对位姿权重矩阵，具体如下：

其中，Ω_i表示i时刻的相对位姿的权重矩阵，score_i是i时刻相对位姿配准得分，I是单位矩阵，K表示时刻数量。

9.根据权利要求8所述的城市驾驶车辆连续定位方法，其特征在于，

步骤7所述融合后的位置具体定义为：

其中，

表示第i个时刻的融合后位置，x_i表示第i个时刻的融合后位置的X轴坐标，y_i表示第i个时刻的融合后位置的Y轴坐标，z_i表示第i个时刻的融合后位置的Z轴坐标，K表示时刻的数量。

10.根据权利要求9所述的城市驾驶车辆连续定位方法，其特征在于，

步骤8所述将融合后的位置结合误差修正模型进行估计得到相对位姿变换累计误差，具体为：

具体地，误差定义为实时相对位姿变换与融合后位置的差值；

漂移误差修正模型定义为下式：

其中，Δe_i为i时刻的相对位姿变换误差模型，

和

分别为i时刻相对位姿变换位置和i时刻的融合后位置，

表示n+1时刻的融合后位置，

表示n时刻的融合后位置，j表示上一次更新相对位姿变换误差模型的时刻，K表示时刻的数量；

当有三维点云中车辆位置或城市环境三维地图匹配位置时，可以使用上式更新相对位姿变换误差模型；

步骤8所述进一步修正当前时刻的环境三维点云与第一个时刻的环境三维点云的相对位姿，具体如下：

式中，

表示i时刻相对位姿变换的位置，t′_i表示i时刻经过误差校正后的位置，t_n+1表示n+1时刻相对位姿变换的位置，t_n表示n时刻相对位姿变换的位置，Δe_h为h时刻的相对位姿变换误差模型，实时输出校正后的位姿t′_i作为最终定位结果。

Images