一种基于VIO和卫星导航系统融合的地图生成方法及装置制造方法及图纸

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

【技术实现步骤摘要】
一种基于VIO和卫星导航系统融合的地图生成方法及装置
本专利技术涉及地图生成
，具体而言，涉及一种基于VIO和卫星导航系统融合的地图生成方法及装置。
技术介绍
VIO(VisualInertialOdometry,视觉惯性里程计)是近些年受到广泛关注的一种定位方法，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和卫星导航系统融合的地图生成装置，相机与本文档来自技高网...

【技术保护点】
1.一种基于VIO和卫星导航系统融合的地图生成方法，相机与惯性测量单元IMU均安装于车辆，且所述IMU的采集频率大于所述相机的采集频率，卫星导航系统的采集频率小于所述相机的采集频率，其特征在于，包括：/n获取所述相机采集的所述车辆的周围环境的视频帧图像、所述卫星导航系统采集的导航速度和导航位置以及所述IMU采集的IMU加速度和IMU角速度；/n从所获取的视频帧图像中确定两相邻关键帧图像，基于所述IMU在第一时间段内采集的IMU加速度和IMU角速度，得到所述IMU在所述第一时间段内的第一位置变化量、速度变化量和第一姿态变化量，基于两相邻关键帧图像中的特征点的位置，确定所述相机在所述第一时间段内的第二位置变化量和第二姿态变化量，其中，所述第一时间段为采集所述两个相邻关键帧图像的采集时间点之差；/n将所述第一位置变化量、所述速度变化量、所述第一姿态变化量、所述第二位置变化量、所述第二姿态变化量和所述卫星导航系统在所述第一时间段内采集的导航速度进行融合，得到在第一坐标系下的所述IMU在所述第一时间段内的视觉惯导里程计VIO轨迹并缓存，其中，所述第一坐标系为所述相机拍摄第一帧视频帧图像时对应的相机坐标系；/n确定所述导航位置对应的权重并基于所确定的权重对所述导航位置进行缓存；/n当缓存的VIO轨迹达到预设长度阈值或缓存的导航位置的数量达到预设数量阈值时，基于缓存的导航位置及对应的权重对缓存的VIO轨迹进行位姿图优化，得到优化后的VIO轨迹；/n根据所述优化后的VIO轨迹以及所述车辆的周围环境感知信息生成高精度地图。/n...

【技术特征摘要】
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.如权利要...

【专利技术属性】
技术研发人员：罗金辉，王笑非，穆北鹏，
申请(专利权)人：北京初速度科技有限公司，
类型：发明
国别省市：北京;11