CN115511970B - 一种面向自主泊车的视觉定位方法 - Google Patents

一种面向自主泊车的视觉定位方法 Download PDF

Info

Publication number
CN115511970B
CN115511970B CN202211469663.9A CN202211469663A CN115511970B CN 115511970 B CN115511970 B CN 115511970B CN 202211469663 A CN202211469663 A CN 202211469663A CN 115511970 B CN115511970 B CN 115511970B
Authority
CN
China
Prior art keywords
map
point
current frame
frame image
points
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
CN202211469663.9A
Other languages
English (en)
Other versions
CN115511970A (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.)
BYD Auto Co Ltd
Shenzhen Automotive Research Institute of Beijing University of Technology
Original Assignee
BYD Auto Co Ltd
Shenzhen Automotive Research Institute of Beijing University of Technology
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 BYD Auto Co Ltd, Shenzhen Automotive Research Institute of Beijing University of Technology filed Critical BYD Auto Co Ltd
Priority to CN202211469663.9A priority Critical patent/CN115511970B/zh
Publication of CN115511970A publication Critical patent/CN115511970A/zh
Application granted granted Critical
Publication of CN115511970B publication Critical patent/CN115511970B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/269—Analysis of motion using gradient-based methods
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

一种面向自主泊车的视觉定位方法，涉及自主泊车领域。对于当前帧图像中任意一个特征点：获取任意一个特征点在空间中对应地图点的状态概率；根据所述地图点的状态概率确定所述地图点在当前帧图像中对应的特征点的初始状态概率；获取所述地图点在当前帧图像中对应的特征点的观测概率；根据所述初始状态概率和所述观测概率确定所述地图点在当前帧图像中对应的特征点的状态概率；根据所述地图点在当前帧图像中对应的特征点的状态概率，更新所述地图点的状态概率，根据所述地图点的状态概率确定所述地图点的地图点类型；获取静态点的地图点构建环境地图，用以视觉定位。

Description

