CN109916431B - 一种针对四轮移动机器人的车轮编码器标定算法 - Google Patents

Abstract

本发明涉及移动机器人定位技术领域，公开了一种针对四轮移动机器人的车轮编码器标定算法。包括：机器人沿特定的轨迹运动，并收集车轮编码器的测量值；在每一个时间段内，运行迭代最近点算法以得到激光雷达传感器坐标系原点的位姿估计值，测量和估计值组成每个时间段的数据样本；线性参数估计：基于机器人左右轮半径r_L、r_R和轮距b构建线性参数

对J₂₁、J₂₂进行线性估计；标定参数估计：确定标定参数为b,l_x,l_y,l_θ，其中l为传感器坐标系与机器人本体坐标系之间的相对位姿关系，建立标定问题描述中得到的对数似然函数，计算标定参数。采用本发明的车轮编译器的标定算法，可以在多种环境下准确得到传感器、执行器以及位姿关系的参数，无需人工干预。

Description

一种针对四轮移动机器人的车轮编码器标定算法

技术领域

本发明涉及移动机器人定位技术领域，特别是一种针对四轮移动机器人的车轮编码器标定算法。

背景技术

移动机器人定位导航是机器人研究领域的一个热点，为了提高定位精度，移动机器人通常融合多个传感器的数据进行定位，从而对各类传感器取长补短。在进行多传感器数据融合前，对各传感器的标定是必不可少的环节。轮式移动机器人按提供动力的车轮数量可主要分为两轮驱动，四轮驱动与多轮驱动。相对于两轮移动机器人，在复杂的地面环境中，四轮移动机器人的可通过性和车身的稳定性更好，更适合在崎岖不平的环境下工作。

在移动机器人上布置的传感器可以分为2类，本体感知传感器(proprioceptivesensor)以及外部感知传感器(exteroceptive sensor)。本体感知传感器用于感知机器人本体的状态信息，如机器人车轮转速、关节角度、温度等；而外部感知传感器一般用于感知外部环境的信息，如障碍物的方位。移动机器人比较典型的传感器布置形式为里程计(本体感知传感器)加一个或多个外部感知传感器(如激光雷达)。里程计定位方法是通过车轮编码器读数结合移动机器人运动学参数进行计算实现定位的，但由于机器人加工及安装存在误差，从而导致运动学参数存在误差，因此计算得到的里程计定位信息包含系统误差。外部感知传感器，如激光雷达，获取的定位信息是传感器本体的定位信息，在传感器信息融合过程中，需要将所有的传感器定位信息转换到同一坐标系下进行融合。由于外部感知传感器同样存在安装误差，因此其信息在进行坐标转换的过程中也引入了系统误差。上述两类系统误差降低了移动机器人的定位精度，影响了机器人的导航性能。由于移动机器人定位导航以及地图创建等大部分上层算法都需要利用基于车轮编码的里程计信息，因此移动机器人里程计系统误差标定一直以来都获得很大的关注。

发明内容

本发明所要解决的技术问题是：针对上述存在的问题，提供了一种针对四轮移动机器人的车轮编码器标定算法。

本发明采用的技术方案如下：一种针对四轮移动机器人的车轮编码器标定算法，包括以下过程：

步骤1，机器人沿特定的轨迹运动，并收集车轮编码器的测量值；

步骤2，在每一个时间段内，运行迭代最近点算法以得到激光雷达传感器坐标系原点的位姿估计值

车轮编码器的测量值和激光雷达传感器坐标系原点的位姿估计值组成每个时间段的数据样本；

步骤3，针对每一个数据样本，基于机器人左轮半径r_L、右轮半径r_R和左右轮距b构建机器人线性参数

对参数J₂₁、J₂₂进行线性估计；

步骤4，针对每一个数据样本，确定标定参数为b,

其中

为传感器坐标系与机器人本体坐标系之间的相对位姿关系，x、y为坐标轴，

为传感器坐标系相对于机器人本体坐标系的旋转角，标定问题描述为寻找最能解释测量结果的参数的最大似然问题，建立标定问题描述中得到的对数似然函数，计算标定参数。

进一步的，所述步骤1中，所述车轮编码器的测量参数包括左车轮转速ω_L(t)和右车轮转速ω_R(t)。

进一步的，所述步骤1中的轨迹采用封闭的轨迹。

进一步的，所述封闭的轨迹采用矩形或者8字型。

进一步的，所述步骤3包括以下过程：针对每一个数据样本，机器人转角

