CN117109638B

CN117109638B - 一种在线单舵轮agv参数标定方法、系统、设备及介质

Info

Publication number: CN117109638B
Application number: CN202311376425.8A
Authority: CN
Inventors: 周军; 侯梦魁; 付周; 贾凯龙; 马永鑫; 李新宇; 崔梓豪; 张盟; 刘道龙
Original assignee: Shandong University
Current assignee: Shandong University
Priority date: 2023-10-24
Filing date: 2023-10-24
Publication date: 2024-03-15
Anticipated expiration: 2043-10-24
Also published as: CN117109638A

Abstract

本发明提供了一种在线单舵轮AGV参数标定方法、系统、设备及介质，所述方案属于AGV导航控制技术领域，通过采用双目相机替代激光雷达传感器来对单舵轮AGV进行内外参标定，能够获取的环境信息更加丰富，提供相对较高精度的位姿增量估计，且实时性更高；且通过对相邻图像间的特征点匹配可以计算出相机在空间中的运动，作为后续进行单舵轮里程计内外参标定的基础；同时，为提高相机运动估计的准确性，采用对错误匹配点对进行汉明距离粗去除结合PROSAC算法精去除的策略，对已经匹配好的特征点对做进一步的滤除，过滤掉可能存在的错误的特征匹配，进一步提高了相机运动估计的精确性以及单舵轮内外参标定的准确性。

Description

一种在线单舵轮AGV参数标定方法、系统、设备及介质

技术领域

本发明属于AGV导航控制技术领域，尤其涉及一种在线单舵轮AGV参数标定方法、系统、设备及介质。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

随着传感器技术和室内2D SLAM（simultaneous localization and mapping）技术的不断演进，传感器融合技术正逐渐崭露头角，成为机器人建图与定位研究的主要趋势。机器人通过多个或多种传感器的数据融合，建立数据之间的约束关系，从而实现对周围环境的高精度建图和定位。然而，当机器人采用轮式编码器进行辅助建图和定位时，机械制造、安装误差以及机器人长时间运行导致的轮胎形变、滑动和磨损等因素会导致里程计的内部和外部参数发生微小变化。这些微小变化对机器人的建图和定位性能产生显著影响，因此需要定期进行参数标定以确保建图和定位的准确性和稳定性。

发明人发现，目前已有的单舵轮模型都是基于激光雷达进行离线标定，但离线标定效率低、精度低；采用激光雷达进行标定存在精度不高、实时性不好、系统成本高等问题。以上的方法不能较好满足室内SLAM环境下的自动化实时高精度标定要求。

发明内容

本发明为了解决上述问题，提供了一种在线单舵轮AGV参数标定方法、系统、设备及介质，所述方案采用双目相机替代激光雷达传感器来对单舵轮AGV进行内外参标定，相较于激光雷达，双目相机能够提供相对较高精度的位姿增量估计，尤其是在短时间的小范围运动中表现良好，实时性更高，环境适应性较强，成本更加低廉；同时，通过对相邻图像间的特征点匹配可以计算出相机在空间中的运动，作为后续进行单舵轮里程计内外参标定的基础。

根据本发明实施例的第一个方面，提供了一种在线单舵轮AGV参数标定方法，包括：

获取相邻两帧图像的信息，计算两帧图像数据之间双目相机的位姿增量;其中，所述图像的获取采用设置于单舵轮AGV上的双目相机；

根据双目相机的图像数据时间戳对获得的轮式里程计数据进行插值，得到与相机数据的时间戳对应的轮式里程计数据；

基于两帧图像数据时间戳之间的轮式里程计数据，计算得到单舵轮AGV的位姿增量；

基于双目相机位姿增量的旋转增量与单舵轮AGV位姿增量的旋转增量相等的关系，计算得到单舵轮的内参；

基于双目相机位姿增量的平移增量、单舵轮AGV位姿增量的平移增量以及双目相机的安装位姿三者的关系，计算得到单舵轮的外参。

