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

《面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法.pdf》由会员分享，可在线阅读，更多相关《面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法.pdf（16页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 202010727561.7 (22)申请日 2020.07.24 (71)申请人 北京航空航天大学 地址 100191 北京市海淀区学院路37号 (72)发明人 段续庭姜航田大新周建山 赵元昊林椿眄郑坤贤 (74)专利代理机构 北京华仁联合知识产权代理 有限公司 11588 代理人 王希刚 (51)Int.Cl. G06F 17/18(2006.01) G01C 21/16(2006.01) H04W 4/44(2018.01) H04W 64/00(2009.01) (54)。

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

3、题， 以及达到高级自动驾驶 的目标。 权利要求书4页 说明书10页 附图1页 CN 111949943 A 2020.11.17 CN 111949943 A 1.一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方法， 其特征在于： 通 过下述步骤实现： 步骤一、 利用LTE-V2X车联网技术实现无人车初步定位； A、 首先在弱6NSS信号区域合理部署路侧LTE信号基站的数量与位置， 使得车辆在运行 过程中始终至少处于3个路侧LTE信号基站的良好通信范围内， 无人车作为移动节点行驶于 道路中， 路侧LTE信号基站作为固定定位基站； B、 无人车发送定位请求request并且标记发送时间。

4、， 然后路侧LTE信号基站接收到无人 车发来的定位请求之后， 标记请求到达时间， 接着计算发送与到达的时间差t1， 以及请求处 理时间t2， 然后向无人车发送定位处理消息， 无人车接收到定位处理消息之后标记消息到 达时间， 所以由此可知， 无人车从发送请求到接收到处理信息所经历了时间为： t32t1+t2 (1) 车辆发送请求到路侧LTE信号基站的时间t1为： 由于晶振频率收到石英晶体漂移的影响， 导致计时系统存在误差， 车辆发送请求到路 侧LTE信号基站接收到请求的时间t4为： 其中e1， e2分别表示车辆与路侧LTE信号基站的晶振漂移误差， C： 无人车近似处于二维平面， 其位置假设为(x。

5、， y)， 并且与路侧LTE信号基站处于视距 状态， 无人车发送定位请求信号到路侧LTE信号基站接收得到的时间为t4， 利用TOA测距算 法以及三边定位算法计算得到无人车在车道的位置信息： 其中c为无线信号传播速率， (xi， yi)为路侧LTE信号基站的位置坐标， 步骤二、 无人车基于激光雷达高精度地图匹配技术实现定位； A： 固态激光雷达布置于无人车车轴中心车顶处， 激光雷达获取周围环境激光点云数 据， 并对数据进行处理， 车辆通过获取GPS信号来判断是否需要进行特征匹配定位技术， 当 GPS信号低于阈值的时候， 则采用特征匹配定位技术来弥补6PS信号差的问题， 高精度地图 匹配定位主要采。

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

7、加载到网格内， 然后计算每一个网格内 的正态分布概率密度函数pdfi， 首先计算均值向量然后计算协方差矩阵， 最后通过均 值向量和协方差矩阵求出每一个网格内的正态分布概率密度函数pdfi， 由于需要用到协方 差矩阵的逆矩阵， 所以格子内不少于三个点， 一般至少要保证有5个点， 计算公式如下： 权利要求书 1/4 页 2 CN 111949943 A 2 C： 此时需要求出输入源点云点云相对于目标点云的初始坐标变换参数， 坐标变换通常 涉及到平移与旋转， 平移通过平移向量表示， 旋转通过旋转矩阵表示， 旋转是关于xyz三个 固定轴的旋转， 转角由rpy表示， 记该时刻下扫描到的激光点的坐标为激光。

8、点相对 于全局目标点云的坐标为通过坐标变换公式可以将当前帧扫描的激光点云坐标 映射到全局目标点云： X R33X+T31 (8) Ttx ty tzT (10) 其中R表示旋转矩阵， T表示平移向量， 这一步是寻找一个合适的初始坐标变换使得目 标点云大致处于参考点云的坐标平面当中， 这一步是提供变换参数的初值， 为下一步变换 参数的迭代提供距离最优点较近的初值， 在自动驾驶里初始值的提供可以依靠gps， 里程计 或者是imu惯性导航， 利用上述传感器获取到此时自动驾驶车辆的位置以及姿态， 通过坐标 变换得到相对于参考点云的坐标变换参数， 也就是位移T与旋转R， 在弱GNSS信号区域内单 纯依靠。

9、里程计或者imu会产生累计误差， 所有需要利用步骤一车路协同LTE-V2X车联网定位 技术提供无人车初步定位位置， 利用imu陀螺仪求出旋转； D： 将输入源点云根据初始变换参数将坐标转换到全局目标点云， 此时输入源点云会分 布在目标点云网格中， 然后根据转换后的目标点云的坐标， 利用对应所在网格的正态分布 概率密度， 求出对应的概率； E： 算法目的是寻找最佳坐标变换参数使得目标点云位于参考点云的可能性最大化， 算法需要优化的参数是对目标点云的坐标变换(包括平移和旋转)， 给定目标点云 和变换参数令空间转换函数表示利用变换参数来移动目标点 至参考点云； F： 然后将每一个点的概率乘积起来作为。

10、目标似然函数。 通过似然函数找到概率乘积最 大时的坐标转换关系。 简单来说就是由最大的概率找到最优的坐标转换， 最大似然函数如 下： 权利要求书 2/4 页 3 CN 111949943 A 3 G： 将最大似然函数作负对数变换， 易于优化求解； H： 对最大似然函数进一步化简变形得到目标函数 I： 利用牛顿法进行迭代更新， 求出最佳变换参数， 完成配准。 迭代公式如下： 其中H为目标函数的海塞矩阵， g为目标的梯度向量； ndt配准完成之后， 此时输入源激 光点云已配准至全局目标点云， 从而由高精度地图来得到精准的车辆位姿信息； 步骤三、 利用线性卡尔曼滤波进行融合定位 A： 由步骤二可知，。

11、 车体中心的位置为Xi(xi， yi)T， 由高精度地图特征匹配可以得到车 体的航向角所以车体的状态转移方程如下： 无人车运动过程中的观测方程如下： ZiXi+Vi (20) 观测值Zi则是通过LTE-V2X无线信号测量得到的车辆定位结果， Vi观测噪声的协方差矩 阵Ri表示的是LTE-V2X无线信号测量定位结果的精度指标， 并且为常数项矩阵。 在无线信号定位测量值进行一阶泰勒展开得到： 令高精度定位匹配所得到的无人车位置与航向角的协方差矩阵为M， 并且该矩阵为常 数矩阵： 权利要求书 3/4 页 4 CN 111949943 A 4 由此能够得到转移噪声Wi的协方差矩阵Oi， 该矩阵表示的高。

12、精度地图特征匹配定位所 得到的坐标增量精度， 该值与无人车行驶距离以及航向角有关： 车辆初始位置均由LTE-V2X无线信号定位得到； C： 代入初值位置以及初始协方差矩阵迭代求解， 可以得到每一时刻无人车的最佳位置 估计； 首先计算无人车在i时刻的预测值： 接着计算预测值的协方差矩阵： 然后计算卡尔曼增益矩阵： 接着由系统观测值进行修正得到无人车位置的最优估计： 。 权利要求书 4/4 页 5 CN 111949943 A 5 一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定 位方法 技术领域 0001 本发明专利涉及无人驾驶车辆定位系统和车路无线通信技术领域中融合定位方 法， 用以实现。

13、解决弱信号区域的车辆局部相对定位的问题， 以及促进高级自动驾驶的发展。 背景技术 0002 无人驾驶技术是近年来智能交通发展的重要前沿领域， 其中包含了感知定位， 路 径规划， 决策控制等部分组成。 无人驾驶车辆需要提供精确的定位信息来实现安全正确的 行驶。 无人驾驶定位技术主要通过集成GNSS， IMU， 车载摄像头， 超声波雷达， 激光雷达等传 感器来获得车辆的厘米级位置信息。 当前主流的无人驾驶定位技术主要由三种： 一种是基 于GNSS全球卫星定位系统， 通过一组卫星的伪距， 星历等信息实现车辆的全局定位。 一种是 基于航迹推算的定位方法， 主要是通过车载惯性传感器IMU以及里程计， 根。

