CN112904358B - 基于几何信息的激光定位方法 - Google Patents

Abstract

本发明公开了一种基于几何信息的激光定位方法。该方法用于实现机器人在工作空间内的定位，包括：在机器人工作空间中的设定区域布置至少三个激光反射板；构建包括激光反射板的机器人工作空间对应的二维场景地图；根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置；基于机器人在二维场景地图下的位置，对机器人的位姿进行预测估计。本发明的基于几何信息的激光定位方法通过在需要进行精确定位的设定区域布置激光反射板作为反射标记物，能够实现机器人在设定区域的精确定位，提高机器人的控制精度，定位可靠性高，算法复杂度和计算量低。

Description

基于几何信息的激光定位方法

技术领域

本发明涉及机器人定位技术领域，具体涉及一种基于几何信息的激光定位方法。

背景技术

随着机器人技术的不断发展，移动机器人在自然灾害和核泄漏救援、极地和外星探险、军事侦察和作战、工业制造和物流自动化、民用车辆智能化等方面具有广阔的应用前景，可以代替人类从事各种危险、繁重、枯燥的工作。自主导航能力是移动机器人执行工作任务的前提条件。现有的移动机器人导航方式主要有基于外界自然环境的激光导航方式，基于外界自然环境的激光导航方式又称为自然导航，该导航方式以激光雷达探测到的环境轮廓信息作为参考，进行导航，由于仅以环境轮廓信息作为参考，自然导航的精度和可靠性相对较低，特别是在环境信息较为单一的情况下，如较长的走廊，且算法复杂度相对较高。

发明内容

为解决上述现有技术中存在的部分或全部技术问题，本发明提供一种基于几何信息的激光定位方法。

为此，本发明公开了一种基于几何信息的激光定位方法，所述方法用于实现机器人在工作空间内的定位，包括以下步骤：

S1，在机器人工作空间中的设定区域布置至少三个激光反射板；

S2，构建包括激光反射板的机器人工作空间对应的二维场景地图；

S3，根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置；

S4，基于机器人在二维场景地图下的位置，对机器人的位姿进行预测估计。

在一些可选的实施方式中，所述构建包括激光反射板的机器人工作空间对应的二维场景地图，包括以下步骤：

S21，设定全局坐标系和局部坐标系；

S22，控制机器人运动，利用激光雷达扫描工作空间环境，获取一组机器人极坐标系下的点数据；

S23，对点数据进行数据聚类和特征提取，确定激光反射板的位置信息；

S24，完成二维场景地图构建。

在一些可选的实施方式中，全局坐标系和局部坐标下采用以下方式进行设定：

选取工作空间内的一个设定位置作为坐标原点o，选取一个设定方向作为x轴正方向，选取与设定方向垂直的方向作为y轴正方向，建立全局坐标系xoy；

选取机器人中心作为坐标原点O，选取机器人的前进方向作为X轴正方向，选取与机器人的前进方向垂直的方向作为Y轴正方向，建立局部坐标系XOY。

在一些可选的实施方式中，利用以下公式1对点数据进行数据聚类，以将点数据分成若干个点簇，一个点簇对应一个物体；

其中，r_k,k+1＝|r_k-r_k+1|表示两点的几何距离，r_k和r_k+1分别表示两点与激光雷达的距离，r_min＝min{r_k,r_k+1}，r_min表示点数据中所有的两点几何距离的最小值，C₀和β表示***参数，φ表示激光雷达角度分辨率。

在一些可选的实施方式中，采用以下方式进行特征提取，以确定激光反射板的位置信息：

设定数据聚类过程中获得如以下公式2表示的n个物体；

当物体的极径与激光反射板的实际半径在设定误差范围内时，认定相应的物体为激光反射板；

其中，E_seg表示物体集合，seg_k表示第k个物体，(x_k,y_k)表示第k个物体中心在全局坐标系下的位置，D_k表示第k个物体的极径，θ_k表示第k个物体的主轴方向。

在一些可选的实施方式中，所述根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置，包括以下步骤：

S31，根据激光雷达的观测信息，确定激光反射板在局部坐标系下的观测信息；

S32，利用角度匹配方式和/或距离匹配方式对激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息进行匹配；

