用于定位装置和里程计的标定方法、程序产品和标定装置与流程

1.本发明涉及自主移动设备的定位领域，尤其涉及一种用于标定自主移动设备的定位装置的定位装置标定方法、一种用于标定自主移动设备的里程计的里程计标定方法以及相应的计算机程序产品和标定装置。


背景技术：

2.随着经济快速增长、人力成本逐渐上升、劳动领域愈加复杂，自主移动设备、例如移动机器人得到越来越广泛的应用。自主移动设备确定其自身的位置是完成其各项任务的前提，也一直是学术界和工业领域研究的热点。这其中往往需要有里程计的参与，由里程计提供自主移动设备所发生的位移，并由此作为自主移动设备的位置或作为计算位置的输入条件。显然，越准确的里程计对于自主移动设备的定位所带来帮助越大，而提高里程计精度的方法之一就是进行准确的标定。
3.通常，为了对自主移动设备的里程计进行标定，可采用离线标定或在线标定。
4.离线标定需要通过人工测量的方式获取自主移动设备位姿(如通过直尺测量或从刻度纸上获取)，既可能人为引入测量误差，又无法自动化，效率较低。
5.在线标定通过设置在自主移动设备机身上的外部感知式定位装置(例如，激光定位传感器)对自主移动设备位姿进行实时估计，实现自动化标注。但由于外部感知式定位装置存在安装误差，从而在测量信息中引入了新的系统误差。因此，需要对外部感知式定位装置也进行标定。
6.目前，已公开了一种在线标定方法，其中，使用2个误差标定滤波器(ekf)对里程计系统误差和外部感知式定位装置安装误差进行迭代标定，标定其中一个误差时忽略另一个误差。该方法具有较强的非线性且操作过程较复杂；并且根据机器人运动模型将引起误差的6个参数分为线性与非线性，在进行参数的非线性估计时需要经过大量的非线性运算，且参数间耦合关系不易直观理解。
7.现有技术在用于自主移动设备的里程计和定位装置的标定方面仍然存在诸多不足。


技术实现要素：