14、据上一时刻的位 姿推断得到下一时刻的位姿信息， 该方法存在累积误差。 最后是基于环境特征匹配的方法， 主要通过车载激光雷达实时扫描周围环境信息， 通过已有的精准高精度地图与之进行特征 匹配， 从而得到精确的局部信息。 该方法优点明显， 能够实现获取厘米级的定位信息。 目前 使用的激光雷达主要是机械旋转式激光雷达， 该雷达获取车辆周围360度环境信息， 一方面 能够获取丰富的环境信息， 但是另一方面由于激光雷达不断地进行旋转使用， 使得机械式 激光雷达具有较短的使用寿命。 相对于旋转式激光雷达， 固定式激光雷达能够减小激光雷 达体积， 不通过旋转来获取周围信息， 使得激光雷达具有较好的使用寿命。。

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

16、客位于其中， 方向 盘， 油门刹车等驾驶介面已经不需要。 第五级自动驾驶车辆将完全自动化， ， 不论任何环境、 路况， 都不需要人类驾驶介入操控。 目前L5级自动驾驶的汽车只是未来的发展设想。 0004 智能车联网系统是指通过各种无线通信方法达到对路网车辆状态信息与道路交 通环境信息统一高效管理的系统， 能够依照不同的需求对路网车辆进行监督管理， 提高路 网的运行效率， 其作为汽车、 电子、 信息通信、 道路交通运输等行业深度融合的新型产业形 态， 正在蓬勃发展。 其中， 采用的主要通信技术包括无线射频识别技术、 无线通信技术、 传感 技术和网络技术等。 路网中的车辆状态信息表示车辆的自身信息。

17、及运动参数； 道路交通环 境信息是指交通路况、 道路基础设施信息以及服务信息等。 车联网技术能够实现车与车、 车 与路、 车与人之间信息的共享。 车路协同系统作为智能车联网系统的子系统主要实现路网 中车与路之间的交互通信， 从而完成相应的功能需求， 比如跨视距信息发布， 车辆信息交互 说明书 1/10 页 6 CN 111949943 A 6 等功能， 随着 5G技术的发展， 车路协同系统的功能会越发丰富与高效。 0005 无人驾驶汽车是我国人工智能发展的重点项目， 无人驾驶技术与车路协同系统相 结合能够实现真正意义上的高效安全的L5级无人驾驶技术， 能够满足其安全， 性能的高精 度要求。 无。

18、人驾驶定位技术能够利用GNSS全球卫星定位系统获取准确的位置信息， 但是在 卫星信号薄弱区域， GNSS的可靠性会降低， 不能满足无人驾驶所需的定位精度， 从而影响到 无人车的性能与安全。 此时需要无人车进行自适应局部定位， 找到自身车体所处道路的空 间位置以及相对位姿， 从而使得车辆能够在弱信号区域内安全行驶。 在该区域部署车路协 同系统路侧通信单元， 通过车路间通信链路以及无线定位技术， 实现无人车在道路空间的 定位， 将无人车自身匹配定位与车路协同定位融合， 能够进一步提高无人车在道路中的定 位精度。 0006 通常情况下智能车联网系统， 能够通过GNSS卫星信号来确定路网中车辆的真实位。

19、 置， 但是在山区或者高楼群中GNSS信号会受到遮挡导致该区域内GNSS信号较差， 而这对于 路网中车辆的定位有着较为严重的影响。 无人驾驶依靠高精度的定位来满足安全需求， 当 无人车辆行驶于该区域时会降低车辆与行人的安全性， 所以需要寻找一种有效的解决方法 来使得无人车辆在弱GNSS信号区域内保证有效地定位精度。 0007 由此， 如何有效利用现有的技术资源， 来解决城市或隧道等弱卫星信号覆盖区域 的车辆位置信息获取问题， 是当前智能车路协同系统发展中需要迫切解决的间题， 也是实 现高级自动驾驶所需要解决的问题。 发明内容 0008 车辆位置信息是无人驾驶系统中重要的信息资源， 对于单车智能。

20、系统来说， 高精 度的车辆定位信息是其保证在正确车道上行驶的必备要求， 也是保证其安全行驶的必要要 求。 对于多车无人驾驶系统来说， 实时获取精准的每一辆无人驾驶车辆的位置信息能够保 证多车跟随系统的稳定性与安全性， 同时也是多车系统拓扑形态变更的安全保障。 但是在 弱GNSS区域下， 无人驾驶的定位精度会受到一定的影响， 难以保证单车以及多车智能无人 驾驶系统的安全性， 本发明基于一种面向高级自动驾驶的V2X和激光点云配准的车辆融合 定位方法， 主要是通过无人车在经过弱GNSS信号区域内时， 通过自身高精度地图匹配定位 方法计算车辆位置， 以及利用与路侧LTE信号基站通信并感知车辆位置， 然。

21、后利用线性卡尔 曼滤波算法对两种计算结果进行融合， 从而提高无人车定位精度。 其特征在于： 在区域路网 内， 无人车辆通过LTE-V2X 定位技术一方面来消除车辆里程计与惯性导航单元的累计误 差， 一方面为激光雷达高精度地图定位技术提供粗略位置估计， 另一方面两者融合进一步 提高车辆定位精度， 具体方法如下： 0009 步骤一、 利用车路协同LTE-V2X定位技术实现无人车初步定位； 0010 C、 首先在弱GNSS信号区域合理部署路侧LTE信号基站的数量与位置， 使得车辆在 运行过程中始终至少处于3个路侧LTE信号基站的良好通信范围内。 无人车作为移动节点行 驶于道路中， 路侧LTE信号基站。

22、作为固定定位基站。 0011 D、 无人车发送定位请求request并且标记发送时间。 然后路侧LTE信号基站接收到 无人车发来的定位请求之后， 标记请求到达时间。 接着计算发送与到达的时间差t1， 以及请 求处理时间t2， 然后向无人车发送定位处理消息。 无人车接收到定位处理消息之后标记消 说明书 2/10 页 7 CN 111949943 A 7 息到达时间， 所以由此可知， 无人车从发送请求到接收到处理信息所经历了时间为： 0012 t32ti+t2 (1) 0013 车辆发送请求到路侧LTE信号基站的时间t1为： 0014 0015 由于晶振频率收到石英晶体漂移的影响， 导致计时系统存。

23、在误差。 车辆发送请求 到路侧 LTE信号基站接收到请求的时间t4为： 0016 0017 其中e1， e2分别表示车辆与路侧LTE信号基站的晶振漂移误差。 0018 C： 无人车近似处于二维平面， 其位置假设为(x， y)， 并且与路侧LTE信号基站处于 视距状态， 无人车发送定位请求信号到路侧LTE信号基站接收得到的时间为t4， 利用TOA测 距算法以及三边定位算法计算得到无人车在车道的位置信息： 0019 0020 其中c为无线信号传播速率， (xi， yi)为路侧LTE信号基站的位置坐标。 0021 步骤二、 无人车基于激光雷达高精度地图匹配技术实现定位； 0022 A： 固态激光雷达。

24、布置于无人车车轴中心车顶处， 激光雷达获取周围环境激光点云 数据， 并对数据进行处理。 车辆通过获取6PS信号来判断是否需要进行特征匹配定位技术， 当GPS 信号低于阈值的时候， 则采用特征匹配定位技术来弥补GPS信号差的问题。 高精度地 图匹配定位主要采用ndt配准算法实现， 具体步骤如下： 0023 B： 激光雷达实时获取周围环境的激光点云数据， 由于采用固态激光雷达， 所以每 一帧获取的激光点云数据的空间范围为以激光雷达为中心的锥形。 在整个特征匹配过程中 需要将全局高精度地图作为ndt配准算法的目标点云， 并且对其进行网格化处理。 将开始进 行特征匹配定位的时刻记为t， 此后获取的每一。

25、帧原始环境激光点云数据需要去除距离车 体较近与较远的激光点集， 然后利用体素滤波过滤剩下的激光点云数据， 在保持点云统计 特征的情况下降低激光点云数据集的尺寸。 最后将降采样后的过滤点云作为ndt配准算法 的输入源点云； 0024 C： 对于全局目标点云， 划分好网格之后， 将点云加载到网格内， 然后计算每一个网 格内的正态分布概率密度函数pdfi， 首先计算均值向量然后计算协方差矩阵， 最后通 过均值向量和协方差矩阵求出每一个网格内的正态分布概率密度函数pdfi。 由于需要用到 协方差矩阵的逆矩阵， 所以格子内不少于三个点， 一般至少要保证有5个点， 计算公式如下： 0025 0026 00。

26、27 说明书 3/10 页 8 CN 111949943 A 8 0028 D： 此时需要求出输入源点云点云相对于目标点云的初始坐标变换参数。 坐标变换 通常涉及到平移与旋转， 平移通过平移向量表示， 旋转通过旋转矩阵表示， 旋转是关于xyz 三个固定轴的旋转， 转角由rpy表示。 记该时刻下扫描到的激光点的坐标为激光点 相对于全局目标点云的坐标为通过坐标变换公式可以将当前帧扫描的激光点 云坐标映射到全局目标点云： 0029 X R33X+T31 (8) 0030 0031 Ttx ty tzT (10) 0032 其中R表示旋转矩阵， T表示平移向量。 这一步是寻找一个合适的初始坐标变换使 。

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

28、云网格中， 然后根据转换后的目标点云的坐标， 利用对应所在网格的正态 分布概率密度， 求出对应的概率。 0034E： 算法目的是寻找最佳坐标变换参数使得目标点云位于参考点云的可能性最 大化。 算法需要优化的参数 是对目标点云的坐标变换(包括平移和旋转)。 给定目标点云 和变换参数令空间转换函数表示利用变换参数 来移动目标点 至参考点云。 0035 F： 然后将每一个点的概率乘积起来作为目标似然函数。 通过似然函数找到概率乘 积最大时的坐标转换关系。 简单来说就是由最大的概率找到最优的坐标转换， 最大似然函 数如下： 0036 0037 G： 将最大似然函数作负对数变换， 易于优化求解。 003。

29、8 0039H： 对最大似然函数进一步化简变形得到目标函数 说明书 4/10 页 9 CN 111949943 A 9 0040 0041 0042 I： 利用牛顿法进行迭代更新， 求出最佳变换参数， 完成配准。 迭代公式如下： 0043 0044 0045 0046 0047 其中H为目标函数的海塞矩阵， g为目标的梯度向量。 0048 ndt配准完成之后， 此时输入源激光点云已配准至全局目标点云， 从而由高精度地 图来得到精准的车辆位姿信息。 0049 步骤三、 利用线性卡尔曼滤波进行融合定位 0050 A： 由步骤二可知， 车体中心的位置为Xi(xi， yi)T， 由高精度地图特征匹配可。

30、以得 到车体的航向角所以车体的状态转移方程如下： 0051 0052 无人车运动过程中的观测方程如下： 0053 ZiXi+Vi (20) 0054 观测值Zi则是通过LTE-V2X无线信号测量得到的车辆定位结果， Vi观测噪声的协方 差矩阵Ri表示的是LTE-V2X无线信号测量定位结果的精度指标， 并且为常数项矩阵。 0055B：在无线信号定位测量值进行一阶泰勒展开得到： 0056 0057 令高精度定位匹配所得到的无人车位置与航向角的协方差矩阵为M， 并且该矩阵 为常数矩阵： 0058 0059 由此能够得到转移噪声Wi的协方差矩阵Oi， 该矩阵表示的高精度地图特征匹配定 位所得到的坐标增。

31、量精度， 该值与无人车行驶距离以及航向角有关： 说明书 5/10 页 10 CN 111949943 A 10 0060 0061 车辆初始位置均由LTE-V2X无线信号定位得到。 0062 C： 代入初值位置以及初始协方差矩阵迭代求解， 可以得到每一时刻无人车的最佳 位置估计。 0063 首先计算无人车在i时刻的预测值： 0064 0065 接着计算预测值的协方差矩阵： 0066 0067 然后计算卡尔曼增益矩阵： 0068 0069 接着由系统观测值进行修正得到无人车位置的最优估计： 0070 0071 本发明的优点在于： 0072 1、 本发明基于一种面向高级自动驾驶的V2X和激光点云配。

32、准的车辆融合定位方 法， 能够有效缓解等弱GNSS信号覆盖区域的车辆定位问题。 该方案是单车智能驾驶与智能 车联网系统协同运行的有效方案， 能够提高智能车联网中各个车载通信单元与路侧LTE信 号基站的有效利用率， 即保证了在山区或弱GNSS信号区域内的定位精度要求， 也有效地节 约了智能车辆网联系统的部署成本； 0073 2、 本发明基于无人驾驶固态激光雷达的高精度地图匹配定位方法， 在定位初期定 位精度较高， 但是随着时间的推移， 行驶路程的增加， 里程计与惯性导航单元存在累计误 差， 由于以往通过GNSS来进行误差的纠正的方法在这种情况下不能使用， 所以通过LTE- V2X-TOA三边定位。

33、法提供一个车辆的粗略位置估计， 以此来消除里程计与惯性导航单元的 累计误差对定位造成的影响， 该方法综合了车路协同的优点， 具有可行性； 0074 3、 本发明基于一种面向高级自动驾驶的V2X和激光点云配准的车辆融合定位方 法， 保证弱GNSS信号区域内车辆的定位精度， 能够有效提高无人车辆在该区域行驶的安全 性， 能够处理在特定的复杂环境下高级自动驾驶所面临的特殊问题， 有利于实现真正意义 上的高级自动驾驶系统。 附图说明 0075 图1为本发明车辆协同定位方法流程图 具体实施方式 0076 下面结合附图来对本发明做进一步说明。 0077 步骤一、 利用车路协同LTE-V2X定位技术实现无人。

34、车初步定位； 说明书 6/10 页 11 CN 111949943 A 11 0078 E、 首先在弱GNSS信号区域合理部署路侧LTE信号基站的数量与位置， 使得车辆在 运行过程中始终至少处于3个路侧LTE信号基站的良好通信范围内。 无人车作为移动节点行 驶于道路中， 路侧LTE信号基站作为固定定位基站。 0079 F、 无人车发送定位请求request并且标记发送时间。 然后路侧LTE信号基站接收到 无人车发来的定位请求之后， 标记请求到达时间。 接着计算发送与到达的时间差t1， 以及请 求处理时间t2， 然后向无人车发送定位处理消息。 无人车接收到定位处理消息之后标记消 息到达时间， 所。

35、以由此可知， 无人车从发送请求到接收到处理信息所经历了时间为： 0080 t32t1+t2 (1) 0081 车辆发送请求到路侧LTE信号基站的时间t1为： 0082 0083 由于晶振频率收到石英晶体漂移的影响， 导致计时系统存在误差。 车辆发送请求 到路侧 LTE信号基站接收到请求的时间t4为： 0084 0085 其中e1， e2分别表示车辆与路侧LTE信号基站的晶振漂移误差。 0086 C： 无人车近似处于二维平面， 其位置假设为(x， y)， 并且与路侧LTE信号基站处于 视距状态， 无人车发送定位请求信号到路侧LTE信号基站接收得到的时间为t4， 利用TOA测 距算法以及三边定位算。

36、法计算得到无人车在车道的位置信息： 0087 0088 其中c为无线信号传播速率， (xi， yi)为路侧LTE信号基站的位置坐标。 0089 步骤二、 无人车基于激光雷达高精度地图匹配技术实现定位； 0090 A： 固态激光雷达布置于无人车车轴中心车顶处， 激光雷达获取周围环境激光点云 数据， 并对数据进行处理。 车辆通过获取GPS信号来判断是否需要进行特征匹配定位技术， 当GPS 信号低于阈值的时候， 则采用特征匹配定位技术来弥补GPS信号差的问题。 高精度地 图匹配定位主要采用ndt配准算法实现， 具体步骤如下： 0091 B： 激光雷达实时获取周围环境的激光点云数据， 由于采用固态激光。

37、雷达， 所以每 一帧获取的激光点云数据的空间范围为以激光雷达为中心的锥形。 在整个特征匹配过程中 需要将全局高精度地图作为ndt配准算法的目标点云， 并且对其进行网格化处理。 将开始进 行特征匹配定位的时刻记为t， 此后获取的每一帧原始环境激光点云数据需要去除距离车 体较近与较远的激光点集， 然后利用体素滤波过滤剩下的激光点云数据， 在保持点云统计 特征的情况下降低激光点云数据集的尺寸。 最后将降采样后的过滤点云作为ndt配准算法 的输入源点云； 0092 C： 对于全局目标点云， 划分好网格之后， 将点云加载到网格内， 然后计算每一个网 格内的正态分布概率密度函数pdfi， 首先计算均值向量。

38、然后计算协方差矩阵， 最后通 过均值向量和协方差矩阵求出每一个网格内的正态分布概率密度函数pdfi。 由于需要用到 协方差矩阵的逆矩阵， 所以格子内不少于三个点， 一般至少要保证有5个点， 计算公式如下： 说明书 7/10 页 12 CN 111949943 A 12 0093 0094 0095 0096 D： 此时需要求出输入源点云点云相对于目标点云的初始坐标变换参数。 坐标变换 通常涉及到平移与旋转， 平移通过平移向量表示， 旋转通过旋转矩阵表示， 旋转是关于xyz 三个固定轴的旋转， 转角由rpy表示。 记该时刻下扫描到的激光点的坐标为激光点 相对于全局目标点云的坐标为通过坐标变换公式。