一种面向自主泊车的视觉定位方法
技术领域
本发明涉及自主泊车领域，具体涉及一种面向自主泊车的视觉定位方法。
背景技术
自主泊车系统作为自动驾驶中重要的组成部分，能够让智能车辆在复杂、未知的环境中准确停车。视觉SLAM（Simultaneous Localization And Mapping，即时定位与地图构建，主要作用是让机器人在未知环境中，于运动过程中建立环境的模型，同时估计自己的运动。）定位技术作为自主泊车系统中不可缺少的一部分，可以为智能车辆在运动时提供自车位姿并且构建周围环境的地图。
传统的视觉SLAM方法基于运行场景为静态这一假设，然而，当车辆在自主泊车时，由于周围一般存在动态物体，这些运动特征点如果参与计算，那么原先的假设就不再成立。针对这一问题，一些成熟的SLAM方案通过随机采样一致性算法处理异常数据，但当图像中动态物体占比较大时，算法无法收敛。
随着近些年深度学习在图像领域的飞速发展，利用具有先验知识的语义信息来辅助求取相机自身运动模型更加简洁且更为鲁棒，将语义信息添加到动态环境下的SLAM系统，利用先验的知识可以初步判断和分割环境中运动的物体，如室内的行人、街道上的车辆、各种各样的动物，先将图像中这些目标区域去除再进行位姿估计会可靠许多，不过当环境中某些物体类别未被训练集覆盖而不能正确检测时，系统就失去了处理动态物体的能力。
采用语义信息的SLAM系统能够简单准确地分割出动态区域，但是实时性较差且面对未知环境时无法保证运行鲁棒性。采用几何约束的SLAM系统能够较快（传统稠密光流方法除外）区分出动态物体，但是模型设计较为复杂且实际运行时，无法准确获得动态区域。
发明内容
本发明主要解决的技术问题是：车辆自主泊车过程中，由于动态物体干扰，无法准确获得车辆位姿。
根据第一方面，一种实施例中提供一种面向自主泊车的视觉定位方法，包括：
对于当前帧图像中任意一个特征点：
获取所述任意一个特征点在空间中对应地图点的状态概率；
根据所述任意一个特征点在空间中对应地图点的状态概率确定所述地图点在当前帧图像中对应的特征点的初始状态概率；
获取所述地图点在当前帧图像中对应的特征点的观测概率；
根据所述地图点在当前帧图像中对应特征点的初始状态概率和所述地图点在当前帧图像中对应的特征点的观测概率，确定所述地图点在当前帧图像中对应的特征点的状态概率；
根据所述地图点在当前帧图像中对应的特征点的状态概率，更新所述地图点的状态概率，根据所述地图点的状态概率确定所述地图点的地图点类型；
获取所述地图点类型为静态点的地图点构建环境地图，用以视觉定位。
一实施例中，所述获取所述地图点在当前帧图像中对应的特征点的观测概率，包括：
获取拍摄当前帧图像相机的位姿和当前帧图像的稠密光流图；
根据所述当前帧图像相机的位姿对所述当前帧图像的稠密光流图进行分割，以确定当前帧图像中的动态区域；
根据所述动态区域确定所述地图点在当前帧图像中对应的特征点的第一观测概率；
根据目标检测网络获取当前帧图像中各物体检测框的位置和大小；根据所述各物体检测框的位置和大小确定所述地图点在当前帧图像中对应的特征点的第二观测概率；
根据所述第一观测概率和所述第二观测概率确定所述地图点在当前帧图像中对应的特征点的观测概率。
一实施例中，所述根据所述地图点的状态概率确定所述地图点的地图点类型，包括:
当所述地图点的状态概率小于第一设定阈值，则所述地图点为静态点；当所述地图点的状态概率大于或等于所述第一设定阈值，则所述地图点为动态点。
一实施例中，所述获取所述地图点类型为静态点的地图点构建环境地图，用以视觉定位，包括：
获取所述地图点类型为静态点的地图点构建环境地图；
通过代价函数对所述环境地图中当前帧图像中的车辆位姿进行优化，用以视觉定位。
一实施例中，所述通过代价函数对所述环境地图中当前帧图像中的车辆位姿进行优化，用以视觉定位，包括：
通过代价函数对重投影误差进行优化，使得所述重投影误差小于设定误差范围，以对所述环境地图中当前帧图像中的车辆位姿进行优化；所述重投影误差为所述地图点类型为静态点的地图点在当前帧图像中对应的特征点减去所述地图点类型为静态点的地图点在当前帧图像的投影。
一实施例中，所述通过代价函数对重投影误差进行优化，包括：
对所述代价函数增设参数，根据所述地图点的地图点类型对所述参数进行赋值，以消除动态点对代价函数优化的影响。
一实施例中，所述根据所述地图点的地图点类型对所述参数进行赋值，包括：
当所述地图点的状态概率小于第二设定阈值，则所述地图点为高置信度静态点，所述高置信度静态点的参数为1；当所述地图点的状态概率大于第二设定阈值且小于第一设定阈值，则所述地图点为低置信度静态点，所述低置信度静态点的参数为1减去所述地图点的状态概率；所述动态点的参数为0。
一实施例中，所述根据所述地图点在当前帧图像中对应特征点的初始状态概率和所述地图点在当前帧图像中对应的特征点的观测概率，确定所述地图点在当前帧图像中对应的特征点的状态概率，包括：
将所述地图点在当前帧图像中对应特征点的初始状态概率，和所述地图点在当前帧图像中对应的特征点的观测概率，输入至贝叶斯滤波模型，以得到所述地图点在当前帧图像中对应的特征点的状态概率。
一实施例中，所述稠密光流图包括PWC-Net光流估计网络生成的稠密光流图；所述目标检测网络包括Yolov4-tiny网络。
根据第二方面，一种实施例中提供一种计算机可读存储介质，所述介质上存储有程序，所述程序能够被处理器执行以实现如上述的方法。
根据上述实施例的一种面向自主泊车的视觉定位方法和计算机可读存储介质，获取特征点在空间中对应地图点的状态概率，根据地图点的状态概率确定该地图点对应特征点的初始状态概率，获取该地图点对应特征点的观测概率，根据该地图点对应特征点的初始状态概率和观测概率确定该地图点对应特征点的状态概率，根据该状态概率更新对应地图点的状态概率。根据地图点的状态概率确定地图点的地图点类型，根据地图点类型为静态点的地图点构建环境地图，在环境地图中进行定位。本申请对于动态点的确定是根据相邻两帧之间同一像素位置变化获得，因此即使在未知环境下，周围有无法识别语义的动态物体时，仍能保证较高的定位精度和建图效果。
附图说明
图1为目标检测网络产生的检测框示意图；
图2为一种实施例中面向自主泊车的视觉定位方法的流程图一；
图3为一种实施例中部分物体动态概率划分图；
图4为一种实施例中面向自主泊车的视觉定位方法的流程图二；
图5为现有技术的ORBSLAM2流程图；
图6为一种实施例的ORBSLAM2流程图；
图7为一种实施例的环境原始图；
图8为一种实施例中PWC-NET光流估计网络的效果图；
图9为一种实施例中光流图分割后效果图；
图10为一种实施例中Yolov4-tiny目标检测效果图；
图11为一种实施例中面向自主泊车的视觉定位方法的流程图三；
图12为一种实施例中稠密点云创建过程流程图。
具体实施方式
下面通过具体实施方式结合附图对本发明作进一步详细说明。其中不同实施方式中类似元件采用了相关联的类似的元件标号。在以下的实施方式中，很多细节描述是为了使得本申请能被更好的理解。然而，本领域技术人员可以毫不费力的认识到，其中部分特征在不同情况下是可以省略的，或者可以由其他元件、材料、方法所替代。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。
另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。因此，说明书和附图中的各种顺序只是为了清楚描述某一个实施例，并不意味着是必须的顺序，除非另有说明其中某个顺序是必须遵循的。
本文中为部件所编序号本身，例如“第一”、“第二”等，仅用于区分所描述的对象，不具有任何顺序或技术含义。而本申请所说“连接”、“联接”，如无特别说明，均包括直接和间接连接（联接）。
针对自动泊车场景，公开了一种基于实例分割的鲁棒性位姿估计方法，该方法是在实例分割区域结合光流跟踪检测出图像序列中的动态区域并剔除。该算法首先使用实例分割结合目标跟踪算法，锁定潜在的动态物体区域；然后采用光流法在分割区域进行动态点检测，获得分割区域物体的动态概率；最后用动态概率结合语义先验信息判断动态区域。
针对自动泊车场景，还公开了一种基于光流和语义分割的视觉SLAM方法，该方法使用语义分割网络对输入的图像信息进行分割，得到静态区域且预测动态区域；采用稀疏光流法对静态区域和预测的动态区域进行特征追踪；对输入的图像信息中的特征点类型进行判定，并移除动态特征点；将剔除动态特征点的集合作为追踪数据，输入至ORB-SLAM中进行处理，输出位姿结果。
对于自动泊车的场景采用基于语义信息的运动目标滤除方法时，尽管同稀疏光流结合使得动态物体的判断更为准确，但由于目标识别算法能识别的动态物体依赖于训练集中提前确定的语义信息。因此，当环境中出现未知的动态物体时，该物体将无法删除，降低SLAM系统定位精度并且导致构建的地图产生较大畸变。
因此，本申请通过使用光流估计网络估计相连两帧之间光流的变化生成稠密光流图，判断周围环境中是否存在动态物体，之后通过对光流图进行分割，就可以确定图像中动态物体的区域。
请参考图1，传统采用目标检测算法识别图像中动态物体区域并且去除落入其中动态点的方法，可能会由于动态物体位姿如图1所示，导致大量静态点被删除，不利于SLAM系统跟踪的稳定性。同样的，当采用目标分割算法对动态物体进行分割去除时，动态物体若为非刚体时（比如人），系统可能直接由于物体部分区域的运动就将整个物体上其他静止部分的静止点删除，降低了系统定位的精度并且容易导致系统位姿跟踪丢失。
因此，本申请采用目标检测和稠密光流估计相结合的方法，采用贝叶斯滤波模型，估计图像中特征点的状态概率，最后删除关键帧上概率大于一定阈值的特征点，而其余的特征点则分别根据其为动态点的概率赋予不同的权重再进行BA（Bundle Adjustment）优化，从而提高SLAM系统的精确性和稳定性。BA优化是指使用三维点进行投影找出最合适的相机位姿，使得投影得到的二维坐标与真实观测到的二维坐标尽可能一致。
大部分的语义分割算法由于其模型的复杂将其和SLAM相结合后，系统运行耗时较长。同样的，传统的稠密光流算法计算耗时长。基于语义分割和稠密光流的SLAM系统在实际环境中通常难以实时运行。因此，本申请采用轻量化的目标检测网络以及稠密光流估计网络，能够在不影响SLAM系统实时运行情况下快速获取动态物体的区域并且将其删除。
请参考图2，本申请实施例提供一种面向自主泊车的视觉定位方法，包括以下步骤：
对于当前帧图像中任意一个特征点：
步骤S100：获取任意一个特征点在空间中对应地图点的状态概率。
步骤S200：根据任意一个特征点在空间中对应地图点的状态概率确定地图点在当前帧图像中对应的特征点的初始状态概率。
请参考图3为物体运动的概率划分图，根据物体运动的概率划分图预设地图点的概率，根据地图点的状态概率确定对应地图点的状态概率。
一些实施例中，通过如下公式计算地图点在当前帧图像中对应的特征点的初始状态概率：
Figure DEST_PATH_IMAGE002
其中，
Figure 95776DEST_PATH_IMAGE003
为地图点/>
Figure DEST_PATH_IMAGE004
在当前帧图像中对应特征点的初始状态概率；
Figure 622703DEST_PATH_IMAGE005
为状态转移概率；/>
Figure DEST_PATH_IMAGE006
为地图点/>
Figure 746648DEST_PATH_IMAGE007
在在空间中对应地图点的状态概率；/>
Figure DEST_PATH_IMAGE008
为t时刻第i个地图点。
步骤S300：获取地图点在当前帧图像中对应的特征点的观测概率。
请参考图4，一些实施例中，步骤S300获取地图点在当前帧图像中对应的特征点的观测概率，还包括以下步骤：
步骤S310：获取拍摄当前帧图像相机的位姿和当前帧图像的稠密光流图。
将当前帧图像输入进ORBSLAM2（ORBSLAM2是一个支持单目、双目、RGB-D相机的开源SLAM框架，能够实时计算相机的位姿并对周围环境稀疏三维重建）系统跟踪线程中，获得当前帧相机的位姿。请参考图5，ORBSLAM2系统包含四个线程：跟踪线程、局部建图线程、回环检测线程和在回环检测后会启动的全部BA优化线程。请参考图6为本申请改进后的ORBSLAM2流程图，跟踪线程主要根据输入的图像帧粗略估计当前相机的位姿，包括相机的平移和旋转；局部建图线程是对跟踪线程中选择出的关键帧进一步优化位姿和物体的特征点位置；通常位姿的估计由于每一帧的递推会产生累积误差，因此可以通过回环检测线程来检测相机是否到达曾到过的位置，当检测到回环后，通过回环矫正，可以减小误差；最后通过全局BA优化获得最优的相机位姿和特征点位置。
将上一帧图像和当前帧图像输入到光流估计模型中进行光流估计，获得当前帧的稠密光流图。
一些实施例中，光流估计模型为PWC-NET光流估计网络，PWC-NET光流估计网络是一个光流模型，能够根据输入的连续两帧图像，输出表征图像之间每个像素点运动轨迹的光流图。
请参考图7环境原始图，请参考图8为对应的PWC-NET光流估计网络的效果图，光流估计是计算机视觉运动估计中的基础问题，描述了图像中像素的二维运动。PWC-NET光流估计网络在Sintel数据集上能以35 fps的速度运行，Sintel数据集为常用的双目立体匹配数据集，其中部分数据集可以用于光流估计。在该PWC-NET光流估计网络中输入相邻两帧图像，输出得到当前帧的稠密光流图。在稠密光流图的每个像素中都存储了两个值，用于表示上一帧图像和当前帧图像之间每个像素的位移，如下式所示：
Figure 751644DEST_PATH_IMAGE009
/>
Figure DEST_PATH_IMAGE010
为二维向量，表示上一帧和当前帧图像像素在x轴和y轴方向上的偏移；f x 表示上一帧图像的像素和当前帧图像像素在x轴方向上的偏移；f y 表示上一帧图像的像素和当前帧图像像素在y轴方向上的偏移。
对于当前帧图像中的对应像素坐标可通过如下公式获得：
Figure 172874DEST_PATH_IMAGE011
x 2表示当前帧图像的像素x轴方向坐标值；y 2表示当前帧图像的像素y轴方向坐标值；x 1表示前一帧图像的像素x轴方向坐标值；y 1表示前一帧图像的像素y轴方向坐标值；上标T表示向量转置。显示在光流图上，颜色深度表示像素点位移方向，明暗度表示像素点位移距离大小，基于该原理能将运动物体区分出来。
步骤S320：根据当前帧图像相机的位姿对当前帧图像的稠密光流图进行分割，以确定当前帧图像中的动态区域。
如图8中同一物体的各个像素点的光流空间分布是连续的，因此，在光流出现较大变化的位置可以被认为是不同物体的边界，或是同一物体内不同部分的边界。一些实施例中，采用边缘分割将稠密光流图根据光流出现较大变化的位置分为若干个区域。通过如下公式获得每个区域内的每个像素运动的残差
Figure DEST_PATH_IMAGE012
：
Figure 588812DEST_PATH_IMAGE013
Figure DEST_PATH_IMAGE014
表示前一帧图像和当前帧图像像素在x轴和y轴方向上的偏移；/>
Figure 731211DEST_PATH_IMAGE015
表示当前帧相机的位姿/>
Figure DEST_PATH_IMAGE016
对应在稠密光流图上的偏移。
由于静态区域不会移动，因此静态区域上一帧图像和当前帧图像像素在稠密光流图上的偏移和当前帧相机的位姿对应在稠密光流图上的偏移相同，相减后的残差值很小。进而可以通过设置残差的阈值将动态区域分割出来，如下式：
Figure 274319DEST_PATH_IMAGE017
δ表示每个区域内的每个像素运动的残差的阈值；N表示稠密光流图中像素点总数。
请参考图9为动态区域分割后的效果图，其中白色区域为动态区域。
步骤S330：根据动态区域确定图点在当前帧图像中对应的特征点的第一观测概率。
步骤S340：根据目标检测网络获取当前帧图像中各物体检测框的位置和大小；根据各物体检测框的位置和大小确定地图点在当前帧图像中对应的特征点的第二观测概率。
一些实施例中采用的目标检测网络为Yolov4-tiny网络模型，Yolov4-tiny为一个轻量化的目标检测网络模型，在COCO数据集（一个可以用于图像检测，语义分割和图像标题生成的大规模数据集）上能以37fps速度运行，能够快速输出带有检测结果的各物体对应的检测框位置以及坐标。请参考图10，为Yolov4-tiny目标检测效果图。
步骤S350：根据第一观测概率和第二观测概率确定地图点在当前帧图像中对应的特征点的观测概率。
步骤S400：根据地图点在当前帧图像中对应特征点的初始状态概率和地图点在当前帧图像中对应的特征点的观测概率，确定地图点在当前帧图像中对应的特征点的状态概率。
一些实施例中，将地图点在当前帧图像中对应特征点的初始状态概率，和地图点在当前帧图像中对应的特征点的观测概率，输入至贝叶斯滤波模型，以得到地图点在当前帧图像中对应的特征点的状态概率。
采用的光流分割确定的第一观测概率和采用的目标检测确定的第二观测概率确定的观测概率会存在误差，如果仅以一帧的观测值来判断特征点的动静态是不合理的，因此为了检测异常值，一些实施例中采用贝叶斯滤波模型，结合多帧图像的光流分割结果和目标检测结果来判断当前帧特征点的动静态，并将特征点的动静态通过概率形式表示。因此，通过如下公示计算得到地图点在当前帧图像中对应的特征点的状态概率：
Figure 756247DEST_PATH_IMAGE019
其中，
Figure DEST_PATH_IMAGE020
为地图点/>
Figure 343086DEST_PATH_IMAGE021
在当前帧图像中对应特征点的初始状态概率；
Figure DEST_PATH_IMAGE022
为地图点/>
Figure 444553DEST_PATH_IMAGE023
在当前帧图像中对应的特征点的观测概率；/>
Figure DEST_PATH_IMAGE024
为归一化常数；
Figure 525773DEST_PATH_IMAGE025
为地图点/>
Figure 111475DEST_PATH_IMAGE008
在当前帧图像中对应的特征点的状态概率。
步骤S500：根据地图点在当前帧图像中对应的特征点的状态概率，更新地图点的状态概率，根据地图点的状态概率确定地图点的地图点类型。
一些实施例中，步骤S500中根据地图点的状态概率确定地图点的地图点类型的确定方式为：当地图点的状态概率小于第一设定阈值，则地图点为静态点；当地图点的状态概率大于或等于第一设定阈值，则地图点为动态点。
步骤S600：获取地图点类型为静态点的地图点构建环境地图，用以视觉定位。
请参考图11，一些实施例中，步骤S600获取地图点类型为静态点的地图点构建环境地图，用以视觉定位，还包括以下步骤：
步骤S610：获取地图点类型为静态点的地图点构建环境地图。
步骤S620：通过代价函数对环境地图中当前帧图像中的车辆位姿进行优化，用以视觉定位位。
一些实施例中，通过代价函数对重投影误差进行优化，使得重投影误差小于设定误差范围，以对环境地图中当前帧图像中的车辆位姿进行优化；重投影误差为地图点类型为静态点的地图点在当前帧图像中对应的特征点减去地图点类型为静态点的地图点在当前帧图像的投影。
若地图点类型为静态点的地图点和相机位姿计算准确，那么将地图点类型为静态点的地图点投影到当前帧图像上时，该地图点在当前帧上的投影应当与该地图点在当前帧图像中对应的特征点重合，因此通过最小化重投影误差来优化地图点和相机位姿
Figure DEST_PATH_IMAGE026
，一些实施例中，该相机位姿即为车辆位姿。最小化重投影误差通过如下公式计算得到：
Figure DEST_PATH_IMAGE028
/>
其中，
Figure 432997DEST_PATH_IMAGE029
为重投影误差；x t，j 为t时刻地图点在当前帧图像中对应的第j个特征点；/>
Figure DEST_PATH_IMAGE030
为t-1时刻第j个静态地图点；/>
Figure 533677DEST_PATH_IMAGE031
为世界坐标系下t时刻相机位姿；/>
Figure DEST_PATH_IMAGE032
为投影函数，通过旋转和平移组成。
一些实施例中，对代价函数增设参数，根据地图点的地图点类型对参数进行赋值，以消除动态点对代价函数优化的影响。当地图点的状态概率小于第二设定阈值，则地图点为高置信度静态点，高置信度静态点的参数为1；当地图点的状态概率大于第二设定阈值且小于第一设定阈值，则地图点为低置信度静态点，低置信度静态点的参数为1减去地图点的状态概率；动态点的参数为0。
一些实施例中，第一设定阈值为
Figure 150079DEST_PATH_IMAGE033
，第二设定阈值为/>
Figure DEST_PATH_IMAGE034
，则高置信度静态点、低置信度静态点和动态点通过如下公式表示：
Figure DEST_PATH_IMAGE036
其中，
Figure DEST_PATH_IMAGE037
表示地图点/>
Figure 747544DEST_PATH_IMAGE023
的状态概率。
一些实施例中，剔除动态区域的方式是对代价函数增设参数W j ，通过对不同地图点对参数W j 赋予不同的值，以消除动态点对于优化的影响。
Figure DEST_PATH_IMAGE039
其中，
Figure DEST_PATH_IMAGE040
表示地图点/>
Figure DEST_PATH_IMAGE041
的状态概率。
一些实施例中，增设参数后的代价函数通过如下公式表示：
Figure DEST_PATH_IMAGE042
其中，
Figure DEST_PATH_IMAGE043
为协方差矩阵。
请参考图12，利用双目相机左右两帧图像进行稠密深度估计，构建稠密地图需要已知每帧图像的相机位姿和稠密深度，对于动态场景来说还需要知道动态物体在图像中的位置，从而在建图过程中将动态区域删除。由于本申请采用视觉传感器为双目相机，因此场景中的深度信息可以直接通过双目相机获得的左右两帧图像计算得到深度估计这一步骤，图像中每一帧的相机位姿为已知，动态区域通过光流分割可以获得排除动态点的这一步骤。于是，根据上述信息，对于一幅图像中的像素，首先根据动态区域来判断其运动属性，如果该点为动态点，则将其在建图过程中排除；若该点属于静态点，则根据相机位姿估计结果，深度信息，将其反投影至世界坐标系下。最终将每一帧图像获得的稠密点云拼接以及降采样处理后，便能获得车辆泊车时周围的静态环境稠密地图，用于后续的导航。
本申请对于动态点的确定并不是通过语义信息而是根据相邻帧间同一像素位置变化获得，因此即使在未知环境下，周围有无法识别语义的动态物体时，仍能保证较高的定位精度和建图效果。并且，本申请采用轻量化光流估计网络模型与目标检测模型，因此能够进行实时的定位。本申请还采用目标检测和稠密光流估计相结合的方法，采用贝叶斯滤波模型，估计图像中特征点的状态概率，最后只删除关键帧上概率大于一定阈值的地图点，而其余的地图点则分别根据其为动态点的概率赋予不同的权重再进行优化，从而提高SLAM系统的运行稳定性。
本领域技术人员可以理解，上述实施方式中各种方法的全部或部分功能可以通过硬件的方式实现，也可以通过计算机程序的方式实现。当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器、随机存储器、磁盘、光盘、硬盘等，通过计算机执行该程序以实现上述功能。例如，将程序存储在设备的存储器中，当通过处理器执行存储器中程序，即可实现上述全部或部分功能。另外，当上述实施方式中全部或部分功能通过计算机程序的方式实现时，该程序也可以存储在服务器、另一计算机、磁盘、光盘、闪存盘或移动硬盘等存储介质中，通过下载或复制保存到本地设备的存储器中，或对本地设备的系统进行版本更新，当通过处理器执行存储器中的程序时，即可实现上述实施方式中全部或部分功能。
以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (9)