8.本发明的目的在于提供一种改进的用于标定自主移动设备的定位装置和里程计的方法，以至少在一个方面克服上述现有技术的不足。
9.根据本发明的第一方面，提供了一种用于标定自主移动设备的定位装置的定位装置标定方法，其中，所述定位装置固定地安装在自主移动设备上并设置成能够输出定位装置的位姿信息，其中，所述定位装置标定方法包括以下步骤：
10.使自主移动设备在平面上匀速地沿具有互不相等的半径的圆弧形的至少两条运动路径运动，其中，在沿每条运动路径运动的过程中，分别在一组至少两个不同的测量时刻记录由定位装置输出的一组位姿信息；以及
11.根据定位装置输出的至少两组位姿信息，确定定位装置的标定参数，其中，定位装置的标定参数表示定位装置在自主移动设备中的位置和方向。
12.可选地，定位装置的标定参数包括定位装置的坐标系原点在自主移动设备的坐标系下的横坐标rxs和纵坐标rys以及定位装置的坐标系相对于自主移动设备的坐标系的角度θs。
13.可选地，定位装置的标定参数通过运动过程中定位装置在自主移动设备上的位姿保持不变的性质来确定。
14.可选地，各组测量时刻的数量分别大于2。
15.可选地，运动路径为一个完整的圆周。
16.可选地，位姿信息表示定位装置在世界坐标系中的位姿。
17.可选地，定位装置的坐标系原点在自主移动设备的坐标系下的横坐标rxs和纵坐标rys根据所述至少两组位姿信息利用以下公式得出：
[0018][0019]
其中，和分别表示在第k 1个测量时刻定位装置在世界坐标系中的横、纵坐标和姿态角，k＞0，θ0表示在第1个测量时刻自主移动设备在世界坐标系中的姿态角，θ
k_0
表示自主移动设备从第1个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。
[0020]
可选地，定位装置的坐标系相对于自主移动设备的坐标系的角度θs通过以下公式得出：
[0021][0022]
θk＝θ0 θ
k_0
[0023]
其中，θk表示在第k 1个测量时刻自主移动设备在世界坐标系中的姿态角。
[0024]
可选地，位姿信息表示定位装置在各组测量时刻中的第1个测量时刻的定位装置的坐标系中的位姿。
[0025]
可选地，定位装置的标定参数rxs、rys和θs根据所述至少两组位姿信息利用以下公式得出：
[0026][0027][0028]
其中，和分别表示在第k 1个测量时刻定位装置在相应的第1个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，k＞0，θ
(k-1)_0
表示自主移动设备从第1个测量时刻到第k个测量时刻的转动角，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。
[0029]
可选地，位姿信息表示定位装置在各组测量时刻中的前一测量时刻的定位装置的坐标系中的位姿。
[0030]
可选地，定位装置的标定参数rxs、rys和θs根据所述至少两组位姿信息利用以下公式得出：
[0031][0032]
其中，和分别表示在第k 1个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，和分别表示在第k个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，k＞0，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。
[0033]
可选地，各组测量时刻的数量分别大于2，每组测量时刻的间隔时间相等。
[0034]
可选地，定位装置的标定参数rxs、rys和θs根据所述至少两组位姿信息利用以下公式得出：
[0035][0036]
其中，和分别表示在第k 1个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，和分别表示在第k个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，k＞0，等式左边表示两个测量时刻之间的位姿变化的平均值，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。
[0037]
可选地，所述至少两条运动路径包括至少三条运动路径。
[0038]
可选地，通过优化目标函数确定定位装置的标定参数，所述目标函数使得在所有具有不同半径的运动路径下计算结果的总误差最小。
[0039]
可选地，将所述至少三条运动路径分为分别包括两条运动路径的多组运动路径，针对每组运动路径所对应的位姿信息进行处理并确定处理结果的标准差和均值。
[0040]
可选地，如果标准差较大，则重新进行标定。
[0041]
根据本发明的第二方面，提供了一种用于标定自主移动设备的里程计的里程计标定方法，其中，所述自主移动设备具有差速轮运动系统，所述里程计标定方法包括：利用安装在自主移动设备上的定位装置进行自动标定，其中，通过根据本发明的定位装置标定方法标定所述定位装置。
[0042]
可选地，里程计的标定参数包括自主移动设备的左轮半径r
l
、右轮半径rr和轮距b，左轮半径r
l
，、右轮半径rr和轮距b利用以下公式得出：
[0043][0044][0045]
其中，ω表示自主移动设备的坐标系的角速度，ωr和ω
l
分别表示左轮的角速度和右轮的角速度。
[0046]
可选地，里程计的标定参数与定位装置的标定参数一起在定位装置标定方法的步骤s2中被确定。
[0047]
可选地，在一组总数量大于2的测量时刻之间间隔相等的时间t的情况下，利用以
下公式计算里程计的标定参数和/或定位装置的标定参数。
[0048]
θ
k_(k-1)
＝t
·
ω
[0049]
其中，θ
k(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角。
[0050]
根据本发明的第三方面，提供了一种计算机程序产品，其包括计算器程序指令，其中，当所述计算机程序指令被一个或多于一个处理器执行时，所述处理器能够执行根据本发明的定位装置标定方法或根据本发明的里程计标定方法。
[0051]
根据本发明的第四方面，提供了一种标定装置，所述装置包括处理器和与处理器通信连接的计算机可读存储装置，计算机可读存储装置中存储有计算机程序，当所述计算机程序被处理器执行时，用于实施根据本发明的定位装置标定方法或根据本发明的里程计标定方法。
[0052]
本发明通过自主移动设备的至少两次圆弧运动，确定定位装置在自主移动设备上的位置、姿态。与此同时，可确定里程计的标定参数。这种标定方法操作简单，标定结果准确，并且标定速度快。
附图说明
[0053]
下面，通过参看附图更详细地描述本发明，可以更好地理解本发明的原理、特点和优点。附图包括：
[0054]
图1示意性地示出了根据本发明的一个示例性实施例的用于标定自主移动设备的定位装置的定位装置标定方法；
[0055]
图2示意性地示出了根据本发明的一个示例性实施例的沿圆弧形的运动路径运动的自主移动设备；以及
[0056]
图3示意性地示出了根据本发明的一个示例性实施例的标定装置。
具体实施方式
[0057]
为了使本发明所要解决的技术问题、技术方案以及有益的技术效果更加清楚明白，以下将结合附图以及多个示例性实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，而不是用于限定本发明的保护范围。
[0058]
本发明适用于自主移动设备，其可以是任何能够自主地进行空间移动的机械设备，例如无人车、无人机、机器人等。例如，自主移动设备可以是仓储机器人、清扫型机器人、家庭陪护机器人、迎宾机器人等。
[0059]
自主移动设备通常安装有里程计(本体感知传感器)。在自主移动设备移动过程中，里程计可以输出自主移动设备的移动距离、角度等数据。里程计主要基于光电编码器在采样周期内脉冲的变化量计算出自主移动设备相对于地面移动的距离和方向角的变化量，从而计算出自主移动设备在语义地图中的初始位置。自主移动设备通常还安装有外部感知式定位装置，例如激光雷达、深度相机等。定位装置可以是任意基于tof定位原理的定位装置。所述定位装置能够确定定位装置的位姿，并且在标定之后能够用来确定自主移动设备的位姿。
[0060]
下面结合附图，详细说明本发明的定位装置标定过程和里程计标定过程。
[0061]
图1示意性地示出了根据本发明的一个示例性实施例的用于标定自主移动设备的
定位装置的定位装置标定方法。所述定位装置固定地安装在自主移动设备上并设置成能够输出定位装置的位姿信息。所述定位装置标定方法包括以下步骤：
[0062]
步骤s1：使自主移动设备在平面、例如平坦的地面上匀速地沿至少两条具有互不相等的半径的圆弧形的运动路径运动，其中，在沿每条运动路径运动的过程中，分别在一组至少两个不同的测量时刻记录由定位装置输出的一组位姿信息；以及
[0063]
步骤s2：根据定位装置输出的至少两组位姿信息，确定定位装置的标定参数，其中，定位装置的标定参数表示定位装置在自主移动设备中的位置和方向。
[0064]
由此，可通过自主移动设备的圆弧运动，确定定位装置在自主移动设备上的位置、姿态，这种定位装置标定方法操作简单，标定结果准确，并且标定速度快。应理解，位姿信息包括位置信息和姿态角信息。
[0065]
步骤s1可具体地包括以下步骤：
[0066]
步骤s11：使自主移动设备在平面上匀速地沿具有第一半径的圆弧形的运动路径运动，其中，在运动过程中的第一组至少两个不同的测量时刻记录定位装置输出的第一组位姿信息；
[0067]
步骤s12：使自主移动设备在平面上匀速地沿具有与第一半径不同的第二半径的圆弧形的运动路径运动，其中，在运动过程中的第二组至少两个不同的测量时刻记录定位装置输出的第二组位姿信息；
[0068]
在步骤s1中，各组测量时刻的数量至少为2。可选地，各组测量时刻的数量分别大于2。
[0069]
可选地，运动路径为一个完整的圆周。运动路径也可以是小于或大于360
°
的圆弧。
[0070]
图2示意性地示出了根据本发明的一个示例性实施例的沿圆弧形的运动路径运动的自主移动设备。图2中示出了世界坐标系xowy、自主移动设备的坐标系xory和定位装置的坐标系xosy。所述自主移动设备具有差速轮运动系统。差速轮运动系统包括两个驱动轮：左轮和右轮，其中，左轮半径为r
l
，右轮半径为rr，轮距为b。自主移动设备的坐标系xory的原点or位于两个驱动轮轴线的中心，x方向可指向自主移动设备的前进方向，y方向可指向自主移动设备的左轮。里程计的标定参数可包括自主移动设备的左轮半径r
l
、右轮半径rr和轮距b。
[0071]
根据本发明，用于标定自主移动设备的里程计的里程计标定方法可包括：利用安装在自主移动设备上的定位装置进行自动标定，其中，通过根据本发明的定位装置标定方法标定所述定位装置。
[0072]
根据两轮差速运动系统的运动模型，自主移动设备的角速度ω满足以下公式(1)：
[0073][0074]
其中，ωr和ω
l
分别表示右轮和左轮的角速度。通过设置合适的ωr和ω
l
可使自主移动设备沿圆弧形运动路径运动。已知的ωr和ω
l
可进一步被用于确定里程计的标定参数和定位装置的标定参数。
[0075]
当自主移动设备在平面上匀速地沿圆弧形的运动路径运动时，运动路径的半径可用公式(2)表示：
[0076][0077]
其中，ρ表示运动路径的半径。
[0078]
自主移动设备的左轮半径r
l
、右轮半径rr和轮距b可利用以上公式(1)和公式(2)得出。
[0079]
如上文所述，定位装置固定地安装在自主移动设备上，并且可使定位装置安装平面与自主移动设备的运动平面、即地面平行。定位装置的标定参数可包括定位装置的坐标系原点在自主移动设备的坐标系下的横坐标rxs和纵坐标rys以及定位装置的坐标系相对于自主移动设备的坐标系的角度θs。θs可用定位装置的坐标系的x轴相对于自主移动设备的坐标系的x轴之间的夹角来表示。由于定位装置固定地安装在自主移动设备上，因此，在任意时刻，定位装置在自主移动设备上的位姿(rxs，rys，θs)都保持不变，定位装置的标定参数可通过这种刚体位置不变性来确定。
[0080]
图2中示意性地用实线示出了沿圆弧形的运动路径运动的自主移动设备在一组测量时刻中的第1个测量时刻的位姿，并用虚线示出了沿同一运动路径运动的自主移动设备在同一组测量时刻中的第2个测量时刻的位姿。
[0081]
在一个示例性实施例中，位姿信息表示定位装置在世界坐标系中的位姿。
[0082]
定位装置的坐标系原点在自主移动设备的坐标系下的横坐标rxs和纵坐标rys可根据所述至少两组位姿信息利用以下公式(3)得出：
[0083][0084]
其中，和分别表示在第1个测量时刻定位装置在世界坐标系中的横、纵坐标和姿态角，和分别表示在第k 1个测量时刻定位装置在世界坐标系中的横、纵坐标和姿态角，k＞0，θ0表示在第1个测量时刻自主移动设备在世界坐标系中的姿态角，θ
k_0
表示自主移动设备从第1个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。
[0085]
通过公式(3)可知，当自主移动设备沿圆弧形的运动轨迹运动时，在世界坐标系下定位装置在第k个测量时刻相对于第1个测量时刻的位姿的变化可由以下参数确定：在第1个测量时刻自主移动设备在世界坐标系中的姿态角θ0、自主移动设备从第1个测量时刻到第k 1个测量时刻的转动角θ
k_0
、定位装置的坐标系原点在自主移动设备的坐标系下的横坐标rxs和纵坐标rys以及自主移动设备的运动路径的半径ρ。通过定位装置的测量能够得到在
不同测量时刻定位装置相对于世界坐标系的位姿。通过自主移动设备沿至少两条半径不同的圆弧形的运动轨迹运动，可得到足够多的方程来求解未知的参数。
[0086]
在步骤s2中，可将公式(3)与公式(1)和公式(2)联立，使得里程计的标定参数与定位装置的标定参数一起被确定。
[0087]
定位装置的坐标系相对于自主移动设备的坐标系的角度θs可通过以下公式得出：
[0088][0089]
θk＝θ0 θ
k_0
[0090]
其中，θk表示在第k 1个测量时刻自主移动设备在世界坐标系中的姿态角。
[0091]
在一个示例性实施例中，位姿信息表示定位装置在各组测量时刻中的第1个测量时刻的定位装置的坐标系中的位姿。这尤其适用于没有获取全局地图的情况。
[0092]
定位装置的标定参数rxs、rys和θs可根据所述至少两组位姿信息利用以下公式(4)得出：
[0093][0094]
其中，和分别表示在第k 1个测量时刻定位装置在相应的第1个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，k＞0，θ
(k-1)_0
表示自主移动设备从第1个测量时刻到第k个测量时刻的转动角，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。在步骤s2中，可将公式(4)与公式(1)和公式(2)联立，使得里程计的标定参数与定位装置的标定参数一起被确定。
[0095]
为了使求解更简便，在一个示例性实施例中，位姿信息表示定位装置在各组测量时刻中的前一测量时刻的定位装置的坐标系中的位姿。这也特别适用于没有获取全局地图的情况。
[0096]
定位装置的标定参数rxs、rys和θs根据所述至少两组位姿信息利用以下公式(5)得出：
[0097][0098]
其中，和分别表示在第k 1个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，和分别表示在第k个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，k＞0，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。
[0099]
可选地，一组总数量大于2的测量时刻之间的间隔时间固定为t。那么，定位装置从任意测量时刻到下一测量时刻的转动角将是恒定。如以下公式(6)所示：
[0100]
θ
k_(k-1)
＝t
·
ω
[0101]
其中，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角。该公式可用于计算里程计的标定参数和/或定位装置的标定参数。
[0102]
在测量时刻之间的间隔时间固定的情况下，定位装置从任意测量时刻到下一测量时刻的位姿变化也将是恒定的。使每组测量时刻的间隔时间相等可有利地简化测量和计算。
[0103]
因此，定位装置的标定参数rxs、rys和θs根据位姿信息利用以下公式(7)得出：
[0104][0105]
其中，和分别表示在第k 1个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，和
分别表示在第k个测量时刻定位装置在第k个测量时刻的定位装置的坐标系中的横、纵坐标和姿态角，k＞0，等式左边表示两个测量时刻之间的位姿变化的平均值，θ
k_(k-1)
表示自主移动设备从第k个测量时刻到第k 1个测量时刻的转动角，ρ表示自主移动设备的运动路径的半径。当定位装置误差服从高斯白噪声分布、测量时刻足够多时，能够有效地减小误差，并且大大减小计算量。由于位姿变化恒定，因此，通过计算数据的标准差，能够验证测量数据的正确性。当测量数据的标准差过大或者位姿变化的分布过多地偏离正态分布时，可表明数据异常。
[0106]
可将公式(1)、公式(2)、公式(6)和公式(7)联立，从而确定里程计的标定参数与定位装置的标定参数。在此，时间间隔t、右轮的角速度ωr、左轮的角速度ω
l
和定位装置在两个测量时刻之间的位姿变化的平均值是已知的。通过方程组，可输出待求解的里程计的标定参数与定位装置的标定参数以及过程变量θ
k_(k-1)
、ω和ρ。
[0107]
在一个示例性实施例中，所述至少两条运动路径包括至少三条运动路径。由此，可由定位装置确定至少三组位姿信息。在步骤s2中，可根据定位装置输出的所述至少三组位姿信息，确定定位装置的标定参数。更多的运动路径有利于减小误差。
[0108]
可选地，通过优化目标函数确定定位装置的标定参数，所述目标函数使得在所有具有不同半径的运动路径下计算结果的总误差最小。
[0109]
例如，将所述至少三条运动路径分为分别包括两条运动路径的多组运动路径，针对每组运动路径所对应的位姿信息进行处理并确定处理结果的标准差和均值。所述标准差和均值例如可以是相邻两个测量时刻之间的位姿变化的标准差和均值。替代地，所述标准差和均值也可以是所确定的定位装置的标定参数的标准差和均值。
[0110]
标准差可用于衡量标定的准确性。可选地，如果标准差较大，则重新进行标定。
[0111]
图3示意性地示出了根据本发明的一个示例性实施例的标定装置。所述标定装置可包括处理器10和与处理器10通信连接的计算机可读存储装置20。计算机可读存储装置20中存储有用于标定的计算机程序指令。处理器10可调用计算机可读存储装置20中存储的计算机程序指令，并执行根据本发明的标定方法。
[0112]
在本发明中，计算机可读存储介质20例如可包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(smart media card，smc)，安全数字(secure digital，sd)卡，闪存卡(flash card)、至少一个磁盘存储器件、闪存器件、或其它易失性固态存储器件。处理器10可以是中央处理单元(central processing unit，cpu)，还可以是其它通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者也可以是任何常规的处理器等。
[0113]
标定装置例如还包括：用于与用户进行交互的用户接口30，其例如包括显示屏和/或输入单元比如键盘；用于与网络服务器进行数据通信的网络接口40，其例如包括标准的有线接口和/或无线接口；用于连接标定装置的各组件的通信总线50。
[0114]
另外，本发明还涉及一种计算机程序产品，其包括计算器程序指令，当所述计算机程序指令被一个或多于一个处理器10执行时，所述处理器10能够执行根据本发明的定位装
置标定方法或根据本发明的里程计标定方法。
[0115]
尽管这里详细描述了本发明的特定实施方式，但它们仅仅是为了解释的目的而给出的，而不应认为它们对本发明的范围构成限制。在不脱离本发明精神和范围的前提下，各种替换、变更和改造可被构想出来。