CN115435816A - 在线双舵轮agv内外参标定方法、系统、介质及设备 - Google Patents

在线双舵轮agv内外参标定方法、系统、介质及设备 Download PDF

Info

Publication number
CN115435816A
CN115435816A CN202211381472.7A CN202211381472A CN115435816A CN 115435816 A CN115435816 A CN 115435816A CN 202211381472 A CN202211381472 A CN 202211381472A CN 115435816 A CN115435816 A CN 115435816A
Authority
CN
China
Prior art keywords
point cloud
wheel
laser radar
data
steering
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202211381472.7A
Other languages
English (en)
Other versions
CN115435816B (zh
Inventor
周军
付周
李文广
李留昭
杨旭浩
李昭
马永鑫
李新宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong University
Original Assignee
Shandong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shandong University filed Critical Shandong University
Priority to CN202211381472.7A priority Critical patent/CN115435816B/zh
Publication of CN115435816A publication Critical patent/CN115435816A/zh
Application granted granted Critical
Publication of CN115435816B publication Critical patent/CN115435816B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明提供了一种在线双舵轮AGV内外参标定方法、系统、介质及设备，属于双舵轮AGV智能导航控制技术领域，解决了离线标定效率较低的问题。标定方法，包括：计算当前帧激光雷达位姿，将当前帧激光雷达位姿减去上一帧激光雷达位姿，得到激光雷达位姿增量；获取与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；根据激光雷达位姿增量标定双舵轮AGV的内参，根据当前帧激光雷达点云数据、轮式里程计数据以及双舵轮AGV的内参，标定双舵轮AGV的外参。本发明采用在线标定的方式，将标定结果用于后续的地图构建，可以实时更新参数，实时校正误差，提高了在线双舵轮AGV的工作效率。

Description

在线双舵轮AGV内外参标定方法、系统、介质及设备
技术领域
本发明涉及双舵轮AGV智能导航控制技术领域，特别涉及一种在线双舵轮AGV内外参标定方法、系统、介质及设备。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。
轮式里程计与激光雷达的内外参数，由于机械制造、安装误差以及双舵轮AGV（Automated Guided Vehicle）长期工作导致装置变形及轮子磨损等因素，导致轮式里程计的内置参数以及轮式里程计与激光雷达之间的外置参数发生改变，会对双舵轮AGV建图与导航定位有重大影响，因此，需要对两者进行定期标定。
发明人发现，现有常见标定方法大都是由标定人员手动控制小车行驶一定的距离，通过构建轮式里程计与激光雷达的关系并采用非线性求解获取两者相对的外部参数，再通过实际测量得到的位置与角度变化以及轮式里程计的两轮角速度，线性计算轮式里程计内置参数，上述方法是在离线条件下进行的，效率低、精度低，无法实现双舵轮AGV内外参的自动在线标定。
发明内容
为了解决现有技术的不足，本发明提供了一种在线双舵轮AGV内外参标定方法、系统、介质及设备，采用在线标定的方法，将标定结果用于后续的地图构建，可以实时更新参数，实时校正误差，避免了由于轮式里程计参数不准、轮子磨损以及激光雷达相对于双舵轮AGV中心安装位置不准给建图和定位带来的误差。
为了实现上述目的，本发明采用如下技术方案：
本发明第一方面提供了一种在线双舵轮AGV内外参标定方法。
一种在线双舵轮AGV内外参标定方法，包括以下过程：
获取当前帧激光雷达点云数据，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，将当前帧激光雷达位姿减去上一帧激光雷达位姿，得到激光雷达位姿增量；其中，上一帧激光雷达位姿根据上一帧激光雷达点云数据与栅格地图得到；
根据当前帧激光雷达点云数据对轮式里程计数据进行插值，得到与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；
根据激光雷达位姿增量以及插值后的轮式里程计数据标定双舵轮AGV的内参，根据激光雷达位姿增量、基于插值后的轮式里程计数据计算出的双舵轮AGV位姿增量以及双舵轮AGV的内参，标定双舵轮AGV的外参。
作为本发明第一方面可选的一种实现方式，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，包括：
设当前帧激光雷达点云数据为P，获取当前帧激光雷达点云数据P后，根据未插值处理的轮式里程计数据得到激光雷达的先验位姿，将当前帧激光雷达点云数据P旋转到先验位姿处得到点云P0；
将点云P0以设定角度分辨率m1为步长顺时针旋转n次，再将点云P0以一定角度分辨率m1为步长逆时针旋转n次，得到一个包含2n个点云的点云集合V；
将点云集合V里的每个点云分别沿X轴以一定步长step平移j以及沿Y轴以一定步长step平移k次，平移后的点云位于搜索窗口内，得到包含2n*j*k个点云的新点云集合V1；
获取新点云集合V1中的各点云每个点的栅格坐标，并通过栅格坐标获取对应栅格的占用概率，新点云集合V1中的任一点云得分为此点云中每个点对应栅格的占用概率之和除以此点云中点的个数，得到新点云集合V1中的点云最高得分；
判断新点云集合V1中的点云最高得分是否大于判定匹配成功的最低阈值，如是，则新点云集合V1中得分最高的点云对应的激光雷达坐标系原点的位姿为当前帧激光雷达位姿，如否，扩大搜索窗口回到将点云P0以设定角度分辨率m1为步长顺时针旋转n次的步骤继续执行。
作为本发明第一方面可选的一种实现方式，内参数包括：前舵轮的半径r 1 、后舵轮的半径r 2 、前舵轮转角零漂β 1 和后舵轮转角零漂β 2 ，内参数的标定，包括：
Figure 938001DEST_PATH_IMAGE001
；
Figure 641515DEST_PATH_IMAGE002
；
Figure 593290DEST_PATH_IMAGE003
；
Figure 331439DEST_PATH_IMAGE004
；
其中，L为前后舵轮连线长度的1/2，atn2(·)为返回指定值的反正切函数，
Figure 710468DEST_PATH_IMAGE005
分别为X*列向量的第一、第二、第三和第四行的元素，
Figure 901278DEST_PATH_IMAGE006
；
其中，
Figure 656744DEST_PATH_IMAGE007
；
Figure 514979DEST_PATH_IMAGE008
；
式中，
Figure 64909DEST_PATH_IMAGE009
表示根据第k帧插值后的轮式里程计数据得出的前轮驱动轮转动角度，
Figure 8594DEST_PATH_IMAGE010
表示根据第k帧插值后的轮式里程计数据得出的后轮驱动轮转动角度，
Figure 36593DEST_PATH_IMAGE011
表示根据第k帧插值后的轮式里程计数据得出的前轮转向角，
Figure 749334DEST_PATH_IMAGE012
表示根据第k帧插值后的轮式里程计数据得出的后轮转向角，b为常数。
作为本发明第一方面可选的一种实现方式，双舵轮AGV的外参数为：激光雷达的位姿参数；
双舵轮AGV的外参数的标定，包括：
将外参数作为输出数据，将插值后的轮式里程计数据作为输入数据，构建最小二乘问题，通过求解最小二乘问题标定外参。
本发明第二方面提供了一种在线双舵轮AGV内外参标定系统。
一种在线双舵轮AGV内外参标定系统，包括：
激光雷达位姿增量计算模块，被配置为：获取当前帧激光雷达点云数据，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，将当前帧激光雷达位姿减去上一帧激光雷达位姿，得到激光雷达位姿增量；其中，上一帧激光雷达位姿根据上一帧激光雷达点云数据与栅格地图得到；
轮式里程计数据获取模块，被配置为：根据当前帧激光雷达点云数据对轮式里程计数据进行插值，得到与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；
内外参标定模块，被配置为：根据激光雷达位姿增量以及插值后的轮式里程计数据标定双舵轮AGV的内参，根据激光雷达位姿增量、基于插值后的轮式里程计数据计算出的双舵轮AGV位姿增量以及双舵轮AGV的内参，标定双舵轮AGV的外参。
作为本发明第二方面可选的一种实现方式，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，包括：
设当前帧激光雷达点云数据为P，获取当前帧激光雷达点云数据P后，根据未插值处理的轮式里程计数据得到激光雷达的先验位姿，将当前帧激光雷达点云数据P旋转到先验位姿处得到点云P0；
将点云P0以设定角度分辨率m1为步长顺时针旋转n次，再将点云P0以一定角度分辨率m1为步长逆时针旋转n次，得到一个包含2n个点云的点云集合V；
将点云集合V里的每个点云分别沿X轴以一定步长step平移j以及沿Y轴以一定步长step平移k次，平移后的点云位于搜索窗口内，得到包含2n*j*k个点云的新点云集合V1；
获取新点云集合V1中的各点云每个点的栅格坐标，并通过栅格坐标获取对应栅格的占用概率，新点云集合V1中的任一点云得分为此点云中每个点对应栅格的占用概率之和除以此点云中点的个数，得到新点云集合V1中的点云最高得分；
判断新点云集合V1中的点云最高得分是否大于判定匹配成功的最低阈值，如是，则新点云集合V1中得分最高的点云对应的激光雷达坐标系原点的位姿为当前帧激光雷达位姿，如否，扩大搜索窗口回到将点云P0以设定角度分辨率m1为步长顺时针旋转n次的步骤继续执行。
作为本发明第二方面可选的一种实现方式，内参数包括：前舵轮的半径r 1 、后舵轮的半径r 2 、前舵轮转角零漂β 1 和后舵轮转角零漂β 2 ，内参数的标定，包括：
Figure 735744DEST_PATH_IMAGE001
；
Figure 901146DEST_PATH_IMAGE013
；
Figure 998415DEST_PATH_IMAGE003
；
Figure 300084DEST_PATH_IMAGE004
；
其中，L为前后舵轮连线长度的1/2，atn2(·)为返回指定值的反正切函数，
Figure 457396DEST_PATH_IMAGE005
分别为X*列向量的第一、第二、第三和第四行的元素，
Figure 110094DEST_PATH_IMAGE006
；
其中，
Figure 745475DEST_PATH_IMAGE007
；
Figure 432808DEST_PATH_IMAGE014
；
式中，
Figure 761021DEST_PATH_IMAGE009
表示根据第k帧插值后的轮式里程计数据得出的前轮驱动轮转动角度，
Figure 635436DEST_PATH_IMAGE010
表示根据第k帧插值后的轮式里程计数据得出的后轮驱动轮转动角度，
Figure 340087DEST_PATH_IMAGE011
表示根据第k帧插值后的轮式里程计数据得出的前轮转向角，
Figure 616347DEST_PATH_IMAGE012
表示根据第k帧插值后的轮式里程计数据得出的后轮转向角，b为常数。
作为本发明第二方面可选的一种实现方式，双舵轮AGV的外参数为：激光雷达的位姿参数；
双舵轮AGV的外参数的标定，包括：
将外参数作为输出数据，将插值后的轮式里程计数据作为输入数据，构建最小二乘问题，通过求解最小二乘问题标定外参。
本发明第三方面提供了一种计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如本发明第一方面所述的在线双舵轮AGV内外参标定方法中的步骤。
本发明第四方面提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的在线双舵轮AGV内外参标定方法中的步骤。
与现有技术相比，本发明的有益效果是：
1、本发明所述的在线双舵轮AGV内外参标定方法，通过轮式里程计数据推算出一个先验位姿，从先验位姿附近范围内进行扫描匹配，相对全局范围盲目的扫描匹配，减小了计算量和时间，提高了效率。
2、本发明所述的在线双舵轮AGV内外参标定方法，采用在线标定的方式，将标定结果用于后续的地图构建，可以实时更新参数，实时校正误差，避免了由于轮式里程计参数不准、轮子磨损以及激光雷达相对于双舵轮AGV中心安装位置不准给建图和定位带来的误差。
3、本发明所述的在线双舵轮AGV内外参标定方法，假定参数的标称值是先验已知的，估计的调整量相对较小，线性误差较小，可以在无人看管的情况下运行，无需人工干预，没有任何仪器需要预先校准，不需要标称参数作为初始猜测，全局最优解以封闭形式存在，只要激活双舵轮AGV所有的参数，双舵轮AGV的轨迹可以自由选择。
本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
图1为本发明实施例1提供的双舵轮AGV的结构示意图；
图2为本发明实施例1提供的内外参标定的方法的流程示意图；
图3为本发明实施例1提供的扫描匹配原理示意图；
图4为本发明实施例1提供的激光点云帧间匹配方法的流程示意图；
图5为本发明实施例1提供的变换到先验位姿处的点云P0示意图；
图6为本发明实施例1提供的将点云P0按一定步长旋转后得到的点云集合V的示意图；
图7为本发明实施例1提供的将点云集合V做平移变换后得到的新点云集合V1示意图；
图8为本发明实施例1提供的双舵轮AGV转向模型示意图；
图9为本发明实施例1提供的各坐标系关系示意图；
图10为本发明实施例1提供的轮式里程计位姿变换计算流程示意图；
其中，1-舵轮；2-万向轮。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
实施例1：
本发明实施例1提供了一种在线双舵轮AGV内外参标定方法，对图1所示的双舵轮全向双舵轮AGV的以下参数进行估计（其中1为舵轮，2为万向轮）：
（1）内在参数：前舵轮半径r 1 、后舵轮半径r 2 以及前舵轮转角零漂β 1 、后舵轮转角零漂β 2 ；
（2）外在参数：激光雷达的位姿参数
Figure 115462DEST_PATH_IMAGE015
；其中
Figure 477173DEST_PATH_IMAGE016
分别为激光雷达在双舵轮AGV坐标系的x坐标、y坐标和方向角。
本实施例中，双舵轮AGV的前后轮均为独立控制的舵轮，同时具备转向驱动，通过设定转向角度和速度来驱动和控制，前后轮分布在双舵轮AGV的中轴线上；
双舵轮AGV坐标系{R}放在前后舵轮连线的中点处，即双舵轮AGV的参考点处，整体的位姿和速度在逻辑上由坐标系原点的运动状态来描述；
世界坐标系{W}在环境中是固定的，激光雷达坐标系{S}固定在双舵轮AGV的车体上，因此它的相对姿势随时间变化是保持恒定的，
Figure 454356DEST_PATH_IMAGE017
是激光雷达坐标系{S}相对于双舵轮AGV坐标系{R}的恒定相对姿态。
具体的标定方法如图2所示，包括：
获取当前帧激光雷达点云数据，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得出当前帧激光雷达点云数据计算出的当前帧激光位姿P k ，将当前帧激光位姿减去上一帧激光位姿P k-1 ，得到由当前帧激光雷达点云数据计算出的相对于上一帧激光雷达位姿的位姿增量s k ；其中，上一帧激光雷达位姿根据上一帧激光雷达点云数据与栅格地图得到；
获取到时间戳晚于最新激光雷达点云数据的轮式里程计数据之后，根据当前帧激光雷达点云数据的时间戳对轮式里程计数据进行插值，得到与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；
由当前帧激光雷达点云数据计算出的位姿增量s k 标定双舵轮AGV的内参r 1 、r 2 、β 1 、β 2 ；
利用上一步标定出的内参、由当前帧激光雷达点云数据点云匹配计算出的激光雷达位姿增量s k 、基于插值后的轮式里程计数据计算出的双舵轮AGV位姿增量r k 以及两个位姿增量之间的变换关系标定出双舵轮AGV外参，标定完所有参数之后，将标定出的参数作为后续建图以及定位时的参数输入，参与后续的地图构建和点云匹配。
更具体的，包括以下过程：
S1：激光点云帧间匹配
在栅格地图中真实坐标通过栅格坐标表示，每个栅格有一个占用概率possibility，障碍物所处区域对应的栅格的占用概率值会比较高，而空旷区域对应的栅格区域占用概率会比较低。扫描匹配的原理就是将点云数据在栅格地图地面不断做平移、旋转变换，获取变换后的点云中的每个点对应栅格的占用概率，将所有点的占用概率求和除以点的总数即为点云的得分，得分最大的点云对应的位姿即为该帧点云对应的实际位姿。
如图3所示，粗线栅格即为有障碍物的区域，该区域栅格占用概率比较高，左侧所示的激光雷达点云位姿与实际环境并不匹配，点云的得分就较低，右侧所示的激光雷达点云位姿与实际环境更加匹配，得分较高，就可以作为该帧点云的实际位姿。由于实际栅格地图可能比较大，如果直接在整个地图进行扫描匹配会比较耗费时间，因此需要先根据轮式里程计数据推算出一个先验位姿，再从先验位姿周围范围内进行扫描匹配，如果在该范围内匹配不成功，再扩大搜索范围进行扫描匹配。
如图4所示，具体步骤如下：
步骤1：设当前帧激光雷达点云数据为P，获取当前帧激光雷达点云数据P后，此时的点云坐标是相对于激光雷达坐标系的，根据未插值处理的轮式里程计数据得到激光雷达的先验位姿，将当前帧激光雷达点云数据P旋转到先验位姿处得到点云P0，如图5；
步骤2：将点云P0以一定角度分辨率m1为步长顺时针旋转n次，再将点云P0以一定角度分辨率m1为步长逆时针旋转n次，得到一个包含2n个点云的点云集合V，如图6；
步骤3：将点云集合V里的每个点云分别沿X轴、Y轴以一定步长step分别平移j、k次，保证平移后的点云位于搜索窗口内，得到包含2n*j*k个点云的新点云集合V1，如图7；
步骤4：获取新点云集合V1中点云每个点的栅格坐标，并通过栅格坐标获取对应栅格的占用概率possibility，新点云集合V1中的任一点云的得分score为此点云中所有点对应栅格的占用概率possibility之和除以此点云中点的个数，即score=∑possibility/N，N为点云中点的个数，得到新点云集合V1中的点云最高得分；
步骤5：判断新点云集合V1中的点云最高得分是否大于判定匹配成功的最低阈值（自定义设定的最低阈值），如是，则新点云集合V1中得分最高的点云对应的激光雷达坐标系原点的位姿即为当前帧激光雷达位姿，如否，则扩大搜索窗口回到步骤3；
步骤6：计算当前帧激光雷达位姿与前一帧激光雷达位姿之间的位姿变换
Figure 850703DEST_PATH_IMAGE018
（即由当前帧激光雷达点云数据计算出的激光雷达位姿增量）。
S2：轮式里程计航迹推算
如图8所示，前后舵轮连线的中点为双舵轮AGV的参考点C，双舵轮AGV旋转中心为O’（即瞬心），双舵轮AGV的运动可以理解为绕着图8的瞬心O’做旋转运动，当其走直线的时候半径R=∞，运动瞬心在无穷远处，两个舵轮的转角和轮子转速决定了双舵轮AGV的运动方向。
双舵轮AGV的运动学模型为：
Figure 520718DEST_PATH_IMAGE019
（1）
式中，v x 为双舵轮AGV线速度在x方向的分量，v y 为线速度在y方向的分量，ω为双舵轮AGV的角速度，ω1为前轮角速度，ω2为后轮角速度，v 1 为前轮线速度，v 2 为后轮线速度，α 1 为前轮偏角，α 2 为后轮偏角，L为前后舵轮连线长度的1/2，r 1 为前舵轮半径，r 2 为后舵轮半径。
由于零漂β 1 、β 2 的存在，使得舵轮转向机构传感器反馈的数值不能反映当前舵轮的真实转角，从而对双舵轮AGV的运动状态产生错误估计，考虑到零漂之后，式（1）变形为：
Figure 369726DEST_PATH_IMAGE020
（2）
如图9，在相邻两帧数据之间，根据插值后的轮式里程计数据计算出的轮式里程计位姿增量r k 等于在
Figure 885021DEST_PATH_IMAGE021
时间间隔上的激光雷达位姿增量s k 。
轮式里程计位姿增量r k 计算流程如图10所示。
S3：内参标定
根据插值后的轮式里程计数据计算出的轮式里程计旋转增量
Figure 401453DEST_PATH_IMAGE022
的表达式计算为：
Figure 976791DEST_PATH_IMAGE023
（3）
其中，ω 1 (t)为t时刻的前轮角速度，ω 2 (t)为t时刻的后轮角速度，α 1 (t)为t时刻的前轮偏角，α 2 (t)为t时刻的后轮偏角。
假设转向角α1(t)，α2(t)在时间间隔
Figure 578673DEST_PATH_IMAGE024
内恒定，则上式可变形为：
Figure 897659DEST_PATH_IMAGE025
（4）
式中，
Figure 268598DEST_PATH_IMAGE026
分别为时间间隔
Figure 14837DEST_PATH_IMAGE027
内前轮驱动轮转动角度、后轮驱动轮转动角度，可以由反馈的编码器数据转换过来。
将标定参数与输入数据分开，将标定参数放到矩阵X中：
Figure 572857DEST_PATH_IMAGE028
（5）
根据给定时间
Figure 961113DEST_PATH_IMAGE029
内传感器反馈数据（前轮转向角α1，后轮转向角α2，前轮驱动轮转动角度θ1、后轮驱动轮转动角度θ2），由式（4）可以得出线性系统AX=b（AX=b是式（4）的矩阵形式，b为常数），A是k个轮式里程计数据的矩阵形式，其中：
Figure 920979DEST_PATH_IMAGE030
（6）
式中，
Figure 103698DEST_PATH_IMAGE031
表示根据第k帧插值后的轮式里程计数据得出的前轮驱动轮转动角度，
Figure 414594DEST_PATH_IMAGE032
表示根据第k帧插值后的轮式里程计数据得出的后轮驱动轮转动角度，
Figure 340962DEST_PATH_IMAGE033
表示根据第k帧插值后的轮式里程计数据得出的前轮转向角，
Figure 155334DEST_PATH_IMAGE034
表示根据第k帧插值后的轮式里程计数据得出的后轮转向角，如果有两个以上的独立方程，则线性系统是超定的。因此，为得到更好地满足给定条件的X值，需计算最小化误差
Figure 774534DEST_PATH_IMAGE035
，得出的通解
Figure 307147DEST_PATH_IMAGE036
，可以通过计算矩阵A的Moore-Penrose伪逆来解，则计算出的内参为：
Figure 771626DEST_PATH_IMAGE001
（7）
Figure 706084DEST_PATH_IMAGE013
（8）
Figure 496185DEST_PATH_IMAGE037
（9）
Figure 516094DEST_PATH_IMAGE004
（10）。
S4：外参标定
解决了内在参数标定后，可以使用内在参数标定外参，
Figure 49844DEST_PATH_IMAGE038
和l之间的关系可以用特殊欧几里德李代数se(2)上的标准复合算子⊕来表示，则图9的位姿变换关系可表示为：
Figure 838808DEST_PATH_IMAGE039
（11）
式中，k表示第k帧数据，r k 的旋转分量
Figure 268652DEST_PATH_IMAGE040
已在前面内参标定时计算得出，r k 的位移分量
Figure 41436DEST_PATH_IMAGE041
和
Figure 847718DEST_PATH_IMAGE042
的计算方法和
Figure 756768DEST_PATH_IMAGE040
的计算方法类似：
Figure 888672DEST_PATH_IMAGE043
（12）
式中
Figure 617594DEST_PATH_IMAGE044
分别表示在第k帧激光雷达点云数据和第k+1帧激光雷达点云数据之间的前、后轮转向角，
Figure 493146DEST_PATH_IMAGE045
分别表示在第k帧激光雷达点云数据和第k+1帧激光雷达点云数据之间的前、后驱动轮转过的角度。
令：
Figure 256703DEST_PATH_IMAGE046
（13）
Figure 293929DEST_PATH_IMAGE047
为根据当前帧激光雷达点云数据得出的位姿与插值后的轮式里程计数据得出的位姿的误差，构建平方和函数：
Figure 775726DEST_PATH_IMAGE048
（14）
式中，
Figure 189390DEST_PATH_IMAGE049
为e k 的位移分量，T为矩阵的转置，n为激光雷达点云的总帧数。
利用最小二乘法求解
Figure 807453DEST_PATH_IMAGE050
使平方和函数E最小，从而标定外参
Figure 750001DEST_PATH_IMAGE051
。
实施例2：
本发明实施例2提供了一种在线双舵轮AGV内外参标定系统，包括：
激光雷达位姿增量计算模块，被配置为：获取当前帧激光雷达点云数据，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，将当前帧激光雷达位姿减去上一帧激光雷达位姿，得到激光雷达位姿增量；其中，上一帧激光雷达位姿根据上一帧激光雷达点云数据与栅格地图得到；
轮式里程计数据获取模块，被配置为：根据当前帧激光雷达点云数据对轮式里程计数据进行插值，得到与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；
内外参标定模块，被配置为：根据激光雷达位姿增量以及插值后的轮式里程计数据标定双舵轮AGV的内参，根据激光雷达位姿增量、基于插值后的轮式里程计数据计算出的双舵轮AGV位姿增量以及双舵轮AGV的内参，标定双舵轮AGV的外参。
所述系统的工作方法与实施例1提供的在线双舵轮AGV内外参标定方法的流程示意图。
实施例3：
本发明实施例3提供了一种计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如本发明实施例1所述的在线双舵轮AGV内外参标定方法中的步骤。
实施例4：
本发明实施例4提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明实施例1所述的在线双舵轮AGV内外参标定方法中的步骤。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器和光学存储器等）上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备（系统）、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图中一个流程或多个流程和/或方框图中一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图中一个流程或多个流程和/或方框图中一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图中一个流程或多个流程和/或方框图中一个方框或多个方框中指定的功能的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体（Read-Only Memory，ROM）或随机存储记忆体（Random AccessMemory，RAM）等。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种在线双舵轮AGV内外参标定方法，其特征在于，
包括以下过程：
获取当前帧激光雷达点云数据，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，将当前帧激光雷达位姿减去上一帧激光雷达位姿，得到激光雷达位姿增量；其中，上一帧激光雷达位姿根据上一帧激光雷达点云数据与栅格地图得到；
根据当前帧激光雷达点云数据对轮式里程计数据进行插值，得到与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；
根据激光雷达位姿增量以及插值后的轮式里程计数据标定双舵轮AGV的内参，根据激光雷达位姿增量、基于插值后的轮式里程计数据计算出的双舵轮AGV位姿增量以及双舵轮AGV的内参，标定双舵轮AGV的外参。
2.如权利要求1所述的在线双舵轮AGV内外参标定方法，其特征在于，
将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，包括：
设当前帧激光雷达点云数据为P，获取当前帧激光雷达点云数据P后，根据未插值处理的轮式里程计数据得到激光雷达的先验位姿，将当前帧激光雷达点云数据P旋转到先验位姿处得到点云P0；
将点云P0以设定角度分辨率m1为步长顺时针旋转n次，再将点云P0以一定角度分辨率m1为步长逆时针旋转n次，得到一个包含2n个点云的点云集合V；
将点云集合V里的每个点云分别沿X轴以一定步长step平移j以及沿Y轴以一定步长step平移k次，平移后的点云位于搜索窗口内，得到包含2n*j*k个点云的新点云集合V1；
获取新点云集合V1中的各点云每个点的栅格坐标，并通过栅格坐标获取对应栅格的占用概率，新点云集合V1中的任一点云得分为此点云中每个点对应栅格的占用概率之和除以此点云中点的个数，得到新点云集合V1中的点云最高得分；
判断新点云集合V1中的点云最高得分是否大于判定匹配成功的最低阈值，如是，则新点云集合V1中得分最高的点云对应的激光雷达坐标系原点的位姿为当前帧激光雷达位姿，如否，扩大搜索窗口回到将点云P0以设定角度分辨率m1为步长顺时针旋转n次的步骤继续执行。
3.如权利要求1所述的在线双舵轮AGV内外参标定方法，其特征在于，
内参数包括：前舵轮的半径r 1 、后舵轮的半径r 2 、前舵轮转角零漂β 1 和后舵轮转角零漂β 2 ，内参数的标定，包括：
Figure 729335DEST_PATH_IMAGE001
；
Figure 40231DEST_PATH_IMAGE002
；
Figure 966599DEST_PATH_IMAGE003
；
Figure 780971DEST_PATH_IMAGE004
；
其中，L为前后舵轮连线长度的1/2，atn2(·)为返回指定值的反正切函数，
Figure 400171DEST_PATH_IMAGE005
分别为X*列向量的第一、第二、第三和第四行的元素，
Figure 932784DEST_PATH_IMAGE006
；
其中，
Figure 662842DEST_PATH_IMAGE007
；
Figure 331721DEST_PATH_IMAGE008
；
式中，
Figure 856243DEST_PATH_IMAGE009
表示根据第k帧插值后的轮式里程计数据得出的前轮驱动轮转动角度，
Figure 141731DEST_PATH_IMAGE010
表示根据第k帧插值后的轮式里程计数据得出的后轮驱动轮转动角度，
Figure 144322DEST_PATH_IMAGE011
表示根据第k帧插值后的轮式里程计数据得出的前轮转向角，
Figure 933287DEST_PATH_IMAGE012
表示根据第k帧插值后的轮式里程计数据得出的后轮转向角，b为常数。
4.如权利要求1所述的在线双舵轮AGV内外参标定方法，其特征在于，
双舵轮AGV的外参数为：激光雷达的位姿参数；
双舵轮AGV的外参数的标定，包括：
将外参数作为输出数据，将插值后的轮式里程计数据作为输入数据，构建最小二乘问题，通过求解最小二乘问题标定外参。
5.一种在线双舵轮AGV内外参标定系统，其特征在于，
包括：
激光雷达位姿增量计算模块，被配置为：获取当前帧激光雷达点云数据，将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，将当前帧激光雷达位姿减去上一帧激光雷达位姿，得到激光雷达位姿增量；其中，上一帧激光雷达位姿根据上一帧激光雷达点云数据与栅格地图得到；
轮式里程计数据获取模块，被配置为：根据当前帧激光雷达点云数据对轮式里程计数据进行插值，得到与当前帧激光雷达点云数据的时间戳对应的插值后的轮式里程计数据；
内外参标定模块，被配置为：根据激光雷达位姿增量以及插值后的轮式里程计数据标定双舵轮AGV的内参，根据激光雷达位姿增量、基于插值后的轮式里程计数据计算出的双舵轮AGV位姿增量以及双舵轮AGV的内参，标定双舵轮AGV的外参。
6.如权利要求5所述的在线双舵轮AGV内外参标定系统，其特征在于，
将当前帧激光雷达点云数据与已有的栅格地图进行扫描匹配，得到当前帧激光雷达位姿，包括：
设当前帧激光雷达点云数据为P，获取当前帧激光雷达点云数据P后，根据未插值处理的轮式里程计数据得到激光雷达的先验位姿，将当前帧激光雷达点云数据P旋转到先验位姿处得到点云P0；
将点云P0以设定角度分辨率m1为步长顺时针旋转n次，再将点云P0以一定角度分辨率m1为步长逆时针旋转n次，得到一个包含2n个点云的点云集合V；
将点云集合V里的每个点云分别沿X轴以一定步长step平移j以及沿Y轴以一定步长step平移k次，平移后的点云位于搜索窗口内，得到包含2n*j*k个点云的新点云集合V1；
获取新点云集合V1中的各点云每个点的栅格坐标，并通过栅格坐标获取对应栅格的占用概率，新点云集合V1中的任一点云得分为此点云中每个点对应栅格的占用概率之和除以此点云中点的个数，得到新点云集合V1中的点云最高得分；
判断新点云集合V1中的点云最高得分是否大于判定匹配成功的最低阈值，如是，则新点云集合V1中得分最高的点云对应的激光雷达坐标系原点的位姿为当前帧激光雷达位姿，如否，扩大搜索窗口回到将点云P0以设定角度分辨率m1为步长顺时针旋转n次的步骤继续执行。
7.如权利要求5所述的在线双舵轮AGV内外参标定系统，其特征在于，
内参数包括：前舵轮的半径r 1 、后舵轮的半径r 2 、前舵轮转角零漂β 1 和后舵轮转角零漂β 2 ，内参数的标定，包括：
Figure 894289DEST_PATH_IMAGE001
；
Figure 135915DEST_PATH_IMAGE013
；
Figure 207776DEST_PATH_IMAGE003
；
Figure 116826DEST_PATH_IMAGE004
；
其中，L为前后舵轮连线长度的1/2，atn2(·)为返回指定值的反正切函数，
Figure 717572DEST_PATH_IMAGE005
分别为X*列向量的第一、第二、第三和第四行的元素，
Figure 977652DEST_PATH_IMAGE006
；
其中，
Figure 611062DEST_PATH_IMAGE007
；
Figure 374619DEST_PATH_IMAGE014
；
式中，
Figure 411845DEST_PATH_IMAGE009
表示根据第k帧插值后的轮式里程计数据得出的前轮驱动轮转动角度，
Figure 893642DEST_PATH_IMAGE010
表示根据第k帧插值后的轮式里程计数据得出的后轮驱动轮转动角度，
Figure 41727DEST_PATH_IMAGE011
表示根据第k帧插值后的轮式里程计数据得出的前轮转向角，
Figure 925369DEST_PATH_IMAGE012
表示根据第k帧插值后的轮式里程计数据得出的后轮转向角，b为常数。
8.如权利要求5所述的在线双舵轮AGV内外参标定系统，其特征在于，
双舵轮AGV的外参数为：激光雷达的位姿参数；
双舵轮AGV的外参数的标定，包括：
将外参数作为输出数据，将插值后的轮式里程计数据作为输入数据，构建最小二乘问题，通过求解最小二乘问题标定外参。
9.一种计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如权利要求1-4任一项所述的在线双舵轮AGV内外参标定方法中的步骤。
10.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-4任一项所述的在线双舵轮AGV内外参标定方法中的步骤。
CN202211381472.7A 2022-11-07 2022-11-07 在线双舵轮agv内外参标定方法、系统、介质及设备 Active CN115435816B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211381472.7A CN115435816B (zh) 2022-11-07 2022-11-07 在线双舵轮agv内外参标定方法、系统、介质及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211381472.7A CN115435816B (zh) 2022-11-07 2022-11-07 在线双舵轮agv内外参标定方法、系统、介质及设备

Publications (2)

Publication Number Publication Date
CN115435816A true CN115435816A (zh) 2022-12-06
CN115435816B CN115435816B (zh) 2023-04-11

Family

ID=84253098

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211381472.7A Active CN115435816B (zh) 2022-11-07 2022-11-07 在线双舵轮agv内外参标定方法、系统、介质及设备

Country Status (1)

Country Link
CN (1) CN115435816B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116148824A (zh) * 2023-04-17 2023-05-23 机科发展科技股份有限公司 一种激光无人叉车导航参数自动校准系统及方法
CN116974290A (zh) * 2023-09-25 2023-10-31 杭叉集团股份有限公司 一种双舵轮agv舵轮角度校准方法及装置
CN117109638A (zh) * 2023-10-24 2023-11-24 山东大学 一种在线单舵轮agv参数标定方法、系统、设备及介质

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113091771A (zh) * 2021-04-13 2021-07-09 清华大学 一种激光雷达-相机-惯导联合标定方法及系统
WO2021253193A1 (zh) * 2020-06-15 2021-12-23 深圳市大疆创新科技有限公司 多组激光雷达外参的标定方法、标定装置和计算机存储介质
CN114325667A (zh) * 2022-01-05 2022-04-12 上海三一重机股份有限公司 组合导航设备与激光雷达的外参标定方法及装置
CN114440928A (zh) * 2022-01-27 2022-05-06 杭州申昊科技股份有限公司 激光雷达与里程计的联合标定方法、机器人、设备和介质
CN114674345A (zh) * 2022-03-21 2022-06-28 北京航空航天大学 一种惯导/相机/激光测速仪在线联合标定方法
WO2022170847A1 (zh) * 2021-02-09 2022-08-18 中国科学院深圳先进技术研究院 一种基于激光和视觉融合的在线标定方法
CN114966629A (zh) * 2022-05-12 2022-08-30 之江实验室 一种基于ekf算法框架的车体激光雷达外参标定方法
CN114994641A (zh) * 2022-06-10 2022-09-02 杭州海康机器人技术有限公司 一种参数标定方法、装置、电子设备及存储介质

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021253193A1 (zh) * 2020-06-15 2021-12-23 深圳市大疆创新科技有限公司 多组激光雷达外参的标定方法、标定装置和计算机存储介质
WO2022170847A1 (zh) * 2021-02-09 2022-08-18 中国科学院深圳先进技术研究院 一种基于激光和视觉融合的在线标定方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113091771A (zh) * 2021-04-13 2021-07-09 清华大学 一种激光雷达-相机-惯导联合标定方法及系统
CN114325667A (zh) * 2022-01-05 2022-04-12 上海三一重机股份有限公司 组合导航设备与激光雷达的外参标定方法及装置
CN114440928A (zh) * 2022-01-27 2022-05-06 杭州申昊科技股份有限公司 激光雷达与里程计的联合标定方法、机器人、设备和介质
CN114674345A (zh) * 2022-03-21 2022-06-28 北京航空航天大学 一种惯导/相机/激光测速仪在线联合标定方法
CN114966629A (zh) * 2022-05-12 2022-08-30 之江实验室 一种基于ekf算法框架的车体激光雷达外参标定方法
CN114994641A (zh) * 2022-06-10 2022-09-02 杭州海康机器人技术有限公司 一种参数标定方法、装置、电子设备及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
卢纪凤;罗磊;时轮;: "基于UMBmark和EKF的差速移动机器人传感器系统误差标定方法" *
程金龙等: "车载激光雷达外参数的标定方法", 《光电工程》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116148824A (zh) * 2023-04-17 2023-05-23 机科发展科技股份有限公司 一种激光无人叉车导航参数自动校准系统及方法
CN116974290A (zh) * 2023-09-25 2023-10-31 杭叉集团股份有限公司 一种双舵轮agv舵轮角度校准方法及装置
CN116974290B (zh) * 2023-09-25 2023-12-19 杭叉集团股份有限公司 一种双舵轮agv舵轮角度校准方法及装置
CN117109638A (zh) * 2023-10-24 2023-11-24 山东大学 一种在线单舵轮agv参数标定方法、系统、设备及介质
CN117109638B (zh) * 2023-10-24 2024-03-15 山东大学 一种在线单舵轮agv参数标定方法、系统、设备及介质

Also Published As

Publication number Publication date
CN115435816B (zh) 2023-04-11

Similar Documents

Publication Publication Date Title
CN115435816B (zh) 在线双舵轮agv内外参标定方法、系统、介质及设备
CN108955688B (zh) 双轮差速移动机器人定位方法及系统
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN110243380B (zh) 一种基于多传感器数据与角度特征识别的地图匹配方法
CN110162046B (zh) 基于事件触发型模型预测控制的无人车路径跟随方法
CN111610523B (zh) 一种轮式移动机器人的参数校正方法
CN111521195B (zh) 一种智能机器人
CN104964683B (zh) 一种室内环境地图创建的闭环校正方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN115752507A (zh) 基于二维码导航的在线单舵轮agv参数标定方法及系统
CN112731354A (zh) Agv上激光雷达位姿的自标定方法
CN111158379A (zh) 一种方向盘零偏自学习的无人车轨迹跟踪方法
CN111930126A (zh) 一种基于差速轮组agv的导航纠偏方法
Moore et al. Simultaneous local and global state estimation for robotic navigation
CN115993089B (zh) 基于pl-icp的在线四舵轮agv内外参标定方法
CN113887060B (zh) 一种新型的自动泊车系统车辆定位方法
De Cecco Sensor fusion of inertial-odometric navigation as a function of the actual manoeuvres of autonomous guided vehicles
CN114442054A (zh) 一种移动机器人的传感器与底盘联合标定系统及方法
CN115655311A (zh) 一种基于扫描匹配的阿克曼型机器人里程计标定方法
US20220083791A1 (en) Orientation-agnostic lane tracking in a vehicle
CN114993285B (zh) 一种基于四轮全向全驱移动机器人的二维激光雷达建图方法
CN114252073A (zh) 一种机器人姿态数据融合方法
CN117109638B (zh) 一种在线单舵轮agv参数标定方法、系统、设备及介质
CN117519190B (zh) 一种新的铰接车辆控制方法
CN113758491B (zh) 基于多传感器融合无人车的相对定位方法、系统及车辆

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant