CN116907485A - 建图方法、装置、电子设备和存储介质 - Google Patents

建图方法、装置、电子设备和存储介质 Download PDF

Info

Publication number
CN116907485A
CN116907485A CN202310877156.7A CN202310877156A CN116907485A CN 116907485 A CN116907485 A CN 116907485A CN 202310877156 A CN202310877156 A CN 202310877156A CN 116907485 A CN116907485 A CN 116907485A
Authority
CN
China
Prior art keywords
initialization
measurement unit
inertial measurement
gyroscope
accelerometer
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.)
Pending
Application number
CN202310877156.7A
Other languages
English (en)
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.)
Uisee Technologies Beijing Co Ltd
Original Assignee
Uisee Technologies Beijing 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 Uisee Technologies Beijing Co Ltd filed Critical Uisee Technologies Beijing Co Ltd
Priority to CN202310877156.7A priority Critical patent/CN116907485A/zh
Publication of CN116907485A publication Critical patent/CN116907485A/zh
Pending legal-status Critical Current

Links

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/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
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • 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/20—Instruments for performing navigational calculations
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data

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)

Abstract

本公开实施例公开了一种建图方法、装置、电子设备和存储介质，该方法包括：获取图像数据，根据图像数据中的特征数据建立初始视觉地图，并基于初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、关键帧集合中各关键帧位姿以及尺度恢复地图；基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；在惯性测量单元的数据至少部分满足初始化条件的情况下，对惯性测量单元进行初始化，确定惯性测量单元的初始化结果；根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图。本公开在惯性测量单元中部分初始化成功的情况下就加入建图，并且对初始化结果进行验证，提高初始化的有效性，进而，提高建图的效率以及准确性。

Description

