CN115752507A - 基于二维码导航的在线单舵轮agv参数标定方法及*** - Google Patents

Abstract

本发明提供了一种基于二维码导航的在线单舵轮AGV参数标定方法及***，根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量；根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量；根据两次检测到二维码时的时间戳对激光雷达数据进行插值，得到根据激光雷达计得到的第三位姿增量；根据第一位姿增量和第二位姿增量进行内参标定，根据第一位姿增量和第三位姿增量进行外参标定；本发明结合利用二维码获取的AGV位姿增量进行内外参标定，相比单纯使用激光雷达获取的位姿增量进行标定的策略，准确率更高。

Description

基于二维码导航的在线单舵轮AGV参数标定方法及***

技术领域

本发明涉及AGV导航控制技术领域，特别涉及一种基于二维码导航的在线单舵轮AGV参数标定方法及***。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。

对于轮式里程计与激光雷达的内外参数，由于机械制造、安装误差以及机器人长期工作导致装置变形及轮子磨损等因素，导致轮式里程计的内置参数以及轮式里程计与雷达之间的外置参数会发生改变。

发明人发现，已有的单舵轮模型都是基于激光雷达进行离线标定，误差较大、效率低、精度低，无法满足室内环境下的AGV自动化实时高精度标定要求。

发明内容

为了解决现有技术的不足，本发明提供了一种基于二维码导航的在线单舵轮AGV参数标定方法及***，结合利用二维码获取的AGV位姿增量进行内外参标定，相比单纯使用激光雷达获取的位姿增量进行标定的策略，准确率更高。

为了实现上述目的，本发明采用如下技术方案：

本发明第一方面提供了一种基于二维码导航的在线单舵轮AGV参数标定方法。

一种基于二维码导航的在线单舵轮AGV参数标定方法，包括以下过程：

获取相邻两次检测得到的AGV相对于二维码的位姿以及二维码的编号；

根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量；

根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量；

根据两次检测到二维码时的时间戳对激光雷达数据进行插值，得到根据激光雷达计得到的第三位姿增量；

根据第一位姿增量和第二位姿增量进行内参标定，根据第一位姿增量和第三位姿增量进行外参标定。

本发明第二方面提供了一种基于二维码导航的在线单舵轮AGV参数标定***。

一种基于二维码导航的在线单舵轮AGV参数标定***，包括：

数据获取模块，被配置为：获取相邻两次检测得到的AGV相对于二维码的位姿以及二维码的编号；

第一位姿增量计算模块，被配置为：根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量；

第二位姿增量计算模块，被配置为：根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量；

第三位姿增量计算模块，被配置为：根据两次检测到二维码时的时间戳对激光雷达数据进行插值，得到根据激光雷达计得到的第三位姿增量；

内外参标定模块，被配置为：根据第一位姿增量和第二位姿增量进行内参标定，根据第一位姿增量和第三位姿增量进行外参标定。

本发明第三方面提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方面所述的基于二维码导航的在线单舵轮AGV参数标定方法中的步骤。

本发明第四方面提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的基于二维码导航的在线单舵轮AGV参数标定方法中的步骤。

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

1、本发明所述的基于二维码导航的在线单舵轮AGV参数标定方法及***，结合利用二维码获取的AGV位姿增量进行内外参标定，相比单纯使用激光雷达获取的位姿增量进行标定的策略，准确率更高。

2、本发明所述的基于二维码导航的在线单舵轮AGV参数标定方法及***，采用在线标定的策略，将标定结果用于后续的地图构建，可以实时更新参数，实时校正误差，避免了由于轮式里程计参数不准、轮子磨损以及雷达相对于机器人中心安装位置不准给建图和定位带来的误差。

3、本发明所述的基于二维码导航的在线单舵轮AGV参数标定方法及***，假定参数的标称值是先验已知的，估计的调整量相对较小，线性误差较小，可以在无人看管的情况下运行，无需人工干预，没有任何仪器需要预先校准，不需要标称参数作为初始猜测，全局最优解以封闭形式存在，机器人的轨迹可以自由选择。

4、本发明所述的基于二维码导航的在线单舵轮AGV参数标定方法及***，可以先标定出内参，然后再使用标定出的内参进行更加精确的航迹推算，利用精确的航迹推算推测出的轮式里程计位姿作为雷达位姿计算的先验位姿，推测出的雷达位姿增量准确率更高。

