CN113781645A - 一种面向室内泊车环境的定位和建图方法 - Google Patents

一种面向室内泊车环境的定位和建图方法 Download PDF

Info

Publication number
CN113781645A
CN113781645A CN202111009280.9A CN202111009280A CN113781645A CN 113781645 A CN113781645 A CN 113781645A CN 202111009280 A CN202111009280 A CN 202111009280A CN 113781645 A CN113781645 A CN 113781645A
Authority
CN
China
Prior art keywords
camera
representing
positioning
around
indoor
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.)
Granted
Application number
CN202111009280.9A
Other languages
English (en)
Other versions
CN113781645B (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.)
Tongji University
Original Assignee
Tongji University
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 Tongji University filed Critical Tongji University
Priority to CN202111009280.9A priority Critical patent/CN113781645B/zh
Publication of CN113781645A publication Critical patent/CN113781645A/zh
Application granted granted Critical
Publication of CN113781645B publication Critical patent/CN113781645B/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
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders
    • 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/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30264—Parking
    • 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系统(simultaneouslocalization and mapping)是不可缺少的核心组成部分，它可以估计车辆运动并构建地图。其中视觉传感器(前视相机)和惯性测量单元(IMU)构成了基本传感器配置。两个传感器相互协作构成了VI-SLAM系统。其中前视相机可以在纹理丰富的场景中稳定工作，它通过匹配两幅图片中的特征点估计相机的位姿。然而在剧烈抖动或者纹理缺失的环境中，仅靠前视相机很难完成实际场景的应用需求。IMU通过直接测量传感器本体的角速度和加速度来估计自身的运动，从而与相机进行互补，使得视觉模块在猛烈加速或减速时，保持一定的定位精度，防止跟踪丢失。
目前，在VI-SLAM系统中通常采用紧耦合的方式把IMU的状态和相机状态合并在一起，共同构建运动方程和观测方程，然后进行状态估计。例如MSCKF(Mourikis et al.，Amulti-state constraint kalman filter for vision-aided inertial navigation，2007)，基于EKF的实时视觉惯导系统可以在大尺度真实环境下提供精确的位姿；VIORB(Mur-Artal et al.，Visual-inertial monocular SLAM with map reuse，2017)，通过将IMU信息融入ORB-SLAM系统中，构建具有尺度信息的稳定、鲁棒的VI-SLAM系统，在自动驾驶领域应用广泛。但是这些常规紧耦合的方式仅仅使用了环境中的低层次语义特征，而这些特征在地下车库中往往受到光照等因素的影响，导致跟踪结果不稳定，在环境复杂的地下停车环境中基本无法使用；同时这些方法都缺乏对环境的语义理解，无法构建包含语义信息，尤其是包含库位信息的语义地图，因此无法满足室内泊车任务的基本需求。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种面向室内泊车环境的定位和建图方法。
本发明的目的可以通过以下技术方案来实现：
一种面向室内泊车环境的定位和建图方法，步骤包括：
S1、对车辆上前视相机、环视相机系统和惯性测量单元分别进行标定；
S2、获取前视相机、环视相机系统和惯性测量单元采集的数据；
S3、对采集的数据进行紧耦合优化，包括：
通过前视相机的数据构建视觉约束项；
通过环视相机系统的数据构建环视约束项；
通过测量单元的数据构建惯导约束项；
S4、根据建视觉约束项、环视约束项和惯导约束项创建联合优化模型，对模型求解获取车库地图信息和车辆的定位信息。
进一步地，所述视觉约束项ev的表达式为：
ev＝∑∑eijQijeij
eij＝zij-f(TiPj)
式中，i表示时刻，j表示路标，zij表示拍摄图像上的像素位置，eij表示实际观测数据和针孔模型之间的误差，Ti表示相机位姿，Pj为路标j的3D位置，Qij表示协方差矩阵。
进一步地，所述环视约束项Es的表达式为：
ES＝eadj+eobs
Figure BDA0003238286100000021
Figure BDA0003238286100000022
式中，eadj表示邻接误差项，eobs表示观测误差项，
Figure BDA0003238286100000023
和
Figure BDA0003238286100000024
表示两个相邻泊车位在世界坐标系下的坐标，
Figure BDA0003238286100000025
表示第i个泊车位在前视坐标系下的坐标，dij表示两个相邻泊车位宽度和的一半，
Figure BDA0003238286100000026
表示相机位姿，M为地图中所有的相邻泊车位组成的集合，Ni是地图中的泊车位构成的集合。
进一步地，惯导约束项EI的表达式为：
EI＝ΣΣemnqmnemn
emn＝[Remn,Vemn,Pemn]
式中，Remn,Vemn,Pemn分别表示旋转、速度和位移误差项，m和n表示从第m个关键帧和第n个关键帧，每一个误差项都由惯性测量单元预积分计算值和测量值的差别定义，qmn表示两帧之间的信息矩阵。
进一步地，前视相机的标定包括：选取地面上具有已知坐标的三维点；获得三维点在前视相机中的成像像素坐标；利用已知坐标的三维点和像素坐标，通过PnP算法求解地面坐标系和前视坐标系的关系。
进一步地，环视相机系统的标定包括：将环视相机系统拍摄的图像去畸变；确定无畸变透视图像到鸟瞰环视图像的点对关系，得到逆透视图像；将环视相机系统中每个相机获得的逆透视图像拼接成完整的鸟瞰环视图。
进一步地，环视相机系统包括至少四个分布于车前、车后和车辆两侧的环视相机。
进一步地，惯性测量单元的标定包括：惯性测量单元的内参标定以及惯性测量单元和前视相机的外参标定。
进一步地，所述步骤S4中，联合优化模型的表达式为：
T,P＝argminE＝argmin ev+ES+EI
式中，T和P为待优化变量，分别表示相机位姿以及地图点位置，Es表示环视约束项，ev表示视觉约束项，EI表示惯导约束项；
通过求解最小二乘问题，最小化视觉误差项、环视误差项和惯导约束项，得到最优的相机位姿以及地图点位置。
进一步地，联合优化模型的求解采用牛顿法，梯度下降法，高斯牛顿法或列文伯格-马夸特法。
与现有技术相比，本发明具有以下有益效果：
本发明通过融合前视视觉特征、IMU的运动特征以及地下泊车环境中地面上的语义信息，求解相机位姿并构建周围环境的语义地图，从而完成周围环境的感知并实现自主泊车位任务。该方法由于使用了地面上稳固的语义信息，因此即使地下车库内运动物体较多或者光照条件差，所求解得到的定位结果精度不会受到较大影响。同时由于所构建的地图中包含了地面上的泊车位信息，因此它提供了适用于自主泊车任务的高精度语义地图。同时通过地面上的泊车位语义特征以及相邻泊车位间的结构信息进行后端优化，增加了系统的鲁棒性。
附图说明
图1为本发明的真题流程示意图。
图2是采用现有技术的SLAM系统结果展示示意图。
图3是采用本发明方案后SLAM系统结果展示示意图。
图4是泊车位检测结果展示。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
本实施例提供了一种面向室内泊车环境的定位和建图方法，是车辆上自主泊车系统的重要组成部分，可以仅通过车辆搭载的传感器推测出车辆位置，为泊车过程中的自主导航提供地图和定位，为自主泊车提供技术支撑，为决策规划层提供技术支撑。具体展开如下：
一、传感器配置：
本实施例在车辆上需要布置前视相机、环视相机系统和IMU(惯性测量单元)。其中环视相机系统为四个环视相机分别固定在车辆车身的前后左右四个方向。前视相机安装在车辆前方挡风玻璃下方，其位置比环视系统中的环视相机略高；IMU和前视相机绑定在一起。所有这些传感器能获得在时间同步的数据。具体使用的前视相机型号为小觅Mynt EyeD-50型号，该相机可以提供硬件同步的IMU数据和前视数据；环视相机型号为ov10640-490型广角鱼眼相机。
二、自主泊车系统中的算法步骤，如图1所示：
步骤S1、对车辆上前视相机、环视相机系统和惯性测量单元分别进行标定。
(一)前视相机的标定为选取地面上具有已知坐标的三维点；获得三维点在前视相机中的成像像素坐标；利用已知坐标的三维点和像素坐标，通过PnP算法求解地面坐标系和前视坐标系的关系。具体为：
(1)已知地面标定场上的点P的齐次坐标P＝(X,Y,Z,1)T,该点在前视去畸变图的投影点x1＝(u1,v1,1)；
(2)定义增广矩阵T＝[R|t]为3×4的矩阵，包含了前视相机相对于地面的位姿R和平移t；
(3)根据相机成像模型和欧式变换下坐标的定义可以得到：
Figure BDA0003238286100000051
(4)去s后，可得到约束：
Figure BDA0003238286100000052
Figure BDA0003238286100000053
(5)定义T的行向量：
t1＝(t1,t2,t3,t4)
t2＝(t5,t6,t7,t8)
t3＝(t9,t10,t11,t12)
(6)将上述步骤(3)中的公式整理：
Figure BDA0003238286100000054
Figure BDA0003238286100000055
(7)步骤(6)中提供了两个关于t的线性约束，假设一共有N个特征点，则可以列出如下线性方程组：
Figure BDA0003238286100000056
(8)由于t有12维，所以最少通过6对匹配点既可以实现T的解，当匹配点大于6对时，可以使用SVD等方法对超定方程求最小二乘解。
(二)环视相机系统的标定，将环视相机系统拍摄的图像去畸变；确定无畸变透视图像到鸟瞰环视图像的点对关系，得到逆透视图像；将环视相机系统中每个相机获得的逆透视图像拼接成完整的鸟瞰环视图。环视相机系统的标定关键是需要确定无畸变透视图像到鸟瞰环视图像的点对关系，具体为：
(1)预先准备棋盘格标定场，标定场的大小为10m×10m，每个正方形格子的边长为1m；
(2)将装有摄像头的实验车开至标定场中央指定位置，规定车辆坐标系的原点为后轮连线与车辆中轴线的交点；
(3)停放车辆后，使所有的摄像头均能够拍摄到清晰可见的棋盘格角点，将棋盘格角点作为标定的标志点；
(4)记录标定场上每个标志点在车辆坐标系下的坐标，它们在图像坐标系里面的值可以直接确定。
(三)IMU的标定
(1)将IMU静置一段时间，搜集该时间段内的IMU数据，通过Allan方差法对IMU的内参进行标定。
(2)准备含有可以被前视相机检测到角点信息的标定板图像，并打印该图像，将其置于平稳，光照充足的地方；
(3)将前视相机和IMU固定在一起，并同时移动前视相机和IMU，使得前视相机可以拍摄到标定板图像，同时移动IMU让IMU三个轴都被激活；
(4)获得一定时间内拍摄的标定板图像和IMU数据包，通过Kalibr库完成完成前视相机和IMU的外参标定。
步骤S2、车辆在地下车库运行时获取前视相机、环视相机系统和惯性测量单元采集的数据。
步骤S3、对采集的数据进行紧耦合优化，建立三类约束。
(1)通过前视相机的数据构建视觉约束项
(1-1)相机将三维世界中的坐标点(单位：米)映射到二维图像平面(单位：像素)的过程，可以用针孔模型进行描述。假设在第i时刻处对路标j进行了一次观测，对应到图像上的像素位置zij。那么观测方程可以写成如下形式：
zij＝f(TiPj)+vij,vij～N(0,Qij)
其中Ti为相机位姿。Pj为路标j的3D位置。vij为观测噪声，满足零均值的高斯分布，Qij为协方差矩阵。
(1-2)在噪声的影响下，通过带噪声的数据来推断位姿T和地图点P。因此对于在第j个地图点Pj和第i时刻对应的相机位姿Ti，每个视觉误差项en ij是实际观测数据与针孔模型之间的误差：
eij＝zij-f(TiPj)
(1-3)由于每个时刻下对于每个地图点的观测都是相互独立的。对于滑动窗口内的所有关键帧及其对应的所有观测点，将所有的视觉误差项相加，得到系统的视觉误差项，即视觉约束项：
ev＝∑∑eijQijeij。
(二)通过测量单元的数据构建惯导约束项
(2-1)两个连续关键帧之间的运动(旋转、速度、位移)可以分别通过IMU预积分值和视觉里程计来计算。每个惯导误差项emn用来约束第m个关键帧和第n个关键帧：
emn＝[Remn,Vemn,Pemn]
式中，Remn,Vemn,Pemn分别表示旋转、速度和位移误差项，m和n表示从第m个关键帧和第n个关键帧，每一个误差项都由惯性测量单元预积分计算值和测量值的差别定义，qmn表示两帧之间的信息矩阵。
(2-2)IMU误差项EI定义为：
EI＝∑∑emnqmnemn。
(三)通过环视相机系统的数据构建环视约束项
首先，利用获得的环视相机系统的标定结果，将车辆前后左右四个方向拍摄的鱼眼图像拼接成环视图，并在环视图中检测地下泊车环境中的泊车位信息；
然后，将检测得到的泊车位信息和地图中的泊车位进行数据关联；由于地下泊车库中地面泊车位信息磨损严重或者被车辆遮挡，因此数据关联是基于空间位置的数据关联；
最后，利于利用得到的关联泊车位信息构建环视约束项。
环视约束项Es可以通过邻接误差项Eadj和观测约束项Eobs来表示，保证了环视约束项的精度和鲁棒性，具体表达式为：
ES＝eadj+eobs
邻接误差项Eadj是地下泊车环境下特殊的相邻约束形成的误差，它根据是否存在相邻语义信息来预先确定该语义信息的位置。观测误差项Eobs代表语义信息和观测帧之间观测约束形成的误差。
邻接误差项：由于地面上的语义信息均不是独立存在的，它都与其相邻的语义信息形成一种邻接约束。该约束的目的是使得原本相邻的语义目标之间保持相邻的约束状态。在语义目标检测过程中，可以获得相邻的泊车位，这样的相关联的信息即为相邻语义信息。由于两个相邻泊车位信息的距离应当为两个泊车位长度和的一半，因此泊车位的相邻误差eadj形式如下：
Figure BDA0003238286100000081
其中M为地图中所有的相邻泊车位组成的集合，
Figure BDA0003238286100000082
和
Figure BDA0003238286100000083
表示两个相邻泊车位在世界坐标系下的坐标，dij表示两个相邻泊车位宽度和的一半。
观测约束项：当观测数据和语义地标之间正常关联后，该观测数据就在语义地标和观测帧位姿之间形成了一项约束。通过构建ICP(iterative closest point)损失并通过最小化损失优化当前帧位姿。滑动窗口中所有的关键帧与在这些关键帧中能够观测到的语义地标之间都可以构建损失项，对于所有损失项进行加和就构造了语义地标的观测损失eobs：
Figure BDA0003238286100000084
其中
Figure BDA0003238286100000085
分别表示第i个泊车位在世界坐标系和前视坐标系下的坐标，
Figure BDA0003238286100000086
表示相机位姿，Ni是地图中的泊车位构成的集合。
步骤S4、根据建视觉约束项、环视约束项和惯导约束项创建联合优化模型，对模型求解获取车库地图信息和车辆的定位信息。
对车辆在行进过程中所有的观测数据分别构建视觉约束项、惯导约束项和环视约束项，其具体公式如下。
T,P＝argminE＝argmin ev+ES+EI
其中T,P为待优化变量，分别表示相机位姿以及地图点位置。通过求解最小二乘问题，最小化前视视觉误差项、环视误差项和惯导约束项，就可以得到最优的相机位姿以及地图点位置。常见的优化方法包括牛顿法，梯度下降法，高斯牛顿法和列文伯格-马夸特法。
以下结合具体实验对本发明的有益效果进行说明：
实验设置：为了采集一定规模的地下泊车环境数据，用于分析比较本发明所能达到的效果，在实验中，使用改装车辆在同济地下车库进行数据采集。实验在配有Intel(R)Core(TM)i7-6700HQ CPU,Nvidia(R)Quadro(R)M1000M显卡和16GB RAM的工作站上运行，编程语言为C++。
实验一：定性结果展示：
图2展示的是无泊车辅助优化的SLAM系统的在同济大学电信学院地下车库的运行结果。该车库的每一排由12个库位组成，每两个库位彼此相邻为一组，两组车库之间有墙。从图中可以看出，由于IMU恢复尺度很难绝对精确，所以地图的整体尺度偏小，而且车辆转弯处定位不准，上方车位存在一定错位；另一方面，每组车库存在一定重合，这是定位误差、环视图误差和泊车位的检测误差共同导致的。
图3展示的是泊车位辅助定位的SLAM系统的运行结果。从图中可以可出，经过泊车位辅助优化后，环视图泊车位更准确；首先整体尺度更为合理，上方转弯处的车库位基本平齐，下方转弯处两排车库位在现实环境中确实存在一定的距离。同时，每组车库之间的距离和两排车库之间的距离也更加合理，更符合真实场景环境的空间分布。此外，经过优化后，每组内的两个相邻车库之间基本正常贴合，不再存在明显重合区域。
实验二：回环误差
目前SLAM常用的精度评价指标包括相对位姿误差(RPE)，均方根误差(RMSE)，绝对轨迹误差(ATE)等，但是这些评价指标依赖于真实路线的获取。通常情况下，像车载环境这样的非手持情况，真实路径依赖GPS定位，但是在室内环境中，GPS没有信号，而在车辆低速运行的时候，依赖车辆航位推算数据又存在着较大的偏差。UWB(Ultra WideBand)早期被用于近距离高速数据传输，近年来也被用于精确的室内定位，但是UWB的部署太过昂贵，需要多个基站，一个中小型的地下车库需要耗费80-100万的资金，所以很难落地于现实环境。因此针对当前的室内停车场环境，很难获得车辆行进的真实轨迹，也无法使用常见方案评价精度。
在这种情况，一种常用的评价方案就是回环误差。因为在实际环境中，SLAM系统可以允许绝对位置存在一定的定位误差，但是当车辆行驶到相同位置时，这种定位误差必须一致，这样就可以保证车辆可以采取固定的驾驶策略。因此可以通过预先选择测试点，读取车辆重新访问测试点的坐标来评价定位精度。当定位足够精确时，环绕车库一周重新行驶到测试点时，SLAM系统定位坐标应当与上一次行驶到测试点时一致。
实际操作时，首先由人手动驾驶车辆，初始化车辆停车场地图，待地图稳定后(一般需要环绕三圈以上)，即可进行精度评测。可以在第一圈由人手动驾驶车辆，在测试点处记录当前坐标，这个坐标即为参考坐标。之后的驾驶若条件允许，可通过车辆的控制与规划模块，依照第一圈人工驾驶路线进行自动驾驶，这样可以保证车辆基本精准驶过测试点。当然，车辆如果不具备自动驾驶功能，可以在车辆行驶到测试点时，在地面记录车辆位置，之后每一圈由人工驾驶，尽量精确的停靠在测试点上并记录位置。
通过设置三个评测点，分别测量运行三圈后的回环误差，得到下列表格：
Figure BDA0003238286100000101
Figure BDA0003238286100000102
Figure BDA0003238286100000111
经过计算，可以得到上述三点的平均重定位误差为0.040227m。
为了证明本方法中的有效性，实验对比了本方法的结果和现有解决方案：[1]“Mur-Artal,R.Tardós,J.D.Visual-Inertial Monocular SLAM with Map Reuse.IEEERobot.Autom.Lett.2017,2,796–803.”以及[2]“J.Zhao,Y.Huang,X.He,et al.,“Visualsemantic landmark-based robust mapping and localization for autonomous indoorparking,”Sensors,vol.19,no.1,pp.161:1–20,Jan.2019.”。实验结果如果所示：
方法 重访问误差(m)
[1] 0.199
[2] 0.280
本方法 0.08
从上述表格中可以看出本方法的在地下泊车环境下定位的有效性。
实验三：语义目标识别率：
圈数 泊车位
第一圈 23/24＝95.8％
第二圈 24/24＝100％
建图完成 24/24＝100％
从图中可以看出，当自动驾驶车辆行驶2圈后可以识别所有的泊车位信息。其中泊车位的检测结果如图4所示。
实验四：实时性
在车辆建图时，车辆的运行速度保持在8-15km/h，经测试得到如下结果：
循环次数 泊车位
(0,100) 0.04733516999
(100,200) 0.042679044999
(200,500) 0.05220795800
(500,1000) 0.06069974900000009
(1000,2000) 0.06479910200000003
(2000,3000) 0.06664350399999999
从表格中可以看到，在500帧以内，系统的平均处理时长在0.0522s，即系统帧率可以达到20帧；在3000个关键帧时，车辆已经完成闭环，系统的平均处理时长0.0666s，即达到每秒15帧。
事实上，系统的帧率可以通过改变图像的大小和提取ORB特征点的数量来提升，这意味着如果对帧率还有更高的要求，可以通过对图像进行缩放或减少提取ORB特征点的数目，牺牲一定的精度，进一步提升系统处理速度。
实验三：相邻泊车位相邻点距离
由于地下车库的特殊场景，相邻车库位入口线的内侧库位点是重合的，即距离为零，所以可以通过计算相邻车库位的距离来衡量系统的性能。统计12组相邻车库位入口线的内侧库位点的距离，得到如下结果：
Figure BDA0003238286100000121
Figure BDA0003238286100000131
由上面的表格可以看到，利用泊车位辅助优化后，相邻泊车位之间的误差明显下降，一定程度上证明了本发明方法优化的有效性。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种面向室内泊车环境的定位和建图方法，其特征在于，步骤包括：
S1、对车辆上前视相机、环视相机系统和惯性测量单元分别进行标定；
S2、获取前视相机、环视相机系统和惯性测量单元采集的数据；
S3、对采集的数据进行紧耦合优化，包括：
通过前视相机的数据构建视觉约束项；
通过环视相机系统的数据构建环视约束项；
通过测量单元的数据构建惯导约束项；
S4、根据建视觉约束项、环视约束项和惯导约束项创建联合优化模型，对模型求解获取车库地图信息和车辆的定位信息。
2.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，所述视觉约束项ev的表达式为：
ev＝∑∑eijQijeij
eij＝zij-f(TiPj)
式中，i表示时刻，j表示路标，zij表示拍摄图像上的像素位置，eij表示实际观测数据和针孔模型之间的误差，Ti表示相机位姿，Pj为路标j的3D位置，Qij表示协方差矩阵。
3.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，所述环视约束项Es的表达式为：
ES＝eadj+eobs
Figure FDA0003238286090000011
Figure FDA0003238286090000012
式中，eadj表示邻接误差项，eobs表示观测误差项，
Figure FDA0003238286090000013
和
Figure FDA0003238286090000014
表示两个相邻泊车位在世界坐标系下的坐标，
Figure FDA0003238286090000015
表示第i个泊车位在前视坐标系下的坐标，dij表示两个相邻泊车位宽度和的一半，
Figure FDA0003238286090000016
表示相机位姿，M为地图中所有的相邻泊车位组成的集合，Ni是地图中的泊车位构成的集合。
4.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，惯导约束项EI的表达式为：
EI＝∑∑emnqmnemn
emn＝[Remn,Vemn,Pemn]
式中，Remn,Vemn,Pemn分别表示旋转、速度和位移误差项，m和n表示从第m个关键帧和第n个关键帧，每一个误差项都由惯性测量单元预积分计算值和测量值的差别定义，qmn表示两帧之间的信息矩阵。
5.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，前视相机的标定包括：选取地面上具有已知坐标的三维点；获得三维点在前视相机中的成像像素坐标；利用已知坐标的三维点和像素坐标，通过PnP算法求解地面坐标系和前视坐标系的关系。
6.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，环视相机系统的标定包括：将环视相机系统拍摄的图像去畸变；确定无畸变透视图像到鸟瞰环视图像的点对关系，得到逆透视图像；将环视相机系统中每个相机获得的逆透视图像拼接成完整的鸟瞰环视图。
7.根据权利要求6所述的一种面向室内泊车环境的定位和建图方法，其特征在于，环视相机系统包括至少四个分布于车前、车后和车辆两侧的环视相机。
8.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，惯性测量单元的标定包括：惯性测量单元的内参标定以及惯性测量单元和前视相机的外参标定。
9.根据权利要求1所述的一种面向室内泊车环境的定位和建图方法，其特征在于，所述步骤S4中，联合优化模型的表达式为：
T,P＝argmin E＝argmin ev+ES+EI
式中，T和P为待优化变量，分别表示相机位姿以及地图点位置，Es表示环视约束项，ev表示视觉约束项，EI表示惯导约束项；
通过求解最小二乘问题，最小化视觉误差项、环视误差项和惯导约束项，得到最优的相机位姿以及地图点位置。
10.根据权利要求9所述的一种面向室内泊车环境的定位和建图方法，其特征在于，联合优化模型的求解采用牛顿法，梯度下降法，高斯牛顿法或列文伯格-马夸特法。
CN202111009280.9A 2021-08-31 2021-08-31 一种面向室内泊车环境的定位和建图方法 Active CN113781645B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111009280.9A CN113781645B (zh) 2021-08-31 2021-08-31 一种面向室内泊车环境的定位和建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111009280.9A CN113781645B (zh) 2021-08-31 2021-08-31 一种面向室内泊车环境的定位和建图方法

Publications (2)

Publication Number Publication Date
CN113781645A true CN113781645A (zh) 2021-12-10
CN113781645B CN113781645B (zh) 2024-03-26

Family

ID=78840105

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111009280.9A Active CN113781645B (zh) 2021-08-31 2021-08-31 一种面向室内泊车环境的定位和建图方法

Country Status (1)

Country Link
CN (1) CN113781645B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114863096A (zh) * 2022-04-02 2022-08-05 合众新能源汽车有限公司 室内停车场的语义地图构建及定位方法和装置

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107180215A (zh) * 2017-05-31 2017-09-19 同济大学 基于库位和二维码的停车场自动建图与高精度定位方法
CN109443348A (zh) * 2018-09-25 2019-03-08 同济大学 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN109631896A (zh) * 2018-07-23 2019-04-16 同济大学 一种基于车辆视觉和运动信息的停车场自主泊车定位方法
US20190234746A1 (en) * 2016-09-14 2019-08-01 Zhejiang University Method for simultaneous localization and mapping
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111862672A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法
WO2021017212A1 (zh) * 2019-07-26 2021-02-04 魔门塔(苏州)科技有限公司 一种多场景高精度车辆定位方法、装置及车载终端

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190234746A1 (en) * 2016-09-14 2019-08-01 Zhejiang University Method for simultaneous localization and mapping
CN107180215A (zh) * 2017-05-31 2017-09-19 同济大学 基于库位和二维码的停车场自动建图与高精度定位方法
CN109631896A (zh) * 2018-07-23 2019-04-16 同济大学 一种基于车辆视觉和运动信息的停车场自主泊车定位方法
CN109443348A (zh) * 2018-09-25 2019-03-08 同济大学 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
WO2021017212A1 (zh) * 2019-07-26 2021-02-04 魔门塔(苏州)科技有限公司 一种多场景高精度车辆定位方法、装置及车载终端
CN111862672A (zh) * 2020-06-24 2020-10-30 北京易航远智科技有限公司 基于顶视图的停车场车辆自定位及地图构建方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
程传奇;郝向阳;李建胜;刘智伟;胡鹏;: "基于非线性优化的单目视觉/惯性组合导航算法", 中国惯性技术学报, no. 05 *
董伯麟;柴旭;: "基于IMU/视觉融合的导航定位算法研究", 压电与声光, no. 05 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114863096A (zh) * 2022-04-02 2022-08-05 合众新能源汽车有限公司 室内停车场的语义地图构建及定位方法和装置
WO2023184869A1 (zh) * 2022-04-02 2023-10-05 合众新能源汽车股份有限公司 室内停车场的语义地图构建及定位方法和装置
CN114863096B (zh) * 2022-04-02 2024-04-16 合众新能源汽车股份有限公司 室内停车场的语义地图构建及定位方法和装置

