CN106289327A - 一种差分移动机器人里程计标定方法 - Google Patents

Abstract

本发明公开了一种差分移动机器人里程计标定方法，所述方法包括：步骤1：机器人在预设区域内进行行走，利用运动捕捉系统对车辆进行位置定位；步骤2：将机器人行走位移传输到处理器，同时读取机器人里程计计算出的行走位移；步骤3：基于运动捕捉系统获得的机器人行走位移和程计计算出的行走位移，计算出补偿参数，对运动捕捉系统进行补偿；步骤4：重复进行步骤1‑步骤3，不断迭代修正补偿参数，直至机器人行走路线与设定一致，实现了差分移动机器人标定效率和精度较高的技术效果。

Description

一种差分移动机器人里程计标定方法

技术领域

本发明涉及机器人运动控制领域，具体地，涉及一种差分移动机器人里程计标定方法。

背景技术

差分移动机器人是一种差分驱动的车体，这种车体具有零转向半径、体积小、运动稳定、价格低廉、负载大等诸多优点，有非常良好的应用和研究价值。由于差分移动机器人使用里程计进行位移、速度的解算，因此需要对机器人里程计进行标定，将系统误差校正到允许误差范围之内。

为了减少系统误差对机器人直线运动和旋转运动产生影响，目前通用的标定方法是设计直线行驶使用标尺测量实际位移，设计原地旋转方式使用角度尺测量实际角度变化，设计修正系数对进行里程计标定。这种方法的优点在于能够简易实线里程计的基本标定要求，实现成本低，简单易行；缺点是标尺和角度尺测量精度不高，不利于实现里程计精准标定的需求，且测量过程中引入大量的测量误差。

综上所述，本申请发明人在实现本申请发明技术方案的过程中，发现上述技术至少存在如下技术问题：

在现有技术中，现有的差分移动机器人标定方法存在效率较低、精度较差、测量误差较大的技术问题。

发明内容

本发明提供了一种差分移动机器人里程计标定方法，解决了现有的差分移动机器人标定方法存在效率较低、精度较差、测量误差较大的技术问题，实现了差分移动机器人标定效率和精度较高的技术效果。

为解决上述技术问题，本申请提供了一种差分移动机器人里程计标定方法，所述方法包括：

步骤1：机器人在预设区域内进行行走，利用运动捕捉系统对车辆进行位置定位；

步骤2：将机器人行走位移传输到处理器，同时读取机器人里程计计算出的行走位移；

步骤3：基于运动捕捉系统获得的机器人行走位移和程计计算出的行走位移，计算出补偿参数，对里程计相关参数进行补偿；

步骤4：重复进行步骤1-步骤3，不断迭代修正补偿参数，直至机器人行走路线与设定一致。

其中，本申请中的一种基于运动捕捉系统视频的差分机器人底盘标定方法的解决途径，利用计算机视觉技术对机器人行走路线进行定位，将捕捉系统测得行走结果与实际行走结果综合分析与迭代计算，达到对机器人底盘进行标定的要求，该方法可脱离人为手工测量带来的测量误差，也可提高测量精度，提高标定的准确性。

其中，运动捕捉系统为现有技术中的系统，具体为：运动捕捉系统选用6个光学相机，均匀环绕被测物体周围，将机器人车身平面上的三点作为标记，记录并识别这些标志在视野空间中的位置，从而得到车身的运动轨迹。将运动轨迹转化为世界坐标系下，机器人车身的运动轨迹可以在坐标系下表示。此外，记录车身上三个标志点，可以计算出车身旋转的角度，从而得到完整的路径、位置和方位信息约束。

本发明提供一种基于运动捕捉方式的里程计标定方法，机器人在给定的区域内运动，通过运动捕捉系统提供机器人定位信息进行标定的方法(创新点：替换了卷尺手测，改用了视觉测量)，首先机器人按照给定区域进行行走，利用运动捕捉相机对车辆进行位置定位，将机器人行走位移传输到电脑终端，同时读取机器人里程计计算出的行走位移。根据两者位移计算出补偿参数，对系统进行补偿。重复进行行走标定过程，不断迭代修正补偿系数，直到机器人行走路线与设定一致，且结果误差满足系统的精度要求。

进一步的，所述机器人建立有运动学模型，为了表征机器人在平面内中的位置，建立平面全局参考框架和机器人局部参考框架。选定空间中一点作为全局参考框架的原点，从原点为坐标系基准点，建立直角坐标系，横轴X和纵轴Y的指向为选定方向；选取机器人底盘几何中心点作为位置参考点，坐标位置由x和y确定。机器人的初始位置为(X₀,Y₀,θ₀),经过时间δt之后，运动到位置(X,Y,θ)坐标系内机器人的坐标变化为：

$\left\{\begin{array}{c}x=x_0+\frac{{\mathrm{\δd}}_r+{\mathrm{\δd}}_l}{2}cos\mathrm{\θ}\\ y=y_0+\frac{{\mathrm{\δd}}_r+{\mathrm{\δd}}_l}{2}\sin \mathrm{\θ}\end{array}\right.---\left(3\right)$

其中，θ为机器人移动的偏航角度，θ₀为上一时刻的偏航角度，δd_r和δd_l为左右两轮行走的位移。

进一步的，所述机器人建立有里程计系统误差模型，机器人系统带有误差修正项的运动公式为：

$\left\{\begin{array}{c}\mathrm{\θ}={\mathrm{\θ}}_0+\frac{{\mathrm{\δd}}_r-{\mathrm{\δd}}_l}{E_b*b_{no\min al}}\\ \mathrm{\δ}dl=E_s*Cl*Dl/2*vl*\mathrm{\δ}t\\ \mathrm{\δ}dr=E_s*Cr*Dr/2*vr*\mathrm{\δ}t\end{array}\right.---\left(7\right)$

其中，θ为机器人移动的偏航角度，θ₀为上一时刻的偏航角度，δd_r和δd_l为左右两轮行走的位移，E_b为两轮轮间距不精确带来的误差，E_s为两轮直径的实际平均值与标称平均值不相等带来的误差，b_nominal测量的轮间距，机器人左右两轮的补偿系数为Cl和Cr，Dr和Dl分别为机器人右轮与左轮直径，vl和vr为左右两轮的运动角速度，δt为运动时间。

进一步的，方法中的误差标定包括：误差E_s标定、误差E_d标定、误差E_b标定。

进一步的，误差E_s标定具体包括：

STEP1系统初始化设定：选定机器人起始位置为原点，里程计的初始里程和速度设定为零；

STEP2机器人直线运动设定：设定机器人直线行驶的距离S_ordometry长度为L，记录机器人行走距离为S_ordometry时，使用运动捕捉系统记录距离出发点的实际位移S_actual；

STEP3重复实验：重复进行STEP2实验，得到五组S_actual数据；

STEP4计算补偿参数E_s：取五次S_actual的平均值计算补偿参数E_s，修正系统模型，重复进行STEP1-STEP3实验，将参数E_s迭代更新，直至S_actual满足精度要求。

进一步的，误差E_d、E_b标定具体包括：

STEP1系统初始化设定：选定机器人起始位置为原点，里程计的初始里程和速度设定为零；

STEP2机器人矩形顺时针运动设定：选定边长为L的正方形区域，系统设定机器人顺时针运动，利用标定的E_s修正δd_r和δd_l；设定机器人在正方形每条边的运动距离为L，在拐角处旋转90度，利用运动捕捉系统记录运动结束时的终点位置与起点位置是的横轴与纵轴偏移x和y；

STEP3顺时针重复实验：重复进行STEP2实验，记录五组横轴与纵轴偏移，得到平均值x_cw和y_cw；

STEP4机器人矩形顺时针运动设定：同STEP2，机器人设定沿着正方形进行逆时针方向运动一周，利用运动捕捉系统记录运动结束时的终点位置与起点位置是的横轴与纵轴偏移x和y；

STEP5逆时针重复实验：重复进行STEP4实验，记录五组横轴与纵轴偏移，得到平均值x_ccw和y_ccw；

STEP6计算补偿参数E_d和E_b：

利用(x_cw,y_cw)或(x_ccw,y_ccw)计算过渡补偿参数α和β：

$\left\{\begin{array}{c}\mathrm{\α}=(x_{ccw}-x_{ccw})/(-4L)\\ \mathrm{\β}=-(x_{cw}+x_{ccw})/(-4L)\end{array}\right.---\left(8\right)$

补偿参数E_d和E_b为：

R＝(L/2)/sin(β/2) (9)

利用重复进行STEP1-STEP5实验，将参数E_d和E_b迭代更新，直至机器人行走路线基本与正方形区域重合，行走偏移x和y满足精度要求。

本申请提供的一个或多个技术方案，至少具有如下技术效果或优点：

1)利用运动捕捉定位方式进行机器人底盘里程计的标定，应用中，实现了运动定位对里程计码盘定位的修正功能，实现了非手动测量进行标定的繁琐低精度的方法，丰富了底盘里程计的标定手段；

2)设计行走矩形区域路线实现机器人直线和旋转角度的双重校正，应用中，机器人行走在设计的矩形区域路线上，避免重复行走直线和重复圆圈行走进行标定的低效率，减少了工作任务量。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定；

图1机器人行走标定方法示意图；

图2机器人行走标定方法功能模块示意图；

图3移动机器人运动模型分析示意图；

图4机器人顺时针行走标定方法示意图；

图5机器人逆时针行走标定方法示意图。

具体实施方式

本发明提供了一种差分移动机器人里程计标定方法，解决了现有的差分移动机器人标定方法存在效率较低、精度较差、测量误差较大的技术问题，实现了差分移动机器人标定效率和精度较高的技术效果。

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在相互不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述范围内的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

针对现有差分机器人底盘标定方法存在的问题，本发明提供一种基于运动捕捉方式的里程计标定方法。如图1所示，机器人在给定的区域内运动，通过运动捕捉系统提供机器人定位信息进行标定的方法(创新点：替换了卷尺手测，改用了视觉测量)。图2为标定的流程图，首先机器人按照给定区域进行行走，利用运动捕捉相机对车辆进行位置定位，将机器人行走位移传输到电脑终端，同时读取机器人里程计计算出的行走位移。根据两者位移计算出补偿参数，对系统进行补偿。重复进行行走标定过程，不断迭代修正补偿系数，直到机器人行走路线与设定一致，里程计的里程结果和实际行走结果满足系统的精度要求。其运动学模型(见图3)、误差模型与标定步骤如下。

1)建立差分驱动机器人的运动学

在指定的时间间隔δt内，计算机器人两轮行走的位移和角度变化。如图3所示，假定机器人的初始位置为(X₀,Y₀,θ₀),经过时间δt之后，运动到位置(X,Y,θ)。因此，左右两轮的运动位移δdl,r为

$\left\{\begin{array}{c}\mathrm{\δ}dl=Dl/2*vl*\mathrm{\δ}t\\ \mathrm{\δ}dr=Dr/2*vr*\mathrm{\δ}t\end{array}\right.---\left(1\right)$

其中，vl和vr为左右两轮的运动角速度，Dr和Dl分别为机器人左右两轮直径。

机器人移动的偏航角度θ的变化为

$\mathrm{\θ}={\mathrm{\θ}}_0+\frac{{\mathrm{\δd}}_r-{\mathrm{\δd}}_l}{b_{no\min al}}---\left(2\right)$

其中，θ为机器人此时的偏航角度，θ₀为上一时刻的偏航角度，δd_r和δd_l为左右两轮行走的位移，w为机器人两轮间距。机器人的实际路程为两轮行走的路程的平均值，因此，坐标系内机器人的坐标变化为

$\left\{\begin{array}{c}x=x_0+\frac{{\mathrm{\δd}}_r+{\mathrm{\δd}}_l}{2}cos\mathrm{\θ}\\ y=y_0+\frac{{\mathrm{\δd}}_r+{\mathrm{\δd}}_l}{2}sin\mathrm{\θ}\end{array}\right.---\left(3\right)$

2)建立差分驱动机器人的系统误差模型

机器人里程计的系统误差模型主要分为三部分，分别为“两轮轮直径不等引起的误差”、“两轮轮间距不精确带来的误差”和“两轮直径的实际平均值与标称平均值不相等带来的误差”，误差分别记为E_d、E_b和E_s。

定义E_d为两轮实际直径之比

$E_d=\frac{Dr}{Dl}---\left(4\right)$

其中，Dr和Dl分别为机器人右轮与左轮直径。理想的E_d的值为1，E_d决定了机器人直线运动的精度。

定义E_b为实际轮间距与测量的轮间距

$E_b=\frac{b_{actual}}{b_{no\min al}}---\left(5\right)$

其中，b_actual和b_nominal分别为实际轮间距与测量的轮间距。理想的E_b的值为1，E_b决定了机器人旋转运动的精度。

定义E_s为实际运动路程与里程计记录路程之比：

$E_s=\frac{S_{actual}}{S_{ordometry}}---\left(6\right)$

其中，S_actual和S_ordometry分别为机器人实际运动的里程和里程计记录机器人运动的里程。理想的E_s的值为1。E_s代表了两轮直径的实际平均值与标称平均值不相等带来的误差，表现为里程计数据不能实时表示实际行走的路程，同时影响机器人的直线运动和旋转运动的精度，需要在机器人系统误差E_d、E_b标定之前，进行先行标定。

定义机器人左右两轮的补偿系数为Cl和Cr：

$\left\{\begin{array}{c}Cr=\frac{2}{1/E_d+1}\\ Cl=\frac{2}{E_d+1}\end{array}\right.---\left(6\right)$

于是，最终得到了机器人系统带有误差修正项的运动公式：

$\left\{\begin{array}{c}\mathrm{\θ}={\mathrm{\θ}}_0+\frac{{\mathrm{\δd}}_r-{\mathrm{\δd}}_l}{E_b*b_{no\min al}}\\ \mathrm{\δ}dl=E_s*Cl*Dl/2*vl*\mathrm{\δ}t\\ \mathrm{\δ}dr=E_s*Cr*Dr/2*vr*\mathrm{\δ}t\end{array}\right.---\left(7\right)$

实现步骤为：

1)误差E_s标定

STEP1系统初始化设定

选定机器人起始位置为原点，里程计的初始里程和速度设定为零。

STEP2机器人直线运动设定

设定机器人直线行驶的距离S_ordometry长度为L。记录机器人行走距离为S_ordometry时，使用运动捕捉系统记录距离出发点的实际位移S_actual。

STEP3重复实验

重复进行STEP2实验，得到五组S_actual数据。

STEP4计算补偿参数E_s

取五次S_actual的平均值计算补偿参数E_s，修正系统模型。重复进行STEP1-STEP3实验，将参数E_s迭代更新，直至S_actual满足精度要求为止。

2)误差E_d、E_b标定

STEP1系统初始化设定

选定机器人起始位置为原点，里程计的初始里程和速度设定为零。

STEP2机器人矩形顺时针运动设定

选定边长为L的正方形区域，系统设定机器人沿着边缘顺时针运动，利用步骤1)中标定的E_s修正δd_r和δd_l。如图4所示，设定机器人在正方形每条边的运动距离为L，在拐角处旋转90°。利用运动捕捉系统记录运动结束时的终点位置与起点位置是的横轴与纵轴偏移x和y。