进一步的，所述获取相邻两帧图像的信息，计算两帧图像数据之间相机的位姿增量，具体为：获取相邻两帧图像的ORB特征，并根据检测到的Oriented FAST角点位置计算BRIEF描述子；使用汉明距离对两帧图像中的BRIEF描述子进行匹配，获得有效匹配点；基于所述有效匹配点，利用SVD方法及最小二乘法，获得所述位姿增量。

进一步的，所述使用汉明距离对两帧图像中的BRIEF描述子进行匹配，获得有效匹配点，具体为:计算两帧图像间各描述子之间的距离，若描述子之间的距离小于两倍预设阈值，则为有效匹配点，若不小于，则为无效匹配点。

进一步的，对于获得的有效匹配点，基于PROSAC算法进行错误匹配点的二次滤除。

进一步的，所述得到与相机数据的时间戳对应的轮式里程计数据，具体为：对于获取的轮式里程计数据，判断最近的轮式里程计数据时间戳是否大于当前帧图像数据时间戳，若否，则舍弃并等待新的轮式里程计数据；若是，则对时间进行离散化，并根据相邻两帧图像数据时间戳对里程计数据进行线性插值。

进一步的，所述单舵轮的内参舵轮转角零漂以及单舵轮半径/>，具体表示如下：

其中，表示通解/>的第j行元素，j的取值为1或2，L为舵轮中心与AGV中心之间的距离。

进一步的，所述单舵轮的外参基于双目相机位姿增量、单舵轮AGV位姿增量/>以及双目相机的安装位姿/>三者的关系获得，所述关系表示如下：

其中，为李代数上的标准复合算子。

根据本发明实施例的第二个方面，提供了一种在线单舵轮AGV参数标定系统，包括：

相机位姿增量计算单元，其用于获取相邻两帧图像的信息，计算两帧图像数据之间双目相机的位姿增量;其中，所述图像的获取采用设置于单舵轮AGV上的双目相机；

轮式里程计位姿变换单元，其用于根据双目相机的图像数据时间戳对获得的轮式里程计数据进行插值，得到与相机数据的时间戳对应的轮式里程计数据；

单舵轮AGV位姿增量计算单元，其用于基于两帧图像数据时间戳之间的轮式里程计数据，计算得到单舵轮AGV的位姿增量；

内参标定单元，其用于基于双目相机位姿增量的旋转增量与单舵轮AGV位姿增量的旋转增量相等的关系，计算得到单舵轮的内参；

外参标定单元，其用于基于双目相机位姿增量的平移增量、单舵轮AGV位姿增量的平移增量以及双目相机的安装位姿三者的关系，计算得到单舵轮的外参。

根据本发明实施例的第三个方面，提供了一种电子设备，包括存储器、处理器及存储在存储器上运行的计算机程序，所述处理器执行所述程序时实现所述的一种在线单舵轮AGV参数标定方法。

根据本发明实施例的第四个方面，提供了一种非暂态计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现所述的一种在线单舵轮AGV参数标定方法。

与现有技术相比，本发明的有益效果是：

（1）本发明提供了一种在线单舵轮AGV参数标定方法及系统，所述方案采用双目相机替代激光雷达传感器来对单舵轮AGV进行内外参标定，相较于激光雷达，双目相机所能获取的环境信息更加丰富，能够提供相对较高精度的位姿增量估计，尤其是在短时间的小范围运动中表现良好，实时性更高；同时，通过对相邻图像间的特征点匹配可以计算出相机在空间中的运动，作为后续进行单舵轮里程计内外参标定的基础。

（2）本发明所述方案为提高相机运动估计的准确性，提出采用对错误匹配点对进行汉明距离粗去除结合PROSAC算法精去除的策略，对已经匹配好的特征点对做进一步的滤除，过滤掉可能存在的错误的特征匹配，进一步提高了相机运动估计的精确性以及单舵轮内外参标定的准确性。

（3）本发明所述方案中的相机传感器相较于激光雷达成本更低、也更加轻便，易于获取和维护；标定的过程中不需要对标定参数进行初始估计，也不需要对机器人的运动轨迹进行预设，标定过程更加方便。

（4）本发明所述方案为在线标定，不需要人工参与，可以实时更新参数并校正误差，解决了由于单舵轮里程计参数不准，轮子磨损以及机械振动导致双目相机相对于单舵轮AGV中心安装位置的变化给定位和建图带来误差的问题。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例中所述的单舵轮AGV的结构以及坐标系的建立示意图；

图2为本发明实施例中所述的一种在线单舵轮AGV参数标定方法流程图；

图3为本发明实施例中所述的基于双目相机的视觉里程计计算两帧图像数据之间相机的位姿增量流程图；

图4为本发明实施例中所述的去除误匹配的算法流程图；

图5为本发明实施例中所述的单舵轮运动过程中旋转一定角度时，AGV的运动类比示意图；

图6为本发明实施例中所述的各坐标系之间的转换关系示意图；

图7为本发明实施例中所述的轮式里程计位姿变换示意图。

具体实施方式

下面结合附图与实施例对本发明做进一步说明。

应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

术语解释：

ORB：Oriented FAST and Rotated BRIEF

实施例一：

本实施例的目的是提供一种在线单舵轮AGV参数标定方法。

需要说明的是，本实施例所述方案需要对图1所示的单舵轮AGV的以下参数进行估计：

（1）内在参数：单舵轮的半径以及舵轮转角零漂/>

（2）外在参数：双目相机的安装位姿参数l=[lx，ly，lθ]，其中，lx，ly，lθ分别为双目相机坐标系原点在单舵轮AGV坐标系的x坐标、y坐标和方向角。

单舵轮AGV（Automated Guided Vehicle）是一种自动导引车辆，车辆的后方具有单个舵轮用于转向操作。它的工作原理是通过设定舵轮的速度和转向角度来驱动整个AGV的运动和转向。整机还搭配四个从动万向轮，是一种特殊类型的AGV，用于在工业、物流和仓储等领域中实现自动化运输和物流操作。

单舵轮AGV的结构示意图以及坐标系的建立如图1所示。单舵轮AGV坐标用{V}表示，坐标系原点与AGV读码器中心点O重合。以机器人前进方向为X轴的正向，单舵轮也位于x轴上。AGV坐标系原点的状态代表着整机的姿态和速度。世界坐标系{W}是固定的坐标系。令q = (qx，qy，qθ)是表示 AGV 坐标系{V}在世界坐标系{W}中的坐标表示。l= (lx，ly，lθ)是相机坐标系{C}相对于单舵轮AGV 坐标系{V}的相对位姿。

为实现单舵轮AGV的高精度定位，本实施例所述方案提供的一种在线单舵轮AGV参数标定方法流程如下图2所示，具体包括如下步骤：

步骤（1）：获取相邻两帧图像的信息，对其进行ORB特征的提取、匹配、改进误匹配点对剔除，最终根据正确的匹配点对估计相邻两帧图像数据之间双目相机的位姿增量；

其中，如图3所示，所述步骤（1）具体包括如下处理过程：

1）提取相邻帧图像中的ORB特征；

其中，由于特征是图像信息的另一种数字表达形式，ORB特征是具有代表性的图像特征，在SLAM中的特征提取与匹配环节，ORB特征具有旋转不变性、尺度不变性、较好的实时性等优点，因此能兼顾SLAM系统较好的精度和较高的鲁棒性。

提取两幅图像中的ORB特征主要包括两个步骤，首先提取“Oriented FAST”角点，主要检测局部像素灰度变化明显的地方，原理是如果一个像素与它相邻的像素差别较大,则大概率是角点；然后对前一步提取出特征点的周围区域进行描述，即ORB特征中的描述子（BRIEF）。

2）使用Hamming距离对两幅图像中的BRIEF描述子进行特征匹配；

具体的，当我们对两幅图像中的特征进行匹配时，实质上是对他们的BRIEF描述子进行匹配。BRIEF描述子为二进制，两个描述子之间的距离采用汉明距离来表示。它代表的是两个二进制串不同位数的个数。个数的数字越小，距离越小，代表两个特征越相似，越有可能产生匹配。

3）根据汉明距离对错误匹配点对进行粗去除；以及基于PROSAC算法对错误匹配点对进行精去除；

需要注意的是并不是所有的匹配点对都是正确的，需要对错误的匹配点对进行去除。本实施例所述方案采用一种改进的误匹配去除方法，来最大限度地去除错误匹配点对，并对正确的匹配点对进行保留。所述改进的误匹配去除方法具体为：

首先使用基于汉明距离设置阈值的方式进行粗去除，其原理为计算所有匹配点对的最大距离与最小距离，然后遍历所有匹配点对，如果匹配点对描述子之间的距离大于两倍的最小距离，则匹配错误，进行去除，最小距离的下限设置为预设值（该值可根据实际需求进行设定，本实施例设置为30）。例如：假设计算完匹配点对的距离之后，发现最小距离为30.最大距离为80，小于2倍最小距离为正确匹配点对的话，就是取30-60之间的。

紧接着基于PROSAC（Progressive Sample Consensus）算法对错误匹配点对进行精去除，算法流程图可以用图4表示。经过对改进的误匹配去除方法的数据集验证，可以发现匹配点对数量被有效精简，并且保留的都是正确的匹配。因此该改进的误匹配去除方法能够进一步提升相机位姿增量估计的精度，进而为后续提升单舵轮内外参标定的准确度做准备。

其中，如图4所示，所述PROSAC算法具体包括如下步骤：

步骤1：设置PROSAC算法参数；

步骤2：随机选择一个样本来拟合单应矩阵；

步骤3：使用估计的单应矩阵将源点映射到目标点；

步骤4：计算映射后的点与目标点之间的距离是否小于内点阈值，若否，则舍弃；若是，则将匹配点加入到内点集合中，并更新内点集合；

步骤5：判断是否满足终止迭代的阈值条件，若是，则结束算法，若否，则返回至步骤2继续执行。

4）根据正确的特征匹配求解ICP,估计两帧图像之间相机的位姿增量；

具体的，我们使用SVD方法来求解已知匹配点对的相机运动估计问题，定义第k对点的误差项，如式子（1）所示

其中，为第一个图像中的数据，/>为第二个图像中的数据，R为相邻两帧图像数据之间相机的旋转增量，t为相邻两帧图像数据之间相机的平移增量。

然后构建如式子（2）的最小二乘问题，求使得误差平方和达到最小的R和t，即相邻两帧图像数据之间相机的旋转增量R以及平移增量t。转换到二维空间下，根据求解的R和t，经过变换，可以用一个位姿增量来描述。

其中，双目相机位姿增量，根据公式（2）求解得到，位姿增量由R和t组成。可以由一个变换矩阵表示：/>=/>。

步骤（2）：根据双目相机的图像数据时间戳对轮式里程计数据进行插值，得到与相机数据的时间戳对应的单舵轮里程计数据;并基于获得的轮式里程计数据，计算两帧图像数据时间戳之间由单舵轮里程计数据得出的AGV中心的位姿增量；

具体的，如图5所示，单舵轮位于X轴上，在运动过程中，设单舵轮的转角为，速度为/>；AGV的旋转中心为O，某段时间内其运动可以看作绕着瞬心/>做半径为R的圆弧运动，其速度为/>，旋转了/>角度。

建立单舵轮AGV的运动模型：