1.一种面向自主泊车的视觉定位方法, 其特征在于，包括：
对于当前帧图像中任意一个特征点：
获取所述任意一个特征点在空间中对应地图点的状态概率；
根据所述任意一个特征点在空间中对应地图点的状态概率确定所述地图点在当前帧图像中对应的特征点的初始状态概率；
获取所述地图点在当前帧图像中对应的特征点的观测概率，
其中，获取拍摄当前帧图像相机的位姿和当前帧图像的稠密光流图；
根据所述当前帧图像相机的位姿对所述当前帧图像的稠密光流图进行分割，以确定当前帧图像中的动态区域；
根据所述动态区域确定所述地图点在当前帧图像中对应的特征点的第一观测概率；
根据目标检测网络获取当前帧图像中各物体检测框的位置和大小；根据所述各物体检测框的位置和大小确定所述地图点在当前帧图像中对应的特征点的第二观测概率；
根据所述第一观测概率和所述第二观测概率确定所述地图点在当前帧图像中对应的特征点的观测概率；
根据所述地图点在当前帧图像中对应特征点的初始状态概率和所述地图点在当前帧图像中对应的特征点的观测概率，确定所述地图点在当前帧图像中对应的特征点的状态概率；
根据所述地图点在当前帧图像中对应的特征点的状态概率，更新所述地图点的状态概率，根据所述地图点的状态概率确定所述地图点的地图点类型；
获取所述地图点类型为静态点的地图点构建环境地图，用以视觉定位。
2.如权利要求1所述的面向自主泊车的视觉定位方法，其特征在于，所述根据所述地图点的状态概率确定所述地图点的地图点类型，包括：
当所述地图点的状态概率小于第一设定阈值，则所述地图点为静态点；当所述地图点的状态概率大于或等于所述第一设定阈值，则所述地图点为动态点。
3.如权利要求1所述的面向自主泊车的视觉定位方法，其特征在于，所述获取所述地图点类型为静态点的地图点构建环境地图，用以视觉定位，包括：
获取所述地图点类型为静态点的地图点构建环境地图；
通过代价函数对所述环境地图中当前帧图像中的车辆位姿进行优化，用以视觉定位。
4.如权利要求3所述的面向自主泊车的视觉定位方法，其特征在于，所述通过代价函数对所述环境地图中当前帧图像中的车辆位姿进行优化，用以视觉定位，包括：
通过代价函数对重投影误差进行优化，使得所述重投影误差小于设定误差范围，以对所述环境地图中当前帧图像中的车辆位姿进行优化；所述重投影误差为所述地图点类型为静态点的地图点在当前帧图像中对应的特征点减去所述地图点类型为静态点的地图点在当前帧图像的投影。
5.如权利要求4所述的面向自主泊车的视觉定位方法，其特征在于，所述通过代价函数对重投影误差进行优化，包括：
对所述代价函数增设参数，根据所述地图点的地图点类型对所述参数进行赋值，以消除动态点对代价函数优化的影响。
6.如权利要求5所述的面向自主泊车的视觉定位方法，其特征在于，所述根据所述地图点的地图点类型对所述参数进行赋值，包括：
当所述地图点的状态概率小于第二设定阈值，则所述地图点为高置信度静态点，所述高置信度静态点的参数为1；当所述地图点的状态概率大于第二设定阈值且小于第一设定阈值，则所述地图点为低置信度静态点，所述低置信度静态点的参数为1减去所述地图点的状态概率；所述动态点的参数为0。
7.如权利要求1所述的面向自主泊车的视觉定位方法，其特征在于，所述根据所述地图点在当前帧图像中对应特征点的初始状态概率和所述地图点在当前帧图像中对应的特征点的观测概率，确定所述地图点在当前帧图像中对应的特征点的状态概率，包括：
将所述地图点在当前帧图像中对应特征点的初始状态概率，和所述地图点在当前帧图像中对应的特征点的观测概率，输入至贝叶斯滤波模型，以得到所述地图点在当前帧图像中对应的特征点的状态概率。
8.如权利要求1所述的面向自主泊车的视觉定位方法，其特征在于，所述稠密光流图包括PWC-Net光流估计网络生成的稠密光流图；所述目标检测网络包括Yolov4-tiny网络。
9.一种计算机可读存储介质，其特征在于，所述介质上存储有程序，所述程序能够被处理器执行以实现如权利要求1-8中任一项所述的方法。
CN202211469663.9A 2022-11-23 2022-11-23 一种面向自主泊车的视觉定位方法 Active CN115511970B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211469663.9A CN115511970B (zh) 2022-11-23 2022-11-23 一种面向自主泊车的视觉定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211469663.9A CN115511970B (zh) 2022-11-23 2022-11-23 一种面向自主泊车的视觉定位方法

