CN110515381B - 用于定位机器人的多传感器融合算法 - Google Patents

Abstract

本发明公开了一种用于定位机器人的多传感器融合算法，包括：S1、获取移动机器人在t_k‑1时刻轨迹跟踪控制器输出的t_k时刻的控制量以计算第一状态变量的预测值；S2:获取里程计在t_k时刻的第二状态变量并运用扩展卡尔曼滤波得到第一校正值；S3、获取陀螺仪在t_k时刻的第三状态变量运用扩展卡尔曼滤波得到第二校正值；S4、判断扫码器在t_k时刻是否有读数；当步骤S3的判断结果为否时返回至步骤S1；当步骤S3的判断结果为是时跳入步骤S5、获取扫码器在t_k时刻的第四状态变量运用扩展卡尔曼滤波得到第三校正值。本发明的有益之处在于提供的用于定位机器人的多传感器融合算法通过扩展卡尔曼滤波器融合多个传感器的测量值使得移动机器人的定位更加精确。

Description

用于定位机器人的多传感器融合算法

技术领域

本发明涉及一种用于定位机器人的多传感器融合算法。

背景技术

在移动机器人中，任何一个传感器都可能存在误差，单独的依赖某个传感器的读数都无法获得移动机器人精确的位姿信息，如何综合多个传感器的读数以获得更加贴近真实值的一个新的估计值成为了大家研究的重点。

发明内容

本发明提供了一种用于定位机器人的多传感器融合算法，采用如下的技术方案：

一种用于定位机器人的多传感器融合算法，包括以下步骤：

S1:获取移动机器人在t_k-1时刻轨迹跟踪控制器输出的t_k时刻的控制量，根据轨迹跟踪控制器输出的控制量以及移动机器人在t_k-1时刻的第一状态变量计算移动机器人在t_k时刻的第一状态变量的预测值；

S2:获取里程计在t_k时刻的第二状态变量，并对步骤S1获得的t_k时刻第一状态变量的预测值运用扩展卡尔曼滤波进行融合得到第一校正值；

S3:获取陀螺仪在t_k时刻的第三状态变量，并对步骤S2获得的第一校正值运用扩展卡尔曼滤波进行融合得到第二校正值；

S4:判断扫码器在t_k时刻是否有读数；

当步骤S4的判断结果为否时，将第二校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1；

当步骤S4的判断结果为是时，跳入步骤S5:获取扫码器在t_k时刻的第四状态变量，并对步骤S3获得的第二校正值运用扩展卡尔曼滤波进行融合得到第三校正值，并将第三校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1。

进一步地，控制量包括线速度和角速度；

第一状态变量包括移动机器人在世界坐标系中的横坐标、纵坐标和航向角。

进一步地，第一状态变量在t_k-1时刻的值为(x_k-1,y_k-1,θ_k-1)，则第一状态变量在t_k时刻的预测值(x_k,y_k,θ_k)通过以下公式求得：

其中，v_k表示移动机器人在t_k时刻的线速度，w_k表示移动机器人在t_k时刻的角速度。

进一步地，第二状态变量包括移动机器人在世界坐标系中的横坐标、纵坐标和航向角；

里程计根据移动机器人的左轮编码器和右轮编码器的脉冲增量更新第二状态变量。

进一步地，第三状态变量包括移动机器人在世界坐标系中的航向角。

进一步地，第四状态变量包括移动机器人在世界坐标系中的横坐标、纵坐标和航向角。

进一步地，将里程计的时间戳通过线性插值的方式调整成与轨迹跟踪控制器对齐；

将陀螺仪的时间戳通过线性插值的方式调整成与轨迹跟踪控制器对齐。

进一步地，当扫码器的滞后时间小于等于定位机器人的数据采样周期时，将里程计的时间戳通过线性插值的方式调整成与轨迹跟踪控制器对齐。

进一步地，当扫码器的滞后时间大于定位机器人的数据采样周期时，通过里程计保存的数据对扫码器的第四状态变量进行修正，通过修正后的第四状态变量对步骤S3获得的第二校正值运用扩展卡尔曼滤波进行融合得到第三校正值，并将第三校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1。

进一步地，将里程计t_k时刻的第二状态变量的值替换成修正后的第四状态变量的值。

本发明的有益之处在于提供的用于定位机器人的多传感器融合算法通过扩展卡尔曼滤波器融合多个传感器的测量值，使得移动机器人的定位更加精确。

本发明的有益之处还在于提供的用于定位机器人的多传感器融合算法通过线性插值法将多个传感器的时间戳和步骤S1中的轨迹跟踪控制器的时间戳对齐，提高了移动机器人的定位精度。

本发明的有益之处还在于提供的用于定位机器人的多传感器融合算法通过里程计修正扫码器的读数，解决了扫码器读数滞后的问题，提高了移动机器人的定位精度。

附图说明

图1是本发明的用于定位机器人的多传感器融合算法的流程图；

图2是本发明的移动机器人运动示意图。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

如图1所示为本发明的一种用于定位机器人的多传感器融合算法，包括：S1:获取移动机器人在t_k-1时刻轨迹跟踪控制器输出的t_k时刻的控制量，根据轨迹跟踪控制器输出的控制量以及移动机器人在t_k-1时刻的第一状态变量计算移动机器人在t_k时刻的第一状态变量的预测值；S2:获取里程计在t_k时刻的第二状态变量，并对步骤S1获得的t_k时刻第一状态变量的预测值运用扩展卡尔曼滤波进行融合得到第一校正值；S3:获取陀螺仪在t_k时刻的第三状态变量，并对步骤S2获得的第一校正值运用扩展卡尔曼滤波进行融合得到第二校正值；S4:判断扫码器在t_k时刻是否有读数；当步骤S4的判断结果为否时，将第二校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1；当步骤S4的判断结果为是时，跳入步骤S5:获取扫码器在t_k时刻的第四状态变量，并对步骤S3获得的第二校正值运用扩展卡尔曼滤波进行融合得到第三校正值，并将第三校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1。

关于多个不同频率的传感器的融合：将多个传感器的测量信息进行依次处理，比如有三个传感器的测量值，接收到第一个传感器的数据时，用第一个传感器的测量值做一次完整的扩展卡尔曼滤波过程(包含预测与校正)；接收到第二个传感器的数据时只进行扩展卡尔曼滤波的校正而不进行预测，即直接使用第一次完整卡扩展尔曼滤波过程得到的状态估计结果作为一步预测，然后使用第二个传感器的测量值进行测量更新过程，得到第二次扩展卡尔曼滤波过程的最优状态估计；同样，接收到第三个传感器的数据时只进行扩展卡尔曼滤波的校正而不进行预测，即直接使用第二次扩展卡尔曼滤波过程的最优状态估计作为一步预测，然后使用第三个传感器的测量值进行测量更新过程，得到第三次扩展卡尔曼滤波过程的最优状态估计。在本发明中，传感器的数量为3，具体分别为里程计、陀螺仪和扫码器，以下具体说明上述步骤。

对于步骤S1：获取移动机器人在t_k-1时刻轨迹跟踪控制器输出的t_k时刻的控制量，根据轨迹跟踪控制器输出的控制量以及移动机器人在t_k-1时刻的第一状态变量计算移动机器人在t_k时刻的第一状态变量的预测值。

具体而言，控制量包括线速度v_k和角速度，第一状态变量包括移动机器人在世界坐标系中的横坐标x、纵坐标y和航向角θ。

如图2所示为移动机器人运动示意图，移动机器人的运动学方程为：

其中，v表示机器人的线速度，ω表示机器人的角速度，其相应的离散方程为：

其中，SamplePeriod为给定的采样周期，在已知移动机器人的采样周期SamplePeriod和t_k-1时刻的第一状态变量的情况下，就可以根据上面的离散方程计算移动机器人在t_k时刻的第一状态变量的预测值。

对于步骤S2:获取里程计在t_k时刻的第二状态变量，并对步骤S1获得的t_k时刻第一状态变量的预测值运用扩展卡尔曼滤波进行融合得到第一校正值。

假设系统的过程方程是一个非线性离散随机差分方程，则：

x_k＝f(x_k-1,u_k)+w_k,

w_k～N(0,Q_k),

其中

表示系统的状态变量，u_k表示系统的输入控制量，w_k表示均值为0、协方差矩阵为Q_k且服从正态分布的过程噪声，定义雅可比矩阵：

假设测量变量

测量方程为：

z_k＝h(x_k)+v_k,

v_k～N(0,R_k),

其中v_k表示均值为0、协方差矩阵为R_k且服从正态分布的测量噪声，定义雅可比矩阵：

扩展卡尔曼滤波分为两个步骤，即时间更新和测量更新，时间更新方程又称为预测方程，测量更新方程又称为校正方程。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的预测；在校正阶段，滤波器利用当前状态的测量值修正预测阶段获得的预测值，以获得一个更接进真实值的新估计值。

预测：

计算卡尔曼增益K_k：

校正：

其中

为在已知系统第k-1步状态变量的情况下对第k步的状态变量的先验状态估计，x_k为已知测量变量z_k时对第k步的状态变量的后验状态估计，u_k表示系统的输入控制量，A_k是函数f关于x的雅可比矩阵，

为先验状态估计的误差的协方差矩阵，P_k为后验状态估计的误差的协方差矩阵，H_k表示函数h关于x的雅可比矩阵，Q_k表示过程噪声协方差矩阵，R_k表示测量噪声协方差矩阵，K_k为卡尔曼增益，I为单位矩阵。

在步骤S2中，移动机器人的里程计的第二状态变量包括机器人在世界坐标系中的横坐标、纵坐标、航向角，分别记为x_odom,y_odom,θ_odom，当里程计的时间戳和S1中的轨迹跟踪控制器的时间戳不对齐时，采用线性插值来求得与轨迹跟踪控制器的时间戳对齐的里程计的读数。具体而言，假设S1的时间戳为t，最近的里程计的时间戳为t_k-1,t_k，相应的里程计的第二状态变量分别为x_k-1,y_k-1,θ_k-1和x_k,y_k,θ_k，利用线性插值我们插值得出t时刻里程计的第二状态变量：

这种线性插值求解时间戳对齐时刻的里程计的第二状态变量的方法在时间间隔很短的情况下精度是很高的。里程计的频率上百赫兹，在时间间隔这么小的情况下，采取线性插值求解时间戳对齐后的里程计的第二状态变量是精确的。

进一步地，移动机器人的运动学方程为：

其对应的离散方程为：

记状态变量X＝(x,y,θ)^T，控制变量u_k＝(v_k,w_k)^T，则上式简化为：

X_k＝f(X_k-1,u_k),

根据步骤S1获得的在t_k-1时刻轨迹跟踪控制器输出的控制量u_k＝(v_k,w_k)^T，我们就可以计算得到t_k时刻状态变量X的预测值，并且我们可以求得雅可比矩阵：

里程计的测量方程为：

Z_k＝h(X_k)＝(x_k，y_k，θ_k)^T，

于是我们可以求得雅可比矩阵：

这样我们就可以使用前面介绍的扩展卡尔曼滤波融合里程计的测量值对状态变量X的预测值进行修正，以获得一个更加接近真实值的新估计值，此处将修正后的值记为第一校正值。

进一步地，在步骤S2中，里程计的第二状态变量x＝(x_odom,y_odom,θ_odom)^T更新方法为：

记

分别是里程计的左轮编码器和右轮编码器在[t_k-1,t_k]时间段内的脉冲增量，理想情况下，不考虑打滑和路面颠簸，则左右轮移动距离为：

其中R^L,R^R分别是左右轮半径。记N是轮子转动一圈时编码器的增量，d是小车轴距，那么[t_k-1,t_k]时间段内小车移动距离、旋转角度和曲率半径分别为：

里程计的更新方程为：

在已知里程计在t_k-1时刻的第二状态变量以及里程计的左轮编码器和右轮编码器在[t_k-1,t_k]时间段内的脉冲增量的基础上，通过以上公式可以得到里程计在t_k时刻的第二状态变量。

对于步骤S3:获取陀螺仪在t_k时刻的第三状态变量，并对步骤S2获得的第一校正值运用扩展卡尔曼滤波进行融合得到第二校正值。

具体而言，陀螺仪的第三状态变量指移动机器人在世界坐标系中航向角，记为θ_gyro，同步骤S3，当陀螺仪的时间戳和S1中的轨迹跟踪控制器的时间戳不对齐时，采用线性插值来求得与轨迹跟踪控制器的时间戳对齐的陀螺仪的读数。具体而言，假设S1的时间戳为t，最近的陀螺仪的时间戳为t_k-1,t_k，相应的陀螺仪的第三状态变量分别为θ_k-1和θ_k，利用线性插值我们插值得出t时刻陀螺仪的第三状态变量：

这种线性插值求解时间戳对齐时刻的陀螺仪的第三状态变量的方法在时间间隔很短的情况下精度是很高的。陀螺仪的频率上百赫兹，在时间间隔这么小的情况下，采取线性插值求解时间戳对齐后的陀螺仪的第三状态变量是精确的。

在步骤S3中，将步骤S2融合里程计测量值后得到的第一校正值作为步骤S3的预测值，再利用陀螺仪的测量值对第一校正值做修正，具体的修正步骤如下：

陀螺仪的观测方程为：

Z_k＝h(X_k)＝θ_k,

可以求得雅可比矩阵：

H_k＝(0 0 1),

这样我们就可以使用前面介绍的扩展卡尔曼滤波融合陀螺仪的测量值对状态变量θ的预测值进行修正，以获得一个更加接近真实值的新估计值，此处将该新的估计值记为第二校正值。

对于步骤S4:判断扫码器在t_k时刻是否有读数。

具体而言，在本发明中，扫码器作为移动机器人中的第三个传感器，其只有在扫描到二维码图案时才会产生读数，在移动机器人运动过程中，只有在某些固定的点上设有二维码图案，因此，在移动机器人运行过程中，绝大部分时间扫码器并没有采集到二维码图案。在每个采样校正周期中，在执行完步骤S3后判断扫码器是否有读数，但判断结果为否时，则返回步骤S1执行下一运算周期，此时，将第二校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1执行下一周期的预测和校正。当步骤S3的判断结果为是时，跳入步骤S5。

对于步骤S5：获取扫码器在t_k时刻的第四状态变量，并对步骤S3获得的第二校正值运用扩展卡尔曼滤波进行融合得到第三校正值，并将第三校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1。

在步骤S5中，扫码器的第四状态变量包括机器人在世界坐标系中的横坐标、纵坐标、航向角，分别记为x_landmark,y_landmark,θ_landmark：

由于扫码器摄取到二维码到解码需要很长时间，经常会导致扫码器的数据滞后，在扫码器的滞后时间小于等于一个SamplePeriod的情况下，当扫码器的时间戳和S1中的轨迹跟踪控制器的时间戳不对齐时，采用线性插值来求得与轨迹跟踪控制器的时间戳对齐的扫码器的读数。具体而言，假设S1的时间戳为t，最近的扫码器的时间戳为t_k-₁,t_k，相应的扫码器的第四状态变量分别为x_k-1,y_k-1,θ_k-1和x_k,y_k,θ_k，利用线性插值我们插值得出t时刻陀螺仪的第四状态变量：

这种线性插值求解时间戳对齐时刻的陀螺仪的第四状态变量的方法在时间间隔很短的情况下精度是很高的。

将步骤S3融合陀螺仪的测量值后得到的第二校正值作为步骤S5的预测值，再利用扫码器的测量值做修正，具体的修正步骤如下：

进一步地，扫码器的观测方程为：

Z_k＝h(X_k)＝(x_k，y_k，θ_k)^T，

于是可以求得雅可比矩阵：

这样就可以使用前面介绍的扩展卡尔曼滤波融合扫码器的测量值对状态变量X＝(x,y,θ)^T的预测值进行修正，以获得一个更加接近真实值的新估计值，此处记为第三校正值，将第三校正值作为t_k时刻的第一状态变量返回至步骤S1执行下一周期的预测和校正。

通常，扫码器的滞后时间为几十毫秒，而一个SamplePeriod一般设为几毫秒，因此，扫码器的滞后时间大于一个SamplePeriod。在移动机器人运行过程中，实时保存里程计之前一段时间的左轮编码器和右轮编码器的脉冲增量信息，该一段时间设置成比扫码器的滞后时间长，在本发明中取200ms。在扫码器的滞后时间大于一个SamplePeriod的情况下，假设当前时刻里程计的时间戳为t_k，当前时刻扫码器的时间戳为t₀，在里程计存储的过去200ms的里程计的时间戳中找出第一个比扫码器的时间戳t₀早的里程计的时间戳t，并用t₀时刻的扫码器的第四状态变量的读数作为t时刻的里程计的第二状态变量的值，然后根据t时刻至t_k时刻的左轮编码器和右轮编码器的脉冲增量信息依据步骤S2中介绍的里程计更新方程计算得到当前时刻t_k里程计一个新的第二状态变量x_new，将该新的第二状态变量x_new作为扫码器当前的第四状态变量的修正值，把步骤S3融合陀螺仪测量值后得到的第二校正值作为步骤S5的预测值，再利用经过里程计修正后的陀螺仪的测量值x_new对第二校正值做修正，具体的修正步骤如下：

