基于网格分割与双地图耦合的机器人RGB-DSLAM方法

基于网格分割与双地图耦合的机器人rgb-d slam方法
技术领域
1.本发明涉及一种室内动态环境的移动机器人视觉同步定位与建图方法。


背景技术：

2.同时定位与地图构建(simultaneous localization and mapping,slam)是移动机器人实现自主导航和完成复杂环境交互任务的基础，使得机器人可以在未知环境中一方面确定自身位置，另一方面构建场景的地图。rgb-d(rgb-depth)相机可以直接采集到彩色和深度图像且成本较低，因此基于rgb-d相机的视觉 slam系统被广泛应用于室内移动机器人等领域。实际室内场景中常常包含各种运动物体，对slam系统形成位姿估计和地图构建两方面的干扰。
3.现有的视觉slam系统大部分都假设场景为静态，并根据这一假设简化位姿估计。这些slam算法将动态特征点加入位姿计算，在稀疏点云地图中生成对应的错误地图点，使位姿估计产生偏差。因此，需要一种有效的方法区分动态特征或区域。运动物体还将影响slam系统的稠密建图，错误的位姿估计使得算法错误叠加多帧观测信息，造成地图扭曲。即便位姿估计正确，同一运动物体在不同时刻的信息被多次加入地图将形成物体运动的“残影”，无法正确反映场景的真实状态。
4.而针对室内动态环境，现有的解决方案大都在稠密建图或者计算效率方面存在不足，因此在保证计算效率、不依赖高性能硬件的同时，实现具备实时稠密建图能力的视觉slam具有重要意义。


技术实现要素：

