[发明专利]一种基于激光SLAM的扫描匹配方法

权利要求书

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，由当前位姿更新当前的地图；返回步骤二进行下一时刻的位姿求解。