建图方法、装置、电子设备和存储介质
技术领域
本公开涉及自动驾驶技术领域，尤其涉及一种建图方法、装置、电子设备和存储介质。
背景技术
建图与定位是自动驾驶中的关键技术之一。对于无人车来说，通常需要对周围环境进行建图，然后在建好的地图中实现定位功能。目前，主流的单目视觉方案存在无法确定尺度信息且不能应对快速移动的问题，因此，可以引入惯性测量单元(InertialMeasurement Unit，imu)，这样可以结合视觉及惯性测量单元的优点，取长补短。
然而，针对imu来说，在初始化阶段，并不一定有足够的运动对其进行激励，会导致多次无效初始化。并且，imu中的陀螺仪或加速度计中的任意一个初始化失败，就会导致imu信息均无法使用的问题，进而，导致建图***长时间等待imu初始化成功，影响建图效率以及效果，而且，未对imu初始化的结果进行验证，无法保证imu的实际可用性。
发明内容
为了解决上述技术问题或者至少部分地解决上述技术问题，本公开实施例提供了一种建图方法、装置、电子设备和存储介质，以在惯性测量单元中的加速度计和陀螺仪中至少一个初始化成功的情况下，加入建图，提高建图的效率以及准确性。
第一方面，本公开实施例提供了一种建图方法，该方法包括：
获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
第二方面，本公开实施例还提供了一种建图装置，该装置包括：
第一建图模块，用于获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
初始化条件判断模块，用于基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
初始化结果确定模块，用于在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
第二建图模块，用于根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
第三方面，本公开实施例还提供了一种电子设备，所述电子设备包括：一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上所述的建图方法。
第四方面，本公开实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上所述的建图方法。
本公开实施例提供的建图方法，通过根据图像数据中的特征数据建立初始视觉地图，并基于初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、关键帧集合中各关键帧位姿以及尺度恢复地图，进而，基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件，以判断加速度计和陀螺仪能否进入初始化流程，在惯性测量单元的数据至少部分满足初始化条件的情况下，对惯性测量单元进行初始化，确定惯性测量单元的初始化结果，以判断初始化后的加速度计或陀螺仪能否用于建图，根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图，实现了在惯性测量单元中的加速度计和陀螺仪中至少一个初始化成功的情况下，加入建图，并且对初始化结果进行验证，提高初始化的有效性，进而，提高建图的效率以及准确性。
附图说明
结合附图并参考以下具体实施方式，本公开各实施例的上述和其他特征、优点及方面将变得更加明显。贯穿附图中，相同或相似的附图标记表示相同或相似的元素。应当理解附图是示意性的，原件和元素不一定按照比例绘制。
图1为本公开实施例中的一种建图方法的流程图；
图2为本公开实施例中的一种建图装置的结构示意图；
图3为本公开实施例中的一种电子设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的实施例。虽然附图中显示了本公开的某些实施例，然而应当理解的是，本公开可以通过各种形式来实现，而且不应该被解释为限于这里阐述的实施例，相反提供这些实施例是为了更加透彻和完整地理解本公开。应当理解的是，本公开的附图及实施例仅用于示例性作用，并非用于限制本公开的保护范围。
需要注意，本公开中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分，并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。
本公开实施方式中的多个装置之间所交互的消息或者信息的名称仅用于说明性的目的，而并不是用于对这些消息或信息的范围进行限制。
常用的建图技术在***初始化阶段，并不一定有足够的运动对惯性测量单元进行激励，无法对运动进行估计和判断，可能导致多次进行无效的初始化。并且，没有对惯性测量单元中的不同传感器进行区分，存在因陀螺仪或加速度计其中一个初始化失败，导致二者信息都无法使用的情况。在***初始化后，没有对初始化求解得到的加速度计和陀螺仪偏置结果进行验证，直接投入使用，存在建图准确性差的问题，后续***运行中的鲁棒性无法得到保证。
针对上述问题，本公开实施例提供了一种建图方法，在惯性测量单元中的加速度计和陀螺仪中至少一个初始化成功的情况下，就能够加入建图，提高建图的效率以及准确性。
图1为本公开实施例中的一种建图方法的流程图。该方法可以由建图装置执行，该装置可以采用软件和/或硬件的方式实现，该装置可配置于电子设备中。如图1所示，该方法具体可以包括如下步骤：
S110、获取图像数据，根据图像数据中的特征数据建立初始视觉地图，并基于初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、关键帧集合中各关键帧位姿以及尺度恢复地图。
其中，图像数据为车辆行驶过程中采集的一系列图像数据。特征数据是图像数据中的图像特征。初始视觉地图是基于SFM(Structure from Motion，二维运动图像中的三维重建)算法构建出的视觉地图。轮速计信息是轮速计同步采集的信息，方向盘转角信息是同步采集得到的方向盘的转角。关键帧集合由多个关键帧组成。关键帧位姿是关键帧集合中各关键帧的位置和姿态。尺度恢复地图是通过轮速计信息以及方向盘转角信息对初始视觉地图进行尺度恢复处理得到的地图。
具体的，获取图像数据，对图像数据中提取的特征信息使用对极几何及PNP(Perspective-n-Point，求解3D到2D点对运动的方法)等算法建立初始视觉地图。通过初始视觉地图结合里程计中的轮速计信息和方向盘转角信息进一步恢复地图尺度，并得到真实尺度下的关键帧集合、关键帧集合中各关键帧位姿以及尺度恢复地图。
S120、基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件。
其中，惯性测量单元的数据包括加速度计数据和陀螺仪数据，初始化条件包括加速度计初始化条件以及陀螺仪初始化条件，加速度计初始化条件用于判断加速度计能否进入后续初始化流程，陀螺仪初始化条件用于判断陀螺仪能否进入后续初始化流程。
具体的，根据各关键帧的位姿，分别判断加速度计数据是否满足加速度计初始化条件以及陀螺仪数据是否满足陀螺仪初始化条件，以确定加速度计和陀螺仪是否能够进入后续的初始化流程，提高后续初始化成功的概率，避免大量的无效初始化。
在上述示例的基础上，可以通过下述方式来基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件：
基于各关键帧位姿中的旋转姿态以及预先设置的旋转阈值，判断陀螺仪数据是否满足陀螺仪初始化条件；
基于各关键帧位姿中的位置、各关键帧位姿对应的时间点以及预先设置的加速度阈值，判断加速度计数据是否满足加速度计初始化条件。
其中，旋转姿态是关键帧位姿中包含的姿态信息。旋转阈值是用于判断陀螺仪数据是否满足陀螺仪初始化条件的阈值。位置是关键帧位姿中包含的位置信息。时间点是各关键帧位姿对应的采集时间。加速度阈值是用于判断加速度计数据是否满足加速度计初始化条件的阈值。
具体的，根据前后两个关键帧之间各图像数据的旋转姿态的累计旋转量与旋转阈值进行比较。若大于，则确定陀螺仪数据满足陀螺仪初始化条件，否则，确定陀螺仪数据不满足陀螺仪初始化条件。根据前后两个关键帧之间各图像数据的位置和相应的时间点，可以确定各速度，进而，根据各速度和相应时间点，可以确定累计加速度值，将累计加速度值与加速度阈值进行比较。若大于，则确定加速度计数据满足加速度计初始化条件，否则，确定加速度计数据不满足加速度计初始化条件。
示例性的，可以通过下述公式判断陀螺仪数据是否满足陀螺仪初始化条件：
其中，为四元数的乘法符号，/>为第i帧关键帧的旋转姿态，qt为旋转阈值。若从第i帧到第i+j帧(相邻两个关键帧)的累计旋转量变化大于旋转阈值，则认为能够对陀螺仪进行后续初始化，否则不对陀螺仪进行后续初始化。
可以通过下述公式判断加速度计数据是否满足加速度计初始化条件：
式中，vi为第i帧关键帧的速度，速度可以通过前后两关键帧位移除以移动时间(时间点的差值)得到，ti为第i帧关键帧的时间点，at为加速度阈值。若从第i帧到第i+j帧(相邻两个关键帧)的累计加速度值变化大于加速度阈值，则认为能够对加速度计进行后续初始化，否则不对加速度计进行后续初始化。
S130、在惯性测量单元的数据至少部分满足初始化条件的情况下，对惯性测量单元进行初始化，确定惯性测量单元的初始化结果。
其中，至少部分满足包括全部满足以及部分满足。惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功。
可选的，惯性测量单元的初始化结果为初始化部分成功包括：加速度计初始化成功且陀螺仪初始化失败，加速度计初始化成功且陀螺仪数据不满足陀螺仪初始化条件，加速度计数据不满足加速度计初始化条件且陀螺仪初始化成功，或者，加速度计初始化失败且陀螺仪初始化成功；惯性测量单元的初始化结果为初始化失败包括：加速度计初始化失败且陀螺仪初始化失败，加速度计初始化失败且陀螺仪数据不满足陀螺仪初始化条件，或者，加速度计数据不满足加速度计初始化条件且陀螺仪初始化失败。
具体的，在惯性测量单元的数据至少部分满足初始化条件的情况下，说明加速度计和陀螺仪中的至少一个可以进行相应的初始化。在加速度计和陀螺仪中的至少一个初始化后，分析得到对应的初始化结果，以判断当前部件的初始化是否成功，能否用于后续建图。
在上述示例的基础上，可以通过下述方式来确定惯性测量单元的初始化结果：
若满足下述计算式十三，则确定加速度计的初始化结果为初始化成功，若不满足下述计算式十三，则确定加速度计的初始化结果为初始化失败；
计算式十三为：
其中，为预先标定的加速度偏置白噪声，na为加速度计的比例系数，/>为预设时间内的加速度计偏置变化量，Δt为时间间隔；
若满足下述计算式十四，则确定陀螺仪的初始化结果为初始化成功，若不满足下述计算式十四，则确定陀螺仪的初始化结果为初始化失败；
计算式十四为：
其中，为预先标定的陀螺仪的偏置白噪声，ng为陀螺仪的比例系数，/>为预设时间内的陀螺仪的偏置变化量，Δt为时间间隔；
若加速度计的初始化结果和陀螺仪的初始化结果均为初始化成功，则确定惯性测量单元的初始化结果为初始化成功；
若加速度计初始化成功且陀螺仪初始化失败，加速度计初始化成功且陀螺仪数据不满足陀螺仪初始化条件，加速度计数据不满足加速度计初始化条件且陀螺仪初始化成功，或者，加速度计初始化失败且陀螺仪初始化成功，则确定惯性测量单元的初始化结果为初始化部分成功；
若加速度计初始化失败且陀螺仪初始化失败，加速度计初始化失败且陀螺仪数据不满足陀螺仪初始化条件，或者，加速度计数据不满足加速度计初始化条件且陀螺仪初始化失败，则确定惯性测量单元的初始化结果为初始化失败。
其中，加速度偏置白噪声以及加速度计的比例系数均为预先标定的加速度计的相关参数。加速度计偏置变化量是通过初始化后的加速度计得到的偏置数据。陀螺仪的偏置白噪声以及陀螺仪的比例系数均为预先标定的陀螺仪的相关参数。陀螺仪的偏置变化量是通过初始化后的陀螺仪得到的偏置数据。
具体的，若加速度计数据满足加速度计初始化条件，对加速度计进行初始化，则根据初始化后的加速度计，可以确定预设时间内的加速度计偏置变化量，进而，可以结合加速度偏置白噪声以及加速度计的比例系数，判断是否满足计算式十三，若满足，则表明加速度计初始化成功，若不满足，则表明加速度计初始化失败。若陀螺仪数据满足陀螺仪初始化条件，对陀螺仪进行初始化，则根据初始化后的陀螺仪，可以确定预设时间内的陀螺仪偏置变化量，进而，可以结合加速度偏置白噪声以及陀螺仪的比例系数，判断是否满足计算式十四，若满足，则表明陀螺仪初始化成功，若不满足，则表明陀螺仪初始化失败。进一步的，可以根据加速度计的三种情况(不满足加速度计初始化条件，满足加速度计初始化条件且初始化成功以及满足加速度计初始化条件且初始化失败)以及陀螺仪的三种情况，确定惯性测量单元的初始化结果，在此不做赘述。
S140、根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图。
其中，目标地图是基于惯性测量单元对尺度恢复地图进行约束处理后得到的地图。
具体的，在惯性测量单元的初始化结果为初始化成功或初始化部分成功时，通过惯性测量单元的相关数据对尺度恢复地图进行约束处理后得到目标地图。在惯性测量单元的初始化结果为初始化失败时，表明惯性测量单元中的加速度计和陀螺仪当前都无法使用，无法参与建图，将尺度恢复地图确定为目标地图。
在上述示例的基础上，可以通过下述方式来根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图：
若惯性测量单元的初始化结果为初始化部分成功或初始化成功，则根据惯性测量单元的初始化参数对关键帧位姿进行约束，并对尺度恢复地图进行增量处理，得到目标地图；
若惯性测量单元的初始化结果为初始化失败，则将尺度恢复地图确定为目标地图。
其中，初始化参数是根据初始化成功的加速度计和/或陀螺仪确定的初始化后的参数，用于后续对关键帧位姿进行约束。
具体的，若惯性测量单元的初始化结果为初始化部分成功或初始化成功，则表明加速度计和陀螺仪中的至少一个初始化成功，因此，根据初始化成功的加速度计和/或陀螺仪的初始化参数对关键帧位姿进行约束，并对尺度恢复地图进行增量处理，得到目标地图。若惯性测量单元的初始化结果为初始化失败时，则表明惯性测量单元中的加速度计和陀螺仪都无法使用，无法参与建图，因此，将尺度恢复地图作为目标地图。
在上述示例的基础上，在根据惯性测量单元的初始化参数对关键帧位姿进行约束之前，还需要确定不同初始化结果下的初始化参数，具体可以是：
若惯性测量单元的初始化结果为初始化成功，则惯性测量单元的初始化参数包括陀螺仪的偏置、视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，通过下述计算式一至五确定初始化参数：
计算式一为：
其中，C1min为第一目标函数，为四元数的乘法符号，I为关键帧集合中的关键帧的总数，/>为视觉坐标系到惯性坐标系的旋转外参，/>为第i帧关键帧在视觉坐标系下的旋转姿态，/>为第i帧关键帧与第i+1帧关键帧之间的惯性旋转预积分量，/>为两帧关键帧之间预积分量对陀螺仪的偏置δbw的雅可比矩阵，δbw为陀螺仪的偏置；
计算式二为：
计算式三为：
计算式四为：
计算式五为：
rg＝‖g‖-‖gw‖
其中，C2min为第二目标函数，rp为位置残差，rv为速度残差，rg为重力向量模长残差，为惯性测量单元的位置预积分量，/>为惯性测量单元的速度预积分量，/>为位置预积分量对加速度计偏置的雅可比矩阵，/>为速度预积分量对加速度计偏置的雅可比矩阵，δba为加速度计偏置，/>为第i帧关键帧对应的惯性测量单元在视觉坐标系的旋转位姿，为第i帧关键帧对应的惯性测量单元在视觉坐标系旋转位姿的逆矩阵，/>为第i帧关键帧的相机在视觉坐标系下的位置，gw为视觉坐标系下的重力向量，/> 为惯性坐标系到相机坐标系的平移，Δti为两帧关键帧之间的时间间隔，/>为第i帧关键帧的速度，vpvg为权重系数矩阵。
需要说明的是，通过以最小值为目标求解计算式一中的第一目标函数，可以得到陀螺仪的偏置，计算式一中，其他各参数均可通过初始化后的陀螺仪确定。通过以最小值为目标求解计算式二中的第二目标函数，计算式二中的部分参数通过计算式三至五确定，通过求解，可以得到视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，其他各参数均可通过初始化后的加速度计确定。
若惯性测量单元的初始化结果为初始化部分成功，初始化部分成功为加速度计初始化成功且陀螺仪初始化失败，或者，加速度计初始化成功且陀螺仪数据不满足陀螺仪初始化条件，则惯性测量单元的初始化参数包括视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，依次通过下述计算式六至八以及计算式二至五确定初始化参数：
计算式六为：
其中，为第i帧关键帧在视觉坐标系下的旋转姿态，/>为第j帧关键帧在视觉坐标系下的旋转姿态，t为第i帧关键帧与第i+1帧关键帧之间的任意时刻，f(·)为插值函数，为第i帧关键帧与第i+1帧关键帧之间t时刻的旋转位姿；
计算式七为：
计算式八为：
其中，为惯性测量单元的位置预积分量，/>为惯性测量单元的速度预积分量，/>为惯性坐标系下的加速度，/>为惯性坐标系下t时刻相对于i时刻的旋转积分值，/>是对/>经过转换后，确定的惯性坐标系下从第i帧关键帧的位姿开始的旋转预积分量。
需要说明的是，因为陀螺仪没有进行初始化，或者初始化失败，因此不满足旋转要求，所以不使用陀螺仪数据，也无法直接得到旋转预积分量。故，通过计算式六至八使用关键帧旋转位姿插值来代替陀螺仪的旋转预积分的过程。
若惯性测量单元的初始化结果为初始化部分成功，初始化部分成功为加速度计数据不满足加速度计初始化条件且陀螺仪初始化成功，或者，加速度计初始化失败且陀螺仪初始化成功，则初始化参数包括陀螺仪的偏置，通过计算式一确定初始化参数。
需要说明的是，陀螺仪初始化成功时，关于陀螺仪的初始化参数无需加速度计相关数据的介入，因此，可以直接使用计算式一确定初始化参数。
在上述示例的基础上，可以通过下述方式来根据惯性测量单元的初始化参数对关键帧位姿进行约束：
若惯性测量单元的初始化结果为初始化部分成功，初始化部分成功为加速度计初始化成功且陀螺仪初始化失败，或者，加速度计初始化成功且陀螺仪数据不满足陀螺仪初始化条件，则通过下述计算式九至计算式十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据残差约束项以及协方差矩阵对关键帧位姿进行约束：
计算式九为：
计算式十为：
计算式十一为：
其中，为位置预积分量误差，/>为速度预积分量误差，δbat为加速度计偏置误差，/>为当前惯性测量单元旋转位姿，na为加速度高斯白噪声，/>为加速度随机游走噪声，Δt为时间间隔，·表示求导数，/>表示残差约束项，/>表示残差约束项的协方差矩阵，F和G为两个传递矩阵，/>为过程矩阵，位置预积分量误差、速度预积分量误差以及加速度计偏置误差基于惯性测量单元的初始化参数以及加速度计测量数据确定；
若惯性测量单元的初始化结果为初始化部分成功，初始化部分成功为加速度计数据不满足加速度计初始化条件且陀螺仪初始化成功，或者，加速度计初始化失败且陀螺仪初始化成功，则通过下述计算式十二、计算式十以及十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据残差约束项以及协方差矩阵对关键帧位姿进行约束：
计算式十二：
计算式十为：
计算式十一为：
其中，分别为旋转预积分量误差，δbwt为陀螺仪偏置误差，wt-bwt为角速度测量值减去偏置，/>是从向量生成反对称矩阵的运算，nw为陀螺仪高斯白噪声，nbw为陀螺仪随机游走噪声，·表示求导数，/>表示残差约束项，/>表示残差约束项的协方差矩阵，F和G为两个传递矩阵，/>为过程矩阵，旋转预积分量误差以及陀螺仪偏置误差基于惯性测量单元的初始化参数以及陀螺仪测量数据确定。
其中，加速度计测量数据是惯性测量单元中的加速度计的读数，陀螺仪测量数据是惯性测量单元中的陀螺仪的读数。
需要说明的是，计算式九和计算式十二是连续时间的误差传递方程，一阶泰勒展开可以得到计算式十，即离散时间的误差传递方程，计算式十一为协方差传递方程，递推可以得到每个时刻参与优化榆树的残差约束项和误差协方差矩阵。
还需要说明的是，若惯性测量单元的初始化结果为初始化成功，则通过下述计算式十五，前述计算式十以及十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据残差约束项以及协方差矩阵对关键帧位姿进行约束：
计算式十五为：
计算式十、计算式十一以及相关参数说明在此不做赘述，可以参考前述说明。
在上述示例的基础上，在确定目标地图之后，还可以对不满足初始化条件或者初始化失败的部件进行延迟初始化处理，具体可以包括以下几种情况：
情况一、若惯性测量单元的初始化结果为初始化部分成功，初始化部分成功为加速度计初始化成功且陀螺仪初始化失败，或者，加速度计初始化成功且陀螺仪数据不满足陀螺仪初始化条件，则对陀螺仪进行延迟初始化处理。
具体的，在情况一下，加速度计初始化成功，但是陀螺仪未经初始化或者初始化失败，因此，需要对陀螺仪进行延迟初始化处理，即等待预设时间后，再重新判断是否满足陀螺仪初始化条件等。
可选的，可以通过下述方式来对陀螺仪进行延迟初始化处理：
等待预设时间，重新获取图像数据，判断陀螺仪数据是否满足陀螺仪初始化条件；
在陀螺仪数据满足陀螺仪初始化条件的情况下，对陀螺仪进行初始化，并确定陀螺仪的初始化结果，在陀螺仪的初始化结果为初始化成功的情况下，更新惯性测量单元的初始化结果，并返回执行根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图；
在陀螺仪数据不满足陀螺仪初始化条件或陀螺仪的初始化结果为初始化失败的情况下，返回执行等待预设时间，重新获取图像数据，判断陀螺仪数据是否满足陀螺仪初始化条件。
其中，预设时间是延迟初始化的等待时间，即计时到达预设时间时，进行延迟初始化。
具体的，等待预设时间，再重新获取图像数据，判断此时的陀螺仪数据是否满足陀螺仪初始化条件。在陀螺仪数据满足陀螺仪初始化条件的情况下，对陀螺仪进行初始化，确定陀螺仪的初始化结果。若初始化结果为初始化成功，则表明陀螺仪可以加入后续建图，因此，可以更新惯性测量单元的初始化结果，并且返回执行根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图，以进行更精确的建图处理。若陀螺仪数据不满足陀螺仪初始化条件或陀螺仪的初始化结果为初始化失败，则表明陀螺仪仍然不可以加入建图，需要继续延迟初始化，即返回执行等待预设时间，重新获取图像数据，判断陀螺仪数据是否满足陀螺仪初始化条件。
情况二、若惯性测量单元的初始化结果为初始化部分成功，初始化部分成功为加速度计数据不满足加速度计初始化条件且陀螺仪初始化成功，或者，加速度计初始化失败且陀螺仪初始化成功，则对加速度计进行延迟初始化处理。
具体的，在情况二下，陀螺仪初始化成功，但是加速度计未经初始化或者初始化失败，因此，需要对加速度计进行延迟初始化处理，即等待预设时间后，再重新判断是否满足加速度计初始化条件等。
可选的，可以通过下述方式来对加速度计进行延迟初始化处理：
等待预设时间，重新获取图像数据，判断加速度计数据是否满足加速度计初始化条件；
在加速度计数据满足加速度计初始化条件情况下，对加速度计进行初始化，并确定加速度计的初始化结果，在加速度计的初始化结果为初始化成功的情况下，更新惯性测量单元的初始化结果，并返回执行根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图；
在加速度计数据不满足加速度计初始化条件或加速度计的初始化结果为初始化失败的情况下，返回执行等待预设时间，重新获取图像数据，判断加速度计数据是否满足加速度计初始化条件。
具体的，等待预设时间，再重新获取图像数据，判断此时的加速度计数据是否满足加速度计初始化条件。在加速度计数据满足加速度计初始化条件的情况下，对加速度计进行初始化，确定加速度计的初始化结果。若初始化结果为初始化成功，则表明加速度计可以加入后续建图，因此，可以更新惯性测量单元的初始化结果，并且返回执行根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图，以进行更精确的建图处理。若加速度计数据不满足加速度计初始化条件或加速度计的初始化结果为初始化失败，则表明加速度计仍然不可以加入建图，需要继续延迟初始化，即返回执行等待预设时间，重新获取图像数据，判断加速度计数据是否满足加速度计初始化条件。
情况三、若惯性测量单元的初始化结果为初始化失败，则对加速度计和陀螺仪进行延迟初始化处理。
具体的，在情况三下，加速度计未经初始化或者初始化失败，且，陀螺仪未经初始化或者初始化失败，因此，需要对加速度计和陀螺仪都进行延迟初始化处理，即等待预设时间后，再重新判断是否满足加速度计初始化条件以及陀螺仪初始化条件等。
可选的，可以通过下述方式来对加速度计和陀螺仪进行延迟初始化处理：
等待预设时间，重新获取图像数据，并返回执行根据图像数据中的特征数据建立初始视觉地图。
具体的，等待预设时间，再重新获取图像数据，由于是重新对加速度计和陀螺仪进行延迟初始化处理，因此，可以直接返回执行根据图像数据中的特征数据建立初始视觉地图，以便于后续重新判断是否满足初始化条件以及初始化结果等。
在上述示例的基础上，在判断惯性测量单元的数据是否满足初始化条件之后，还可以在加速度计数据和陀螺仪数据均不满足初始化条件时，构建目标地图：
在惯性测量单元的数据中的加速度计数据和陀螺仪数据均不满足初始化条件的情况下，将尺度恢复地图确定为目标地图。
具体的，若惯性测量单元的数据中的加速度计数据和陀螺仪数据均不满足初始化条件，则表明惯性测量单元中的加速度计和陀螺仪都无法使用，无法参与建图，因此，将尺度恢复地图作为目标地图。
可选的，在惯性测量单元的数据中的加速度计数据和陀螺仪数据均不满足初始化条件的情况下，还可以对加速度计和陀螺仪进行延迟初始化处理，具体延迟初始化处理的方式与惯性测量单元的初始化结果为初始化失败的情况相同，在此不做赘述。
本实施例提供的建图方法，通过根据图像数据中的特征数据建立初始视觉地图，并基于初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、关键帧集合中各关键帧位姿以及尺度恢复地图，进而，基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件，以判断加速度计和陀螺仪能否进入初始化流程，在惯性测量单元的数据至少部分满足初始化条件的情况下，对惯性测量单元进行初始化，确定惯性测量单元的初始化结果，以判断初始化后的加速度计或陀螺仪能否用于建图，根据惯性测量单元的初始化结果以及尺度恢复地图，确定目标地图，实现了在惯性测量单元中的加速度计和陀螺仪中至少一个初始化成功的情况下，加入建图，并且对初始化结果进行验证，提高初始化的有效性，进而，提高建图的效率以及准确性。
图2为本公开实施例中的一种建图装置的结构示意图。如图2所示：该装置包括：第一建图模块210、初始化条件判断模块220、初始化结果确定模块230以及第二建图模块240。
其中，第一建图模块210，用于获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；初始化条件判断模块220，用于基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；初始化结果确定模块230，用于在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；第二建图模块240，用于根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
本公开实施例提供的建图装置，可执行本公开方法实施例所提供的建图方法中的步骤，具备执行步骤和有益效果此处不再赘述。
图3为本公开实施例中的一种电子设备的结构示意图。下面具体参考图3，其示出了适于用来实现本公开实施例中的电子设备300的结构示意图。图3示出的电子设备仅仅是一个示例，不应对本公开实施例的功能和使用范围带来任何限制。
如图3所示，电子设备300可以包括处理装置(例如中央处理器、图形处理器等)301，其可以根据存储在只读存储器(ROM)302中的程序或者从存储装置308加载到随机访问存储器(RAM)303中的程序而执行各种适当的动作和处理以实现如本公开所述的实施例的方法。在RAM 303中，还存储有电子设备300操作所需的各种程序和数据。处理装置301、ROM302以及RAM 303通过总线304彼此相连。输入/输出(I/O)接口305也连接至总线304。
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在非暂态计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码，从而实现如上所述的建图方法。在这样的实施例中，该计算机程序可以通过通信装置309从网络上被下载和安装，或者从存储装置308被安装，或者从ROM 302被安装。在该计算机程序被处理装置301执行时，执行本公开实施例的方法中限定的上述功能。
需要说明的是，本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行***、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。
上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：
获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
可选的，当上述一个或者多个程序被该电子设备执行时，该电子设备还可以执行上述实施例所述的其他步骤。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行***、装置或设备使用或与指令执行***、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体***、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
方案1、一种建图方法，所述方法包括：
获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
方案2、根据方案1所述的方法，所述惯性测量单元的初始化结果为初始化部分成功包括：所述加速度计初始化成功且所述陀螺仪初始化失败，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功；
所述惯性测量单元的初始化结果为初始化失败包括：所述加速度计初始化失败且所述陀螺仪初始化失败，所述加速度计初始化失败且所述陀螺仪数据不满足所述陀螺仪初始化条件，或者，所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化失败。
方案3、根据方案2所述的方法，
所述根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图，包括：
若所述惯性测量单元的初始化结果为初始化部分成功或初始化成功，则根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束，并对所述尺度恢复地图进行增量处理，得到目标地图；
若所述惯性测量单元的初始化结果为初始化失败，则将所述尺度恢复地图确定为目标地图。
方案4、根据方案3所述的方法，在所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束之前，还包括：
若所述惯性测量单元的初始化结果为初始化成功，则所述惯性测量单元的初始化参数包括陀螺仪的偏置、视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，通过下述计算式一至五确定所述初始化参数：
计算式一为：
其中，C1min为第一目标函数，为四元数的乘法符号，I为所述关键帧集合中的关键帧的总数，/>为视觉坐标系到惯性坐标系的旋转外参，/>为第i帧关键帧在视觉坐标系下的旋转姿态，/>为第i帧关键帧与第i+1帧关键帧之间的惯性旋转预积分量，/>为两帧关键帧之间预积分量对陀螺仪的偏置δbw的雅可比矩阵，δbw为陀螺仪的偏置；
计算式二为：
计算式三为：
计算式四为：
计算式五为：
rg＝‖g‖-‖gw‖
其中，C2min为第二目标函数，rp为位置残差，rv为速度残差，rg为重力向量模长残差，为惯性测量单元的位置预积分量，/>为惯性测量单元的速度预积分量，/>为位置预积分量对加速度计偏置的雅可比矩阵，/>为速度预积分量对加速度计偏置的雅可比矩阵，δba为加速度计偏置，/>为第i帧关键帧对应的惯性测量单元在视觉坐标系的旋转位姿，/>为第i帧关键帧对应的惯性测量单元在视觉坐标系旋转位姿的逆矩阵，/>为第i帧关键帧的相机在视觉坐标系下的位置，gw为视觉坐标系下的重力向量，/> 为惯性坐标系到相机坐标系的平移，Δti为两帧关键帧之间的时间间隔，/>为第i帧关键帧的速度，vpvg为权重系数矩阵。
方案5、根据方案4所述的方法，
在所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束之前，还包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计初始化成功且所述陀螺仪初始化失败，或者，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，则所述惯性测量单元的初始化参数包括视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，依次通过下述计算式六至八以及所述计算式二至五确定所述初始化参数：
计算式六为：
其中，为第i帧关键帧在视觉坐标系下的旋转姿态，/>为第j帧关键帧在视觉坐标系下的旋转姿态，t为第i帧关键帧与第i+1帧关键帧之间的任意时刻，f(·)为插值函数，为第i帧关键帧与第i+1帧关键帧之间t时刻的旋转位姿；
计算式七为：
计算式八为：
其中，为惯性测量单元的位置预积分量，/>为惯性测量单元的速度预积分量，/>为惯性坐标系下的加速度，/>为惯性坐标系下t时刻相对于i时刻的旋转积分值，/>是对/>经过转换后，确定的惯性坐标系下从第i帧关键帧的位姿开始的旋转预积分量。
方案6、根据方案4所述的方法，在所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束之前，还包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功，则所述初始化参数包括陀螺仪的偏置，通过计算式一确定所述初始化参数。
方案7、根据方案3所述的方法，所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束，包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计初始化成功且所述陀螺仪初始化失败，或者，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，则通过下述计算式九至计算式十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据所述残差约束项以及所述协方差矩阵对所述关键帧位姿进行约束：
计算式九为：
计算式十为：
计算式十一为：
其中，为位置预积分量误差，/>为速度预积分量误差，δbat为加速度计偏置误差，/>为当前惯性测量单元旋转位姿，na为加速度高斯白噪声，nba为加速度随机游走噪声，Δt为时间间隔，·表示求导数，/>表示残差约束项，/>表示所述残差约束项的协方差矩阵，F和G为两个传递矩阵，/>为过程矩阵，所述位置预积分量误差、所述速度预积分量误差以及所述加速度计偏置误差基于所述惯性测量单元的初始化参数以及加速度计测量数据确定；
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功，则通过下述计算式十二、计算式十以及十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据所述残差约束项以及所述协方差矩阵对所述关键帧位姿进行约束：
计算式十二：
计算式十为：
计算式十一为：
其中，分别为旋转预积分量误差，δbwt为陀螺仪偏置误差，wt-bwt为角速度测量值减去偏置，/>是从向量生成反对称矩阵的运算，nw为陀螺仪高斯白噪声，nbw为陀螺仪随机游走噪声，·表示求导数，/>表示残差约束项，/>表示所述残差约束项的协方差矩阵，F和G为两个传递矩阵，/>为过程矩阵，所述旋转预积分量误差以及所述陀螺仪偏置误差基于所述惯性测量单元的初始化参数以及陀螺仪测量数据确定。
方案8、根据方案2所述的方法，在所述确定目标地图之后，还包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计初始化成功且所述陀螺仪初始化失败，或者，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，则对所述陀螺仪进行延迟初始化处理；
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功，则对所述加速度计进行延迟初始化处理；
若所述惯性测量单元的初始化结果为初始化失败，则对所述加速度计和所述陀螺仪进行延迟初始化处理。
方案9、根据方案8所述的方法，所述对所述加速度计进行延迟初始化处理，包括：
等待预设时间，重新获取图像数据，判断所述加速度计数据是否满足加速度计初始化条件；
在所述加速度计数据满足所述加速度计初始化条件情况下，对所述加速度计进行初始化，并确定所述加速度计的初始化结果，在所述加速度计的初始化结果为初始化成功的情况下，更新所述惯性测量单元的初始化结果，并返回执行所述根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图；
在所述加速度计数据不满足所述加速度计初始化条件或所述加速度计的初始化结果为初始化失败的情况下，返回执行所述等待预设时间，重新获取图像数据，判断所述加速度计数据是否满足加速度计初始化条件。
方案10、根据方案8所述的方法，所述对所述陀螺仪进行延迟初始化处理，包括：
等待预设时间，重新获取图像数据，判断所述陀螺仪数据是否满足陀螺仪初始化条件；
在所述陀螺仪数据满足所述陀螺仪初始化条件的情况下，对所述陀螺仪进行初始化，并确定所述陀螺仪的初始化结果，在所述陀螺仪的初始化结果为初始化成功的情况下，更新所述惯性测量单元的初始化结果，并返回执行所述根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图；
在所述陀螺仪数据不满足所述陀螺仪初始化条件或所述陀螺仪的初始化结果为初始化失败的情况下，返回执行所述等待预设时间，重新获取图像数据，判断所述陀螺仪数据是否满足陀螺仪初始化条件。
方案11、根据方案8所述的方法，所述对所述加速度计和所述陀螺仪进行延迟初始化处理，包括：
等待预设时间，重新获取图像数据，并返回执行所述根据所述图像数据中的特征数据建立初始视觉地图。
方案12、根据方案1所述的方法，所述基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件，包括：
基于各关键帧位姿中的旋转姿态以及预先设置的旋转阈值，判断所述陀螺仪数据是否满足陀螺仪初始化条件；
基于各关键帧位姿中的位置、各关键帧位姿对应的时间点以及预先设置的加速度阈值，判断所述加速度计数据是否满足加速度计初始化条件。
方案13、根据方案1所述的方法，所述确定所述惯性测量单元的初始化结果，包括：
若满足下述计算式十三，则确定所述加速度计的初始化结果为初始化成功，若不满足下述计算式十三，则确定所述加速度计的初始化结果为初始化失败；
计算式十三为：
其中，为预先标定的加速度偏置白噪声，na为加速度计的比例系数，/>为预设时间内的加速度计偏置变化量，Δt为时间间隔；
若满足下述计算式十四，则确定所述陀螺仪的初始化结果为初始化成功，若不满足下述计算式十四，则确定所述陀螺仪的初始化结果为初始化失败；
计算式十四为：
其中，为预先标定的陀螺仪的偏置白噪声，ng为陀螺仪的比例系数，/>为预设时间内的陀螺仪的偏置变化量，Δt为时间间隔；
若所述加速度计的初始化结果和所述陀螺仪的初始化结果均为初始化成功，则确定所述惯性测量单元的初始化结果为初始化成功；
若所述加速度计初始化成功且所述陀螺仪初始化失败，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功，则确定所述惯性测量单元的初始化结果为初始化部分成功；
若所述加速度计初始化失败且所述陀螺仪初始化失败，所述加速度计初始化失败且所述陀螺仪数据不满足所述陀螺仪初始化条件，或者，所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化失败，则确定所述惯性测量单元的初始化结果为初始化失败。
方案14、根据方案1所述的方法，在所述判断惯性测量单元的数据是否满足初始化条件之后，还包括：
在所述惯性测量单元的数据中的加速度计数据和陀螺仪数据均不满足所述初始化条件的情况下，将所述尺度恢复地图确定为目标地图。
方案15、一种建图装置，包括：
第一建图模块，用于获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
初始化条件判断模块，用于基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
初始化结果确定模块，用于在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
第二建图模块，用于根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
方案16、一种电子设备，所述电子设备包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如方案1-14中任一项所述的建图方法。
方案17、一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如方案1-14中任一项所述的建图方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的公开范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述公开构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (10)

