CN112304321B - 一种基于视觉和imu的车辆融合定位方法及车载终端 - Google Patents

一种基于视觉和imu的车辆融合定位方法及车载终端 Download PDF

Info

Publication number
CN112304321B
CN112304321B CN201910681732.4A CN201910681732A CN112304321B CN 112304321 B CN112304321 B CN 112304321B CN 201910681732 A CN201910681732 A CN 201910681732A CN 112304321 B CN112304321 B CN 112304321B
Authority
CN
China
Prior art keywords
pose
track
fusion
positioning
vehicle
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
CN201910681732.4A
Other languages
English (en)
Other versions
CN112304321A (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.)
Beijing Momenta Technology Co Ltd
Original Assignee
Beijing Momenta Technology 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 Beijing Momenta Technology Co Ltd filed Critical Beijing Momenta Technology Co Ltd
Priority to CN201910681732.4A priority Critical patent/CN112304321B/zh
Publication of CN112304321A publication Critical patent/CN112304321A/zh
Application granted granted Critical
Publication of CN112304321B publication Critical patent/CN112304321B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

本发明实施例公开一种基于视觉和IMU的车辆融合定位方法及车载终端。该方法包括：根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对相似约束优化函数进行迭代求解，将相似约束优化函数取最优化解时的估计融合位姿，确定为车辆在第一时刻融合后的定位位姿；其中，第一轨迹为第一视觉位姿和多个第二视觉位姿对应的轨迹，第二轨迹为第一IMU位姿和多个第二IMU位姿对应的轨迹。应用本发明实施例提供的方案，无需增加硬件设备的耗费，能够在无GNSS信号的场景中实现对车辆位姿的定位。

Description

一种基于视觉和IMU的车辆融合定位方法及车载终端
技术领域
本发明涉及智能驾驶技术领域，具体而言，涉及一种基于视觉和IMU的车辆融合定位方法及车载终端。
背景技术
在智能驾驶技术领域中，对车辆进行定位是智能驾驶中的重要环节。通常，当车辆在户外行驶时，可以根据全球导航卫星系统(Global Navigation Satellite System，GNSS)采集的数据，经过处理后确定车辆精确的定位位姿。当车辆行驶至卫星定位信号较弱或无信号的停车场中时，无法再利用GNSS数据确定车辆的位姿。
相关技术中，通常预先在停车场中设置多个激光发射器或者射频识别设备(RadioFrequency Identification，RFID)等检测设备，当车辆行驶至对应的检测设备时，可以根据车辆与检测设备之间的信息交互识别车辆的位姿。这种定位方式也能够确定车辆在停车场中的位姿，但是需要在停车场中布置大量硬件设备，这样也就增加了硬件设备的耗费成本。
发明内容
本发明提供了一种基于视觉和IMU的车辆融合定位方法及车载终端，无需增加硬件设备的耗费，以在无GNSS信号的场景中实现对车辆位姿的定位。具体的技术方案如下。
第一方面，本发明实施例公开了一种基于视觉和IMU的车辆融合定位方法，包括：
获取相机设备在第一时刻采集的道路图像，并基于所述道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在所述第一时刻的第一视觉位姿；
获取惯性测量单元IMU在第一时刻采集的IMU数据，并基于所述IMU数据，推算所述车辆在所述第一时刻的第一IMU位姿；
获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿；其中，多个第二时刻为所述第一时刻之前的时刻；
根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿；
其中，所述第一轨迹为所述第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为所述第一IMU位姿和多个第二IMU位姿对应的轨迹。
第二方面，本发明实施例公开了一种车载终端，包括：处理器、相机设备和惯性测量单元IMU；所述处理器包括：视觉位姿确定模块、IMU位姿确定模块、历史位姿获取模块和融合位姿确定模块；
视觉位姿确定模块，用于获取相机设备在第一时刻采集的道路图像，并基于所述道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在所述第一时刻的第一视觉位姿；
IMU位姿确定模块，用于获取IMU在第一时刻采集的IMU数据，并基于所述IMU数据，推算所述车辆在所述第一时刻的第一IMU位姿；
历史位姿获取模块，用于获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿；其中，多个第二时刻为所述第一时刻之前的时刻；
融合位姿确定模块，用于根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿；
其中，所述第一轨迹为所述第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为所述第一IMU位姿和多个第二IMU位姿对应的轨迹。
由上述内容可知，本发明实施例提供的基于视觉和IMU的车辆融合定位方法及车载终端，可以根据第一轨迹和第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变估计融合位姿的取值，对相似约束优化函数进行迭代求解，当相似约束优化函数取最优化解时的估计融合位姿，即为车辆在第一时刻融合后的定位位姿。本发明实施例在确定车辆的融合后定位位姿时，使用车辆原有的相机设备和IMU设备，无需增加硬件设备，能够在无GNSS信号的场景中实现对车辆位姿的定位。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
本发明实施例的创新点包括：
1、根据一段时间内的多个视觉位姿和多个IMU位姿与估计融合位姿之间的相似性，构建相似约束优化函数，不断调整估计融合位姿的取值，当该相似约束优化函数取最优解时，估计融合位姿即为融合后的定位位姿。构建的相似约束优化函数能够考虑到两种位姿与融合位姿之间的多种约束，因此所确定的融合后定位位姿的准确性更高，并且无需增加硬件设备，能够在无GNSS信号的场景中实现对车辆的定位。
2、给出了构建的相似约束优化函数的具体形式，并提供了迭代求解相似约束优化函数的具体实施方式，能够更准确地确定两种位姿的融合后定位位姿。
3、根据视觉位姿的定位精度为视觉位姿赋予权重，当定位精度高时视觉位姿的权重相对较大，定位精度低时视觉位姿的权重相对较小，这样能够提高融合时的合理性，进而提高融合位姿的准确性。
4、根据车轮速检测设备采集的车辆的行驶速度推算IMU位姿，相比于直接使用IMU数据中的加速度确定行驶速度，能够消除行驶速度的累计误差，提高IMU位姿的准确性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例。对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的基于视觉和IMU的车辆融合定位方法的一种流程示意图；
图2为第一轨迹和第二轨迹中各个位置点的一种示意图；
图3为图1中步骤S140的一种流程示意图；
图4为本发明实施例提供的基于视觉和IMU的车辆融合定位方法的另一种流程示意图；
图5为本发明实施例提供的车载终端的一种结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。例如包含的一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。
本发明实施例公开了一种基于视觉和IMU的车辆融合定位方法及车载终端，无需增加硬件设备的耗费，以在无GNSS信号的场景中实现对车辆位姿的定位。其中，IMU为惯性测量单元(Inertial Measurement Unit)。下面对本发明实施例进行详细说明。
图1为本发明实施例提供的基于视觉和IMU的车辆融合定位方法的一种流程示意图。该方法应用于电子设备。该电子设备可以为普通计算机、服务器或者智能终端设备等，也可以为车载电脑或车载工业控制计算机(Industrial personal Computer，IPC)等车载终端。该方法具体包括以下步骤。
S110：获取相机设备在第一时刻采集的道路图像，并基于道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在第一时刻的第一视觉位姿。
其中，道路图像包含车辆行驶时周围的道路特征和背景部分。道路特征包括但不限于道路上的车道线、路灯杆、交通牌、边缘线、停止线、红绿灯和地面的其他标识。边缘线包括但不限于车道边缘线和泊车位边缘线。第一视觉位姿为车辆的位姿。本实施例中，位姿包括位置和姿态。
预设地图可以为预先建立的高精度地图。该预设地图中可以包括各个位置点的道路特征。预设地图中的位置点可以二维坐标点或三维坐标点表示。
本实施例中，相机设备和IMU可以均设置于车辆上。在实际应用中，可以按照预设频率确定视觉位姿。
S120：获取IMU在第一时刻采集的IMU数据，并基于IMU数据，推算车辆在第一时刻的第一IMU位姿。
其中，IMU数据可以包括角速度和加速度等数据。
通常，IMU采集数据的频率高于相机设备采集图像的频率。当第一时刻确定时，获取IMU在第一时刻采集的IMU数据时，可以从IMU采集的大量IMU数据中确定时间戳为第一时刻的IMU数据。
基于IMU数据，推算车辆在第一时刻的第一IMU位姿时，具体可以包括：获取上一定位位姿，基于该IMU数据，推算车辆在第一时刻的第一IMU位姿。
在实际应用中，可以按照预设频率推算车辆的IMU位姿。具体的，可以按照与确定视觉位姿的频率相同的频率推算车辆的IMU位姿。
S130：获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿。
其中，多个第二时刻为第一时刻之前的时刻，各个第二时刻早于第一时刻。在获取多个第二视觉位姿和多个第二IMU位姿时，可以从存储的多个时刻的视觉位姿和多个时刻的IMU位姿中确定。
S140：根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变估计融合位姿的取值，对相似约束优化函数进行迭代求解，将相似约束优化函数取最优化解时的估计融合位姿，确定为车辆在第一时刻融合后的定位位姿。
其中，第一轨迹为所述第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为第一IMU位姿和多个第二IMU位姿对应的轨迹。估计融合位姿为多个，包括在第一时刻和各个第二时刻的估计融合位姿。
第一轨迹中的各个位置点为第一视觉位姿和各个第二视觉位姿对应的位置点，第二轨迹中的各个位置点为第一IMU位姿和各个第二IMU位姿对应的位置点。参见图2所示，在由第一时刻t1和各个第二时刻tn组成的一段时间内，第一轨迹和第二轨迹并不是重合的。其中，第一轨迹中的各个位置点采用空心圆圈表示，第二轨迹中的各个位置点采用实心圆圈表示，车辆的大致行驶方向为从左到右。
由于不同的定位方式均存在一定的定位误差，这导致第一轨迹和第二轨迹并不是重合的，每个位置点之间存在较小的区别。第一轨迹中各个位置点与对应的估计融合位姿之间是比较接近的，第二轨迹中的相邻位置点之间的特征与对应的相邻估计融合位姿之间的特征是相似的。
本实施例中，第一轨迹与第二轨迹中对应位置点之间满足相似变换，对应位置点之间均存在对应的融合变换系数，例如，图2中第一时刻的第一视觉位姿和第一IMU位姿之间存在对应的融合变换系数；同样的，其他时刻的第二视觉位姿和第二IMU位姿之间也存在对应的融合变换系数。
由上述内容可知，本实施例可以根据第一轨迹和第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变估计融合位姿的取值，对相似约束优化函数进行迭代求解，当相似约束优化函数取最优化解时的估计融合位姿，即为车辆在第一时刻融合后的定位位姿。本实施例在确定车辆的融合后定位位姿时，使用车辆原有的相机设备和IMU设备，无需增加硬件设备，能够在无GNSS信号的场景中实现对车辆位姿的定位。并且，本实施例是根据第一轨迹和第二轨迹分别与估计融合位姿之间的相似关系，以及第一轨迹和第二轨迹中各个位置点之间的融合变换系数，构建相似约束优化函数，融合过程考虑各种约束关系能够使得确定的融合定位位姿更准确，更接近于真实值。
在本发明的另一实施例中，基于图1所示实施例，步骤S110，基于道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在第一时刻的第一视觉位姿时，具体可以包括以下步骤1a～4a。
步骤1a：确定车辆的估计位姿。本步骤可以根据车辆的上一定位位姿确定该估计位姿。例如，可以直接将上一定位位姿确定为估计位姿，也可以将对上一定位位姿做预设变换后的位姿作为估计位姿。上一定位位姿，可以为上一时刻基于视觉确定的视觉位姿，也可以为上一时刻确定的融合后定位位姿。
步骤2a：基于车辆的估计位姿，确定第一道路特征与第二道路特征之间的参考映射误差。
其中，第一道路特征为道路图像中的道路特征，采用的是道路图像中的位置表示。第二道路特征为预设地图中与第一道路特征匹配成功的道路特征，也是预设地图中的道路特征，采用的是预设地图所在坐标系中的坐标来表示。
在确定第一映射误差时，可以将第一道路特征和第二道路特征映射到同一坐标系中后确定映射误差。本步骤具体可以包括以下实施方式：
实施方式一，根据车辆的估计位姿，以及第一道路特征在道路图像中的位置，计算第一道路特征映射至预设地图中的第一映射位置；计算第一映射位置与第二道路特征在预设地图中的位置之间的误差，得到参考映射误差。
在本实施方式中，通过将第一道路特征映射到预设地图所在的坐标系中，对第一道路特征与第二道路特征的位置进行对比，得到参考映射误差。
根据估计位姿，以及第一道路特征在道路图像中的位置，计算第一道路特征映射至预设地图中的第一映射位置时，具体可以根据图像坐标系与世界坐标系之间的转换关系，以及估计位姿，将第一道路特征在道路图像中的位置转换至世界坐标系中，得到第一映射位置。其中，图像坐标系为道路图像所在的坐标系，世界坐标系为预设地图所在的坐标系。图像坐标系与世界坐标系之间的转换关系，可以通过图像坐标系与相机坐标系之间的内参矩阵，以及相机坐标系与世界坐标系之间的旋转矩阵和平移矩阵得到。
实施方式二，根据车辆的估计位姿，以及第二道路特征在预设地图中的位置，计算第二道路特征映射至道路图像所在坐标系中的第二映射位置；计算第一道路特征在道路图像中的位置与第二映射位置之间的误差，得到参考映射误差。
在本实施方式中，通过将第二道路特征映射到道路图像所在的坐标系中，对第一道路特征与第二道路特征的位置进行对比，得到参考映射误差。
根据车辆的估计位姿，以及第二道路特征在预设地图中的位置，计算第二道路特征映射至道路图像所在坐标系中的第二映射位置时，可以根据图像坐标系与世界坐标系之间的转换关系，以及车辆的估计位姿，将第二道路特征在预设地图中的位置转换至图像坐标系中，得到第二映射位置。
上述两种实施方式对应两种不同的映射方式，在实际应用中可以择一使用。
步骤3a：当参考映射误差大于预设误差阈值时，调整车辆的估计位姿，并执行步骤2a中基于车辆的估计位姿，确定第一道路特征与第二道路特征之间的参考映射误差的步骤。
当参考映射误差大于预设误差阈值时，认为该估计位姿与车辆的真实定位位姿之间还存在较大差距，可以继续进行迭代。
步骤4a：当参考映射误差不大于预设误差阈值时，根据车辆的当前估计位姿确定车辆在第一时刻的第一视觉位姿。
当参考映射误差不大于所述预设误差阈值时，认为该估计位姿与车辆的真实定位位姿非常接近，定位精度已经达到要求。
上述实施方式中，基于道路图像的道路特征与预设地图中的道路特征之间的匹配结果，通过迭代方式确定车辆的定位位姿的方式，能够更准确地确定车辆的视觉位姿。
在本发明的另一实施例中，基于图1所示实施例，步骤S140，根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变估计融合位姿的取值，对相似约束优化函数进行迭代求解，将相似约束优化函数取最优化解时的估计融合位姿，确定为车辆在第一时刻融合后的定位位姿的步骤，具体可以按照图3所示流程示意图进行，包括：
步骤S141：根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：
Figure BDA0002144999770000091
其中，S(ti)和Rs(ti)分别为第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，Pf(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，Pv(ti)和Rv(ti)分别为第一轨迹中ti时刻位置点的位置和姿态，Po(ti)和Ro(ti)分别为第二轨迹中ti时刻位置点的位置和姿态，t1为第一时刻，tn为多个第二时刻中的最早时刻，n为第一时刻和多个第二时刻的总数量，min为取最小值函数。
上述P代表位置，R代表姿态，下标为v的量为视觉位姿中的量，下标为o的量为IMU位姿中的量，下标为s的量为融合变换系数中的量。S代表第一轨迹中的位置点与第二轨迹中对应的位置点之间的缩放比例，Rs代表第一轨迹中的位置点与第二轨迹中对应的位置点之间的旋转矩阵。
由于基于视觉得到的视觉位姿是针对每个时刻得到的，而基于IMU的位姿推算是基于相邻时刻之间的相对量得到的，因此视觉位姿与估计融合位姿之间为单时刻约束，而IMU位姿与估计融合位姿之间为从上一时刻到当前时刻的约束。
相似约束优化函数E中的第一项表示每个视觉位姿中的位置与对应估计融合位姿中的位置之间应比较接近，第二项表示每个视觉位姿中的姿态与对应估计融合位姿中的姿态之间应比较接近，第三项表示相邻IMU位姿之间的位移与对应的相邻估计融合位姿之间的位移应比较接近，第四项表示相邻IMU位姿之间的变换角度与对应的相邻估计融合位姿之间的变换角度应比较接近，第五项表示相邻位置点之间的缩放比例应比较接近，第六项表示相邻位置点之间的旋转矩阵应比较接近。根据上述约束关系，能够构建出相似约束优化函数E。当E能取得最小值时的估计融合位姿和Rf，Pf，S，Rs为最优化解。
步骤S142：确定估计融合位姿的初始值，确定Rf，Pf，S，Rs的初始值。
估计融合位姿的初始值可以通过在对应的视觉位姿或者IMU位姿上叠加一定的扰动量得到。Rf，Ff，S，Rs的初始值可以根据经验值确定。
或者，Rf，Pf，S，Rs的初始值可以按照以下方式给出：
Rf(t)的初始值等于Rv(t)，Pf(t)的初始值等于Pv(t)，S(t)的初始值取1，Rs(t)的初始值等于
Figure BDA0002144999770000101
步骤S143：根据估计融合位姿的当前取值和Rf，Pf，S，Rs的当前取值，确定相似约束优化函数的当前函数值。
具体的，估计融合位姿的当前取值和Rf，Pf，S，Rs的当前取值代入步骤S141中相似约束优化函数E的表达式，得到当前函数值。
步骤S144：获取相似约束优化函数的上一函数值，判断上一函数值与当前函数值的差值绝对值是否大于预设差值阈值，如果是，则执行步骤S145；如果否，则执行步骤S146。
其中，预设差值阈值可以为预先根据经验值确定的值，调整预设差值阈值能够调整融合位姿的精度。当上一函数值与当前函数值的差值绝对值大于预设差值阈值时，认为相似约束优化函数的函数值还可以更小。当上一函数值与当前函数值的差值绝对值不大于预设差值阈值时，认为相似约束优化函数的函数值已经非常接近于最小值。
步骤S145：调整估计融合位姿的取值以及Rf，Pf，S，Rs的取值，返回执行步骤S143，根据估计融合位姿的当前取值和Rf，Ff，S，Rs的当前取值，确定相似约束优化函数的当前函数值的步骤。
调整估计融合位姿的取值以及Rf，Pf，S，Rs的取值时，可以根据上一估计融合位姿的取值以及上一Rf，Ff，S和Rs的取值，以及当前函数值的变化趋势进行调整。
步骤S146：将估计融合位姿的当前取值，确定为车辆在第一时刻融合后的定位位姿。
综上，本实施例给出了构建的相似约束优化函数的具体形式，并提供了迭代求解相似约束优化函数的具体实施方式，能够更准确地确定两种位姿的融合后定位位姿。
在本发明的另一实施例中，基于图1所示实施例，可以采用以下步骤1b和2b确定各个第二时刻。
步骤1b：将早于第一时刻的多个时刻作为待选择时刻，确定从各个待选择时刻到第一时刻车辆的行驶距离。
在车辆的行驶过程中，可以记录每个时刻的位姿。根据记录的每个时刻的位姿，可以确定各个待选时刻到第一时刻车辆的行驶距离。
步骤2b：当车辆的行驶距离满足预设距离条件时，将车辆的行驶距离对应的待选择时刻确定为时刻最早的第二时刻tn，将(t1，tn]区间中的各个时刻作为各个第二时刻。
其中，预设距离条件可以包括：车辆的行驶距离等于预设距离，车辆的行驶距离位于预设距离区间中等。(t1，tn]表示左开右闭区间，该区间不包含t1时刻。[t1，tn]为选定的时间窗口，采用该时间窗口中的视觉位姿和IMU位姿进行融合。
例如，当车辆的行驶距离达到10m时，将10m处的时刻作为时间窗口的边界点。时间窗口对应的行驶距离可以理解为上述预设距离，具体可以为10m、15m等值。预设距离可以为预先根据经验确定的值。
综上，本实施例中，根据车辆的行驶距离选取合适的时间窗口，较大或者较小的时间窗口都不利于实现对两种轨迹的融合，选取合适的时间窗口能够使得融合位姿更准确。
在本发明的另一实施例中，基于图1所示实施例可以得到图4所示实施例，该实施例具体包括以下步骤S410～S480。
S410：获取相机设备在第一时刻采集的道路图像，并基于道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在第一时刻的第一视觉位姿。
本步骤S410与S110相同，具体说明可以参见步骤S110中的内容。
S420：根据第一视觉位姿，确定第一道路特征与第二道路特征之间的映射误差。
其中，第一道路特征为所述道路图像中的道路特征，第二道路特征为预设地图中与第一道路特征匹配成功的道路特征。
在根据第一视觉位姿，确定第一道路特征与第二道路特征之间的映射误差时，可以将第一道路特征和第二道路特征映射到同一坐标系中后确定映射误差。本步骤具体可以包括以下实施方式：
根据第一视觉位姿，以及第一道路特征在道路图像中的位置，计算第一道路特征映射至预设地图中的第一映射位置；计算第一映射位置与第二道路特征在预设地图中的位置之间的误差，得到该映射误差；或者，
根据第一视觉位姿，以及第二道路特征在预设地图中的位置，计算第二道路特征映射至道路图像所在坐标系中的第二映射位置；计算第一道路特征在道路图像中的位置与第二映射位置之间的误差，得到该映射误差。
S430：从预设地图包含的多个不同地图区域中，确定所述第一视觉位姿所在的目标地图区域。
本实施例中，可以预先根据预设地图包含的道路特征，将预设地图划分成多个不同地图区域，每个地图区域中的道路特征之间具有关联性或者位置相近性。地图区域可以为圆形区域、矩形区域或其他区域形状。
在确定目标地图区域时，具体可以将第一视觉位姿中的位置坐标所在的地图区域确定为目标地图区域。
S440：根据预先建立的目标地图区域中映射误差与定位误差之间的对应关系，确定所述映射误差对应的定位误差，作为所述第一视觉位姿的定位精度。
本实施例中，可以预先建立各个不同地图区域中映射误差与定位误差之间的对应关系，从各个不同地图区域中映射误差与定位误差之间的对应关系中，确定目标地图区域中映射误差与定位误差之间的对应关系。
其中，映射误差与定位误差之间的对应关系可以采用以定位误差为变量的映射误差函数表示。在确定该映射误差对应的定位误差时，可以将该映射误差代入映射误差函数，得到该映射误差对应的定位误差。
定位误差可以理解为当前的视觉位姿与真实位姿之间的差值，也可以表示定位位姿的精度。例如定位误差可以为5cm、10cm等。定位误差越大，定位精度越小，定位误差越小，定位精度越大。
步骤S420中确定映射误差时采用的映射方式，应与在建立映射误差与定位误差之间的对应关系时采用相同的映射方式。
S450：获取IMU在第一时刻采集的IMU数据，并基于IMU数据，推算车辆在第一时刻的第一IMU位姿。
S460：获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿。其中，多个第二时刻为第一时刻之前的时刻。
本实施例中，步骤S450、S460分别与S120、S130相同，具体说明可以参见前述内容。
S470：获取各个第二视觉位姿的定位精度，根据预设的定位精度与权重的对应关系，确定第一视觉位姿的定位精度对应的权重，以及各个第二视觉位姿的定位精度对应的权重。
在获取各个第二视觉位姿的定位精度时，可以从存储的各个视觉位姿对应的定位精度中获取。
其中，预设的定位精度与权重的对应关系可以包括但不限于以下形式：
W＝1-1/G
其中，W为权重，G为定位精度。对上述对应关系W＝1-1/G进行适当的参数修改，例如增加预设系数等，可以得到其他形式的对应关系。
定位精度越大时，权重越大；定位精度越小时，权重越小。当定位精度非常小，即视觉定位失效时，权重可以为0。
S480：根据第一轨迹中各个位置点对应的权重，第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变估计融合位姿的取值，对相似约束优化函数进行迭代求解，将相似约束优化函数取最优化解时的估计融合位姿，确定为车辆在第一时刻融合后的定位位姿。
其中，第一轨迹为第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为第一IMU位姿和多个第二IMU位姿对应的轨迹。
权重越大，在定位融合时视觉位姿所起的作用越大；权重越小，在定位融合时视觉位姿所起的作用越小。权重可以对融合定位进行灵活的调节。
综上，本实施例可以根据视觉位姿的定位精度为视觉位姿赋予权重，当定位精度高时视觉位姿的权重相对较大，定位精度低时视觉位姿的权重相对较小，这样能够提高融合时的合理性，进而提高融合位姿的准确性。
在本发明的另一实施例中，基于图4所示实施例，S440，根据预先建立的目标地图区域中映射误差与定位误差之间的对应关系，确定所述映射误差对应的定位误差的步骤，包括：
将映射误差cost代入以下预先建立的目标地图区域中的映射误差函数g0，求解得到多个定位误差(Δx，Δy)：
g0(Δx，Δy)＝a0Δx2+b0ΔxΔy+c0Δy2+d0Δx+e0Δy+f0
其中，a0、b0、c0、d0、e0、f0为预先确定的函数系数；
将求解得到的多个定位误差中的最大值确定为与映射误差对应的定位误差r：
Figure BDA0002144999770000141
其中，
Figure BDA0002144999770000142
且
Figure BDA0002144999770000143
Figure BDA0002144999770000144
C＝2(a0e0 2+c0d0 2+(f0-cost)b0 2-2b0d0e0-a0c0(f0-cost))。
本实施例中，不同地图区域对应的映射误差函数的表达形式不同，具体可以是函数系数不同。上述映射误差函数g0(Δx，Δy)＝a0Δx2+b0ΔxΔy+c0Δy2+d0Δx+e0Δy+f0为抛物面，映射误差cost可以理解为平面，将映射误差cost代入映射误差函数g0，即是求抛物面与平面的交点。根据数学知识可知，该交点为椭圆，椭圆上的点都是求解得到的定位误差(Δx,Δy)。而求解得到的多个定位误差中的最大值即为椭圆的长轴和短轴(xerr和yerr)。
综上，本实施例提供了根据映射误差函数确定该映射误差对应的定位误差的具体实施方式，本方法在实际应用中更易于实施。
在本发明的另一实施例中，基于图4所示实施例中，可以采用以下步骤1c～4c建立目标地图区域中映射误差与定位误差之间的对应关系。
步骤1c：获取在目标地图区域中采集的样本道路图像和对应的样本道路特征，以及样本道路图像对应的车辆的标准定位位姿，获取预设地图中与样本道路特征匹配成功的第三道路特征。
其中，上述标准定位位姿为相机模块采集样本道路图像时确定的车辆的定位位姿，标准定位位姿可以理解为不存在定位误差的定位位姿。
步骤2c：对标准定位位姿增加多个不同的扰动量，得到多个扰动定位位姿。扰动定位位姿可以理解为以标准定位位姿为基准得到的车辆的虚拟定位位姿。
步骤3c：根据样本道路特征和第三道路特征，确定多个扰动定位位姿对应的扰动映射误差。
针对不同的扰动定位位姿，可以根据步骤2a中提到的映射方式，将样本道路特征和第三道路特征映射到同一坐标系中后确定扰动映射误差。本步骤可以包括以下实施方式；
针对每个扰动定位位姿，根据该扰动定位位姿，以及样本道路特征在样本道路图像中的位置，计算样本道路特征映射至预设地图中的第三映射位置，计算第三映射位置与第三道路特征在预设地图中的位置之间的误差，得到扰动映射误差；或者，
针对每个扰动定位位姿，根据该扰动定位位姿，以及第三道路特征在预设地图中的位置，计算第三道路特征映射至样本道路图像所在坐标系中的第四映射位置，计算第四映射位置与样本道路特征在样本道路图像中的位置之间的误差，得到扰动映射误差。
当已知道路图像中的道路特征和预设地图中匹配成功的道路特征，以及对应的定位位姿时，映射误差match_err可以采用以下函数表示：
match_err＝MapMatching(ppose，Iseg，Imap)
其中，ppose为定位位姿，Iseg为道路图像中的道路特征，Imap为预设地图中匹配成功的道路特征。
步骤4c：基于预先设定的目标地图区域中的与定位误差相关的映射误差函数，求解映射误差函数与多个扰动定位位姿对应的扰动映射误差之间的残差取最小值时的映射误差函数，得到目标地图区域中映射误差与定位误差之间的函数关系。
其中，预先设定的目标地图区域中的与定位误差相关的映射误差函数，可以理解为预设的包含未知量的映射误差函数。例如，可以将映射误差函数设置为以下二次曲面形式：
g(Δx，Δy)＝aΔx2+bΔxΔy+cΔy2+dΔx+eΔy+f
多个扰动定位位姿对应的扰动映射误差可以采用以下函数表示：
match_err＝MapMatching(pgt+Δp，Iseg，Imap)
本步骤在具体实施时可以包括：
求解以下最小值函数
Figure BDA0002144999770000161
得到a0、b0、c0、d0、e0和f0，将求解得到的a0、b0、c0、d0、e0和f0代入g后的函数g0作为映射误差函数。在标准定位位姿足够精确的情况下，求解得到的g0应为抛物面。
其中，映射误差函数为g(Δx，Δy)，g(Δx，Δy)＝aΔx2+bΔxΔy+cΔy2+dΔx+eΔy+f；pgt为标准定位位姿，扰动量为Δp＝{Δx，Δy，0}，Δx，Δy∈Ω，Ω为目标地图区域，Iseg为样本道路特征，Imap为第三道路特征；MapMatching(pgt+Δp，Iseg，Imap)为多个扰动定位位姿pgt+Δp对应的扰动映射误差。g(Δx，Δy)-MapMatching(pgt+Δp，Iseg，Imap)表示映射误差函数与多个扰动定位位姿对应的扰动映射误差之间的残差。
Figure BDA0002144999770000171
代表以a，b，c，d，e，f为待求解量的最小值函数。||·||为范数符号。
针对预设地图中的每个地图区域，均可以采用上述方式求解得到对应的映射误差函数g。
综上，本实施例中，在建立映射误差与定位误差之间的对应关系时，首先得到一个图像帧对应的样本道路特征和预设地图中匹配成功的道路特征，以及该图像帧对应的标准定位位姿，在该标准定位位姿的基础上增加多个扰动量，基于建立的残差函数，求解得到该地图区域中的对应关系。这样能够更快速地建立不同地图区域中的对应关系，也为确定车辆的定位误差提供了可实施的方式。
在本发明的另一实施例中，基于图4所示实施例，步骤S480中根据第一轨迹中各个位置点对应的权重，第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数的步骤，包括：
根据第一轨迹中各个位置点对应的权重，第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：
Figure BDA0002144999770000172
其中，S(ti)和Rs(ti)分别为第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，Ff(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，Pv(ti)和Rv(ti)分别为第一轨迹中ti时刻位置点的位置和姿态，Po(ti)和Ro(ti)分别为第二轨迹中ti时刻位置点的位置和姿态，Wi为第一轨迹中ti时刻位置点的权重，t1为第一时刻，tn为多个第二时刻中的最早时刻，n为第一时刻和多个第二时刻的总数量。上述相似约束优化函数中每一项表示的约束含义同步骤S141中的说明。
迭代求解上述相似约束优化函数得到最优解，进而确定融合后的定位位姿的过程可以参见图3所示实施例的说明。
综上，本实施例给出了针对视觉位姿添加权重后，构建相似约束优化函数的具体实施方式，能够确定两种位姿更准确的融合结果。
在本发明的另一实施例中，基于图1所示实施例，步骤S120中基于IMU数据，推算车辆在第一时刻的第一IMU位姿的步骤，具体包括以下步骤1d～2d。
步骤1d：获取第一时刻的上一时刻车辆的上一定位位姿，获取根据车轮速检测设备采集的数据确定的在该上一时刻车辆的上一行驶速度。
其中，上一定位位姿为上一时刻车辆的定位位姿，该定位位姿可以为融合后的位姿。上一行驶速度为上一时刻车辆的行驶速度。上一时刻可以包括第一时刻之前、与第一时刻最接近的定位时刻。
步骤2d：基于上一定位位姿、上一行驶速度和IMU数据，推算车辆在第一时刻的第一IMU位姿。
具体的，基于上一定位位姿、上一行驶速度和该IMU数据，采用以下公式推算车辆在第一时刻的第一IMU位姿：
P0(t1)＝P0(t2)+R0(t2)·v(t2)(t1-t2)
R0(t1)＝(t1-t2)·R0(t2)·Rz(ω(t1))·Ry(ω(t1))·Rx(ω(t1))
其中，
Figure BDA0002144999770000181
Figure BDA0002144999770000182
v(t2)为上一行驶速度，P0(t2)和R0(t2)分别为上一定位位姿中的位置和姿态，P0(t1)和R0(t1)分别为第一IMU位姿中的位置和姿态，ω(t1)为IMU数据中的角速度，x、y、z为IMU所在坐标系的坐标轴，也可以为车辆所在坐标系，t1为第一时刻，t2为上一时刻。
在确定上一行驶速度时，也可以采用IMU数据中的加速度通过积分得到。
综上，本实施例根据车轮速检测设备采集的车辆的行驶速度推算IMU位姿，相比于直接使用IMU数据中的加速度确定行驶速度，能够消除行驶速度的累计误差，提高IMU位姿的准确性。
图5为本发明实施例提供的车载终端的一种结构示意图。该实施例与图1所示实施例相对应。该车载终端包括：处理器510、相机设备520和IMU530；处理器510包括：视觉位姿确定模块、IMU位姿确定模块、历史位姿获取模块和融合位姿确定模块(图中未示出)。
视觉位姿确定模块，用于获取相机设备520在第一时刻采集的道路图像，并基于道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在第一时刻的第一视觉位姿；
IMU位姿确定模块，用于获取IMU530在第一时刻采集的IMU数据，并基于IMU数据，推算车辆在第一时刻的第一IMU位姿；
历史位姿获取模块，用于获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿；其中，多个第二时刻为第一时刻之前的时刻；
融合位姿确定模块，用于根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变估计融合位姿的取值，对相似约束优化函数进行迭代求解，将相似约束优化函数取最优化解时的估计融合位姿，确定为车辆在第一时刻融合后的定位位姿；
其中，第一轨迹为第一视觉位姿和多个第二视觉位姿对应的轨迹，第二轨迹为第一IMU位姿和多个第二IMU位姿对应的轨迹。
在本发明的另一实施例中，基于图5所示实施例，融合位姿确定模块，具体用于：
根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：
Figure BDA0002144999770000201
其中，S(ti)和Rs(ti)分别为第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，Ff(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，Pv(ti)和Rv(ti)分别为第一轨迹中ti时刻位置点的位置和姿态，Po(ti)和Ro(ti)分别为第二轨迹中ti时刻位置点的位置和姿态，t1为第一时刻，tn为多个第二时刻中的最早时刻，n为第一时刻和多个第二时刻的总数量；
确定估计融合位姿的初始值，确定所述Rf，Pf，S，Rs的初始值；
根据估计融合位姿的当前取值和Rf，Pf，S，Rs的当前取值，确定相似约束优化函数的当前函数值；
获取相似约束优化函数的上一函数值，判断上一函数值与当前函数值的差值绝对值是否大于预设差值阈值；
如果是，则调整估计融合位姿的取值以及Rf，Pf，S，Rs的取值，返回执行根据估计融合位姿的当前取值和Rf，Pf，S，Rs的当前取值，确定相似约束优化函数的当前函数值的操作；
如果否，则将估计融合位姿的当前取值，确定为车辆在第一时刻融合后的定位位姿。
在本发明的另一实施例中，基于图5所示实施例，处理器510还包括：第二时刻确定模块(图中未示出)，用于采用以下操作确定各个第二时刻：
将早于第一时刻的多个时刻作为待选择时刻，确定从各个待选择时刻到第一时刻车辆的行驶距离；
当车辆的行驶距离满足预设距离条件时，将车辆的行驶距离对应的待选择时刻确定为时刻最早的第二时刻tn，将(t1，tn]区间中的各个时刻作为各个第二时刻。
在本发明的另一实施例中，基于图5所示实施例，处理器510还包括：
映射误差确定模块(图中未示出)，用于在确定在第一时刻的第一视觉位姿之后，根据第一视觉位姿，确定第一道路特征与第二道路特征之间的映射误差；其中，第一道路特征为道路图像中的道路特征，第二道路特征为预设地图中与第一道路特征匹配成功的道路特征；
目标区域确定模块(图中未示出)，用于从预设地图包含的多个不同地图区域中，确定第一视觉位姿所在的目标地图区域；
定位精度确定模块(图中未示出)，用于根据预先建立的目标地图区域中映射误差与定位误差之间的对应关系，确定映射误差对应的定位误差，作为第一视觉位姿的定位精度；
融合位姿确定模块，根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数时，包括：
获取各个第二视觉位姿的定位精度；
根据预设的定位精度与权重的对应关系，确定第一视觉位姿的定位精度对应的权重，以及各个第二视觉位姿的定位精度对应的权重；
根据第一轨迹中各个位置点对应的权重，第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数。
在本发明的另一实施例中，基于图5所示实施例，定位精度确定模块，具体用于：
将映射误差cost代入以下预先建立的目标地图区域中的映射误差函数g0，求解得到多个定位误差(Δx，Δy)：
g0(Δx，Δy)＝a0Δx2+b0ΔxΔy+c0Δy2+d0Δx+e0Δy+f0
其中，a0、b0、c0、d0、e0、f0为预先确定的函数系数；
将求解得到的多个定位误差中的最大值确定为与映射误差对应的定位误差r：
Figure BDA0002144999770000221
其中，
Figure BDA0002144999770000222
且
Figure BDA0002144999770000223
Figure BDA0002144999770000224
C＝2(a0e0 2+c0d0 2+(f0-cost)b0 2-2b0d0e0-a0c0(f0-cost))。
在本发明的另一实施例中，基于图5所示实施例，融合位姿确定模块，根据第一轨迹中各个位置点对应的权重，第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数时，包括：
根据第一轨迹中各个位置点对应的权重，第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及第一轨迹和第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：
Figure BDA0002144999770000225
其中，S(ti)和Rs(ti)分别为第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，Ff(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，Pv(ti)和Rv(ti)分别为第一轨迹中ti时刻位置点的位置和姿态，Po(ti)和Ro(ti)分别为第二轨迹中ti时刻位置点的位置和姿态，Wi为第一轨迹中ti时刻位置点的权重，t1为第一时刻，tn为多个第二时刻中的最早时刻，n为第一时刻和多个第二时刻的总数量。
在本发明的另一实施例中，基于图5所示实施例，IMU位姿确定模块，基于IMU数据，推算车辆在第一时刻的第一IMU位姿时，包括：
获取第一时刻的上一时刻车辆的上一定位位姿，获取根据车轮速检测设备采集的数据确定的在上一时刻车辆的上一行驶速度；
基于上一定位位姿、上一行驶速度和IMU数据，推算车辆在第一时刻的第一IMU位姿。
该终端实施例与图1所示方法实施例是基于同一发明构思得到的实施例，相关之处可以相互参照。上述终端实施例与方法实施例相对应，与该方法实施例具有同样的技术效果，具体说明参见方法实施例。
本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
本领域普通技术人员可以理解：实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。

Claims (8)

1.一种基于视觉和IMU的车辆融合定位方法，其特征在于，包括：
获取相机设备在第一时刻采集的道路图像，并基于所述道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在所述第一时刻的第一视觉位姿；
获取惯性测量单元IMU在第一时刻采集的IMU数据，并基于所述IMU数据，推算车辆在所述第一时刻的第一IMU位姿；
获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿；其中，多个第二时刻为所述第一时刻之前的时刻；
根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中同一时刻的各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿；
其中，所述第一轨迹为所述第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为所述第一IMU位姿和多个第二IMU位姿对应的轨迹；
所述根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿的步骤，包括：
根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：
Figure FDA0004005399770000021
其中，所述S(ti)和Rs(ti)分别为所述第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，所述Pf(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，所述Pv(ti)和Rv(ti)分别为所述第一轨迹中ti时刻位置点的位置和姿态，所述Po(ti)和Ro(ti)分别为所述第二轨迹中ti时刻位置点的位置和姿态，所述t1为所述第一时刻，所述tn为多个第二时刻中的最早时刻，所述n为所述第一时刻和多个第二时刻的总数量；
确定所述估计融合位姿的初始值，确定所述Rf,Pf,S,Rs的初始值；
根据所述估计融合位姿的当前取值和所述Rf,Pf,S,Rs的当前取值，确定所述相似约束优化函数的当前函数值；
获取所述相似约束优化函数的上一函数值，判断所述上一函数值与所述当前函数值的差值绝对值是否大于预设差值阈值；
如果是，则调整所述估计融合位姿的取值以及所述Rf,Pf,S,Rs的取值，返回执行所述根据所述估计融合位姿的当前取值和所述Rf,Pf,S,Rs的当前取值，确定所述相似约束优化函数的当前函数值的步骤；
如果否，则将所述估计融合位姿的当前取值，确定为所述车辆在所述第一时刻融合后的定位位姿。
2.如权利要求1所述的方法，其特征在于，采用以下方式确定各个第二时刻：
将早于所述第一时刻的多个时刻作为待选择时刻，确定从各个待选择时刻到所述第一时刻所述车辆的行驶距离；
当所述车辆的行驶距离满足预设距离条件时，将所述车辆的行驶距离对应的待选择时刻确定为时刻最早的第二时刻tn，将(t1，tn]区间中的各个时刻作为各个第二时刻。
3.如权利要求1所述的方法，其特征在于，在确定在所述第一时刻的第一视觉位姿之后，还包括：
根据所述第一视觉位姿，确定第一道路特征与第二道路特征之间的映射误差；其中，所述第一道路特征为所述道路图像中的道路特征，所述第二道路特征为所述预设地图中与所述第一道路特征匹配成功的道路特征；
从所述预设地图包含的多个不同地图区域中，确定所述第一视觉位姿所在的目标地图区域；
根据预先建立的目标地图区域中映射误差与定位误差之间的对应关系，确定所述映射误差对应的定位误差，作为所述第一视觉位姿的定位精度；
所述根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数的步骤，包括：
获取各个第二视觉位姿的定位精度；
根据预设的定位精度与权重的对应关系，确定所述第一视觉位姿的定位精度对应的权重，以及各个第二视觉位姿的定位精度对应的权重；
根据第一轨迹中各个位置点对应的权重，所述第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数。
4.如权利要求3所述的方法，其特征在于，所述根据预先建立的目标地图区域中映射误差与定位误差之间的对应关系，确定所述映射误差对应的定位误差的步骤，包括：
将所述映射误差cost代入以下预先建立的目标地图区域中的映射误差函数g0，求解得到多个定位误差(Δx,Δy)：
g0(Δx,Δy)＝a0Δx2+b0ΔxΔy+c0Δy2+d0Δx+e0Δy+f0
其中，所述a0、b0、c0、d0、e0、f0为预先确定的函数系数；
将求解得到的多个定位误差中的最大值确定为与所述映射误差对应的定位误差r：
Figure FDA0004005399770000041
/>
其中，
Figure FDA0004005399770000042
且
Figure FDA0004005399770000043
Figure FDA0004005399770000044
C＝2(a0e0 2+c0d0 2+(f0-cost)b0 2-2b0d0e0-a0c0(f0-cost))。
5.如权利要求3所述的方法，其特征在于，所述根据第一轨迹中各个位置点对应的权重，所述第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数的步骤，包括：
根据第一轨迹中各个位置点对应的权重，所述第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E’：
Figure FDA0004005399770000045
其中，所述S(ti)和Rs(ti)分别为所述第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，所述Pf(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，所述Pv(ti)和Rv(ti)分别为所述第一轨迹中ti时刻位置点的位置和姿态，所述Po(ti)和Ro(ti)分别为所述第二轨迹中ti时刻位置点的位置和姿态，W(ti)为所述第一轨迹中ti时刻位置点的权重，t1为所述第一时刻，tn为多个第二时刻中的最早时刻，所述n为所述第一时刻和多个第二时刻的总数量。
6.如权利要求1所述的方法，其特征在于，所述基于所述IMU数据，推算所述车辆在所述第一时刻的第一IMU位姿的步骤，包括：
获取所述第一时刻的上一时刻所述车辆的上一定位位姿，获取根据车轮速检测设备采集的数据确定的在所述上一时刻所述车辆的上一行驶速度；
基于所述上一定位位姿、所述上一行驶速度和所述IMU数据，推算所述车辆在所述第一时刻的第一IMU位姿。
7.一种车载终端，其特征在于，包括：处理器、相机设备和惯性测量单元IMU；所述处理器包括：视觉位姿确定模块、IMU位姿确定模块、历史位姿获取模块和融合位姿确定模块；
视觉位姿确定模块，用于获取相机设备在第一时刻采集的道路图像，并基于所述道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在所述第一时刻的第一视觉位姿；
IMU位姿确定模块，用于获取IMU在第一时刻采集的IMU数据，并基于所述IMU数据，推算车辆在所述第一时刻的第一IMU位姿；
历史位姿获取模块，用于获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿；其中，多个第二时刻为所述第一时刻之前的时刻；
融合位姿确定模块，用于根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中同一时刻的各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿；
其中，所述第一轨迹为所述第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为所述第一IMU位姿和多个第二IMU位姿对应的轨迹；
所述融合位姿确定模块，具体用于：
根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：
Figure FDA0004005399770000061
其中，所述S(ti)和Rs(ti)分别为所述第一轨迹与第二轨迹中ti时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，所述Pf(ti)和Rf(ti)分别为ti时刻的估计融合位姿中的位置和姿态，所述Pv(ti)和Rv(ti)分别为所述第一轨迹中ti时刻位置点的位置和姿态，所述Po(ti)和Ro(ti)分别为所述第二轨迹中ti时刻位置点的位置和姿态，所述t1为所述第一时刻，所述tn为多个第二时刻中的最早时刻，所述n为所述第一时刻和多个第二时刻的总数量；
确定所述估计融合位姿的初始值，确定所述Rf,Pf,S,Rs的初始值；
根据所述估计融合位姿的当前取值和所述Rf,Pf,S,Rs的当前取值，确定所述相似约束优化函数的当前函数值；
获取所述相似约束优化函数的上一函数值，判断所述上一函数值与所述当前函数值的差值绝对值是否大于预设差值阈值；
如果是，则调整所述估计融合位姿的取值以及所述Rf,Pf,S,Rs的取值，返回执行所述根据所述估计融合位姿的当前取值和所述Rf,Pf,S,Rs的当前取值，确定所述相似约束优化函数的当前函数值的操作；
如果否，则将所述估计融合位姿的当前取值，确定为所述车辆在所述第一时刻融合后的定位位姿。
8.如权利要求7所述的终端，其特征在于，所述处理器还包括：
映射误差确定模块，用于在确定在所述第一时刻的第一视觉位姿之后，根据所述第一视觉位姿，确定第一道路特征与第二道路特征之间的映射误差；其中，所述第一道路特征为所述道路图像中的道路特征，所述第二道路特征为所述预设地图中与所述第一道路特征匹配成功的道路特征；
目标区域确定模块，用于从所述预设地图包含的多个不同地图区域中，确定所述第一视觉位姿所在的目标地图区域；
定位精度确定模块，用于根据预先建立的目标地图区域中映射误差与定位误差之间的对应关系，确定所述映射误差对应的定位误差，作为所述第一视觉位姿的定位精度；
所述融合位姿确定模块，根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数时，包括：
获取各个第二视觉位姿的定位精度；
根据预设的定位精度与权重的对应关系，确定所述第一视觉位姿的定位精度对应的权重，以及各个第二视觉位姿的定位精度对应的权重；
根据第一轨迹中各个位置点对应的权重，所述第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数。
CN201910681732.4A 2019-07-26 2019-07-26 一种基于视觉和imu的车辆融合定位方法及车载终端 Active CN112304321B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910681732.4A CN112304321B (zh) 2019-07-26 2019-07-26 一种基于视觉和imu的车辆融合定位方法及车载终端

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910681732.4A CN112304321B (zh) 2019-07-26 2019-07-26 一种基于视觉和imu的车辆融合定位方法及车载终端

Publications (2)

Publication Number Publication Date
CN112304321A CN112304321A (zh) 2021-02-02
CN112304321B true CN112304321B (zh) 2023-03-28

Family

ID=74328830

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910681732.4A Active CN112304321B (zh) 2019-07-26 2019-07-26 一种基于视觉和imu的车辆融合定位方法及车载终端

Country Status (1)

Country Link
CN (1) CN112304321B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113554712A (zh) * 2021-06-29 2021-10-26 北京百度网讯科技有限公司 自动驾驶车辆的配准方法、装置、电子设备和车辆

Family Cites Families (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8259994B1 (en) * 2010-09-14 2012-09-04 Google Inc. Using image and laser constraints to obtain consistent and improved pose estimates in vehicle pose databases
CN103942768B (zh) * 2013-01-18 2017-05-24 诺基亚技术有限公司 图像融合的方法和装置
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
US10510160B2 (en) * 2016-12-20 2019-12-17 Samsung Electronics Co., Ltd. Multiscale weighted matching and sensor fusion for dynamic vision sensor tracking
CN108492316A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 一种终端的定位方法和装置
US10311833B1 (en) * 2018-03-27 2019-06-04 Seiko Epson Corporation Head-mounted display device and method of operating a display apparatus tracking an object
CN108717712B (zh) * 2018-05-29 2021-09-03 东北大学 一种基于地平面假设的视觉惯导slam方法
CN109029433B (zh) * 2018-06-28 2020-12-11 东南大学 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN109166149B (zh) * 2018-08-13 2021-04-02 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109341706B (zh) * 2018-10-17 2020-07-03 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109509230B (zh) * 2018-11-13 2020-06-23 武汉大学 一种应用于多镜头组合式全景相机的slam方法
CN109993113B (zh) * 2019-03-29 2023-05-02 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Also Published As

Publication number Publication date
CN112304321A (zh) 2021-02-02

Similar Documents

Publication Publication Date Title
CN112304302B (zh) 一种多场景高精度车辆定位方法、装置及车载终端
CN109341706B (zh) 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
Suhr et al. Sensor fusion-based low-cost vehicle localization system for complex urban environments
CN110440801B (zh) 一种定位感知信息获取方法、装置及系统
CN112307810B (zh) 一种视觉定位效果自检方法及车载终端
KR20200119920A (ko) 자동 발렛 파킹 시스템의 위치 추정 장치 및 방법
CN111524169A (zh) 用神经网络基于传感器数据和地图数据的图像配准的定位
Wu et al. Robust LiDAR-based localization scheme for unmanned ground vehicle via multisensor fusion
CN113252051A (zh) 一种地图构建方法及装置
CN113252022A (zh) 一种地图数据处理方法及装置
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN115792894A (zh) 一种多传感器融合车辆目标跟踪方法、系统及存储介质
CN113405555B (zh) 一种自动驾驶的定位传感方法、系统及装置
CN111829514A (zh) 一种适用于车辆底盘集成控制的路面工况预瞄方法
Hara et al. Vehicle localization based on the detection of line segments from multi-camera images
CN112304321B (zh) 一种基于视觉和imu的车辆融合定位方法及车载终端
CN112446915A (zh) 一种基于图像组的建图方法及装置
CN112304322B (zh) 一种视觉定位失效后的重启方法及车载终端
CN113312403B (zh) 地图获取方法、装置、电子设备及存储介质
CN112284399B (zh) 一种基于视觉和imu的车辆定位方法及车载终端
CN114049767B (zh) 一种边缘计算方法、装置及可读存储介质
CN115900732A (zh) 基于路侧相机和车载单元的组合导航方法及系统
WO2022116572A1 (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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20220304

Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing

Applicant after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd.

Address before: 100083 room 28, 4 / F, block a, Dongsheng building, 8 Zhongguancun East Road, Haidian District, Beijing

Applicant before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant