CN114216458A

CN114216458A - 基于多传感器融合的智能推料机器人组合导航系统及方法

Info

Publication number: CN114216458A
Application number: CN202111516593.3A
Authority: CN
Inventors: 李秀智; 郭继冬; 焦振宇; 张祥银
Original assignee: Beijing University of Technology
Current assignee: Beijing University of Technology
Priority date: 2021-12-07
Filing date: 2021-12-07
Publication date: 2022-03-22

Abstract

本发明公开了基于多传感器融合的智能推料机器人组合导航系统及方法，用于解决牧场机器人导航路线不够灵活、在牛栏中导航横向距离获取不够准确平滑以及航向角漂移问题。主要包括以下步骤：使用人机交互界面设定机器人行走路线，机器人由充电桩通过磁导航行驶至牛栏处，在牛栏中使用超声波、惯性传感器(IMU)以及轮式里程计进行组合导航，在横向距离的获取上采用卡尔曼滤波器融合超声波和轮式里程计获得更加稳定平滑的距离，针对航向角漂移问题，采用软件对IMU进行复位矫正，有效消除了航向角漂移的问题。本发明导航效果良好，导航精度可达0.05m，被国内知名牧场所接受。

Description

基于多传感器融合的智能推料机器人组合导航系统及方法

技术领域

本发明属于畜牧业自动化领域，是一种基于多传感器融合的智能推料机器人组合导航系统的技术和方法。

背景技术

畜牧业已经成为我国牧民的支撑产业，是其收入经济的主要来源。目前为止，国内养牛牧场的饲养方式多为人工饲养，费时费力，并且无法保证作业精度和作业频率。为解决人工饲养带来的问题，突破牧场养牛自动化的瓶颈，行业内相继研制了不同种类的推料机器人，但导航与控制性能还有较大的改进空间。

目前，在机器人的导航方式上，主要有轨道导航、超声波导航、磁条导航、磁钉导航以及标志线导航。其中轨道导航需要事先铺设悬挂轨道,对牧场坏境改动较大。磁导航需要事先铺设大量的磁钉或者磁条，成本较高，施工相对麻烦。

目前推料机器人的导航问题主要有以下几点：

第一，不同的牧场环境，要求不同的机器人工作路线。因此系统需灵活的路径设置途径；

第二，基于超声波传感器的牛栏测距，是当前主流的导航控制方案。但单一传感器难以避免测量噪声，且牛在吃草过程中，对超声波测距造成非常严重的动态干扰；

第三，推料机器人相对牛栏的航向角，缺少有效的测量方法。

针对上述问题，本文发明了基于多传感器融合的智能推料机器人组合导航系统，对上述问题提出了有效的解决办法。

发明内容

本发明的目的是提供一种基于多传感器融合的推料机器人组合导航系统的装置和方法。

本发明的主要装置包括人机交互界面(触摸屏DOP-107BV)超声波传感器、惯性传感器(IMU)和电机以及编码器。机器人组合导航方法步骤如下：

准备工作：在机器人上安装好相应的电机、编码器、超声波传感器、IMU 等。

步骤1：通过人机交互界面设定推料机器人导航路径。

步骤2：通过磁条导航驶出充电桩，行驶至牛栏处。

步骤3：机器人行驶至牛栏处后，启动超声波、惯性传感器(IMU)、轮式里程计进行组合定位导航。

步骤4：行驶至牛栏尽头，检测到磁条，根据步骤1判定机器人行进方向，导航至牛栏的另一侧。

步骤5：步骤4执行完毕后，重复执行步骤3，直至再次检测到磁条，根据步骤1判定机器人行进方向，根据磁条导航回到充电桩。

根据上述方案的本发明，所述的人机交互界面采用触摸屏，根据牧场的实际环境可以灵活设定机器人行进的方向路线。

根据上述方案的本发明，所述步骤二，主要用来引导机器人在牛栏之外的充电桩行驶至牛栏处。

根据上述方案的本发明，所述步骤三，机器人行驶至牛栏后，导航的核心问题是如何准确获得机器人相对牛栏的横向距离，并保证滤除牛头以及其余的外部干扰，如何获取机器人相对牛栏的航向角，并消除由于航向角漂移所导致的导航偏移问题。针对横向距离的获取，进入牛栏之后，使用超声波测量机器人相对牛栏的横向距离，并且对超声波采集的数据进行预处理，滤除设定距离之外以及发生突变的距离，同时采集编码器数据进行轮式里程计推算，使用卡尔曼滤波器对处理之后的超声波数据和轮式里程计数据进行滤波得到机器人相对牛栏的横向距离，有效地滤除了超声波的偶然误差以及牛头和其他外部因素的干扰。在进行轮式里程计推算的过程中，我们采用IMU测量得到的航向角代替轮子编码器推算出的航向角进行计算，这样大大的提高了轮式里程计推算的位置精度。轮式里程计推算公式如下：

