车辆的自动驾驶定位方法、装置、车辆及存储介质

本技术涉及雷达定位，特别涉及一种车辆的自动驾驶定位方法、装置、车辆及存储介质。

背景技术：

1、相关技术中，基于4d毫米波雷达的定位方法均基于4d毫米波雷达的点云信息实现，主要包含以下两类：

2、一类基于点云目标的多普勒效应速度，基于最小二乘法计算车辆当前速度，再结合时间戳计算得到两帧之间的车辆运动，累计后完成实时局部定位。这类方法忽略了感知到的目标位置，仅基于多普勒信息进行定位，容易受动态物体影响，造成较大的误差，且长时域情况下定位效果不够稳定。

3、另一类则对两帧之间的点云进行目标匹配，基于匹配结果计算点云之间的位姿变换，进而累计完成定位。这类方法利用了点云目标的位置信息，效果有所提升。但由于4d毫米波雷达的点云是从原始感知数据中采样生成，其帧间稳定性较差，且容易受到多径效应的影响，造成点云质量较差，噪声多且稀疏，因此会有较大的帧间匹配误差，导致定位精度也不佳。

4、综上所述，相关技术中，基于4d毫米波雷达的定位方法均无法利用其丰富的原始信息，且定位效果较不稳定，存在较大误差，影响后续对定位信息的使用，有待改进。

技术实现思路

1、本技术提供一种车辆的自动驾驶定位方法、装置、车辆及存储介质，以解决相关技术中，难以充分利用4d毫米波雷达的原始信息，且定位效果较不稳定，存在较大误差的技术问题。

2、本技术第一方面实施例提供一种车辆的自动驾驶定位方法，包括以下步骤：获取至少一个目标的4d毫米波雷达张量；利用所述4d毫米波雷达张量计算车辆的实际车速；基于相邻帧之间的张量误差，构建位姿变换优化方程，并求解所述位姿变换优化方程，得到与所述4d毫米波雷达张量匹配的所述车辆的当前帧的先验位姿变换；将所述实际车速和所述当前帧的先验位姿变换输入至预先构建的扩展卡尔曼滤波器，以得到所述车辆的观测位姿变换，并基于所述观测位姿变换得到所述车辆的定位结果。

3、可选地，在本技术的一个实施例中，所述获取至少一个目标的4d毫米波雷达张量，包括：获取原始4d张量，其中，所述原始4d张量由距离、方向角、俯仰角和多普勒速度构成；对所述原始4d张量进行多普勒维度降维，得到所述4d毫米波雷达张量。

4、可选地，在本技术的一个实施例中，所述利用所述4d毫米波雷达张量计算车辆的实际车速，包括：基于所述4d毫米波雷达张量和迭代重加权的最小二乘法得到相应的目标权重，并根据所述目标权重从所述至少一个目标中分离出动态目标和/或静态目标；基于所述静态目标的4d毫米波雷达张量，计算所述车辆的速度矢量，和/或基于所述动态目标的4d毫米波雷达张量和所述动态目标的目标速度径向分量，计算所述速度矢量；基于所述速度矢量和上一帧的速度矢量，得到所述实际车速。

5、可选地，在本技术的一个实施例中，所述基于相邻帧之间的张量误差，构建位姿变换优化方程，并求解所述位姿变换优化方程，得到与所述4d毫米波雷达张量匹配的所述车辆的当前帧的先验位姿变换，包括：将所述4d毫米波雷达张量与上一帧的4d毫米波雷达张量进行对比，得到所述张量误差；基于所述张量误差和预设优化约束，构建相应的目标函数；求解所述目标函数，得到满足第二预设收敛条件的当前帧的先验位姿变换。

6、可选地，在本技术的一个实施例中，所述将所述实际车速和所述当前帧的先验位姿变换输入至预先构建的扩展卡尔曼滤波器，得到所述车辆的观测位姿变换，包括：基于所述车辆的上一帧的后验位姿和协方差，得到所述上一帧后验位姿对应的线性化车辆运动方程和线性化观测方程；根据所述实际车速、所述线性化车辆运动方程和线性化观测方程估计所述车辆的当前先验位姿和先验协方差，并基于所述当前先验位姿和先验协方差计算对应的卡尔曼增益；基于所述卡尔曼增益和观测位姿变换得到当前帧的后验位姿和协方差。

