CN111949943A - 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法 - Google Patents

Abstract

一种面向高级自动驾驶的V2X车联网和单车激光雷达点云特征匹配与融合的车辆融合定位方法，无人驾驶车辆通过激光雷达实时获取周围环境信息，同时与现有的高精度地图进行特征匹配实现车辆在车道上的局部定位。车辆与路侧LTE信号基站之间通过LTE‑V2X车联网技术以及无线定位技术实现车辆的定位。通过充分融合两种定位方式，实现更加精准的车路协同定位。本发明充分利用了单车智能中激光雷达高精度地图特征匹配定位技术特点以及车路无线通信定位优势，可以有效实现无人驾驶车辆在弱卫星信号覆盖区域的定位问题，以及达到高级自动驾驶的目标。

Description

一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定 位方法

技术领域

本发明专利涉及无人驾驶车辆定位系统和车路无线通信技术领域中融合定位方法，用以实现解决弱信号区域的车辆局部相对定位的问题，以及促进高级自动驾驶的发展。

背景技术

无人驾驶技术是近年来智能交通发展的重要前沿领域，其中包含了感知定位，路径规划，决策控制等部分组成。无人驾驶车辆需要提供精确的定位信息来实现安全正确的行驶。无人驾驶定位技术主要通过集成GNSS，IMU，车载摄像头，超声波雷达，激光雷达等传感器来获得车辆的厘米级位置信息。当前主流的无人驾驶定位技术主要由三种：一种是基于GNSS全球卫星定位系统，通过一组卫星的伪距，星历等信息实现车辆的全局定位。一种是基于航迹推算的定位方法，主要是通过车载惯性传感器IMU以及里程计，根据上一时刻的位姿推断得到下一时刻的位姿信息，该方法存在累积误差。最后是基于环境特征匹配的方法，主要通过车载激光雷达实时扫描周围环境信息，通过已有的精准高精度地图与之进行特征匹配，从而得到精确的局部信息。该方法优点明显，能够实现获取厘米级的定位信息。目前使用的激光雷达主要是机械旋转式激光雷达，该雷达获取车辆周围360度环境信息，一方面能够获取丰富的环境信息，但是另一方面由于激光雷达不断地进行旋转使用，使得机械式激光雷达具有较短的使用寿命。相对于旋转式激光雷达，固定式激光雷达能够减小激光雷达体积，不通过旋转来获取周围信息，使得激光雷达具有较好的使用寿命。

高级自动驾驶一般对应3个等级，分别是L3，L4以及L5级自动驾驶。其中L3级自动驾驶为有条件的自动控制，自动驾驶系统可自动控制车辆在大多数路况下，驾驶注意力不需专注于路况，双手甚至可以离开方向盘。但是车辆在道路上行驶过程中，驾驶仍然需要随时准备接管车辆。L4级自动驾驶为高度自动化驾驶系统，该驾驶系统仍然保留方向盘等操作介面，驾驶员可以对其操控来驾驶汽车。车辆在开启自动驾驶模式之后，车辆将设置好目的地之后按已经规划好的路线行驶，无需干预全面驾驶，但是仍然需要在高速公路或市区等特定区域进行人为干预。L5级自动驾驶则为全自动化驾驶系统，只有乘客位于其中，方向盘，油门刹车等驾驶介面已经不需要。第五级自动驾驶车辆将完全自动化，，不论任何环境、路况，都不需要人类驾驶介入操控。目前L5级自动驾驶的汽车只是未来的发展设想。

智能车联网系统是指通过各种无线通信方法达到对路网车辆状态信息与道路交通环境信息统一高效管理的系统，能够依照不同的需求对路网车辆进行监督管理，提高路网的运行效率，其作为汽车、电子、信息通信、道路交通运输等行业深度融合的新型产业形态，正在蓬勃发展。其中，采用的主要通信技术包括无线射频识别技术、无线通信技术、传感技术和网络技术等。路网中的车辆状态信息表示车辆的自身信息及运动参数；道路交通环境信息是指交通路况、道路基础设施信息以及服务信息等。车联网技术能够实现车与车、车与路、车与人之间信息的共享。车路协同系统作为智能车联网系统的子系统主要实现路网中车与路之间的交互通信，从而完成相应的功能需求，比如跨视距信息发布，车辆信息交互等功能，随着 5G技术的发展，车路协同系统的功能会越发丰富与高效。

