CN110530399B - 双轮差速移动机器人里程计标定的轮间距修正方法 - Google Patents

Abstract

本发明涉及双轮差速移动机器人技术领域，尤其是涉及一种双轮差速移动机器人里程计标定的轮间距修正方法。本发明通过设置在位于左右轮中心的自旋中心正前方的激光雷达来探测预设在机器人一侧的反光条的位置，计算出所述双轮差速移动机器人的朝向Oθ_i和自旋中心O与反光条的位置P_reflector之间连线OP_reflector的夹角

根据该夹角

的变化值计算得到所述双轮差速移动机器人的实际自旋角，与里程计旋转角对比，并通过线性拟合得到轮间距的修正系数。相较于现有技术，本发明的轮间距的修正方法具有测量精确，高度自动化，无需人工实时参与和测量，成本低等优点。

Description

双轮差速移动机器人里程计标定的轮间距修正方法

技术领域

本发明涉及双轮差速移动机器人技术领域，尤其是涉及一种用于对双轮差速移动机器人的里程计进行初始标定时修正轮间距的方法，该方法适用于任意同时安装有里程计和激光雷达的双轮差速移动机器人，和能够等效为双轮差速模型而同时安装有激光雷达的其他移动机器人。

背景技术

随着经济快速增长、人力成本逐渐上升、劳动领域愈加复杂，机器人替代自然工人已成为一种趋势。在全部机器人领域中，移动机器人是一种在复杂环境下工作的，具有自行组织、自主运行、自主规划的智能机器人。由于其自身的特点，移动机器人(下文中简称为“机器人”)确定其自身的位置是完成其各项任务的前提。人们为使其获取自身的位置而开发了很多方法，如基于磁条、色带、二维码、激光SLAM等获取位置的方法，这其中往往需要有里程计的参与，由里程计向提供机器人其所发生的位移，并由此作为机器人的位置或作为计算位置的输入条件。显然，越准确的里程计对于机器人的定位所带来帮助越大，而提高里程计精度的方法之一就是进行详细的标定。

对于采用两轮差速模型的机器人，标定过程的主要目的在于获取到对(两轮)轮直径和轮间距这两个参数的修正值。在这种情况下，标定过程即在机器人发生了一段位移之后，一方面通过外部测量准确获取到这段位移，另一方面通过里程计获取其读数作为测量值，然后调整轮直径和轮间距使得测量值尽可能贴近真实值，最贴近真实值的那组参数就是标定的结果。由于里程计的结果可以直接获取，标定的主要问题存在于获取真实位移的过程中。

目前，对于双轮差速移动机器人的里程计进行标定存在以下问题：

A)对于需要人工测量位移(如通过直尺测量或从刻度纸上获取)的标定方法，既可能人为引入测量误差，又无法自动化，效率较低。

B)对于依赖IMU(惯性测量单元)来获取位移的方法，要么受到高精度IMU的高成本的限制，要么屈从于低精度IMU对于发生位移的过程的限制(如不能打滑、小心急停、不能碰撞等)，同时由于从IMU中提取位移主要基于线速度和角速度对时间的积分，所以网络延迟等会影响实时性的因素也会带来不利影响。

C)对于同时使用IMU和里程计来计算位移的标定过程(如通过拓展卡尔曼滤波过程)，一方面在逻辑上有陷入循环论证的危险，即使用里程计来标定里程计，另一方面对算法的要求也很高，对于基于卡尔曼滤波的算法而言受到其本身对于误差的高斯分布假设的限制。

D)对于直接使用激光数据，根据周围环境计算机器人位移的标定过程，显然受到激光雷达精度的限制，同时也会受到周围环境的影响，如定位周围环境的精度不高，或从激光数据中提取固定位置的精度不高，等。

发明内容

有鉴于此，本发明的目的是解决现有技术的问题，提供可用于双轮差速移动机器人里程计的自动化标定，不需要人工实时参与并小心控制或测量，测量精度更高的轮间距修正方法。