S33，根据完成匹配的激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息，采用基于残差的最小二乘迭代算法和/或多边定位算法确定机器人在二维场景地图下的位置。

在一些可选的实施方式中，利用角度匹配方式对激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息进行匹配，采用以下方式进行：

设定工作空间环境中有m个激光反射板，m个激光反射板在全局坐标下的坐标为l_i(x_i,y_i)，1≤i≤m，激光雷达观测到q个激光反射板，q个激光反射板的观测方位角为α_j，1≤j≤q；

根据机器人在前一时刻的位姿分别计算m个激光反射板与机器人前进方向的理论方位角，将q个激光反射板的观测方位角与m个激光反射板的理论方位角进行比较，当观测方位角与理论方位角的差值在设定角度误差范围内，表示当前观测方位角对应的激光反射板与当前理论方位角对应的激光反射板相匹配。

在一些可选的实施方式中，利用距离匹配方式对激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息进行匹配，采用以下方式进行：

设定工作空间环境中有m个激光反射板，m个激光反射板在全局坐标下的坐标为l_i(x_i,y_i)，1≤i≤m，激光雷达观测到q个激光反射板，q个激光反射板与激光雷达的距离为p_j，q个激光反射板的观测方位角为α_j，1≤j≤q；

根据q个激光反射板与激光雷达的距离和q个激光反射板的观测方位角，计算任意两个观测到的激光反射板间的绝对距离，根据m个激光反射板在全局坐标下的坐标计算m个激光反射板中任意两个激光反射板之间的真实距离，将绝对距离与真实距离进行比较，当绝对距离与真实距离的差值在设定距离误差范围内，表示当前绝对距离对应的两个激光反射板与当前真实距离对应的两个激光反射板相匹配。

在一些可选的实施方式中，采用基于残差的最小二乘迭代算法确定机器人在二维场景地图下的位置包括：

选定一组激光反射板的坐标和方位角，建立选定的一组激光反射板坐标及方位角与激光雷达观测位置点的一组集合关系表达式；

利用泰勒级数展开略去高次项，构成观测值与理论值的残差方程组，使用最小二乘法求解出激光雷达的位置。

在一些可选的实施方式中，采用扩展卡尔曼滤波算法对机器人的位姿进行预测估计。

本发明技术方案的主要优点如下：

本发明的基于几何信息的激光定位方法通过在需要进行精确定位的设定区域布置激光反射板作为反射标记物，能够实现机器人在设定区域的精确定位，提高机器人的控制精度，定位可靠性高，算法复杂度和计算量低。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明一实施例的基于几何信息的激光定位方法的流程图；

图2为本发明一实施例的坐标系的示意图；

图3为本发明一实施例的基于几何信息的激光定位方法的另一流程图；

图4为本发明一实施例的激光反射板的观测位置示意图；

图5为本发明一实施例的三角定位算法的原理图；

图6为本发明一实施例的三边定位算法的原理图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

以下结合附图，详细说明本发明实施例提供的技术方案。

参见图1，本发明一实施例提供了一种基于几何信息的激光定位方法，该方法用于实现机器人在工作空间内的定位，包括以下步骤：

S1，在机器人工作空间中的设定区域布置至少三个激光反射板；

S2，构建包括激光反射板的机器人工作空间对应的二维场景地图；

S3，根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置；

S4，基于机器人在二维场景地图下的位置，对机器人的位姿进行预测估计。

以下对本发明一实施例提供的基于几何信息的激光定位方法的各个步骤进行具体说明。

S1，在机器人工作空间中的设定区域布置至少三个激光反射板。

具体地，根据机器人的实际定位需求，在环境可识别性较差或者需要进行精确定位的设定区域，例如目标点附近，布置至少三个激光反射板。

其中，为了保证能够实现机器人的精确定位，布置的多个激光反射板不能构成正多边形，且机器人上安装的激光雷达在机器人移动过程中至少能够观测到三个激光反射板，当激光反射板大于三个，能够提高机器人的定位精度，但计算量也会相应增加。

机器人在工作空间中未布置激光反射板的其他区域移动时，可以采用基于外界自然环境的激光导航方式对机器人进行定位。

S2，构建包括激光反射板的机器人工作空间对应的二维场景地图。

具体地，本发明一实施例中，构建包括激光反射板的机器人工作空间对应的二维场景地图包括以下步骤S21-S24；

S21，设定全局坐标系和局部坐标系；

参见图2，本发明一实施例中，选取工作空间内的一个设定位置作为坐标原点o，选取一个设定方向作为x轴正方向，选取与设定方向垂直的方向作为y轴正方向，建立全局坐标系xoy；选取机器人中心作为坐标原点O，选取机器人的前进方向作为X轴正方向，选取与机器人的前进方向垂直的方向作为Y轴正方向，建立局部坐标系XOY。

全局坐标系是在机器人的整个工作空间内设定一个地图坐标原点而建立的二维坐标系，在机器人工作空间环境中预先布置的激光反射板位置及机器人的定位导航计算均基于设定的全局坐标系，机器人的定位属于全局定位，即当机器人在设有全局坐标系的工作空间内行走时，能快速、准确地确定机器人的位姿。

局部坐标系也可以称为移动坐标系，局部坐标系是基于机器人上安装的激光雷达建立的坐标系，局部坐标系能够随着机器人的运动而运动，通过局部坐标系能够得到在机器人的导航定位过程中测量到的激光反射板相对于机器人的位置及角度，结合测量到的激光反射板到机器人的距离，能实现激光反射板的静态和动态匹配，局部坐标系的设定是得到机器人当前全局位姿的基础。

S22，控制机器人运动，利用激光雷达扫描工作空间环境，获取一组机器人极坐标系下的点数据；

机器人极坐标系表示以机器人中心为极点，机器人前进方向为极轴正方向的极坐标系，每个机器人极坐标下的点数据包括极径和极角，极径为相应的测量点与激光雷达的距离，极角为相应的测量点和激光雷达的连线与机器人前进方向的夹角。

S23，对点数据进行数据聚类和特征提取，确定激光反射板的位置信息；

由于激光雷达获得的数据量大，而其中部分数据受环境噪声影响，或者不是感兴趣的点，对进一步利用数据建立二维场景地图不利，因此需要对激光雷达获取的数据进行预处理。

本发明一实施例中，采用数据聚类对激光雷达获取的数据进行预处理。

数据聚类即数据分割(Data clustering)，数据聚类的目的是将一帧原始测量点的数据分成若干个点簇，一个点簇对应一个物体。在进行数据聚类时，先根据激光雷达与两个点的距离求出两点的几何距离，根据两点的几何距离是否在设定阈值内判断两点是否属于同一组，即，是否属于一个点簇，若两点的几何距离在设定阈值内，则该两点属于同一组，若两点的几何距离不在设定阈值内，则该两点不属于同一个组。

具体地，利用以下公式1对点数据进行数据聚类；

式中，r_k,k+1＝|r_k-r_k+1|表示两点的几何距离，r_k和r_k+1分别表示两点与激光雷达的距离，r_min＝min{r_k,r_k+1}，r_min表示点数据中所有的两点几何距离的最小值，C₀和β表示***参数，φ表示激光雷达角度分辨率，***参数β用于减少所分割部分对激光雷达到物体距离的依赖性，可以通过实际数据计算获取，***参数C₀用于调节激光雷达的纵向误差，可以通过经验值给定。

其中，激光雷达与点的距离通过激光雷达扫描获取的数据可以得到。

进一步地，在完成点数据的数据聚类后，采用以下方式进行特征提取，以确定激光反射板的位置信息：

假设上述数据聚类过程中获得如以下公式2表示的n个物体；

当物体的极径与激光反射板的实际半径在设定误差范围内时，认定相应的物体为激光反射板；

式中，E_seg表示物体集合，seg_k表示第k个物体，(x_k,y_k)表示第k个物体中心在全局坐标系下的位置，D_k表示第k个物体的极径，θ_k表示第k个物体的主轴方向。

本发明一实施例中，利用物体的极径特征进行特征提取，确定激光反射板对应的物体，从而确定激光反射板的位置信息。

S24，完成二维场景地图构建；

完成所有激光反射板和其他物体的识别定位后，即完成二维场景地图的构建，后续的机器人定位和导航均基于该步骤构建的二维场景地图。