无人驾驶汽车是我国人工智能发展的重点项目，无人驾驶技术与车路协同系统相结合能够实现真正意义上的高效安全的L5级无人驾驶技术，能够满足其安全，性能的高精度要求。无人驾驶定位技术能够利用GNSS全球卫星定位系统获取准确的位置信息，但是在卫星信号薄弱区域，GNSS的可靠性会降低，不能满足无人驾驶所需的定位精度，从而影响到无人车的性能与安全。此时需要无人车进行自适应局部定位，找到自身车体所处道路的空间位置以及相对位姿，从而使得车辆能够在弱信号区域内安全行驶。在该区域部署车路协同系统路侧通信单元，通过车路间通信链路以及无线定位技术，实现无人车在道路空间的定位，将无人车自身匹配定位与车路协同定位融合，能够进一步提高无人车在道路中的定位精度。

通常情况下智能车联网系统，能够通过GNSS卫星信号来确定路网中车辆的真实位置，但是在山区或者高楼群中GNSS信号会受到遮挡导致该区域内GNSS信号较差，而这对于路网中车辆的定位有着较为严重的影响。无人驾驶依靠高精度的定位来满足安全需求，当无人车辆行驶于该区域时会降低车辆与行人的安全性，所以需要寻找一种有效的解决方法来使得无人车辆在弱GNSS信号区域内保证有效地定位精度。

由此，如何有效利用现有的技术资源，来解决城市或隧道等弱卫星信号覆盖区域的车辆位置信息获取问题，是当前智能车路协同系统发展中需要迫切解决的间题，也是实现高级自动驾驶所需要解决的问题。

发明内容

车辆位置信息是无人驾驶系统中重要的信息资源，对于单车智能系统来说，高精度的车辆定位信息是其保证在正确车道上行驶的必备要求，也是保证其安全行驶的必要要求。对于多车无人驾驶系统来说，实时获取精准的每一辆无人驾驶车辆的位置信息能够保证多车跟随系统的稳定性与安全性，同时也是多车系统拓扑形态变更的安全保障。但是在弱GNSS区域下，无人驾驶的定位精度会受到一定的影响，难以保证单车以及多车智能无人驾驶系统的安全性，本发明基于一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法，主要是通过无人车在经过弱GNSS信号区域内时，通过自身高精度地图匹配定位方法计算车辆位置，以及利用与路侧LTE信号基站通信并感知车辆位置，然后利用线性卡尔曼滤波算法对两种计算结果进行融合，从而提高无人车定位精度。其特征在于：在区域路网内，无人车辆通过LTE-V2X 定位技术一方面来消除车辆里程计与惯性导航单元的累计误差，一方面为激光雷达高精度地图定位技术提供粗略位置估计，另一方面两者融合进一步提高车辆定位精度，具体方法如下：

步骤一、利用车路协同LTE-V2X定位技术实现无人车初步定位；

C、首先在弱GNSS信号区域合理部署路侧LTE信号基站的数量与位置，使得车辆在运行过程中始终至少处于3个路侧LTE信号基站的良好通信范围内。无人车作为移动节点行驶于道路中，路侧LTE信号基站作为固定定位基站。

D、无人车发送定位请求request并且标记发送时间。然后路侧LTE信号基站接收到无人车发来的定位请求之后，标记请求到达时间。接着计算发送与到达的时间差t₁，以及请求处理时间t₂，然后向无人车发送定位处理消息。无人车接收到定位处理消息之后标记消息到达时间，所以由此可知，无人车从发送请求到接收到处理信息所经历了时间为：

t₃＝2t_i+t₂ (1)

车辆发送请求到路侧LTE信号基站的时间t₁为：

由于晶振频率收到石英晶体漂移的影响，导致计时系统存在误差。车辆发送请求到路侧 LTE信号基站接收到请求的时间t₄为：

其中e₁，e₂分别表示车辆与路侧LTE信号基站的晶振漂移误差。