扫码器的观测方程为：

Z_k＝h(X_k)＝(x_k，y_k，θ_k)^T，

于是可以求得雅可比矩阵：

这样就可以使用前面介绍的扩展卡尔曼滤波融合经过里程计校正后的陀螺仪的测量值x_new对状态变量X＝(x,y,θ)^T的预测值进行修正，以获得一个更加接近真实值的新估计值。此处记为第三校正值，将第三校正值作为t_k时刻的第一状态变量返回至步骤S1执行下一周期的预测和校正。

以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。

Claims (6)

1.一种用于定位机器人的多传感器融合算法，其特征在于，包括以下步骤：

S1:获取移动机器人在t_k-1时刻轨迹跟踪控制器输出的t_k时刻的控制量，根据所述轨迹跟踪控制器输出的控制量以及移动机器人在t_k-1时刻的第一状态变量计算移动机器人在t_k时刻的所述第一状态变量的预测值；

S2:获取里程计在t_k时刻的第二状态变量，并对步骤S1获得的t_k时刻所述第一状态变量的预测值运用扩展卡尔曼滤波进行融合得到第一校正值；

S3:获取陀螺仪在t_k时刻的第三状态变量，并对步骤S2获得的第一校正值运用扩展卡尔曼滤波进行融合得到第二校正值；

S4:判断扫码器在t_k时刻是否有读数；

当步骤S4的判断结果为否时，将所述第二校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1；

当步骤S4的判断结果为是时，跳入步骤S5:获取扫码器在t_k时刻的第四状态变量，并对步骤S3获得的所述第二校正值运用扩展卡尔曼滤波进行融合得到第三校正值，并将所述第三校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1；

将所述里程计的时间戳通过线性插值的方式调整成与所述轨迹跟踪控制器对齐；

将所述陀螺仪的时间戳通过线性插值的方式调整成与所述轨迹跟踪控制器对齐；

当所述扫码器的滞后时间小于等于定位机器人的数据采样周期时，将所述里程计的时间戳通过线性插值的方式调整成与所述轨迹跟踪控制器对齐；

当所述扫码器的滞后时间大于定位机器人的数据采样周期时，假设当前时刻所述里程计的时间戳为t_k，当前时刻所述扫码器的时间戳为t₀，在所述里程计存储的过去预定时间内的所述里程计的时间戳中找出第一个比所述扫码器的时间戳t₀早的所述里程计的时间戳t，并用t₀时刻的所述扫码器的第四状态变量的读数作为t时刻的所述里程计的第二状态变量的值，并通过该t时刻的所述里程计的第二状态变量的值计算出t_k时刻的所述里程计的一个新的所述第二状态变量的值，并将所述里程计的新的所述第二状态变量的值作为所述扫码器的第四状态变量的修正值，再通过所述第四状态变量的修正值对步骤S3获得的所述第二校正值运用扩展卡尔曼滤波进行融合得到所述第三校正值，并将所述第三校正值作为移动机器人在t_k时刻的第一状态变量返回至步骤S1。

2.根据权利要求1所述的用于定位机器人的多传感器融合算法，其特征在于，

所述控制量包括线速度和角速度；

所述第一状态变量包括移动机器人在世界坐标系中的横坐标、纵坐标和航向角。

3.根据权利要求2所述的用于定位机器人的多传感器融合算法，其特征在于，

所述第一状态变量在t_k-1时刻的值为(x_k-1,y_k-1,θ_k-1)，则所述第一状态变量在t_k时刻的预测值(x_k,y_k,θ_k)通过以下公式求得：

其中，v_k表示移动机器人在t_k时刻的线速度，w_k表示移动机器人在t_k时刻的角速度。

4.根据权利要求1所述的用于定位机器人的多传感器融合算法，其特征在于，

所述第二状态变量包括移动机器人在世界坐标系中的横坐标、纵坐标和航向角；

所述里程计根据移动机器人的左轮编码器和右轮编码器的脉冲增量更新所述第二状态变量。

5.根据权利要求1所述的用于定位机器人的多传感器融合算法，其特征在于，

所述第三状态变量包括移动机器人在世界坐标系中的航向角。

6.根据权利要求1所述的用于定位机器人的多传感器融合算法，其特征在于，

所述第四状态变量包括移动机器人在世界坐标系中的横坐标、纵坐标和航向角。

Images