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

权利要求书

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)。