基于GPS、IMU以及双目视觉的地图构建方法及系统
专利汇可以提供基于GPS、IMU以及双目视觉的地图构建方法及系统专利检索，专利查询，专利分析的服务。并且本 发明 公开一种基于GPS、IMU以及 双目视觉 的地图构建方法及系统，利用IMU以及双目视觉对其进行校正并在隧道等无GPS 信号 或信号弱的地方使用视觉导航。能够很好的解决视觉SLAM中的问题，并提高SLAM系统的鲁棒性。采用了视觉信息和IMU信息融合的方法，将多种 传感器 数据进行融合，提供了新的 算法 结构，构建稳定有效的 框架 ，对无人驾驶 汽车 或 机器人 进行稳定有效的自主 定位 与地图构建，实现GPS的定位信息进行校正及补充，应用到无人驾驶汽车和机器人系统，进行大规模应用。，下面是基于GPS、IMU以及双目视觉的地图构建方法及系统专利的具体信息内容。
1.一种基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，包括以下步骤：
步骤一、初始化无人驾驶汽车或机器人的位姿,确定无人驾驶汽车行驶或机器人运动的轨迹，校正IMU惯性测量单元、双目摄像头等数据保持时间一致性；
步骤二、进行IMU惯性测量单元和双目摄像头以及基于RTK解算的GNSS接收器的信息采集与提取，从而进行视觉特征跟踪；
步骤三、基于流型的IMU预积分，包括：IMU预积分计算、计算预计分雅可比和协方差矩阵、计算残差雅可比；
步骤四、IMU初始化(视觉惯性联合初始化)，这是视觉和惯性数据融合的第一步，是一段松耦合过程，将视觉数据和快速的IMU数据相结合，包括：陀螺仪偏置标定(零偏)、尺度恢复和重力加速度预估计、加速度计偏置标定和尺度重力加速度优化；
步骤五、使用紧耦合优化模型进行优化，包括：视觉惯性相邻帧紧耦合优化、视觉惯性局部地图紧耦合优化；
步骤六、估计无人驾驶汽车或机器人轨迹和周围环境三维地图的相似程度，得出实时位姿；
步骤七、构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。
2.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤2)信息采集与提取中：
在每次双目摄像机采集到图像后，对图像进行ORB特征的提取，提取ORB特征后，将提取的结果存储；
对无人驾驶汽车或机器人内部的IMU数据进行实时采集，并送入基于流型的IMU预积分中进行处理。
3.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法及系统，其特征在于，在步骤3)基于流型的IMU预积分中：
所述的IMU预积分计算中，世界坐标系、IMU坐标系以及相机坐标系通常用W、B、C来表示，以一定的时间间隔Δt来采样，测量IMU的加速度aB和角速度ωB。IMU测量模型：
其中， 表示坐标系B下IMU测量模型的角速度和加速度，BωWB(t)∈R3表示在
坐标系B下IMU中心B相对于世界坐标系W的瞬时角速度，RWB表示IMU中心B相对于世界坐标系W的旋转矩阵， 为RWB的转置，Wa(t)∈R3表示在W坐标下IMU的瞬时加速度，Wg表示在W坐标系下重力矢量，b表示零偏，η表示高斯随机噪声，ba、bg分别表示加速度和重力矢量零偏，ηa、ηg分别表示加速度和重力矢量高斯随机噪声；
旋转R、平移p和速度v的导数可以表示为：
其中，Wv表示在W坐标系下IMU的瞬时速度， 表示B相对于W的瞬时角速度，RWB表示IMU中心B相对于世界坐标系W的旋转矩阵，Wp表示W坐标系下的瞬时平移， 为其
导数；
世界坐标系下的旋转R、平移p和速度v可由一般的积分公式求得,离散时间下采用欧拉积分可以将上面连续时间积分改写并联立可得：
Δt为一定的时间间隔，ηgd(t)、ηad(t)表示离散时间下加速度和重力矢量高斯随机噪声，ba、bg分别表示加速度和重力矢量零偏，ηad、ηgd分别表示加速度和重力矢量高斯随机噪声， 为IMU测量模型的角速度和加速度，这时候的状态量是世界坐标系下的；
设现在有i和j两个相邻关键帧，需要已知i时刻对应的IMU状态量，求解离散时间下j时刻关键帧对应的IMU预积分值：
Δt为一定的时间间隔，Δtij为i、j时刻之间的时间差，g为重力矢量， 分别表示随时间变化的重力矢量和加速度零偏， 表示随时间变化的IMU测量模型角速度， 表示随时间变化的IMU测量模型速度，Rj、Ri、vj、vi、pj、pi分别为j时刻和i时刻的旋转、速度和平移，Rk为随时间变化的旋转矩阵， 为随时间变化的离散加速度和重力矢量高斯随机噪声；
为了避免重复积分采用IMU预积分方法计算i、j时刻的状态量的相对变化，也就是将参考坐标系由世界坐标系转换到IMU的i时刻坐标系下，计算得到IMU状态量在i和j时刻之间的相对量：
Rj、Ri、vj、vi、pj、pi分别为j时刻和i时刻的旋转、速度和平移， 分别为随时间变化的加速度和重力矢量零偏， 表示随时间变化的IMU测量模型角速度， 表示随时间变化的IMU测量模型速度， 为随时间变化的离散加速度和重力矢量高斯随机噪声，ΔRij、Δvij、Δpij为i、j之间的旋转、速度和平移差， 为ΔRi的转置，Δtij为i、j时刻之间的时间差，Δt为一定的时间间隔，ΔRik、Δvik为i、k时刻的旋转和速度差；
从上面的公式可以看出，整个预积分公式和偏置，噪声都有关系，需要分别分析偏置和噪声对系统的影响，噪声服从高斯正态分布，偏执服从随机游走模型；
在初始化使用的IMU预积分是没有偏置的一阶项，雅可比矩阵是在初始化完成之后进行优化之前才计算而来的，得到IMU准确的预积分值(真值)，和估计值从而得到IMU的残差；
系统是服从高斯分布的，协方差矩阵是按照高斯分布计算得到，是个9*9的矩阵；
所述计算残差雅可比：
残差：
待估计的参数：
ΔRij、Δvij、Δpij为i、j时刻的旋转、速度和平移差， 为器平均值， 为i
时刻的加速度和重力矢量零偏， 为其平均值，δba、δbg为其更新量，φ为旋转，Δtij帧间时间差，Ri、Rj、vi、vj、pi、pj表示i、j时刻的旋转矩阵、速度和平移， 为其转置；
位移残差对应的雅可比如上，j时刻相应的雅可比也可如此推导，速度残差和旋转的雅可比公式与平移相似。至此IMU预积分完成。
4.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤4)IMU初始化(视觉惯性联合初始化)中：
利用两个联系关键帧的旋转量来估计陀螺仪的偏置，在一个常量bg的基础上加上一个微小 的改变，通过最小化旋转误差来计算出陀螺仪偏置的更新量：
其中，N是关键帧的个数，ΔRi,i+1是两个连续关键帧预积分旋转测量量，J为协方差矩阵， 分别为i+1时刻的B坐标系相对于W坐标系和i时刻的W坐标系相对于B坐标系的旋转矩阵；给以零初始值，上述函数通过G2O优化，求解最小目标函数对应的 即为陀螺仪的初始偏置；
当求出零偏后将其代入预积分公式会重新计算一遍预积分值，使预积分数值更加准确；
要进行尺度恢复和重力加速度预估计，首先需要建立预估计状态向量X＝[s,gW],其中，s是尺度，gW是世界坐标系下的重力加速度；
这里使用了三个关键帧(用1、2、3来表示)联立视觉和IMU预积分数据构建一个AX＝B的最小二乘超定方程，至少需要四个关键帧，采用奇异值分解求最小二乘问题，速度较慢但是精度高；
I为单位矩阵，WPC为W坐标系下相机中心C的位移，RWB、RWC为W坐标系相对于B坐标系和C坐标系的旋转矩阵，Δtnm为两帧之间的时间差(n,m＝1,2,3)；
由于尺度恢复和重力加速度预估计的计算过程没有考虑到加速度计偏置的影响，使得重力加速度和加速度计偏置难以区分，很可能导致系统病态性问题，所以进行加速度计偏置标定和尺度重力加速度优化；
设惯性坐标系I的重力加速度方向 gW的归一化单位矢量
θ为角度。然后计算世界坐标系W和惯性坐标系I之间的旋转矩阵：
然后把重力加速度表示成：
由于RWI是绕Z轴的旋转，所以只受X,Y方向的影响：
包含加速度的偏置：
采用三个连续的关键帧来消除速度量：
其中[](:,1:2)表示矩阵的前两列，加速度偏置更新量 δθ为微小角度变量，WPC为W坐标系下相机中心C的位移，CPB为IMU中心B在C坐标系下的平移，s是尺度，Δtnm、Δpnm、Δvnm分别是两帧之间的时间、位移和速度差(n,m＝1,2,3)，RWB、RWC、RWI为W坐标系相对于B坐标系、C坐标系和I坐标系的旋转矩阵， 为I坐标系下gI的归一化单位矢量，J为协方差矩阵；只优化重力加速度的方向，每次优化得到的数据都会被重新代入计算出最新的gW继续优化，直达变量收敛。
5.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤5)使用紧耦合优化模型进行优化中：
视觉惯性相邻帧紧耦合优化存在两种优化方式：(1)出现地图更新：
首先构建整体优化状态向量，包含当前时刻j的旋转 平移速度 位移 加速
度计偏置 和陀螺仪偏置 整体误差方程包含视觉重投影误差Eproj和IMU测量误差EIMU；
视觉重投影方程就是简单的针孔相机重投影方程，并没有什么特别其残差，雅可比计算和信息矩阵与一般视觉无异；
IMU测量误差方程包含两部分，分别是P,V,R和ba,bg偏置；其残差，残差雅可比矩阵和信息矩阵在IMU预积分过程已经结算完成，直接带入即可：
eb＝bj-bj (40)
eR、ep、ev、eb为旋转、平移、速度和偏置误差，RWB、RWC、RWI为W坐标系相对于B坐标系、C坐标系和I坐标系的旋转矩阵，b为偏置；
(2)没出现地图更新：
如上所述，首先构建整体优化状态向量，包括当前时刻j+1和上一时刻j的旋转
平移速度 位移 加速度计偏置 和陀螺仪偏置 整体
误差方程包含视觉重投影误差Eproj、IMU测量误差EIMU和先验误差Eprior：
视觉和IMU的误差方程和前面相同，先验误差是j时刻的状态量；
eR、ep、ev、eb为旋转、平移、速度和偏置误差， 为其转置，RWB为W坐标系相对于B坐标系旋转矩阵，RBW为B坐标系相对于W坐标系旋转矩阵，b为偏置，ρ为系数，WpB、WvB为IMU中心B在W坐标系下的平移和速度；
视觉惯性局部地图紧耦合优化时，需要优化的关键帧包含有视觉重投影误差和IMU测量误差。
6.根据权利要求1所述的基于GPS、IMU以及双目视觉的地图构建方法，其特征在于，在步骤7)的闭环检测中，构建地图并利用词袋模型进行闭环检测，对已有地图进行校正，进一步提高三维地图的精度。
7.一种基于GPS、IMU以及双目视觉的地图构建系统，其特征在于：
该系统包括车载运算中心、双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器；
所述车载运算中心将ROS框架将双目摄像头、IMU惯性测量单元和基于RTK解算的GNSS接收器采集到的数据进行存储以及处理；
所述基于RTK解算的GNSS接收器接收GPS定位信息；
所述双目摄像机采集周围图像信息进行点云建模；
所述IMU惯性测量单元采集测量汽车三轴姿态角以及加速度；
硬件层面融合可以将双目摄像头和IMU集成在整个电路板上，全部信号传输都是在电路板板内完成，直接在控制视觉系统的微处理器以及控制IMU的微处理器之间进行数据的交换，通过视觉处理把环境或是目标信息更细节的东西提升给相关算法之后，可以提升整个系统的性能。也可以采用纯数据的交互和融合，通过数据总线进行传感器间数据的交换。
双目摄像头、IMU惯性测量单元、基于RTK解算的GNSS接收器的数据输出端口通过数据总线将采集到的数据输入到车载运算中心中，在车载运算中心中进行数据的存储以及通过算法对双目摄像头、IMU惯性测量单元数据进行融合并对基于RTK解算的GNSS接收器得到的GPS信号进行校正。
基于GPS、IMU以及双目视觉的地图构建方法及系统
技术领域：
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 基于电热法破碎技术的绿色拆除方法 | 2020-05-11 | 341 |
| 一种致密岩石的饱和装置及方法 | 2020-05-12 | 417 |
| 一种给水管道交叉布置方法 | 2020-05-13 | 772 |
| 一种悬挂式网络工控机 | 2020-05-08 | 890 |
| 一种穿越浅埋偏压松散堆积体大跨度隧道综合进洞结构施工方法 | 2020-05-11 | 432 |
| 一种飞行/推进系统/喷流噪声综合实时模型建模方法 | 2020-05-13 | 759 |
| 一种超临界锅炉的启动计算方法及系统 | 2020-05-08 | 374 |
| 一种轴承式运动解耦竖向隔振装置 | 2020-05-12 | 800 |
| 一种建筑地板铺设装置 | 2020-05-13 | 264 |
| 带麦克风的重力支架 | 2020-05-08 | 770 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