Publications (2)

Publication Number Publication Date
CN115511970A CN115511970A (zh) 2022-12-23
CN115511970B true CN115511970B (zh) 2023-03-24

Family

ID=84513543

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211469663.9A Active CN115511970B (zh) 2022-11-23 2022-11-23 一种面向自主泊车的视觉定位方法

Country Status (1)

Country Link
CN (1) CN115511970B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117115786B (zh) * 2023-10-23 2024-01-26 青岛哈尔滨工程大学创新发展中心 一种联合分割跟踪的深度估计模型训练方法及使用方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111882611A (zh) * 2020-07-17 2020-11-03 北京三快在线科技有限公司 一种地图构建方法及装置
CN113362358A (zh) * 2021-06-02 2021-09-07 东南大学 一种动态场景下基于实例分割的鲁棒性位姿估计方法
WO2022041596A1 (zh) * 2020-08-31 2022-03-03 同济人工智能研究院(苏州)有限公司 一种适用于室内动态环境的视觉slam方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112884835A (zh) * 2020-09-17 2021-06-01 中国人民解放军陆军工程大学 一种基于深度学习之目标检测的视觉slam方法
CN112465858A (zh) * 2020-12-10 2021-03-09 武汉工程大学 基于概率网格滤波的语义视觉slam方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111882611A (zh) * 2020-07-17 2020-11-03 北京三快在线科技有限公司 一种地图构建方法及装置
WO2022041596A1 (zh) * 2020-08-31 2022-03-03 同济人工智能研究院(苏州)有限公司 一种适用于室内动态环境的视觉slam方法
CN113362358A (zh) * 2021-06-02 2021-09-07 东南大学 一种动态场景下基于实例分割的鲁棒性位姿估计方法