其中，为AGV线速度在AGV坐标系x方向的分量，/>为舵轮线速度，/>为舵轮转向角，/>为AGV角速度，/>为舵轮自身旋转的角速度，/>为单舵轮的半径，L为舵轮中心与AGV中心之间的距离。

由于零漂（即零点漂移）的影响，使得舵轮转向机构传感器反馈的数值与当前舵轮的真实转角常常是不相符的，从而错误估计机器人的运动状态，把零漂考虑进来之后，式（3）变形为：

如图7所示，对于获取的轮式里程计数据，判断最近的轮式里程计数据时间戳是否大于当前帧图像数据时间戳，若否，则舍弃，并等待新的里程计数据；若是，则对时间进行离散化，并根据相邻两帧图像数据时间戳对里程计数据进行线性插值，同时，计算出相邻两帧图像数据之间里程计的位姿变化。

其中，各坐标系之间的转换关系示意图如图6所示。轮式里程计位姿变化计算流程如图7所示。

在时间间隔[,/>]内，单舵轮AGV的位姿增量中的相对旋转增量/>的表达式计算为：

假设转向角在时间间隔[/>,/>]内恒定，则上式可变形为：

式中，为在时间间隔[/>,/>]内单舵轮驱动轮转动角度，可以由反馈的编码器数据转换得到。/>代表[/>,/>]时间戳内由轮式里程计数据得到的转向角。

步骤（3）：利用双目相机位姿增量的旋转增量与AGV中心位姿增量/>的旋转增量相等的关系计算单舵轮的内参；

我们把式子（6）进行分解，表示成两个矩阵相乘的形式，其中：

然后将待标定参数用矩阵X表示：

最后把矩阵扩展到k个单舵轮里程计传感器数据的矩阵形式：

同样地，矩阵b由k个双目相机对应时间间隔内的相对旋转增量表示，由前面改进视觉里程计计算得到。

其中，为[/>,/>]时间间隔上的双目相机坐标系的相对旋转增量。

由，即在相邻两帧数据之间，轮式里程计计算出的相对旋转增量/>等于[/>,/>]时间间隔上的双目相机坐标系的相对旋转增量/>。于是可建立矩阵方程/>。已知未知参数有2个，但是存在有两个以上的独立方程，为超定方程组，无解但是有最小二乘解：其通解的计算公式为：/>，则计算出的内参为：

通解为两行一列的矩阵，其中，/>表示通解/>的第j行元素，j的取值为1和2。

步骤（4）：利用双目相机位姿增量的平移增量与AGV中心位姿增量/>的位移增量以及双目相机的安装位姿三者的关系计算单舵轮的外参。

其中，外参的标定可以利用到先前标定好的内参，轮式里程计信息提供的先验位姿可以用于双目相机计算自身位姿，使用标定完的内参得出来的先验位姿更加准确，使得双目相机的定位也更加准确。

图6中和/>之间的位姿变换关系可以用李代数se(2)上的标准复合算子/>来表示，则可表示为：

式中，的旋转分量/>已在前面内参标定时计算得出。/>的平移分量/>（单舵轮只有x方向上的平移分量）的计算方法和/>的计算方法类似，/>为单舵轮的半径：

令：

其中，为相机数据得出的位姿与里程计数据得出的位姿的误差，构建误差的平方和函数：

式中，=/>为第k帧数据误差/>的平移增量，T为矩阵的转置。

利用最小二乘法求解外参（lx，ly，lθ）使最小，从而标定外参。

其中，双目相机的安装位姿参数l=（lx，ly，lθ），其中，lx，ly，lθ分别为双目相机坐标系原点在单舵轮AGV坐标系的x坐标、y坐标和方向角。

实施例二：

本实施例的目的是提供一种在线单舵轮AGV参数标定系统。

一种在线单舵轮AGV参数标定系统，包括：

进一步的，本实施例所述系统与实施例一所述方法相对应，其技术细节在实施例一中进行了详细描述，故此处不再赘述。

实施例三：

本实施例的目的是提供一种电子设备。

一种电子设备，包括存储器、处理器及存储在存储器上运行的计算机程序，所述处理器执行所述程序时实现所述的一种在线单舵轮AGV参数标定方法。

实施例四：

本实施例的目的是提供一种非暂态计算机可读存储介质。

一种非暂态计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现所述的一种在线单舵轮AGV参数标定方法。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种在线单舵轮AGV参数标定方法，其特征在于，包括：

获取相邻两帧图像的信息，计算两帧图像数据之间双目相机的位姿增量；其中，所述图像的获取采用设置于单舵轮AGV上的双目相机；

基于双目相机位姿增量的平移增量、单舵轮AGV位姿增量的平移增量以及双目相机的安装位姿三者的关系，计算得到单舵轮的外参；

所述获取相邻两帧图像的信息，计算两帧图像数据之间双目相机的位姿增量，具体为：获取相邻两帧图像的ORB特征，并根据检测到的Oriented FAST角点位置计算BRIEF描述子；使用汉明距离对两帧图像中的BRIEF描述子进行匹配，获得有效匹配点；基于所述有效匹配点，利用SVD方法及最小二乘法，获得所述位姿增量；

所述使用汉明距离对两帧图像中的BRIEF描述子进行匹配，获得有效匹配点，具体为:计算两帧图像间各描述子之间的距离，若描述子之间的距离小于两倍预设阈值，则为有效匹配点，若不小于，则为无效匹配点；

对于获得的有效匹配点，基于PROSAC算法进行错误匹配点的二次滤除；

所述PROSAC算法具体包括如下步骤：

步骤1：设置PROSAC算法参数；

步骤2：随机选择一个样本来拟合单应矩阵；

步骤3：使用估计的单应矩阵将源点映射到目标点；

步骤5：判断是否满足终止迭代的阈值条件，若是，则结束算法，若否，则返回至步骤2继续执行；

所述双目相机位姿增量，根据公式（2）求解得到；

2.如权利要求1所述的一种在线单舵轮AGV参数标定方法，其特征在于，所述得到与相机数据的时间戳对应的轮式里程计数据,具体为：对于获取的轮式里程计数据，判断最近的轮式里程计数据时间戳是否大于当前帧图像数据时间戳，若否，则舍弃并等待新的轮式里程计数据；若是，则对时间进行离散化，并根据相邻两帧图像数据时间戳对里程计数据进行线性插值。

3.如权利要求1所述的一种在线单舵轮AGV参数标定方法，其特征在于，单舵轮的内参舵轮转角零漂以及单舵轮半径/>，具体表示如下：

4.如权利要求1所述的一种在线单舵轮AGV参数标定方法，其特征在于，单舵轮的外参基于双目相机位姿增量、单舵轮AGV位姿增量/>以及双目相机的安装位姿/>三者的关系获得，所述关系表示如下：

其中，为李代数上的标准复合算子。

5.一种在线单舵轮AGV参数标定系统，其特征在于，包括：

相机位姿增量计算单元，其用于获取相邻两帧图像的信息，计算两帧图像数据之间双目相机的位姿增量；其中，所述图像的获取采用设置于单舵轮AGV上的双目相机；

外参标定单元，其用于基于双目相机位姿增量的平移增量、单舵轮AGV位姿增量的平移增量以及双目相机的安装位姿三者的关系，计算得到单舵轮的外参；

所述PROSAC算法具体包括如下步骤：

步骤1：设置PROSAC算法参数；

步骤2：随机选择一个样本来拟合单应矩阵；

步骤3：使用估计的单应矩阵将源点映射到目标点；

所述双目相机位姿增量，根据公式（2）求解得到；

6.一种电子设备，包括存储器、处理器及存储在存储器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-4任一项所述的一种在线单舵轮AGV参数标定方法。

7.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-4任一项所述的一种在线单舵轮AGV参数标定方法。