STEP3顺时针重复实验

重复进行STEP2实验，记录五组横轴与纵轴偏移，得到平均值x_cw和y_cw。

STEP4机器人矩形顺时针运动设定

同STEP2，如图5所示，机器人设定沿着正方形边缘进行逆时针方向运动一周。利用运动捕捉系统记录运动结束时的终点位置与起点位置是的横轴与纵轴偏移x和y。

STEP5逆时针重复实验

重复进行STEP4实验，记录五组横轴与纵轴偏移，得到平均值x_ccw和y_ccw。

STEP6计算补偿参数E_d和E_b

利用(x_cw,y_cw)或(x_ccw,y_ccw)计算过渡补偿参数α和β

$\left\{\begin{array}{c}\mathrm{\α}=(x_{ccw}-x_{ccw})/(-4L)\\ \mathrm{\β}=-(x_{cw}+x_{ccw})/(-4L)\end{array}\right.---\left(8\right)$

于是，补偿参数E_d和E_b为

R＝(L/2)/sin(β/2) (9)

利用重复进行STEP1-STEP5实验，将参数E_d和E_b迭代更新，直至机器人行走路线基本与正方形区域重合，行走偏移x和y满足精度要求为止。

如图3所示，其具体实施步骤为：

1)绘制定长的矩形区域，机器人初始位置选定为矩形的左上或右下拐角处，两轮中心线与行驶线垂直。机器人操作人员设置机器人操作方式为远程操作模式，清零里程计初始状态，并设置运动捕捉系统初始位置为坐标原点；