S3，根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置。

参见图3，具体地，根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置，包括以下步骤：

S31，根据激光雷达的观测信息，确定激光反射板在局部坐标系下的观测信息；

在完成机器人工作空间对应的二维场景地图构建后，当机器人在工作空间内运动时，激光雷达会实时返回扫描数据信息，针对激光雷达获取的扫描数据信息，通过采用步骤2所述的数据聚类和特征提取对扫描数据信息进行数据处理后，能够得到激光反射板在局部坐标系下的观测信息，该观测信息包括激光反射板到激光雷达的距离和激光反射板的方位角，激光反射板的方位角表示激光反射板和激光雷达的连线与机器人前进方向所成夹角。

S32，利用角度匹配方式和/或距离匹配方式对激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息进行匹配；

为了实现机器人的定位计算，需要对激光反射板的观测信息和几何信息进行匹配，即，将激光雷达观测到的激光反射板(可能混有非激光反射板的干扰信号)与工作空间环境中的实际参考激光反射板逐一对应，提取出有效的激光反射板信息，然后才能进行机器人的定位计算。在实际应用中，一些激光反射板可能看不见或者与另外的激光反射板混淆，导致的结果可能产生错误的定位结果。为此，本发明一实施例中，通过角度匹配方式和/或距离匹配方式，将扫描得到的激光反射板在局部坐标系下的观测信息与激光反射板在全局坐标系下的实际几何信息一一匹配。

角度匹配方式的原理是将在全局坐标下的激光反射板的坐标值转换为方向角输入程序中作为“理论值”，当激光雷达扫描到一个激光反射板信号(也可能是干扰信号)后得到一个角度值，该角度值可以称为“观测值”，设定一个较小的误差范围，让观测值与对应得到的理论值进行比较，如果两者的差值在设定角度误差范围内，则认为观测得到的是激光反射板，将该观测值进行保存，否则认为观测到的是干扰信号而进行剔除。

具体地，利用角度匹配方式对激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息进行匹配，采用以下方式进行：

设定工作空间环境中有m个激光反射板，m个激光反射板在全局坐标下的坐标为l_i(x_i,y_i)，1≤i≤m，激光雷达观测到q个激光反射板，q个激光反射板的观测方位角为α_j，1≤j≤q；

根据机器人在前一时刻的位姿分别计算m个激光反射板与机器人前进方向的理论方位角，将q个激光反射板的观测方位角与m个激光反射板的理论方位角进行比较，当观测方位角与理论方位角的差值在设定角度误差范围内，表示当前观测方位角对应的激光反射板与当前理论方位角对应的激光反射板相匹配。

进行激光反射板匹配的目的要确定观测方位角α_j实际上是工作空间环境中的哪个参考反射板与机器人前进方向的夹角，由于激光雷达的观测周期很短，在此过程中机器人位移很小，因此可以借助机器人前一时刻位姿计算出参考激光反射板与机器人前进方向的理论方位角，若计算得到的第i个激光反射板的理论方位角与观测得到的第j个激光反射板的观测方位角在设定角度误差范围内时，就认定观测到的第j个激光反射板与第i个参考激光反射板相互匹配，从而得到一组有效的激光反射板坐标l_i(x_i,y_i)和方位角α_j。

进一步地，距离匹配方式是在满足静态位姿计算的前提条件下进行的，要求此时机器人是静止不动的。距离匹配方式的原理是激光雷达以一定频率进行激光扫描，测得从激光发出到激光雷达接收到激光从激光反射板反射回来的时间间隔，利用时间飞行原理(Time Of Flight，TOF)，计算出激光雷达与各个观测到的激光反射板之间的距离，生成一个观测激光反射板列表，与全局坐标系下的实际参考激光反射板进行匹配。

具体地，利用距离匹配方式对激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息进行匹配，采用以下方式进行：

设定工作空间环境中有m个激光反射板，m个激光反射板在全局坐标下的坐标为l_i(x_i,y_i)，1≤i≤m，激光雷达观测到q个激光反射板，q个激光反射板与激光雷达的距离为p_j，q个激光反射板的观测方位角为α_j，1≤j≤q；

根据q个激光反射板与激光雷达的距离和q个激光反射板的观测方位角，计算任意两个观测到的激光反射板间的绝对距离，根据m个激光反射板在全局坐标下的坐标计算m个激光反射板中任意两个激光反射板之间的真实距离，将绝对距离与真实距离进行比较，当绝对距离与真实距离的差值在设定距离误差范围内，表示当前绝对距离对应的两个激光反射板与当前真实距离对应的两个激光反射板相匹配。

参见图4，某一时刻激光雷达观测到的激光反射板位置如图4所示，XOY表示局部坐标系，M_j表示观测到的激光反射板。进行激光反射板匹配的目的要确定激光雷达观测到的激光反射板实际上是工作空间环境中的哪个参考激光反射板，由于机器人静止不动，相应的激光雷达也是静止不动，因此根据观测到的激光反射板与激光雷达的距离和激光反射板的观测方位角，可以应用余弦定理计算出任意两个观测到的激光反射板之间的绝对距离M_1,2,M_1,3,…,M_1,q,M_2,1,M_2,3,…,M_2,q,…,M_q,1,M_q,2,…,M_q,q-1。同时，由于工作空间环境中的所有激光反射板在全局坐标系下的坐标已知，根据激光反射板在全局坐标下的坐标能够计算出任意两个激光反射板之间的真实距离，将绝对距离与真实距离进行比较，当绝对距离与真实距离的差值在设定距离误差范围内，则表示当前绝对距离对应的两个激光反射板与当前真实距离对应的两个激光反射板相匹配，从而将观测到的激光反射板与全局坐标系下的激光反射板匹配，得到有效的激光反射板坐标l_i(x_i,y_i)、距离p_j和方位角α_j。

S33，利用完成匹配的激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息，采用基于残差的最小二乘迭代算法和/或多边定位算法确定机器人在二维场景地图下的位置。

在完成激光反射板的匹配后，利用匹配后的激光反射板在局部坐标系下的观测信息和激光反射板在全局坐标系下的几何信息，能够计算确定机器人在二维场景地图下的位置。

本发明一实施例中，在利用角度匹配方式进行匹配的基础上，采用基于残差的最小二乘迭代算法确定机器人在二维场景地图下的位置。

具体地，采用基于残差的最小二乘迭代算法确定机器人在二维场景地图下的位置包括：

选定一组激光反射板的坐标和方位角，建立选定的一组激光反射板坐标及方位角与激光雷达观测位置点的一组集合关系表达式；

利用泰勒级数展开略去高次项，构成观测值与理论值的残差方程组，使用最小二乘法求解出激光雷达的位置。

由于激光雷达安装在机器人上，确定了激光雷达的位置即确定了机器人的位置。

进一步地，本发明一实施例中，在利用距离匹配方式进行匹配的基础上，采用多边定位算法确定机器人在二维场景地图下的位置。

本发明一实施例中，多边定位算法包括三角定位算法和三边定位算法，在采用三角定位算法或三边定位算法进行机器人的定位时，必须保证机器人在移动过程中至少检测到三块激光反射板。

具体地，三角定位算法是通过测量激光反射板与机器人纵轴间的夹角来实现定位的。参见图5，测量得到机器人与激光反射板A和激光反射板B之间的夹角∠ARB，以及机器人与激光反射板B和激光反射板C之间的夹角∠BRC，利用外接圆的作法，作两个外接圆和外接圆。根据外接圆的性质：过不在同一直线上的三点可作一个圆且只有一个圆，则机器人一定位于两个圆的相交处，两个外接圆有两个交点，其中一个交点是激光反射板B，另一个交点R则为机器人的位置。

具体地，三边定位算法是通过测量全局坐标已知的激光反射板与机器人之间的距离来实现机器人定位的。参见图6，测量得到机器人到A、B和C三个激光反射板的距离分别为R_a、R_b和R_c。根据圆的定义：到定点距离等于定长的动点的轨迹，分别以位置点A、位置点B和位置点C为圆心，以R_a、R_b和R_c为半径作圆，则机器人的位置在三个所作圆的交点。由于采用三边定位算法相较于采用三角定位算法，所需计算量和观测量较少，实现起来更为容易，本发明一实施例中，优先使用三边定位算法。

如果当前机器人位置恰与若干个激光反射板之间的距离都相等，那么就可以根据激光反射板优化选取准则，得出的定位精度很高，但事实是机器人周围不总是有均匀分布的激光反射板，机器人也不总是在均匀分布的激光反射板中心。因此，机器人在移动过程中，如何根据周围环境合理的选取激光反射板使得定位精度最高成为必要，由于己经得到了精度标准，则对于三点定位选择激光反射板问题，可以找出所有有效识别点的全部三个激光反射板组合，根据给定的精度度量标准估计其精度，然后比较选择一组最优的组合。

S4，基于机器人在二维场景地图下的位置，对机器人的位姿进行预测估计。

由于机器人的激光定位过程是激光雷达每转一圈进行一次计算，是一个离散、不连续的过程，因此在利用定位算法明确了机器人在每一个定位计算点所处的精确位置后，需要对机器人在两个定位计算点之间运动时可能产生的误差提前进行修正，保证机器人沿着规划路径到达目标位置，而这就需要对机器人的位姿和环境地图的均值和方差进行估计。为此，本发明一实施例中，采用扩展卡尔曼滤波算法(EKF算法，Extended KalmanFilter)对机器人的位姿进行预测估计。

卡尔曼滤波是一种基于概率模型的状态和参数的估计，根据前一时刻的***状态，估计出当前的***状态，然后以当前时刻的实际观测值作为反馈，来修正估计的状态，卡尔曼滤波算法可分为两个步骤，包括状态预测和测量更新。

假设***离散状态空间模型为：

式中，x(k)表示k时刻***的状态变量，u(k)表示k时刻***的控制变量，z(k)表示k时刻传感器的测量变量，w(k)表示***的过程噪声，v(k)表示k时刻传感器的观测噪声，A和H分别表示***的状态转移矩阵和测量矩阵，B表示***的输入控制矩阵。

卡尔曼滤波通过状态更新方程及时地由当前***状态和输入变量估计出下一步的***状态(先验估计)，而测量更新则将新的测量信号加入已经在状态更新方程中得到的先验估计状态，并最终得到***状态的后验估计。

对于非线性***，其过程模型和观测模型如下所示：

/>

式中，X(k)、Z(k)和u(k)分别表示时刻***的状态、观测量和***的控制输入，w(k)和v(k)均表示高斯白噪声，f(X(k),u(k))表示运动方程，h(X(k))表示观测方程。

参见图3，利用EKF-SLAM算法可以对机器人的位姿和环境地图的均值和方差进行估计。在EKF-SLAM算法中，***噪声均被默认为服从高斯分布，因此利用***状态的均值和协方差对***的状态输出进行描述。在EKF-SLAM算法中，***状态包括机器人的位姿和己检测到的所有激光反射板的位置，根据机器人的运动状态方程，并结合机器人当前的位姿和控制变量的输入通过EKF滤波器对机器人的下一刻位姿进行预测估计。当激光雷达传感器检测到外部激光反射板时，根据传感器测量方程，结合机器人当前位姿、环境激光反射板以及传感器的测量信息采用EKF滤波器进行***状态的更新。

EKF-SLAM的具体实现过程可描述如下：将机器人的控制信号输入到***状态方程之中，利用EKF对***的状态进行递归估计，以测量误差为依据进行估计和校正，对增广的***状态向量和增广的***协方差矩阵进行更新替换，上述状态预测和观测更新交替进行，不断逼近***状态真实值，以完成EKF的估计过程。

本发明一实施例提供的基于几何信息的激光定位方法通过在需要进行精确定位的设定区域布置激光反射板作为反射标记物，能够实现机器人在设定区域的精确定位，提高机器人的控制精度，定位可靠性高，算法复杂度和计算量低。

需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。此外，本文中“前”、“后”、“左”、“右”、“上”、“下”均以附图中表示的放置状态为参照。

最后应说明的是：以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种基于几何信息的激光定位方法，其特征在于，所述方法用于实现机器人在工作空间内的定位，包括以下步骤：

S1，在机器人工作空间中的设定区域布置至少三个激光反射板；

