对激光雷达的测量端点去畸变的方法和扫地机与流程

本发明涉及激光探测与感知技术领域，尤其涉及一种对激光雷达的测量端点去畸变的方法和扫地机。

背景技术：

随着生活的水平的提高，扫地机因为操作简单、使用方便，越来越多地走入了人们生活，和家庭办公联系在了一起，成为了小家电中重要的一员，深受欢迎。

扫地机在清扫过程中，其激光雷达(lds)的基座固定在扫地机本体上，在激光雷达的测量部件旋转时，扫地机本体(底盘)和激光雷达基座可能会同时旋转或同时静止。

在扫地机静止时，激光雷达基座和扫地机本体(底盘)都相对地面静止，激光雷达的测量部件相对地面旋转。此时，激光雷达测量的是当前场景的一个静态扫描帧，扫描帧的信息为当前场景的一个静态测量。激光雷达的测量部件相对激光雷达基座旋转的角度，即为激光雷达旋转部件相对地面旋转的角度，扫描帧的360度就是相对地面的360度。

但是，在扫地机旋转时，扫地机本体(底盘)和激光雷达基座可能会相对地面同时旋转且不同步，那么此时激光雷达测量的将是当前场景的一个动态扫描帧，即激光雷达的测量部件相对于激光雷达基座的旋转角度与相对于地面的旋转角度不同。激光雷达测量一周的角度相对于地面来说可能会多于一周或少于一周。具体地，每个激光雷达的测量端点对应一个同时刻的扫地机本体坐标系，所以对于一个扫描帧的数据来说，由于扫地机本体坐标系的旋转和平移导致一帧数据分别会投影到扫地机本体的不同坐标系下，而如果此时还认为是在同一个扫地机本体坐标系下组成一个扫描帧，则会发现扫描出来的环境形状产生了畸变，比如直线变成了弧线等情况。将产生畸变的扫描帧带入slam算法进行扫描匹配，会产生错误的位姿和地图。需要进行畸变补偿，以消除扫地机本体坐标系运动导致的扫描帧畸变。

技术实现要素：

为了解决上述技术问题，本发明提供了一种对激光雷达的测量端点去畸变的方法和扫地机。

根据本发明的一个方面，提供了一种对激光雷达的测量端点去畸变的方法，其中，所述激光雷达可旋转地固定在可运动的机器人上，所述方法包括：

建立坐标系，其包括建立世界坐标系w系、机器人本体坐标系b系以及激光雷达坐标系l系；

将所述l系下的一扫描帧中的测量端点转换到所述b系，得到所述测量端点在所述b系下的位姿；

将所述测量端点在所述b系下的位姿转换到所述w系，得到所述测量端点在所述w系下的位姿；

将所述测量端点在所述w系下的位姿回转到同一个时刻的所述b系，得到去畸变后的所述测量端点在所述b系下的坐标；以及

将去畸变后的所述测量端点在所述b系下的坐标回转到所述l系。

优选地，所述w系为地面惯性坐标系，所述b系为以所述机器人的中心为原点的直角坐标系，所述l系包括以所述激光雷达的中心为原点的直角坐标系和以所述激光雷达的中心为极点的极坐标系。

优选地，将所述l系下的一扫描帧中的测量端点转换到所述b系包括：

将所述l系下的一扫描帧中的测量端点的极坐标转换为所述l系下的直角坐标；以及

根据所述激光雷达的中心在所述b系的投影求出所述l系相对于所述b系的杆臂值。

优选地，将去畸变后的所述测量端点在所述b系下的坐标回转到所述l系包括：

将所述测量端点在所述b系下的坐标转换为所述测量端点在所述l系下的直角坐标；以及

将所述测量端点在所述l系下的直角坐标转换为所述l系下的极坐标。

优选地，所述激光雷达的中心在所述b系的投影位于所述b系的x轴的正轴上，且所述l系的x轴在所述b系的投影与所述b系的x轴重合。

优选地，所述扫描帧为所述激光雷达在所述l系下旋转360度的完整扫描帧。