5.为了克服现有技术在动态环境下受到动态物体干扰而发生位姿估计漂移、地图构建错误的不足，本发明提出一种基于网格分割与双地图耦合的机器人rgb-dslam方法，适用于室内动态环境，提升了移动机器人slam方法在该工况下的性能。
6.本发明解决其技术问题所采用的技术方案是：
7.一种基于网格分割与双地图耦合的机器人rgb-d slam方法，包括以下步骤：
8.步骤1：对rgb-d相机拍摄的图像，提取并匹配灰度图像中的orb特征点，基于单应变换原理对图像进行单应运动补偿，并使用根据光流理论设计的双向补偿光流法来识别图像中的动态特征点，过程如下：
9.步骤1.1：对由rgb-d相机拍摄的图像，提取灰度图像中的orb特征点并进行特征匹配；
10.步骤1.2：首先求解单应矩阵，单应矩阵描述两个平面之间的映射关系，单应变换前后点的坐标对应关系由式(1)计算，在图像包含明确前景和背景且运动前景不占据画面的绝大部分像素的情形下，单应变换可以补偿由于相机自身运动引起的图像背景变化。
11.x
t 1
＝h
t 1,t
x
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
12.其中，h
t 1,t
为第t帧到第t 1帧之间的3
×
3单应矩阵，x
t
和x
t 1
分别为第t 帧和第t
1帧之间对应点的齐次坐标，由于x
t
和x
t 1
为齐次坐标，该等式为齐次坐标等式，即等式两边乘以任意非0的常数仍然成立，将式(1)表示为(2)的形式；
[0013][0014]
其中，x
t
＝[x
t y
t 1]
t
，x
t 1
＝[x
t 1 y
t 1 1]
t
，h
11
至h
33
为单应矩阵h
t 1,t
各元素，k为任意非0常数；
[0015]
两帧图像的单应矩阵计算依赖两帧之间的对应点信息，由于实际图像中的特征点对不可能全部满足同一刚体透视变换，使用lmeds(least median of squares) 方法确定参与单应矩阵计算的内点，使用levenberg-marquardt方法迭代求解单应矩阵；
[0016][0017]
其中，(x
i,t
,y
i,t
)和(x
i,t 1
,y
i,t 1
)分别为第i个t帧和t 1帧中的对应坐标，median 表示取所有数据样本的中值；
[0018]
如式(3)，将变换后坐标和与实际对应坐标之间的像素距离作为误差项， lmeds的求解使得选出点集误差的中值最小化；
[0019]
所需的两帧之间的对应点信息，通过光流法求得；将第t帧中提取的orb特征点利用lucas-kanade稀疏光流法向第t 1帧追踪得到对应点位置；
[0020]
步骤1.3：提出基于光流原理的双向补偿光流法，步骤1.2已经对前后两帧t与t 1进行前向光流计算，根据对应点信息计算单应矩阵，对第t帧图像应用该单应变换得到补偿图像，此时从第t 1帧向补偿图像做反向光流追踪得到消除干扰的反向光流场；
[0021]
在双向补偿光流过程中，记第t帧的时刻为t，t时刻位于(x,y)处的像素，在 t dt时刻运动到(x dx,y dy)处，根据光流法中的灰度不变假设，有前向光流灰度关系式(4)，其中i(x,y,t)表示t时刻(x,y)处的像素灰度，i(x dx,y dy,t dt)表示t dt时刻(x dx,y dy)处的像素灰度，前向光流的意义主要在于建立前后两帧之间的对应点联系，计算单应矩阵；
[0022]
i(x dx,y dy,t dt)＝i(x,y,t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0023]
i(xb,yb,t)＝i(x dx,y dy,t dt)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0024]
反向光流过程中，由t dt时刻的像素位置(x dx,y dy)在经单应补偿的t 时刻图像中计算得到对应点位置(xb,yb)，用i(xb,yb,t)表示该时刻和位置的像素灰度，有反向光流灰度关系式(5)；
[0025]
前向光流的光流矢量为式(6)，反向光流的光流矢量定义为该过程中原本光流失量的相反矢量，如式(7)；
[0026]vforward
＝(x dx,x dy)-(x,y)＝(dx,dy)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0027][0028]
其中，v
forward
为前向光流矢量，v
backward
为反向光流矢量；
[0029]
两个矢量的差值(x
–
xb,y-yb)表征了单应运动补偿对于相机自身运动引起的背景运动的补偿，使得v
backward
接近于运动前景的位移；
[0030]
和前向光流的结果相比，反向光流计算后静止背景中特征点的移动得到了有效的抵消，从而反向光流矢量可以一定程度表征一个特征点的运动方向和速度；
[0031]
从多帧图像序列角度考虑，双向补偿光流法为了提高可靠性，在计算t帧和t 1帧之间的反向光流场1的基础上，同理计算t帧和t 2帧之间的反向光流场2 并对比分析两个光流场中的光流失量；
[0032]
除了一些显著错误的点，反向光流场的时间跨度更长，运动前景的移动更为显著，按以下条件判断特征点是否做稳定的运动：
[0033]
1)两个矢量的模均大于一定阈值δ；
[0034]
2)场2中矢量的模大于场1中对应矢量模；
[0035]
3)两矢量的夹角小于180
°
，即内积大于0；
[0036][0037]
以上条件表述为式(8)，v1和v2分别为两个光流场中的对应光流矢量，δ选取为一个极小的值，受运动程度影响小；
[0038]
步骤2：将输入的图像划分为矩形网格区域，将动态特征点转换为动态区域表示，并根据几何连通性和深度值聚类方法优化动态区域，得到最终的网格化运动分割图像，过程如下：
[0039]
步骤2.1：将输入的图像划分为20
×
20的矩形网格区域。通过网格划分将动态特征点转换为动态区域，将含有动态特征点的区块标记为疑似动态区块，同时对每个矩形区域内的动态特征点数量进行计数；
[0040]
步骤2.2：在得到的动态区域结果基础上，基于几何连通性处理其中的孤立区块与被包围区块；若某动态区块孤立且动态特征点计数少于设定阈值(取为3)，则标记该区块为静止；若某静态区块被较多动态区块包围，则标记该区块为运动；
[0041]
步骤2.3：将深度图像降采样至20
×
20分辨率，对应所划分的图像网格，降低聚类算法计算量；以深度值为特征，使用k-means 方法进行深度值聚类，聚类结果能够在一定程度上区分不同深度层次物体，参照深度聚类结果扩展和填充动态区域，若动态区域中的一个连通区域与某聚类簇内的一个连通区域重合度高于设定的阈值ε，将动态区域周围属于同一聚类簇的非动态区域，也标记为动态区域；
[0042][0043]
重合度r的计算方法如式(9)所示，s为重合区域面积，s1为聚类簇连通区域面积，s2为动态区域面积；
[0044]
步骤3：根据网格分割对当前帧中的特征点运动性质做出判断，初始化稀疏点云地图，利用最小化重投影误差的方式计算当前帧去除动态影响的相机位姿；过程如下：
[0045]
步骤3.1：若输入图像为视频流中的第一帧，使用该帧特征点的深度测量值计算三维点坐标，插入到地图中，进行稀疏点云地图的初始化；
[0046]
步骤3.2：结合rgb图像和深度图像得出动态区域分割结果后，根据网格分割对当前帧中的特征点运动性质做出判断，落在动态区域内的特征点为动态特征点，记为集合χs；
[0047]
步骤3.3：利用最小化重投影误差的方式计算当前帧的相机位姿，如式(10)，其中，不对动态特征点计算误差项，不加入优化问题；
[0048][0049]
其中，t
cw
为世界坐标系到相机坐标系的坐标变换，即相机的位姿，ρ(
·
)为huber鲁棒损失函数，xi为静态特征点坐标，pi为对应的三维点坐标；
[0050]
π(
·
)为相机的投影函数，对某个三维点[x y z]
t
，有：
[0051][0052]
其中所涉及的其余变量为相机内参；
[0053]
σ为信息矩阵，有：
[0054]
σ＝n
·eꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0055]
n为当前特征点所在图像金字塔的层数，e为3
×
3的单位矩阵；
[0056]
利用高斯-牛顿方法对最小化重投影误差问题进行求解，优化得到相机的位姿估计；
[0057]
步骤4：结合相机位姿、rgb-d图像、网格化运动分割图像，构建场景的静态八叉树地图，过程如下：
[0058]
步骤4.1：为了降低计算量、减少冗余，更新地图时对图像进行1/4降采样，在行和列上每4个像素选取1个像素进行更新；将网格化运动分割应用到八叉树地图的建立当中，算法只根据静态区域内的图像信息更新八叉树地图，不在八叉树地图当中插入动态区域对应节点，一定程度避免动态物体的信息被记录到地图当中；
[0059]
步骤4.2：在更新地图的时候，采用一种光线遍历方法，从相机光心o向平面像素点pi对应的空间点pi投射一道光线，遍历光线穿越的所有节点，存在以下三种情况：
[0060]
(4.2.1)光线穿过若干数量的被占据节点，则将路径上所有的被占据节点置为空，将末端节点置为占据；
[0061]
(4.2.2)光线未穿过被占据节点，但末端落在被占据节点上，则不更新地图；
[0062]
(4.2.3)光线未穿过被占据节点，且末端不落在被占据节点上，则将末端节点置为占据；
[0063]
八叉树中的光线遍历计算，通过三维数值微分算法实现；
[0064]
步骤5：逐步构建场景的稀疏点云地图，在这一过程中结合八叉树地图进行双地图耦合，在关键帧上使用基于网格分割和八叉树地图光线遍历的方法筛选静态地图点，更新稀疏点云地图，保障定位精度，过程如下：
[0065]
步骤5.1：在关键帧上，通过rgb-d相机的深度测量结果，还原部分orb 特征点在三
维空间中的坐标，将他们加入到稀疏点云地图中用于特征匹配和位姿计算；
[0066]
步骤5.2：属于动态物体的地图点被加入地图后，将导致后期在该位置上进行位姿跟踪时产生错误匹配，影响位姿估计精度；为了减轻这种效应，可以使用运动分割生成的动态区域掩膜帮助稀疏点云地图筛选一部分动态地图点。除此之外，还将采用一种基于八叉树地图光线遍历法的方法进一步剔除一部分动态地图点，实现两种地图之间的耦合，通过先前对场景三维结构的认知来指导稀疏点云建图；
[0067]
根据特征点的二维坐标可以在深度图中查询对应的深度值，记为zc；进一步将特征点的二维坐标p＝[u v 1]
t
反投影为相机坐标系中的三维坐标 pc＝[x
c y
c zc]
t
，元素对应坐标各轴分量，k为相机内参矩阵：
[0068]
pc＝zck-1
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0069]
根据相机位姿变换t
cw
得到该点在世界坐标系中的坐标pw＝[x y z]
t
：
[0070]
pw＝t
cw-1
pc＝r-1
(p
c-t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0071]
相机位姿变换t
cw
表示为旋转矩阵r和平移向量t，相机在世界坐标系中的坐标p
cam
即为t，表示为三轴分量的形式，有：
[0072]
p
cam
＝t＝[x
cam y
cam z
cam
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0073]
在八叉树地图中进行从p
cam
向pw方向的光线遍历，投射的光线为一条射线，而不仅仅遍历两点之间的节点；记第一个被穿越节点的坐标为pw′
＝[x
′ꢀy′ꢀz′
]
t
，变换到相机坐标系，得到该点的深度值zc′
；由于八叉树地图中信息根据先前关键帧更新，在八叉树中进行光线遍历得到的深度值与相机测量得到的深度值有较大出入的地图点很可能在关键帧之间发生运动；
[0074]
rgb-d相机在测量较远物体的深度时误差较大，因此只对4m内的地图点做深度值检查，当遍历深度zc′
在测量深度zc附近的误差范围内，地图点得到保留，被插入到稀疏点云地图中，将该过程表示为式(16)：
[0075][0076]
其中δ为误差范围大小，这一过程的误差来源于采用的rgb-d相机的测量误差、位姿估计带来的误差、八叉树建模的精度，表示为：
[0077]
δ＝δz δ
t
δoꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0078]
其中，δz为rgb-d相机对深度的测量误差，δ
t
为位姿估计的误差，δo为八叉树建模精度的误差，δz是关于测量的实际深度的函数，实际深度越大，误差越大，可以对所用的rgb-d相机进行合理建模和误差分析得到关系式。
[0079]
本发明的技术构思为：本发明是一种适用于室内动态环境的基于网格分割与双地图耦合的机器人rgb-d slam方法，提出网格分割方法，代替像素级分割，引入单应运动补偿与根据光流理论设计的双向补偿光流法，结合几何连通性与深度值聚类，进行逐帧动态区域分割，避免动态特征点参与位姿优化，并大幅优化计算速度。同时构建稀疏点云地图和静态八叉树地图，并进行耦合，在关键帧上使用基于网格分割和八叉树地图光线遍历的方法筛选静态地图点，更新稀疏点云地图，保障定位精度。
[0080]
本发明的有益效果主要表现在：(1)对硬件要求较低，消除动态场景干扰，定位精度高；(2)计算效率高；(3)动态物体分割和标记；(4)在线稠密建图。
附图说明
[0081]
图1是本发明具体实施方式的系统流程图；
[0082]
图2是本发明具体实施方式的双向补偿光流原理示意图；
[0083]
图3是本发明具体实施方式的多帧光流信息对比融合示意图；
[0084]
图4是本发明具体实施方式得到的网格化运动分割；
[0085]
图5是本发明具体实施方式的光线遍历法原理示意图；
[0086]
图6是本发明具体实施方式构建的场景稀疏点云地图；
[0087]
图7是本发明具体实施方式构建的场景八叉树地图。
具体实施方式
[0088]
下面结合附图对本发明作进一步描述。
[0089]
参照图1～图7，一种基于网格分割与双地图耦合的机器人rgb-d slam方法包括以下步骤：
[0090]
步骤1：对rgb-d相机拍摄的图像，提取并匹配灰度图像中的orb特征点，基于单应变换原理对图像进行单应运动补偿，并使用根据光流理论设计的双向补偿光流法来识别图像中的动态特征点，过程如下：
[0091]
步骤1.1：对由rgb-d相机拍摄的图像，提取灰度图像中的orb特征点并进行特征匹配。
[0092]
步骤1.2：首先求解单应矩阵，单应矩阵描述两个平面之间的映射关系，单应变换前后点的坐标对应关系由式(1)计算，在图像包含明确前景和背景且运动前景不占据画面的绝大部分像素的情形下，单应变换补偿由于相机自身运动引起的图像背景变化。
[0093]
x
t 1
＝h
t 1,t
x
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0094]
其中，h
t 1,t
为第t帧到第t 1帧之间的3
×
3单应矩阵，x
t
和x
t 1
分别为第t 帧和第t 1帧之间的对应点的齐次坐标，由于x
t
和x
t 1
为齐次坐标，该等式为齐次坐标等式，即等式两边乘以任意非0的常数仍然成立，将式(1)表示为(2) 的形式；
[0095][0096]
其中，x
t
＝[x
t y
t 1]
t
，x
t 1
＝[x
t 1 y
t 1 1]
t
，h
11
至h
33
为单应矩阵h
t 1,t
各元素，k为任意非0常数；
[0097]
两帧图像的单应矩阵计算依赖两帧之间的对应点信息。由于实际图像中的特征点对不可能全部满足同一刚体透视变换，使用lmeds(least median of squares) 方法确定参与单应矩阵计算的内点，使用levenberg-marquardt方法迭代求解单应矩阵；
[0098]
[0099]
其中，(x
i,t
,y
i,t
)和(x
i,t 1
,y
i,t 1
)分别为第i个t帧和t 1帧中的对应坐标，median 表示取所有数据样本的中值；
[0100]
如式(3)，将变换后坐标和与实际对应坐标之间的像素距离作为误差项， lmeds的求解使得选出点集误差的中值最小化；
[0101]
本步骤中所需的两帧之间的对应点信息，通过光流法求得；将第t帧中提取的 orb特征点利用lucas-kanade稀疏光流法向第t 1帧追踪得到对应点位置；
[0102]
步骤1.3：提出基于光流原理的双向补偿光流法，步骤1.2已经对前后两帧t与 t 1进行前向光流计算，根据对应点信息计算单应矩阵对第t帧图像应用该单应变换得到补偿图像，此时从第t 1帧向补偿图像做反向光流追踪得到消除干扰的反向光流场；
[0103]
在双向补偿光流过程中，记第t帧的时刻为t，t时刻位于(x,y)处的像素，在 t dt时刻运动到(x dx,y dy)处；根据光流法中的灰度不变假设，有前向光流灰度关系式(4)，其中i(x,y,t)表示t时刻(x,y)处的像素灰度，前向光流的意义主要在于建立前后两帧之间的对应点联系，计算单应矩阵；
[0104]
i(x dx,y dy,t dt)＝i(x,y,t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)
[0105]
i(xb,yb,t)＝i(x dx,y dy,t dt)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0106]
反向光流过程中，由t dt时刻的像素位置(x dx,y dy)在经单应补偿的t 时刻图像中计算得到对应点位置(xb,yb)，用i(xb,yb,t)表示该时刻和位置的像素灰度，有反向光流灰度关系式(5)。
[0107]
前向光流的光流矢量为式(6)，反向光流的光流矢量定义为该过程中原本光流失量的相反矢量，如式(7)；
[0108]vforward
＝(x dx,x dy)-(x,y)＝(dx,dy)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0109]vbackward
＝-((xb,yb)-(x dx,y dy))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0110]
＝(dx x-xb,dy y-yb)
[0111]
其中，v
forward
为前向光流矢量，v
backward
为反向光流矢量；
[0112]
两个矢量的差值(x
–
xb,y-yb)表征了单应运动补偿对于相机自身运动引起的背景运动的补偿，使得v
backward
接近于运动前景的位移；
[0113]
和前向光流的结果相比，反向光流计算后静止背景中特征点的移动得到了有效的抵消，从而反向光流矢量可以一定程度表征一个特征点的运动方向和速度；
[0114]
从多帧图像序列角度考虑，双向补偿光流法为了提高可靠性，在计算t帧和t 1帧之间的反向光流场1的基础上，同理计算t帧和t 2帧之间的反向光流场2 并对比分析两个光流场中的光流失量；
[0115]
除了一些显著错误的点，反向光流场2的时间跨度更长，运动前景的移动更为显著，按以下条件判断特征点是否做稳定的运动：
[0116]
1)两个矢量的模均大于一定阈值δ；
[0117]
2)反向光流场2中矢量的模大于反向光流场1中对应矢量模；
[0118]
3)两矢量的夹角小于180
°
，即内积大于0。
[0119][0120]
以上条件表述为式(8)，v1和v2分别为两个光流场中的对应光流矢量，δ选取为一个极小的值，受运动程度影响小。
[0121]
如图2所示，为双向补偿光流原理示意图，描述了本发明提出的双向补偿光流法的原理和实施流程。
[0122]
如图3所示，为多帧光流信息对比融合示意图，描述了本发明融合多帧之间的反向光流场信息的方式，考虑了短时间内物体运动方向和速度的一致性，去除了光流法错误跟踪和相机高频抖动的干扰。
[0123]
步骤2：将输入的图像划分为矩形网格区域，将动态特征点转换为动态区域表示，并根据几何连通性和深度值聚类方法优化动态区域，得到最终的网格化运动分割图像，过程如下：
[0124]
步骤2.1：将输入的图像划分为20
×
20的矩形网格区域。通过网格划分将动态特征点转换为动态区域。将含有动态特征点的区块标记为疑似动态区块，同时对每个矩形区域内的动态特征点数量进行计数；
[0125]
步骤2.2：在得到的动态区域结果基础上，基于几何连通性处理其中的孤立区块与被包围区块；若某动态区块孤立且动态特征点计数少于设定阈值(取为3)，则标记该区块为静止；若某静态区块被较多动态区块包围，则标记该区块为运动；
[0126]
步骤2.3：将深度图像降采样至20
×
20分辨率，对应所划分的图像网格，降低聚类算法计算量；以深度值为特征，使用k-means 方法进行深度值聚类。聚类结果能够在一定程度上区分不同深度层次物体，参照深度聚类结果扩展和填充动态区域；若动态区域中的一个连通区域与某聚类簇内的一个连通区域重合度高于一定的阈值ε，将动态区域周围属于同一聚类簇的非动态区域，也标记为动态区域。
[0127][0128]
重合度计算方法如式(9)所示，s为重合区域面积，s1为聚类簇连通区域面积，s2为动态区域面积；
[0129]
如图4所示，为本发明具体实施方式经过优化得到的网格化运动分割图像。图中通过网格标识出画面中的动态区域。
[0130]
步骤3：根据网格分割对当前帧中的特征点运动性质做出判断，初始化稀疏点云地图，利用最小化重投影误差的方式计算当前帧去除动态影响的相机位姿，过程如下：
[0131]
步骤3.1：若输入图像为视频流中的第一帧，使用该帧特征点的深度测量值计算三维点坐标，插入到地图中，进行稀疏点云地图的初始化；
[0132]
步骤3.2：结合rgb图像和深度图像得出动态区域分割结果后，根据网格分割对当前帧中的特征点运动性质做出判断，落在动态区域内的特征点为动态特征点，记为集合χs；
[0133]
步骤3.3：利用最小化重投影误差的方式计算当前帧的相机位姿，如式(10)，其中，
不对动态特征点计算误差项，不加入优化问题；
[0134][0135]
其中，t
cw
为世界坐标系到相机坐标系的坐标变换，即相机的位姿，ρ(
·
)为 huber鲁棒损失函数，xi为静态特征点坐标，pi为对应的三维点坐标；
[0136]
π(
·
)为相机的投影函数，对某个三维点[x y z]
t
，有：
[0137][0138]
其中所涉及的其余变量为相机内参；
[0139]
σ为信息矩阵，有：
[0140]
σ＝n
·eꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0141]
n为当前特征点所在图像金字塔的层数，e为3
×
3的单位矩阵；
[0142]
利用高斯-牛顿方法对最小化重投影误差问题进行求解，优化得到相机的位姿估计；
[0143]
步骤4：结合相机位姿、rgb-d图像、网格化运动分割图像，构建场景的静态八叉树地图，过程如下：
[0144]
步骤4.1：为了降低计算量、减少冗余，更新地图时对图像进行1/4降采样，在行和列上每4个像素选取1个像素进行更新。将网格化运动分割应用到八叉树地图的建立当中，算法只根据静态区域内的图像信息更新八叉树地图，不在八叉树地图当中插入动态区域对应节点，一定程度避免动态物体的信息被记录到地图当中；
[0145]
步骤4.2：在更新地图的时候，采用一种光线遍历方法。从相机光心o向平面像素点pi对应的空间点pi投射一道光线，遍历光线穿越的所有节点。存在以下三种情况：
[0146]
(4.2.1)光线穿过若干数量的被占据节点，则将路径上所有的被占据节点置为空，将末端节点置为占据；
[0147]
(4.2.2)光线未穿过被占据节点，但末端落在被占据节点上，则不更新地图；
[0148]
(4.2.3)光线未穿过被占据节点，且末端不落在被占据节点上，则将末端节点置为占据；
[0149]
八叉树中的光线遍历计算，通过三维数值微分算法实现。
[0150]
如图5所示，为本发明具体实施方式的光线遍历法原理示意图。图中展示了光线遍历过程中光线穿过占据节点的三种情况。
[0151]
如图7所示，为本发明具体实施方式构建的场景八叉树地图。由于位姿估计未受动态物体干扰，地图未出现扭曲，房间结构清晰。所得到的八叉树地图分辨率高，包含节点的颜色信息，还原了场景的原貌。地图根据运动分割和后续的观察更新有效地剔除了动态物体导致的“残影”，仅包含场景中的静态要素，适用于移动机器人导航等用途。
[0152]
步骤5：逐步构建场景的稀疏点云地图，在这一过程中结合八叉树地图进行双地图耦合，在关键帧上使用基于网格分割和八叉树地图光线遍历的方法筛选静态地图点，更新稀疏点云地图，保障定位精度，过程如下：
[0153]
步骤5.1：在关键帧上，通过rgb-d相机的深度测量结果，还原部分orb 特征点在三维空间中的坐标，将他们加入到稀疏点云地图中用于特征匹配和位姿计算；
[0154]
如图6所示，为本发明具体实施方式构建的场景稀疏点云地图；
[0155]
步骤5.2：属于动态物体的地图点被加入地图后，将导致后期在该位置上进行位姿跟踪时产生错误匹配，影响位姿估计精度；为了减轻这种效应，可以使用运动分割生成的动态区域掩膜帮助稀疏点云地图筛选一部分动态地图点；除此之外，还将采用一种基于八叉树地图光线遍历法的方法进一步剔除一部分动态地图点，实现两种地图之间的耦合，通过先前对场景三维结构的认知来指导稀疏点云建图。
[0156]
根据特征点的二维坐标可以在深度图中查询对应的深度值，记为zc，进一步将特征点的二维坐标p＝[u v 1]
t
反投影为相机坐标系中的三维坐标 pc＝[x
c y
c zc]
t
，元素对应坐标各轴分量，k为相机内参矩阵：
[0157]
pc＝zck-1
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(13)
[0158]
根据相机位姿变换t
cw
得到该点在世界坐标系中的坐标pw＝[x y z]
t
：
[0159]
pw＝t
cw-1
pc＝r-1
(p
c-t)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0160]
相机位姿变换t
cw
可以表示为旋转矩阵r和平移向量t，相机在世界坐标系中的坐标p
cam
即为t，表示为三轴分量的形式，有：
[0161]
p
cam
＝t＝[x
cam y
cam z
cam
]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)
[0162]
在八叉树地图中进行从p
cam
向pw方向的光线遍历。与之前不同，投射的光线为一条射线，而不仅仅遍历两点之间的节点。记第一个被穿越节点的坐标为 pw′
＝[x
′ꢀy′ꢀz′
]
t
，变换到相机坐标系，得到该点的深度值zc′
，由于八叉树地图中信息根据先前关键帧更新，在八叉树中进行光线遍历得到的深度值与相机测量得到的深度值有较大出入的地图点很可能在关键帧之间发生运动；
[0163]
rgb-d相机在测量较远物体的深度时误差较大，因此只对4m内的地图点做深度值检查，当遍历深度zc′
在测量深度zc附近的误差范围内，地图点得到保留，被插入到稀疏点云地图中，将该过程表示为式(16)：
[0164][0165]
其中δ为误差范围大小，这一过程的误差主要来源于采用的rgb-d相机的测量误差、位姿估计带来的误差、八叉树建模的精度，表示为：
[0166]
δ＝δz δ
t
δoꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0167]
其中，δz为rgb-d相机对深度的测量误差，δ
t
为位姿估计的误差，δo为八叉树建模精度的误差，δz是关于测量的实际深度的函数，实际深度越大，误差越大，可以对所用的rgb-d相机进行合理建模和误差分析得到关系式。
[0168]
表1为tum数据集上本发明的绝对轨迹误差统计值
[0169][0170]
表1
[0171]
本发明的具体实施方式在tum数据集上的测试结果如表1所示。使用绝对轨迹误差的rmse(均方根误差)作为评价指标来评价本发明在室内动态环境下准确定位的能力，使用orb-slam3算法作为基准进行对比。“sitting”图像序列仅包含极少的人物肢体运动，称为低动态序列，“walking”图像序列包含较多的人物行走画面，称为高动态序列。在低动态序列中，本发明与orb-slam3有相似的定位精度。在高动态序列中，orb-slam3在多个场景中都受到动态物体的严重干扰，发生位姿漂移，产生了很高的定位误差，而本发明输出的结果相较 orb-slam3方法有较大的提升。
[0172]
本说明书的实施例所述的内容仅仅是对发明构思的实现形式的列举，仅作说明用途。本发明的保护范围不应当被视为仅限于本实施例所陈述的具体形式，本发明的保护范围也及于本领域的普通技术人员根据本发明构思所能想到的等同技术手段。