为了达到上述目的，本发明提供一种利用反光条对双轮差速移动机器人的轮间距进行修正，从而用于里程计标定的方法。

本发明具体采用如下技术方案：

一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，该双轮差速移动机器人的自旋中心位于左右轮中心连线的中点，且激光雷达安装在所述自旋中心的正前方，所述轮间距修正方法包括以下步骤：

S1、在所述双轮差速移动机器人一侧设置反光条，所述反光条应位于所述激光雷达的视界中；

S2、令所述双轮差速移动机器人以预设的角速度开始原地自旋，记录数据，所述数据包括所述激光雷达扫描所述反光条得到激光数据和所述双轮差速移动机器人自旋的里程信息；

S3、通过里程信息，定位到所述双轮差速移动机器人开始稳定自旋的时刻；

从每一帧激光数据中提取反光条位置，包括以下步骤：

(1)从一帧激光数据中挑选出强度合适的激光数据点，把每个激光数据点及其在全部激光数据中的序号保存为反光条的候选位置；

(2)如果没有任何位置存在反光条激光数据点，则不再处理这一帧激光数据；如果只有一个位置存在反光条激光数据点，则返回那个激光数据点所对应的位置作为所需提取的反光条位置；

(3)遍历全部候选位置，把序号相连的这些位置保存为从开始序号到结束序号的左闭右闭区间；孤立序号保存为长度为1的区间；

(4)遍历这一帧激光数据后，如果没有区间，则不再处理这一帧激光数据；

(5)确定反光条所在区间：如果只有一个区间，则这个区间就是反光条区间，进入(6)；如果多于一个区间，则对每一个区间按以下步骤进行检测：

1)对区间内全部激光数据求算术平均作为激光雷达到反光条的距离，然后用这个距离乘以激光雷达的角分辨率作为对反光条宽度的估算，如果估算值超过真实宽度，则将该区间标记为假；

2)通过激光雷达在1)中距离上的光斑大小和反光条宽度估算在那个距离上的反光条区间的范围，如果这个实际区间范围和估算范围之差大于1，则将该区间标记为假；

3)如果这个区间的位置显著偏离处理上一帧激光数据得到的反光条位置，则将该区间标记为假；

4)如果这个区间没有被标记为假，则将之标记为真；

全部区间检测完毕后，如果真区间数量不止一个，选取区间长度最大的真区间；如果最大长度真区间不止一个，则选取平均激光强度最高的最大长度真区间；如果出现了相同的最大光强最大长度真区间，选取第一个。

(6)遍历反光条区间，对其位置求平均，作为所要提取的反光条位置；

(7)把这个反光条位置转化为相对于激光雷达的方位角θ_i和距离l_i，保存。

S4、从该时刻之后的每一帧激光数据中提取所述反光条相对于所述激光雷达的位置，将所述反光条的位置P_reflector转换为相对于所述激光雷达的距离l和方位角θ；

S5、当全部激光数据处理结束后，整理全部的距离和方位角(l_i,θ_i)，计算出所述双轮差速移动机器人的朝向Oθ_i和自旋中心O与反光条的位置P_reflector之间连线OP_reflector的夹角

根据该夹角/>

的变化值计算得到所述双轮差速移动机器人的实际自旋角，与里程计旋转角对比，并通过线性拟合得到的斜率就是修正系数，通过该修正系数修正所述双轮差速移动机器人的轮间距。其中，计算夹角/>

的方法为：根据余弦定理可得

R²-2l_iRcosθ_i+l_i ²＝L²

其中，R是激光雷达到自旋中心的距离，L是自旋中心到反光条P_reflector的距离，

令U＝L²-R²，

b_i＝2l_icosθ_i，原方程变为

U＝a_i-b_iR

从而解得R和U，进而得到R和L，通过解三角形△Oθ_iP_reflector即可求得所述夹角

该方法需要基于以下三点假设：