1.一种建图方法，其特征在于，所述方法包括：
获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
2.根据权利要求1所述的方法，其特征在于，所述惯性测量单元的初始化结果为初始化部分成功包括：所述加速度计初始化成功且所述陀螺仪初始化失败，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功；
所述惯性测量单元的初始化结果为初始化失败包括：所述加速度计初始化失败且所述陀螺仪初始化失败，所述加速度计初始化失败且所述陀螺仪数据不满足所述陀螺仪初始化条件，或者，所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化失败。
3.根据权利要求2所述的方法，其特征在于，所述根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图，包括：
若所述惯性测量单元的初始化结果为初始化部分成功或初始化成功，则根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束，并对所述尺度恢复地图进行增量处理，得到目标地图；
若所述惯性测量单元的初始化结果为初始化失败，则将所述尺度恢复地图确定为目标地图。
4.根据权利要求3所述的方法，其特征在于，在所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束之前，还包括：
若所述惯性测量单元的初始化结果为初始化成功，则所述惯性测量单元的初始化参数包括陀螺仪的偏置、视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，通过下述计算式一至五确定所述初始化参数：
计算式一为：
其中，C1min为第一目标函数，为四元数的乘法符号，I为所述关键帧集合中的关键帧的总数，/>为视觉坐标系到惯性坐标系的旋转外参，/>为第i帧关键帧在视觉坐标系下的旋转姿态，/>为第i帧关键帧与第i+1帧关键帧之间的惯性旋转预积分量，/>为两帧关键帧之间预积分量对陀螺仪的偏置δbw的雅可比矩阵，δbw为陀螺仪的偏置；
计算式二为：
计算式三为：
计算式四为：
计算式五为：
rg＝‖g‖-‖gw‖
其中，C2min为第二目标函数，rp为位置残差，rv为速度残差，rg为重力向量模长残差，为惯性测量单元的位置预积分量，/>为惯性测量单元的速度预积分量，/>为位置预积分量对加速度计偏置的雅可比矩阵，/>为速度预积分量对加速度计偏置的雅可比矩阵，δba为加速度计偏置，/>为第i帧关键帧对应的惯性测量单元在视觉坐标系的旋转位姿，为第i帧关键帧对应的惯性测量单元在视觉坐标系旋转位姿的逆矩阵，/>为第i帧关键帧的相机在视觉坐标系下的位置，gw为视觉坐标系下的重力向量，/> 为惯性坐标系到相机坐标系的平移，Δti为两帧关键帧之间的时间间隔，/>为第i帧关键帧的速度，vpvg为权重系数矩阵。
5.根据权利要求4所述的方法，其特征在于，在所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束之前，还包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计初始化成功且所述陀螺仪初始化失败，或者，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，则所述惯性测量单元的初始化参数包括视觉坐标系下的重力向量、加速度计偏置以及各关键帧的速度，依次通过下述计算式六至八以及所述计算式二至五确定所述初始化参数：
计算式六为：
其中，为第i帧关键帧在视觉坐标系下的旋转姿态，/>为第j帧关键帧在视觉坐标系下的旋转姿态，t为第i帧关键帧与第i+1帧关键帧之间的任意时刻，f(·)为插值函数，/>为第i帧关键帧与第i+1帧关键帧之间t时刻的旋转位姿；
计算式七为：
计算式八为：
其中，为惯性测量单元的位置预积分量，/>为惯性测量单元的速度预积分量，/>为惯性坐标系下的加速度，/>为惯性坐标系下t时刻相对于i时刻的旋转积分值，/>是对经过转换后，确定的惯性坐标系下从第i帧关键帧的位姿开始的旋转预积分量。
6.根据权利要求3所述的方法，其特征在于，所述根据所述惯性测量单元的初始化参数对所述关键帧位姿进行约束，包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计初始化成功且所述陀螺仪初始化失败，或者，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，则通过下述计算式九至计算式十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据所述残差约束项以及所述协方差矩阵对所述关键帧位姿进行约束：
计算式九为：
计算式十为：
计算式十一为：
其中，为位置预积分量误差，/>为速度预积分量误差，δbat为加速度计偏置误差，为当前惯性测量单元旋转位姿，na为加速度高斯白噪声，/>为加速度随机游走噪声，Δt为时间间隔，·表示求导数，/>表示残差约束项，/>表示所述残差约束项的协方差矩阵，F和G为两个传递矩阵，/>为过程矩阵，所述位置预积分量误差、所述速度预积分量误差以及所述加速度计偏置误差基于所述惯性测量单元的初始化参数以及加速度计测量数据确定；
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功，则通过下述计算式十二、计算式十以及十一确定惯性测量单元的残差约束项以及协方差矩阵，并根据所述残差约束项以及所述协方差矩阵对所述关键帧位姿进行约束：
计算式十二：
计算式十为：
计算式十一为：
其中，分别为旋转预积分量误差，δbwt为陀螺仪偏置误差，wt-bwt为角速度测量值减去偏置，/>是从向量生成反对称矩阵的运算，nw为陀螺仪高斯白噪声，/>为陀螺仪随机游走噪声，·表示求导数，/>表示残差约束项，/>表示所述残差约束项的协方差矩阵，F和G为两个传递矩阵，/>为过程矩阵，所述旋转预积分量误差以及所述陀螺仪偏置误差基于所述惯性测量单元的初始化参数以及陀螺仪测量数据确定。
7.根据权利要求2所述的方法，其特征在于，在所述确定目标地图之后，还包括：
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计初始化成功且所述陀螺仪初始化失败，或者，所述加速度计初始化成功且所述陀螺仪数据不满足所述陀螺仪初始化条件，则对所述陀螺仪进行延迟初始化处理；
若所述惯性测量单元的初始化结果为初始化部分成功，所述初始化部分成功为所述加速度计数据不满足所述加速度计初始化条件且所述陀螺仪初始化成功，或者，所述加速度计初始化失败且所述陀螺仪初始化成功，则对所述加速度计进行延迟初始化处理；
若所述惯性测量单元的初始化结果为初始化失败，则对所述加速度计和所述陀螺仪进行延迟初始化处理。
8.一种建图装置，其特征在于，包括：
第一建图模块，用于获取图像数据，根据所述图像数据中的特征数据建立初始视觉地图，并基于所述初始视觉地图、轮速计信息以及方向盘转角信息，确定关键帧集合、所述关键帧集合中各关键帧位姿以及尺度恢复地图；
初始化条件判断模块，用于基于各关键帧位姿，判断惯性测量单元的数据是否满足初始化条件；其中，所述惯性测量单元的数据包括加速度计数据和陀螺仪数据，所述初始化条件包括加速度计初始化条件以及陀螺仪初始化条件；
初始化结果确定模块，用于在所述惯性测量单元的数据至少部分满足所述初始化条件的情况下，对所述惯性测量单元进行初始化，确定所述惯性测量单元的初始化结果；其中，所述惯性测量单元的初始化结果包括初始化失败、初始化部分成功以及初始化成功；
第二建图模块，用于根据所述惯性测量单元的初始化结果以及所述尺度恢复地图，确定目标地图。
9.一种电子设备，其特征在于，所述电子设备包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-7中任一项所述的建图方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的建图方法。
CN202310877156.7A 2023-07-17 2023-07-17 建图方法、装置、电子设备和存储介质 Pending CN116907485A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310877156.7A CN116907485A (zh) 2023-07-17 2023-07-17 建图方法、装置、电子设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310877156.7A CN116907485A (zh) 2023-07-17 2023-07-17 建图方法、装置、电子设备和存储介质