Also Published As

Publication number Publication date
CN113781645B (zh) 2024-03-26

Similar Documents

Publication Publication Date Title
US10860871B2 (en) Integrated sensor calibration in natural scenes
CN107341453B (zh) 一种车道线提取方法及装置
CN111986506B (zh) 基于多视觉系统的机械式停车位泊车方法
JP5227065B2 (ja) 三次元機械地図、三次元機械地図生成装置、ナビゲーション装置及び自動運転装置
CN108759823B (zh) 基于图像匹配的指定道路上低速自动驾驶车辆定位及纠偏方法
CN111220153B (zh) 基于视觉拓扑节点和惯性导航的定位方法
CN109443348B (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN102967305B (zh) 基于大小回字标志物的多旋翼无人机位姿获取方法
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN108733039A (zh) 一种机器人室内导航定位的方法与装置
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN111862673B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
CN106651953A (zh) 一种基于交通指示牌的车辆位姿估计方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
EP4124829B1 (en) Map construction method, apparatus, device and storage medium
Shunsuke et al. GNSS/INS/on-board camera integration for vehicle self-localization in urban canyon
CN109871739B (zh) 基于yolo-sioctl的机动站自动目标检测与空间定位方法
CN208323361U (zh) 一种基于深度视觉的定位装置及机器人
CN111986261A (zh) 一种车辆定位方法、装置、电子设备及存储介质
CN113781645B (zh) 一种面向室内泊车环境的定位和建图方法
Hara et al. Vehicle localization based on the detection of line segments from multi-camera images
WO2023283987A1 (zh) 无人系统的传感器安全性检测方法、设备及存储介质
CN113330278A (zh) 一种导航装置、方法及相关设备
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
Yuan et al. Estimation of vehicle pose and position with monocular camera at urban road intersections

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