1.机器人自旋时其旋转中心就是两轮中点且在自旋时保持其绝对位置不变。当双轮差速机器人自旋时，如果两个轮子的线速度绝对互为相反数，则这条假设就是真实情况；实际过程中两轮线速度的绝对值之间可能存在很小的差异，这些差异往往来自于测量精度和机械精度不足，但这些差异往往并不足以破坏这个假设，例如实测机器人自旋时的旋转中心在自旋时的漂移不超过毫米级。

2.激光雷达安装在机器人自旋中心正前方(即自旋中心到激光雷达的连线垂直于两轮连线)。在安装过程中，一般选择将激光雷达安装在机器人最前方边缘的中心位置，这往往是在设计机器人结构时所决定的。而由于实际安装精度和机械加工精度等问题，激光雷达可能稍稍偏离中心位置，但偏离量不会超过这两个精度，所以假设一般成立。

3.反光条的宽度足够小，小到能够以一个点代替这个反光条。即本方法把激光雷达探测到的反光条抽象为一个点。例如本方法在具体实施时所使用的反光条宽度为30mm，假设成立。

由于采用上述技术方案，本发明达到以下有益效果：

双轮差速机器人利用反光条和激光雷达对于轮间距的修正过程自动进行，不需要人工实时参与，效率更高；

对移动机器人里程计标定的环境没有特殊要求，也不需要人工对机器人进行小心控制或小心测量，避免了人工操作误差；

利用机器人稳定自旋时激光雷达扫描反光条获取的激光数据来提取反光条位置，并转化为距离和方位角，提取的反光条位置(相对于激光雷达)精度较高，避免了从激光数据中提取固定位置的精度不足和测量从机器人自旋中心到激光雷达的距离时精度不足的问题；

标定时仅需设置反光条，不需要额外的测量仪器，成本较低。

附图说明

图1为应用本实施例的双轮差速移动机器人的激光雷达与反光条相对位置的几何关系示意图；

图2为本实施例双轮差速移动机器人里程标定的轮间距修正方法的整体流程图；

图3为图2中处理激光数据的子流程图；

图4为图3中标记区间子流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明，但并不限于以下实施例。

本实施例一种双轮差速移动机器人里程计标定的轮间距修正方法，该双轮差速移动机器人的自旋中心位于左右轮中心连线的中点，且激光雷达安装在所述自旋中心的正前方，所述轮间距修正方法，参见图2所示的算法总流程图，包括以下步骤：

S1、在双轮差速移动机器人一侧设置反光条，所述反光条应位于所述激光雷达的视界中。反光条的宽度应足够小，小到能够以一个点代替这个反光条。即本方法把激光雷达探测到的反光条抽象为一个点。因此，反光条的宽度优选为小于或等于30mm，例如本实施例所使用的反光条宽度为30mm。反光条距离机器人0.8-1米，本实施为大致1米左右的位置。反光条设置在双轮差速移动机器人一侧是为了尽量使反光条位于机器人上的激光雷达视界边界的附近，以保证旋转时激光雷达能够探测到反光条的时间；而反光条与机器人的距离设为1米左右是为了保持激光数据准确的前提下为机器人留下足够的安全距离，防止机器人旋转时碰到反光条，又要使得反光条足够靠近机器人，这是为了让更多的激光数据点落在反光条上。

S2、设定机器人自旋的角速度，令所述双轮差速移动机器人以预设的角速度开始原地自旋。预设的角速度不必很大，本实施例为0.1rad/s。测量反光条的宽度(即激光能够探测到的尺度)，这是本方法所需要的参数。准备好保存数据的命令，其中需要保存的信息有激光数据(同时保留激光强度和距离，其中前者是为了提取反光条的位置)和里程信息。开始记录数据，所述数据包括所述激光雷达扫描所述反光条得到激光数据和所述双轮差速移动机器人自旋的里程信息。机器人按照设定的角速度开始原地自旋，方向应保证反光条更长久地停留在激光雷达视界中。

当反光条即将移出激光视界之前停止，保存数据。

