[发明专利]一种基于Apriltag标签与因子图的SLAM方法

说明书

本发明提供了一种基于Apriltag标签与因子图的SLAM方法，包括：首先读取预设数据并构建坐标系；以预设数据为因子，初始化因子图；使用相机采集标签图像；利用相机标定畸变参数对标签图像进行预处理；提取Apriltag标签角点与ID；以Apriltag标签角点的图像坐标值为映射因子更新因子图；判断是否提供相机载体的里程计(Odom)；采集载体里程计数据；以载体里程计数据为相对位姿因子更新因子图；优化因子图，计算物体，Apriltag标签，载体，相机的位姿，最后更新地图。利用本发明能够提供高精度的载体位姿数据，并构建高精度的地图信息。

技术领域

本发明属于机器人自主导航技术领域，涉及一种基于Apriltag标签与因子图的机器人同步定位与制图(SLAM)方法。

背景技术

SLAM技术是机器人自主导航领域的关键与研究热点，是实现机器人环境感知的关键基础技术，在工业，物流，服务等领域有广泛的应用前景。Apriltag是一种视觉基准标定(fiducial marker)系统，提供标签(Tag)与相机之间空间位姿关系以及自身ID信息。目前基于标签定位技术大多采用PnP法实现，此类技术缺少整体优化与回环检测，位姿估算受测量误差影响大。

目前多数视觉SLAM技术直接从图像中提取自然特征点进行匹配来实现定位与制图。传统特征点提取存在数据量大，易丢失，匹配计算复杂，抗干扰能力差等缺点；同时，此类SLAM技术依赖于网格化生成算法来感知空间物体存在，易受点云噪声干扰。此类技术存在计算复杂，制图信息不完整的缺点。

发明内容

本发明的目的：本发明提供一种基于Apriltag标签(简称标签)与因子图的SLAM方法，以解决现有SLAM技术计算量大，空间物体感知能力差，构建地图不完整且误差大的问题；通过本发明公开的方法，利用人工设定的特征点(标签角点)，预先有限设定的空间数据以及因子图，能够实现载体的高精度定位，空间物体建模，以及获得完整精确的地图。

技术方案：本发明所述的基于Apriltag标签与因子图的SLAM方法包括：

步骤1，读取预设数据，构建世界(World)坐标系，物体(Object)坐标系，标签(Apriltag)坐标系，载体(Robot)坐标系，和相机(Camera)坐标系；

其中，所述的构建世界坐标系，物体坐标系，标签坐标系，载体坐标系，和相机坐标系的具体方法为：

步骤1.1，定义李代数位姿和李群sE(3)变换矩阵其中是平移(translation)向量，是旋转(rotation)向量；

其中，位姿ξ与变换矩阵T存在映射公式：

步骤1.2，如图2所示，以初始时刻某点的位置为原点构建世界坐标系；以物体某顶点为原点构建物体坐标系；以标签的第一个角点为原点，所在平面为X-O-Y平面构建标签坐标系；以载体中心为原点构建载体坐标系；以相机焦点为原点构建相机坐标系；设物体在世界坐标系下位姿为标签在物体坐标系下位姿为相机在载体坐标系下位姿为载体在时刻t在世界坐标系下位姿为标签在时刻t在相机坐标系下位姿为

为对应变换矩阵；

其中，所述的预设数据为人工测量数据和设定数据，其包括：

设定为物体在世界坐标系下，标签在物体坐标系下或相机在载体坐标系下的位姿测量值，对应变换矩阵∑为协方差矩阵；