CN113218407B - 一种基于vio和卫星导航系统融合的地图生成方法及装置 - Google Patents

一种基于vio和卫星导航系统融合的地图生成方法及装置 Download PDF

Info

Publication number
CN113218407B
CN113218407B CN202010069459.2A CN202010069459A CN113218407B CN 113218407 B CN113218407 B CN 113218407B CN 202010069459 A CN202010069459 A CN 202010069459A CN 113218407 B CN113218407 B CN 113218407B
Authority
CN
China
Prior art keywords
navigation
imu
time period
vio
speed
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
CN202010069459.2A
Other languages
English (en)
Other versions
CN113218407A (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 Chusudu Technology Co ltd
Original Assignee
Beijing Chusudu 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 Chusudu Technology Co ltd filed Critical Beijing Chusudu Technology Co ltd
Priority to CN202010069459.2A priority Critical patent/CN113218407B/zh
Priority to PCT/CN2020/121344 priority patent/WO2021147391A1/zh
Publication of CN113218407A publication Critical patent/CN113218407A/zh
Application granted granted Critical
Publication of CN113218407B publication Critical patent/CN113218407B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • G01C21/32—Structuring or formatting of map data
    • 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
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. 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
    • 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

Abstract

本发明实施例公开一种基于VIO和卫星导航系统融合的地图生成方法及装置。该方法可以实时将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到IMU在第一时间段内的VIO轨迹并缓存，确定导航位置对应的权重并基于权重对导航位置进行缓存，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化得到优化后的VIO轨迹，最后基于优化后的VIO轨迹生成高精度地图。由于在优化缓存的VIO轨迹时是基于缓存的导航位置的不同权重进行优化的，使得准确度较高的导航位置在优化时所起的作用较大，使得优化后的VIO轨迹的准确度较高，进一步提高了所生成的高精度地图的准确度。

Description

一种基于VIO和卫星导航系统融合的地图生成方法及装置
技术领域
本发明涉及地图生成技术领域，具体而言，涉及一种基于VIO和卫星导航系统融合的地图生成方法及装置。
背景技术
VIO(Visual Inertial Odometry,视觉惯性里程计)是近些年受到广泛关注的一种定位方法，VIO使用从图像中提取出来的特征点或者图像亮度变化追踪相机在环境中的位姿变化，同时使用惯性测量单元IMU提供图像帧间的运动约束，由此，通过对位姿变化和图像帧间的运动约束进行融合之后输出更加鲁棒稳定的定位结果。
目前，在建立高精度地图时，VIO可以提供高精度的IMU帧间运动，全球定位系统卫星导航系统可以提供世界坐标系下的IMU的绝对位置约束，然后实时融合IMU采集信息、相机所采集的视频帧图像以及卫星导航系统采集的信息就可以得到IMU的VIO轨迹，然后基于IMU的VIO轨迹生成高精度地图，其中，VIO轨迹可以表征IMU在轨迹形成时间段内的位置和姿态。
但是，由于卫星导航系统采集导航位置时容易受到高楼、林荫、隧道等环境的干扰，使得卫星导航系统采集的导航位置存在较大偏差，导致在实时融合IMU采集信息、相机所采集的视频帧图像以及卫星导航系统采集的信息时，会导致融合后的VIO轨迹被偏差较大的导航位置带偏，使得所得到的VIO轨迹不够准确，进一步导致所生成的高精度地图不够准确。
发明内容
本发明提供了一种基于VIO和卫星导航系统融合的地图生成方法及装置，以提高所生成的高精度地图的准确度。具体的技术方案如下：
第一方面，本发明实施例提供了一种基于VIO和卫星导航系统融合的地图生成方法，相机与惯性测量单元IMU均安装于车辆，且所述IMU的采集频率大于所述相机的采集频率，卫星导航系统的采集频率小于所述相机的采集频率，包括：
获取所述相机采集的所述车辆的周围环境的视频帧图像、所述卫星导航系统采集的导航速度和导航位置以及所述IMU采集的IMU加速度和IMU角速度；
从所获取的视频帧图像中确定两相邻关键帧图像，基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量，其中，所述第一时间段为采集所述两个相邻关键帧图像的采集时间点之差；
将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，所述第一坐标系为所述相机拍摄第一帧视频帧图像时对应的相机坐标系；
确定所述导航位置对应的权重并基于所确定的权重对所述导航位置进行缓存；
当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹；
根据所述优化后的VIO轨迹以及所述车辆的周围环境感知信息生成高精度地图。
可选的，所述基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量的步骤，包括：
基于预设预积分公式对所述IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量；
根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定所述相机在所述第一时间段的起始点的第一位置和第一姿态，以及在所述第一时间段的终点的第二位置和第二姿态；
基于所述第一位置和所述第二位置确定所述相机在所述第一时间段内的第二位置变化量；
基于所述第一姿态和所述第二姿态确定所述相机在所述第一时间段内的第二姿态变化量。
可选的，所述将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存的步骤，包括：
基于预设速度变换公式将所述卫星导航系统在所述第一时间段内采集的导航速度转换为IMU速度，其中，所述预设速度变换公式表征导航速度与IMU速度之间的转换关系；
根据卡尔曼滤波方法或者非线性优化方法，将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述IMU速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存。
可选的，所述确定所述导航位置的权重并基于所确定的权重对所述导航位置进行缓存的步骤，包括：
对所述卫星导航系统在所述第一时间段内采集的导航速度进行积分得到所述IMU在所述第一时间段内的平移量；
计算所述卫星导航系统在所述第一时间段的起始点采集的导航位置与所述卫星导航系统在所述第一时间段的终点采集的导航位置的第一差值；
计算所述平移量与所述第一差值之间的第二差值；
将所述导航位置中位于预设差值范围内的第二差值对应的第一导航位置的权重设定为第一预设权重并缓存所述第一导航位置，将所述导航位置中除所述第一导航位置外的第二导航位置的权重设定为第二预设权重并缓存所述第二导航位置，其中，所述第一预设权重大于所述第二预设权重；
或者，将所述导航位置中位于预设差值范围内的第二差值对应的第三导航位置的权重设定为所述第一预设权重并缓存所述第三导航位置。
可选的，所述确定所述导航位置的权重并基于所确定的权重对所述导航位置进行缓存的步骤，包括：
实时接收卫星发送的卫星信号的信噪比；
确定所述卫星导航系统在第二时间段内采集的第四导航位置，其中，所述第二时间段为所接收的信噪比小于预设信噪比阈值的时间段；
将所述导航位置中除所述第四导航位置外的第五导航位置的权重设定为第三预设权重并缓存所述第五导航位置，将所述第四导航位置的权重设定为第四预设权重并缓存所述第四导航位置，其中，所述第三预设权重大于所述第四预设权重；
或者，将所述导航位置中除所述第四导航位置外的第六导航位置的权重设定为所述第三预设权重并缓存所述第六导航位置。
可选的，所述基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹的步骤，包括：
基于缓存的导航位置及对应的权重以及缓存的VIO轨迹建立关于优化后的VIO轨迹的argmin函数；
通过非线性优化算法求解所述argmin函数，得到优化后的VIO轨迹。
可选的，所述预设长度阈值为200m，所述预设数量阈值为100个。
第二方面，本发明实施例提供了一种基于VIO和卫星导航系统融合的地图生成装置，相机与惯性测量单元IMU均安装于车辆，且所述IMU的采集频率大于所述相机的采集频率，卫星导航系统的采集频率小于所述相机的采集频率，包括：
获取模块，用于获取所述相机采集的所述车辆的周围环境的视频帧图像、所述卫星导航系统采集的导航速度和导航位置以及所述IMU采集的IMU加速度和IMU角速度；
变化量确定模块，用于从所获取的视频帧图像中确定两相邻关键帧图像，基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量，其中，所述第一时间段为采集所述两个相邻关键帧图像的采集时间点之差；
融合模块，用于将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，所述第一坐标系为所述相机拍摄第一帧视频帧图像时对应的相机坐标系；
缓存模块，用于确定所述导航位置对应的权重并基于所确定的权重对所述导航位置进行缓存；
优化模块，用于当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹；
生成模块，用于根据所述优化后的VIO轨迹以及所述车辆的周围环境感知信息生成高精度地图。
可选的，所述变化量确定模块，包括：
预积分子模块，用于基于预设预积分公式对所述IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量；
确定子模块，用于根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定所述相机在所述第一时间段的起始点的第一位置和第一姿态，以及在所述第一时间段的终点的第二位置和第二姿态；
位置变化量确定子模块，用于基于所述第一位置和所述第二位置确定所述相机在所述第一时间段内的第二位置变化量；
姿态变化量确定子模块，用于基于所述第一姿态和所述第二姿态确定所述相机在所述第一时间段内的第二姿态变化量。
可选的，所述融合模块，包括：
转换子模块，用于基于预设速度变换公式将所述卫星导航系统在所述第一时间段内采集的导航速度转换为IMU速度，其中，所述预设速度变换公式表征导航速度与IMU速度之间的转换关系；
融合子模块，用于根据卡尔曼滤波装置或者非线性优化装置，将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述IMU速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存。
可选的，所述缓存模块，包括：
平移量计算子模块，用于对所述卫星导航系统在所述第一时间段内采集的导航速度进行积分得到所述IMU在所述第一时间段内的平移量；
第一差值计算子模块，用于计算所述卫星导航系统在所述第一时间段的起始点采集的导航位置与所述卫星导航系统在所述第一时间段的终点采集的导航位置的第一差值；
第二差值计算子模块，用于计算所述平移量与所述第一差值之间的第二差值；
第一缓存子模块，用于将所述导航位置中位于预设差值范围内的第二差值对应的第一导航位置的权重设定为第一预设权重并缓存所述第一导航位置，将所述导航位置中除所述第一导航位置外的第二导航位置的权重设定为第二预设权重并缓存所述第二导航位置，其中，所述第一预设权重大于所述第二预设权重；
或者，
第二缓存子模块，用于将所述导航位置中位于预设差值范围内的第二差值对应的第三导航位置的权重设定为所述第一预设权重并缓存所述第三导航位置。
可选的，所述缓存模块，包括：
接收子模块，用于实时接收卫星发送的卫星信号的信噪比；
第四导航位置确定子模块，用于确定所述卫星导航系统在第二时间段内采集的第四导航位置，其中，所述第二时间段为所接收的信噪比小于预设信噪比阈值的时间段；
第三缓存子模块，用于将所述导航位置中除所述第四导航位置外的第五导航位置的权重设定为第三预设权重并缓存所述第五导航位置，将所述第四导航位置的权重设定为第四预设权重并缓存所述第四导航位置，其中，所述第三预设权重大于所述第四预设权重；
或者，
第四缓存子模块，用于将所述导航位置中除所述第四导航位置外的第六导航位置的权重设定为所述第三预设权重并缓存所述第六导航位置。
可选的，所述优化模块，包括：
函数建立子模块，用于基于缓存的导航位置及对应的权重以及缓存的VIO轨迹建立关于优化后的VIO轨迹的argmin函数；
求解子模块，用于通过非线性优化算法求解所述argmin函数，得到优化后的VIO轨迹。
可选的，所述预设长度阈值为200m，所述预设数量阈值为100个。
由上述内容可知，本发明实施例可以基于IMU在第一时间段内采集的IMU加速度和IMU角速度，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定相机在所述第一时间段内的第二位置变化量和第二姿态变化量，然后实时将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到在第一坐标系下的IMU在第一时间段内的视觉惯导里程计VIO轨迹并缓存，然后确定导航位置对应的权重并基于所确定的权重对导航位置进行缓存，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹，最后基于优化后的VIO轨迹生成高精度地图。由此，由缓存的VIO轨迹提供关键帧图像的帧间位置约束和姿态约束，缓存的具有不同权重的导航位置提供绝对位置约束，由于在优化缓存的VIO轨迹时是基于缓存的导航位置的不同权重进行优化的，使得准确度较高的导航位置在优化VIO轨迹时所起的作用较大，使得优化后的VIO轨迹的准确度较高，进一步提高了所生成的高精度地图的准确度。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
本发明实施例的创新点包括：
1、由缓存的VIO轨迹提供关键帧图像的帧间位置约束和姿态约束，缓存的具有不同权重的导航位置提供绝对位置约束，由于在优化缓存的VIO轨迹时是基于缓存的导航位置的不同权重进行优化的，使得准确度较高的导航位置在优化VIO轨迹时所起的作用较大，使得优化后的VIO轨迹的准确度较高，进一步提高了所生成的高精度地图的准确度。
2、本发明实施例提出的将导航速度与导航位置分开使用的方法，可以实时利用导航速度提供尺度约束，同时牺牲一定导航位置的实时性，利用缓存的具有不同权重的导航位置提供位置约束，能够保证得到的VIO轨迹在导航位置存在较大偏差的区域内也可以维持较高的准确度。同时，本发明实施例提出的基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹的方式，可以得到相对精度和绝对精度均较高的VIO轨迹。
3、基于导航速度与导航位置之间存在一致性的原则，计算在第一时间段内基于导航速度求得的IMU的平移量和基于导航位置求得的第一差值之间的第二差值，判断第二差值是否位于预设差值范围内，并基于判断结果确定导航位置的权重并基于所确定的权重对导航位置进行缓存。
4、基于所接收到的卫星信号的信噪比来判断导航位置是否准确，并基于判断结果确定导航位置的权重并基于所确定的权重对导航位置进行缓存。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例。对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的基于VIO和卫星导航系统融合的地图生成方法的一种流程示意图；
图2为本发明实施例提供的在第一坐标系下的IMU的VIO轨迹示意图；
图3为本发明实施例提供的对缓存的VIO轨迹进行位姿图优化得到优化后的VIO轨迹的示意图；
图4为本发明实施例提供的基于VIO和卫星导航系统融合的地图生成装置的一种结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。例如包含的一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。
本发明实施例公开了一种基于VIO和卫星导航系统融合的地图生成方法及装置，能够提高得到的VIO轨迹的准确度，进一步提高所生成的高精度地图的准确度。下面对本发明实施例进行详细说明。
参见图1，图1为本发明实施例提供的基于VIO和卫星导航系统融合的地图生成方法的一种流程示意图。该方法应用于电子设备，相机与惯性测量单元IMU均安装于车辆，且IMU的采集频率大于相机的采集频率，卫星导航系统的采集频率小于相机的采集频率，其中，卫星导航系统可以为BDS(BeiDou Navigation Satellite System，中国北斗卫星导航系统)或者GPS(Global Positioning System，全球定位系统)。
该方法具体包括以下步骤S110-S160：
S110：获取相机采集的车辆的周围环境的视频帧图像、卫星导航系统采集的导航速度和导航位置以及IMU采集的IMU加速度和IMU角速度。
为了建立高精度地图，需要驾驶员驾驶安装有相机与IMU(Inertialmeasurementunit，惯性测量单元)的车辆在预设地区内内行驶，该预设地区为需要建立高精度地图的地区，其中，高精度地图为精度大于预设精度阈值的电子导航地图。
由于相机与IMU均安装于车辆，因此在车辆行驶过程中，相机可以实时采集车辆的周围环境的视频帧图像，IMU可以实时采集IMU加速度和IMU角速度，卫星导航系统实时采集导航速度和导航位置。
因此，电子设备可以获取相机采集的车辆的周围环境的视频帧图像、卫星导航系统采集的导航速度和导航位置以及IMU采集的IMU加速度和IMU角速度，示例性的，当卫星导航系统为GPS时，导航速度为GPS速度，导航位置为GPS位置。
S120：从所获取的视频帧图像中确定两相邻关键帧图像，基于IMU在第一时间段内采集的IMU加速度和IMU角速度，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定相机在第一时间段内的第二位置变化量和第二姿态变化量，其中，第一时间段为采集两个相邻关键帧图像的采集时间点之差。
在获取相机采集的车辆的周围环境的视频帧图像后，需要从所获取的视频帧图像中确定两相邻关键帧图像，其中，上述从所获取的视频帧图像中确定两相邻关键帧图像，可以包括：
提取所获取的每一视频帧图像的特征点，基于特征匹配方法或者光流跟踪方法对各个特征点进行跟踪，将跟踪成功的视频帧图像设置为关键帧图像，从关键帧图像中确定两相邻关键帧图像。
由于在相机所采集的某些视频帧图像中可以观测到真实世界中的同一物体，又由于相机在拍摄位于不同视频帧图像中的该同一物体时的位置和姿态是变化的，为了后续得到相机的位置变化量和姿态变化量，需要确定包含同一物体的相邻两帧图像，也就是两相邻关键帧图像。
为了确定两相邻关键帧图像，可以提取所获取的每一视频帧图像的特征点，基于特征匹配方法或者光流跟踪方法对各个特征点进行跟踪，将跟踪成功的视频帧图像设置为关键帧图像，由于关键帧图像可能有多帧，而每相邻的两关键帧图像的处理方式是相同的，因此，只需从关键帧图像中确定两相邻关键帧图像即可，需要说明的是，每个关键帧图像对应卫星导航系统在相机采集该关键帧图像时所采集得到的导航位置。
由此，通过特征匹配方法或者光流跟踪方法对各个特征点进行跟踪的方式，确定两相邻关键帧图像。
为了得到IMU的VIO(visual-inertial odometry，视觉惯性里程计)轨迹，在确定两相邻关键帧图像后，需要基于IMU在第一时间段内采集的IMU加速度和IMU角速度，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定相机在一时间段内的第二位置变化量和第二姿态变化量，其中，第一时间段为采集两个相邻关键帧图像的采集时间点之差。
上述基于IMU在第一时间段内采集的IMU加速度和IMU角速度，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定相机在一时间段内的第二位置变化量和第二姿态变化量，可以包括：
基于预设预积分公式对IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量；
根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定相机在第一时间段的起始点的第一位置和第一姿态，以及在第一时间段的终点的第二位置和第二姿态；
基于第一位置和第二位置确定相机在第一时间段内的第二位置变化量；
基于第一姿态和第二姿态确定相机在第一时间段内的第二姿态变化量。
由于IMU的采集频率大于相机的采集频率，因此，在第一时间段内可以获取IMU采集的多个IMU加速度和多个IMU角速度，然后基于预设预积分公式对IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，就可以得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量。
为了得到相机在第一时间段内的第二位置变化量和第二姿态变化量，需要得到相机在第一时间段的起始点的第一位置和第一姿态，以及在第一时间段的终点的第二位置和第二姿态，具体可以为根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定相机在第一时间段的起始点的第一位置和第一姿态，以及在第一时间段的终点的第二位置和第二姿态。
在得到第一位置和第一姿态，以及第二位置和第二姿态后，即可基于第一位置和第二位置确定相机在第一时间段内的第二位置变化量，基于第一姿态和第二姿态确定相机在第一时间段内的第二姿态变化量。
由此，通过对IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分的方式，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置以及多视图几何原理，可以确定相机在第一时间段的起始点的第一位置和第一姿态，以及在第一时间段的终点的第二位置和第二姿态，然后基于第一位置、第一姿态、第二位置和第二姿态得到相机在第一时间段内的第二位置变化量和相机在第一时间段内的第二姿态变化量。
S130：将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到在第一坐标系下的IMU在第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，第一坐标系为相机拍摄第一帧视频帧图像时对应的相机坐标系。
由于卫星导航系统采集导航位置时容易受到高楼、林荫、隧道等环境的干扰，使得卫星导航系统采集的导航位置存在较大偏差，但采集的导航速度并不受上述环境的干扰，因此较为精准，使得在得到第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量后，可以将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到在第一坐标系下的IMU在第一时间段内的视觉惯导里程计VIO轨迹并缓存。
其中，第一坐标系为相机拍摄第一帧视频帧图像时对应的相机坐标系，IMU在第一时间段内的VIO轨迹包含两个相邻关键帧图像对应的位置和姿态，由于随着时间的增加，会缓存多个的IMU在第一时间段内的VIO轨迹，使得缓存的VIO轨迹逐渐变长，则缓存的VIO轨迹包含多个关键帧图像对应的位置和姿态。
上述将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到在第一坐标系下的IMU在第一时间段内的VIO轨迹并缓存，可以包括：
基于预设速度变换公式将卫星导航系统在第一时间段内采集的导航速度转换为IMU速度，其中，预设速度变换公式表征导航速度与IMU速度之间的转换关系；
根据卡尔曼滤波方法或者非线性优化方法，将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和IMU速度进行融合，得到在第一坐标系下的IMU在第一时间段内的VIO轨迹并缓存。
由于卫星导航系统所采集到的导航速度并不是IMU速度，因此，需要基于预设速度变换公式将卫星导航系统在第一时间段内采集的导航速度转换为IMU速度，其中，预设速度变换公式表征导航速度与IMU速度之间的转换关系。
在转换得到IMU速度后，即可根据卡尔曼滤波方法或者非线性优化方法，将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和IMU速度进行融合，得到在第一坐标系下的IMU在第一时间段内的VIO轨迹。
由于上述融合是基于导航速度进行融合，而未融合导航位置，因此，只是为VIO轨迹添加了速度约束，还需要进一步为VIO轨迹添加位置约束，因此，为了后续为VIO轨迹添加位置约束，在得到在第一坐标系下的IMU在第一时间段内的VIO轨迹后，需要缓存VIO轨迹。
由此，通过卡尔曼滤波方法或者非线性优化方法，将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和IMU速度进行融合，得到在第一坐标系下的IMU在第一时间段内的VIO轨迹并缓存。
参见图2，图2为在第一坐标系下的IMU的VIO轨迹示意图，图2中，五角星代表关键帧图像中的特征点，三角形代表关键帧图像，箭头代表导航速度，圆点代表IMU测量值包括IMU加速度和IMU角速度，五角星与三角形相连说明五角星代表的特征点是位于相连的三角形代表的关键帧图像中，图2中的VIO轨迹包含四个关键帧图像和4个导航速度相邻两关键帧图像之间有多个IMU测量值。
S140：确定导航位置对应的权重并基于所确定的权重对导航位置进行缓存；
由于卫星导航系统采集导航位置时容易受到高楼、林荫、隧道等环境的干扰，使得卫星导航系统采集的导航位置存在较大偏差，本发明实施例中为了得到准确度较高的VIO轨迹，采用分段缓存导航位置的方式，识别出每段缓存的导航位置中较大偏差的导航位置，滤除所识别出的导航位置或者将所识别出的导航位置的权重设置的较小，以便在后续基于导航位置可以得到准确度较高的VIO轨迹，因此，在缓存步骤S130得到的VIO轨迹后，需要确定导航位置对应的权重并基于所确定的权重对导航位置进行缓存。
上述确定导航位置对应的权重并基于所确定的权重对导航位置进行缓存的方式有多种，包括但不限于以下几种：
第一种：
步骤S140可以包括：
对卫星导航系统在第一时间段内采集的导航速度进行积分得到IMU在第一时间段内的平移量；
计算卫星导航系统在第一时间段的起始点采集的导航位置与卫星导航系统在第一时间段的终点采集的导航位置的第一差值；
计算平移量与第一差值之间的第二差值；
将导航位置中位于预设差值范围内的第二差值对应的第一导航位置的权重设定为第一预设权重并缓存第一导航位置，将导航位置中除第一导航位置外的第二导航位置的权重设定为第二预设权重并缓存第二导航位置，其中，第一预设权重大于第二预设权重；
或者，将导航位置中位于预设差值范围内的第二差值对应的第三导航位置的权重设定为第一预设权重并缓存第三导航位置。
由于卫星导航系统采集的导航速度与导航位置之间存在一致性，因此，可以分别基于导航速度和导航位置求取IMU在第一时间段内的移动量，如果所求取的两个移动量之间相差较小，说明第一时间段对应的导航速度较为准确，反之则不准确。
具体可以为对卫星导航系统在第一时间段内采集的导航速度进行积分得到IMU在第一时间段内的平移量，计算卫星导航系统在第一时间段的起始点采集的导航位置与卫星导航系统在第一时间段的终点采集的导航位置的第一差值，计算平移量与第一差值之间的第二差值。
位于预设差值范围内的第二差值对应的导航位置较为准确，因此，将较为准确的导航位置的权重设置的大一些，不那么准确的导航位置的权重设置的小一些，具体可以为将导航位置中位于预设差值范围内的第二差值对应的第一导航位置的权重设定为第一预设权重并缓存第一导航位置，将导航位置中除第一导航位置外的第二导航位置的权重设定为第二预设权重并缓存第二导航位置，其中，第一预设权重大于第二预设权重。
由于随着时间的增长，第一时间段是有多个的，因此，针对每个第一时间段都要计算一次第二差值，然后基于第二差值确定导航位置的权重并基于权重进行缓存。
例如：假设包括两个相邻的第一时间段s1和s2，也就是说第一时间段s1的终点为第一时间段s2的起始点，卫星导航系统在第一时间段s1的起始点采集的导航位置为L1，在第一时间段s1的终点也就是在第一时间段s2的起始点采集的导航位置为L2，在第一时间段s2的终点采集的导航位置为L3；
则对卫星导航系统在第一时间段s1内采集的导航速度进行积分得到IMU在第一时间段s1内的平移量M1；
计算卫星导航系统在第一时间段s1的起始点采集的导航位置L1与卫星导航系统在第一时间段s1的终点采集的导航位置L2的第一差值L2-L1；
计算平移量M1与第一差值L2-L1之间的第二差值M1-(L2-L1)；
对卫星导航系统在第一时间段s2内采集的导航速度进行积分得到IMU在第一时间段s2内的平移量M2；
计算卫星导航系统在第一时间段s2的起始点采集的导航位置L2与卫星导航系统在第一时间段s2的终点采集的导航位置L3的第一差值L3-L2；
计算平移量M2与第一差值L3-L2之间的第二差值M2-(L3-L2)；
假设第二差值M1-(L2-L1)位于预设差值范围内，第二差值M2-(L3-L2)不位于预设差值范围内，则将导航位置L1、L2和L3中，位于预设差值范围内的第二差值M1-(L2-L1)对应的第一导航位置L1和L2的权重设定为第一预设权重并缓存第一导航位置L1和L2，将导航位置L1、L2和L3中除第一导航位置L1和L2外的第二导航位置L3的权重设定为第二预设权重并缓存第二导航位置L3。
上述方式为不论导航位置的准确度是否较高都进行缓存，当然，也可以采取不缓存准确度较低的导航位置的方式，具体可以为导航位置中位于预设差值范围内的第二差值对应的第三导航位置的权重设定为第一预设权重并缓存第三导航位置。
由此，基于导航速度与导航位置之间存在一致性的原则，计算在第一时间段内基于导航速度求得的IMU的平移量和基于导航位置求得的第一差值之间的第二差值，判断第二差值是否位于预设差值范围内，并基于判断结果确定导航位置的权重并基于所确定的权重对导航位置进行缓存。
第二种：
步骤S140可以包括：
实时接收卫星发送的卫星信号的信噪比；
确定卫星导航系统在第二时间段内采集的第四导航位置，其中，第二时间段为所接收的信噪比小于预设信噪比阈值的时间段；
将导航位置中除第四导航位置外的第五导航位置的权重设定为第三预设权重并缓存第五导航位置，将第四导航位置的权重设定为第四预设权重并缓存第四导航位置，其中，第三预设权重大于第四预设权重；
或者，将导航位置中除第四导航位置外的第六导航位置的权重设定为第三预设权重并缓存第六导航位置。
由于卫星导航系统采集导航位置时容易受到高楼、林荫、隧道等环境的干扰，使得卫星导航系统采集的导航位置存在较大偏差，而当车辆行驶在高楼、林荫、隧道等环境下时，电子设备所接收到的卫星发送的卫星信号的信噪比较小，因此，可以基于所接收到的卫星信号的信噪比的大小来确定导航速度是否准确，信噪比大则导航速度较为准确，反之则不准确。
具体可以为实时接收卫星发送的卫星信号的信噪比，确定卫星导航系统在第二时间段内采集的第四导航位置，其中，第二时间段为所接收的信噪比小于预设信噪比阈值的时间段，信噪比小说明第四导航位置不准确，因此，将导航位置中除第四导航位置外的第五导航位置的权重设定为第三预设权重并缓存第五导航位置，将第四导航位置的权重设定为第四预设权重并缓存第四导航位置，其中，第三预设权重大于第四预设权重。
上述方式为不论导航位置的准确度是否较高都进行缓存，当然，也可以采取不缓存准确度较低的导航位置的方式，具体可以为将导航位置中除第四导航位置外的第六导航位置的权重设定为第三预设权重并缓存第六导航位置。
由此，基于所接收到的卫星信号的信噪比来判断导航位置是否准确，并基于判断结果确定导航位置的权重并基于所确定的权重对导航位置进行缓存。
150：当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹。
缓存了VIO轨迹以及导航位置后，为了给VIO轨迹添加位置约束，当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，即可基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹。其中，预设长度阈值和预设数量阈值需要根据具体情况来进行确定，示例性的，预设长度阈值可以为200m，预设数量阈值可以为100个，当然，预设长度阈值并不只限于200m，预设数量阈值并不只限于100个。
上述基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹的步骤，可以包括：
基于缓存的导航位置及对应的权重以及缓存的VIO轨迹建立关于优化后的VIO轨迹的argmin函数；
通过非线性优化算法求解所述argmin函数，得到优化后的VIO轨迹。
其中，argmin函数可以为下述公式：为
其中，Xi＝[Ri，Pi]为优化后的VIO轨迹包含的第i个关键帧图像对应的位置和姿态，Ri为缓存的VIO轨迹包含的第i个关键帧图像对应的位置，Pi为为缓存的VIO轨迹包含的第i个关键帧图像对应的姿态，ki为Zi对应的权重，Zi为第i个关键帧图像对应的缓存的导航位置，Xi V＝[Ri V，Pi V]为缓存的VIO轨迹中第i个关键帧图像对应的位置和姿态，Ri V为缓存的VIO轨迹中第i个关键帧图像对应的位置，Pi V为缓存的VIO轨迹中第i个关键帧图像对应的姿态。
通过非线性优化算法求解上述公式，得到优化后的VIO轨迹包含的关键帧图像的位置和姿态，也就得到优化后的VIO轨迹。
参见图3，图3为对缓存的VIO轨迹进行位姿图优化得到优化后的VIO轨迹的示意图，图3中，左侧的实线的线段代表缓存的VIO轨迹，左侧位于虚线线段上的三角形代表关键帧图像对应的准确度较高的缓存的导航位置，不位于虚线线段上的三角形代表关键帧图像对应的准确度较低的缓存的导航位置，左侧的缓存的VIO轨迹和缓存的导航位置经过位姿图优化后得到右侧的优化后的VIO轨迹。
S160：根据优化后的VIO轨迹以及车辆的周围环境感知信息生成高精度地图。
在得到优化后的VIO轨迹后，即可根据优化后的VIO轨迹以及车辆的周围环境感知信息生成高精度地图。本发明实施例提供的方法虽然牺牲了一定导航位置的实时性，但是在高精度地图的生成过程中对实时性的要求较低，因此，所牺牲的实时性可以忽略不计。
本发明实施例提出的将导航速度与导航位置分开使用的方法，可以实时利用导航速度提供尺度约束，同时牺牲一定导航位置的实时性，利用缓存的具有不同权重的导航位置提供位置约束，能够保证得到的VIO轨迹在导航位置存在较大偏差的区域内也可以维持较高的准确度。同时，本发明实施例提出的基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹的方式，可以得到相对精度和绝对精度均较高的VIO轨迹。
由上述内容可知，本实施例可以基于IMU在第一时间段内采集的IMU加速度和IMU角速度，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定相机在所述第一时间段内的第二位置变化量和第二姿态变化量，然后实时将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到在第一坐标系下的IMU在第一时间段内的视觉惯导里程计VIO轨迹并缓存，然后确定导航位置对应的权重并基于所确定的权重对导航位置进行缓存，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹，最后基于优化后的VIO轨迹生成高精度地图。由此，由缓存的VIO轨迹提供关键帧图像的帧间位置约束和姿态约束，缓存的具有不同权重的导航位置提供绝对位置约束，由于在优化缓存的VIO轨迹时是基于缓存的导航位置的不同权重进行优化的，使得准确度较高的导航位置在优化VIO轨迹时所起的作用较大，使得优化后的VIO轨迹的准确度较高，进一步提高了所生成的高精度地图的准确度。
图4为本发明实施例提供的基于VIO和卫星导航系统融合的地图生成装置的一种结构示意图，相机与惯性测量单元IMU均安装于车辆，且所述IMU的采集频率大于所述相机的采集频率，卫星导航系统的采集频率小于所述相机的采集频率，该装置可以包括：
获取模块401，用于获取所述相机采集的所述车辆的周围环境的视频帧图像、所述卫星导航系统采集的导航速度和导航位置以及所述IMU采集的IMU加速度和IMU角速度；
变化量确定模块402，用于从所获取的视频帧图像中确定两相邻关键帧图像，基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量，其中，所述第一时间段为采集所述两个相邻关键帧图像的采集时间点之差；
融合模块403，用于将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，所述第一坐标系为所述相机拍摄第一帧视频帧图像时对应的相机坐标系；
缓存模块404，用于确定所述导航位置对应的权重并基于所确定的权重对所述导航位置进行缓存；
优化模块405，用于当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹；
生成模块406，用于根据所述优化后的VIO轨迹以及所述车辆的周围环境感知信息生成高精度地图。
由上述内容可知，本发明实施例可以基于IMU在第一时间段内采集的IMU加速度和IMU角速度，得到IMU在第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定相机在所述第一时间段内的第二位置变化量和第二姿态变化量，然后实时将第一位置变化量、速度变化量、第一姿态变化量、第二位置变化量、第二姿态变化量和卫星导航系统在第一时间段内采集的导航速度进行融合，得到在第一坐标系下的IMU在第一时间段内的视觉惯导里程计VIO轨迹并缓存，然后确定导航位置对应的权重并基于所确定的权重对导航位置进行缓存，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹，最后基于优化后的VIO轨迹生成高精度地图。由此，由缓存的VIO轨迹提供关键帧图像的帧间位置约束和姿态约束，缓存的具有不同权重的导航位置提供绝对位置约束，由于在优化缓存的VIO轨迹时是基于缓存的导航位置的不同权重进行优化的，使得准确度较高的导航位置在优化VIO轨迹时所起的作用较大，使得优化后的VIO轨迹的准确度较高，进一步提高了所生成的高精度地图的准确度。
在本发明的另一实施例中，所述变化量确定模块402，可以包括：
预积分子模块，用于基于预设预积分公式对所述IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量；
确定子模块，用于根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定所述相机在所述第一时间段的起始点的第一位置和第一姿态，以及在所述第一时间段的终点的第二位置和第二姿态；
位置变化量确定子模块，用于基于所述第一位置和所述第二位置确定所述相机在所述第一时间段内的第二位置变化量；
姿态变化量确定子模块，用于基于所述第一姿态和所述第二姿态确定所述相机在所述第一时间段内的第二姿态变化量。
在本发明的另一实施例中，所述融合模块403，可以包括：
转换子模块，用于基于预设速度变换公式将所述卫星导航系统在所述第一时间段内采集的导航速度转换为IMU速度，其中，所述预设速度变换公式表征导航速度与IMU速度之间的转换关系；
融合子模块，用于根据卡尔曼滤波装置或者非线性优化装置，将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述IMU速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存。
在本发明的另一实施例中，所述缓存模块404，可以包括：
平移量计算子模块，用于对所述卫星导航系统在所述第一时间段内采集的导航速度进行积分得到所述IMU在所述第一时间段内的平移量；
第一差值计算子模块，用于计算所述卫星导航系统在所述第一时间段的起始点采集的导航位置与所述卫星导航系统在所述第一时间段的终点采集的导航位置的第一差值；
第二差值计算子模块，用于计算所述平移量与所述第一差值之间的第二差值；
第一缓存子模块，用于将所述导航位置中位于预设差值范围内的第二差值对应的第一导航位置的权重设定为第一预设权重并缓存所述第一导航位置，将所述导航位置中除所述第一导航位置外的第二导航位置的权重设定为第二预设权重并缓存所述第二导航位置，其中，所述第一预设权重大于所述第二预设权重；
或者，
第二缓存子模块，用于将所述导航位置中位于预设差值范围内的第二差值对应的第三导航位置的权重设定为所述第一预设权重并缓存所述第三导航位置。
在本发明的另一实施例中，所述缓存模块404，可以包括：
接收子模块，用于实时接收卫星发送的卫星信号的信噪比；
第四导航位置确定子模块，用于确定所述卫星导航系统在第二时间段内采集的第四导航位置，其中，所述第二时间段为所接收的信噪比小于预设信噪比阈值的时间段；
第三缓存子模块，用于将所述导航位置中除所述第四导航位置外的第五导航位置的权重设定为第三预设权重并缓存所述第五导航位置，将所述第四导航位置的权重设定为第四预设权重并缓存所述第四导航位置，其中，所述第三预设权重大于所述第四预设权重；
或者，
第四缓存子模块，用于将所述导航位置中除所述第四导航位置外的第六导航位置的权重设定为所述第三预设权重并缓存所述第六导航位置。
在本发明的另一实施例中，所述优化模块405，可以包括：
函数建立子模块，用于基于缓存的导航位置及对应的权重以及缓存的VIO轨迹建立关于优化后的VIO轨迹的argmin函数；
求解子模块，用于通过非线性优化算法求解所述argmin函数，得到优化后的VIO轨迹。
在本发明的另一实施例中，所述预设长度阈值为200m，所述预设数量阈值为100个。
上述装置实施例与方法实施例相对应，与该方法实施例具有同样的技术效果，具体说明参见方法实施例。装置实施例是基于方法实施例得到的，具体的说明可以参见方法实施例部分，此处不再赘述。
本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
本领域普通技术人员可以理解：实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。

Claims (10)

1.一种基于VIO和卫星导航系统融合的地图生成方法，相机与惯性测量单元IMU均安装于车辆，且所述IMU的采集频率大于所述相机的采集频率，卫星导航系统的采集频率小于所述相机的采集频率，其特征在于，包括：
获取所述相机采集的所述车辆的周围环境的视频帧图像、所述卫星导航系统采集的导航速度和导航位置以及所述IMU采集的IMU加速度和IMU角速度；
从所获取的视频帧图像中确定两相邻关键帧图像，基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量，其中，所述第一时间段为采集所述两个相邻关键帧图像的采集时间点之差；
将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，所述第一坐标系为所述相机拍摄第一帧视频帧图像时对应的相机坐标系；
确定所述导航位置对应的权重并基于所确定的权重对所述导航位置进行缓存；
当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹；
根据所述优化后的VIO轨迹以及所述车辆的周围环境感知信息生成高精度地图。
2.如权利要求1所述的方法，其特征在于，所述基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量的步骤，包括：
基于预设预积分公式对所述IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量；
根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定所述相机在所述第一时间段的起始点的第一位置和第一姿态，以及在所述第一时间段的终点的第二位置和第二姿态；
基于所述第一位置和所述第二位置确定所述相机在所述第一时间段内的第二位置变化量；
基于所述第一姿态和所述第二姿态确定所述相机在所述第一时间段内的第二姿态变化量。
3.如权利要求1所述的方法，其特征在于，所述将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存的步骤，包括：
基于预设速度变换公式将所述卫星导航系统在所述第一时间段内采集的导航速度转换为IMU速度，其中，所述预设速度变换公式表征导航速度与IMU速度之间的转换关系；
根据卡尔曼滤波方法或者非线性优化方法，将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述IMU速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存。
4.如权利要求1所述的方法，其特征在于，所述确定所述导航位置的权重并基于所确定的权重对所述导航位置进行缓存的步骤，包括：
对所述卫星导航系统在所述第一时间段内采集的导航速度进行积分得到所述IMU在所述第一时间段内的平移量；
计算所述卫星导航系统在所述第一时间段的起始点采集的导航位置与所述卫星导航系统在所述第一时间段的终点采集的导航位置的第一差值；
计算所述平移量与所述第一差值之间的第二差值；
将所述导航位置中位于预设差值范围内的第二差值对应的第一导航位置的权重设定为第一预设权重并缓存所述第一导航位置，将所述导航位置中除所述第一导航位置外的第二导航位置的权重设定为第二预设权重并缓存所述第二导航位置，其中，所述第一预设权重大于所述第二预设权重；
或者，将所述导航位置中位于预设差值范围内的第二差值对应的第三导航位置的权重设定为所述第一预设权重并缓存所述第三导航位置。
5.如权利要求1所述的方法，其特征在于，所述确定所述导航位置的权重并基于所确定的权重对所述导航位置进行缓存的步骤，包括：
实时接收卫星发送的卫星信号的信噪比；
确定所述卫星导航系统在第二时间段内采集的第四导航位置，其中，所述第二时间段为所接收的信噪比小于预设信噪比阈值的时间段；
将所述导航位置中除所述第四导航位置外的第五导航位置的权重设定为第三预设权重并缓存所述第五导航位置，将所述第四导航位置的权重设定为第四预设权重并缓存所述第四导航位置，其中，所述第三预设权重大于所述第四预设权重；
或者，将所述导航位置中除所述第四导航位置外的第六导航位置的权重设定为所述第三预设权重并缓存所述第六导航位置。
6.如权利要求1所述的方法，其特征在于，所述基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹的步骤，包括：
基于缓存的导航位置及对应的权重以及缓存的VIO轨迹建立关于优化后的VIO轨迹的argmin函数；
通过非线性优化算法求解所述argmin函数，得到优化后的VIO轨迹。
7.如权利要求1所述的方法，其特征在于，所述预设长度阈值为200m，所述预设数量阈值为100个。
8.一种基于VIO和卫星导航系统融合的地图生成装置，相机与惯性测量单元IMU均安装于车辆，且所述IMU的采集频率大于所述相机的采集频率，卫星导航系统的采集频率小于所述相机的采集频率，其特征在于，包括：
获取模块，用于获取所述相机采集的所述车辆的周围环境的视频帧图像、所述卫星导航系统采集的导航速度和导航位置以及所述IMU采集的IMU加速度和IMU角速度；
变化量确定模块，用于从所获取的视频帧图像中确定两相邻关键帧图像，基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量，其中，所述第一时间段为采集所述两个相邻关键帧图像的采集时间点之差；
融合模块，用于将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，所述第一坐标系为所述相机拍摄第一帧视频帧图像时对应的相机坐标系；
缓存模块，用于确定所述导航位置对应的权重并基于所确定的权重对所述导航位置进行缓存；
优化模块，用于当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹；
生成模块，用于根据所述优化后的VIO轨迹以及所述车辆的周围环境感知信息生成高精度地图。
9.如权利要求8所述的装置，其特征在于，所述变化量确定模块，包括：
预积分子模块，用于基于预设预积分公式对所述IMU在第一时间段内采集的IMU加速度和IMU角速度进行预积分，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量；
确定子模块，用于根据两相邻关键帧图像中的特征点的位置以及多视图几何原理，确定所述相机在所述第一时间段的起始点的第一位置和第一姿态，以及在所述第一时间段的终点的第二位置和第二姿态；
位置变化量确定子模块，用于基于所述第一位置和所述第二位置确定所述相机在所述第一时间段内的第二位置变化量；
姿态变化量确定子模块，用于基于所述第一姿态和所述第二姿态确定所述相机在所述第一时间段内的第二姿态变化量。
10.如权利要求8所述的装置，其特征在于，所述融合模块，包括：
转换子模块，用于基于预设速度变换公式将所述卫星导航系统在所述第一时间段内采集的导航速度转换为IMU速度，其中，所述预设速度变换公式表征导航速度与IMU速度之间的转换关系；
融合子模块，用于根据卡尔曼滤波装置或者非线性优化装置，将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述IMU速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的VIO轨迹并缓存。
CN202010069459.2A 2020-01-21 2020-01-21 一种基于vio和卫星导航系统融合的地图生成方法及装置 Active CN113218407B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202010069459.2A CN113218407B (zh) 2020-01-21 2020-01-21 一种基于vio和卫星导航系统融合的地图生成方法及装置
PCT/CN2020/121344 WO2021147391A1 (zh) 2020-01-21 2020-10-16 一种基于vio和卫星导航系统融合的地图生成方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010069459.2A CN113218407B (zh) 2020-01-21 2020-01-21 一种基于vio和卫星导航系统融合的地图生成方法及装置

Publications (2)

Publication Number Publication Date
CN113218407A CN113218407A (zh) 2021-08-06
CN113218407B true CN113218407B (zh) 2024-04-12

Family

ID=76991916

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010069459.2A Active CN113218407B (zh) 2020-01-21 2020-01-21 一种基于vio和卫星导航系统融合的地图生成方法及装置

Country Status (2)

Country Link
CN (1) CN113218407B (zh)
WO (1) WO2021147391A1 (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115493512B (zh) * 2022-08-10 2023-06-13 思看科技(杭州)股份有限公司 数据处理方法、三维扫描系统、电子装置和存储介质
CN115393531B (zh) * 2022-10-27 2023-02-03 安胜（天津）飞行模拟系统有限公司 一种eVTOL导航合成视觉方法及系统
CN115793001B (zh) * 2023-02-07 2023-05-16 立得空间信息技术股份有限公司 一种基于惯导复用的视觉、惯导、卫导融合定位方法
CN115959122B (zh) * 2023-03-10 2023-06-02 杭州枕石智能科技有限公司 泊车场景下车辆定位方法、装置、电子设备及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法
CN110100151A (zh) * 2017-01-04 2019-08-06 高通股份有限公司 在视觉惯性测距中使用全球定位系统速度的系统及方法
CN110136199A (zh) * 2018-11-13 2019-08-16 北京初速度科技有限公司 一种基于摄像头的车辆定位、建图的方法和装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107533801A (zh) * 2013-11-01 2018-01-02 国际智能技术公司 使用测绘车辆的地图测绘技术
CN109341706B (zh) * 2018-10-17 2020-07-03 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN109374008A (zh) * 2018-11-21 2019-02-22 深动科技（北京）有限公司 一种基于三目摄像头的图像采集系统及方法
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN110310304B (zh) * 2019-06-14 2021-07-23 达闼机器人有限公司 单目视觉建图和定位方法、装置、存储介质及移动设备

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110100151A (zh) * 2017-01-04 2019-08-06 高通股份有限公司 在视觉惯性测距中使用全球定位系统速度的系统及方法
CN110136199A (zh) * 2018-11-13 2019-08-16 北京初速度科技有限公司 一种基于摄像头的车辆定位、建图的方法和装置
CN109900265A (zh) * 2019-03-15 2019-06-18 武汉大学 一种camera/mems辅助北斗的机器人定位算法

Also Published As

Publication number Publication date
WO2021147391A1 (zh) 2021-07-29
CN113218407A (zh) 2021-08-06

Similar Documents

Publication Publication Date Title
CN113218407B (zh) 一种基于vio和卫星导航系统融合的地图生成方法及装置
EP3566021B1 (en) Systems and methods for using a global positioning system velocity in visual-inertial odometry
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
US10267924B2 (en) Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry
CN108535755B (zh) 基于mems的gnss/imu车载实时组合导航方法
CN102252676B (zh) 运动姿态数据获取、人体运动姿态追踪方法及相关设备
US9488480B2 (en) Method and apparatus for improved navigation of a moving platform
CN109855621B (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
JP2022518911A (ja) 乗り物のセンサとカメラアレイからの構造化された地図データの生成
CN108235735A (zh) 一种定位方法、装置及电子设备、计算机程序产品
CN111854740B (zh) 能够在交通工具中进行航位推算的惯性导航系统
CN109917440A (zh) 一种组合导航方法、系统及车辆
US11615711B2 (en) Precision localization and geofencing governance system and method for light electric vehicles
KR100525517B1 (ko) 차량 항법 시스템 및 그 제어방법
CN117268370A (zh) 确定移动方向与传感器行进方向间的失准角进行航位推算
CN110595465A (zh) 一种基于gnss和imu的定位定姿系统
CN115683124A (zh) 确定行驶轨迹的方法
CN112729283A (zh) 一种基于深度相机/mems惯导/里程计组合的导航方法
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法
CN113048987A (zh) 一种车载导航系统定位方法
JP2022149051A (ja) 地図生成装置、地図生成システム、地図生成方法、およびプログラム
CN114624754B (zh) 一种时空定位与近场补偿的自动驾驶定位装置与方法
RU2772620C1 (ru) Создание структурированных картографических данных с помощью датчиков транспортного средства и массивов камер
US20230252887A1 (en) Method and device for operating an infrastructure sensor system
CN113885063A (zh) 融合pdr/gnss的室内外无缝定位系统及方法

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