Also Published As

Publication number Publication date
CN115511970A (zh) 2022-12-23

Similar Documents

Publication Publication Date Title
CN108955718B (zh) 一种视觉里程计及其定位方法、机器人以及存储介质
WO2022088982A1 (zh) 三维场景构建方法、装置、系统和存储介质
CN112132897A (zh) 一种基于深度学习之语义分割的视觉slam方法
CN112991413A (zh) 自监督深度估测方法和系统
US20180308240A1 (en) Method for estimating the speed of movement of a camera
US11830216B2 (en) Information processing apparatus, information processing method, and storage medium
EP1612731B1 (en) Computer modeling of physical scenes
WO2019057179A1 (zh) 一种基于点线特征的视觉slam方法和装置
CN108648194B (zh) 基于cad模型三维目标识别分割和位姿测量方法及装置
Berrio et al. Camera-LIDAR integration: Probabilistic sensor fusion for semantic mapping
JP7209115B2 (ja) 複数の相対的に接近する様に動いているリジッドなオブジェクトの検出、３ｄ再現および追跡
Munoz-Banon et al. Targetless camera-lidar calibration in unstructured environments
CN110738667A (zh) 一种基于动态场景的rgb-d slam方法和系统
CN115511970B (zh) 一种面向自主泊车的视觉定位方法
US20230019499A1 (en) Image processing system and method
CN111161334A (zh) 一种基于深度学习的语义地图构建方法
CN113822996B (zh) 机器人的位姿估计方法及装置、电子设备、存储介质
Saleem et al. Neural network-based recent research developments in SLAM for autonomous ground vehicles: A review
Fehr et al. Reshaping our model of the world over time
CN113052907A (zh) 一种动态环境移动机器人的定位方法
CN116147618B (zh) 一种适用动态环境的实时状态感知方法及系统
KR101766823B1 (ko) 불규칙한 조도 변화에 강건한 영상 기반 주행거리 측정 시스템 및 방법
Angladon et al. Room floor plan generation on a project tango device
US20230020713A1 (en) Image processing system and method
CN113570713B (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