CN111190191A - 一种基于激光slam的扫描匹配方法 - Google Patents

Abstract

本发明涉及一种基于激光SLAM的扫描匹配方法。本发明利用激光雷达采集物体距离传感器的角度及距离的点云信息，通过匹配不同时刻下的点云来计算传感器的相对运动的位姿变化，这一过程称为扫描匹配。扫描匹配方法可以结合里程计信息来估计位姿，以获得较好的位姿估计。然而现实中诸如无人机，扫地机器人等应用中，通常无里程计信息。这使得机器人的定位与建图能力在很大程度上依赖于扫描匹配技术的精确性。本发明本可以在无里程计的情况下，达到较好的位姿估计精度。

Description

一种基于激光SLAM的扫描匹配方法

技术领域

本发明涉及定位和建图技术领域，具体涉及一种基于激光SLAM的扫描匹配方法。

背景技术

在实际应用中，机器人需要在未知的环境中进行有效的路径规划并自主探索环境，这使得它必须具备自主创建环境地图并且实现准确自我定位的能力。这一问题被称为SLAM(即时定位和地图构建)。SLAM是一个具有挑战性的问题，因为地图的质量与机器人位姿估计的精度之间存在着相互依存的关系：不准确的位姿估计将会导致不精确的地图，反之亦然。而实际造成位姿或地图误差的因素众多，诸如传感器本身的精度和噪声，过于简单的地图表示以及不能保证收敛到全局极值的定位等。

激光SLAM利用激光雷达采集物体距离传感器的角度及距离的点云信息，通过匹配不同时刻下的点云来计算传感器的相对运动的位姿变化，这一过程称为扫描匹配。扫描匹配方法可以结合里程计信息来估计位姿，以获得较好的位姿估计。然而现实中诸如无人机，扫地机器人等应用中，通常无里程计信息。这使得机器人的定位与建图能力在很大程度上依赖于扫描匹配技术的精确性。

发明内容

本发明为了克服现有扫描匹配技术的缺陷，提供了一种基于激光SLAM的扫描匹配方法，在无里程计的情况下，在一定程度上克服了原有方法可能导致计算结果不收敛和局部近似不够准确的缺陷，达到较好的位姿估计精度，并有效提高了对环境地图的构建质量。

发明采用以下技术方案予以实现，包括以下步骤：

步骤一、采用占据栅格地图表示环境地图,在每一时刻的点云中确定每个栅格的占据概率；栅格地图将环境地图划分成等大的有限个网格，实际环境中的每个点所在的网格只有两种状态，占据即存在障碍物或者空闲即不存在障碍物；

即：m＝∑_im_i，i＝1...n；

其中m_i为栅格地图中的栅格，m表示整个环境地图；

对于栅格地图，以激光的观测数据和机器人的位姿估计每一个栅格m_i在t时刻的状态p：p(m_i|z_1:t,x_1:t)；其中z_1:t是激光从1时刻到t时刻的所有观测数据，x_1:t是机器人在从1时刻到t时刻的所有位姿；而栅格m_i在t时刻的状态由其在之前时刻的状态和第t时刻的观测数据和位姿决定；栅格地图的网格中存储该网格被占据的概率值，并使用二值贝叶斯滤波方法不断更新网格值，以此确定该网格最终的状态，p＝1表示占据，p＝0表示空闲；

步骤二、匹配两时刻间的点云，使其匹配最大化，从而得到机器人的相对位姿；激光SLAM中的定位问题由扫描匹配来解决，扫描匹配方法通过最大化重叠已存在地图和当前点云，求解机器人的当前位姿和上一时刻位姿间的相对平移T和旋转θ；为了求解出机器人两个时刻间的相对位姿，通常将当前扫描点{p_i}经上一时刻位姿q的旋转变换后投影到已存在地图M，使用欧式距离度量扫描点{p_i}到地图M的匹配程度；该问题是一个最小化问题，描述为：

图的欧式投影；q⊕p_i为扫描点通过位姿的旋转变换：

其中R(θ)为旋转矩阵，T为平移矩阵；

使用高斯牛顿法求解该最小化问题，并确定每次梯度下降的方向；对于地图M中的某一点P_m,其地图值为M(P_m)，同时其导数表示为

该点处的地图值可由距该点最近的四个整点坐标P₀₀、P₀₁、P₁₀、P₁₁的地图值估计和计算；计算公式如下：

其中x,y为点P_m的坐标；

在栅格地图的表示中，两个相邻网格间的距离为1；在栅格地图中,对于某时刻机器人位姿q，最小化函数定义为：

q^*表示寻找一个在激光扫描点和地图间最优匹配时的变换，该变换即为两次激光扫描间机器人的位姿变化；S_i(q)表示机器人当前位姿下扫描端点s_i的世界坐标：S_i(q)＝R(θ)s_i+T；首先通过旋转矩阵将扫描端点进行旋转变换：R(θ)s_i，然后加入移矩阵T即可得到扫描端点s_i的世界坐标；将q^*转化为求解误差最小时的位姿增量Δq：

通过对其求导并令导数为0可得：

其中：

表示扫描端点s_i所在栅格的地图值导数；同时H矩阵表示为该导数的平方；

步骤三、使用线搜索方法寻找每次梯度下降的最优步长；用于线搜索方法求解梯度下降方向最优步长，该问题描述为求解一个一元最优化问题：

其中f(x)＝1-M(S_i(x))，则

表示最小化的目标函数，h为梯度下降的方向，可由高斯牛顿法求得；α为步长，该方法的思想即为求解使得函数

最小的步长α；此时机器人位姿通过目标函数的最优解α求得：q＝x+αh；S_i(x+αh)表示位姿x+αh下扫描端点s_i的世界坐标；M(S_i(x+αh))则表示扫描端点s_i所在栅格的地图值；α需在强Wolfe条件的线搜索方法中满足：

其中0<c₁<c₂<1，c₁，c₂均为自定义参数，通常令c₁＝0.001,c₂＝0.1；

线搜索方法在搜索方向选择为“最速下降方向”即负梯度方向时，能达到一个“全局收敛”的状态，此时其收敛速度为线性收敛速度；

步骤四、根据扫描匹配结果求出相对位姿；根据扫描匹配求得的位姿增量更新当前机器人位姿q＝Δq+q，由当前位姿更新当前的地图；返回步骤二进行下一时刻的位姿求解。

附图说明

图1为本发明的流程图。

具体实施方式

本发明为实现上述方法的功能将采用如下的实现过程。

为实现上述方法本方案需要用到下面的设备：AGV小车，PC机一台，激光雷达一台。激光雷达搭载于AGV小车上，AGV小车用于搭载激光雷达在环境中运动，PC机用于控制AGV小车运动，同时解析并可视化激光雷达获取的环境扫描数据。激光雷达要求能获取环境扫描信息；PC机要求能够远程控制AGV小车，同时需要安装相应的雷达驱动及ROS机器人操作***，以便用于可视化激光雷达的建图结果。

一种基于激光SLAM的扫描匹配方法包括以下步骤：

步骤一、采用占据栅格地图表示环境地图,在每一时刻的点云中确定每个栅格的占据概率。

栅格地图将环境地图划分成等大的有限个网格，实际环境中的每个点所在的网格只有两种状态，占据即存在障碍物或者空闲即不存在障碍物。

即：m＝∑_im_i，i＝1...n；

其中m_i为栅格地图中的栅格，m表示整个环境地图。

对于栅格地图，以激光的观测数据和机器人的位姿估计每一个栅格m_i在t时刻的状态p：p(m_i|z_1:t,x_1:t)；其中z_1:t是激光从1时刻到t时刻的所有观测数据，x_1:t是机器人在从1时刻到t时刻的所有位姿。而栅格m_i在t时刻的状态由其在之前时刻的状态和第t时刻的观测数据和位姿决定。栅格地图的网格中存储该网格被占据的概率值，并使用二值贝叶斯滤波方法不断更新计算网格值，以此确定该网格最终的状态(p＝1表示占据，p＝0表示空闲)。

步骤二、匹配两时刻间的点云，使其匹配最大化，从而得到机器人的相对位姿。激光SLAM中的定位问题由扫描匹配来解决，扫描匹配方法通过最大化重叠已存在地图和当前点云，求解机器人的当前位姿和上一时刻位姿间的相对平移T和旋转θ。为了求解出机器人两个时刻间的相对位姿，通常将当前扫描点{p_i}经上一时刻位姿q的旋转变换后投影到已存在地图M，使用欧式距离度量扫描点{p_i}到地图M的匹配程度。该问题是一个最小化问题，描述为：

其中

为变换后的点到地图的欧式投影。

为扫描点通过位姿的旋转变换：

其中R(θ)为旋转矩阵，T为平移矩阵。

使用高斯牛顿法求解该最小化问题，并确定每次梯度下降的方向。

栅格地图中的离散值不允许直接计算其导数，同时也限制了地图能够实现的精度。出于这个原因，通常采取双线性插值法估计栅格的概率值和导数。由此栅格地图能够视为具有连续概率分布的值。

对于地图M中的某一点P_m,其地图值为M(P_m)，同时其导数表示为

该点处的地图值可由距该点最近的四个整点坐标P₀₀、P₀₁、P₁₀、P₁₁的地图值估计和计算。计算公式如下：

其中x,y为点P_m的坐标。

在栅格地图的表示中，两个相邻网格间的距离为1(指地图坐标系，它们之间的实际距离由地图分辨率即网格大小决定)。在栅格地图中,对于某时刻机器人位姿q，最小化函数定义为：

q^*表示寻找一个在激光扫描点和地图间最优匹配时的变换，该变换即为两次激光扫描间机器人的位姿变化。S_i(q)表示机器人当前位姿下扫描端点s_i的世界坐标：S_i(q)＝R(θ)s_i+T；首先通过旋转矩阵将扫描端点进行旋转变换：R(θ)s_i，然后加入移矩阵T即可得到扫描端点s_i的世界坐标。将q^*转化为求解误差最小时的位姿增量Δq：

通过对其求导并令导数为0可得：

其中：

表示扫描端点s_i所在栅格的地图值导数。同时H矩阵表示为该导数的平方。

步骤三、使用线搜索方法寻找每次梯度下降的最优步长。

线搜索方法通常用于求解梯度下降方向最优步长，该问题描述为求解一个一元最优化问题：

其中f(x)＝1-M(S_i(x))，则

表示最小化的目标函数，h为梯度下降的方向，可由高斯牛顿法求得。α为步长，该方法的思想即为求解使得函数

最小的步长α。此时机器人位姿通过目标函数的最优解α求得：q＝x+αh；S_i(x+αh)表示位姿x+αh下扫描端点s_i的世界坐标。M(S_i(x+αh))则表示扫描端点s_i所在栅格的地图值。

为提高搜索效率，采用了一种非精确的线搜索方法，α需在强Wolfe条件的线搜索方法中满足：

其中0<c₁<c₂<1，c₁，c₂均为自定义参数，可手动设置，通常令c₁＝0.001,c₂＝0.1。

一种非精确的线搜索方法，首先寻找一个包含解的区间，然后运用zoom方法寻找满足约束条件的解。具体包括以下步骤：

步骤⑴.寻找一个步长的下界使得在该区间内包含最优解α^*。

步骤⑵.计算当前步长值下的函数

判断是否满足充分小条件，如果不满足则说明最优解在(α_i-1,α_i)之间，转至zoom方法。验证强Wolfe条件是否满足，若满足α^*即为α_i，输出α_i。若不满足条件且当前梯度为正值时，交换上一个步长并转至zoom方法。

步骤⑶.上述条件均不满足，则重复步骤⑵求解下一个步长点。

zoom方法通过一个步长的区间(α_lo,α_hi)，求解该区间内满足强Wolfe条件的步长。zoom方法步骤如下：1.判断当前

是否满足Wolfe的条件，若不满足则缩减区间，计算下一个步长点，转至步骤⑵。2.判断是否满足强Wolfe的条件，若满足α^*即为α_j，输出α_j。3.判断当前区间是否为递减区间，若是则调整区间，使其满足算法输入条件，然后重复执行zoom方法。

线搜索方法在搜索方向选择为“最速下降方向”即负梯度方向时，能达到一个“全局收敛”的状态，此时其收敛速度为线性收敛速度。

步骤四、根据扫描匹配结果求出相对位姿。根据扫描匹配求得的位姿增量更新当前机器人位姿q＝Δq+q，由当前位姿更新当前的地图。返回步骤二进行下一时刻的位姿求解。

Claims (3)

1.一种基于激光SLAM的扫描匹配方法，其特征在于：包括以下步骤：

步骤一、采用占据栅格地图表示环境地图，在每一时刻的点云中确定每个栅格的占据概率；栅格地图将环境地图划分成等大的有限个网格，实际环境中的每个点所在的网格只有两种状态，占据即存在障碍物或者空闲即不存在障碍物；

即：m＝∑_im_i，i＝1...n；

其中m_i为栅格地图中的栅格，m表示整个环境地图；

对于栅格地图，以激光的观测数据和机器人的位姿估计每一个栅格m_i在t时刻的状态p：p(m_i|z_1：t，x_1：t)；其中z_1：t是激光从1时刻到t时刻的所有观测数据，x_1：t是机器人在从1时刻到t时刻的所有位姿；而栅格m_i在t时刻的状态由其在之前时刻的状态和第t时刻的观测数据和位姿决定；栅格地图的网格中存储该网格被占据的概率值，并使用二值贝叶斯滤波方法不断更新网格值，以此确定该网格最终的状态，p＝1表示占据，p＝0表示空闲；

步骤二、匹配两时刻间的点云，使其匹配最大化，从而得到机器人的相对位姿；激光SLAM中的定位问题由扫描匹配来解决，扫描匹配方法通过最大化重叠已存在地图和当前点云，求解机器人的当前位姿和上一时刻位姿间的相对平移T和旋转θ；为了求解出机器人两个时刻间的相对位姿，通常将当前扫描点{p_i}经上一时刻位姿q的旋转变换后投影到已存在地图M，使用欧式距离度量扫描点{p_i}到地图M的匹配程度；该问题是一个最小化问题，描述为：

其中

为变换后的点到地图的欧式投影；

为扫描点通过位姿的旋转变换：

其中R(θ)为旋转矩阵，T为平移矩阵；

使用高斯牛顿法求解该最小化问题，并确定每次梯度下降的方向；对于地图M中的某一点P_m，其地图值为M(P_m)，同时其导数表示为

该点处的地图值可由距该点最近的四个整点坐标P₀₀、P₀₁、P₁₀、P₁₁的地图值估计和计算；计算公式如下：

其中x，y为点P_m的坐标；

在栅格地图的表示中，两个相邻网格间的距离为1；在栅格地图中，对于某时刻机器人位姿q，最小化函数定义为：

q^*表示寻找一个在激光扫描点和地图间最优匹配时的变换，该变换即为两次激光扫描间机器人的位姿变化；S_i(q)表示机器人当前位姿下扫描端点s_i的世界坐标：S_i(q)＝R(θ)s_i+T；首先通过旋转矩阵将扫描端点进行旋转变换：R(θ)s_i，然后加入移矩阵T即可得到扫描端点s_i的世界坐标；将q^*转化为求解误差最小时的位姿增量Δq：

通过对其求导并令导数为0可得：

其中：

表示扫描端点s_i所在栅格的地图值导数；同时H矩阵表示为该导数的平方；

步骤三、使用线搜索方法寻找每次梯度下降的最优步长；用于线搜索方法求解梯度下降方向最优步长，该问题描述为求解一个一元最优化问题：

其中f(x)＝1-M(S_i(x))，则

表示最小化的目标函数，h为梯度下降的方向，可由高斯牛顿法求得；α为步长，该方法的思想即为求解使得函数

最小的步长α；此时机器人位姿通过目标函数的最优解α求得：q＝x+αh；S_i(x+αh)表示位姿x+αh下扫描端点s_i的世界坐标；M(S_i(x+αh))则表示扫描端点s_i所在栅格的地图值；α需在强Wolfe条件的线搜索方法中满足：

其中0＜c₁＜c₂＜1，c₁，c₂均为自定义参数，通常令c₁＝0.001，c₂＝0.1；

线搜索方法在搜索方向选择为“最速下降方向”即负梯度方向时，能达到一个“全局收敛”的状态，此时其收敛速度为线性收敛速度；

步骤四、根据扫描匹配结果求出相对位姿；根据扫描匹配求得的位姿增量更新当前机器人位姿q＝Δq+q，由当前位姿更新当前的地图；返回步骤二进行下一时刻的位姿求解。

2.如权利1所述的一种基于激光SLAM的扫描匹配方法，其特征在于：步骤三所述的线搜索方法，首先寻找一个包含解的区间，然后运用zoom方法寻找满足约束条件的解；具体包括以下步骤：

步骤(1).寻找一个步长的下界使得在该区间内包含最优解α^*；

步骤(2).计算当前步长值下的函数

判断是否满足充分小条件，如果不满足则说明最优解在(α_i-1，α_i)之间，转至zoom方法；验证强Wolfe条件是否满足，若满足α^*即为α_i，输出α_i；若不满足条件且当前梯度为正值时，交换上一个步长并转至zoom方法；

步骤(3).上述条件均不满足，则重复步骤(2)求解下一个步长点。

3.如权利2所述的一种基于激光SLAM的扫描匹配方法，其特征在于：所述的zoom方法通过一个步长的区间(α_lo，α_hi)，求解该区间内满足强Wolfe条件的步长；具体步骤如下：1.判断当前

是否满足Wolfe的条件，若不满足则缩减区间，计算下一个步长点，转至步骤(2)；2.判断是否满足强Wolfe的条件，若满足α^*即为α_j，输出α_j；3.判断当前区间是否为递减区间，若是则调整区间，使其满足输入条件，然后重复执行zoom方法。

Images