激光雷达与里程计的联合标定方法、机器人、设备和介质与流程

1.本技术属于机器人技术领域，具体涉及一种激光雷达与里程计的联合标定方法。


背景技术：

2.激光雷达和里程计(轮式编码器)是移动机器人中最常用的两个传感器，特别是在工业和民用领域的轮式机器人中，如自主导航车，无人车和轮式服务机器人等，基于编码器和激光雷达融合的里程计可以提升机器人对环境的感知和自身定位的能力。
3.在实际应用中，编码器和激光雷达通常被放置在不同的位置，导致两种传感器的坐标系不一致，在实际使用之前，需要对这两个坐标系之间的相对位置进行校准，即标定激光雷达的外参数。此外，由于机械加工和安装误差、机器人长期运行导致的轮子磨损等因素，里程计的内参数通常会与机械给定值存在误差，需定期进行标定。总之，准确的内、外参数标定结果对移动机器人系统的状态估计和环境感知具有重要的意义。
4.目前常见的激光—里程计标定方案中，激光雷达外参数和里程计内参数是分开标定的。针对激光雷达外参数，常见的方法是：由标定人员控制小车运动，分别计算出激光雷达和里程计各自的相对运动，然后利用非线性优化的方法求解出外参数。针对里程计内参数，常见的标定方法是：控制机器人直线行驶一定距离，读取里程计给出的位移，并用标尺测量实际位移；控制机器人原地旋转一定角度，读取里程计给出的角度变化，并用角度尺测量实际角度变化；根据实际测量值和里程计测量值的误差，修正里程计内参数。以上的标定方法存在效率和精度较低、导致传感器测量误差较大的问题。


技术实现要素：