7、本技术第二方面实施例提供一种车辆的自动驾驶定位装置，包括：获取模块，用于获取至少一个目标的4d毫米波雷达张量；第一计算模块，用于利用所述4d毫米波雷达张量计算车辆的实际车速；第二计算模块，用于基于相邻帧之间的张量误差，构建位姿变换优化方程，并求解所述位姿变换优化方程，得到与所述4d毫米波雷达张量匹配的所述车辆的当前帧的先验位姿变换；定位模块，用于将所述实际车速和所述当前帧的先验位姿变换输入至预先构建的扩展卡尔曼滤波器，以得到所述车辆的观测位姿变换，并基于所述观测位姿变换得到所述车辆的定位结果。

8、可选地，在本技术的一个实施例中，所述获取模块包括：获取单元，用于获取原始4d张量，其中，所述原始4d张量由距离、方向角、俯仰角和多普勒速度构成；降维单元，用于对所述原始4d张量进行多普勒维度降维，得到所述4d毫米波雷达张量。

9、可选地，在本技术的一个实施例中，所述第一计算模块包括：分离单元，用于基于所述4d毫米波雷达张量和迭代重加权的最小二乘法得到相应的目标权重，并根据所述目标权重从所述至少一个目标中分离出动态目标和/或静态目标；第一计算单元，用于基于所述静态目标的4d毫米波雷达张量，计算所述车辆的速度矢量，和/或基于所述动态目标的4d毫米波雷达张量和所述动态目标的目标速度径向分量，计算所述速度矢量；第二计算单元，用于基于所述速度矢量和上一帧的速度矢量，得到所述实际车速。

10、可选地，在本技术的一个实施例中，所述第二计算单元包括：第三计算单元，用于将所述4d毫米波雷达张量与上一帧的4d毫米波雷达张量进行对比，得到所述张量误差；构建单元，用于基于所述张量误差和预设优化约束，构建相应的目标函数；第四计算单元，用于求解所述目标函数，得到满足第二预设收敛条件的当前帧的先验位姿变换。

11、可选地，在本技术的一个实施例中，所述定位模块包括：第五计算单元，用于基于所述车辆的上一帧的后验位姿和协方差，得到所述上一帧后验位姿对应的线性化车辆运动方程和线性化观测方程；第六计算单元，用于根据所述实际车速、所述线性化车辆运动方程和线性化观测方程估计所述车辆的当前先验位姿和先验协方差，并基于所述当前先验位姿和先验协方差计算对应的卡尔曼增益；第七计算单元，用于基于所述卡尔曼增益和观测位姿变换得到当前帧的后验位姿和协方差。

12、本技术第三方面实施例提供一种车辆，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如上述实施例所述的车辆的自动驾驶定位方法。

13、本技术第四方面实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储计算机指令，所述计算机指令用于使所述计算机执行如上述实施例所述的车辆的自动驾驶定位方法。

14、本技术第五方面实施例提供一种计算机程序产品，包括计算机程序，所述计算机程序被执行时，用于实现如上的车辆的自动驾驶定位方法。

15、本技术实施例可以根据至少一个目标的4d毫米波雷达张量计算车辆的实际车速，并基于相邻帧之间的张量误差，构建位姿变换优化方程，从而求解位姿变换优化方程，得到与4d毫米波雷达张量匹配的车辆的当前帧的先验位姿变换，进而将实际车速和当前帧的先验位姿变换输入至预先构建的扩展卡尔曼滤波器，以得到车辆的观测位姿变换，并基于观测位姿变换得到车辆的定位结果，充分利用4d毫米波雷达的原始信息，得到精度更高的定位结果。由此，解决了相关技术中，难以充分利用4d毫米波雷达的原始信息，且定位效果较不稳定，存在较大误差的技术问题。

16、本技术附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。