39、可以将当前帧扫描的激光点云 坐标映射到全局目标点云： 0097 X R33X+T31 (8) 0098 0099 Ttx ty tzT (10) 0100 其中R表示旋转矩阵， T表示平移向量。 这一步是寻找一个合适的初始坐标变换使 得目标点云大致处于参考点云的坐标平面当中， 这一步是提供变换参数的初值， 为下一步 变换参数的迭代提供距离最优点较近的初值。 在自动驾驶里初始值的提供可以依靠gps， 里 程计或者是imu惯性导航， 利用上述传感器获取到此时自动驾驶车辆的位置以及姿态， 通过 坐标变换得到相对于参考点云的坐标变换参数， 也就是位移T与旋转R。 在弱GNSS信号区域 内单纯依靠里程计。

40、或者imu会产生累计误差， 所有需要利用步骤一车路协同LTE-V2X定位技 术提供无人车初步定位位置， 利用imu陀螺仪求出旋转。 0101 D： 将输入源点云根据初始变换参数将坐标转换到全局目标点云， 此时输入源点云 会分布在目标点云网格中， 然后根据转换后的目标点云的坐标， 利用对应所在网格的正态 分布概率密度， 求出对应的概率。 0102E： 算法目的是寻找最佳坐标变换参数使得目标点云位于参考点云的可能性最 大化。 算法需要优化的参数 是对目标点云的坐标变换(包括平移和旋转)。 给定目标点云 和变换参数令空间转换函数表示利用变换参数 来移动目标点 至参考点云。 0103 F： 然后将每一。

41、个点的概率乘积起来作为目标似然函数。 通过似然函数找到概率乘 积最大时的坐标转换关系。 简单来说就是由最大的概率找到最优的坐标转换， 最大似然函 数如下： 说明书 8/10 页 13 CN 111949943 A 13 0104 0105 G： 将最大似然函数作负对数变换， 易于优化求解。 0106 0107H： 对最大似然函数进一步化简变形得到目标函数 0108 0109 0110 I： 利用牛顿法进行迭代更新， 求出最佳变换参数， 完成配准。 迭代公式如下： 0111 0112 0113 0114 0115 其中H为目标函数的海塞矩阵， g为目标的梯度向量。 0116 ndt配准完成之后，。

42、 此时输入源激光点云已配准至全局目标点云， 从而由高精度地 图来得到精准的车辆位姿信息。 0117 步骤三、 利用线性卡尔曼滤波进行融合定位 0118 A： 由步骤二可知， 车体中心的位置为Xi(xi， yi)T， 由高精度地图特征匹配可以得 到车体的航向角所以车体的状态转移方程如下： 0119 0120 无人车运动过程中的观测方程如下： 0121 ZiXi+Vi (20) 0122 观测值Zi则是通过LTE-V2X无线信号测量得到的车辆定位结果， Vi观测噪声的协方 差矩阵Ri表示的是LTE-V2X无线信号测量定位结果的精度指标， 并且为常数项矩阵。 0123B：在无线信号定位测量值进行一阶。

43、泰勒展开得到： 0124 0125 令高精度定位匹配所得到的无人车位置与航向角的协方差矩阵为M， 并且该矩阵 说明书 9/10 页 14 CN 111949943 A 14 为常数矩阵： 0126 0127 由此能够得到转移噪声Wi的协方差矩阵Oi， 该矩阵表示的高精度地图特征匹配定 位所得到的坐标增量精度， 该值与无人车行驶距离以及航向角有关： 0128 0129 车辆初始位置均由LTE-V2X无线信号定位得到。 0130 C： 代入初值位置以及初始协方差矩阵迭代求解， 可以得到每一时刻无人车的最佳 位置估计。 0131 首先计算无人车在i时刻的预测值： 0132 0133 接着计算预测值的协方差矩阵： 0134 0135 然后计算卡尔曼增益矩阵： 0136 0137 接着由系统观测值进行修正得到无人车位置的最优估计： 0138。 说明书 10/10 页 15 CN 111949943 A 15 图1 说明书附图 1/1 页 16 CN 111949943 A 16 。