C：无人车近似处于二维平面，其位置假设为(x，y)，并且与路侧LTE信号基站处于视距状态，无人车发送定位请求信号到路侧LTE信号基站接收得到的时间为t₄，利用TOA测距算法以及三边定位算法计算得到无人车在车道的位置信息：

其中c为无线信号传播速率，(x_i，y_i)为路侧LTE信号基站的位置坐标。

步骤二、无人车基于激光雷达高精度地图匹配技术实现定位；

A：固态激光雷达布置于无人车车轴中心车顶处，激光雷达获取周围环境激光点云数据，并对数据进行处理。车辆通过获取6PS信号来判断是否需要进行特征匹配定位技术，当GPS 信号低于阈值的时候，则采用特征匹配定位技术来弥补GPS信号差的问题。高精度地图匹配定位主要采用ndt配准算法实现，具体步骤如下：

B：激光雷达实时获取周围环境的激光点云数据，由于采用固态激光雷达，所以每一帧获取的激光点云数据的空间范围为以激光雷达为中心的锥形。在整个特征匹配过程中需要将全局高精度地图作为ndt配准算法的目标点云，并且对其进行网格化处理。将开始进行特征匹配定位的时刻记为t，此后获取的每一帧原始环境激光点云数据需要去除距离车体较近与较远的激光点集，然后利用体素滤波过滤剩下的激光点云数据，在保持点云统计特征的情况下降低激光点云数据集的尺寸。最后将降采样后的过滤点云作为ndt配准算法的输入源点云；

C：对于全局目标点云，划分好网格之后，将点云加载到网格内，然后计算每一个网格内的正态分布概率密度函数pdf_i，首先计算均值向量

然后计算协方差矩阵∑，最后通过均值向量和协方差矩阵求出每一个网格内的正态分布概率密度函数pdf_i。由于需要用到协方差矩阵的逆矩阵，所以格子内不少于三个点，一般至少要保证有5个点，计算公式如下：

D：此时需要求出输入源点云点云相对于目标点云的初始坐标变换参数。坐标变换通常涉及到平移与旋转，平移通过平移向量表示，旋转通过旋转矩阵表示，旋转是关于xyz三个固定轴的旋转，转角由rpy表示。记该时刻下扫描到的激光点的坐标为

激光点相对于全局目标点云的坐标为

通过坐标变换公式可以将当前帧扫描的激光点云坐标映射到全局目标点云：

X′＝R_3×3X+T_3×1 (8)

T＝[t_x t_y t_z]^T (10)

其中R表示旋转矩阵，T表示平移向量。这一步是寻找一个合适的初始坐标变换使得目标点云大致处于参考点云的坐标平面当中，这一步是提供变换参数的初值，为下一步变换参数的迭代提供距离最优点较近的初值。在自动驾驶里初始值的提供可以依靠gps，里程计或者是imu惯性导航，利用上述传感器获取到此时自动驾驶车辆的位置以及姿态，通过坐标变换得到相对于参考点云的坐标变换参数，也就是位移T与旋转R。在弱GNSS信号区域内单纯依靠里程计或者imu会产生累计误差，所有需要利用步骤一车路协同LTE-V2X定位技术提供无人车初步定位位置，利用imu陀螺仪求出旋转。

D：将输入源点云根据初始变换参数将坐标转换到全局目标点云，此时输入源点云会分布在目标点云网格中，然后根据转换后的目标点云的坐标，利用对应所在网格的正态分布概率密度，求出对应的概率。

E：算法目的是寻找最佳坐标变换参数

使得目标点云位于参考点云的可能性最大化。算法需要优化的参数

是对目标点云的坐标变换(包括平移和旋转)。给定目标点云

和变换参数

令空间转换函数

表示利用变换参数

来移动目标点

至参考点云。

F：然后将每一个点的概率乘积起来作为目标似然函数。通过似然函数找到概率乘积最大时的坐标转换关系。简单来说就是由最大的概率找到最优的坐标转换，最大似然函数如下：

G：将最大似然函数作负对数变换，易于优化求解。

H：对最大似然函数进一步化简变形得到目标函数

I：利用牛顿法进行迭代更新，求出最佳变换参数，完成配准。迭代公式如下：