其中x(k+1)、y(k+1)、θ(k+1)分别为k+1时刻的机器人坐标及航向角,x(k)、y(k)、θ(k)分别为k时刻机器人坐标及航向角。Δs为机器人在采样时刻内走过的距离，Δθ代表了采样时间内机器人航向角的增量,cosθ代表角度θ的余弦值，sinθ代表角度θ的正弦值。这里的θ采用IMU测量得到的航向角。

卡尔曼滤波器滤波过程如下，

状态方程为：

x_k＝x_k-1+Δs_k,k-1×sin(θ_k-1)+w_k-1 (1.2)

观测方程为：

z_k＝x_k+v_k (1.3)

其中x_k代表k时刻状态值，x_k-1代表k-1时刻的状态值，z_k代表k时刻观测值， w_k-1和v_k分被代表了k-1时刻状态过程噪声和k时刻观测过程的噪声，并且均服从正态分布，Δs_k,k-1代表由k-1时刻到k时刻内机器人的行走距离，sinθ_k-1表示 k-1时刻θ角的正弦值。滤波器主要分为以下的预测和更新两个部分，其中预测部分公式为：

更新阶段公式为：

式中：

表示k时刻横向距离的先验估计，

表示k-1时刻横向距离的后验估计，

表示k时刻先验距离估计协方差，P_k表示k时刻横向距离后验协方差，P_k-1表示k-1时刻横向距离后验协方差，K_k表示k时刻卡尔曼滤波增益，Q是状态噪声的协方差，R是观测噪声的协方差。

通过卡尔曼滤波器处理超声波和轮式里程计测量到的数据，可以得到较为平滑稳定的横向距离，为机器人的控制提供了保障。

针对IMU航向角漂移的问题，采用软件滤波进行复位矫正，主要思想为：连续采集机器人相对牛栏的航向角以及距离牛栏的横向距离，如果航向角和距离牛栏的横向距离都在我们设定阈值内变化，这时候我们认为机器人和牛栏处在一个相对平行的位置前进，此刻比较此时的航向角和初始的航向角，如果当前时刻航向角相对初始时刻航向角发生了较大偏移，则对航向角进行复位操作，所谓复位操作就是清除航向角以往所有的测量数据，复位后航向角为0，这样便消除航向角误差，有效地消除了因为机器人长距离行驶造成的航向角漂移问题。为机器人的航向控制提供了稳定的角度。

根据上述方案的本发明，所述步骤四，当推料机器人行驶至一侧牛栏的尽头，磁传感器会检测到预先铺设在牛栏尽头的一条横向的磁条，根据步骤1中，所设定的路径，决定当前的转向，并根据磁条导引至另一侧牛栏。

根据上述方案的本发明，所述步骤五，进入另一侧牛栏后，重复步骤三中内容，直至走到本侧牛栏尽头，检测到预先铺设的磁条，根据步骤1中预选设定的路径，决定当前转向，并根据磁条导航至充电桩停下，等待下一次推料任务的开始。

附图说明

图1为本发明流程图。

图2为机器人实体图片。

图3为实验现场环境图。

图4为牛场布局图片。

图5为人机交互界面路径设定图。

图6为航向角示意图图。

图7为卡尔曼滤波器流程图。

图8为实验前后效果图。

具体实施方式

正如背景技术所介绍的,现有技术中存在不足,为解决如上技术问题,本发明的一种典型方法，如图1所示，主要步骤如下:

用作实验的机器人如图2所示，实验环境如3所示，牛场布局如图4所示，机器人从充电桩出发运行一圈后回到牛栏，这时候我们根据图5中人机交互界面设定相应的形式路线，路线为：

充电桩—>掉头—>磁条导航—>超声波组合导航—>右转—>磁条导航—>超声波组合导航—>右转—>磁条导航—>充电桩。

路径设定完毕之后，机器人开始工作，首先进行掉头驶出充电桩，沿磁条进行导航至牛栏。

机器人从充电桩运行至牛栏之后，开启超声波、IMU、轮式里程计的组合导航模式，首先记录当前时刻机器人距离牛栏的初始横向距离，记为x(0)，横向距离展示如图6所示，根据编码器测量到数据进行数据推算，得到机器人线速度为v。采样时间为Δt，IMU采集的航向角为θ，其中航向角展示如图6所示，根据步骤三中里程计的推算公式，可以得到机器人相对牛栏的横向距离为：

其中，x为当前时刻机器人距离牛栏的横向距离，x_last为上一采样时刻机器人距离牛栏的横向坐标，其初值即为x(0)，v代表机器人的线速度，Δt代表采样时间。首先对超声波采集数据进行滤波预处理，同时进行轮式里程计的推算，采用卡尔曼滤波器来融合超声波和轮式里程计数据。滤波器主要分为以下的预测和更新两个部分，其中预测部分公式为：