S3、通过里程信息，定位到所述双轮差速移动机器人开始稳定自旋的时刻；

从每一帧激光数据中提取反光条位置，如图3所示，包括以下步骤：

(1)从一帧激光数据中挑选出强度合适的激光数据点，把每个激光数据点及其在全部激光数据中的序号保存为反光条的候选位置；

(2)如果没有任何位置存在反光条激光数据点，则不再处理这一帧激光数据；如果只有一个位置存在反光条激光数据点，则返回那个激光数据点所对应的位置作为所需提取的反光条位置；

(3)遍历全部候选位置，把序号相连的这些位置保存为从开始序号到结束序号的左闭右闭区间；孤立序号保存为长度为1的区间；

(4)遍历这一帧激光数据后，如果没有区间，则不再处理这一帧激光数据；

(5)确定反光条所在区间：如果只有一个区间，则这个区间就是反光条区间，进入(6)；如果多于一个区间，则对每一个区间按以下步骤进行检测，即算法进入如图4所示的标记区间子流程：

1)对区间内全部激光数据求算术平均作为激光雷达到反光条的距离，然后用这个距离乘以激光雷达的角分辨率作为对反光条宽度的估算，如果估算值超过真实宽度，则将该区间标记为假；

2)通过激光雷达在1)中距离上的光斑大小和反光条宽度估算在那个距离上的反光条区间的范围，如果这个实际区间范围和估算范围之差大于1，则将该区间标记为假；

3)如果这个区间的位置显著偏离处理上一帧激光数据得到的反光条位置，则将该区间标记为假；

4)如果这个区间没有被标记为假，则将之标记为真；

全部区间检测完毕后，如果真区间数量不止一个，选取区间长度最大的真区间；如果最大长度真区间不止一个，则选取平均激光强度最高的最大长度真区间；如果出现了相同的最大光强最大长度真区间，选取第一个。

(6)遍历反光条区间，对其位置求平均，作为所要提取的反光条位置；

(7)把这个反光条位置转化为相对于激光雷达的方位角θ_i和距离l_i，保存。

S4、从该时刻之后的每一帧激光数据中提取所述反光条相对于所述激光雷达的位置，将所述反光条的位置P_reflector转换为相对于所述激光雷达的距离l和方位角θ；

S5、当全部激光数据处理结束后，整理全部的距离和方位角(l_i,θ_i)，计算出所述双轮差速移动机器人的朝向Oθ_i和自旋中心O与反光条的位置P_reflector之间连线OP_reflector的夹角

参见图1，对于每对距离和方位角，其都满足图中的几何关系(图中做出了两对数据(l_i,θ_i)和(l_j,θ_j)，分别位于反光条两侧)。

其中，计算夹角

的方法为：根据余弦定理可得

R²-2l_iRcosθ_i+l_i ²＝L²

其中，R是激光雷达到自旋中心的距离，L是自旋中心到反光条P_reflector的距离，

令U＝L²-R²，a_i＝l_i ²，b_i＝2l_icosθ_i，原方程变为

U＝a_i-b_iR

从而解得R和U，进而得到R和L，通过解三角形△Oθ_iP_reflector即可求得所述夹角

这个角度/>

的变化值就是自旋角，其本身和自旋角无关，其变化率就是自旋角速度。有了自旋角作为实际角度，再和里程计得到的自旋角(即里程计角度的变化)做对比，通过线性拟合计算得到的斜率就是修正系数(线性拟合的截距无意义，因为反光条自旋角和里程计自旋角都是角度差)。最后通过该修正系数修正所述双轮差速移动机器人的轮间距。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (7)

1.一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，该双轮差速移动机器人的自旋中心位于左右轮中心连线的中点，且激光雷达安装在所述自旋中心的正前方，所述轮间距修正方法包括以下步骤：

S1、在所述双轮差速移动机器人一侧设置反光条，所述反光条应位于所述激光雷达的视界中；