其中L_k是一个由ω_L与ω_R积分所得的行向量；

因为激光雷达传感器是固定于移动机器人本体，传感器的旋转角和车体旋转角是相等的，激光雷达传感器旋转角的估计值

代替机器人旋转角

通过加权最小二乘方法估算J₂₁，J₂₂。

进一步的，所述步骤4中，标定问题描述过程为：将移动机器人以及其传感器位姿关系由如下公式表示：

其中，q^k，q^k+1分别为k和k+1时刻机器人本体坐标系与世界坐标系之间的位姿变换关系，s^k为k时刻到k+1时刻传感器坐标系发生的位姿转换，

定义为欧几里得群的群运算：

标定问题定义为寻找最能解释测量结果的参数的最大似然问题，得到对数似然

函数为：

其中，‖a‖² _A＝a^TAa是向量a的A范数，

并且定义

为

的逆运算：

进一步的，当传感器为激光雷达时，s^k可以通过迭代最近点算法估计得到。

进一步的，所述步骤4中，标定参数的计算过程为：

根据对数似然函数与移动机器人动力学模型，我们可以计算出小车移动速度v以及利用线性方程获得

最终，

可写为线性方程

其中

可以通过积分求得；

将剩余的未知参数组合为一个5维向量

那么对数似然函数可写为关于未知量

的二次函数，为

的形式其中C表示一常数；

接下来定义一个Q_k2×5的矩阵，矩阵中均为已知量或常量：

似然最大化简化为具有二次约束的二次问题：

且

由上式可得，

其中

最后通过解带约束的最小二乘可得到待标定的相关参数。

进一步的，首先，建立对四轮移动机器人运动模型：

假设机器人在一个平面上移动，移动机器人的线速度在车身坐标系中表示为v＝[v_x v_y 0]^T，旋转角速度矢量为ω＝[0 0 ω]；

描述机器人在全局坐标系下的状态向量为q＝[X Y θ]^T，则

表示全局坐标系下的速度的向量，其中

变量

和

与车身坐标系中速度矢量计算如下：

忽略车轮和表面之间的纵向滑动，得出：

v_xi＝r_iω_i

其中，v_xi是在车身坐标系中的第i个车轮总速度矢量v_i的纵向分量，r_i表示该车轮的有效滚动半径，整个移动小车旋转角速度用以下公式求得：

其中，v_i为第i个轮子绕旋转半径方向的速度，L_i为第i个轮子到旋转中心的距离，V为移动小车绕旋转半径方向的速度，L_c为小车中心到旋转中心的距离，ω为小车旋转角速度。‖·‖表示欧几里得范数；

获得车轮转速与机器人速度之间的近似关系：

其中，J是关于移动机器人本体相关参数的函数：

与现有技术相比，采用上述技术方案的有益效果为：采用本发明的车轮编译器的标定算法，可以准确得到代表其单个传感器、执行器以及它们之间位姿关系的参数；本发明的标定方法的全局最优解是以封闭的形式找到的，只要它们能够激发所有参数，机器人轨迹可以自由选择，因此可以实现无人参与的情况下运行，无需人工干预，无需预先校准仪器，无需将名义参数作为初始值；本发明专利技术方案可以在多种环境下进行标定。

附图说明

图1是全向移动四轮小车速度、坐标关系示意图。

图2是四轮滑动导向移动机器人转向运动的运动分解示意图。

图3是世界坐标系、移动机器人本体坐标系、传感器坐标支架变换关系以及各变量定义示意图。

图4是标定过程中移动小车行走的矩形轨迹示意图。

图5是标定过程中移动小车行走的8字型轨迹示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

首先，四轮移动机器人运动模型建立：

如图1所示，假设机器人在一个平面上移动，左侧每个车轮的有效半径为r_L，右侧每个车轮半径为r_R，ω_L和ω_R分别为左右轮的转速。那么可以得出车轮转速与机器人速度之间的近似关系：

其中，J是关于移动机器人本体相关参数的函数：

其中b为车轮轮距。

从上述系列方程可以清楚地看出，理论上，小车本体的速度v_x和ω可以通过相关参数矩阵转换为小车车轮转速ω_L和ω_R。至此，四轮移动机器人的运动模型建模完成，后面介绍本标定算法标定车轮半径r_L，r_R和轮距b，以及传感器安装位置相对于移动机器人本体的位姿关系。

标定问题描述：

如图3所示，q^k，q^k+1分别为k和k+1时刻机器人本体坐标系与世界坐标系之间的位姿变换关系，

为传感器坐标系与机器人本体坐标系之间的相对位姿关系，s^k为k时刻到k+1时刻传感器坐标系发生的位姿转换，移动小车以及其传感器位姿关系可由如下公式表示：

其中，

定义为欧几里得群的群运算：

当传感器为激光雷达时，s^k可以通过迭代最近点算法(Point-to-Line IterativeClosest Point，PLICP)估计得到。

接下来，将标定问题定义为寻找最能解释测量结果的参数的最大似然问题，并得到关于参数和测量结果的目标函数。我们假设可以得到传感器位移的估计值

是以均值为s^k，协方差为∑k的高斯分布。对数似然

为：

其中，‖a‖² _A＝a^TAa是向量a的A范数，

并且定义

为

的逆运算：

至此我们已经将校准简化为一个优化问题。

车轮编码器的标定算法：

(1)在小车绕特定的轨迹运动时，收集车轮编码器的相关测量值，包括左右车轮转速ω_L(t),ω_R(t)；

通常情况下，我们使移动机器人在封闭无人干扰的环境中，可以沿着图4中的矩形、或者图5中的“8”字形的闭合轨迹路径运动。

(2)在每一个时间段内，运行迭代最近点算法以得到激光雷达传感器坐标系原点的位姿估计值

通过迭代最近点算法得到前后两帧时刻之间激光雷达的坐标系变换矩阵。同时，通过车轮编码器测量并记录移动机器人左右两侧轮子的转速ω_L(t)，ω_R(t)。最终

与ω_L(t)，ω_R(t)组成一组数据样本：

(3)因为有多个时间段，以下步骤循环N次，N为整个标定过程中所有时间段的次数；

A.J₂₁,J₂₂的线性估计：

参数

只需考虑小车旋转测量值

便可独立于其他参数而估计得到。实际上，小车旋转角度和激光雷达传感器旋转角度是都相等的，从机器人的运动学分析可知，机器人的旋转位移是关于轮速和里程计参数的线性函数，

与J₂₁,J₂₂呈线性关系，因此，通过线性最小二乘法可以方便、可靠地恢复参数。更准确地说，机器人转角

其中L_k可以通过积分计算得到。

B.可以通过转角的估计值

标准差

通过最小二乘方法估算求得J₂₁，J₂₂，其他标定参数的非线性估计：

此时，通过A过程，我们认为参数

已经被估算出来。接下来的标定剩下的其他参数：b,

参数b一旦知道，我们便可以根据

知道r_R和r_L。

上述标定参数可以通过标定问题描述为寻找最能解释测量结果的参数的最大似然问题，建立标定问题描述中得到的对数似然函数，计算得到标定参数。

下面介绍一种标定参数计算的实施例：

因为

根据标定问题描述中所得的对数似然函数可得如下等式：

根据动力学模型，我们可以计算出小车移动速度v以及利用线性方程获得

最终，

可写为线性方程

其中

为可求得的已知量。

我们将剩余的未知参数组合为一个5维向量

那么对数似然函数可写为关于

的二次函数。

则对数似然函数可改写为

的形式，其中C为一常数：

其中：

我们已经将似然最大化简化为具有二次约束的二次问题：

且

由上式可得，

其中

最后通过解带约束的最小二乘可得到待标定的相关参数。先根据已知量计算出Q_k以及M，之后利用拉格朗日方法计算出

移动轨迹选择：

上述针对四轮移动机器人的车轮编码器标定算法，在实际实验测试中，不必要使移动机器人按照特定的轨迹路径行走。移动小车测得的数据越多，标定结果会更准确，选择在封闭环境中让待标定移动机器人自由移动行走，以测得足够数量的数据。如果移动机器人轮子与测试地面摩擦力较小，则在控制移动机器人移动时建议速度小一点，以防止轮速过快导致车轮打滑和传感器震动产生测量噪声，最终影响标定结果的准确度。在实际测试标定中，我们使移动机器人沿矩形、“8”字形的封闭轨迹运动，如图4、5所示。该轨迹包含直线运动和旋转运动，能有效激发各标定参数，使得各传感器内外参数得到准确地标定结果。

本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。如果本领域技术人员，在不脱离本发明的精神所做的非实质性改变或改进，都应该属于本发明权利要求保护的范围。

Claims (9)

1.一种针对四轮移动机器人的车轮编码器标定算法，其特征在于，包括以下过程：

步骤1，机器人沿特定的轨迹运动，并收集车轮编码器的测量值；

步骤2，在每一个时间段内，运行迭代最近点算法以得到激光雷达传感器坐标系原点的位姿估计值

车轮编码器的测量值和激光雷达传感器坐标系原点的位姿估计值组成每个时间段的数据样本；

步骤3，针对每一个数据样本，基于机器人左轮半径r_L、右轮半径r_R和左右轮距b构建机器人线性参数

对参数J₂₁、J₂₂进行线性估计；

步骤4，针对每一个数据样本，确定标定参数为b,l_x,l_y,l_θ,其中l为传感器坐标系与机器人本体坐标系之间的相对位姿关系，x、y为坐标轴，l_θ为传感器坐标系相对于机器人本体坐标系的旋转角，标定问题描述为寻找最能解释测量结果的参数的最大似然问题，建立标定问题描述中得到的对数似然函数，计算标定参数。

2.如权利要求1所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，所述步骤1中，所述车轮编码器的测量参数包括左车轮转速ω_L(t)和右车轮转速ω_R(t)。

3.如权利要求1所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，所述步骤1中的轨迹采用封闭的轨迹。

4.如权利要求3所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，所述封闭的轨迹采用矩形或者8字型。

5.如权利要求2所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，所述步骤3包括以下过程：针对每一个数据样本，机器人转角

其中L_k是一个由ω_L与ω_R积分所得的行向量；

因为激光雷达传感器是固定于移动机器人本体，传感器的旋转角和车体旋转角是相等的，激光雷达传感器旋转角的估计值

代替机器人旋转角

通过加权最小二乘方法估算J₂₁，J₂₂。

6.如权利要求5所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，所述步骤4中，标定问题描述过程为：将移动机器人以及其传感器位姿关系由如下公式表示：

其中，q^k，q^k+1分别为k和k+1时刻机器人本体坐标系与世界坐标系之间的位姿变换关系，s^k为k时刻到k+1时刻传感器坐标系发生的位姿转换，⊕定义为欧几里得群的群运算：

标定问题定义为寻找最能解释测量结果的参数的最大似然问题，得到对数似然函数为：

其中，||a||² _A＝a^TAa是向量a的A范数，

并且定义

为

的逆运算：

7.如权利要求6所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，当传感器为激光雷达时，s^k可以通过迭代最近点算法估计得到。

8.如权利要求7所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，所述步骤4中，标定参数的计算过程为：

根据对数似然函数与移动机器人动力学模型，计算出小车移动速度v以及利用线性方程获得

最终,

可写为线性方程

其中

可以通过积分求得；

将剩余的未知参数组合为一个5维向量

那么对数似然函数可写为关于未知量

的二次函数，为

的形式其中C表示一常数；

均为中间参数，

为机器人转角的估计值，

为激光雷达传感器坐标系原点的x轴位姿估计值，

为激光雷达传感器坐标系原点的y轴位姿估计值；

接下来定义一个Q_k2×5的矩阵，矩阵中均为已知量或常量：

似然最大化简化为具有二次约束的二次问题：

且

由上式可得，

其中

最后通过解带约束的最小二乘可得到待标定的相关参数。

9.如权利要求8所述的针对四轮移动机器人的车轮编码器标定算法，其特征在于，首先，建立对四轮移动机器人运动模型：

假设机器人在一个平面上移动，移动机器人的线速度在车身坐标系中表示为v＝[v_xv_y0]^T，旋转角速度矢量为ω＝[0 0ω]；

描述机器人在全局坐标系下的状态向量为q＝[XYθ]^T，则

表示全局坐标系下的速度的向量，其中

变量

和

与车身坐标系中速度矢量计算如下：

忽略车轮和表面之间的纵向滑动，得出：

v_xi＝r_iω_i

其中，v_xi是在车身坐标系中的第i个车轮总速度矢量v_i的纵向分量，r_i表示该车轮的有效滚动半径，整个移动小车旋转角速度用以下公式求得：

其中，v_i为第i个轮子绕旋转半径方向的速度，L_i为第i个轮子到旋转中心的距离，v为移动小车绕旋转半径方向的速度，L_c为小车中心到旋转中心的距离，ω为小车旋转角速度，||·||表示欧几里得范数；

获得车轮转速与机器人速度之间的近似关系：

其中，J是关于移动机器人本体相关参数的函数：

Images