CN111242996B - 一种基于Apriltag标签与因子图的SLAM方法 - Google Patents

Abstract

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

Description

一种基于Apriltag标签与因子图的SLAM方法

技术领域

本发明属于机器人自主导航技术领域，涉及一种基于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在相机坐标系下位姿为

为对应变换矩阵；

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

设定

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

∑为协方差矩阵；

设定Z_Δξ为两个实体x₁和x₂之间的相对位姿测量值；其中，实体是物体，标签和载体的统称；x₁和x₂的位姿是ξ₁和ξ₂；相对位姿测量值可以表示为物体-物体之间，标签-标签之间，载体在两个时间点之间的相对位姿差；对应变换矩阵设为T₁，T₂，

∑为协方差矩阵；

设定Z_t＝{t_o，t₁，...，t_n}为在标签坐标系下标签角点的坐标设定值；

设定Z_o＝{p₁，p₂...，p_n}为在物体坐标系下物体的顶点坐标集合，以网格化的形式体现物体的空间存在；

设定d＝{d_x，d_y，d_z}为一个位姿旋转矩阵的旋转轴方向，∑为协方差矩阵；

设定K是相机内参矩阵；

步骤2，以预设数据为因子(Factot)，初始化因子图；

其中，所述的因子图是在给定观测Z的情况下，估计未知状态变量X的位姿。所述因子图是以X为节点，以条件概率P(X|Z)为因子约束位姿。本发明采用最大后验概率估计(MAP)最大化因子势能，以公式表述：

其中，条件概率pⁱ(X_i|Z)用高维高斯分布

来描述，以公式表述：

其中，设ξ_x是X_i的位姿，

是以观测Z为均值

的位姿，变换矩阵

变换矩阵

因子势能取负对数，优化问题转化为最小化非线性最小二乘(nonlinear least-square)的和：

其中，所述的因子包括：

位姿因子p_p：依所述步骤1中测量的物体，标签，相机位姿数据构建位姿因子：

相对位姿因子p_r：依所述步骤1中测量实体相对位姿数据构建相对位姿因子：

方向因子p_d：依所述步骤1中实体位姿ξ旋转矩阵的旋转轴方向设定数据构建方向因子：

其中，φ是位姿ξ的旋转向量；

步骤3，使用相机采集标签图像；

步骤4，利用相机标定畸变参数对标签图像进行预处理；

步骤5，获取图像中标签角点坐标与标签ID信息；

如图3所示，Z_u＝{u₀，u₁，...，u_n}为标签角点在图像坐标系中的二维坐标测量值；角点为标签边角的顶点。标签可以是单张Apriltag标签或者为多张Apriltag标签的组合。∑为协方差矩阵；

步骤6，以标签角点为映射因子更新因子图；

所述的映射因子，是依标签角点在图像坐标系中的坐标计算值与步骤5中标签角点在图像坐标系中的坐标测量值为数据构建的因子，以公式表述：

其中，标签角点在图像坐标系中的坐标计算值计算方法，依据公式：

其中，

为标签角点在时刻t在相机坐标系下的位姿，

为

的平移向量，u′_n为计算所得角点在图像坐标系下的坐标值；Z是

的z轴数据；

步骤7，采集载体的里程计数据；

其中，所述的载体里程计数据

可以是轮子编码器(wheelodometry)的里程计数据。

进一步的，所述的载体里程计数据

可以是视觉里程计(visual odometry)数据。

步骤8，以载体的里程计数据为相对位姿因子更新因子图；

其中，所述的因子为步骤2所述的相对位姿因子：

其中，

是载体在当前时间t和上次记录时间t_pre位姿的变换矩阵。

步骤9，优化因子图，计算出物体，标签，载体，相机的位姿，最后更新地图；

其中，所述的因子图优化方法选自高斯-牛顿迭代法和列文伯格-马夸尔特迭代法中的一种；

采用因子图优化方法，优化因子图，获得优化了的

通过采用步骤1.1中映射公式计算出物体，标签的位姿，载体与相机在t时刻在的位姿；采用步骤1中设定物体顶点坐标集合，在世界坐标系中构建关于物体的占据栅格地图(Occupancy GridMap)和体素栅格地图(Voxel Grid Map)；

步骤10，跳转至步骤3。

有益效果：本发明与现有技术相比，其显著优点是：使用本发明公开的方法，能够有效结合人工先验数据与系统优化结果；通过预先输入物体顶点数据结合系统优化了的物体位姿，可以完整构建空间地图；以人工设置的标签为基础构建因子图，有效降低运算数量级，且具有特征点不易丢失，运算结果精度高的特点，通过图优化可以获得高精度载体导航信息，并构建精准地图。另外需要特别指出的是，步骤2中位姿因子，相对位姿因子与方位因子用于进一步优化计算结果，可以获得更佳的技术效果，实施中因子可做有限必要设定，确保系统优化效果即可，未设定的实体位姿可通过图优化获得。

附图说明