优选地，所述机器人的运动包括平移和旋转。

优选地，所述机器人为扫地机。

根据本发明的另一个方面，提供了一种扫地机，其包括：

本体，其用于进行清扫；

激光雷达，其相对于所述本体旋转，扫描并获取扫描帧的测量端点；以及

处理器，其用于执行上述的对激光雷达的测量端点去畸变的方法。

优选地，所述处理器还用于：

将去畸变后的所述测量端点在所述l系下的极坐标带入到slam算法中以进行扫描匹配。

与现有技术相比，上述方案中的一个或多个实施例可以具有如下优点或有益效果：

应用本发明提供的对激光雷达的测量端点去畸变的方法，将一扫描帧中的不同时刻的多个测量端点从激光雷达坐标系转换到机器人本体坐标系，再转换到世界坐标系，再整体回转到同一个时刻的机器人本体坐标系，并进一步回转到激光雷达坐标系，从而消除了因扫地机本体(底盘)和地面相对旋转而导致的激光雷达的测量端点的畸变和扫描帧的畸变。在此基础上，本发明还提供了一种扫地机，其将去畸变后的激光雷达的测量端点带入到slam算法中以进行扫描匹配，消除了扫地机位姿和出图的运动畸变。

本发明的其它特征和优点将在随后的说明书中阐述，并且部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及说明书附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。

图1为根据本发明一实施例的对激光雷达的测量端点去畸变的方法的流程图。

图2为根据本发明一实施例的l系在b系上的投影图。

具体实施方式

以下将结合附图及实施例来详细说明本发明的实施方式，借此对本发明如何应用技术手段来解决技术问题，并达成技术效果的实现过程能充分理解并据以实施。需要说明的是，只要不构成冲突，本发明中的各个实施例以及各实施例中的各个特征可以相互结合，所形成的技术方案均在本发明的保护范围之内。

同时，在以下说明中，出于解释的目的而阐述了许多具体细节，以提供对本发明实施例的彻底理解。然而，对本领域的技术人员来说显而易见的是，本发明可以不用这里的具体细节或者所描述的特定方式来实施。

实施例一

为了解决现有技术中激光雷达测量的端点畸变的技术问题，本发明一实施例提供了一种对激光雷达的测量端点去畸变的方法。其中，激光雷达的测量端点也可称为激光点云数据。

该方法的构思如下：由于激光雷达测量的是当前环境的轮廓，所以随着扫地机的旋转，虽然当前环境的轮廓的不同区域的稠密性不同，但是其测量的都是真实的环境轮廓，因而激光雷达的测量端点在地面(世界坐标系)的投影不会产生畸变。本方法依次取一扫描帧中的激光雷达测量的端点数据逐个投影到地面(世界坐标系)，然后将这一扫描帧数据整体变换到同一个时刻的扫地机本体坐标系下，以得到去畸变的一帧数据，再将去畸变后的这一扫描帧数据在扫地机本体坐标系下的坐标回转到激光雷达坐标系，以解决激光雷达的测量端点畸变的问题。

在本发明一实施例中，激光雷达可旋转地固定在可运动的机器人上。在此，需要说明的是，该机器人优选是扫地机，但也可以是安装有激光雷达的其他类型的机器人，本发明不限于此。在本发明一实施例中，机器人的运动包括机器人的平移和旋转。

图1为根据本发明一实施例的对激光雷达的测量端点去畸变的方法的流程图。如图1所示，该方法包括：

步骤s10：建立坐标系，其包括建立世界坐标系w系、机器人本体坐标系b系以及激光雷达坐标系l系；

步骤s11：将所述l系下的一扫描帧中的测量端点转换到所述b系，得到所述测量端点在所述b系下的位姿；

步骤s12：将所述测量端点在所述b系下的位姿转换到所述w系，得到所述测量端点在所述w系下的位姿；

步骤s13：将所述测量端点在所述w系下的位姿回转到同一个时刻的所述b系，得到去畸变后的所述测量端点在所述b系下的坐标；