其中H为目标函数的海塞矩阵，g为目标的梯度向量。

ndt配准完成之后，此时输入源激光点云已配准至全局目标点云，从而由高精度地图来得到精准的车辆位姿信息。

步骤三、利用线性卡尔曼滤波进行融合定位

A：由步骤二可知，车体中心的位置为X_i＝(x_i，y_i)^T，由高精度地图特征匹配可以得到车体的航向角

所以车体的状态转移方程如下：

无人车运动过程中的观测方程如下：

Z_i＝X_i+V_i (20)

观测值Z_i则是通过LTE-V2X无线信号测量得到的车辆定位结果，V_i观测噪声的协方差矩阵R_i表示的是LTE-V2X无线信号测量定位结果的精度指标，并且为常数项矩阵。

B：

在无线信号定位测量值进行一阶泰勒展开得到：

令高精度定位匹配所得到的无人车位置与航向角的协方差矩阵为M，并且该矩阵为常数矩阵：

由此能够得到转移噪声W_i的协方差矩阵O_i，该矩阵表示的高精度地图特征匹配定位所得到的坐标增量精度，该值与无人车行驶距离以及航向角有关：

车辆初始位置均由LTE-V2X无线信号定位得到。

C：代入初值位置以及初始协方差矩阵迭代求解，可以得到每一时刻无人车的最佳位置估计。

首先计算无人车在i时刻的预测值：

接着计算预测值的协方差矩阵：

然后计算卡尔曼增益矩阵：

接着由系统观测值进行修正得到无人车位置的最优估计：

本发明的优点在于：

1、本发明基于一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法，能够有效缓解等弱GNSS信号覆盖区域的车辆定位问题。该方案是单车智能驾驶与智能车联网系统协同运行的有效方案，能够提高智能车联网中各个车载通信单元与路侧LTE信号基站的有效利用率，即保证了在山区或弱GNSS信号区域内的定位精度要求，也有效地节约了智能车辆网联系统的部署成本；

2、本发明基于无人驾驶固态激光雷达的高精度地图匹配定位方法，在定位初期定位精度较高，但是随着时间的推移，行驶路程的增加，里程计与惯性导航单元存在累计误差，由于以往通过GNSS来进行误差的纠正的方法在这种情况下不能使用，所以通过LTE-V2X-TOA三边定位法提供一个车辆的粗略位置估计，以此来消除里程计与惯性导航单元的累计误差对定位造成的影响，该方法综合了车路协同的优点，具有可行性；

3、本发明基于一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法，保证弱GNSS信号区域内车辆的定位精度，能够有效提高无人车辆在该区域行驶的安全性，能够处理在特定的复杂环境下高级自动驾驶所面临的特殊问题，有利于实现真正意义上的高级自动驾驶系统。

附图说明

图1为本发明车辆协同定位方法流程图

具体实施方式

下面结合附图来对本发明做进一步说明。

步骤一、利用车路协同LTE-V2X定位技术实现无人车初步定位；

E、首先在弱GNSS信号区域合理部署路侧LTE信号基站的数量与位置，使得车辆在运行过程中始终至少处于3个路侧LTE信号基站的良好通信范围内。无人车作为移动节点行驶于道路中，路侧LTE信号基站作为固定定位基站。

F、无人车发送定位请求request并且标记发送时间。然后路侧LTE信号基站接收到无人车发来的定位请求之后，标记请求到达时间。接着计算发送与到达的时间差t₁，以及请求处理时间t₂，然后向无人车发送定位处理消息。无人车接收到定位处理消息之后标记消息到达时间，所以由此可知，无人车从发送请求到接收到处理信息所经历了时间为：

t₃＝2t₁+t₂ (1)

车辆发送请求到路侧LTE信号基站的时间t₁为：

由于晶振频率收到石英晶体漂移的影响，导致计时系统存在误差。车辆发送请求到路侧 LTE信号基站接收到请求的时间t₄为：

其中e₁，e₂分别表示车辆与路侧LTE信号基站的晶振漂移误差。

C：无人车近似处于二维平面，其位置假设为(x，y)，并且与路侧LTE信号基站处于视距状态，无人车发送定位请求信号到路侧LTE信号基站接收得到的时间为t₄，利用TOA测距算法以及三边定位算法计算得到无人车在车道的位置信息：

其中c为无线信号传播速率，(x_i，y_i)为路侧LTE信号基站的位置坐标。

步骤二、无人车基于激光雷达高精度地图匹配技术实现定位；

A：固态激光雷达布置于无人车车轴中心车顶处，激光雷达获取周围环境激光点云数据，并对数据进行处理。车辆通过获取GPS信号来判断是否需要进行特征匹配定位技术，当GPS 信号低于阈值的时候，则采用特征匹配定位技术来弥补GPS信号差的问题。高精度地图匹配定位主要采用ndt配准算法实现，具体步骤如下：

B：激光雷达实时获取周围环境的激光点云数据，由于采用固态激光雷达，所以每一帧获取的激光点云数据的空间范围为以激光雷达为中心的锥形。在整个特征匹配过程中需要将全局高精度地图作为ndt配准算法的目标点云，并且对其进行网格化处理。将开始进行特征匹配定位的时刻记为t，此后获取的每一帧原始环境激光点云数据需要去除距离车体较近与较远的激光点集，然后利用体素滤波过滤剩下的激光点云数据，在保持点云统计特征的情况下降低激光点云数据集的尺寸。最后将降采样后的过滤点云作为ndt配准算法的输入源点云；

C：对于全局目标点云，划分好网格之后，将点云加载到网格内，然后计算每一个网格内的正态分布概率密度函数pdf_i，首先计算均值向量

然后计算协方差矩阵∑，最后通过均值向量和协方差矩阵求出每一个网格内的正态分布概率密度函数pdf_i。由于需要用到协方差矩阵的逆矩阵，所以格子内不少于三个点，一般至少要保证有5个点，计算公式如下：

D：此时需要求出输入源点云点云相对于目标点云的初始坐标变换参数。坐标变换通常涉及到平移与旋转，平移通过平移向量表示，旋转通过旋转矩阵表示，旋转是关于xyz三个固定轴的旋转，转角由rpy表示。记该时刻下扫描到的激光点的坐标为

激光点相对于全局目标点云的坐标为

通过坐标变换公式可以将当前帧扫描的激光点云坐标映射到全局目标点云：

X′＝R_3×3X+T_3×1 (8)

T＝[t_x t_y t_z]^T (10)

其中R表示旋转矩阵，T表示平移向量。这一步是寻找一个合适的初始坐标变换使得目标点云大致处于参考点云的坐标平面当中，这一步是提供变换参数的初值，为下一步变换参数的迭代提供距离最优点较近的初值。在自动驾驶里初始值的提供可以依靠gps，里程计或者是imu惯性导航，利用上述传感器获取到此时自动驾驶车辆的位置以及姿态，通过坐标变换得到相对于参考点云的坐标变换参数，也就是位移T与旋转R。在弱GNSS信号区域内单纯依靠里程计或者imu会产生累计误差，所有需要利用步骤一车路协同LTE-V2X定位技术提供无人车初步定位位置，利用imu陀螺仪求出旋转。

D：将输入源点云根据初始变换参数将坐标转换到全局目标点云，此时输入源点云会分布在目标点云网格中，然后根据转换后的目标点云的坐标，利用对应所在网格的正态分布概率密度，求出对应的概率。

E：算法目的是寻找最佳坐标变换参数

使得目标点云位于参考点云的可能性最大化。算法需要优化的参数

是对目标点云的坐标变换(包括平移和旋转)。给定目标点云

和变换参数

令空间转换函数

表示利用变换参数

来移动目标点

至参考点云。

F：然后将每一个点的概率乘积起来作为目标似然函数。通过似然函数找到概率乘积最大时的坐标转换关系。简单来说就是由最大的概率找到最优的坐标转换，最大似然函数如下：

G：将最大似然函数作负对数变换，易于优化求解。

H：对最大似然函数进一步化简变形得到目标函数

I：利用牛顿法进行迭代更新，求出最佳变换参数，完成配准。迭代公式如下：

其中H为目标函数的海塞矩阵，g为目标的梯度向量。

ndt配准完成之后，此时输入源激光点云已配准至全局目标点云，从而由高精度地图来得到精准的车辆位姿信息。

步骤三、利用线性卡尔曼滤波进行融合定位

A：由步骤二可知，车体中心的位置为X_i＝(x_i，y_i)^T，由高精度地图特征匹配可以得到车体的航向角

所以车体的状态转移方程如下：

无人车运动过程中的观测方程如下：

Z_i＝X_i+V_i (20)

观测值Z_i则是通过LTE-V2X无线信号测量得到的车辆定位结果，V_i观测噪声的协方差矩阵R_i表示的是LTE-V2X无线信号测量定位结果的精度指标，并且为常数项矩阵。

B：

在无线信号定位测量值进行一阶泰勒展开得到：

令高精度定位匹配所得到的无人车位置与航向角的协方差矩阵为M，并且该矩阵为常数矩阵：

由此能够得到转移噪声W_i的协方差矩阵O_i，该矩阵表示的高精度地图特征匹配定位所得到的坐标增量精度，该值与无人车行驶距离以及航向角有关：

车辆初始位置均由LTE-V2X无线信号定位得到。

C：代入初值位置以及初始协方差矩阵迭代求解，可以得到每一时刻无人车的最佳位置估计。

首先计算无人车在i时刻的预测值：

接着计算预测值的协方差矩阵：

然后计算卡尔曼增益矩阵：

接着由系统观测值进行修正得到无人车位置的最优估计：

。

Claims (1)

1.一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法，其特征在于：通过下述步骤实现：

步骤一、利用LTE-V2X车联网技术实现无人车初步定位；

A、首先在弱6NSS信号区域合理部署路侧LTE信号基站的数量与位置，使得车辆在运行过程中始终至少处于3个路侧LTE信号基站的良好通信范围内，无人车作为移动节点行驶于道路中，路侧LTE信号基站作为固定定位基站；

B、无人车发送定位请求request并且标记发送时间，然后路侧LTE信号基站接收到无人车发来的定位请求之后，标记请求到达时间，接着计算发送与到达的时间差t₁，以及请求处理时间t₂，然后向无人车发送定位处理消息，无人车接收到定位处理消息之后标记消息到达时间，所以由此可知，无人车从发送请求到接收到处理信息所经历了时间为：

t₃＝2t₁+t₂ (1)

车辆发送请求到路侧LTE信号基站的时间t₁为：

由于晶振频率收到石英晶体漂移的影响，导致计时系统存在误差，车辆发送请求到路侧LTE信号基站接收到请求的时间t₄为：

其中e₁，e₂分别表示车辆与路侧LTE信号基站的晶振漂移误差，

C：无人车近似处于二维平面，其位置假设为(x，y)，并且与路侧LTE信号基站处于视距状态，无人车发送定位请求信号到路侧LTE信号基站接收得到的时间为t₄，利用TOA测距算法以及三边定位算法计算得到无人车在车道的位置信息：

其中c为无线信号传播速率，(x_i，y_i)为路侧LTE信号基站的位置坐标，

步骤二、无人车基于激光雷达高精度地图匹配技术实现定位；

A：固态激光雷达布置于无人车车轴中心车顶处，激光雷达获取周围环境激光点云数据，并对数据进行处理，车辆通过获取GPS信号来判断是否需要进行特征匹配定位技术，当GPS信号低于阈值的时候，则采用特征匹配定位技术来弥补6PS信号差的问题，高精度地图匹配定位主要采用ndt配准算法实现，具体步骤如下：激光雷达实时获取周围环境的激光点云数据，由于采用固态激光雷达，所以每一帧获取的激光点云数据的空间范围为以激光雷达为中心的锥形，在整个特征匹配过程中需要将全局高精度地图作为ndt配准算法的目标点云，并且对其进行网格化处理，将开始进行特征匹配定位的时刻记为t，此后获取的每一帧原始环境激光点云数据需要去除距离车体较近与较远的激光点集，然后利用体素滤波过滤剩下的激光点云数据，在保持点云统计特征的情况下降低激光点云数据集的尺寸，最后将降采样后的过滤点云作为ndt配准算法的输入源点云；

B：对于全局目标点云，划分好网格之后，将点云加载到网格内，然后计算每一个网格内的正态分布概率密度函数pdf_i，首先计算均值向量

然后计算协方差矩阵∑，最后通过均值向量和协方差矩阵求出每一个网格内的正态分布概率密度函数pdf_i，由于需要用到协方差矩阵的逆矩阵，所以格子内不少于三个点，一般至少要保证有5个点，计算公式如下：

C：此时需要求出输入源点云点云相对于目标点云的初始坐标变换参数，坐标变换通常涉及到平移与旋转，平移通过平移向量表示，旋转通过旋转矩阵表示，旋转是关于xyz三个固定轴的旋转，转角由rpy表示，记该时刻下扫描到的激光点的坐标为

激光点相对于全局目标点云的坐标为

通过坐标变换公式可以将当前帧扫描的激光点云坐标映射到全局目标点云：

X′＝R_3×3X+T_3×1 (8)

T＝[t_x t_y t_z]^T (10)

其中R表示旋转矩阵，T表示平移向量，这一步是寻找一个合适的初始坐标变换使得目标点云大致处于参考点云的坐标平面当中，这一步是提供变换参数的初值，为下一步变换参数的迭代提供距离最优点较近的初值，在自动驾驶里初始值的提供可以依靠gps，里程计或者是imu惯性导航，利用上述传感器获取到此时自动驾驶车辆的位置以及姿态，通过坐标变换得到相对于参考点云的坐标变换参数，也就是位移T与旋转R，在弱GNSS信号区域内单纯依靠里程计或者imu会产生累计误差，所有需要利用步骤一车路协同LTE-V2X车联网定位技术提供无人车初步定位位置，利用imu陀螺仪求出旋转；

D：将输入源点云根据初始变换参数将坐标转换到全局目标点云，此时输入源点云会分布在目标点云网格中，然后根据转换后的目标点云的坐标，利用对应所在网格的正态分布概率密度，求出对应的概率；

E：算法目的是寻找最佳坐标变换参数

使得目标点云位于参考点云的可能性最大化，算法需要优化的参数

是对目标点云的坐标变换(包括平移和旋转)，给定目标点云

和变换参数

令空间转换函数

表示利用变换参数

来移动目标点

至参考点云；

F：然后将每一个点的概率乘积起来作为目标似然函数。通过似然函数找到概率乘积最大时的坐标转换关系。简单来说就是由最大的概率找到最优的坐标转换，最大似然函数如下：

G：将最大似然函数作负对数变换，易于优化求解；

H：对最大似然函数进一步化简变形得到目标函数

I：利用牛顿法进行迭代更新，求出最佳变换参数，完成配准。迭代公式如下：

其中H为目标函数的海塞矩阵，g为目标的梯度向量；ndt配准完成之后，此时输入源激光点云已配准至全局目标点云，从而由高精度地图来得到精准的车辆位姿信息；

步骤三、利用线性卡尔曼滤波进行融合定位

A：由步骤二可知，车体中心的位置为X_i＝(x_i，y_i)^T，由高精度地图特征匹配可以得到车体的航向角

所以车体的状态转移方程如下：

无人车运动过程中的观测方程如下：

Z_i＝X_i+V_i (20)

观测值Z_i则是通过LTE-V2X无线信号测量得到的车辆定位结果，V_i观测噪声的协方差矩阵R_i表示的是LTE-V2X无线信号测量定位结果的精度指标，并且为常数项矩阵。

在无线信号定位测量值进行一阶泰勒展开得到：

令高精度定位匹配所得到的无人车位置与航向角的协方差矩阵为M，并且该矩阵为常数矩阵：

由此能够得到转移噪声W_i的协方差矩阵O_i，该矩阵表示的高精度地图特征匹配定位所得到的坐标增量精度，该值与无人车行驶距离以及航向角有关：

车辆初始位置均由LTE-V2X无线信号定位得到；

C：代入初值位置以及初始协方差矩阵迭代求解，可以得到每一时刻无人车的最佳位置估计；

首先计算无人车在i时刻的预测值：

接着计算预测值的协方差矩阵：

然后计算卡尔曼增益矩阵：

接着由系统观测值进行修正得到无人车位置的最优估计：

。

Images