CN108036792A - 一种用于移动机器人的里程计与测量位姿的数据融合方法 - Google Patents

Abstract

本发明公开了一种用于移动机器人的里程计与测量位姿的数据融合方法，移动机器人的控制单元在没有关于移动机器人位姿的测量值的外部信息输入时，根据移动机器人的初始位姿、里程计的读数以及当前每个主动轮上里程计的标度因数持续计算预测移动机器人位姿，并在有外部信息输入时，对移动机器人的位姿以及所有标度因数进行校正。本发明的用于移动机器人的里程计与测量位姿的数据融合方法利用最优估计的方法，在模型中建立了主动轮的标度因数误差，通过对标度因数误差的实时估计与补偿，可以提高其对温度、载荷等影响因素变化的鲁棒性，进而提高导航系统的精度，从根本上解决移动机器人偏航的问题。

Description

一种用于移动机器人的里程计与测量位姿的数据融合方法

技术领域

本发明涉及地面移动机器人导航定位领域，尤其涉及一种里程计与外部位置及朝向的数据融合方法。

背景技术

随着信息技术、电子技术的发展，地面移动机器人作为人工智能的一个分支，已经开始获得广泛的应用。但由于温度、载荷等的变化，机器人平台车轮的半径是变化的，以一个标称的车轮半径来计算平台实际行驶的距离，进而计算其当前的位置及朝向，会带来越来越大的误差，从而使机器人偏离预定的路线。

随着激光雷达、机器人视觉以及视觉标签等技术的发展，多传感器融合技术已经成为地面移动机器人导航与定位的发展趋势。利用这些传感器提供的信息，与里程计融合，可以提高系统导航的精度。

在公开号为CN106525053A的专利中描述了一种基于多传感器融合的移动机器人室内定位方法，使用基于航迹推算的里程计位置数据补偿激光匹配定位中无法识别相似环境的情况。将惯性单元和里程计的传感器定位数据进行融合，并把激光雷达匹配定位作为参考指标，在一定程度上减小了里程计的累积误差，但是没有考虑里程计的标度因数变化，不能从根本上解决移动机器人的偏航问题。

目前的方法，大多数均采用各种数据融合、优化或者滤波的方法，这些方法采用的模型，均没有考虑里程计的标度因数的变化，而仅仅估计了平台的位置误差，从而不能得到最优的结果。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明提供一种借助于测量位姿对里程计的测量误差进行估计与补偿、解决移动机器人偏航问题的用于移动机器人的里程计与测量位姿的数据融合方法。

技术方案：为实现上述目的，本发明的用于移动机器人的里程计与测量位姿的数据融合方法如下：移动机器人具有多个主动轮，每个主动轮上均设有里程计，其特征在于：移动机器人的控制单元在没有关于移动机器人位姿的测量值的外部信息输入时，根据移动机器人的初始位姿、里程计的读数以及当前每个主动轮上里程计的标度因数持续计算预测移动机器人位姿，并在有外部信息输入时，对移动机器人的位姿以及所有标度因数进行校正。

进一步地，控制单元基于里程计DR计算模型预测移动机器人的位姿，并基于卡尔曼滤波器对外部信息以及收到外部信息时控制单元预测的位姿进行数据融合得到最优估计状态，再基于最优估计状态对移动机器人的位姿以及所有标度因数进行校正，卡尔曼滤波器的系统状态包含每个里程计的标度因数误差。

进一步地，里程计DR计算模型的参数包含移动机器人主动轮里程计的脉冲数与里程计的标度因数；控制系统根据移动机器人的初始位姿、里程计的读数、主动轮的半径以及当前里程计的标度因数，计算预测当前移动机器人的位姿。

进一步地，卡尔曼滤波器的系统状态包含移动机器人的位置误差、朝向误差以及每个里程计的标度因数误差，在没有外部信息输入时，卡尔曼滤波器进行时间更新，计算一步预测的预测状态以及预测协方差；当有外部信息输入时，卡尔曼滤波器进行测量更新，根据所述外部信息，计算得到最优估计状态，并对所述预测协方差进行更新。

进一步地，所述控制单元将最优估计状态中对应的每个标度因数误差对应累加到各里程计当前的标度因数上。

进一步地，控制系统对移动机器人的位姿与标度因数的校正过程循环进行，控制单元在完成一次校正过程后，以该次校正过程得到系统状态的的最优估计状态为基础进行下一次校正过程，下一次校正过程的初始位姿由该次校正过程的最优估计状态直接或间接得出。

进一步地，所述外部信息由安装在所述移动机器人上的测量单元获得。

进一步地，所述测量单元为激光雷达或超声波传感器。

进一步地，所述测量单元为视觉元件，在移动机器人的移动区域内固定设置有所述视觉元件可读取的视觉标签。

有益效果：本发明的用于移动机器人的里程计与测量位姿的数据融合方法利用最优估计的方法，在模型中建立了主动轮的标度因数误差，通过对标度因数误差的实时估计与补偿，可以提高其对温度、载荷等影响因素变化的鲁棒性，进而提高导航系统的精度，从根本上解决移动机器人偏航的问题。

附图说明

附图1为移动机器人的模型；

附图2为移动机器人的标度因数修正流程图；

附图中各标号意义如下：

里程计坐标系(车体坐标系)-OoXoYo；

外部位置方位坐标系-OcXcYc；

导航坐标系(世界坐标系)-OwXwYw；

以里程计坐标系为参考，测量单元的相对位置的偏差为(Lx,Ly,θ)，主动轮(装有里程计的主动轮)半径为R，主动轮间距离为Tr。

具体实施方式

下面结合附图对本发明作更进一步的说明。

移动机器人一般具有两个个主动轮，每个主动轮上均设有里程计，通过控制两个主动轮的转速，可以实现移动机器人的前进、后退、原地旋转等动作，结合里程计实时读取每个主动轮的转动角度情况，可以实现移动机器人较为精确的控制，使移动机器人具有较高的行走精度，移动机器人的模型如附图1所示。

在轮式移动机器人的导航计算中，现有技术都是以在理想状况下主动轮的标称半径结合主动轮的里程计读数计算移动机器人的行走里程，在实际使用过程中，其主动轮转动一圈实际走过的距离具有误差，该误差的影响因素较多，首先温度可以使轮径变化，而且在使用过程中，随着轮子使用时间的延长，其会产生磨损，此外，移动机器人负载与否、负载轻重也对主动轮行走一圈实际走过的距离也有较大影响，因此在实际使用中，各种各样的因素在影响移动机器人的行走精度，且这些因素本身的产生具有随机性，各种因素混合产生使得仅依靠里程计无法对机器人进行精准控制，在移动机器人行走时间较长的状况下，会产生较大的累积误差。

基于上述问题，本发明提出了一种用于移动机器人的里程计与测量位姿的数据融合方法，具体如下：

移动机器人的每个主动轮上的里程计的输出为一段时间内的脉冲数，一个脉冲代表一定的距离，将其定义为里程计标度因数，轮子每旋转一周，会有固定个脉冲输出，据此可以大概计算一个里程计标度因数。

在上文中提及了仅依靠里程计产生的数据无法完成对移动机器人的精确控制，因此需要在移动机器人上设置测量单元，测量单元可以测量移动机器人较为精确的位姿数据，形成测量值并作为外部信息输入移动机器人的控制系统，移动机器人的控制系统依据测量单元产生的外部信息对移动机器人的位姿进行校正，并对里程计的标度因数进行校正。

测量单元的形式可以是传感器或视觉元件等可以确定移动机器人的比较精确位置的元件，在第一种实施例中，所述测量单元为激光雷达，激光雷达可以根据扫描的周边环境的轮廓判断移动机器人在地图中的较为精确的位置产生关于测量位姿的外部信息输入控制系统，测量单元也可以是围绕移动机器人设置的多个超声波传感器，多个超声波传感器对周围环境的探测原理类似于激光雷达，其精度较激光雷达低。在另一种实施例中，所述测量单元为视觉元件，如相机、扫码器等，在移动机器人的移动区域内固定设置有所述视觉元件可读取的视觉标签，如二维码标签等，移动机器人走到一定位置，视觉元件可读取视觉标签上的信息，从而可以获知移动机器人的较为精确的位置，形成外部信息输入控制系统。

