一种定位方法、装置、移动机器人及存储介质与流程

本发明实施方式涉及移动机器人技术领域，特别是涉及一种定位方法、装置、移动机器人及存储介质。

背景技术：

随着技术的发展和人们生活水平的提高，诸如清洁机器人、服务机器人、远程监控机器人、移动机器人等移动机器人逐渐进入人们的生活中。基于激光雷达的清扫机器人通常需要根据激光雷达的感知数据进行环境地图的构建，在建图过程需要实时估计机器人的位姿(位置和方向)。

常规的定位方法是通过里程计提供一个定位初始值，该定位初始值的准确性受里程计模型、打滑、噪声等因素的影响，通常与真实值存在误差，且方向误差大于位置误差，通过设定一个搜索范围和采样间隔，搜索范围包括位置范围和角度范围，从而确定候选位姿，此方式进行定位的精度与采样间隔的大小密切相关，采样间隔越小，定位精度越高，反之，越低。采样间隔越小，候选位姿数就越多，计算量就越大，不利于系统的实时性。由于里程计的方向误差较大，真正的方向可能会在搜索角度范围的边缘，甚至超出角度搜索范围，使匹配结果变差，得不到最好的位姿。

有鉴于此，现有技术亟待改进。

技术实现要素：

为了解决上述技术问题，本发明实施例提供一种定位方法、装置、移动机器人及存储介质，解决目前通过搜索范围和采样间隔定位存在计算量大的技术问题，减少计算量，并提高角度估计的准确性。

为了解决上述技术问题，本发明实施例提供以下技术方案：

第一方面，本发明实施例提供一种定位方法，应用于移动机器人，其特征在于，所述移动机器人包括一雷达传感器，所述方法包括：

获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；

判断所述映射点云是否包括第一直线数据段，

若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应，根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，并根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位；

若判断为否，则根据所述初始位姿生成第二候选位姿集，根据所述第二候选位姿集对所述移动机器人进行定位。

在一些实施例中，所述根据所述第一直线数据段和所述第二直线数据段确定角度差，包括：

所述第一直线数据段包括映射点集，对所述映射点集进行直线拟合，确定第一直线方程，根据所述第一直线方程确定所述第一直线数据段在预设地图中的第一直线角度；

在预设地图上查找与所述第一直线数据段的映射点集对应的障碍点集，对所述障碍点集进行直线拟合，确定第二直线方程，根据所述第二直线方程确定所述障碍点集在预设地图中的第二直线角度；

计算第一直线角度和第二直线角度的角度差。

在一些实施例中，所述根据所述第一直线数据段和所述第二直线数据段确定角度差，包括：

若识别到所述雷达数据中的多个第一直线数据段，则确定每一第一直线数据段对应的直线方程，确定多个第一直线角度；

计算多个第一直线角度与第二直线角度的多个角度差，对多个角度差求平均值，将平均值作为所述第一直线角度和第二直线角度的角度差。

在一些实施例中，所述基于所述角度差对所述移动机器人的初始位姿进行校正，包括：

确定所述移动机器人在预设地图中的初始位姿为(x，y，ψ)；

所述根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，包括：

所述角度差为δψ，校正后的初始位姿为(x，y，ψ+δψ)。

在一些实施例中，所述根据校正后的初始位姿生成第一候选位姿集，包括：

根据预设的第一搜索范围，生成第一候选位姿集，其中，所述第一搜索范围包括第一角度搜索范围和第一位置搜索范围，所述第一角度搜索范围和所述第一位置搜索范围根据校正后的初始位姿生成。

在一些实施例中，所述根据所述初始位姿生成第二候选位姿集，包括：

根据预设的第二搜索范围，生成第二候选位姿集，其中，所述第二搜索范围包括第二角度搜索范围、第二位置搜索范围，所述第二角度搜索范围大于所述第一角度搜索范围。

在一些实施例中，所述根据所述第一直线数据段在预设地图上查找第二直线数据段，包括：

根据所述初始位姿确定所述雷达传感器的中心在所述预设地图上的中心映射点；

所述第一直线数据段的点集包括多个映射点，确定由所述中心映射点出发朝所述多个映射点延伸的多条扫描射线；

在所述多条扫描射线方向上分别查找与所述多个映射点对应的多个障碍点，每一所述障碍点与相邻的映射点的距离满足预设距离范围，所述多个障碍点形成所述第二直线数据段。

在一些实施例中，在获取雷达传感器发送的雷达数据之后，所述方法还包括：

将所述雷达数据从极坐标系转换为雷达坐标系；

再将所述雷达数据从雷达坐标系转换为地图坐标系。

第二方面，本发明实施例提供一种定位装置，应用于移动机器人，所述移动机器人包括一雷达传感器，所述装置包括：

映射单元，用于获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；

判断单元，用于判断所述映射点云是否包括第一直线数据段，

位姿校正单元，用于根据所述判断单元的判断结果执行步骤:若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应，根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，并根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位；

若判断为否，则根据所述初始位姿生成第二候选位姿集，根据所述第二候选位姿集对所述移动机器人进行定位。

第三方面，本发明实施例提供一种移动机器人，包括：

至少一个处理器；和

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的定位方法。

第四方面，本发明实施例提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上所述的定位方法。

本发明实施方式的有益效果是：区别于现有技术的情况，本发明实施方式提供一种定位方法，应用于移动机器人，所述移动机器人包括一雷达传感器，所述方法包括：获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；判断所述映射点云是否包括第一直线数据段，若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应，根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，并根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位。从而，一方面，通过识别映射点云中的第一直线数据段，能够得到移动机器人在场景中的角度，从而减小搜索范围，以减少计算量，另一方面，通过直线拟合，计算第一直线数据段和第二直线数据段的角度差，从而对初始位姿进行校正，能够提高定位的准确性。

附图说明

一个或多个实施例通过与之对应的附图进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1是本发明实施例提供的一种移动机器人的结构示意图；

图2是本发明实施例提供的一种定位方法的流程示意图；

图3是图2中的步骤s30的细化流程图；

图4是图2中的步骤s40的细化流程图；

图5是本发明实施例提供的一种障碍点的示意图；

图6是图2中的步骤s60的细化流程图；

图7是本发明实施例提供的一种定位装置的结构示意图。

具体实施方式

为了便于理解本发明，下面结合附图和具体实施方式，对本发明进行更详细的说明。需要说明的是，当元件被表述“固定于”另一个元件，它可以直接在另一个元件上、或者其间可以存在一个或多个居中的元件。当一个元件被表述“连接”另一个元件，它可以是直接连接到另一个元件、或者其间可以存在一个或多个居中的元件。本说明书所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。

除非另有定义，本说明书所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本说明书中在本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是用于限制本发明。本说明书所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

基于激光雷达的清扫机器人通常需要根据激光雷达的感知数据进行环境地图的构建，在建图过程需要实时估计机器人的位姿(位置和方向)。常规的定位方法通过里程计提供一个定位初始值，该定位初始值的准确性受里程计模型、打滑、噪声等因素的影响，通常与真实值存在误差，且方向误差大于位置误差，设定一个搜索范围和采样间隔，搜索范围包括位置范围(如：x,y∈(-0.2m,0.2m),这样就生成了一个矩形搜索区域)和角度范围(如：angle∈(-20度,20度)),x和y的采样间隔可设置为0.01，角度采样间隔可设置为0.5°，那么就可以生成候选位姿数为：(0.2*2/0.025)×(0.2*2/0.025)×(20*2/0.5)＝16*16*80，将雷达数据根据候选位姿投影到地图上，所有映射点的概率值的和即为该位姿的匹配度，将匹配度最高的位姿作为移动机的定位位姿。

该方法中，定位的精度与采样间隔的大小密切相关，采样间隔越小，定位精度越高，反之，越低。采样间隔越小，候选位姿数就越多，计算量就越大，不利于系统的实时性。想要同时追求精度和效率不可行。另外，角度间隔不能太大，因为当雷达数据的距离值较大时，映射的位置就会相差很大。

由于里程计的方向误差较大，真正的方向可能会在搜索角度范围的边缘，甚至超出角度搜索范围，使匹配结果变差，得不到最好的位姿。

而通过对家居环境的观察，可以发现，家居环境场景中直线信息比较丰富且数据稳定，本发明通过识别环境中的直线信息，计算得到移动机在场景中的角度，以此减小监督搜索范围，可以提高角度估计的准确性，也可减少计算量。

下面结合附图对本发明进行详细说明。

请参阅图1，图1是本发明实施例提供的一种移动机器人的结构示意图；

如图1所示，移动机器人10可以包括设备主体，安装设置于所述设备主体上的若干信息采集装置11以及与所述信息采集装置连接的控制器12。

设备主体是移动机器人10的主体结构，提供基础的结构框架并用以容纳多个不同的功能模组来组成完整的移动机器人10。

信息采集装置11是移动机器人10感知外界信息并与外部世界进行交互的设备。这些信息采集装置可以根据实际的需要，选择合适的类型并设置相应的数量。

具体的，请继续参阅图1，所述信息采集装置至少可以包括：导航装置11d。

导航装置11d是为机器人提供定位和地图构建服务，实时提供移动机器人当前位置信息的功能模块。其具体可以采用任何类型的结构和功能模块来实现。例如基于slam(simultaneouslocalizationandmapping，即时定位与地图构建)，以设定尺寸的范围不断的进行探索，在行走的过程中建立整个待清洁环境的地图。

在其他实施方式中，所述信息采集装置包括：触发装置11a、动作采集装置11b和深度传感器11c。其中，触发装置11a是用于采集触发指令，以启动物体标定流程，进入视教模式的交互设备。动作采集装置11b则用于识别动作指令，解析视教者肢体动作的指令含义。深度传感器11c是采集环境和物体与机器人之间的距离(亦即深度信息)的设备。其可以采集立体结构表面的三维坐标信息从而令机器人可以构建一个三维立体空间。

控制器12是整个移动机器人的控制核心，可以接收外界信息并调用相应的计算机软件程序执行一个或者多个逻辑处理步骤，例如完成物体标定的过程。该控制器具体可以采用任何类型的结构而实现，包括但不限于微处理器等。

该控制器12可以包括：处理器、存储器以及通信模块，本发明实施例以总线连接为例，建立所述处理器、存储器以及通信模块任意两者之间的通信连接。

处理器是移动机器人的控制中心，利用各种接口和线路连接整个移动机器人的各个部分，通过运行或执行存储在存储器内的软件程序和/或模块，以及调用存储在存储器内的数据，执行移动机器人的各种功能和处理数据，从而对移动机器人进行整体监控。处理器可包括一个或多个处理单元；优选的，处理器可集成应用处理器和调制解调处理器，其中，应用处理器主要处理操作系统、用户界面和应用程序等，调制解调处理器主要处理无线通信。可以理解的是，上述调制解调处理器也可以不集成到处理器中。

移动机器人10还可以包括给各个部件供电的电源(比如电池)，优选的，电源可以通过电源管理系统与处理器逻辑相连，从而通过电源管理系统实现管理充电、放电、以及功耗管理等功能。

在设备主体上安装有相应的行走机构14以及清扫机构15。本领域技术人员根据机器人的实际应用场景的不同，还可以增设或者减少其它的一个或者多个不同的功能模块。

其中，行走机构13是为扫地机器人提供移动能力，带动扫地机器人沿设定的路线移动的结构装置。该行走机构具体可以采用任何类型的移动装置实现，例如滚轮、履带式等。

清扫机构14是用于实现地面清扫任务的一个或者多个设备，包括但不限于储水箱、拖布、v型滚刷和储尘箱等。清扫机构可以具有可拆卸的特性，针对不同的清洁工作任务，更换使用不同的清扫机构对特定面积的区域执行清扫工作。

移动机器人10还包括雷达传感器15，所述雷达传感器15包括一个激光发射口和一个激光接收口，所述激光发射口用于发射激光，所述激光接收口用于接收雷达数据。在本发明实施例中，所述雷达传感器为激光雷达传感器。

另外，移动机器人10包括一些未示出的功能模块，例如里程计，在此不再赘述。

优选的，本发明实施例还提供一种移动机器人，包括处理器，存储器，存储在存储器上并可在所述处理器上运行的计算机程序，该计算机程序被处理器执行时实现上述电池检测方法实施例的各个过程，且能达到相同的技术效果，为避免重复，这里不再赘述。

本发明实施例的移动机器人10以多种形式存在，包括但不限于：清洁机器人、服务机器人、远程监控机器人、移动机器人等移动机器人等。在一些实施例中，移动机器人10的数量可以为一个，亦可以为多个。

请参阅图2，图2是本发明实施例提供的一种定位方法的流程示意图；

如图2所示，该定位方法，应用于移动机器人，所述移动机器人包括一雷达传感器，该定位方法的执行主体为所述移动机器人的处理器，所述方法包括步骤s10至步骤s70：

步骤s10：获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；

具体的，所述移动机器人包括里程计，所述里程计可以提供所述移动机器人在预设地图中的初始位姿数据(该值的准确性受里程计模型、打滑、噪声等因素的影响，通常与真实值存在误差，且方向误差大于位置误差，因此将初始位姿进行校正后才能得到比较准确的初始位姿值)，其中，所述初始位姿包括所述移动机器人在预设地图中的位置和方向。本实施方式中，所述预设地图可以是栅格概率地图，在栅格概率地图中，概率值越大的位置，存在障碍物的概率越大。所述移动机器人可以通过所述雷达传感器测量障碍的距离数据构建所述栅格概率地图。在其他实施方式中，所述预设地图也可以为其他类型的地图，比如拓扑地图等。

所述雷达传感器可以是机械激光雷达或固态激光雷达，所述雷达传感器包括一个激光发射口和一个接收口，进行一次发射和接收得到一个数据，雷达旋转360度，若其角度步长为1度，则可得到360个数据，当然，上述角度步长可以根据实际需要进行设置，在此不作限定。

所述雷达数据在所述初始位姿下获得，所述雷达数据是基于以雷达传感器为中心的极坐标系的坐标数据。通过根据所述初始位姿将所述雷达数据映射到预设地图上，实际上是将雷达数据进行坐标转换，可以得到所述雷达数据在地图坐标系上的映射数据。

具体的，所述根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云，包括：

将所述雷达数据从极坐标系转换为雷达坐标系；

再将所述雷达数据从雷达坐标系转换为地图坐标系。

可以理解的是，雷达采集的数据是基于雷达坐标系的，每个数据采用极坐标表示，而地图是2d的，首先将每个相对于雷达坐标系的数据转换成欧式坐标，即(x,y)，在根据雷达坐标系在地图坐标系下的相对位姿，将每个雷达坐标转换到地图坐标系下的位置，该过程即为投影。

雷达坐标系在地图坐标系下的相对位姿＝雷达坐标系在机器人坐标系下的位姿×机器人坐标系在地图坐标系中的位姿。雷达坐标系在机器人坐标系下的位姿是移动机在设计时的结构定义的，固定不变，而机器人坐标系在地图坐标系中的位姿就是机器人在地图中的定位结果，随移动机运动而实时变化，候选位姿就是假设的机器人坐标系在地图坐标系中的位姿。

具体的，所述雷达坐标系为2d笛卡尔坐标系，将所述雷达数据从极坐标系转换为雷达坐标系，通过以下公式(1)完成：

其中，ρ为雷达传感器在θ角度处的探测距离。

具体的，将所述雷达数据从雷达坐标系转换为地图坐标系，通过以下公式(2)完成：

其中，里程计估计的初始位姿为(xodom,yodom,ψodom)，[x,y]为雷达坐标系下的2d坐标，x^m，y^m为地图坐标系下的2d坐标。

该步骤s20：判断所述映射点云是否包括第一直线数据段，

在步骤s20中，所述映射点云包括依次排列的多个映射点，所述多个映射点用于表征障碍物的轮廓，按照探测顺序将多个映射点划分为多个映射点组，每组映射点包括至少两个映射点，对所述多个映射点组分别进行直线拟合得到多个线段，依次计算上述相邻的两个线段的夹角，若判断上述夹角小于或等于预设角度阈值，则判定相邻的两个映射点组位于同一直线上，从而可以识别所述映射点云中的一个或多个第一直线数据段。若上述夹角大于预设角度阈值，此时截断数据，即下一个数据作为起点重新开始统计。所述预设角度阈值可以根据需要人为设置，例如：设置为±10度。

其中，所述雷达传感器包括一个激光发射口和一个接收口，进行一次发射和接收得到一个数据，雷达旋转360度，若其角度步长为1度，则可得到360个数据。旋转一圈后，将探测的360个数据发送出来，数据保存在一个数组中，数据的索引号就是探测顺序。根据所述雷达数据的探测顺序，可以确定对应的映射点云中的每个映射点的探测顺序。

可以理解的是，通过计算相邻雷达数据的角度差，可以得到一个或多个第一直线数据段，为了提高直线角度计算的准确性，通过设置一数量阈值，使得所述第一直线数据段的数量大于所述数量阈值。若所述第一直线数据段的数量大于所述数量阈值，则可以通过求平均值的方式计算第一直线角度。

步骤s30：若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应；

请参阅图3，图3是图2中的步骤s30的细化流程图；

如图3所示，在步骤s30中，所述根据所述第一直线数据段在预设地图上查找第二直线数据段，包括步骤s31至步骤s33：

步骤s31：根据所述初始位姿确定所述雷达传感器的中心在所述预设地图上的中心映射点。

步骤s32：所述第一直线数据段的点集包括多个映射点，确定由所述中心映射点出发朝所述多个映射点延伸的多条扫描射线。

步骤s33：在所述多条扫描射线方向上分别查找与所述多个映射点对应的多个障碍点，每一所述障碍点与相邻的映射点的距离满足预设距离范围。所述多个障碍点形成所述第二直线数据段。

具体的，所述栅格概率地图中概率值越大的位置，存在障碍物的概率越大。假如有一面墙，真正属于墙的位置，概率值较大，而墙的两侧概率值会逐渐降低。通过在所述多条扫描射线方向上分别查找与所述多个映射点对应的多个障碍点，具体的，在数据映射点附近，从近距离到远距离查找第一个概率峰值点作为该雷达点在地图中的障碍点，从而可以在所述栅格概率地图上查找到与所述多个映射点相对应的多个障碍点，所述多个障碍点可以表征所述第一直线数据段所对应的障碍物轮廓的真实位置，从而可以根据所述所述多个障碍点形成所述第二直线数据段，所述第二直线数据段和所述第一直线数据段的角度差可以作为校正初始位姿的可靠依据。

步骤s40：根据所述第一直线数据段和所述第二直线数据段确定角度差；

请再参阅图4，图4是图2中的步骤s40的细化流程图；

在步骤s40中，所述根据所述第一直线数据段和所述第二直线数据段确定角度差，包括步骤s41至步骤s44：

步骤s41：所述第一直线数据段包括映射点集，对所述映射点集进行直线拟合，确定第一直线方程，根据所述第一直线方程确定所述第一直线数据段在预设地图中的第一直线角度；

步骤s42：在预设地图上查找与所述第一直线数据段的映射点集对应的障碍点集，

步骤s43：对所述障碍点集进行直线拟合，确定第二直线方程，根据所述第二直线方程确定所述障碍点集在预设地图中的第二直线角度；

步骤s44：计算第一直线角度和第二直线角度的角度差。

在步骤s41中，所述映射点集由所述第一直线数据段上的多个映射点组成。通过最小二乘法拟合第一直线数据段的多个映射点，得到第一直线数据段的直线方程，以确定第一直线数据段在预设地图中的直线角度。通过拟合第一直线数据段的数据，即第一直线数据段的多个映射点，生成第一直线方程，例如方程y＝ax+b，从而确定每一直线方程的直线角度arctan(a)。

可以理解的是，若识别到所述映射点云中的多个第一直线数据段，则确定每一第一直线数据段对应的第一直线方程，确定多个第一直线角度。在步骤s44中，计算多个第一直线角度与第二直线角度的多个角度差，对多个角度差求平均值，将平均值作为所述第一直线角度和第二直线角度的角度差。

在步骤s42中，所述障碍点集由与所述第一直线数据段的多个映射点对应的多个障碍点组成。

在步骤s43中，通过最小二乘法拟合第二直线数据段的多个障碍点，得到第二直线数据段的直线方程，以确定第二直线数据段在预设地图中的直线角度。通过拟合第二直线数据段的数据，即第二直线数据段的多个障碍点，生成第二直线方程，例如方程y＝ax+b，从而确定每二直线方程的直线角度arctan(a)。

具体的，所述障碍点集为栅格概率地图中的障碍物，由于栅格概率地图的复杂性，所述障碍点集中的障碍点存在错误或定位偏差的现象，因此，通过基于ransac的最小二乘法进行直线拟合，剔除外点，得到更准确的直线角度。

在步骤s44中，所述计算第一直线角度和第二直线角度的角度差，包括：

若识别到所述雷达数据中的多个第一直线数据段，则确定每一第一直线数据段对应的直线方程，确定多个第一直线角度；

计算多个第一直线角度与第二直线角度的多个角度差，对多个角度差求平均值，将平均值作为所述第一直线角度和第二直线角度的角度差，例如：所述角度差δψ＝对应直线角度-映射直线角度，校正后的位姿为：(xodom,yodom,ψodom+δψ)。在本发明实施例中，所述移动机器人包括一里程计，所述初始位姿由里程计获取。

通过计算多个第一直线数据段的多个第一直线角度的平均值，本发明能够减少误差。

步骤s50：基于所述角度差对所述移动机器人的初始位姿进行校正；

在步骤s50中，所述基于所述角度差对所述移动机器人的初始位姿进行校正，包括：

确定所述移动机器人在预设地图中的初始位姿为(x，y，ψ)；

所述根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，包括：

所述角度差为δψ，校正后的初始位姿为(x，y，ψ+δψ)。

相对于校正前的初始位姿，校正后的初始位姿的角度更加准确，后续设置搜索窗口(包括角度搜索范围和位置搜索范围)进行位姿匹配时，可以将角度搜索范围可设置较小(甚至是不进行角度搜索)，且在真实值附近搜索也可提高匹配的准确性。

请参阅图5，图5是本发明实施例提供的一种障碍点的示意图；

如图5所示，设置障碍点搜索范围，通过在雷达中心与数据映射点形成的扫描射线上查找所述栅格概率地图中的障碍点，在数据映射点附近，从近距离到远距离查找第一个概率峰值点作为该雷达点在地图中的障碍点。

步骤s60：根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位。

在步骤s60中，所述根据校正后的初始位姿生成第一候选位姿集，包括：

根据预设的第一搜索范围，生成第一候选位姿集，其中，所述第一搜索范围包括第一角度搜索范围和第一位置搜索范围，所述第一角度搜索范围和所述第一位置搜索范围根据校正后的初始位姿生成。

具体的，所述第一搜索范围包括第一角度搜索范围以及第一位置搜索范围，根据所述第一角度搜索范围以及第一位置搜索范围，可以确定第一候选位姿集，具体的，根据位置采样间隔和角度采样间隔，确定候选位姿数，生成第一候选位姿集，所述第一候选位姿集的数量＝(第一位置搜索范围的长度/位置采样间隔)*(第一角度搜索范围/角度采样间隔)。

可以理解的是，所述第一角度搜索范围和所述第一位置搜索范围根据校正后的初始位姿生成，由于已经基于第一直线角度和第二直线角度的角度差，对移动机器人的初始位姿进行校正，因此，所述第一角度搜索范围可以设置较小，或者，可以免除搜索过程。

请参阅图6，图6是图2中的步骤s20的细化流程图；

在步骤s60中，根据所述第一候选位姿集对所述移动机器人进行定位，包括步骤61至步骤62：

步骤s61：基于所述第一候选位姿集，计算所述第一候选位姿集中的每一候选位姿的匹配度；

具体的，确定每一候选位姿在栅格概率地图上对应的每一障碍点的概率值，计算全部障碍点的概率值之和，将所述全部障碍点的概率值之和确定为该候选位姿的匹配度。

可以理解的是，所述栅格概率地图为2d概率地图，类似于一个图像，每个像素的概率值表征了该处存在障碍物的概率，每插入一个雷达数据时，就会改变对应像素的概率，若某个像素被雷达数据映射到的次数越多，其概率值越高，反之，没被映射到的概率值就会更低。

步骤s62：确定匹配度最高的候选位姿为所述移动机器人的当前位姿。

具体的，确定所述候选位姿集中的每一候选位姿的匹配度，将匹配度最高的候选位姿确定为所述移动机器人的当前位姿。

步骤s70：若判断为否，则根据所述初始位姿生成第二候选位姿集，根据所述第二候选位姿集对所述移动机器人进行定位。

在步骤s70中，所述根据所述初始位姿生成第二候选位姿集，包括：

根据预设的第二搜索范围，生成第二候选位姿集，其中，所述第二搜索范围包括第二角度搜索范围、第二位置搜索范围，所述第二角度搜索范围大于所述第一角度搜索范围。

可以理解的是，当没有识别到所述雷达数据中的第一直线数据段时，代表搜索范围较小，此时需要扩大搜索范围，具体的，通过预设的第二搜索范围，其中，所述第二搜索范围包括第二角度搜索范围、第二位置搜索范围，所述第二角度搜索范围大于所述第一角度搜索范围，所述第二位置搜索范围等于所述第一位置搜索范围。

通过所述第二候选位姿集，确定所述移动机器人的当前位姿，其处理方式与第一候选位姿集的处理方式相似，在此不再赘述。