附图说明

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

图1为本发明实施例1提供的AGV结构示意图；

图2为本发明实施例1提供的基于二维码导航的在线单舵轮AGV参数标定方法的流程示意图；

图3为本发明实施例1提供的轮式里程计航迹推算示意图；

图4为本发明实施例1提供的轮式里程计位姿变换示意图；

图5为本发明实施例1提供的坐标系转换示意图；

其中，1-舵轮；2-万向轮。

具体实施方式

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

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

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

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

实施例1：

本发明实施例1提供了一种基于二维码导航的在线单舵轮AGV参数标定方法，对图1所示的单舵轮AGV的以下参数进行估计：

(1)内在参数：舵轮的半径r₁以及舵轮转角零漂β₁；

(2)外在参数：激光雷达的安装位姿参数l＝[lx，ly，lθ]，其中lx，ly，lθ分别为激光雷达在AGV坐标系的x坐标、y坐标和方向角。

该AGV具有的一个舵轮1，同时具备转向和驱动能力，通过设定速度和转向角度来驱动速度和控制，还有四个从动万向轮2；

AGV参考系{R}放在AGV的读码器中心点O处，以机器人前向为x轴正方向，舵轮分布于AGV的x轴上，整体的位姿和速度在逻辑上由坐标系原点的运动状态来描述；

世界坐标系{W}在环境中是固定的，激光雷达坐标系{S}固定在车体上，因此它的相对姿势随时间变化是保持恒定的，令p＝[px，py，pθ]表示AGV坐标系{R}在世界坐标系{W}中的位置和方向的状态向量；

l＝[lx，ly，lθ]是传感器坐标系{S}相对于AGV坐标系{R}的恒定相对位姿，该AGV通过二维码定位和激光雷达融合定位，在大范围内使用激光雷达定位，在需要精确定位或者激光雷达效果较差的杂乱地方使用二维码定位，实现AGV的精准定位。

如图2所示，所述的标定方法，包括以下过程：

S1：检测到二维码之后，根据读码器读取的信息计算AGV相对于二维码的相对位姿以及二维码的编号；

S2：根据二维码的编号查询事先存储的对应编号二维码的在世界坐标系下的位姿信息；

S3：由于读码器读取的是AGV相对于二维码的位姿不在同一坐标系下，所以需要判断当前检测到的二维码和上一次检测到的二维码是否为同一个；

如果是，则两次的由二维码读取的AGV位姿是基于同一个坐标系，可以直接计算位姿增量d^k(即第一位姿增量)；否则，需要将两次的AGV相对于二维码的位置信息都转换到世界坐标系下，然后才能计算位姿增量d^k(即第一位姿增量)；

S4：根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，计算两时间戳之间的根据轮式里程计得出的AGV位姿增量r^k(即第二位姿增量)；

S5：利用第一位姿增量d^k和第二位姿增量r^k计算内参；

S6：通过对激光雷达数据进行插值，获取插值后的激光雷达数据，根据插值后的激光雷达数据，计算当前时刻对应的根据激光雷达数据得到的激光雷达位姿；

S7：根据两次检测到二维码时间戳之间的激光雷达位姿，计算激光雷达位姿增量s^k(即第三位姿增量)；

S8：利用第一位姿增量d^k和第三位姿增量s^k计算外参。

具体的，通过二维码的检测结果计算AGV位姿，得到第一位姿增量d^k，包括：

(1)事先布置好二维码，并且将二维码在世界坐标系下的位姿信息存储下来；

(2)在AGV经过二维码附近时，通过专用的工业AGV定位二维码读码器可以获取AGV中心相对于二维码的位姿，同时读码器还可以读取二维码的编号等信息，从而获取该二维码在世界坐标系的位姿，结合AGV相对于二维码的位姿就可以得出AGV在世界坐标系下的位姿q^k；

(3)计算第k次检测到二维码时-机器人位姿q^k相对于上一次检测到二维码时机器人位姿q^k-1的位姿增量d^k。

轮式里程计的航迹推算，包括：

