本发明属于机器人自主导航技术领域，涉及一种基于apriltag标签与因子图的机器人同步定位与制图(slam)方法。
背景技术：
slam技术是机器人自主导航领域的关键与研究热点，是实现机器人环境感知的关键基础技术，在工业，物流，服务等领域有广泛的应用前景。apriltag是一种视觉基准标定(fiducialmarker)系统，提供标签(tag)与相机之间空间位姿关系以及自身id信息。目前基于标签定位技术大多采用pnp法实现，此类技术缺少整体优化与回环检测，位姿估算受测量误差影响大。
目前多数视觉slam技术直接从图像中提取自然特征点进行匹配来实现定位与制图。传统特征点提取存在数据量大，易丢失，匹配计算复杂，抗干扰能力差等缺点；同时，此类slam技术依赖于网格化生成算法来感知空间物体存在，易受点云噪声干扰。此类技术存在计算复杂，制图信息不完整的缺点。
技术实现要素：
本发明的目的：本发明提供一种基于apriltag标签(简称标签)与因子图的slam方法，以解决现有slam技术计算量大，空间物体感知能力差，构建地图不完整且误差大的问题；通过本发明公开的方法，利用人工设定的特征点(标签角点)，预先有限设定的空间数据以及因子图，能够实现载体的高精度定位，空间物体建模，以及获得完整精确的地图。
技术方案：本发明所述的基于apriltag标签与因子图的slam方法包括：
步骤1，读取预设数据，构建世界(world)坐标系，物体(object)坐标系，标签(apriltag)坐标系，载体(robot)坐标系，和相机(camera)坐标系；
其中，所述的构建世界坐标系，物体坐标系，标签坐标系，载体坐标系，和相机坐标系的具体方法为：
步骤1.1，定义李代数
其中，位姿ξ与变换矩阵t存在映射公式：
步骤1.2，如图2所示，以初始时刻某点的位置为原点构建世界坐标系；以物体某顶点为原点构建物体坐标系；以标签的第一个角点为原点，所在平面为x-o-y平面构建标签坐标系；以载体中心为原点构建载体坐标系；以相机焦点为原点构建相机坐标系；设物体在世界坐标系下位姿为
其中，所述的预设数据为人工测量数据和设定数据，其包括：
设定
设定zδξ为两个实体x1和x2之间的相对位姿测量值；其中，实体是物体，标签和载体的统称；x1和x2的位姿是ξ1和ξ2；相对位姿测量值可以表示为物体-物体之间，标签-标签之间，载体在两个时间点之间的相对位姿差；对应变换矩阵设为t1，t2，
设定zt＝{to，t1，...，tn}为在标签坐标系下标签角点的坐标设定值；
设定zo＝{p1，p2...，pn}为在物体坐标系下物体的顶点坐标集合，以网格化的形式体现物体的空间存在；
设定d＝{dx，dy，dz}为一个位姿旋转矩阵的旋转轴方向，∑为协方差矩阵；
设定k是相机内参矩阵；
步骤2，以预设数据为因子(factot)，初始化因子图；
其中，所述的因子图是在给定观测z的情况下，估计未知状态变量x的位姿。所述因子图是以x为节点，以条件概率p(x|z)为因子约束位姿。本发明采用最大后验概率估计(map)最大化因子势能，以公式表述：
其中，条件概率pi(xi|z)用高维高斯分布
其中，设ξx是xi的位姿，
因子势能取负对数，优化问题转化为最小化非线性最小二乘(nonlinearleast-square)的和：
其中，所述的因子包括：
位姿因子pp：依所述步骤1中测量的物体，标签，相机位姿数据构建位姿因子：
相对位姿因子pr：依所述步骤1中测量实体相对位姿数据构建相对位姿因子：
方向因子pd：依所述步骤1中实体位姿ξ旋转矩阵的旋转轴方向设定数据构建方向因子：
其中，φ是位姿ξ的旋转向量；
步骤3，使用相机采集标签图像；
步骤4，利用相机标定畸变参数对标签图像进行预处理；
步骤5，获取图像中标签角点坐标与标签id信息；
如图3所示，zu＝{u0，u1，...，un}为标签角点在图像坐标系中的二维坐标测量值；角点为标签边角的顶点。标签可以是单张apriltag标签或者为多张apriltag标签的组合。∑为协方差矩阵；
步骤6，以标签角点为映射因子更新因子图；
所述的映射因子，是依标签角点在图像坐标系中的坐标计算值与步骤5中标签角点在图像坐标系中的坐标测量值为数据构建的因子，以公式表述：
其中，标签角点在图像坐标系中的坐标计算值计算方法，依据公式：
其中，
步骤7，采集载体的里程计数据；
其中，所述的载体里程计数据
进一步的，所述的载体里程计数据
步骤8，以载体的里程计数据为相对位姿因子更新因子图；
其中，所述的因子为步骤2所述的相对位姿因子：
其中，
步骤9，优化因子图，计算出物体，标签，载体，相机的位姿，最后更新地图；
其中，所述的因子图优化方法选自高斯-牛顿迭代法和列文伯格-马夸尔特迭代法中的一种；
采用因子图优化方法，优化因子图，获得优化了的
步骤10，跳转至步骤3。
有益效果：本发明与现有技术相比，其显著优点是：使用本发明公开的方法，能够有效结合人工先验数据与系统优化结果；通过预先输入物体顶点数据结合系统优化了的物体位姿，可以完整构建空间地图；以人工设置的标签为基础构建因子图，有效降低运算数量级，且具有特征点不易丢失，运算结果精度高的特点，通过图优化可以获得高精度载体导航信息，并构建精准地图。另外需要特别指出的是，步骤2中位姿因子，相对位姿因子与方位因子用于进一步优化计算结果，可以获得更佳的技术效果，实施中因子可做有限必要设定，确保系统优化效果即可，未设定的实体位姿可通过图优化获得。
附图说明
图1是本发明所涉及方法的流程示意图。
图2是本发明因子图示意图。
图3是本发明apriltag标签角点，在标签坐标系，图像坐标系，及相机坐标系下应对关系图
具体实施方式
以下实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。
实施例一：如图1所示，本实施例提供的基于apriltag标签与因子图的slam方法包括：
步骤1：读取预设数据，构建世界(world)坐标系，物体(object)坐标系，标签(apriltag)坐标系，载体(robot)坐标系，相机(camera)坐标系；
其中，构建世界(world)坐标系，物体(object)坐标系，标签(apriltag)坐标系，载体(robot)坐标系，相机(camera)坐标系的方法：
步骤1.1：定义李代数
步骤1.2：如图2所示，以初始时刻某点的位置为原点构建世界坐标系；以物体某顶点为原点构建物体坐标系；以标签的第一个角点为原点，所在平面为x-o-y平面构建标签坐标系；以载体中心为原点构建载体坐标系；以相机焦点为原点构建相机坐标系；设物体在世界坐标系下位姿为
其中，步骤1中所述的预设数据为人工测量数据和设定数据，其包括：
设
设zδξ为两个实体x1和x2之间的相对位姿测量值；其中，实体是物体，标签和载体的统称；x1和x2的位姿是ξ1和ξ2；相对位姿测量值可以表示为物体-物体之间，标签-标签之间，载体在两个时间点之间的相对位姿；对应变换矩阵设为t1，t2，
设zt＝{t0，t1，...，tn}为标签角点在标签坐标系中的坐标设定值；
设zo＝{p1，p2，...，pn}为在物体坐标系下物体的顶点坐标集合，以网格化的形式体现物体的空间存在；
设d＝{dx，dy，dz}为一个位姿旋转矩阵的旋转轴方向；∑为协方差矩阵；d是对位姿旋转轴方向的约束；如图2所示，d＝{0，0，1}可表示为实体(object_1)旋转轴方向与z轴同向，即实体俯仰角，横滚角为0；约束物体在世界坐标系里空间竖直，将空间竖直这一先验知识引入因子图；
设定k是相机内参矩阵；
步骤2：以预设数据为因子(factor)，初始化因子图；
其中，构建因子图的方法：
步骤2.1：在给定观测z的情况下，估计未知状态变量x的位姿。所述因子图是以x为节点，以条件概率p(x|z)为因子约束位姿。本发明采用最大后验概率估计(map)最大化因子势能，以公式表述：
步骤2.2：以条件概率pi(xi|z)用高维高斯分布
其中，设ξx是xi的位姿，
步骤2.3：对因子势能取负对数，优化问题转化为最小化非线性最小二乘(nonlinearleast-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如果汉明距离小于预设最大值hmax，返回角点集合zu与标签id；
如图3所示，zu＝{u0，u1，...，un}为标签角点在图像坐标系中的二维坐标测量值；角点为标签边角的顶点。标签可以是单张apriltag标签或者为多张apriltag标签的组合。∑为协方差矩阵；
步骤6：以标签角点为映射因子更新因子图；
所述的映射因子，是依标签角点在图像坐标系中的坐标计算值与步骤5中标签角点在图像坐标系中的坐标测量值为数据构建的因子，以公式表述：
其中，标签角点在图像坐标系中的坐标计算值计算方法，依据公式：
其中，设
步骤7：判断是否提供载体里程计数据；如果不存在里程计数据，则转至步骤9；如果存在里程计数据，采集载体的里程计数据；
其中所述的载体里程计数据
步骤8：以载体里程计数据为相对位姿因子更新因子图；
其中，以载体位移为相对位姿因子更新因子图的方法为：
步骤8.1：如图2所示，以
步骤9：优化因子图；计算出物体，标签，载体，相机的位姿，最后更新地图。
其中，所述的因子图优化方法可采用列文伯格-马夸尔特迭代法；
采用因子图优化方法，优化因子图，获得优化了的
实施例二：在实施例一基础上进行优化，特别之处在于：
所述的步骤7中所述的载体里程计数据
所述的步骤9中所述的因子图优化方法采用高斯-牛顿迭代法。
1.一种基于apriltag标签与因子图的slam方法,其特征在于，包括以下步骤：
步骤1.读取预设数据，构建世界(world)坐标系，物体(object)坐标系,标签(apriltag)坐标系，载体(robot)坐标系，和相机(camera)坐标系；
其中，预设数据包括：物体在世界坐标系下，标签在物体坐标系下,相机在载体坐标系下的位姿测量值；实体之间的相对位姿测量值；标签角点在标签坐标系中的坐标设定值；物体的顶点坐标集合；实体位姿旋转矩阵的旋转轴方向；相机内参矩阵；
步骤2.以预设数据为因子(factor)，初始化因子图；
其中，因子图是在给定观测z的情况下，估计未知状态变量x的位姿；依据公式：
步骤3.使用相机采集标签图像；
步骤4.利用相机标定畸变参数对标签图像进行预处理；
步骤5.获取图像中标签角点坐标与标签id信息；
步骤6.以标签角点为映射因子更新因子图；
步骤7.采集载体的里程计数据；
步骤8.以载体的里程计数据为相对位姿因子更新因子图；
步骤9.优化因子图，计算出物体，apriltag标签，载体，相机的位姿，最后更新地图；
步骤10.跳转至步骤3。
2.根据权利要求1所述的方法，其特征在于，所述步骤2中，所述因子具体包括：
依所述步骤1中测量的物体，标签，相机位姿测量数据构建位姿因子：
依所述步骤1中实体相对位姿测量数据构建相对位姿因子：
依所述步骤1中实体位姿旋转矩阵的旋转轴方向设定数据构建方向因子：
3.根据权利要求1所述的方法，其特征在于，所述步骤6中，依据标签角点在标签坐标系中的坐标设定值计算出标签角点在图像坐标系中的坐标计算值，依据公式：
依据标签角点在图像坐标系中的坐标计算值与步骤5中标签角点的测量值构建映射因子，依据公式：
4.根据权利要求1所述的方法，其特征在于，所述步骤8中，以载体的里程计数据为相对位姿因子，依据公式：
5.根据权利要求1所述的方法，其特征在于，所述步骤9中，因子图优化方法选自高斯-牛顿迭代法和列文伯格-马夸尔特迭代法中的一种。
6.根据权利要求1所述的方法，其特征在于，所述步骤9中，更新地图的方法：
采用因子图优化方法，优化因子图，获得优化了的