一方面，通过识别映射点云中的第一直线数据段，能够得到移动机器人在场景中的角度，从而减小搜索范围，以减少计算量，另一方面，通过直线拟合，计算第一直线数据段和第二直线数据段的角度差，从而对初始位姿进行校正，能够提高定位的准确性，其中，所述第一直线数据段表征的是雷达传感器测量到的障碍物轮廓信息和位置信息，存在测量误差；所述第二直线数据段表征的是与所述第一直线数据段对应的障碍物的真实轮廓信息和真实位置信息。通过计算第一直线数据段和第二直线数据段的角度差，可以提高校正初始位姿的方向。

在本发明实施例中，通过提供一种定位方法，应用于移动机器人，所述移动机器人包括一雷达传感器，所述方法包括：获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；判断所述映射点云是否包括第一直线数据段，若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应，根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，并根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位。从而，一方面，通过识别映射点云中的第一直线数据段，能够得到移动机器人在场景中的角度，从而减小搜索范围，以减少计算量，另一方面，通过直线拟合，计算第一直线数据段和第二直线数据段的角度差，从而对初始位姿进行校正，能够提高定位的准确性。

请再参阅图7，图7是本发明实施例提供的一种定位装置的结构示意图；

如图7所示，该定位装置70，应用于移动机器人，所述移动机器人包括一雷达传感器，包括：

映射单元71，用于获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；

判断单元72，用于判断所述映射点云是否包括第一直线数据段，

位姿校正单元73，用于根据所述判断单元的判断结果执行步骤:若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应，根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，并根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位；

若判断为否，则根据所述初始位姿生成第二候选位姿集，根据所述第二候选位姿集对所述移动机器人进行定位。

在本发明实施例中，所述位姿校正单元，具体用于：

所述第一直线数据段包括映射点集，对所述映射点集进行直线拟合，确定第一直线方程，根据所述第一直线方程确定所述第一直线数据段在预设地图中的第一直线角度；

在预设地图上查找与所述第一直线数据段的映射点集对应的障碍点集，对所述障碍点集进行直线拟合，确定第二直线方程，根据所述第二直线方程确定所述障碍点集在预设地图中的第二直线角度；

计算第一直线角度和第二直线角度的角度差。

在本发明实施例中，所述位姿校正单元，具体还用于：

若识别到所述雷达数据中的多个第一直线数据段，则确定每一第一直线数据段对应的直线方程，确定多个第一直线角度；

计算多个第一直线角度与第二直线角度的多个角度差，对多个角度差求平均值，将平均值作为所述第一直线角度和第二直线角度的角度差。

在本发明实施例中，所述位姿校正单元，具体还用于：

确定所述移动机器人在预设地图中的初始位姿为(x，y，ψ)；

所述根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，包括：

所述角度差为δψ，校正后的初始位姿为(x，y，ψ+δψ)。

在本发明实施例中，通过提供一种定位装置，应用于移动机器人，所述移动机器人包括一雷达传感器，所述装置包括：映射单元，用于获取所述移动机器人在预设地图中的初始位姿，以及雷达传感器所采集的雷达数据，根据所述初始位姿将所述雷达数据映射到预设地图上，生成映射点云；判断单元，用于判断所述映射点云是否包括第一直线数据段，位姿校正单元，用于根据所述判断单元的判断结果执行步骤:若判断为是，则根据所述第一直线数据段在预设地图上查找第二直线数据段，所述第二直线数据段的点集与所述第一直线数据段的点集在预设地图上相对应，根据所述第一直线数据段和所述第二直线数据段确定角度差，基于所述角度差对所述移动机器人的初始位姿进行校正，并根据校正后的初始位姿生成第一候选位姿集，根据所述第一候选位姿集对所述移动机器人进行定位；若判断为否，则根据所述初始位姿生成第二候选位姿集，根据所述第二候选位姿集对所述移动机器人进行定位。

本发明实施例提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上所述的定位方法。

上述计算机可读存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(sram)，电可擦除可编程只读存储器(eeprom)，可擦除可编程只读存储器(eprom)，可编程只读存储器(prom)，只读存储器(rom)，磁存储器，快闪存储器，磁盘或光盘。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

需要说明的是，本发明的说明书及其附图中给出了本发明的较佳的实施方式，但是，本发明可以通过许多不同的形式来实现，并不限于本说明书所描述的实施方式，这些实施方式不作为对本发明内容的额外限制，提供这些实施方式的目的是使对本发明的公开内容的理解更加透彻全面。并且，上述各技术特征继续相互组合，形成未在上面列举的各种实施方式，均视为本发明说明书记载的范围；进一步地，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。