CN112991449A

CN112991449A - 一种agv定位与建图方法、系统、装置及介质

Info

Publication number: CN112991449A
Application number: CN202110300346.3A
Authority: CN
Inventors: 周方华; 魏武; 李艳杰; 高天啸
Original assignee: South China University of Technology SCUT
Current assignee: South China University of Technology SCUT
Priority date: 2021-03-22
Filing date: 2021-03-22
Publication date: 2021-06-18
Anticipated expiration: 2041-03-22
Also published as: CN112991449B

Abstract

本发明公开了一种AGV定位与建图方法、系统、装置及介质，其中方法包括：采集环境中的图像信息；提取每一帧图像的ORB特征点，根据提取的ORB特征点对相邻两帧图像进行特征点匹配；根据配对的ORB特征点的像素位置，获取相邻两帧图像之间的旋转矩阵和平移向量；基于特征点匹配的结果，采用视差角参数化方法对ORB特征点和相机位姿进行参数化；根据旋转矩阵、平移向量、参数化后的ORB特征点和相机位姿构建后端优化目标函数，根据后端优化目标函数对相机位姿和ORB特征点进行优化；根据优化后的相机位姿和ORB特征点建立环境地图。本发明采用视差角参数化方法对相机位姿和特征点进行参数化，避免了后端优化算法不收敛的问题，可广泛应用于AGV控制技术领域。

Description

一种AGV定位与建图方法、系统、装置及介质

技术领域

本发明涉及AGV控制技术领域，尤其涉及一种AGV定位与建图方法、系统、装置及介质。

背景技术

计算机和传感器技术的快速发展在很大程度上推动了对AGV移动平台的定位和建图(SLAM)的研究，而以相机作为传感器的视觉SLAM算法因其成本低廉、轻便易于携带、能够获取丰富的场景信息等诸多优点而备受研究者青睐。目前视觉SLAM主要分为基于特征点法的SLAM算法和基于光流法SLAM算法，其中基于特征点法的SLAM算法的综合性能相对较好，如ORB-SLAM2算法，不仅有较快的处理速度，而且有较高的准确性。

在基于特征点的视觉SLAM算法中，特征点的选取及其参数化方法会在很大程度上影响整体算法的稳定性、鲁棒性和处理速度。目前绝大多数视觉SLAM算法采用XYZ坐标系对特征点进行参数化，虽然这种参数化方法能够非常直观的表示特征点的空间位置，但对于一些距离相机较远的特征点或者当相机方向与AGV运动方向位于同一平面上时，会导致后端优化算法不收敛或优化迭代次数大幅度上升，从而导致算法速度变慢甚至不稳定。逆深度参数化方法采用距离的倒数表示特征点与相机中心的距离，虽然能够很好的处理距离较远的特征点，但仍然无法决绝当相机方向与AGV运动方向位于同一平面上时导致的后端优化算法不收敛的问题。

发明内容

为至少一定程度上解决现有技术中存在的技术问题之一，本发明的目的在于提供一种基于视差角特征点参数化的AGV定位与建图方法、系统、装置及介质。

本发明所采用的技术方案是：

一种AGV定位与建图方法，包括以下步骤：

采集环境中的图像信息；

提取每一帧图像的ORB特征点，根据提取的ORB特征点对相邻两帧图像进行特征点匹配；

根据配对的ORB特征点的像素位置，获取相邻两帧图像之间的旋转矩阵和平移向量；

基于特征点匹配的结果，采用视差角参数化方法对ORB特征点和相机位姿进行参数化；

根据旋转矩阵、平移向量、参数化后的ORB特征点和相机位姿构建后端优化目标函数，根据后端优化目标函数对相机位姿和ORB特征点进行优化；

根据优化后的相机位姿和ORB特征点建立环境地图。

进一步，所述ORB特征点由关键点和描述子组成，所述提取每一帧图像的ORB特征点，包括：

将每一帧图像转换为灰度图和HSV图像，并构建灰度图像金字塔和HSV彩色图像金字塔；

根据所述灰度图像金字塔获取关键点，根据所述关键点和所述HSV彩色图像金字塔获取所述描述子。

进一步，所述关键点为FAST关键点，所述描述子为BRIEF-32描述子，所述BRIEF-32描述子为256位的二进制向量，且二进制向量中的每一位由圆形区域中任意两个像素块的颜色相似度确定；

所述圆形区域以关键点为中心、半径为m个像素；

所述像素块为按照预设方式在所述圆形区域获取的区域；

采用像素块的几何中心至像素块的灰度质心的方向向量表示ORB特征点的方向。

进一步，所述根据提取的ORB特征点对相邻两帧图像进行特征点匹配，包括：

计算相邻两帧图像中ORB特征点两两之间的汉明距离，检测到所述汉明距离小于预设阈值，判定特征点配对成功。

进一步，所述根据配对的ORB特征点的像素位置，获取相邻两帧图像之间的旋转矩阵和平移向量，包括：

对相邻两帧图像中ORB特征点进行筛选，获得多对配对成功的ORB特征点；

将获得的ORB特征点带入对极几何约束关系计算获得本质矩阵E；

根据本质矩阵E解求出两帧图像之间的旋转矩阵R和平移向量t。

进一步，所述采用视差角参数化方法对ORB特征点和相机位姿进行参数化，包括：

相机位姿由当前相机位姿相对于初始相机位姿的欧拉角和平移向量进行描述，设第i个相机位姿为p_i：

p_i＝[α_i β_i γ_ix_i y_i z_i]^T

其中[α_i β_i γ_i]分别为相机位姿p_i的偏航角、俯仰角和滚转角，[x_i y_i z_i]为相机位姿p_i相对于初始位置的平移向量；

ORB特征点的位置由观测到该ORB特征点的关联帧和ORB特征点相对于关联帧相机中心的角度来表示；

若ORB特征点F_j只被某一帧图像p_m观测到一次，将此帧选为该ORB特征点的关联帧，ORB特征点F_j表示为：

F_j＝[ψ_j θ_j]^T

其中，ψ_j为方向角，θ_j为俯仰角；

若ORB特征点被多个图像帧观测到，则选择多个图像帧中视差角较大的两帧图像作为该ORB特征点的主关联帧p_m和副关联帧p_n，ORB特征点F_j则表示为：

F_j＝[ψ_j θ_j ω_j]^T

其中，ψ_j为方向角，θ_j为俯仰角，ω_j表示主关联帧和副关联帧至ORB特征点的方向向量的夹角，称为视差角。

进一步，所述后端优化目标函数通过以下方式构建获得：

根据旋转矩阵R_i和平移向量t_i得到ORB特征点到投影平面的投影方程：

其中：

R_i为旋转矩阵，根据欧拉角计算得出：

R_i＝f(α_i，β_i，γ_i)

为一方向向量，表达式为：

表示从相机中心O_i到ORB特征点F_k的一个向量，表达式为：

角度ξ_k由两个向量的点积计算：

根据投影方程构建后端优化目标函数：

其中，X表示由AGV相机的所有姿态和ORB特征点坐标组成的状态向量，h(X)表示投影方程，Z表示由所有ORB特征点的2D像素坐标组成的向量。

本发明所采用的另一技术方案是：

一种AGV定位与建图系统，包括：

图像采集模块，用于采集环境中的图像信息；

特征匹配模块，用于提取每一帧图像的ORB特征点，根据提取的ORB特征点对相邻两帧图像进行特征点匹配；

移动计算模块，用于根据配对的ORB特征点的像素位置，获取相邻两帧图像之间的旋转矩阵和平移向量；

参数化模块，用于基于特征点匹配的结果，采用视差角参数化方法对ORB特征点和相机位姿进行参数化；

优化模块，用于根据旋转矩阵、平移向量、参数化后的ORB特征点和相机位姿构建后端优化目标函数，根据后端优化目标函数对相机位姿和ORB特征点进行优化；

建图模块，用于根据优化后的相机位姿和ORB特征点建立环境地图。

本发明所采用的另一技术方案是：

一种AGV定位与建图装置，包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上所述方法。

本发明所采用的另一技术方案是：

一种存储介质，其中存储有处理器可执行的程序，所述处理器可执行的程序在由处理器执行时用于执行如上所述方法。

本发明的有益效果是：本发明采用视差角参数化方法对相机位姿和特征点进行参数化，避免了距离相机较远的特征点或位于AGV前进方向上的特征点所造成的后端优化目标函数收敛速度慢或不收敛的问题。

附图说明

为了更清楚地说明本发明实施例或者现有技术中的技术方案，下面对本发明实施例或者现有技术中的相关技术方案附图作以下介绍，应当理解的是，下面介绍中的附图仅仅为了方便清晰表述本发明的技术方案中的部分实施例，对于本领域的技术人员而言，在无需付出创造性劳动的前提下，还可以根据这些附图获取到其他附图。

图1是本发明实施例中一种AGV定位与建图方法的步骤流程图；

图2是本发明实施例中视差角参数化方法示例图；

图3是本发明实施例中ORB特征点重投影过程示意图；

图4是本发明实施例中视差角角度参数与直角坐标系参数的关系示意图；

图5是本发明实施例中一种基于视差角特征点参数化的AGV定位与建图方法的总流程图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，若干的含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。

本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。

本实施例提供一种AGV定位与建图方法，该方法主要基于视差角特征点参数化，采用方向角、俯仰角和视差角三个角度变量描述特征点的空间位置，分别采用欧拉角和平移向量表示相机的姿态和位置，并以此参数化方法构建后端优化的目标函数，从而解决了在某些条件下优化算法的收敛速度慢或不收敛的问题，从而提高了SLAM系统的处理速度和鲁棒性。如图1和图5所示，该方法包括以下步骤：

S1、采集环境中的图像信息。

通过AGV自身携带的相机不断地采集环境中的图像信息，并对采集到的图像进行去畸变处理。

S2、提取每一帧图像的ORB特征点，根据提取的ORB特征点对相邻两帧图像进行特征点匹配。

对每一帧图像提取ORB特征点，并对相邻两帧图像进行特征点匹配；ORB特征点由FAST关键点和BRIEF描述子组成，同时为了使特征点具备方向不变性，采用像素块几何中心至像素块灰度中心的方向向量表示特征点的方向；通过计算前后两帧图像中特征点两两之间的汉明距离，选择距离小于阈值的配对点视为匹配成功。

在提取每帧图像的ORB特征点时，图像转换为灰度图和HSV图像，并分别构建图像金字塔。通过建立图像金字塔，在金字塔的每一层都进行特征点的提取，从而形成尺度空间，保证特征点的尺度不变性。

在灰度图像金字塔的每一层中提取FAST关键点；FAST关键点是一种角点，通过比较某一像素点与其临近点的灰度值大小进行判定。

以灰度图中某一像素点p(设其灰度值为I_p)为圆心，在以3个像素单位为半径的圆周上共有16个像素点，设其灰度值为I_i(i＝1,2,...,16)：

其中：

在本实施例中取阈值T＝0.2I_p，若N>N₀则认为p点为关键点，N₀通常取12或9，本实施例中取N₀＝9。

为了降低边缘效应，应尽可能的使特征点均匀的分布在整幅图中，在提取关键点之前先把灰度图划分为多个30*30的小区域，分别在各个小区域中提取特征点。设整幅图像共提取了M₀个特征点，期望提取特征点数为M₁，则应满足条件M₀>M₁。

采用灰度质心法为每个关键点计算方向，计算方法如下：

以关键点为中心，选取一个半径为r个像素的圆盘区域Patch，计算出以图像块灰度值为权重的中心点，即灰度质心C：

其中，

假设灰度质心C圆盘几何中心O不重合，则该关键点的方向可以由向量

的方向角θ表示：

θ＝atan2(m₀₁，m₁₀)。

本实施例选择BRIEF-32描述子，即采用一个256bit的二进制向量对一个特征点进行描述。在以关键点作为中心的31*31像素的图像块中，以机器学习的方式选取256对像素点，设每个像素点坐标为(x_i，y_i)，i＝1，2，...，512，构成矩阵D：

为保证特征点描述子的旋转不变性，需要将D矩阵以特征点的方向角θ进行旋转变换：

D_θ＝R_θD

其中R_θ为特征点的方向角θ的旋转矩阵：

D_θ为旋转之后的像素点的坐标构成的矩阵，设其中的一对像素点坐标分别为(x′_i1，y′_i1)，(x′_i2，y′_i2)，与描述子的第i位Des_i相对应。分别在HSV三个单色通道图像中计算以为(x′_i1，y′_i1)，(x′_i2，y′_i2)中心、以2个像素为半径的圆盘形像素块Patch的像素平均值，计算方法如下：

计算这两个像素块的颜色相似度：

其中，Cdist_i为色彩差异，Bdist_i为亮度差异。

则描述子的第i位Des_i根据如下方法定义：

其中ε_C、ε_B分别为色彩差异和亮度差异的阈值，当Cdist_i和Bdist_i均小于阈值时表示两个像素块色彩相近，描述子对应位取值为1，否则表示两个像素块色彩相异，描述子对应位取值为0。

对256对像素点进行上述运算后即可得到一个256位的二进制向量，即该特征点的R-BRIEF描述子。

S3、根据配对的ORB特征点的像素位置，获取相邻两帧图像之间的旋转矩阵和平移向量。

根据配对的特征的像素位置，由对极几何约束条件计算出两帧之间的旋转矩阵R和平移向量t；将步骤S2中筛选出来的多对配对点带入对极几何约束方程计算出本质矩阵E，再由奇异值分解求出两帧之间的旋转矩阵R和平移向量t。由于前后两帧图像往往存在很多对匹配特征点，且其中可能存在错误匹配，通常可以通过求解最小二乘解或采用随机采样一致性(RANSAC)算法减小误匹配造成的影响，本实施例采用随机采样一致性算法。

S4、基于特征点匹配的结果，采用视差角参数化方法对ORB特征点和相机位姿进行参数化。

AGV相机位姿由当前位姿相对于相机初始位姿的欧拉角和平移向量进行描述，设第i个相机位姿为p_i：

p_i＝[α_i β_i γ_ix_i y_i z_i]^T

其中[α_i β_i γ_i]分别为相机Pi的偏航角、俯仰角和滚转角，[x_i y_i z_i]为Pi相对于初始位置的平移向量。

特征点的位置主要由观测到该特征点的关联帧和特征点相对于关联帧相机中心的角度来表示。如图2所示，设特征点由F_j表示，若特征点只被某一帧图像p_m观测到一次，将此帧选为该特征点的关联帧，特征点F_j表示为：

F_j＝[ψ_j θ_j]^T

其中ψ_j为方向角，θ_j为俯仰角，从图2中可以看出它们表示了在世界坐标系下从关联帧p_m到特征点Fj的角度信息。

若特征点被多个图像帧观测到，则选择其中视差角较大的两帧图像作为该特征点的主关联帧p_m和副关联帧p_n，特征点Fj则表示为：

F_j＝[ψ_j θ_j ω_j]^T

其中ψ_j为方向角，θ_j为俯仰角，ω_j表示主关联帧和副关联帧至特征点的方向向量的夹角，称为视差角。

S5、根据旋转矩阵、平移向量、参数化后的ORB特征点和相机位姿构建后端优化目标函数，根据后端优化目标函数对相机位姿和ORB特征点进行优化。

计算特征点重投影误差，构建后端优化目标函数，对位姿和特征点位置进行优化得到精确位姿。

如图3所示，根据步骤S3的计算结果，将特征点重投影到图像中，得到观测方程过程如下：

设摄像机内部参数矩阵为K，相机的旋转矩阵和平移向量分别为R_i和t_i，通过针孔相机投影模型可得到特征点到投影平面的投影方程：

其中：

其中R_i为旋转矩阵，可根据欧拉角计算得出，即：

R_i＝f(α_i，β_i，γ_i)

其中，

为一方向向量，如图4中所示，

表示从相机中心O_i到特征点F_k的一个向量：

其中角度ξ_k可以根据两个向量的点积计算：

进一步地，由上述投影模型可构造出如下目标函数：

其中，X表示由AGV相机的所有姿态和特征点坐标组成的状态向量，h(X)表示投影方程也即观测函数，Z表示由所有特征点的像素坐标组成的向量。使用高斯牛顿法优化该目标函数可以获得最优的机器人姿态和特征点坐标。

S6、根据优化后的相机位姿和ORB特征点建立环境地图。

根据优化结果建立环境地图；将优化得到的位姿和特征点位置与目前已建立的地图进行对比匹配，将新的位姿和特征点加入地图中，然后对地图进行全局优化，得到更为精确的环境地图。

综上所述，本实施例的一种AGV定位与建图方法相对于现有技术，具有如下优点：

(1)本实施例采用视差角参数化方法对相机位姿和特征点进行参数化，从而避免了距离相机较远的特征点或位于AGV前进方向上的特征点所造成的后端优化目标函数收敛速度慢或不收敛的问题。

(2)本实施例构建了基于视差角参数化方法后端优化问题的观测函数，使优化迭代过程更易收敛，提升了优化速度。

(3)本实施例提高了定位与建图算法的处理速度与鲁棒性。

本实施例还提供一种AGV定位与建图系统，包括：

图像采集模块，用于采集环境中的图像信息；

本实施例的一种AGV定位与建图系统，可执行本发明方法实施例所提供的一种AGV定位与建图方法，可执行方法实施例的任意组合实施步骤，具备该方法相应的功能和有益效果。

本实施例还提供一种AGV定位与建图装置，包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如图1所示方法。

本实施例的一种AGV定位与建图装置，可执行本发明方法实施例所提供的一种AGV定位与建图方法，可执行方法实施例的任意组合实施步骤，具备该方法相应的功能和有益效果。

本申请实施例还公开了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存介质中。计算机设备的处理器可以从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行图1所示的方法。

本实施例还提供了一种存储介质，存储有可执行本发明方法实施例所提供的一种AGV定位与建图方法的指令或程序，当运行该指令或程序时，可执行方法实施例的任意组合实施步骤，具备该方法相应的功能和有益效果。

在一些可选择的实施例中，在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如，取决于所涉及的功能/操作，连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外，在本发明的流程图中所呈现和描述的实施例以示例的方式被提供，目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的，其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。

此外，虽然在功能性模块的背景下描述了本发明，但应当理解的是，除非另有相反说明，所述的功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中，或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是，有关每个模块的实际实现的详细讨论对于理解本发明是不必要的。更确切地说，考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下，在工程师的常规技术内将会了解该模块的实际实现。因此，本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本发明。还可以理解的是，所公开的特定概念仅仅是说明性的，并不意在限制本发明的范围，本发明的范围由所附权利要求书及其等同方案的全部范围来决定。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。

计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

在本说明书的上述描述中，参考术语“一个实施方式/实施例”、“另一实施方式/实施例”或“某些实施方式/实施例”等的描述意指结合实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施方式，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施方式进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

以上是对本发明的较佳实施进行了具体说明，但本发明并不限于上述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。