视觉惯性里程计方法、装置及电子设备制造方法及图纸

本公开提供了一种视觉惯性里程计的方法、装置及电子设备，所述方法包括：融合至少一个传感器的位姿信息得到第一初始位姿；根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；根据所述地图点与角点的对应关系来创建后端紧耦合优化；通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。根据本方法，前端采用特征点法和光度法松耦合，后端优化采用特征点法作用和光度法紧耦合的VIO方法，能够满足不同场景下的定位和三维重建，提高算法的鲁棒性。鲁棒性。鲁棒性。

【技术实现步骤摘要】
视觉惯性里程计方法、装置及电子设备


[0001]本公开涉及SLAM(simultaneous localization and mapping，中文名称：即时定位与地图构建，或并发建图与定位)领域，尤其涉及一种视觉惯性里程计(VIO，Visual
‑
Inertial Odometry)的方法、装置及电子设备。

技术介绍

[0002]随着定位导航的发展，尤其是SLAM技术的广泛应用，机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。
[0003]不同场景对SLAM建图、VO(Visual Odometry，VO)定位及三维重建有不同的参数要求。目前视觉SLAM根据原理可分为两类：特征点法SLAM和光度法 SLAM。光度法根据图像中像素梯度变化较大的点的光度误差建立目标函数，优化求解相机的位姿和地图点位置。特征点法首先需要提取图像中的特征点以及描述子，然后根据描述子确定特征点之间的匹配关系，并根据匹配点之间的投影误差建立目标函数。光度法对场景中的纹理要求不高但对光照变化比较敏感，而特征点法对光照变化不敏感但纹理要求较高。因此如何构建满足不同场景需求的SLAM算法是本领域亟需解决的问题。

技术实现思路

