(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210459395.6 (22)申请日 2022.04.27 (71)申请人 华中科技大 学 地址 430074 湖北省武汉市洪山区珞喻路 1037号 (72)发明人 李益群　孙林帅　尹周平　 (74)专利代理 机构 华中科技大 学专利中心 42201 专利代理师 孔娜 (51)Int.Cl. G01C 21/16(2006.01) G01C 21/20(2006.01) G01C 21/32(2006.01) G06T 7/73(2017.01) (54)发明名称 基于MAP及双目VIO建立占据栅格地图的方 法及应用 (57)摘要 本发明属于轮式机器人定位及建图相关技 术领域， 其公开了一种基于MAP及双目VIO建立占 据栅格地图的方法及应用， 包 括以下步骤： (1)计 算视觉重投影残差以及对应的协方差矩阵； 同 时， 计算IMU测量残差以及对应的协方差矩阵； (2)利用MAP构建最小二乘优化模型， 以实现视 觉‑IMU联合位姿优化； (3)建立关键帧与得到的 地图点之间的联系， 将经最小二乘优化模型优化 得到的当前地图点添加索引， 并将索引指向当前 相机位姿所对应的关键帧， 从而构建特征点云地 图； (4)基于当前关键帧及特征点云地图建立占 据栅格地图。 本发明提高了鲁棒性， 且同时具有 定位及建图两 个功能。 权利要求书3页 说明书9页 附图3页 CN 114894182 A 2022.08.12 CN 114894182 A 1.一种基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于， 该方法包括以下步 骤： (1)对双目相机获取的图像进行特征点的提取与匹配， 计算两帧相邻图像之间的相对 位姿并选取关键帧， 进而计算视觉重投影残差以及对应的协方差矩阵； 同时， 对IMU采集到 的数据进行处 理以得到IMU测量残差以及对应的协方差矩阵； (2)根据所计算得到的视觉重投影残差及其协方差矩阵、 IMU测量残差及其协方差矩 阵， 利用MAP构建最小二乘优化模 型， 以实现视觉 ‑IMU联合位姿优化； 其中， 最小二乘优化模 型的表达式为： 其中 α为IMU残差项的权重， β为视觉残差项的权重， H(e )为Huber核函 数， v0为所设定的速度阈值； (3)建立关键帧与得到的地图点之间的联系， 将经最小二乘优化模型优化得到的当前 地图点添加索引， 并将索引指向当前相机位姿所对应的关键帧， 从而构建特 征点云地图； (4)基于当前关键帧及特 征点云地图建立占据栅格地图。 2.如权利 要求1所述的基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于： 通过 IMU预积分获取目标的运行速度， 如果速度大于系统所设定的阈值， 则利用最大后验估计进 行视觉‑IMU联合位姿优化时， 提高IMU 预积分残差项的权重； 如果速度小于系统所设定的阈 值时， 则利用最大后验 估计进行视 觉‑IMU联合位姿优化时， 提高视 觉重投影残差项的权 重。 3.如权利 要求1所述的基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于： 速度 阈值v0的大小取决于目标在当前环境下运行的最大极限速度和最小极限速度， 表达为： v0＝f(vmax,vmin) 其中vmax是目标在当前环境下运行的最大极限速度， vmin是目标在当前环境下运行的最 小极限速度； 该阈值能取 所设定的最高运行速度和最低运行速度的均值， 即 4.如权利要求1所述的基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于： IMU 残差项的权重α和 视觉残差项的权重β 的大小与相机和IMU的采样频率、 当前物体移动的速 度均相关， 表达为： ( α, β )＝g(FSC,FSI,v) 其中， FSC是相机采样频率， FSI是IMU采样频率， v是当前目标的运动速度。 5.如权利要求1 ‑4任一项所述的基于MAP及双目VIO建立占据栅格地 图的方法， 其特征 在于： 对IMU数据的处 理包括以下步骤： 3.1、 建立 IMU数学模型， IMU数 学模型的表达式为： 权　利　要　求　书 1/3 页 2 CN 114894182 A 2其中， 带上标的 代表传感器的测量值， 不带上标的ω、 a代表运动物体的真实值， bg、 ba表示传感器的随机游走误差， ηg、 ηa表示传感器的测量 噪声， RIW是世界坐标系 到IMU坐 标系的相对位姿； 3.2、 计算 IMU测量残差， 所采用的公式为： 其中， 表示IMU从第i个关键帧到第j个关键帧的预积分的测量残差的集合， 分别表示IMU从第i个关键帧到第j个关键帧的姿态、 速度、 位置的预积分 的测量残差 。 6.如权利 要求5所述的基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于： 对双 目相机获得的图像的处 理包括以下步骤： 2.1使用ORB算法对双目相机获取的图像的当前左右帧进行特征点提取与匹配， 利用左 右帧对同一特征点观测的视差和双目相机的内参解算出该特征点的深度值， 并将该特征点 标记为3D地图点； 2.2基于一组匹配好的3D地图点计算两帧相邻图像之间的相对位姿 T； 2.3选取关键帧； 2.4计算视 觉重投影残差及其对应的协方差矩阵。 7.如权利 要求6所述的基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于： 视觉 重投影误差ri,j为 其中， ui,j为第j个3D地图点在i个关键帧中所对应的像素的坐标值， h(Ripj)为相机坐标 系中的3D坐 标值到像素平面2D坐 标值的转 换， Ri为第i个关键帧的姿态， Pj为第j个3D地图点 在世界坐标系下的坐标。 8.如权利 要求6所述的基于MAP及双目VIO建立占据栅格地图的方法， 其特征在于： 选取 关键帧的原则为： 首先， 当前帧与前一帧所匹配成功的特征点数目大于或等于系统所设定 阈值的1.2倍， 即 n≥1.2n0 其中n为当前帧与前一帧所匹配成功的特征点数目， n0为系统所设定阈值， 根据机器人 具体运动的场景而定； 其次， 当前帧距离上一关键帧至少经 过的图像帧数为相机帧率的1.2倍， 即 N‑NlastKF≥1.2F 其中N当前帧的序列数， NlastKF为上一关键帧的序列数， F为相机的帧率， 单位 为fps。 9.如权利要求1 ‑4任一项所述的基于MAP及双目VIO建立占据栅格地 图的方法， 其特征权　利　要　求　书 2/3 页 3 CN 114894182 A 3