[发明专利]一种定位方法和装置

权利要求书

1.一种定位方法，其特征在于，所述定位方法包括：

获取初始点云数据，其中，所述初始点云数据包括预设个数的采样点的采样信息，所述采样信息包括所述采样点的位置坐标和方向角；

计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；

按照预设数量的步长变化量修改所述初始点云数据，得到所述预设数量的初始参考点云数据，并计算每个所述初始参考点云数据与所述历史点云数据之间的点云匹配度，其中，所述步长变化量包括位置坐标变化量和方向角变化量；

根据所述初始点云数据的点云匹配度和每个所述初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；

按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量的第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度；

根据所述第一点云数据的点云匹配度和每个所述第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；

若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息，其中，所述定位信息包括机器人的位置和姿态。

2.根据权利要求1所述的定位方法，其特征在于，所述获取初始点云数据包括：

采集每个所述采样点的里程计信息和激光传感器信息；

根据所述里程计信息计算每个所述采样点的所述位置坐标和所述方向角；

将所述位置坐标和所述方向角与所述激光传感器信息关联，生成所述初始点云数据。

3.根据权利要求1所述的定位方法，其特征在于，所述计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度包括：

按照点云匹配条件，将所述初始点云数据与所述预设时间段内的历史点云数据进行匹配，得到满足所述点云匹配条件的历史点云数据，其中，所述点云匹配条件包括所述初始点云数据中的每个所述采样点与所述历史点云数据中的对应采样点之间的距离的平方和最小；

计算所述初始点云数据与所述满足所述点云匹配条件的历史点云数据之间的角度偏差和距离偏差，其中，所述距离偏差包括所述初始点云数据中的每个所述采样点与所述满足所述点云匹配条件的历史点云数据中的对应采样点之间的距离误差；所述角度偏差是初始点云数据和历史点云数据之间的中心角度偏差，或者是每个对应采样点之间的角度偏差的综合；

按照如下公式计算所述初始点云数据与所述满足所述点云匹配条件的历史点云数据之间的点云匹配度：

其中，S为所述点云匹配度，α为预设的可信度参数，θ为所述角度偏差，β为预设的匹配度参数，M_i为所述距离误差小于预设的误差阈值的所述采样点中第i个采样点对应的距离误差，N为所述距离误差小于预设的误差阈值的所述采样点的个数。

4.根据权利要求1至3任一项所述的定位方法，其特征在于，所述若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息之后，所述定位方法还包括：

若所述第一点云数据的点云匹配度小于所述第二点云数据的点云匹配度，则获取当前的优化次数；

若所述优化次数未达到预设的次数阈值，则将所述第二点云数据识别为所述第一点云数据，执行所述按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度。

5.根据权利要求4所述的定位方法，其特征在于，所述若所述第一点云数据的点云匹配度小于所述第二点云数据的点云匹配度，则获取当前的优化次数之后，所述定位方法还包括：

若所述优化次数达到所述次数阈值，则根据所述第二点云数据输出所述定位信息。