2)操作人员远程操作机器人直线行走，通过远程记录机器人左右里程计的行走距离，通过运动捕捉系统记录机器人两轮分别行走的起点和终点的位移，得到修正系数E_s。设置机器人行走正方形区域，得到修正系数E_d和E_b；

3)重复进行步骤2)中操作，迭代修正里程计的修正系数E_s。当修正系数E_s满足精度要求时，迭代修正系数E_d和E_b。

4)当直线行走与转弯行驶的误差均满足精度要求时，标定结束，得到机器人底盘的最终的标定修正结果。

上述本申请实施例中的技术方案，至少具有如下的技术效果或优点：

1)利用运动捕捉定位方式进行机器人底盘里程计的标定，应用中，实现了运动定位对里程计码盘定位的修正功能，实现了非手动测量进行标定的繁琐低精度的方法，丰富了底盘里程计的标定手段；

2)设计行走矩形区域路线实现机器人直线和旋转角度的双重校正，应用中，机器人行走在设计的矩形区域路线上，避免重复行走直线和重复圆圈行走进行标定的低效率，减少了工作任务量。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (6)

1.一种差分移动机器人里程计标定方法，其特征在于，所述方法包括：

步骤1：机器人在预设区域内进行行走，利用运动捕捉系统对车辆进行位置定位；

步骤2：将机器人行走位移传输到处理器，同时读取机器人里程计计算出的行走位移；

步骤3：基于运动捕捉系统获得的机器人行走位移和程计计算出的行走位移，计算出补偿参数，对里程计相关参数进行补偿；

步骤4：重复进行步骤1-步骤3，不断迭代修正补偿参数，直至机器人行走路线与设定一致。

2.根据权利要求1所述的差分移动机器人里程计标定方法，其特征在于，所述机器人建立有运动学模型，机器人的初始位置为(X₀,Y₀,θ₀),经过时间δt之后，运动到位置(X,Y,θ)坐标系内机器人的坐标变化为：