更新阶段公式为：

卡尔曼滤波器融合数据的过程如图7所示。

在牛栏中行驶的过程中，针对IMU航向角漂移的问题，采用软件滤波的方法对IMU进行复位矫正。

具体做法为：假定设定机器人距离牛栏0.8m进行推料作业，超声波采集的数据有效范围为0.6-1.0m，初始航向角记为θ₀，角度变化阈值设定为T_θ，距离阈值变化设定为T_d，航向角偏差设定为θ_e，行进过程中，每隔30秒同时采集一次当前机器人的航向角和距离牛栏的横向距离，连续采集5次，航向角记为:{θ₁，θ₂，θ₃，θ₄，θ₅}，距离记为:{d₁，d₂，d₃，d₄，d₅}。如果采集到的航向角和横向距离值变化相对平稳，也就是从θ₁到θ₅之间每相邻两次的航向角差值的绝对值均小于T_θ，d₁到d₅之间每相邻两次的横向距离差值的绝对值均小于T_d，且d₁到d₅的测量值均在有效范围内，此时我们认为机器人行走方向和牛栏平行。然后求出当前采集到的航向角θ₁到θ₅的均值记为

此时去计算

与θ₀的差值，如果此差值的绝对值大于我们设定的航向角偏差θ_e，此时我们认为机器人发生了航向角偏移，执行IMU的复位操作，消除当前积累的航向角误差，这样有效的消除了因机器人长距离行驶或其它干扰因素造成的航向角漂移问题。

在一侧牛栏导航结束后，在牛栏尽头预先铺设一段横向的磁条，当磁传感器检测到磁条后，根据步骤1中设定的路径规则，决定当前的转向，并根据磁导航行进至另一侧牛栏。

进入另一侧牛栏之后，重复步骤3的内容，直至磁导航传感器在牛栏尽头检测到磁条，同时根据步骤1中设定的路线规则，决定当前机器人转向，根据磁条导航至充电桩，等待下一轮导航任务的开始。

经过实验，本文发明的基于多传感器融合的推料机器人组合导航系统方法良好，导航精准，实验效果良好。推料前后效果如图8所示。

Claims

1.基于多传感器融合的智能推料机器人组合导航方法，其特征在于，包括以下步骤：

1)通过人机交互界面设定推料机器人导航路径；

2)通过磁条导航驶出充电桩，行驶至牛栏处；

3)机器人行驶至牛栏处后，启动超声波、惯性传感器IMU、轮式里程计进行组合定位导航；

4)行驶至牛栏尽头，检测到磁条，根据步骤1判定机器人行进方向，进行导航至牛栏的另一侧；

5)步骤4执行完毕后，重复执行步骤3，直至再次检测到磁条，根据步骤1判定机器人行进方向，根据磁条导航回到充电桩。

2.根据权利要求1所述的一套基于多传感器融合的智能推料机器人组合导航方法，其特征在于，现场观察牛棚环境，通过台达触摸屏设计的人机交互界面设定导航路径。

3.根据权利要求1所述的一套基于多传感器融合的智能推料机器人组合导航方法，其特征在于，机器人充电桩放置在牛栏之外，机器人从充电桩运行至牛栏之前，采用磁条进行定位导航，直至其运行至牛栏之中。

4.根据权利要求1所述的一套基于多传感器融合的智能推料机器人组合导航方法，其特征在于，行驶至牛栏之后，采用IMU测量机器人相对牛栏的航向角，并使用IMU测量的航向角代替轮子推算的航向角进行里程计推算，对超声波测量的数据进行预处理，然后使用卡尔曼滤波器融合超声波和轮式里程计数据得到机器人相对牛栏的横向距离。

5.根据权利要求1所述的一套基于多传感器融合的智能推料机器人组合导航方法，其特征在于，沿牛栏一侧行驶至尽头处，检测预先铺设的磁条，根据步骤1中预先设定的路径信息，决定机器人的转向，进而沿着磁条导航进行另一侧牛栏。

6.根据权利要求1所述的一套基于多传感器融合的智能推料机器人组合导航方法，其特征在于，当行驶到另一侧牛栏之后，重复步骤4，导航至牛栏尽头时，检测预先铺设的磁条，根据步骤1中预先设定的路径信息，决定机器人的转向，进而沿着磁条导航至充电桩，导航结束，等待下一轮导航任务。

7.根据权利要求1所述的一套基于多传感器融合的智能推料机器人组合导航方法，其特征在于，所述的超声波安装在机器人侧面，IMU和机器人刚性连接；其中轮式里程计采集数据所用的编码器为2500线。