Publications (1)

Publication Number Publication Date
CN116907485A true CN116907485A (zh) 2023-10-20

Family

ID=88359848

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310877156.7A Pending CN116907485A (zh) 2023-07-17 2023-07-17 建图方法、装置、电子设备和存储介质

Country Status (1)

Country Link
CN (1) CN116907485A (zh)

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN110084832B (zh) 相机位姿的纠正方法、装置、***、设备和存储介质
CN111932616B (zh) 一种利用并行计算加速的双目视觉惯性里程计方法
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
EP3852065A1 (en) Data processing method and apparatus
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN113223161B (zh) 一种基于imu和轮速计紧耦合的鲁棒全景slam***和方法
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
JP7173471B2 (ja) ３次元位置推定装置及びプログラム
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
KR20200037502A (ko) 포즈 정보를 출력하는 방법 및 장치
CN107942090B (zh) 一种基于模糊星图提取光流信息的航天器角速度估计方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam***
CN113570716A (zh) 云端三维地图构建方法、***及设备
Panahandeh et al. Vision-aided inertial navigation using planar terrain features
CN114812601A (zh) 视觉惯性里程计的状态估计方法、装置、电子设备
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN116907485A (zh) 建图方法、装置、电子设备和存储介质
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN110864685B (zh) 一种基于松耦合的车辆单目视觉轮式里程计定位方法
CN112129272B (zh) 视觉里程计的实现方法和实现装置
CN113034538A (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