[发明专利]即时定位与地图构建方法和设备

权利要求书

1.一种基于矢量的即时定位与地图构建方法，其用于未知环境中的3D建模与地图构建，包括如下步骤：

以一未知视角获取一帧点云数据；

将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；

合并接近的法向矢量以获得法向矢量列表；

获得已知环境地图的环境特征矢量表；

将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；

将未能在环境特征矢量表中找到相匹配的特征矢量的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；

利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及

根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

2.根据权利要求1所述的方法，其中如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。

3.根据权利要求1所述的方法，其中每一个法向矢量αⁱ由四个元素(αⁱ₀，αⁱ₁，αⁱ₂，αⁱ₃)构成，其中αⁱ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，αⁱ₀、αⁱ₁、αⁱ₂分别表示x、y、z坐标的归一化元素。

4.根据权利要求1所述的方法，其中基于最小均方误差进行所述匹配。

5.一种基于矢量的即时定位与地图构建设备，其用于未知环境中的3D建模与地图构建，包括：

点云数据获取单元，用于以一未知视角获取一帧点云数据；

法向矢量获取单元，用于将所获取的点云数据进行三角形网格化，以形成多个平面，并获得与多个平面中的每一个对应的法向矢量；

合并单元，用于合并接近的法向矢量以获得法向矢量列表；

存储单元，用于存储已知环境地图的环境特征矢量表；

环境特征矢量表获取单元，用于从所述存储单元获取所述环境特征矢量表；

匹配单元，用于将法向矢量列表与所述环境特征矢量表中的各矢量进行匹配；

更新单元，用于将未能匹配的法向矢量列表中的法向矢量添加到环境特征矢量表中以更新环境特征矢量表；

计算单元，用于利用相互匹配的矢量，计算旋转矩阵，进而计算位移；以及

3D定位与地图构建单元，用于根据旋转矩阵和位移的计算结果，完成当前帧与已知环境地图的合并。

6.根据权利要求5所述的设备，进一步包括：

初始化单元，用于如果所述环境特征矢量表不存在，则通过使用该帧的法向矢量表来初始化环境特征矢量表。

7.根据权利要求5所述的设备，其中每一个法向矢量αⁱ由四个元素(αⁱ₀，αⁱ₁，αⁱ₂，αⁱ₃)构成，其中αⁱ₃表示坐标原点到与法向矢量αⁱ垂直的平面的距离，αⁱ₀、αⁱ₁、αⁱ₂分别表示x、y、z坐标的归一化元素。

8.根据权利要求1所述的设备，其中所述匹配单元基于最小均方误差进行所述匹配。