CN112378409B - 动态环境下基于几何与运动约束的机器人rgb-d slam方法 - Google Patents

Abstract

一种动态环境下基于几何与运动约束的机器人RGB‑D SLAM方法，通过RGB‑D相机获取图像数据，图像数据包括RGB图像和深度图像，使用ORB特征点检测算法、RGB图像和深度图像，获取特征点在相机坐标系下的坐标；使用g2o优化算法计算相机位姿，把在相机坐标系下的特征点变换到世界坐标系下，通过不断跟踪同一点在世界坐标系下的坐标，得到该点多次观测速度；通过分析多次观测速度的规律，得知该点是真实运动物体上的还是静止物体上的。分析得到该点是真实运动后，通过卡尔曼滤波算法对动点运动速度、世界坐标系下的坐标进行滤波；最后，把动点和静点一起加入g2o位姿优化。本发明有效适用于动态环境，得到更准确的相机位姿。

Description

动态环境下基于几何与运动约束的机器人RGB-D SLAM方法

技术领域

本发明涉及一种室内动态环境下机器人定位方法。

背景技术

在自主移动机器人智能导航技术的相关研究中，机器人在未知环境下的同步 定位与地图构建(简称SLAM，simultaneous localization and mapping)技术作为关键 性的技术，兼具应用和学术上的双重价值，已然成为近二十年来该领域的研究热 点。在这种趋势下，学者们提出了多种解决SLAM问题的方法，也应用了多种传 感器来解决SLAM中的环境感知问题。

摄像头作为一种廉价、信息量大的传感器，在自主移动机器人领域有极好的 应用前景。视觉SLAM主要通过对多帧图像中相同的特征点进行匹配，求解出相 机的位姿变换。但是视觉SLAM方向还有四大难题，导致其实际应用场景较小。 包括如何处理闭环回路、如何处理大尺度场景、如何处理动态场景、如何处理快 速运动和强旋转。

动态场景指的是摄像头捕获的照片中存在人、车等正在运动的物体。对于稀 疏地图的视觉SLAM系统，动态场景的挑战主要来源于传统视觉SLAM无法区 分出运动特征点与静态特征点，在计算相机位姿时，错误地把运动特征点当成静 态特征点进行计算，导致计算出来的相机位姿和实际位姿有极大偏差，接着导致 地图点计算错误，进一步扩大视觉SLAM的定位偏差。

发明内容

为了克服已有技术的不足，本发明提供了一种室内动态环境下机器人定位方 法，有效适用于动态环境，得到更准确的相机位姿。

本发明解决其技术问题所采用的技术方案是：

一种动态环境下基于几何与运动约束的机器人RGB-D SLAM方法，所述方 法包括如下步骤：

步骤1：进行相机内参标定；

步骤2：在视频流中依次对其中的图像帧进行获取，首先对获取的图像帧建立 图像金字塔，然后分别在图像金字塔上进行图像分块，获取一定窗口大小的图像 区域，然后利用现有的特征点提取算法在每个分块里分别进行特征点提取和建立 对应特征点的描述子；

步骤3：判断该帧是否是视频流里的第一帧，如果是第一帧，则进行初始化操 作，否则跳过这个步骤，过程如下：

步骤3.1：将RGB图像平面的特征点和深度图像中的像素点相互对应；

步骤3.2：将深度图像中的像素点转换为该点离相机平面的距离；

步骤3.3：使用特征点在RGB图像像素平面的坐标和对应的深度图像像素值 转换得到的该点距离相机平面的距离，算出该点在相机坐标系下的坐标P,像素坐 标(u,v)和深度图像素值d转换为坐标P的转换方式为：

P＝K[u v d]^T

其中K为相机内参；

步骤3.4：将该帧的相机坐系定为世界坐标系，并将步骤3.3获取的点加入1 号地图，然后跳转回步骤2；

步骤4，利用特征点描述子的距离匹配图像中的特征点与1、2号地图中的3D 点，计算相机位姿，将相机坐标系下的坐标P转换到世界坐标系下的坐标x，并 进行关键帧选取；过程如下：

步骤4.1，由于该帧非第一帧，即1号地图中必然存在3D点，且这些点被会 认为是静止的3D点，使用当前帧的ORB特征点的描述子分别与1号地图以及2 号地图中的3D点的描述子进行匹配，根据配对的特征点对，使用汉明距离，取 比例为a的汉明距离最小的点作为正确配对点，0<a<1，成功配对后可以得到图像 中这部分特征点在世界坐标系下的坐标；

步骤4.2，当前帧特征点中，成功与1号地图配对上的部分特征点在世界坐标 系下的坐标记为x_i，该点所在的图像金字塔层数为n_i，该点在RGB图像中的像素 坐标和深度图像对应的像素值组合成Q_i＝[u,v,d]，对于当前帧特征点中，成功与2 号地图配对上的部分特征点在世界坐标系下的坐标记为x_j，该点所在的图像金字 塔层数为n_j，该点在RGB图像中的像素坐标和深度图像对应的像素值组合成Q_j＝[u,v,d]，与1号地图中的点不同的是，2号地图中的点可能是运动的，因此，需 要对2号地图中的点坐标进行预测；

步骤4.3，构建残差模型，残差方程为：

其中，

Σ₁＝n_i×E

Σ₂＝λ×n_j×E

λ表示2号地图中的点对残差函数影响的权重，使用g2o对残差函数进行最小化，得到当前帧相机位姿的最优解T_{cw_k}；

步骤4.4，按照步骤3.1～3.3，获得该帧中的特征点在相机坐标系下的坐标， 根据步骤4.3得到的当前帧相机位姿，将相机坐标系下的点转换到世界坐标系下； 若该帧对应的世界坐标系下的点中，可以被其他帧观测到的点的比例少于b， 0<b<1)，否则判断为关键帧；若距离上一次插入关键帧已经过去了较长时间，则 判断为关键帧，对于关键帧，把所有无法与1号地图或者2号地图形成配对的特 征点，转换到世界坐标系下，然后直接加入2号地图中；

步骤5，当前帧中特征点与2号地图中的点进行匹配，进行卡尔曼滤波，然后 计算场景的动态程度；

步骤6，利用相邻两次的速度观测量对2号地图中的点进行筛选；

步骤7，根据场景的动态程度，使用多个关键帧生成新的1号地图点；

步骤8，判断相机运动轨迹是否形成闭环，即相机是否运动到了之前路过的区 域，如果发现闭环，则根据1号地图的点云观测信息对全局关键帧的位姿进行优 化，2号地图点不参与闭环检测和全局优化，避免动态物体的影响。

进一步，所述步骤4中，将地图点分为1号地图点与2号地图点，1号地图 点存放符合对极约束与投影条件的静态点，2号地图存放动态地图点，可疑静点， 状态未知点。

再进一步，所述步骤5的实现过程如下：

步骤5.1，该帧在步骤4.1进行了与2号地图点的配对工作，在步骤4.4中计 算出了当前时刻，其在相机坐标系下的坐标，对于一个运动的点，其世界坐标系 下的的坐标，速度，加速度存在以下关系，即运动方程：

其中，x_k代表第k次观测时计算得出的坐标，v_k代表第k次观测时计算得出 的速度，a_k代表第k次观测时计算得出的加速度，γ_k代表第次k观测加速度的变 化量，作为运动的不确定性，假设其服从均值为0的高斯分布，协方差为σ²；

相机可以观测到的量为相机坐标系下，特征点的3D坐标，观测方程为：

其中，X_{c_k}，T_{cw_k}，w_k分别为k时刻3D点在相机坐标系下的坐标，世界坐 标系到相机坐标系的变换矩阵，观测噪声；

其中，观测噪声符合以下特点：

对于空间中一点，实际像素坐标在±0.5×1.2ⁿ内均匀分布，深度误差根据相机型号确定；

第一次观测，可以得到该点在世界坐标系下的坐标，第二次观测，可以得到 世界坐标系下的速度，第三次观测，得到该点的加速度，对于该点第三次及以上 的观测，可以使用运动方程对该点的世界坐标进行预测，用于步骤4.2的相机位 姿计算；另外，对于该点的多次观测，使用卡尔曼滤波对该点在世界坐标系下的 坐标、速度、加速度、协方差进行更新；

步骤5.2，场景动态程度表示场景内，动态物体所占图像区域与动态物体运动 速度的综合结果，场景内动态物体所占区域越大，场景动态程度越高，动态物体 运动速度越快，场景动态程度越高；

使用公式表示，该帧中与2号地图匹配成功的第i个点速度记为v_i，动态程 度表示为：

更进一步，所述步骤6的实现过程为：

步骤6.1，对于经过三次及以上观测次数的2号地图中的点，会有两次以上的 速度值，取出最后相邻两次的速度值，对于真实运动的点，相邻两次速度的方向 应该趋于一致，两次速度值较大；对于实际静止的点，由于像素坐标是整型变量， 得到的速度大小一般不为0，且方向来回跳动，因此可以使用相邻两次的速度矢 量来判断该点的运动状态；

使用公式表达：

用v_o和v_o-1表示该特征点的最后两次观测速度，||*||₁表示向量*的1范数；

对于静止的点：

||v_o-v_o-1||₁＞＞||v_o+v_o-1||₁

对于真实运动的点：

||v_o-v_o-1||₁＜＜||v_o+v_o-1||₁；

剩余不符合上述两个约束条件的点被认为是错误匹配；

步骤6.2，在关键帧的所有特征点中，部分特征较弱，部分仅出现一次就不会 再次出现，还有一部分可疑静点存在时间较长，后续会被加入到1号地图中成为 静点，为了提高图像帧与2号地图中的点的配对速度，同时去除过多冗余点，需 要对2号地图中的点进行剔除，2号地图中的点如果在未来3帧内没有再次被观 测到，或者被观测5次就被剔除。

所述步骤7的实现过程为：

步骤7.1，若当前帧非关键帧，则跳过该步骤，若当前帧是关键帧，则挑选出 以前的15个与当前关键帧共视程度比较大的老关键帧，根据场景的动态程度，选 择保留其中的8～15个时间跨度最长的老关键帧，场景动态程度越高，保留的关 键帧越多，场景动态程度越低，保留的关键帧越少；

步骤7.2，对于保留下来的老关键帧，进行静态点筛选，根据筛选结果，产生 1号地图点，包括如下子步骤：

步骤7.2.1，使用极线约束排除非静止状态的点，C₁、C₂为运动中相机的光 心所处的位置，相机从C₁运动到C₂，x₁表示C₁观测时，空间中的点所处的位置， x₂～x₄表示若x₁点运动，C₂观测时，x₁可能所处的3个位置，若空间中的点为静 止状态，根据极线约束，该点在两帧中投影的极线应当位于同一平面上，即直线 l₁与l₂应当位于同一平面上，若空间中该点不符合该约束，则可以判断该点是非 静止状态的点；若空间点在像素平面的投影点距离极线的距离

则判断为非静止点，其中n为该点在图像金字塔中的层数；

步骤7.2.2，使用重投影误差排除非静止状态的点，根据C₁观测时，C₁的位 姿和投影的像素点坐标，以及C₂观测时，C₂的位姿，使用重投影方法可以预测 C₂观测时，该点所投影的像素点坐标，

其中K为相机内参，T__cw1为C₁位姿，T_{_cw2}为C₂位姿，u₁，v₁为C₁观测的像素 坐标，z₁为C₁观测的深度值，

为投影在C₂中的预测值；

若实际投影和预测投影之间的像素坐标距离

则判断为非静止状态的点；

步骤7.2.3，对于剩下的关键帧之间的配对点，使用三角化的方式，得到点在 当前关键帧下的相机坐标，最后，转换到世界坐标系下，直接加入1号地图。

本发明的有益效果主要表现在：，有效适用于动态环境，得到更准确的运动状 态。

附图说明

图1是本发明实施例的流程图。

图2是本发明实施例的静态特征点筛选示意图，其中，C₁,C₂代表运动过程中 的两个图像帧，位于不同的位置，x₁为C₁观测时，点所在位置，x₂-x₄代表C₂观 测时，x₁可能的所在位置。

图3是本发明实施例在运行时，从视频流中取出的一帧，该图像帧中主要包 括人1、人后面的门2，桌椅3等物体。

图4是本发明实施例的地图点，其中，(a)为1号地图，(b)为2号地图。

图5是本发明实施例的闭环检测示意图，其中，(a)为闭环检测前的相机轨 迹和地图，(b)为闭环检测以后，对应的相机轨迹图。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图5，一种动态环境下基于几何与运动约束的机器人RGB-D SLAM 方法，室内动态环境下，所述方法包括以下步骤：

步骤1：进行相机内参(相机主点、焦距与畸变系数)标，过程如下：

步骤1.1：利用相机获取多张不同视角下的固定大小的棋盘格图像数据；

步骤1.2：利用张正友相机标定法，对获取到的棋盘格图像数据进行相机内参 计算，获取相机标定结果，记为K；

步骤2：在视频流中依次对其中的图像帧进行获取，首先对获取的图像帧建立 图像金字塔，然后分别在图像金字塔上进行图像分块，获取一定窗口大小的图像 区域，然后利用现有的特征点提取算法在每个分块里分别进行特征点提取和对应 特征点的描述子，考虑到ORB特征点提取算法速度快、特征点多，本发明选择 的特征点表达为ORB特征点；

步骤3：判断该帧是否是视频流里的第一帧，如果是第一帧，则进行初始化操 作，否则跳过这个步骤；过程如下：

步骤3.1：将RGB图像平面的特征点和深度图像中的像素点相互对应；

步骤3.2：将深度图像中的像素点转换为该点离相机平面的距离；

步骤3.3：使用特征点在RGB图像像素平面的坐标和对应的深度图像像素值 转换得到的该点距离相机平面的距离，算出该点在相机坐标系下的坐标P 像素坐标(u,v)和深度图像素值d转换为坐标P的转换方式为：

P＝K[u v d]^T；

步骤3.4：将该帧的相机坐系定为世界坐标系，并将步骤3.3获取的点加入1 号地图，然后跳转回步骤2；

步骤4，利用特征点描述子的距离匹配图像中的特征点与1、2号地图中的3D 点，计算相机位姿，将相机坐标系下的坐标P转换到世界坐标系下的坐标x，并 进行关键帧选取，过程如下：

步骤4.1，由于该帧非第一帧，即1号地图中必然存在3D点，且这些点被会 认为是静止的3D点，使用当前帧的ORB特征点的描述子分别与1号地图以及2 号地图中的3D点的描述子进行匹配，根据配对的特征点对，使用汉明距离，取 其距离最近的前百分之六十(a取0.6)，作为正确的配对点，成功配对后可以得 到图像中这部分特征点在世界坐标系下的坐标；

步骤4.2，当前帧特征点中，成功与1号地图配对上的部分特征点在世界坐标 系下的坐标记为x_i，该点所在的图像金字塔层数为n_i，该点在RGB图像中的像素 坐标和深度图像对应的像素值组合成Q_i＝[u,v,d]，对于当前帧特征点中，成功与2 号地图配对上的部分特征点在世界坐标系下的坐标记为x_j，该点所在的图像金字 塔层数为n_j，该点在RGB图像中的像素坐标和深度图像对应的像素值组合成Q_j＝[u,v,d]，与1号地图中的点不同的是，2号地图中的点可能是运动的，因此，需 要对2号地图中的点坐标进行预测，预测方式见步骤5.1；

步骤4.3，构建残差模型，残差方程为：

其中，

Σ₁＝n_i×E

Σ₂＝λ×n_j×E

λ表示2号地图中的点对残差函数影响的权重，使用经验值0.3，使用g2o对残差 函数进行最小化，得到当前帧相机位姿的最优解T_{cw_k}；

步骤4.4，按照步骤3.1～3.3，获得该帧中的特征点在相机坐标系下的坐标， 根据步骤4.3得到的当前帧相机位姿，将相机坐标系下的点转换到世界坐标系下， 若该帧对应的世界坐标系下的点中，可以被其他帧观测到的点少于75％(b取0.75)， 否则判断为关键帧，若距离上一次插入关键帧已经过去了较长时间，则判断为关 键帧，对于关键帧，把所有无法与1号地图或者2号地图形成配对的特征点，转 换到世界坐标系下，然后直接加入2号地图中；

步骤5，当前帧中特征点与2号地图中的点进行匹配，进行卡尔曼滤波，然后 计算场景的动态程度，包括如下步骤：

步骤5.1，该帧在步骤4.1进行了与2号地图点的配对工作，在步骤4.4中计 算出了当前时刻，其在相机坐标系下的坐标，对于一个运动的点，其世界坐标系 下的的坐标，速度，加速度存在以下关系，即运动方程：

其中，x_k代表第k次观测时计算得出的坐标，v_k代表第k次观测时计算得出 的速度，a_k代表第k次观测时计算得出的加速度，γ_k代表第次k观测加速度的变 化量，作为运动的不确定性，假设其服从均值为0的高斯分布，协方差为σ^；；

相机可以观测到的量为相机坐标系下，特征点的3D坐标，观测方程为：

其中，X_{c_k}，T_{cw_k}，w_k分别为k时刻3D点在相机坐标系下的坐标，世界坐 标系到相机坐标系的变换矩阵，观测噪声；

其中，观测噪声符合以下特点：

对于空间中一点，实际像素坐标在±0.5×1.2n内均匀分布，深度误差根据相 机型号确定，所使用相机型号为kinect v1，深度误差近似为±4％内均匀分布；

第一次观测，可以得到该点在世界坐标系下的坐标，第二次观测，可以得到 世界坐标系下的速度，第三次观测，得到该点的加速度；对于该点第三次及以上 的观测，可以使用运动方程对该点的坐标进行预测，用于步骤4.2的相机位姿计 算。另外，对于该点的多次观测，使用卡尔曼滤波对该点在世界坐标系下的坐标、 速度、加速度、协方差进行更新；

步骤5.2，场景动态程度表示场景内，动态物体所占图像区域与动态物体运动 速度的综合结果，场景内动态物体所占区域越大，场景动态程度越高，动态物体 运动速度越快，场景动态程度越高；

使用公式表示，该帧中与2号地图匹配成功的第i个点速度记为v_i，动态程 度表示为：

步骤6，利用相邻两次的速度观测量对2号地图中的点进行筛选，包括如下步 骤：

步骤6.1，对于经过三次及以上观测次数的2号地图中的点，会有两次以上的 速度值，取出最后相邻两次的速度值。对于真实运动的点，相邻两次速度的方向 应该趋于一致，两次速度值较大。对于实际静止的点，由于像素坐标是整型变量， 得到的速度大小一般不为0，且方向来回跳动，因此可以使用相邻两次的速度矢 量来判断该点的运动状态；

使用公式表达：

用v_o和v_o-1表示该特征点的最后两次观测速度。||*||₁表示向量*的1范数；

对于静止的点：

||v_o-v_o-1||₁＞＞||v_o+v_o-1||₁且||v_o+v_o-1||₁较小；

对于真实运动的点：

||v_o-v_o-1||₁＜＜||v_o+v_o-1||₁

实际操作中，使用经验值||v_o-v_o-1||₁＜0.3×||v_o+v_o-1||₁分离动点情况较好；

使用||v_o-v_o-1||₁＞0.1×||v_o+v_o-1||₁，||v_o+v_o-1||₁＜0.5分离可疑静点情况较好；剩余不符合上述两个约束条件的点被认为是错误匹配；

步骤6.2，在关键帧的所有特征点中，部分特征较弱，部分仅出现一次就不会 再次出现，还有一部分可疑静点存在时间较长，后续会被加入到1号地图中成为 静点，为了提高图像帧与2号地图中的点的配对速度，同时去除过多冗余点，需 要对2号地图中的点进行剔除，2号地图中的点如果在未来3帧内没有再次被观 测到，或者被观测5次就被剔除；

步骤7，根据场景的动态程度，使用多个关键帧生成新的1号地图点，包括如 下步骤：

步骤7.1，若当前帧非关键帧，则跳过该步骤，若当前帧是关键帧，则挑选出 以前的15个与当前关键帧共视程度比较大的老关键帧，根据场景的动态程度，选 择保留其中的8～15个时间跨度最长的老关键帧，场景动态程度越高，保留的关 键帧越多，场景动态程度越低，保留的关键帧越少；

步骤7.2，对于保留下来的老关键帧，进行静态点筛选，根据筛选结果，产生 1号地图点，结合附图进行说明，如图2，包括如下步骤：

步骤7.2.1，使用极线约束排除非静止状态的点，C₁、C₂为运动中相机的光 心所处的位置，相机从C₁运动到C₂，x₁表示C₁观测时，空间中的点所处的位置， x₂～x₄表示若x₁点运动，C₂观测时，x₁可能所处的3个位置。若空间中的点为静 止状态，根据极线约束，该点在两帧中投影的极线应当位于同一平面上，即直线 l₁与l₂应当位于同一平面上。若空间中该点不符合该约束，则可以判断该点是非 静止状态的点，取95％置信度，若空间点在像素平面的投影点距离极线的距离

则判断为非静止点，其中n为该点在图像金字塔中的层数；

步骤7.2.2，使用重投影误差排除非静止状态的点，根据C₁观测时，C₁的位 姿和投影的像素点坐标，以及C₂观测时，C₂的位姿，使用重投影方法可以预测 C₂观测时，该点所投影的像素点坐标，

其中K为相机内参，T__cw1为C₁位姿，T_{_cw2}为C₂位姿，u₁，v₁为C₁观测的像素 坐标，z₁为C₁观测的深度值。

为投影在C₂中的预测值；

取95％置信度，若实际投影和预测投影之间的像素坐标距离

则判断为非静止状态的点；

步骤7.2.3，对于剩下的关键帧之间的配对点，使用三角化的方式，得到点在 当前关键帧下的相机坐标，最后，转换到世界坐标系下，直接加入1号地图；

步骤8，判断相机运动轨迹是否形成闭环，即相机是否运动到了之前路过的区 域，如果发现闭环，则根据1号地图的点云观测信息对全局关键帧的位姿进行优 化，2号地图点不参与闭环检测和全局优化，避免动态物体的影响；

步骤9，输出结果。

本说明书的实施例所述的内容仅仅是对发明构思的实现形式的列举，仅作说 明用途。本发明的保护范围不应当被视为仅限于本实施例所陈述的具体形式，本 发明的保护范围也及于本领域的普通技术人员根据本发明构思所能想到的等同技 术手段。

Claims (5)

1.一种动态环境下基于几何与运动约束的机器人RGB-D SLAM方法，其特征在于，所述方法包括如下步骤：

步骤1：进行相机内参标定；

步骤2：在视频流中依次对其中的图像帧进行获取，首先对获取的图像帧建立图像金字塔，然后分别在图像金字塔上进行图像分块，获取一定窗口大小的图像区域，然后利用现有的特征点提取算法在每个分块里分别进行特征点提取和建立对应特征点的描述子；

步骤3：判断该帧是否是视频流里的第一帧，如果是第一帧，则进行初始化操作，否则跳过这个步骤，过程如下：

步骤3.1：将RGB图像平面的特征点和深度图像中的像素点相互对应；

步骤3.2：将深度图像中的像素点转换为该点离相机平面的距离；

步骤3.3：使用特征点在RGB图像像素平面的坐标和对应的深度图像像素值转换得到的该点距离相机平面的距离，算出该点在相机坐标系下的坐标P,像素坐标(u,v)和深度图像素值d转换为坐标P的转换方式为：

P＝K[u v d]^T

其中K为相机内参；

步骤3.4：将该帧的相机坐系定为世界坐标系，并将步骤3.3获取的点加入1号地图，然后跳转回步骤2；

步骤4，利用特征点描述子的距离匹配图像中的特征点与1、2号地图中的3D点，计算相机位姿，将相机坐标系下的坐标P转换到世界坐标系下的坐标x，并进行关键帧选取；过程如下：

步骤4.1，由于该帧非第一帧，即1号地图中必然存在3D点，且这些点被会认为是静止的3D点，使用当前帧的ORB特征点的描述子分别与1号地图以及2号地图中的3D点的描述子进行匹配，根据配对的特征点对，使用汉明距离，取比例为a的汉明距离最小的点作为正确配对点，0<a<1，成功配对后可以得到图像中这部分特征点在世界坐标系下的坐标；

步骤4.2，当前帧特征点中，成功与1号地图配对上的部分特征点在世界坐标系下的坐标记为x_i，该点所在的图像金字塔层数为n_i，该点在RGB图像中的像素坐标和深度图像对应的像素值组合成Q_i＝[u,v,d]，对于当前帧特征点中，成功与2号地图配对上的部分特征点在世界坐标系下的坐标记为x_j，该点所在的图像金字塔层数为n_j，该点在RGB图像中的像素坐标和深度图像对应的像素值组合成Q_j＝[u,v,d]，与1号地图中的点不同的是，2号地图中的点可能是运动的，因此，需要对2号地图中的点坐标进行预测；

步骤4.3，构建残差模型，残差方程为：

其中，

Σ₁＝n_i×E

Σ₂＝λ×n_j×E

λ表示2号地图中的点对残差函数影响的权重，使用g2o对残差函数进行最小化，得到当前帧相机位姿的最优解T_{cw_k}；

步骤4.4，按照步骤3.1～3.3，获得该帧中的特征点在相机坐标系下的坐标，根据步骤4.3得到的当前帧相机位姿，将相机坐标系下的点转换到世界坐标系下；若该帧对应的世界坐标系下的点中，可以被其他帧观测到的点的比例少于b，0<b<1，否则判断为关键帧；若距离上一次插入关键帧已经过去了较长时间，则判断为关键帧，对于关键帧，把所有无法与1号地图或者2号地图形成配对的特征点，转换到世界坐标系下，然后直接加入2号地图中；

步骤5，当前帧中特征点与2号地图中的点进行匹配，进行卡尔曼滤波，然后计算场景的动态程度；

步骤6，利用相邻两次的速度观测量对2号地图中的点进行筛选；

步骤7，根据场景的动态程度，使用多个关键帧生成新的1号地图点；

步骤8，判断相机运动轨迹是否形成闭环，即相机是否运动到了之前路过的区域，如果发现闭环，则根据1号地图的点云观测信息对全局关键帧的位姿进行优化，2号地图点不参与闭环检测和全局优化，避免动态物体的影响。

2.如权利要求1所述的动态环境下基于几何与运动约束的机器人RGB-D SLAM方法，其特征在于：所述步骤4中，将地图点分为1号地图点与2号地图点，1号地图点存放符合对极约束与投影条件的静态点，2号地图存放动态地图点，可疑静点，状态未知点。

3.如权利要求1或2所述的动态环境下基于几何与运动约束的机器人RGB-D SLAM方法，其特征在于：所述步骤5的实现过程如下：

步骤5.1，该帧在步骤4.1进行了与2号地图点的配对工作，在步骤4.4中计算出了当前时刻，其在相机坐标系下的坐标，对于一个运动的点，其世界坐标系下的的坐标，速度，加速度存在以下关系，即运动方程：

其中，x_k代表第k次观测时计算得出的坐标，v_k代表第k次观测时计算得出的速度，a_k代表第k次观测时计算得出的加速度，γ_k代表第次k观测加速度的变化量，作为运动的不确定性，假设其服从均值为0的高斯分布，协方差为σ²；

相机可以观测到的量为相机坐标系下，特征点的3D坐标，观测方程为：

其中，X_{c_k}，T_{cw_k}，w_k分别为k时刻3D点在相机坐标系下的坐标，世界坐标系到相机坐标系的变换矩阵，观测噪声；

其中，观测噪声符合以下特点：

对于空间中一点，实际像素坐标在±0.5×1.2ⁿ内均匀分布，深度误差根据相机型号确定；

第一次观测，可以得到该点在世界坐标系下的坐标，第二次观测，可以得到世界坐标系下的速度，第三次观测，得到该点的加速度，对于该点第三次及以上的观测，可以使用运动方程对该点的世界坐标进行预测，用于步骤4.2的相机位姿计算；另外，对于该点的多次观测，使用卡尔曼滤波对该点在世界坐标系下的坐标、速度、加速度、协方差进行更新；

步骤5.2，场景动态程度表示场景内，动态物体所占图像区域与动态物体运动速度的综合结果，场景内动态物体所占区域越大，场景动态程度越高，动态物体运动速度越快，场景动态程度越高；

使用公式表示，该帧中与2号地图匹配成功的第i个点速度记为v_i，动态程度表示为：

4.如权利要求1或2所述的动态环境下基于几何与运动约束的机器人RGB-DSLAM方法，其特征在于：所述步骤6的实现过程为：

步骤6.1，对于经过三次及以上观测次数的2号地图中的点，会有两次以上的速度值，取出最后相邻两次的速度值，对于真实运动的点，相邻两次速度的方向应该趋于一致，两次速度值较大；对于实际静止的点，由于像素坐标是整型变量，得到的速度大小一般不为0，且方向来回跳动，因此可以使用相邻两次的速度矢量来判断该点的运动状态；

使用公式表达：

用v_o和v_o-1表示该特征点的最后两次观测速度，||*||₁表示向量*的1范数；

对于静止的点：

||v_o-v_o-1||₁＞＞||v_o+v_o-1||₁；

对于真实运动的点：

||v_o-v_o-1||₁＜＜||v_o+v_o-1||₁；

剩余不符合上述两个约束条件的点被认为是错误匹配；

步骤6.2，在关键帧的所有特征点中，部分特征较弱，部分仅出现一次就不会再次出现，还有一部分可疑静点存在时间较长，后续会被加入到1号地图中成为静点，为了提高图像帧与2号地图中的点的配对速度，同时去除过多冗余点，需要对2号地图中的点进行剔除，2号地图中的点如果在未来3帧内没有再次被观测到，或者被观测5次就被剔除。

5.如权利要求1或2所述的动态环境下基于几何与运动约束的机器人RGB-DSLAM方法，其特征在于：所述步骤7的实现过程为：

步骤7.1，若当前帧非关键帧，则跳过该步骤，若当前帧是关键帧，则挑选出以前的15个与当前关键帧共视程度比较大的老关键帧，根据场景的动态程度，选择保留其中的8～15个时间跨度最长的老关键帧，场景动态程度越高，保留的关键帧越多，场景动态程度越低，保留的关键帧越少；

步骤7.2，对于保留下来的老关键帧，进行静态点筛选，根据筛选结果，产生1号地图点，包括如下子步骤：

步骤7.2.1，使用极线约束排除非静止状态的点，C₁、C₂为运动中相机的光心所处的位置，相机从C₁运动到C₂，x₁表示C₁观测时，空间中的点所处的位置，x₂～x₄表示若x₁点运动，C₂观测时，x₁可能所处的3个位置，若空间中的点为静止状态，根据极线约束，该点在两帧中投影的极线应当位于同一平面上，即直线l₁与l₂应当位于同一平面上，若空间中该点不符合该约束，则可以判断该点是非静止状态的点；若空间点在像素平面的投影点距离极线的距离

则判断为非静止点，其中n为该点在图像金字塔中的层数；

步骤7.2.2，使用重投影误差排除非静止状态的点，根据C₁观测时，C₁的位姿和投影的像素点坐标，以及C₂观测时，C₂的位姿，使用重投影方法可以预测C₂观测时，该点所投影的像素点坐标，

其中K为相机内参，T__cw1为C₁位姿，T_{_cw2}为C₂位姿，u₁，v₁为C₁观测的像素坐标，z₁为C₁观测的深度值，

为投影在C₂中的预测值；

若实际投影和预测投影之间的像素坐标距离

则判断为非静止状态的点；

步骤7.2.3，对于剩下的关键帧之间的配对点，使用三角化的方式，得到点在当前关键帧下的相机坐标，最后，转换到世界坐标系下，直接加入1号地图。

Images