基于上述硬件基础，用于移动机器人的里程计与测量位姿的数据融合方法如下：移动机器人的控制单元在没有关于移动机器人位姿的测量值的外部信息输入时，根据移动机器人的初始位姿、里程计的读数以及当前每个主动轮上里程计的标度因数持续计算预测移动机器人位姿(位置坐标与朝向)，并在有外部信息输入时，对移动机器人的位姿以及所有标度因数进行校正。移动机器人启动时其初始位姿由测量单元测得。

理论上，测量单元所测得的关于移动机器人的位置、姿态的测量值比根据里程计读数计算得到的预测的位姿精度高，因此可以直接以测量单元的测量值为参照对移动机器人的位姿以及所有标度因数进行校正；但由于安装误差，测量单元的坐标系与里程计的坐标系之间(如附图1的OoXoYo坐标系与OcXcYc坐标系)具有偏差，且由于测量单元本身的工作原理限制，也会产生误差，而且测量单元可能产生的误差会远大于预测的位姿的误差，以激光雷达为例，当激光雷达扫描的环境轮廓数据与地图中偏离机器人所在位置很远的另一位置的轮廓信息匹配度较高时，会认为机器人在别处，产生机器人在地图上瞬移几米的现象，而里程计由于其原理限制其位置误差会在比较小的范围内浮动，因此，在大部分情况下，要以测量单元的测量值为主导校正机器人的位姿，而在有些情况下(测量单元测量出错)，需要以预测位姿为主导来校正机器人的位姿。

基于上述论述，控制单元基于里程计DR计算模型预测移动机器人的位姿，并基于卡尔曼滤波器对外部信息以及收到外部信息时控制单元预测的位姿进行数据融合得到该时刻的最优估计状态，再基于最优估计状态对移动机器人的位姿以及所有标度因数进行校正，如附图2所示，卡尔曼滤波器的系统状态包含每个里程计的标度因数误差。具体实施如下：

里程计DR计算模型的参数包括移动机器人的位置与朝向(x、y、θ)以及左右里程计的标度因数s^L与s^R，x与y为移动机器人的位置坐标值，θ为移动机器人的朝向角度值，s^L、s^R分别为左、右两个主动轮上里程计的标度因数，δs^L，δs^R为左、右里程计的标度因数误差。DR计算模型会根据校正后的里程计标度因数、前一时刻移动机器人的位姿信息更新当前机器人的位姿信息。

设在k到k+1时刻，左右轮输出的里程计脉冲数为N^L(k+1),N^R(k+1)，左右轮的标称标度因数为s^L,s^R,δs^L(k+1),δs^R(k+1)为K+1时刻左右里程计误差。则k+1时刻的车辆位置和朝向(x(k+1),y(k+1),θ(k+1))可以通过下式计算：

x(k+1)＝x(k)+D(k+1)cos(θ(k+1))

y(k+1)＝y(k)+D(k+1)sin(θ(k+1))

θ(k+1)＝θ(k)+Δθ(k+1)

s^L(k+1)＝s^L+δs^L(k+1)

s^R(k+1)＝s^R+δs^R(k+1)

卡尔曼滤波器的思想在于：根据k时刻的系统状态以及k+1时刻的外部信息估计k+1时刻的系统状态。

卡尔曼滤波器的系统状态包含移动机器人的位置误差、朝向误差以及每个里程计的标度因数误差，即x＝[δx δy δθ δs^L δs^R]^T。δx，δy，δθ为移动机器人的位置和朝向误差。

卡尔曼滤波器模型的系统方程为：

x(k+1)＝Φ_k+1,kx(k)+G(k)ω(k)

ω为左、右里程计的测量噪声矩阵，

ω^L与ω^R分别为左、右里程计的测量噪声；

Φ_k+1,k为状态转移矩阵，

G为噪声输入矩阵，

卡尔曼滤波器模型的观测模型为：

z(k)＝H(k)x(k)+υ(k)

其中，

其中，为测量单元的测量噪声；

x_cam,N，y_cam,E，θ_cam,nb为测量单元测量出的位置和朝向；

x_odo,N，y_odo,E，θ_odo,nb为每次测量单元输出测量值同一个时间的根据里程计计算出来的位姿。

在没有外部信息输入时，卡尔曼滤波器进行时间更新，计算一步预测的预测状态以及预测协方差；时间更新的过程如下：

其中：A即为系统方程中的Φ_k+1,k；B为系统方程中的G；u为系统方程中的ω为对应的协方差，P_k为对应的协方差，Q是系统过程的协方差。

P₀＝cov(x(0))＝diag(var(δx) var(δy) var(δθ) var(δs^L) var(δs^R))

当有外部信息输入时，卡尔曼滤波器进行测量更新，根据所述外部信息，计算得到最优估计状态，并对所述预测协方差进行更新；测量更新的过程如下：

其中K_k+1为卡尔曼增益，P_k+1为k+1状态下的协方差；

最后，所述控制单元将最优估计状态中对应的每个标度因数误差对应累加到各主动轮的当前标度因数上。

为了能持续保持移动机器人的行走精度，控制系统对移动机器人位姿与标度因数的校正过程往复进行，控制单元在完成一次校正过程后，以该次校正过程得到的最优估计状态与标度因数为基础进行下一次校正过程，下一次校正过程的初始位姿由该次校正过程的最优估计状态间接得出。控制系统将位置误差与朝向误差对应加到移动机器人的预测的位姿数据上就可得到下一阶段的初始位姿。这样就可以不断地实时对里程计的标度因数进行修正，也就是说跑的越远，越准确。

本发明的用于移动机器人的里程计与测量位姿的数据融合方法利用最优估计的方法，在模型中建立了主动轮的标度因数误差，通过对标度因数误差的实时估计与补偿，可以提高其对温度、载荷等影响因素变化的鲁棒性，进而提高导航系统的精度，从根本上解决移动机器人偏航的问题。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种用于移动机器人的里程计与测量位姿的数据融合方法，移动机器人具有多个主动轮，每个主动轮上均设有里程计，其特征在于：移动机器人的控制单元在没有关于移动机器人位姿的测量值的外部信息输入时，根据移动机器人的初始位姿、里程计的读数以及当前每个主动轮上里程计的标度因数持续计算预测移动机器人的位姿，并在有外部信息输入时，对移动机器人的位姿以及所有标度因数进行校正。

2.根据权利要求1所述的一种用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：控制单元基于里程计DR计算模型预测移动机器人的位姿，并基于卡尔曼滤波器对外部信息以及收到外部信息时控制单元预测的位姿进行数据融合得到最优估计状态，再基于最优估计状态对移动机器人的位姿以及所有标度因数进行校正，卡尔曼滤波器的系统状态包含每个里程计的标度因数误差。

3.根据权利要求2所述的一种用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：里程计DR计算模型的参数包含移动机器人主动轮里程计的脉冲数与里程计的标度因数；控制系统根据移动机器人的初始位姿、里程计的读数、主动轮的半径以及当前里程计的标度因数，计算预测当前移动机器人的位姿。

4.根据权利要求2所述的一种用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：卡尔曼滤波器的系统状态包含移动机器人的位置误差、朝向误差以及每个里程计的标度因数误差，在没有外部信息输入时，卡尔曼滤波器进行时间更新，计算一步预测的预测状态以及预测协方差；当有外部信息输入时，卡尔曼滤波器进行测量更新，根据所述外部信息，计算得到最优估计状态，并对所述预测协方差进行更新。

5.根据权利要求3或4所述的一种用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：所述控制单元将最优估计状态中对应的每个标度因数误差对应累加到各里程计当前的标度因数上。

6.根据权利要求2-5任一项所述的一种用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：控制系统对移动机器人的位姿与标度因数的校正过程循环进行，控制单元在完成一次校正过程后，以该次校正过程得到系统状态的的最优估计状态为基础进行下一次校正过程，下一次校正过程的初始位姿由该次校正过程的最优估计状态间接得出。

7.根据权利要求1所述的用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：所述外部信息由安装在所述移动机器人上的测量单元获得。

8.根据权利要求7所述的用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：所述测量单元为激光雷达或超声波传感器。

9.根据权利要求7所述的用于移动机器人的里程计与测量位姿的数据融合方法，其特征在于：所述测量单元为视觉元件，在移动机器人的移动区域内固定设置有所述视觉元件可读取的视觉标签。