S2、令所述双轮差速移动机器人以预设的角速度开始原地自旋，记录数据，所述数据包括所述激光雷达扫描所述反光条得到激光数据和所述双轮差速移动机器人自旋的里程信息；

S3、通过里程信息，定位到所述双轮差速移动机器人开始稳定自旋的时刻；

S4、从该时刻之后的每一帧激光数据中提取所述反光条相对于所述激光雷达的位置，将所述反光条的位置P_reflector转换为相对于所述激光雷达的距离l和方位角θ；

S5、当全部激光数据处理结束后，整理全部的距离和方位角(l_i,θ_i)，计算出所述双轮差速移动机器人的朝向Oθ_i和自旋中心O与反光条的位置P_reflector之间连线OP_reflector的夹角

根据该夹角

的变化值计算得到所述双轮差速移动机器人的实际自旋角，与里程计旋转角对比，并通过线性拟合得到的斜率就是修正系数，通过该修正系数修正所述双轮差速移动机器人的轮间距。

2.根据权利要求1所述的一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，所述步骤S5中，计算夹角

的方法为：根据余弦定理可得

R²+2l_iRcosθ_i+l_i ²＝L²

其中，R是激光雷达到自旋中心的距离，L是自旋中心到反光条P_reflector的距离，

令U＝L²-R²，b_i＝2l_icosθ_i，a_i＝l_i ²，原方程变为

U＝a_i+b_iR

从而解得R和U，进而得到R和L，通过解三角形△Oθ_iP_reflector即可求得所述夹角

3.根据权利要求1所述的一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，所述步骤S4中，从每一帧激光数据中提取反光条位置的方法包括以下步骤：

(1)从一帧激光数据中挑选出强度合适的激光数据点，把每个激光数据点及其在全部激光数据中的序号保存为反光条的候选位置；

(2)如果没有任何位置存在反光条激光数据点，则不再处理这一帧激光数据；如果只有一个位置存在反光条激光数据点，则返回那个激光数据点所对应的位置作为所需提取的反光条位置；

(3)遍历全部候选位置，把序号相连的这些位置保存为从开始序号到结束序号的左闭右闭区间；孤立序号保存为长度为1的区间；

(4)遍历这一帧激光数据后，如果没有区间，则不再处理这一帧激光数据；

(5)确定反光条所在区间：如果只有一个区间，则这个区间就是反光条区间，进入(6)；

(6)遍历反光条区间，对其位置求平均，作为所要提取的反光条位置；

(7)把这个反光条位置转化为相对于激光雷达的方位角θ_i和距离l_i，保存。

4.根据权利要求3所述的一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，所述步骤(5)中，如果多于一个区间，则对每一个区间按以下步骤进行检测：

1)对区间内全部激光数据求算术平均作为激光雷达到反光条的距离，然后用这个距离乘以激光雷达的角分辨率作为对反光条宽度的估算，如果该估算值超过真实宽度，则将该区间标记为假；

2)通过激光雷达在1)中距离上的光斑大小和反光条宽度估算在那个距离上的反光条区间的范围，如果这个实际区间范围和估算范围之差大于1，则将该区间标记为假；

3)如果这个区间的位置显著偏离处理上一帧激光数据得到的反光条位置，则将该区间标记为假；

4)如果这个区间没有被标记为假，则将之标记为真；

全部区间检测完毕后，如果真区间数量不止一个，选取区间长度最大的真区间；如果最大长度真区间不止一个，则选取平均激光强度最高的最大长度真区间；如果出现了相同的最大光强最大长度真区间，选取第一个。

5.根据权利要求1-4任一所述的一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，所述反光条的宽度小于等于30mm。

6.根据权利要求1-4任一所述的一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，所述反光条与所述双轮差速移动机器人之间的距离为0.8-1.2米。

7.根据权利要求1-4任一所述的一种双轮差速移动机器人里程计标定的轮间距修正方法，其特征在于，所述步骤S2中，预设的角速度为0.1rad/s。

Images