图1是本发明所涉及方法的流程示意图。

图2是本发明因子图示意图。

图3是本发明Apriltag标签角点，在标签坐标系，图像坐标系，及相机坐标系下应对关系图

具体实施方式

以下实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。

实施例一：如图1所示，本实施例提供的基于Apriltag标签与因子图的SLAM方法包括：

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

其中，构建世界(World)坐标系，物体(Object)坐标系，标签(Apriltag)坐标系，载体(Robot)坐标系，相机(Camera)坐标系的方法：

步骤1.1：定义李代数

位姿

和李群sE(3)变换矩阵

其中

是平移(translation)向量，

是旋转(rotation)向量；

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

标签在物体坐标系下位姿为

相机在载体坐标系下位姿为

载体在时刻t在世界坐标系下位姿为

标签在时刻t在相机坐标系下位姿为

为对应变换矩阵；空间点P在任意两坐标系A，B下P_A，P_B存在转换关系：

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

设

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

∑为协方差矩阵；

是对物体，标签，相机初始位置的先验；

设Z_Δξ为两个实体x₁和x₂之间的相对位姿测量值；其中，实体是物体，标签和载体的统称；x₁和x₂的位姿是ξ₁和ξ₂；相对位姿测量值可以表示为物体-物体之间，标签-标签之间，载体在两个时间点之间的相对位姿；对应变换矩阵设为T₁，T₂，

∑为协方差矩阵；Z_Δξ可以是两个物体原点的位姿差；两个标签原点的位姿差；载体里程计测量的位移量；

设Z_t＝{t₀，t₁，...，t_n}为标签角点在标签坐标系中的坐标设定值；

设Z_o＝{p₁，p₂，...，p_n}为在物体坐标系下物体的顶点坐标集合，以网格化的形式体现物体的空间存在；

设d＝{d_x，d_y，d_z}为一个位姿旋转矩阵的旋转轴方向；∑为协方差矩阵；d是对位姿旋转轴方向的约束；如图2所示，d＝{0，0，1}可表示为实体(Object_1)旋转轴方向与z轴同向，即实体俯仰角，横滚角为0；约束物体在世界坐标系里空间竖直，将空间竖直这一先验知识引入因子图；

设定K是相机内参矩阵；

步骤2：以预设数据为因子(Factor)，初始化因子图；

其中，构建因子图的方法：

步骤2.1：在给定观测Z的情况下，估计未知状态变量X的位姿。所述因子图是以X为节点，以条件概率P(X|Z)为因子约束位姿。本发明采用最大后验概率估计(MAP)最大化因子势能，以公式表述：

步骤2.2：以条件概率pⁱ(X_i|Z)用高维高斯分布

来描述，以公式表述：

其中，设ξ_x是X_i的位姿，

是以观测Z为均值

的位姿，变换矩阵

变换矩阵

步骤2.3：对因子势能取负对数，优化问题转化为最小化非线性最小二乘(nonlinear least-square)的和：

步骤2.4：依步骤1所述预设数据添加因子：

步骤2.4.1：如图2所示，依所述步骤1中所测量的物体，标签，相机位姿数据构建位姿因子：

步骤2.4.2：如图2所示，依所述步骤1中测量实体相对位姿数据构建相对位姿因子：

步骤2.4.3：如图2所示，依所述步骤1中实体位姿ξ旋转矩阵的旋转轴方向设定数据构建方向因子：

其中，φ是位姿ξ的旋转向量；

步骤3：使用相机采集标签图像；

步骤4：利用相机标定畸变参数对标签图像进行预处理；

步骤5：获取图像中标签角点坐标与标签ID信息；

其中，获取图像中标签角点坐标与标签ID信息的步骤：

步骤5.1根据梯度检测出图像中的各种边缘，获取二值化边缘图像；

步骤5.2对二值化边缘图像使用道格拉斯-普克算法获取标签边际点；

步骤5.3对边际点组成的四边形图像进行编码与解码；

步骤5.4查询编码库获取标签ID与汉明距离；

步骤5.5如果汉明距离小于预设最大值H_max，返回角点集合Z_u与标签ID；

如图3所示，Z_u＝{u₀，u₁，...，u_n}为标签角点在图像坐标系中的二维坐标测量值；角点为标签边角的顶点。标签可以是单张Apriltag标签或者为多张Apriltag标签的组合。∑为协方差矩阵；

步骤6：以标签角点为映射因子更新因子图；

所述的映射因子，是依标签角点在图像坐标系中的坐标计算值与步骤5中标签角点在图像坐标系中的坐标测量值为数据构建的因子，以公式表述：

其中，标签角点在图像坐标系中的坐标计算值计算方法，依据公式：

其中，设

为标签角点在时刻t在相机坐标系下的位姿，

为

的平移向量，u′_n为计算所得角点在图像坐标系下的坐标值；Z是

的z轴数据；

步骤7：判断是否提供载体里程计数据；如果不存在里程计数据，则转至步骤9；如果存在里程计数据，采集载体的里程计数据；