步骤s14：将去畸变后的所述测量端点在所述b系下的坐标回转到所述l系。

在步骤s10中，共建立了三个坐标系，即世界坐标系w系、机器人本体坐标系b系以及激光雷达坐标系l系。其中，世界坐标系w系指的是机器人所在的地面惯性坐标系，与地面保持相对静止。机器人本体坐标系b系指的是原点在机器人上且与机器人保持相对静止的坐标系，可理解为b系的运动与机器人的运动保持一致，而且b系随机器人的运动其存在相对于w系的牵连运动(三维空间运动)。

在本发明一实施例中，激光雷达坐标系l系包括以原点在激光雷达上的直角坐标系和极点在激光雷达上的极坐标系，其中，极坐标系的极点与直角坐标系的原点重合。此外，建立的l系与b系刚性固联，即l系下的直角坐标系的原点与b系的原点之间的距离始终保持不变。

优选地，在本发明一实施例中，w系为直角坐标系，b系为以机器人的中心为原点的直角坐标系，l系下的直角坐标系的原点和极坐标系的极点为激光雷达的中心。

图2为根据本发明一实施例的l系在b系上的投影图。如图2所示，b系的原点定义为ob，其x轴定义为xb，其y轴定义为yb；l系下的直角坐标系的原点和极坐标系的极点在b系上的投影为ol，直角坐标系的x轴定义为xl，直角坐标系的y轴定义为yl。

需要说明的是，为了方便计算，可将激光雷达的中心在b系的投影定位于b系的x轴的正轴上，且l系的x轴在b系的投影与b系的x轴重合，l系的y轴在b系的投影与b系的y轴平行。

在步骤s11中，首先，依次取l系下的包含多个测量端点的一个扫描帧，将l系下的一扫描帧中的测量端点的极坐标转换为l系下的直角坐标，具体可依据如下公式进行转换：

其中，i表示测量端点，一扫描帧可能有n个测量端点，则i＝0，1，…，n-1；为测量端点i在l系下的直角坐标；di为测量端点i在极坐标下的测距值，θi为测量端点i与xl的夹角，[dicos(θi)disin(θi)θi]分别为测量端点i在l系下的直角坐标。

优选地，在本发明一实施例中，上述的一个扫描帧为激光雷达在l系下旋转360度的完整扫描帧。

接下来，根据激光雷达的中心在b系的投影，求出l系相对于b系的杆臂值，具体可依据如下公式进行计算：

其中，为l系相对于b系的杆臂值，为激光雷达中心在b系的投影与b系xb轴的夹角，r为激光雷达中心在b系的投影与机器人中心之间的距离，和r根据激光雷达与机器人的相对位置关系确定。

接下来，根据测量端点i在l系下的直角坐标杆臂值和l系相对于b系的姿态矩阵得到测量端点i在b系下的位姿。具体可依据如下公式进行计算：

其中，为测量端点i在b系下的位姿，姿态矩阵

在步骤s12中，将测量端点i在b系下的位姿转换到w系，得到测量端点i在w系下的位姿。根据测量端点i在b系下的位姿、b系的原点在w系的坐标和b系相对于w系的姿态矩阵，得到测量端点i在w系下的位姿：

其中，为测量端点i在w系下的位姿，为b系的原点在w系的坐标，为b系相对于w系的姿态矩阵，为测量端点i在b系的位姿经过姿态矩阵转换到w系后的值；其中，和是已知的，当机器人为扫地机时，可由imu(inertialmeasurementunit，惯性测量单元)和里程计经过航位推算给出。

在步骤s13中，将测量端点i在w系下的位姿回转到同一个时刻的b系，得到去畸变后的测量端点在b系下的坐标。

举例来说，将测量端点i在w系下的位姿回转到k时刻的b系，能够得到去畸变后的测量端点i在k时刻b系下的坐标。根据上述公式4能够计算得到扫描帧在b系k时刻的原点在w系的坐标值，以及w系相对于k时刻的b系的姿态矩阵根据测量端点i在w系下的位姿、k时刻的b系的原点在w系的坐标、w系相对于k时刻的b系的姿态矩阵计算得到去畸变后的测量端点i在b系下的坐标。具体可依据如下公式进行计算：

其中，k表示某一时刻，其为扫描帧的[0，n-1]个测量端点中任意一个测量端点i被激光雷达测得的时刻，为所有测量端点i在k时刻的b系下的坐标，为k时刻的b系的原点在w系的坐标；其中，和是已知的，当机器人为扫地机时，可由里程计得到。

如前所述，测量端点和扫描帧畸变的原因是由于激光雷达的测量部件与机器人的相对转动，使得一扫描帧的多个测量端点分散到了不同的b系。因此，如果将一扫描帧的所有测量端点转换到一个b系就不会出现畸变。在本发明一实施例中，将一扫描帧的所有测量端点在w系下的位姿都回转到同一个时刻的b系，以消除测量端点和扫描帧的畸变。

下面举例说明k时刻的b系，例如假设一扫描帧有三个测量端点1、2、3，激光雷达测得这三个测量端点的时刻分别为t1、t2、t3，那么t1、t2、t3时刻对应的就是不同的b系，在本发明一实施例中，k时刻的b系指的是t1、t2、t3三个时刻其中一个时刻(例如t2时刻)对应的b系。

在步骤s14中，将去畸变后的测量端点i在b系下的坐标回转到l系，其包括：

首先，将去畸变后的测量端点i在b系下的坐标转换为测量端点i在l系下的直角坐标，具体可依据如下公式进行计算：

其中，为去畸变后的测量端点i在l系下的直角坐标，已知，其为b系相对于l系的姿态矩阵。

接下来，可依据如下公式将去畸变后的测量端点i在l系下的直角坐标转换为l系下的极坐标：

其中，di'为去畸变后的测量端点i在极坐标下的测距值，θi'为去畸变后的测量端点i与xl的夹角，为在直角坐标系下的横坐标，为在直角坐标系下的纵坐标。

综上所述，本发明实施例一提供的对激光雷达的测量端点去畸变的方法，能够消除因激光雷达与机器人的相对运动而导致的激光雷达的测量端点的畸变和扫描帧的畸变。

实施例二

在实施一的基础上，本发明实施例二提供了一种扫地机，其包括：

本体，其用于进行清扫；

激光雷达，其相对于所述本体旋转，扫描并获取扫描帧的测量端点；以及

处理器，其用于执行如实施例一的对激光雷达的测量端点去畸变的方法。

在本发明一实施例中，处理器还用于：将去畸变后的测量端点在l系下的极坐标带入到slam算法中以进行扫描匹配，以消除扫地机位姿和出图的运动畸变。

综上所述，本发明提供了一种对激光雷达的测量端点去畸变的方法，消除了因扫地机本体(底盘)和地面的相对运动而导致的激光雷达的测量端点的畸变和扫描帧的畸变。在此基础上，本发明还提供了一种扫地机，其将去畸变的激光雷达的测量端点带入到slam算法中以进行扫描匹配，消除了扫地机位姿和出图的运动畸变。

应该理解的是，本发明所公开的实施例不限于这里所公开的特定处理步骤或材料，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。

说明书中提到的“实施例”意指结合实施例描述的特定特征、或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语或“实施例”并不一定均指同一个实施例。

此外，所描述的特征或特性可以任何其他合适的方式结合到一个或多个实施例中。在上面的描述中，提供一些具体的细节，例如厚度、数量等，以提供对本发明的实施例的全面理解。然而，相关领域的技术人员将明白，本发明无需上述一个或多个具体的细节便可实现，或者也可采用其它方法、组件、材料等实现。

虽然上述示例用于说明本发明在一个或多个应用中的原理，但对于本领域的技术人员来说，在不背离本发明的原理和思想的情况下，明显可以在形式上、用法及实施的细节上作各种修改而不用付出创造性劳动。因此，本发明由所附的权利要求书来限定。