5.(一)要解决的技术问题
6.鉴于现有技术的上述缺点、不足，本技术提供一种激光雷达与里程计联合标定方法、机器人、设备和介质。
7.(二)技术方案
8.为达到上述目的，本技术采用如下技术方案：
9.第一方面，本技术实施例提供一种激光雷达与里程计联合标定方法，所述激光雷达用于采集激光雷达数据以获取移动机器人的环境信息，所述里程计用于采集里程计数据以获取所述移动机器人的行程信息，该方法包括以下步骤：
10.获取所述移动机器人按照预设路线移动时所述里程计实时采集的里程计数据和所述激光雷达实时采集的激光雷达数据；
11.基于所述里程计数据确定所述移动机器人在预设时间间隔的姿态变化量，将确定的姿态变化量作为第一姿态变换量；
12.基于所述激光雷达数据确定激光坐标系在预设时间间隔的姿态变化量，将确定的姿态变化量作为第二姿态变换量；
13.根据所述第一姿态变换量、所述第二姿态变换量构建残差，通过最小二乘法求解
得到所述里程计的内参数校准值和所述激光雷达的外参数校准值；
14.以所述内参数校准值和所述外参数校准值分别校准所述里程计的内参数和所述激光雷达的外参数。
15.可选地，所述基于所述里程计数据确定所述移动机器人在预设时间间隔的姿态变化量，将确定的姿态变化量作为第一姿态变换量的步骤包括；
16.s21、基于所述里程计数据和预先给定的里程计内参数，根据以下公式得到所述移动机器人的左轮角速度ω
l
和右轮角速度ωr；所述里程计数据包括移动机器人的前进速度检测值v0和旋转角速度检测值ω0，所述里程计内参数包括左右轮半径r
l
，rr和轮间距b；
[0017][0018][0019]
s22、基于所述左轮角速度和右轮角速度、待优化的里程计内参数，根据以下公式计算所述移动机器人当前的前进速度v和旋转角速度ω；
[0020][0021]
s23、基于所述前进速度和所述旋转角速度，通过积分运算得到所述移动机器人的实时姿态；
[0022]
s24、基于所述移动机器人的实时姿态，根据以下公式确定所述移动机器人在预设时间间隔t∈[tk，t
k 1
]的姿态变化量；
[0023]
q＝(q
x
，qy，q
θ
)∈se(2)
[0024][0025]
其中，q为机器人相对于固定坐标系的姿态，q
x
为x坐标，qy为y坐标，q
θ
为角度，se(2)表示二维空间的李群；qk和q
k 1
分别为两个时刻移动机器人的姿态，rk为第一姿态变化量，r
l
为左轮半径，rr为右轮半径，b为左右轮间距，表示qk的逆，
⊙
表示se(2)的运算。
[0026]
可选地，通过基于激光雷达的定位算法或icp点云匹配算法确定激光坐标系在预设时间间隔的姿态变化量
[0027]
可选地，所述第一姿态变换量与所述第二姿态变换量的关系表示为：
[0028][0029]
sk＝(qk⊙
l)-1
⊙
(q
k 1
⊙
l)＝l-1
⊙rk
(r
l
，rr，b)
⊙
l
[0030]
l＝(l
x
，ly，l
θ
)∈se(2)
[0031]
其中，l为激光雷达外参数，l
x
为激光坐标系在机器人坐标系中的x坐标，ly为激光坐标系在机器人坐标系中的y坐标，l
θ
为激光坐标系相对于机器人坐标系的旋转角度，sk为基于第一姿态变换量得到的预设时间间隔的激光坐标系的相对变化量。
[0032]
可选地，所述第二姿态变换量服从高斯分布。
[0033]
可选地，构建的残差表达式为：
localization and mapping,slam)机器人能够实时估计自己在场景中的位姿，从而能够实现自主导航。场景可以是室内，也可以是室外。
[0050]
实施例一
[0051]
本实施例中激光雷达用于采集激光雷达数据以获取两轮差速移动机器人的环境信息，里程计用于采集里程计数据以获取两轮差速移动机器人的行程信息。图1为本技术一个实施例中的激光雷达与里程计联合标定方法流程示意图，如图1所示，本实施例的激光雷达与里程计联合标定方法包括以下步骤：
[0052]
s10、获取移动机器人按照预设路线移动时里程计实时采集的里程计数据和激光雷达实时采集的激光雷达数据；
[0053]
s20、基于里程计数据确定移动机器人在预设时间间隔的姿态变化量，将确定的姿态变化量作为第一姿态变换量；
[0054]
s30、基于激光雷达数据确定激光坐标系在预设时间间隔的姿态变化量，将确定的姿态变化量作为第二姿态变换量；
[0055]
s40、根据第一姿态变换量、第二姿态变换量构建残差，通过最小二乘法求解得到里程计的内参数校准值和激光雷达的外参数校准值；
[0056]
s50、以内参数校准值和外参数校准值分别校准里程计的内参数和激光雷达的外参数。
[0057]
本实施例的激光雷达与里程计联合标定方法，能够同时、快速的标定激光雷达外参数和里程计内参数，不仅标定精度高而且提高了标定效率，从而降低了传感器测量误差，可用于机器人出厂前的参数校准，也可用于机器人长期运行过程的定期校准。
[0058]
为了更好地理解本发明，以下对本实施例中的各步骤进行展开说明。
[0059]
本实施例中，s20包括：
[0060]
s21、基于所述里程计数据和预先给定的里程计内参数，根据以下公式得到移动机器人的左轮角速度ω
l
和右轮角速度ωr；所述里程计数据包括移动机器人的前进速度检测值v0和旋转角速度检测值ω0，所述里程计内参数包括左右轮半径r
l
，rr和轮间距b；
[0061][0062][0063]
s22、基于所述左轮角速度和右轮角速度、待优化的里程计内参数，根据以下公式计算移动机器人当前的前进速度v和旋转角速度ω；
[0064][0065]
s23、基于所述前进速度和所述旋转角速度，通过积分运算得到所述移动机器人的实时姿态；
[0066]
s24、基于所述移动机器人的实时姿态，根据以下公式确定所述移动机器人在预设时间间隔t∈[tk，t
k 1
]的姿态变化量；
[0067]
q＝(q
x
，qy，q
θ
)∈se(2)
ꢀꢀꢀꢀꢀ
(4)
[0068][0069]
其中，q为机器人相对于固定坐标系的姿态，q
x
为x坐标，qy为y坐标，q
θ
为角度，se(2)表示二维空间的李群；qk和q
k 1
分别为两个时刻移动机器人的姿态，rk为第一姿态变化量，r
l
为左轮半径，rr为右轮半径，b为左右轮间距，表示qk的求逆，
⊙
表示se(2)的运算。
[0070]
求逆和加法运算过程如以下公式所示：
[0071][0072][0073]
本实施例s30中，可以通过基于激光雷达的定位算法或迭代最近点(iterative closest point，icp)点云匹配算法确定激光坐标系在预设时间间隔的姿态变化量。具体地，激光雷达的定位算法可以是slam算法。需要说明的是，也可以采用其他的点云匹配算法，以上点云匹配算法仅仅是示例性的说明，并不构成对点云匹配算法的具体限定。
[0074]
具体地，基于测距的定位算法可以是基于接收信号强度测量法、到达时间测量法、到达时间差测量法和到达角度测量法。
[0075]
需要说明的是，上述的基于测距的定位算法仅仅是示例性的说明，并不构成对定位算法的具体限定。
[0076]
本实施例中，所述第一姿态变换量与所述第二姿态变换量的关系即激光坐标系和机器人坐标系的关系，可以表示为：
[0077][0078]
sk＝(qk⊙
l)-1
⊙
(q
k 1
⊙
l)＝l-1
⊙rk
(r
l
，rr，b)
⊙
l
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0079]
l＝(l
x
，ly，l
θ
)∈se(2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0080]
其中，l为激光雷达外参数，l
x
为激光坐标系在机器人坐标系中的x坐标，ly为激光坐标系在机器人坐标系中的y坐标，l
θ
为激光坐标系相对于机器人坐标系的旋转角度，sk为基于第一姿态变换量得到的预设时间间隔的激光坐标系的相对变化量。
[0081]
本实施例中，所述第二姿态变换量服从高斯分布。
[0082]
构建的残差表达式为：
[0083][0084]
其中，第二姿态变换量，σ为高斯分布的方差。
[0085]
之后通过最小二乘法即可求解得到里程计的内参数校准值和激光雷达的外参数校准值，其中，里程计内参数校准值具体包括左右轮半径校准值和轮间距校准值，激光雷达的外参数校准值具体包括激光坐标系在机器人坐标系中的x坐标校准值，激光坐标系在机器人坐标系中的y坐标校准值，激光坐标系相对于机器人坐标系的旋转角度校准值。
[0086]
实施例二
[0087]
本实施例的执行主体可以是机器人中的标定模块，标定模块可以包括存储器和处理器，在其他一些实施例中执行主体还可以是其他可实现相同或相似功能的电子设备，本实施例对此不加以限制。在本实施例中，以机器人中的标定模块为例对本发明的激光雷达与里程计联合标定方法进行说明。
[0088]
需要说明的是，在标定前需要将机器人开至环境特征相对明显、地面相对水平的区域，以尽量保证激光位姿估计的精度。如果使用定位算法估计激光在地图中的姿态，需在该区域预先建好地图。
[0089]
图2为本技术另一个实施例中的激光雷达与里程计联合标定方法流程示，本实施例在实施例一的基础上，对本实施例的具体实现过程进行了详细说明。以下结合图2对本实施例方法的步骤进行具体说明。
[0090]
步骤s1、在标定模块中输入当前机器人的里程计内参数；
[0091]
启动标定程序，开始采集数据；控制机器人，走一个“8”字形路线。
[0092]
步骤s2、接收nav_msgs:odometry类型的里程计数据，根据公式(3)、(4)和给定的里程计内参数，计算得到机器人左、右轮的角速度ω
l
和ωr。
[0093]
根据公式(3)、(5)、(8)计算由里程计估计得到的机器人的姿态。将数据缓存至里程计位姿队列odom_pose。同时，接收激光坐标系姿态，可由定位算法或icp等点云匹配算法给出。将数据缓存至激光坐标系位姿队列laser_pose。
[0094]
步骤s3、机器人行走完毕后，按时间戳同步两个队列的数据，根据公式(5)和(8)-(10)得到各个时间间隔的激光坐标系的位姿变化sk和
[0095]
步骤s4、根据式(11)和同步的数据，优化求解得到内、外参数。
[0096]
步骤s5、根据公式(5)、(9)、(10)，利用得到的内、外参数和队列计算odom_pose，得到新的估计值根据式(11)计算与的残差，过滤掉一部分残差较大的数据。
[0097]
步骤s6、重复步骤s3-s5，迭代计算多次。最后得到内、外参数标定结果。
[0098]
本实施例的激光雷达与里程计联合标定方法通过采集行走过程中里程计估算的机器人在地图中的姿态和定位算法等方式估算的激光在地图中的姿态，迭代计算内外参数，最后输出标定结果，解决了激光—里程计标定效率较低、测量误差较大等问题，可实现机器人自主、在线标定。
[0099]
实施例三
[0100]
本技术第二方面通过实施例三提供了一种移动机器人，图3为本技术又一个实施例中的移动机器人结构示意图，如图3所示，该机器人包括处理器31、里程计32和激光雷达33，处理器31用于执行如上实施例中任意一项所述的激光雷达与里程计联合标定方法。
[0101]
本实施例提供的移动机器人，可用于执行上述方法实施例中激光雷达与里程计联合标定方法的步骤，其实现原理和技术效果类似，本实施例此处不再赘述。
[0102]
实施例四
[0103]
本技术第三方面通过实施例四提供了一种电子设备，包括：存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，计算机程序被处理器执行时实现如上实施例中任意一项所述的激光雷达与里程计联合标定方法的步骤。
[0104]
图4为本技术再一实施例中的电子设备的架构示意图。
[0105]
图4所示的电子设备可包括：至少一个处理器101、至少一个存储器102、至少一个网络接口104和其他的用户接口103。电子设备中的各个组件通过总线系统105耦合在一起。可理解，总线系统105用于实现这些组件之间的连接通信。总线系统105除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图4中将各种总线都标为总线系统105。
[0106]
其中，用户接口103可以包括显示器、键盘或者点击设备(例如，鼠标，轨迹球(trackball)或者触感板等。
[0107]
可以理解，本实施例中的存储器102可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read-onlymemory，rom)、可编程只读存储器(programmablerom，prom)、可擦除可编程只读存储器(erasableprom，eprom)、电可擦除可编程只读存储器(electricallyeprom，eeprom)或闪存。易失性存储器可以是随机存取存储器(randomaccessmemory，ram)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的ram可用，例如静态随机存取存储器(staticram，sram)、动态随机存取存储器(dynamicram，dram)、同步动态随机存取存储器(synchronousdram，sdram)、双倍数据速率同步动态随机存取存储器(doubledatarate sdram，ddrsdram)、增强型同步动态随机存取存储器(enhanced sdram，esdram)、同步连接动态随机存取存储器(synchlinkdram，sldram)和直接内存总线随机存取存储器(directrambusram，drram)。本文描述的存储器102旨在包括但不限于这些和任意其它适合类型的存储器。
[0108]
在一些实施方式中，存储器102存储了如下的元素，可执行单元或者数据结构，或者他们的子集，或者他们的扩展集：操作系统1021和应用程序1022。
[0109]
其中，操作系统1021，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序1022，包含各种应用程序，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序1022中。
[0110]
在本发明实施例中，处理器101通过调用存储器102存储的程序或指令，具体的，可以是应用程序1022中存储的程序或指令，处理器101用于执行第一方面所提供的方法步骤。
[0111]
上述本发明实施例揭示的方法可以应用于处理器101中，或者由处理器101实现。处理器101可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器101中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器101可以是通用处理器、数字信号处理器、专用集成电路、现成可编程门阵列或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件单元组合执行完成。软件单元可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器102，处理器101读取存储器102中的信息，结合其硬件完成上述方法的步骤。
[0112]
另外，结合上述实施例中的激光雷达与里程计联合标定方法，本发明实施例可提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处
理器执行时实现如上方法实施例中的任意一种激光雷达与里程计联合标定方法。
[0113]
应当注意的是，在权利要求中，不应将位于括号之间的任何附图标记理解成对权利要求的限制。词语“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的词语“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。词语第一、第二、第三等的使用，仅是为了表述方便，而不表示任何顺序。可将这些词语理解为部件名称的一部分。
[0114]
此外，需要说明的是，在本说明书的描述中，术语“一个实施例”、“一些实施例”、“实施例”、“示例”、“具体示例”或“一些示例”等的描述，是指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
[0115]
尽管已描述了本发明的优选实施例，但本领域的技术人员在得知了基本创造性概念后，则可对这些实施例作出另外的变更和修改。所以，权利要求应该解释为包括优选实施例以及落入本发明范围的所有变更和修改。
[0116]
显然，本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也应该包含这些修改和变型在内。