$\left\{\begin{array}{c}x=x_0+\frac{{\mathrm{\δd}}_r+{\mathrm{\δd}}_l}{2}cos\mathrm{\θ}\\ y=y_0+\frac{{\mathrm{\δd}}_r+{\mathrm{\δd}}_l}{2}\sin \mathrm{\θ}\end{array}\right.---\left(3\right)$

其中，θ为机器人移动的偏航角度，为全局框架和局部框架的角度差，θ₀为上一时刻的偏航角度，δd_r和δd_l为左右两轮行走的位移，x、y、x₀、y₀均为位置坐标。

3.根据权利要求2所述的差分移动机器人里程计标定方法，其特征在于，所述机器人建立有里程计系统误差模型，机器人系统带有误差修正项的运动公式为：

$\left\{\begin{array}{c}\mathrm{\θ}={\mathrm{\θ}}_0+\frac{{\mathrm{\δd}}_r-{\mathrm{\δd}}_l}{E_b*b_{no\min al}}\\ \mathrm{\δ}dl=E_s*Cl*Dl/2*vl*\mathrm{\δ}t\\ \mathrm{\δ}dr=E_s*Cr*Dr/2*vr*\mathrm{\δ}t\end{array}\right.---\left(7\right)$

其中，θ为机器人移动的偏航角度，θ₀为上一时刻的偏航角度，δd_r和δd_l为左右两轮行走的位移，E_b为两轮轮间距不精确带来的误差，E_s为两轮直径的实际平均值与标称平均值不相等带来的误差，b_nominal测量的轮间距，机器人左右两轮的补偿系数为Cl和Cr，Dr和Dl分别为机器人右轮与左轮直径，vl和vr为左右两轮的运动角速度，δt为运动时间。

4.根据权利要求3所述的差分移动机器人里程计标定方法，其特征在于，方法中的误差标定包括：误差E_s标定、误差E_d标定、误差E_b标定。

5.根据权利要求4所述的差分移动机器人标定方法，其特征在于，误差E_s标定具体包括：

STEP1系统初始化设定：选定机器人起始位置为原点，里程计的初始里程和速度设定为零；

STEP2机器人直线运动设定：设定机器人直线行驶的距离S_ordometry长度为L，记录机器人行走距离为S_ordometry时，使用运动捕捉系统记录距离出发点的实际位移S_actual；

STEP3重复实验：重复进行STEP2实验，得到五组S_actual数据；

STEP4计算补偿参数E_s：取五次S_actual的平均值计算补偿参数E_s，修正系统模型，重复进行STEP1-STEP3实验，将参数E_s迭代更新，直至S_actual满足精度要求。

6.根据权利要求4所述的差分移动机器人里程计标定方法，其特征在于，误差E_d、E_b标定具体包括：

STEP1：系统初始化设定：选定机器人起始位置为原点，里程计的初始里程和速度设定为零；

STEP2：机器人矩形顺时针运动设定：选定边长为L的正方形区域，系统设定机器人顺时针运动，利用标定的E_s修正δd_r和δd_l；设定机器人在正方形每条边的运动距离为L，在拐角处旋转90度，利用运动捕捉系统记录运动结束时的终点位置与起点位置是的横轴与纵轴偏移x和y；

STEP3：顺时针重复实验：重复进行STEP2实验，记录五组横轴与纵轴偏移，得到平均值x_cw和y_cw；

STEP4：机器人矩形顺时针运动设定：同STEP2，机器人设定沿着正方形进行逆时针方向运动一周，利用运动捕捉系统记录运动结束时的终点位置与起点位置是的横轴与纵轴偏移x和y；

STEP5：逆时针重复实验：重复进行STEP4实验，记录五组横轴与纵轴偏移，得到平均值x_ccw和y_ccw；

STEP6：计算补偿参数E_d和E_b：

利用(x_cw,y_cw)或(x_ccw,y_ccw)计算过渡补偿参数α和β：

$\left\{\begin{array}{c}\mathrm{\α}=(x_{ccw}-x_{ccw})/(-4L)\\ \mathrm{\β}=-(x_{cw}+x_{ccw})/(-4L)\end{array}\right.---\left(8\right)$

补偿参数E_d和E_b为：

R＝(L/2)/sin(β/2) (9)

利用重复进行STEP1-STEP5实验，将参数E_d和E_b迭代更新，直至机器人行走路线基本与正方形区域重合，行走偏移x和y满足精度要求。