如图3所示，舵轮分布于AGV的x轴上，AGV旋转中心为O，AGV的运动可以理解为绕着瞬心O’以半径R做旋转运动，当其走直线的时候半径R＝∞，运动瞬心在无穷远处，轮的转角和轮子转速决定了AGV的运动方向。

单舵轮AGV的运动学模型为：

式中，v_x为AGV线速度在AGV坐标系x方向的分量，ω为AGV角速度，v₁为舵轮线速度，α₁为舵轮转向角，L为舵轮距离AGV中心的距离。

由于零漂β₁的存在，使得舵轮转向机构传感器反馈的数值不能反映当前舵轮的真实转角，从而对机器人的运动状态产生错误估计，考虑到零漂之后，式(1)变形为：

轮式里程计位姿变换计算流程如图4所示，获取轮式里程计数据，设定两次检测到二维码时的时间戳为第一时间戳和第二时间戳；

判断获取的最大轮式里程计数据时间戳是否大于当次检测到二维码时的时间戳，如否，则返回获取新的轮式里程计数据；如是，则进入下一步；

对轮式里程计数据进行离散化，根据第一时间戳和第二时间戳对离散化后的轮式里程计数据进行插值，计算第一时间戳和第二时间戳之间的根据轮式里程计得到的第二位姿增量r^k。

内参标定，包括：

如图5所示，为各坐标系的转换示意图，在相邻两帧数据之间，轮式里程计计算出的相对角度增量

等于在[t_k-1,t_k]时间间隔上的利用二维码得到旋转增量

方向增量

的表达式计算为：

假设转向角α₁(t)在时间间隔[t_k,t_k+1]内恒定，则上式可变形为：

式中，

为在时间间隔[t_k,t_k+1]内舵轮驱动轮转动角度，可以由反馈的编码器数据转换过来。

将标定参数与输入数据分开，将标定参数放到矩阵X中：

根据给定时间[t_k,t_k+1]内的n个轮式里程计传感器反馈数据(转向角α₁(t)、驱动轮转动角度θ₁)，由式(4)和

可以得出线性***AX＝b，其中A是k个轮式里程计传感器数据的矩阵形式：

式中，

表示t_k和t_k+1时间戳之间的轮式里程计数据的得出的驱动轮转动角度，

表示t_k和t_k+1时间戳之间的轮式里程计数据的得出的转向角。

其中，

为t_k和t_k+1时间戳之间由二维码定位得出的机器人位姿增量的旋转分量。

如果有两个以上的独立方程，则线性***是超定的，因此，为得到更好地满足给定条件的X值，需计算最小化误差‖AX-b‖²，得出的通解X^*＝arg min‖AX-b‖²，可以通过计算矩阵A的Moore-Penrose伪逆来解，则计算出的内参为：

表示通解X^*的第i行元素，1≤i≤2。

外参标定，包括：

解决了内在参数标定后，可以使用内在参数标定外参，因为激光雷达通过定位算法计算自身位姿时，需要轮式里程计信息提供先验位姿，使用标定完的内参得出来的先验位姿更加准确，使得激光雷达的定位也更加准确。

s^k、d^k和l之间的关系可以用特殊欧几里德李代数se(2)上的标准复合算子

和逆算子

来表示：

则图5的位姿变换关系可表示为：

式中，k表示第k次检测到二维码，s^k由激光雷达数据通过激光雷达定位算法得到雷达位姿增量。

令：

构建平方和函数：

式中，

为第k帧数据误差

的位置分量

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

实施例2：

本发明实施例2提供了一种基于二维码导航的在线单舵轮AGV参数标定***，包括：

数据获取模块，被配置为：获取相邻两次检测得到的AGV相对于二维码的位姿以及二维码的编号；

第一位姿增量计算模块，被配置为：根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量；

第二位姿增量计算模块，被配置为：根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量；

第三位姿增量计算模块，被配置为：根据两次检测到二维码时的时间戳对激光雷达数据进行插值，得到根据激光雷达计得到的第三位姿增量；

内外参标定模块，被配置为：根据第一位姿增量和第二位姿增量进行内参标定，根据第一位姿增量和第三位姿增量进行外参标定。

所述***的工作方法与实施例1提供的基于二维码导航的在线单舵轮AGV参数标定方法相同，这里不再赘述。

实施例3：

本发明实施例3提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明实施例1所述的基于二维码导航的在线单舵轮AGV参数标定方法中的步骤。

实施例4：

本发明实施例4提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明实施例1所述的基于二维码导航的在线单舵轮AGV参数标定方法中的步骤。

本领域内的技术人员应明白，本发明的实施例可提供为方法、***、或计算机程序产品。因此，本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(RandomAccessMemory，RAM)等。

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

Claims (10)

1.一种基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，包括以下过程：

获取相邻两次检测得到的AGV相对于二维码的位姿以及二维码的编号；

根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量；

根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量；

根据两次检测到二维码时的时间戳对激光雷达数据进行插值，得到根据激光雷达计得到的第三位姿增量；

根据第一位姿增量和第二位姿增量进行内参标定，根据第一位姿增量和第三位姿增量进行外参标定。

2.如权利要求1所述的基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，

根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量，包括：

根据二维码的编号确定二维码在世界坐标系下的位姿；

当两次检测到的二维码为同一个时，根据两次检测得到的AGV相对于二维码的位姿以及二维码在世界坐标系下的位姿，得到根据二维码得到的第一位姿增量；

当当两次检测到的二维码不为同一个时，将两次检测得到的AGV相对于二维码的位姿转换到世界坐标系下，得到根据二维码得到的第一位姿增量。

3.如权利要求1所述的基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，

根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量，包括：

获取轮式里程计数据，设定两次检测到二维码时的时间戳为第一时间戳和第二时间戳；

判断获取的最大轮式里程计数据时间戳是否大于当次检测到二维码时的时间戳，如否，则返回获取新的轮式里程计数据；如是，则进入下一步；

对轮式里程计数据进行离散化，根据第一时间戳和第二时间戳对离散化后的轮式里程计数据进行插值，计算第一时间戳和第二时间戳之间的根据轮式里程计得到的第二位姿增量。

4.如权利要求3所述的基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，

根据第二位姿增量，得到第二相对角度增量，第二相对角度增量等于在相邻两次二维码时间戳之间利用二维码得到的旋转增量。

5.如权利要求4所述的基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，

所述内参，包括：舵轮的半径r₁以及舵轮转角零漂β₁；

其中，

表示通解X^*的第i行元素，1≤i≤2，X^*＝arg min‖AX-b‖²；

为t_k和t_k+1时间戳之间由二维码定位得出的机器人位姿增量的旋转分量，

表示t_k和t_k+1时间戳之间的轮式里程计数据的得出的驱动轮转动角度，

表示t_k和t_k+1时间戳之间的轮式里程计数据的得出的转向角，L为舵轮距离AGV中心的距离。

6.如权利要求1所述的基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，

所述外参，包括：激光雷达在AGV坐标系的位姿参数l＝(lx，ly，lθ)；

根据第一位姿增量和第三位姿增量进行外参标定，包括：

构建平方和函数：

利用小二乘法求解使E最小，得到(lx，ly，lθ)，

为第k帧激光雷达数据的误差

的位置分量

T为转置。

7.如权利要求6所述的基于二维码导航的在线单舵轮AGV参数标定方法，其特征在于，

其中，s^k为第三位姿增量，d^k为第一位姿增量。

8.一种基于二维码导航的在线单舵轮AGV参数标定***，其特征在于，包括以下过程：

数据获取模块，被配置为：获取相邻两次检测得到的AGV相对于二维码的位姿以及二维码的编号；

第一位姿增量计算模块，被配置为：根据获取的AGV相对于二维码的位姿以及二维码的编号，得到根据二维码得到的第一位姿增量；

第二位姿增量计算模块，被配置为：根据两次检测到二维码时的时间戳对轮式里程计数据进行插值，得到根据轮式里程计得到的第二位姿增量；

第三位姿增量计算模块，被配置为：根据两次检测到二维码时的时间戳对激光雷达数据进行插值，得到根据激光雷达计得到的第三位姿增量；

内外参标定模块，被配置为：根据第一位姿增量和第二位姿增量进行内参标定，根据第一位姿增量和第三位姿增量进行外参标定。

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

10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7任一项所述的基于二维码导航的在线单舵轮AGV参数标定方法中的步骤。

Images