CN117541655B - 融合视觉语义消除radar建图z轴累积误差的方法 - Google Patents

融合视觉语义消除radar建图z轴累积误差的方法 Download PDF

Info

Publication number
CN117541655B
CN117541655B CN202410033363.9A CN202410033363A CN117541655B CN 117541655 B CN117541655 B CN 117541655B CN 202410033363 A CN202410033363 A CN 202410033363A CN 117541655 B CN117541655 B CN 117541655B
Authority
CN
China
Prior art keywords
millimeter wave
semantic
wave imaging
imaging radar
radar
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202410033363.9A
Other languages
English (en)
Other versions
CN117541655A (zh
Inventor
赵映重
陆新飞
薛旦
史颂华
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Geometry Partner Intelligent Driving Co ltd
Original Assignee
Shanghai Geometry Partner Intelligent Driving Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Geometry Partner Intelligent Driving Co ltd filed Critical Shanghai Geometry Partner Intelligent Driving Co ltd
Priority to CN202410033363.9A priority Critical patent/CN117541655B/zh
Publication of CN117541655A publication Critical patent/CN117541655A/zh
Application granted granted Critical
Publication of CN117541655B publication Critical patent/CN117541655B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/70—Labelling scene content, e.g. deriving syntactic or semantic representations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Evolutionary Computation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Medical Informatics (AREA)
  • Computing Systems (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Computational Linguistics (AREA)
  • Health & Medical Sciences (AREA)
  • Electromagnetism (AREA)
  • General Health & Medical Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Image Processing (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明涉及一种融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法，其中，该方法包括：获取图像数据，并利用预先训练好的神经网络模型进行在线推测，获取当前视觉帧语义；利用4D毫米波成像雷达点云进行帧间匹配，估计出当前车辆在全局参考坐标系下的位置姿态；进行数据关联；对4D毫米波成像雷达建图添加当前已经生成的视觉语义里程计约束，并和4D毫米波成像雷达里程计约束一同进行全局优化，通过构造最小二乘求解方程，求得当前车辆的位置姿态消除z轴的累积误差。本发明还涉及一种相应的系统、装置、处理器及其存储介质。采用了本发明的该方法、系统、装置、处理器及其存储介质，能够在在优化时能大大减少z轴上的累积误差。

Description

融合视觉语义消除radar建图z轴累积误差的方法
技术领域
本发明涉及4D毫米波成像雷达领域，尤其涉及视觉语义领域，具体是指一种融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法、系统、装置、处理器及其计算机可读存储介质。
背景技术
radar基于点云进行SLAM建图，得到环境信息的表征。由于radar点云测量精度的误差，尤其是在俯仰项的测量精度导致点云在z轴上的测量误差较大，精度不高，以及SLAM建图前端处理的累积误差，导致建图时估计的自车Pose的z轴累积误差较大，进而导致SLAM全局地图一致性较差。
目前，对于radar建图z轴累积误差消除的方法，一般用GPS的高程信息对z轴的估计进行约束，进而消除误差，但是GPS容易受建筑物遮挡等造成信号不良，导致GPS的高程信息不准或者输出信息丢失；此外，另外一种方法是通过位置识别闭环检测算法，添加闭环约束来消除z轴的累积误差，但对于场景变化较大位置识别不出来相似地方，进而无法通过闭环的方法消除累积误差。
发明内容
本发明的目的是克服了上述现有技术的缺点，提供了一种融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法、系统、装置、处理器及其计算机可读存储介质。
为了实现上述目的，本发明的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法、系统、装置、处理器及其计算机可读存储介质如下：
该融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法，其主要特点是，所述的方法包括以下步骤：
（1）输入自动驾驶车辆上搭载的前视摄像头获取到的图像数据，并利用预先训练好的神经网络模型，实时在线推测场景中地面上的标记物，并利用前后两帧图像之间的语义特征变换生成视觉语义里程计；
（2）利用4D毫米波成像雷达点云进行帧间匹配，估计出当前车辆在全局参考坐标系下的位置姿态，并利用上一帧4D毫米波成像雷达点云估计的位置姿态，计算出两帧4D毫米波成像雷达点云之间的相对变换；
（3）将当前获取到的视觉语义信息和4D毫米波成像雷达帧间匹配信息进行数据关联，使得二者能够在时间上相对齐；
（4）对4D毫米波成像雷达建图添加当前已经生成的视觉语义里程计约束，并和4D毫米波成像雷达里程计约束一同进行全局优化，通过构造最小二乘求解方程，求得当前车辆的位置姿态，从而使得z轴的累积误差得到消除。
较佳地，所述的步骤（1）具体包括以下步骤：
（1.1）输入当前车辆前视摄像头获取到的图像数据；
（1.2）进行图像数据预处理：划定预设范围的感兴趣处理区域ROI，即以自动驾驶车辆轴中心为坐标原点，将图像帧i所有像素点的像素点保留，其中/>为图像上的某个像素点，/>分别为图像像素坐标在x轴方向的上下边界，/>分别为图像像素坐标在y轴方向的上下边界；将感兴趣处理区域ROI提取完的所有像素点输入到预先训练好的神经网络模型/>中进行推理，其中/>为网络参数, />为参数对应的权重，得到当前语义帧对应的语义信息即检测到的语义信息/>在时间/>下，在摄像头坐标下的位姿表示，具体按照如下公式进行表示：
；
其中，m为提取ROI后的图像上所有像素点个数，为分割网络参数，为分割网络参数对应的权重,/>为图像上某个像素点，/>为网络参数作用机制操作符，/>为当前语义信息位姿；
（1.3）按照以下方式计算语义里程计约束：
；
其中，为两帧语义信息之间的位姿相对变换，/>为前一帧的语义里程计的位姿，T为进行转置处理。
较佳地，所述的步骤（2）具体包括以下步骤：
（2.1）输入以车辆后轴为中心的自车坐标下的4D毫米波成像雷达点云；
（2.2）进行4D毫米波成像雷达匹配：即找到当前4D毫米波成像雷达帧的点云和上一4D毫米波成像雷达帧点云/>之间的相对变换/>，使得上一帧点云经过该相对变换后和当前帧的点云之间的对应点距离误差达到最小，则获得了最精准的匹配，匹配方式如下：
；
其中，R和t为前后4D毫米波成像雷达帧相对运动变换的旋转矩阵和平移向量；
（2.3）计算4D毫米波成像雷达里程计约束：经过4D毫米波成像雷达匹配后，得到当前帧的4D毫米波成像雷达位姿估计信息即/>，并按照以下方式计算4D毫米波成像雷达里程计的约束：
；
其中，为两帧4D毫米波成像雷达帧之间的位姿相对变换，/>为前一4D毫米波成像雷达帧的里程计位姿，/>为当前4D毫米波成像雷达帧的里程计位姿。
较佳地，所述的步骤（3）具体包括以下步骤：
（3.1）以4D毫米波成像雷达匹配信息的时间为基准，寻找对应的视觉语义的匹配信息，即在视觉语义时间戳内找到与当前4D毫米波成像雷达时间戳/>时间最近的语义时间戳/>；
（3.2）利用线性插值对相应的语义信息进行插值处理，从而完成视觉语义信息和4D毫米波成像雷达帧间匹配信息之间的数据关联，具体为：
由步骤（3.1）得到语义时间戳，则通过步骤（1.2）可知语义时间戳/>和上一时刻/>的语义位姿信息分别为/>和/>，则按照如下公式进行插值得到4D毫米波成像雷达时间戳/>对应的语义位姿约束，并以此完成雷达语义的数据关联：
；
其中，为语义和视觉融合后的位姿约束，/>为语义关联雷达后时间戳/>对应的位姿，/>为时间戳/>上一时刻/>的位姿的逆变换，/>和/>分别为雷达当前时刻和上一时刻。
较佳地，所述的步骤（4）具体包括以下步骤：
（4.1）利用前视摄像头和车辆后轴中心的自车坐标之间的外参，将当前获取到的视觉语义信息转换到自车坐标下，使得其与4D毫米波成像雷达里程计约束的坐标达到统一，转换过程如下：
；
其中，为摄像头和自车坐标之间外参，/>为经过外参/>变换后的视觉语义里程计约束；
（4.2）按照以下方式构造求解的误差方程：
（4.2.1）获取语义约束误差：
；
其中，为求解过程中得到视觉语义约束误差，/>为求解过程中得到的语义约束，是求解中间量；
（4.2.2）获取4D毫米波成像雷达匹配的误差：
；
其中，为求解过程中得到的4D毫米波成像雷达里程计约束误差，/>为求解过程中得到的4D毫米波成像雷达里程计约束，是求解中间量；
（4.2.3）获取误差方程：
；
其中，为/>的转置，/>为/>的转置，/>为视觉语义约束在优化求解中的信息矩阵，/>为4D毫米波成像雷达里程计约束在优化时的信息矩阵；
（4.2.4）求解上述误差方程，得到优化后的自车位姿估计的z轴的值：
；
其中， 为当前最终求解后的位姿，包括3×3的旋转矩阵/>和3×1的平移向量/>，从平移向量/>中取出第3维/>作为估计的z轴的值；
（4.3）将已累积的z轴误差带入上述经过优化后的位姿方程而消除，得到误差消除后的z轴值。
该基于上述方法实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的系统，其主要特点是，所述的系统包括：
视觉语义模块，用于通过搭载在车辆上的前视摄像头获取图像数据，并将获取到的图像数据输入到语义分割网络中进行在线推理，以获取当前4D毫米波成像雷达帧的语义信息；
radar建图模块，用于利用4D毫米波成像雷达点云进行帧间匹配，从而估计出当前车辆在全局参考坐标系下的位置姿态；
数据关联模块，分别与所述的视觉语义模块和radar建图模块相连接，用于将获取到的视觉语义信息和4D毫米波成像雷达帧间匹配信息进行数据关联，使得二者能够在时间上相对齐；以及
radar语义融合消除z轴累积误差模块, 分别与所述的视觉语义模块和radar建图模块相连接，用于将生成的视觉语义里程计约束和4D毫米波成像雷达里程计约束共同进行全局优化处理，并通过构造最小二乘求解方程，获取当前车辆的位置姿态，从而使得z轴的累积误差得到消除。
该实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的装置，其主要特点是，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
该实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的处理器，其主要特点是，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
该计算机可读存储介质，其主要特点是，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
采用了本发明的该融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法、系统、装置、处理器及其计算机可读存储介质，通过引入视觉摄像头图像数据，利用神经网络对地面上的标志物进行分割检测得到视觉语义约束，进而添加到radar建图的全局优化模块中进行优化，消除了radar建图中的z轴累积误差。由于优化时考虑了地面标志物的参考信息，而地面参考标志物在z轴上的精度是比较高，所以用一个精度比较高的语义约束能对原来radar建图中的z轴上的值进行约束进而在优化时能大大减少z轴上的累积误差。
附图说明
图1为本发明的融合视觉语义消除radar建图z轴累积误差的方法的流程图。
具体实施方式
为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。
在详细说明根据本发明的实施例前，应该注意到的是，在下文中，术语“包括”、“包含”或任何其他变体旨在涵盖非排他性的包含，由此使得包括一系列要素的过程、方法、物品或者设备不仅包含这些要素，而且还包含没有明确列出的其他要素，或者为这种过程、方法、物品或者设备所固有的要素。
在详细说明本技术方案之前，下面将详细说明本技术方案使用到的部分技术特征具体含义：
Radar：4D毫米波成像雷达
SLAM：即时建图和定位
Pose：位置姿态，包括位置信息，x,y,z以及姿态信息，roll, pitch, yaw
GPS：全球定位系统
ROI：Region of Interesting，感兴趣区域
自车：自动驾驶车辆
与现有技术相比，本技术方案提出一种利用前视摄像头分割地面获取语义信息消除radar累积误差的方法。该方法利用深度学习网络先对分割任务进行模型训练，然后对地面上的标志物，比如车道线，交通标识符等进行实时推理检测，将连续两帧之间的语义变换作为语义约束添加到radar建图的全局优化中，通过求解非线性最小二乘问题，将z轴的累积误差进行消除。因为使用了地面上标志物的参考信息，因此通过地面标识物的约束可以对radar建图的z轴上的误差进行比较好的纠正。
请参阅图1所示，该融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法，其中，所述的方法包括以下步骤：
（1）输入自动驾驶车辆上搭载的前视摄像头获取到的图像数据，并利用预先训练好的神经网络模型，实时在线推测场景中地面上的标记物，并利用前后两帧图像之间的语义特征变换生成视觉语义里程计；
（2）利用4D毫米波成像雷达点云进行帧间匹配，估计出当前车辆在全局参考坐标系下的位置姿态，并利用上一帧4D毫米波成像雷达点云估计的位置姿态，计算出两帧4D毫米波成像雷达点云之间的相对变换；
（3）将当前获取到的视觉语义信息和4D毫米波成像雷达帧间匹配信息进行数据关联，使得二者能够在时间上相对齐；
（4）对4D毫米波成像雷达建图添加当前已经生成的视觉语义里程计约束，并和4D毫米波成像雷达里程计约束一同进行全局优化，通过构造最小二乘求解方程，求得当前车辆的位置姿态，从而使得z轴的累积误差得到消除。
作为本发明的优选实施方式，所述的步骤（1）具体包括以下步骤：
（1.1）输入当前车辆前视摄像头获取到的图像数据；
（1.2）进行图像数据预处理：划定预设范围的感兴趣处理区域ROI，即以自动驾驶车辆轴中心为坐标原点，将图像帧i所有像素点的像素点保留，其中/>为图像上的某个像素点，/>分别为图像像素坐标在x轴方向的上下边界，/>分别为图像像素坐标在y轴方向的上下边界；将感兴趣处理区域ROI提取完的所有像素点输入到预先训练好的神经网络模型/>中进行推理，其中/>为网络参数,/>为参数对应的权重，得到当前语义帧对应的语义信息，即检测到的语义信息/>在时间/>下，在摄像头坐标下的位姿表示，具体按照如下公式进行表示：
；
其中，m为提取ROI后的图像上所有像素点个数，为分割网络参数，为分割网络参数对应的权重,/>为图像上某个像素点，/>为网络参数作用机制操作符，/>为当前语义信息位姿；
（1.3）按照以下方式计算语义里程计约束：
；
其中，为两帧语义信息之间的位姿相对变换，/>为前一帧的语义里程计的位姿，T为进行转置处理。。
作为本发明的优选实施方式，所述的步骤（2）具体包括以下步骤：
（2.1）输入以车辆后轴为中心的自车坐标下的4D毫米波成像雷达点云；
（2.2）进行4D毫米波成像雷达匹配：即找到当前4D毫米波成像雷达帧的点云和上一4D毫米波成像雷达帧点云/>之间的相对变换/>，使得上一帧点云经过该相对变换后和当前帧的点云之间的对应点距离误差达到最小，则获得了最精准的匹配，匹配方式如下：
；
其中，R和t前后4D毫米波成像雷达帧相对运动变换的旋转矩阵和平移向量；
（2.3）计算4D毫米波成像雷达里程计约束：经过4D毫米波成像雷达匹配后，得到当前帧的4D毫米波成像雷达位姿估计信息即/>，并按照以下方式计算4D毫米波成像雷达里程计的约束：
；
其中，为两帧4D毫米波成像雷达帧之间的位姿相对变换，/>为前一4D毫米波成像雷达帧的里程计位姿，/>为当前4D毫米波成像雷达帧的里程计位姿。
作为本发明的优选实施方式，所述的步骤（3）具体为：
（3.1）以4D毫米波成像雷达匹配信息的时间为基准，寻找对应的视觉语义的匹配信息，即在视觉语义时间戳内找到与当前4D毫米波成像雷达时间戳/>时间最近的语义时间戳/>；
（3.2）利用线性插值对相应的语义信息进行插值处理，从而完成视觉语义信息和4D毫米波成像雷达帧间匹配信息之间的数据关联，具体为：
由步骤（3.1）得到语义时间戳，则通过步骤（1.2）可知语义时间戳/>和上一时刻/>的语义位姿信息分别为/>和/>，则按照如下公式进行插值得到4D毫米波成像雷达时间戳/>对应的语义位姿约束，并以此完成雷达语义的数据关联：
；
其中，为语义和视觉融合后的位姿约束，/>为语义关联雷达后时间戳/>对应的位姿，/>为时间戳/>上一时刻/>的位姿的逆变换，/>和/>分别为雷达当前时刻和上一时刻。
作为本发明的优选实施方式，所述的步骤（4）具体包括以下步骤：
（4.1）利用前视摄像头和车辆后轴中心的自车坐标之间的外参，将当前获取到的视觉语义信息转换到自车坐标下，使得其与4D毫米波成像雷达里程计约束的坐标达到统一，转换过程如下：
；
其中，为摄像头和自车坐标之间外参，/>为经过外参/>变换后的视觉语义里程计约束；
（4.2）按照以下方式构造求解的误差方程：
（4.2.1）获取语义约束误差：
；
其中，为求解过程中得到视觉语义约束误差，/>为求解过程中得到的语义约束，是求解中间量；
（4.2.2）获取4D毫米波成像雷达匹配的误差：
；
其中，为求解过程中得到的4D毫米波成像雷达里程计约束误差，/>为求解过程中得到的4D毫米波成像雷达里程计约束，是求解中间量；
（4.2.3）获取误差方程：
；
其中，为/>的转置，/>为/>的转置，/>为视觉语义约束在优化求解中的信息矩阵，/>为4D毫米波成像雷达里程计约束在优化时的信息矩阵；
（4.2.4）求解上述误差方程，得到优化后的自车位姿估计的z轴的值：
；
其中， 为当前最终求解后的位姿，包括3×3的旋转矩阵/>和3×1的平移向量/>，从平移向量/>中取出第3维/>作为估计的z轴的值；
（4.3）将已累积的z轴误差带入上述经过优化后的位姿方程而消除，得到误差消除后的z轴值。
该基于上述所述的方法实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的系统，其中，所述的系统包括：
视觉语义模块，用于通过搭载在车辆上的前视摄像头获取图像数据，并将获取到的图像数据输入到语义分割网络中进行在线推理，以获取当前4D毫米波成像雷达帧的语义信息；
radar建图模块，用于利用4D毫米波成像雷达点云进行帧间匹配，从而估计出当前车辆在全局参考坐标系下的位置姿态；
数据关联模块，分别与所述的视觉语义模块和radar建图模块相连接，用于将获取到的视觉语义信息和4D毫米波成像雷达帧间匹配信息进行数据关联，使得二者能够在时间上相对齐；以及
radar语义融合消除z轴累积误差模块, 分别与所述的视觉语义模块和radar建图模块相连接，用于将生成的视觉语义里程计约束和4D毫米波成像雷达里程计约束共同进行全局优化处理，并通过构造最小二乘求解方程，获取当前车辆的位置姿态，从而使得z轴的累积误差得到消除。
该实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的装置，其中，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
该实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的处理器，其中，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
该计算机可读存储介质，其中，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
在实际应用当中，如图1所示，本技术方案的该融合视觉语义消除radar建图z轴累积误差的方法及其系统，主要步骤包括3大模块，视觉语义模块，radar建图模块，数据关联，以及radar语义融合消除z轴累积误差模块。下面详细介绍各个模块的处理过程：
1.语义模块
语义模块，输入自动驾驶车辆上搭载的前视摄像头产生的图像数据，利用预先训练好的神经网络模型，实时在线推测场景中地面上的标记物，例如车道线，交通指示箭头等语义信息，利用两帧之间的语义特征变换产生出语义里程计。具体的处理流程如下：
a）输入前视摄像头图像数据；
b）图像数据预处理。划定[10m×10m]的感兴趣处理区域ROI，减少推理时的运算量，将预处理后的图像输送到预先训练好的地面分割神经网络模型中进行在线推理检测，得到当语义帧的语义信息，即检测到的语义信息在时间/>在摄像头坐标下的位姿/>表示；
c）计算语义里程计约束，约束是两帧语义信息之间的相对变换/>。计算如公式1所示：
；
其中，为两帧语义信息之间的位姿相对变换，/>为前一帧的语义里程计的位姿，/>为当前帧的语义里程计位姿，/>为当前计算出来的视觉语义约束。
2.radar建图模块
radar建图利用radar点云进行帧间匹配，估计出当前车辆在全局参考坐标下的pose，利用上一帧radar估计的pose，计算出两帧radar之间的相对变换。进而产生radar帧间的相对变换。
a）输入以车辆后轴中心的自车坐标下的radar点云；
b）radar匹配。即找到当前radar帧的点云和上一radar帧点云/>之间的相对变换/>，使得上一帧点云经过该变换后和当前帧的点云之间的对应点距离误差/>达到最小，则认为找到了正确的匹配,即求解如公式2的方程：
；
其中，为上一帧radar帧中第i个点，/>为当前radar帧中第i个点，R和t是前后radar帧相对运动变换/>的旋转矩阵和平移向量。
c）计算radar里程计约束。经过radar匹配后，得到当前帧的radar位姿估计信息即/>，利用如下公式3获得radar里程计的约束：
；
其中，为两帧radar帧之间的位姿相对变换，/>为前一radar帧的里程计位姿，/>为当前radar帧的里程计位姿，/>为当前计算出来的radar里程计约束。
3.数据关联
为了视觉语义信息和radar匹配信息在时间上能进行对齐，需要根据二者的时间进行同步和数据关联，以radar匹配信息的时间为基准，找对应的视觉语义的匹配信息。利用线性插值对相应的语义信息进行插值。
4.radar语义融合消除z轴累积误差
对radar建图添加已经产生的视觉语义里程计约束，和radar里程计约束一起进行全局优化，通过构造最小二乘求解方程，求得位姿使得z轴的累积误差得到消除。
a）由于视觉语义信息是在摄像头坐标下产生的，需要利用摄像头和车辆后轴中心的自车坐标之间的外参将其转换到自车坐标下和radar里程计约束的坐标达到统一，转换过程如下公式4所示，得到变换后的视觉语义里程计约束/>。
；
为摄像头和自车坐标之间外参，/>为步骤1得到的视觉语义的约束，/>为经过外参/>变换后的视觉语义约束。
b）构造求解的误差方程.
语义约束误差为：
；
为求解过程中得到视觉语义约束误差，/>为求解过程中得到的语义约束，是求解中间量，/>为4(a)中得到语义约束，是已知量。
radar匹配的误差：
；
为求解过程中得到radar里程计约束误差，/>为求解过程中得到radar里程计约束，是求解中间量，/>为2(c)中构造的radar里程计约束。
误差方程：
；
和/>分别为在4(b)得到的视觉语义约束误差和radar里程计约束误差，/>为视觉语义约束在优化求解中的信息矩阵，来自1(c), />为radar里程计约束在优化时的信息矩阵，来自于2(c)。这2个值一般可以设为常值。
c）求解方程得到优化后的自车位姿估计的z轴的值。
为当前最终求解后的位姿，包括3×3的旋转矩阵/>和3×1的平移向量/>,从/>中取出第3维/>作为估计的z轴的值。累积的z轴误差通过求解上述优化方程而消除，得到误差消除后的z轴值。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行装置执行的软件或固件来实现。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成的，程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。
在本说明书的描述中，参考术语“一实施例”、“一些实施例”、“示例”、“具体示例”、或“实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
采用了本发明的该融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法、系统、装置、处理器及其计算机可读存储介质，通过引入视觉摄像头图像数据，利用神经网络对地面上的标志物进行分割检测得到视觉语义约束，进而添加到radar建图的全局优化模块中进行优化，消除了radar建图中的z轴累积误差。由于优化时考虑了地面标志物的参考信息，而地面参考标志物在z轴上的精度是比较高，所以用一个精度比较高的语义约束能对原来radar建图中的z轴上的值进行约束进而在优化时能大大减少z轴上的累积误差。
在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (5)

1.一种融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法，其特征在于，所述的方法包括以下步骤：
（1）输入自动驾驶车辆上搭载的前视摄像头获取到的图像数据，并利用预先训练好的神经网络模型，实时在线推测场景中地面上的标记物，并利用前后两帧图像之间的语义特征变换生成视觉语义里程计；
（2）利用4D毫米波成像雷达点云进行帧间匹配，估计出当前车辆在全局参考坐标系下的位置姿态，并利用上一帧4D毫米波成像雷达点云估计的位置姿态，计算出两帧4D毫米波成像雷达点云之间的相对变换；
（3）将当前获取到的视觉语义信息和4D毫米波成像雷达帧间匹配信息进行数据关联，使得二者能够在时间上相对齐；
（4）对4D毫米波成像雷达建图添加当前已经生成的视觉语义里程计约束，并和4D毫米波成像雷达里程计约束一同进行全局优化，通过构造最小二乘求解方程，求得当前车辆的位置姿态，从而使得z轴的累积误差得到消除；
所述的步骤（1）具体包括以下步骤：
（1.1）输入当前车辆前视摄像头获取到的图像数据；
（1.2）进行图像数据预处理：划定预设范围的感兴趣处理区域ROI，即以自动驾驶车辆轴中心为坐标原点，将图像帧所有像素点/>的像素点保留，其中/>为图像上的某个像素点，/>分别为图像像素坐标在x轴方向的上下边界，/>分别为图像像素坐标在y轴方向的上下边界；将感兴趣处理区域ROI提取完的所有像素点输入到预先训练好的神经网络模型/>中进行推理，其中/>为网络参数, />为参数对应的权重，得到当前语义帧对应的语义信息/>，即检测到的语义信息/>在时间/>下，在摄像头坐标下的位姿表示，具体按照如下公式进行表示：
；
其中，为提取ROI后的图像上所有像素点个数，/>为分割网络参数，为分割网络参数对应的权重,/>为图像上某个像素点，/>为网络参数作用机制操作符，/>为当前语义信息位姿；
（1.3）按照以下方式计算语义里程计约束：
；
其中，为两帧语义信息之间的位姿相对变换，/>为前一帧的语义里程计的位姿，T为进行转置处理；
所述的步骤（2）具体包括以下步骤：
（2.1）输入以车辆后轴为中心的自车坐标下的4D毫米波成像雷达点云；
（2.2）进行4D毫米波成像雷达匹配：即找到当前4D毫米波成像雷达帧的点云和上一4D毫米波成像雷达帧点云/>之间的相对变换/>，使得上一帧点云经过该相对变换后和当前帧的点云之间的对应点距离误差达到最小，则获得了最精准的匹配，匹配方式如下：
；
其中，R和t为前后4D毫米波成像雷达帧相对运动变换的旋转矩阵和平移向量；
（2.3）计算4D毫米波成像雷达里程计约束：经过4D毫米波成像雷达匹配后，得到当前帧的4D毫米波成像雷达位姿估计信息即/>，并按照以下方式计算4D毫米波成像雷达里程计的约束：
；
其中，为两帧4D毫米波成像雷达帧之间的位姿相对变换，/>为前一4D毫米波成像雷达帧的里程计位姿，/>为当前4D毫米波成像雷达帧的里程计位姿；
所述的步骤（3）具体包括以下步骤：
（3.1）以4D毫米波成像雷达匹配信息的时间为基准，寻找对应的视觉语义的匹配信息，即在视觉语义时间戳内找到与当前4D毫米波成像雷达时间戳/>时间最近的语义时间戳/>；
（3.2）利用线性插值对相应的语义信息进行插值处理，从而完成视觉语义信息和4D毫米波成像雷达帧间匹配信息之间的数据关联，具体为：
由步骤（3.1）得到语义时间戳，则通过步骤（1.2）可知语义时间戳/>和上一时刻的语义位姿信息分别为/>和/>，则按照如下公式进行插值得到4D毫米波成像雷达时间戳/>对应的语义位姿约束，并以此完成雷达语义的数据关联：
；
其中，为语义和视觉融合后的位姿约束，/>为语义关联雷达后时间戳/>对应的位姿，/>为时间戳/>上一时刻/>的位姿的逆变换，/>和/>分别为雷达当前时刻和上一时刻；
所述的步骤（4）具体包括以下步骤：
（4.1）利用前视摄像头和车辆后轴中心的自车坐标之间的外参，将当前获取到的视觉语义信息转换到自车坐标下，使得其与4D毫米波成像雷达里程计约束的坐标达到统一，转换过程如下：
；
其中，为摄像头和自车坐标之间外参，/>为经过外参/>变换后的视觉语义里程计约束；
（4.2）按照以下方式构造求解的误差方程：
（4.2.1）获取语义约束误差：
；
其中，为求解过程中得到视觉语义约束误差，/>为求解过程中得到的语义约束，是求解中间量；
（4.2.2）获取4D毫米波成像雷达匹配的误差：
；
其中，为求解过程中得到的4D毫米波成像雷达里程计约束误差，/>为求解过程中得到的4D毫米波成像雷达里程计约束，是求解中间量；
（4.2.3）获取误差方程：
；
其中，为/>的转置，/>为/>的转置，/>为视觉语义约束在优化求解中的信息矩阵，/>为4D毫米波成像雷达里程计约束在优化时的信息矩阵；
（4.2.4）求解上述误差方程，得到优化后的自车位姿估计的z轴的值：
；
其中， 为当前最终求解后的位姿，包括3×3的旋转矩阵/>和3×1的平移向量/>，从平移向量/>中取出第3维/>作为估计的z轴的值；
（4.3）将已累积的z轴误差带入上述经过优化后的位姿方程而消除，得到误差消除后的z轴值。
2.一种基于权利要求1所述的方法实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的系统，其特征在于，所述的系统包括：
视觉语义模块，用于通过搭载在车辆上的前视摄像头获取图像数据，并将获取到的图像数据输入到语义分割网络中进行在线推理，以获取当前4D毫米波成像雷达帧的语义信息；
radar建图模块，用于利用4D毫米波成像雷达点云进行帧间匹配，从而估计出当前车辆在全局参考坐标系下的位置姿态；
数据关联模块，分别与所述的视觉语义模块和radar建图模块相连接，用于将获取到的视觉语义信息和4D毫米波成像雷达帧间匹配信息进行数据关联，使得二者能够在时间上相对齐；以及
radar语义融合消除z轴累积误差模块, 分别与所述的视觉语义模块和radar建图模块相连接，用于将生成的视觉语义里程计约束和4D毫米波成像雷达里程计约束共同进行全局优化处理，并通过构造最小二乘求解方程，获取当前车辆的位置姿态，从而使得z轴的累积误差得到消除。
3.一种实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的装置，其特征在于，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
4.一种实现融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
5.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1所述的融合视觉语义消除4D毫米波成像雷达建图z轴累积误差的方法的步骤。
CN202410033363.9A 2024-01-10 2024-01-10 融合视觉语义消除radar建图z轴累积误差的方法 Active CN117541655B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410033363.9A CN117541655B (zh) 2024-01-10 2024-01-10 融合视觉语义消除radar建图z轴累积误差的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410033363.9A CN117541655B (zh) 2024-01-10 2024-01-10 融合视觉语义消除radar建图z轴累积误差的方法

Publications (2)

Publication Number Publication Date
CN117541655A CN117541655A (zh) 2024-02-09
CN117541655B true CN117541655B (zh) 2024-03-26

Family

ID=89788529

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410033363.9A Active CN117541655B (zh) 2024-01-10 2024-01-10 融合视觉语义消除radar建图z轴累积误差的方法

Country Status (1)

Country Link
CN (1) CN117541655B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117808882B (zh) * 2024-02-29 2024-05-17 上海几何伙伴智能驾驶有限公司 退化场景下基于多传感器融合的slam漂移检测与补偿方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法
CN110570449A (zh) * 2019-09-16 2019-12-13 电子科技大学 一种基于毫米波雷达与视觉slam的定位与建图方法
CN111367318A (zh) * 2020-03-31 2020-07-03 华东理工大学 一种基于视觉语义信息的动态障碍环境导航方法和装置
CN115657076A (zh) * 2022-08-08 2023-01-31 合肥工业大学 一种基于固态激光雷达语义融合的定位方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11618480B2 (en) * 2020-11-18 2023-04-04 Aptiv Technologies Limited Kurtosis based pruning for sensor-fusion systems
US20230138762A1 (en) * 2021-10-28 2023-05-04 MFTB Holdco, Inc. Automated Building Floor Plan Generation Using Visual Data Of Multiple Building Images

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109211241A (zh) * 2018-09-08 2019-01-15 天津大学 基于视觉slam的无人机自主定位方法
CN110570449A (zh) * 2019-09-16 2019-12-13 电子科技大学 一种基于毫米波雷达与视觉slam的定位与建图方法
CN111367318A (zh) * 2020-03-31 2020-07-03 华东理工大学 一种基于视觉语义信息的动态障碍环境导航方法和装置
CN115657076A (zh) * 2022-08-08 2023-01-31 合肥工业大学 一种基于固态激光雷达语义融合的定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于激光雷达SLAM和视觉语义信息的半动态场景融合定位方法研究;朱世凡;《中国优秀硕士学位论文全文数据库信息科技辑》;20210715(第7期);I136-358 *

Also Published As

Publication number Publication date
CN117541655A (zh) 2024-02-09

Similar Documents

Publication Publication Date Title
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
US20220292711A1 (en) Pose estimation method and device, related equipment and storage medium
CN117541655B (zh) 融合视觉语义消除radar建图z轴累积误差的方法
CN109631911B (zh) 一种基于深度学习目标识别算法的卫星姿态转动信息确定方法
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN113240813B (zh) 三维点云信息确定方法及装置
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN113220818B (zh) 一种停车场自动建图与高精度定位方法
CN113763548B (zh) 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN113624223B (zh) 一种室内停车场地图构建方法及装置
CN116228969A (zh) 一种基于多传感器融合的林区定位与三维重建方法及系统
CN117593650B (zh) 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN116359873A (zh) 结合鱼眼相机实现车端4d毫米波雷达slam处理的方法、装置、处理器及其存储介质
CN116907469A (zh) 多模态数据联合优化的同步定位及建图方法及系统
CN116258744A (zh) 基于可见光、红外和激光雷达数据融合的目标跟踪方法
CN113240750A (zh) 三维空间信息测算方法及装置
CN113706633A (zh) 一种目标对象的三维信息确定方法和装置
CN117870716A (zh) 地图兴趣点的显示方法、装置、电子设备及存储介质
CN109816726B (zh) 一种基于深度滤波器的视觉里程计地图更新方法和系统
CN111191596A (zh) 一种封闭区域制图方法、装置及存储介质
CN114332187B (zh) 单目目标测距方法及装置
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN116934829B (zh) 无人机目标深度估计的方法、装置、存储介质及电子设备
CN118608435B (zh) 点云的去畸变方法、装置、电子设备及可读存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant