[发明专利]对激光雷达的测量端点去畸变的方法和扫地机

说明书

本发明涉及一种对激光雷达的测量端点去畸变的方法和扫地机，其中，所述激光雷达可旋转地固定在可运动的机器人上，该方法包括：建立坐标系，其包括建立世界坐标系w系、机器人本体坐标系b系以及激光雷达坐标系L系；将所述L系下的一扫描帧中的测量端点转换到所述b系，得到所述测量端点在所述b系下的位姿；将所述测量端点在所述b系下的位姿转换到所述w系，得到所述测量端点在所述w系下的位姿；将所述测量端点在所述w系下的位姿回转到同一个时刻的所述b系，得到去畸变后的所述测量端点在所述b系下的坐标；以及将去畸变后的所述测量端点在所述b系下的坐标回转到所述L系。

技术领域

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

背景技术

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

扫地机在清扫过程中，其激光雷达(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系的杆臂值。