S2，构建包括激光反射板的机器人工作空间对应的二维场景地图；

S3，根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置；

S4，基于机器人在二维场景地图下的位置，对机器人的位姿进行预测估计；

所述根据机器人上安装的激光雷达的观测信息和激光反射板的几何信息，利用定位算法确定机器人在二维场景地图下的位置，包括以下步骤：

S31，根据激光雷达的观测信息，确定激光反射板在设定局部坐标系下的观测信息；

S32，利用角度匹配方式对激光反射板在设定局部坐标系下的观测信息和激光反射板在设定全局坐标系下的几何信息进行匹配；

S33，根据完成匹配的激光反射板在设定局部坐标系下的观测信息和激光反射板在设定全局坐标系下的几何信息，采用基于残差的最小二乘迭代算法确定机器人在二维场景地图下的位置；

利用角度匹配方式对激光反射板在设定局部坐标系下的观测信息和激光反射板在设定全局坐标系下的几何信息进行匹配，采用以下方式进行：

设定工作空间环境中有m个激光反射板，m个激光反射板在全局坐标下的坐标为l_i(x_i,y_i)，1≤i≤m，激光雷达观测到q个激光反射板，q个激光反射板的观测方位角为α_j，1≤j≤q；

根据机器人在前一时刻的位姿分别计算m个激光反射板与机器人前进方向的理论方位角，将q个激光反射板的观测方位角与m个激光反射板的理论方位角进行比较，当观测方位角与理论方位角的差值在设定角度误差范围内，表示当前观测方位角对应的激光反射板与当前理论方位角对应的激光反射板相匹配；

采用基于残差的最小二乘迭代算法确定机器人在二维场景地图下的位置包括：

选定一组激光反射板的坐标和方位角，建立选定的一组激光反射板坐标及方位角与激光雷达观测位置点的一组集合关系表达式；

利用泰勒级数展开略去高次项，构成观测值与理论值的残差方程组，使用最小二乘法求解出激光雷达的位置。

2.根据权利要求1所述的基于几何信息的激光定位方法，其特征在于，所述构建包括激光反射板的机器人工作空间对应的二维场景地图，包括以下步骤：

S21，设定全局坐标系和局部坐标系；

S22，控制机器人运动，利用激光雷达扫描工作空间环境，获取一组机器人极坐标系下的点数据；

S23，对点数据进行数据聚类和特征提取，确定激光反射板的位置信息；

S24，完成二维场景地图构建。

3.根据权利要求2所述的基于几何信息的激光定位方法，其特征在于，全局坐标系和局部坐标下采用以下方式进行设定：

选取工作空间内的一个设定位置作为坐标原点o，选取一个设定方向作为x轴正方向，选取与设定方向垂直的方向作为y轴正方向，建立全局坐标系xoy；

选取机器人中心作为坐标原点O，选取机器人的前进方向作为X轴正方向，选取与机器人的前进方向垂直的方向作为Y轴正方向，建立局部坐标系XOY。

4.根据权利要求3所述的基于几何信息的激光定位方法，其特征在于，利用以下公式1对点数据进行数据聚类，以将点数据分成若干个点簇，一个点簇对应一个物体；

其中，r_k,k+1＝|r_k-r_k+1|表示两点的几何距离，r_k和r_k+1分别表示两点与激光雷达的距离，r_min＝min{r_k,r_k+1}，r_min表示点数据中所有的两点几何距离的最小值，C₀和β表示***参数，φ表示激光雷达角度分辨率。

5.根据权利要求4所述的基于几何信息的激光定位方法，其特征在于，采用以下方式进行特征提取，以确定激光反射板的位置信息：

设定数据聚类过程中获得如以下公式2表示的n个物体；

当物体的极径与激光反射板的实际半径在设定误差范围内时，认定相应的物体为激光反射板；

其中，E_seg表示物体集合，seg_k表示第k个物体，(x_k,y_k)表示第k个物体中心在全局坐标系下的位置，D_k表示第k个物体的极径，θ_k表示第k个物体的主轴方向。

6.根据权利要求1至5中任一项所述的基于几何信息的激光定位方法，其特征在于，采用扩展卡尔曼滤波算法对机器人的位姿进行预测估计。

Images