其中所述的载体里程计数据

是轮子编码器(wheelodometry)的里程计数据。

步骤8：以载体里程计数据为相对位姿因子更新因子图；

其中，以载体位移为相对位姿因子更新因子图的方法为：

步骤8.1：如图2所示，以

为观测数据，载体在当前时间t和上次记录时间t_pre位姿的变换矩阵

为节点构建相对位姿因子：

步骤9：优化因子图；计算出物体，标签，载体，相机的位姿，最后更新地图。

其中，所述的因子图优化方法可采用列文伯格-马夸尔特迭代法；

采用因子图优化方法，优化因子图，获得优化了的

通过采用映射公式计算出物体，标签的位姿，载体与相机在t时刻在的位姿；采用步骤1中设定物体顶点坐标集合，在世界坐标系中构建关于物体的占据栅格地图(Occupancy Grid Map)和体素栅格地图(Voxel Grid Map)。

实施例二：在实施例一基础上进行优化，特别之处在于：

所述的步骤7中所述的载体里程计数据

是视觉里程计(visual odometry)数据；

所述的步骤9中所述的因子图优化方法采用高斯-牛顿迭代法。

Claims (5)

1.一种基于Apriltag标签与因子图的SLAM方法，其特征在于，包括以下步骤：

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

其中，预设数据包括：物体在世界坐标系下，标签在物体坐标系下，相机在载体坐标系下的位姿测量值；实体之间的相对位姿测量值；标签角点在标签坐标系中的坐标设定值；物体的顶点坐标集合；实体位姿旋转矩阵的旋转轴方向；相机内参矩阵；

步骤2.以预设数据为因子Factor，初始化因子图；

其中，所述的因子图是在给定观测Z的情况下，估计未知状态变量X的位姿；所述因子图是以X为节点，以条件概率P(X|Z)为因子约束位姿；其中，所述的因子包括位姿因子，相对位姿因子，方向因子，映射因子；

步骤3.使用相机采集标签图像；

步骤4.利用相机标定畸变参数对标签图像进行预处理；

步骤5.获取图像中标签角点坐标与标签ID信息；

步骤6.以标签角点为映射因子更新因子图；

步骤7.采集载体的里程计数据；

步骤8.以载体的里程计数据为相对位姿因子更新因子图；

步骤9.优化因子图，计算出物体，Apriltag标签，载体，相机的位姿，最后更新地图；

步骤10.跳转至步骤3。

2.根据权利要求1所述的方法，其特征在于，所述步骤2中，所述因子具体包括：

依所述步骤1中测量的物体，标签，相机位姿测量数据构建位姿因子：

其中，

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

∑为协方差矩阵；

依所述步骤1中实体相对位姿测量数据构建相对位姿因子：

其中，Z_Δξ为两个实体x₁和x₂之间的相对位姿测量值；其中，实体是物体，标签和载体的统称；x₁和x₂的位姿是ξ₁和ξ₂；相对位姿测量值可以表示为物体-物体之间，标签-标签之间，载体在两个时间点之间的相对位姿差；对应变换矩阵设为

∑为协方差矩阵；依所述步骤1中实体位姿旋转矩阵的旋转轴方向设定数据构建方向因子：

其中，d＝{d_x，d_y，d_z}为一个位姿旋转矩阵的旋转轴方向，∑为协方差矩阵；φ是位姿ξ的旋转向量。

3.根据权利要求1所述的方法，其特征在于，所述步骤6中，依据标签角点在图像坐标系中的坐标计算值与步骤5中标签角点的测量值构建映射因子，依据公式：

其中，物体在世界坐标系下位姿为

标签在物体坐标系下位姿为

相机在载体坐标系下位姿为

载体在时刻t在世界坐标系下位姿为

标签在时刻t在相机坐标系下位姿为

为对应变换矩阵；其中，标签角点在图像坐标系中的坐标计算值计算方法，依据公式：

其中，

为标签角点在时刻t在相机坐标系下的位姿，

为

的平移向量，u′_n为计算所得角点在图像坐标系下的坐标值；Z是

的z轴数据；Z_u＝{u₀，u₁，...，u_n}为标签角点在图像坐标系中的二维坐标测量值；Z_t＝{t₀，t₁，...，t_n}为在标签坐标系下标签角点的坐标设定值；K是相机内参矩阵；∑为协方差矩阵。

4.根据权利要求1所述的方法，其特征在于，所述步骤9中，因子图优化方法选自高斯-牛顿迭代法和列文伯格-马夸尔特迭代法中的一种。

5.根据权利要求1所述的方法，其特征在于，所述步骤9中，更新地图的方法：

采用因子图优化方法，优化因子图，计算出物体，标签，载体，相机的位姿；采用步骤1中设定物体顶点坐标集合，在世界坐标系中构建关于物体的占据栅格地图(Occupancy GridMap)和体素栅格地图(Voxel Grid Map)。

Images