[0004]为了解决上述技术问题，满足多种光线场景下的定位及三维重建，本公开实施方式的目的在于提供一种视觉惯性里程计方法、装置、电子设备及存储介质。
[0005]根据本公开第一方面，本公开的实施例提供了一种视觉惯性里程计的方法，包括：
[0006]融合至少一个传感器的位姿信息得到第一初始位姿；
[0007]根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；
[0008]通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；
[0009]根据所述地图点与角点的对应关系来创建后端紧耦合优化；
[0010]通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；
[0011]对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。
[0012]进一步的，所述融合至少一个传感器的位姿信息得到第一初始位姿，包括：
[0013]利用惯性传感器(IMU，Inertial Measurement Unit)的旋转得到位姿信息中的旋转信息；
[0014]利用轮式里程计的平移得到位姿信息中的位移信息；
[0015]将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。
[0016]进一步的，所述根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获
得第二初始位姿，包括：
[0017]通过图像传感器获取图像；
[0018]将彩色图像转化为灰度图像；
[0019]提取所述灰度图像中像素梯度变化大于一定阈值的像素点；
[0020]通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；
[0021]根据所述误差函数构建非线性最小二乘函数；
[0022]利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。
[0023]进一步的，所述光度误差函数表示如下：
[0024]r＝ω
h
(I2[x2]‑
(a
21
I1[x1]+b
21
))
[0025]其中，r表示光度误差，ω
h
为Huber权重，I1，I2表示相邻的两幅灰度图像，x1，x2分别是空间中点X在图像中的像素坐标，x2是由x1投影获得，a，b为光度仿射变换参数。
[0026]x2是由x1投影获得，需要两帧之间的相对位姿变换ξ
21
和x1在I1中的逆深度。
[0027]进一步的，所述非线性最小二乘函数表示如下：
[0028][0029]其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差， i、N为自然数。
[0030]进一步的，所述通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点，包括：
[0031]将彩色图像转化为灰度图像；
[0032]提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；
[0033]从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；
[0034]通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；
[0035]根据关联匹配后的数据构建重投影误差函数；
[0036]根据重投影误差函数对所述第二初始位姿进行特征点优化；
[0037]根据特征点优化后的第二初始位姿和所述角点生成地图点。
[0038]进一步的，所述重投影误差函数表示如下：
[0039][0040]其中，ξ
*
为重投影误差，u
i
是已知的2D齐次坐标，s
i
表示尺度，K为相机的内部参数，ξ
^
是未知的相机位姿，P
i
为已知的3D点的齐次坐标，i、N为自然数。
[0041]进一步的，所述根据所述地图点与角点的对应关系来创建后端紧耦合优化，包括：
[0042]通过所述地图点是否由所述角点生成来创建后端紧耦合优化；
[0043]如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，将所述特征点约束和所述光度约束进行紧耦合；
[0044]如果所述地图点不是由所述角点构成，则包含光度约束，并通过非线性最小二乘求解出待优化变量的更新量。
[0045]进一步的，所述通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵，包括：
[0046]根据光度误差函数和重投影误差函数分别获取所述特征点约束和光度约束中待变化变量之间的导数；
[0047]根据所述导数分别构建特征点约束矩阵和光度约束矩阵。
[0048]进一步的，所述对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵，包括：
[0049]将所述特征点约束矩阵中与所述光度约束矩阵中相同的待优化变量部分的相加，获取待优化变量的更新量；
[0050]对所述待优化变量的更新量进行消元处理，得到最终的约束矩阵。
[0051]进一步的，所述方法还包括：
[0052]对所述特征点约束矩阵中与所述光度约束矩阵中的待优化变量进行更新；包括：
[0053]通过所述地图点是否由角点构成来求解更新量；
[0054]如果所述地图点是由所述角点生成，则所述后端紧耦合优化包含特征点约束和光度约束，如果所述地图点不是由所述角点构成，则包含光度约束；
[0055]对所述待优化变量直接进行增量相加，对所述第二位姿中的旋转信息进行相乘。本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种视觉惯性里程计方法，其特征在于，包括：融合至少一个传感器的位姿信息得到第一初始位姿；根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿；通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点；根据所述地图点与角点的对应关系来创建后端紧耦合优化；通过所述后端紧耦合优化构建特征点约束矩阵和光度约束矩阵；对所述特征点矩阵和所述光度约束矩阵进行约束矩阵的拼接，得到最终约束矩阵。2.根据权利要求1所述的方法，其特征在于，所述融合至少一个传感器的位姿信息得到第一初始位姿，包括：利用惯性传感器的旋转得到位姿信息中的旋转信息；利用轮式里程计的平移得到位姿信息中的位移信息；将所述旋转信息和所述位移信息进行信息融合得到第一初始位置。3.根据权利要求1所述的方法，其特征在于，所述根据两帧图像之间的位姿变换对所述初始位姿进行光度优化，获得第二初始位姿，包括：通过图像传感器获取图像；将彩色图像转化为灰度图像；提取所述灰度图像中像素梯度变化大于一定阈值的像素点；通过光流跟踪得到在下一帧图像中的位置并构建光度误差函数；根据所述误差函数构建非线性最小二乘函数；利用所述非线性最小二乘函数优化所述第一初始位姿，获得第二初始位姿。4.根据权利要求3所述的方法，其特征在于，所述光度误差函数表示如下：r＝ω
h
(I2[x2]
‑
(a
21
I1[x1]+b
21
))其中，r表示光度误差，ω
h
为Huber权重，I1，I2表示相邻的两幅灰度图像，x1，x2分别是空间中点X在图像中的像素坐标，x2是由x1投影获得，a，b为光度仿射变换参数。x2是由x1投影获得，需要两帧之间的相对位姿变换ξ
21
和x1在I1中的逆深度。5.根据权利要求3所述的方法，其特征在于，所述非线性最小二乘函数表示如下：其中，J表示雅可比矩阵，ξ为李代数，表示旋转和平移，r表示光度误差，i、N为自然数。6.根据权利要求1所述的方法，其特征在于，所述通过提取和匹配图像中的特征点对所述第二初始位姿进行特征点优化，生成地图点，包括：将彩色图像转化为灰度图像；提取所述灰度图像中像素梯度变化大于一定阈值的像素点作为所述图像的特征点；从所述图像的特征点中筛选出像素梯度变化大于第二阈值的特征点作为角点；通过所述角点的描述与所述第二初始位姿的数据进行关联匹配；根据关联匹配后的数据构建重投影误差函数；根据重投影误差函数对所述第二初始位姿进行特征点优化；根据特征点优化后的第二初始位姿和所述角点生成地图点。
7.根据权利要求6所述的方法，其特征在于，所述重投影误差函数表示如下：其中，ξ
*
为重投...

【专利技术属性】
技术研发人员：郑泽玲，高军强，
申请(专利权)人：达闼科技北京有限公司，
类型：发明
国别省市：