CN111522339A - 畜禽舍巡检机器人自动路径规划与定位方法及装置 - Google Patents

Abstract

本发明涉及巡检机器人技术领域，公开了一种畜禽舍巡检机器人自动路径规划与定位方法及装置，其自动路径规划与定位方法包括：获取畜禽舍当前的环境信息，构建全局地图，在全局地图上规划巡检机器人行走的全局路径；基于巡检机器人行走时的三轴姿态角与加速度信息及行走里程信息，估算并更新巡检机器人实时的行走位姿，控制巡检机器人沿着全局路径行走；本发明在畜禽舍的行走路径与路况不明确的情况下，可快速并精确地对巡检机器人实施导航与定位，其成本低廉，便于维护，有力地弥补了传统的GPS导航及其它需要提前规划路径与铺设辅助设备的导航方式所存在的缺陷，适应于巡检机器人在畜禽舍养殖环境内的导航行走。

Description

畜禽舍巡检机器人自动路径规划与定位方法及装置

技术领域

本发明涉及巡检机器人技术领域，特别是涉及一种畜禽舍巡检机器人自动路径规划与定位方法及装置。

背景技术

近年来，随着畜禽业的快速发展与普及，养殖企业基本实现了畜禽饲养的标准化、规范化及养殖设施的自动化、机械化。在畜禽养殖中，畜禽舍存在多种结构形式，例如：对于蛋鸡饲养而言，鸡舍分为阶梯式鸡舍或H型鸡舍，这些鸡舍具备多列、多层鸡笼，鸡笼的笼长相对较长，笼层多为3层以上。对于这些复杂结构的畜禽舍，更需要配置智能化、精准化、高效化的巡检机器人，以此来用于畜禽饲养中的自动化巡检。

畜禽舍巡检机器人是负责完成空间移动作业的机械设备，并用于在封闭或者半封闭的畜禽舍内进行巡检作业。对于如何实现在复杂的畜禽舍环境中进行巡检，对畜禽舍巡检机器人的路径规划和定位是解决该问题的关键。

下面对机器人自动行走控制领域常见的导航与定位技术进行简要介绍，具体如下所示：

(1)GPS导航与定位，由于GPS定位器在禽舍内会出现信号弱，甚至是无信号的状况，从而畜禽舍巡检机器人无法依赖GPS实现室内定位。

(2)传统的磁导航定位，它是通过在畜畜禽舍地面铺设磁条，并在运动过程中通过测量路径上的磁场信号来获取车辆自身相对于目标跟踪路径之间的位置偏差，从而实现车辆的控制及导航，是一种相对可靠的导航技术。但是，磁导航方式在用于畜禽舍巡检机器人的巡检作业时，需要人为铺设大量电磁轨道，不但施工复杂、周期长、维护成本高，在以后的设施升级与改造中，还需要重新铺设电磁轨道，这在操作上施工复杂，延长了升级周期，从而降低了对畜禽的生产效率。

(3)类似于GPS定位原理，针对局部空间或室内空间定位的UWB技术、WIFI定位技术、超声定位技术、蓝牙室内定位技术相对比较成熟。这些技术通过多个发射器或者多个接收器之间传递信号，测量时间差、信号强度或脉冲相位等随距离变化的信息，进而通过计算得到的距离信息，并最终确定机器人在空间中的位置。然而，无论是室内UWB定位、超声定位、WIFI定位、蓝牙定位，还是上述所示的磁导航定位，对环境的依赖都非常高，且都需要配置辅助设备或标签，且运动路径往往是提前规划的，只能在环境相对可靠、稳定，并有特定作业规则和秩序的场合使用，受限制较多，从而难以适用于畜禽舍巡检机器人在未知路径且崎岖不平的路况下行走，并进行巡检。

(4)在定位过程中还用惯性推算定位和航迹推算定位方法，它们不依赖外部设备、标签定位，利用自身携带的电机编码器或惯性测量单元(IMU，Inertial MeasurementUnit)，通过反馈的电机信号，假设驱动轮按照电机的转动角度实现轮子相应的位移，进而推算出机器人本体的累计位置，而后者通过加速度计和陀螺仪获取运动状态，通过积分运算估计位置信息。然而，在实际应用中，对于惯性推算定位和航迹推算定位而言，虽然不依赖外部设备和标签，但这两种方法都存在累计误差，对于短时间、近距离的位置估计非常有效，但在畜禽舍内，巡检机器人需要24小时不间断巡查，在缺乏外部校正的情况下，误差经过长时间累计而放大，因此不能单独应用于畜禽舍巡检机器人，以实现对畜禽舍的巡检作业。

发明内容

为了克服上述问题或者至少部分地解决上述问题，本发明实施例提供一种畜禽舍巡检机器人自动路径规划与定位方法及装置，用以弥补传统的GPS导航及其它需要提前规划路径与铺设辅助设备的导航方式所存在的缺陷，适应巡检机器人在畜禽舍养殖环境内的行走。

为了解决上述技术问题，本发明实施例一方面提供了一种畜禽舍巡检机器人自动路径规划与定位方法，包括：S1，获取畜禽舍当前的环境信息，构建全局地图，在所述全局地图上规划巡检机器人行走的全局路径；S2，基于所述巡检机器人行走时的三轴姿态角与加速度信息及行走里程信息，估算并更新所述巡检机器人实时的行走位姿，控制所述巡检机器人沿着所述全局路径行走。

其中，还包括：S3，在所述全局地图上进行局部路径规划，控制所述巡检机器人沿着规划的局部路径行走，对所述巡检机器人沿所述全局路径行走时遇到的障碍物进行规避。

其中，S1进一步包括：基于所述巡检机器人行走的初始状态与目标状态，采用A-Star算法在所述全局地图上自动规划一条最短的全局路径；S2进一步包括：基于实时更新的所述巡检机器人的行走位姿，控制所述巡检机器人沿着最短的全局路径行走。

其中，S2中所述估算并更新所述巡检机器人实时的行走位姿进一步包括：基于所述巡检机器人行走时的三轴姿态角与加速度信息，获取所述巡检机器人行走时的预测位姿；将所述预测位姿作为初始位姿与所述巡检机器人在所述全局地图上的位姿进行扫描匹配；进行突变检测，获取扫描匹配位姿；将所述扫描匹配位姿与所述预测位姿进行融合，获取估算的所述巡检机器人实时的行走位姿，并对所述巡检机器人实时的行走位姿进行更新。

其中，S3进一步包括：采用DWA算法在所述全局地图上规划所述巡检机器人行走的局部路径，采用如下公式所示的轨迹评价函数确定所述巡检机器人行走的局部路径：

G(v，w)＝σ·(α·heading(v，w)+β·dist(v，w)+γ·velocity(v，w))；

其中，G(v,w)表示轨迹评价函数，heading(v,w)表示所述巡检机器人与障碍物的夹角，dist(v,w)表示所述巡检机器人距离与其行走轨迹相交的最近的障碍物的距离，velocity(v,w)表示所述巡检机器人的行走速度，α、β、γ依次为heading(v,w)、dist(v,w)及velocity(v,w)的权重，σ为轨迹平滑系数。

其中，S2进一步包括：所述巡检机器人在沿着所述全局路径行走中巡检到生理信息异常的畜禽时，在所述全局地图上采用AMCL定位算法获取所述巡检机器人实时的位置信息，将该位置信息作为生理信息异常的畜禽的位置信息发送至云服务器。

其中，S2进一步包括：在所述巡检机器人沿着所述全局路径行走之前，检测所述巡检机器人内电池的电量，在所述电池的电量低于预设阈值时，控制所述巡检机器人行走至所述全局地图上预设的充电位置，以寻找充电器并进行充电，在所述电池充满电后，控制所述巡检机器人沿所述全局路径行走。

其中，S1进一步包括：基于激光雷达获取畜禽舍当前的环境信息，基于Gmapping算法构建所述全局地图；S2进一步包括：基于惯性测量单元获取所述巡检机器人行走时的三轴姿态角与加速度信息，基于里程测量元件获取所述巡检机器人行走时的行走里程信息。

其中，本发明实施例另一方面提供一种基于如上畜禽舍巡检机器人自动路径规划与定位方法的装置，包括：激光雷达、惯性测量单元、里程测量元件和控制模块；所述激光雷达、所述惯性测量单元及所述里程测量元件分别连接所述控制模块，所述控制模块用于与巡检机器人的行走驱动系统相连接；其中，所述激光雷达用于采集畜禽舍当前的环境信息，所述惯性测量单元用于采集所述巡检机器人行走时的三轴姿态角与加速度信息，所述里程测量元件用于采集所述巡检机器人的行走里程信息，所述控制模块用于基于所述激光雷达、所述惯性测量单元及所述里程测量元件采集到的信息，通过所述行走驱动系统对所述巡检机器人的行走进行导航和定位。

其中，还包括：超声波传感器；所述超声波传感器通讯连接所述控制模块，并用于探测所述巡检机器人行进前侧的障碍物。

本发明实施例中的上述一个或多个技术方案，至少具有如下技术效果之一：

本发明实施例提供的畜禽舍巡检机器人自动路径规划与定位方法及装置，在对巡检机器人进行导航时，不需要配置辅助设备或标签，在畜禽舍内的行走路径没有预先规划的情况下，巡检机器人即可通过自身携带的传感设备，获取畜禽舍当前的环境信息，可基于本领域所公知的SLAM技术自主进行全局地图构建和全局路径规划，然后，基于巡检机器人行走时的三轴姿态角与加速度信息及行走里程信息，估算并更新巡检机器人实时的行走位姿，控制巡检机器人沿着全局路径行走，防止其行走的航迹出现偏差。

在此过程中，通过估算并更新巡检机器人实时的行走位姿，可有效消除巡检机器人沿全局路径进行导航行走时所出现的误差，而基于全局路径对巡检机器人进行的导航也在另一方面有效消除了在对巡检机器人的行走位姿进行长期估算与更新中所产生的累计误差，从而大大提升了巡检机器人在崎岖不平路面或者斜坡路面行走的稳定性和定位的准确性，并适用于巡检机器人在布局变动较频繁的畜禽舍内的巡检作业，进而也有效弥补了传统的GPS导航及其它需要提前规划路径与铺设辅助设备的导航方式所存在的缺陷。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例所示的畜禽舍巡检机器人自动路径规划与定位方法的流程图；

图2为本发明实施例所示的对畜禽舍巡检机器人具体进行导航与定位的流程图；

图3为本发明实施例所示的基于畜禽舍巡检机器人自动路径规划与定位方法的装置的控制结构框图。

图中：1、激光雷达；2、惯性测量单元；3、里程测量元件；4、控制模块；5、行走驱动系统；6、超声波传感器；7、磁力计。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

参见图1，本实施例提供了一种畜禽舍巡检机器人自动路径规划与定位方法，包括：S1，获取畜禽舍当前的环境信息，构建全局地图，在全局地图上规划巡检机器人行走的全局路径；S2，基于巡检机器人行走时的三轴姿态角与加速度信息及行走里程信息，估算并更新巡检机器人实时的行走位姿，控制巡检机器人沿着全局路径行走。

具体的，本实施例所示的畜禽舍巡检机器人自动路径规划与定位方法，在对巡检机器人进行导航时，不需要配置辅助设备或标签，在畜禽舍内的行走路径没有预先规划的情况下，巡检机器人即可通过自身携带的传感设备，例如：激光雷达，以此获取畜禽舍当前的环境信息，可基于本领域所公知的SLAM技术自主进行全局地图构建和全局路径规划，然后，基于巡检机器人行走时的三轴姿态角与加速度信息及行走里程信息，估算并更新巡检机器人实时的行走位姿，控制巡检机器人沿着全局路径行走，防止其行走的航迹出现偏差。

在此过程中，通过估算并更新巡检机器人实时的行走位姿，可有效消除巡检机器人沿全局路径进行导航行走时所出现的误差，而基于全局路径对巡检机器人进行的导航也在另一方面有效消除了在对巡检机器人的行走位姿进行长期估算与更新中所产生的累计误差，从而大大提升了巡检机器人在崎岖不平路面或者斜坡路面行走的稳定性和定位的准确性，适用于巡检机器人在畜禽舍养殖环境内的巡检作业，进而也有效弥补了传统的GPS导航及其它需要提前规划路径与铺设辅助设备的导航方式所存在的缺陷。

在此应指出的是，本实施例中畜禽舍内用于圈养畜禽的单元结构(如鸡笼)作为巡检机器人预巡检的目标物。在现有技术中，基于对环境的探测而实现地图构建与路径自动规划的导航装置包括雷达导航装置、视觉导航装置、声呐导航装置，考虑到激光雷达的抗干扰能力强，可采用激光雷达来获取畜禽舍当前的环境信息，该环境信息包括畜禽舍内多个目标物的距离、高度、方位等参数；为了便于数据处理，采用本领域所公知的SLAM技术自主进行全局地图构建，其中，SLAM技术是指即时定位与地图构建技术，用于巡检机器人在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航。

与此同时，本实施例采用惯性测量单元获取巡检机器人行走时的三轴姿态角与加速度信息，惯性测量单元包括本领域所公知的三轴加速度计与三轴陀螺仪，三轴加速度计用于输出巡检机器人在其坐标系统中的三个坐标轴方向上的加速度信息，而陀螺仪用于输出巡检机器人相对于导航坐标系的三个坐标轴方向上的角速度信息，从而便于基于巡检机器人在三维空间中的角速度以及加速度信息解算出其对应的姿态。其中，基于惯性测量单元的三轴加速度计可以获取巡检机器人相对于其重力方向的夹角，可提升巡检机器人行走在崎岖不平区域进行SLAM的性能，而基于惯性测量单元的三轴陀螺仪可以获取巡检机器人行走角速度信息，以提高巡检机器人基于SLAM进行局部定位的准确性。

其中，步骤S2中可基于惯性测量单元估算并更新巡检机器人实时的行走位姿，具体包括：基于惯性测量单元获取的三轴姿态角与加速度信息，巡检机器人的行走位姿进行预测，获取巡检机器人行走时的预测位姿；将预测位姿作为初始位姿与巡检机器人在全局地图上的位姿进行扫描匹配；进行突变检测，根据检测结果进行滤波器和参考扫描数据更新，获取扫描匹配位姿；将扫描匹配位姿与预测位姿通过滤波器进行融合，获取估算的巡检机器人实时的行走位姿，并可根据所估算的行走位姿对巡检机器人实时的行走位姿进行更新。

另外，本实施例还采用里程测量元件获取巡检机器人的行走里程信息，里程测量元件包括本领域所公知的编码器，该编码器安装在用于驱动巡检机器人前进行走的驱动电机上，并计算巡检机器人沿规划路径行走的里程，以便在规划的地图上对巡检机器人进行定位。

优选地，本实施例基于畜禽舍当前的环境信息，在具体进行地图创建时，采用Gmapping算法构建全局地图，其中，Gmapping算法是基于滤波SLAM框架的常用开源SLAM算法，而Gmapping算法是基于RBpf粒子滤波算法，即将定位和建图过程分离，先进行定位再进行建图，它运用重采样自适应技术，充分考虑粒子耗散问题和粒子逐渐更新权重而收敛的特性降低了畜禽舍巡检机器人位置在粒子滤波中的不确定性，从而使得巡检机器人对行走里程计信息的获取与基于激光雷达优化采集的激光束点阵等信息的采集质量得到大幅度地提升。

与此同时，Gmapping算法采用联合概率分布p(x_1：t，m|z_1：t，u_1：t-1)来估计地图m与巡检机器人的位姿x；其中，m表示地图，x_1:t＝x₁，…，x_t，x_1:t表示巡检机器人的位姿，z_1:t＝z₁，…，z_t，z_1:t表示激光雷达获取的畜禽舍当前的环境信息，u_1:t-1＝u₁，…，u_t-1，u_1:t-1表示巡检机器人的行走里程信息。利用先定位再建图的思想，可进一步将上述联合概率分布分解为下述公式：

p(x_1：t，m|z_1：t，u_1：t-1)＝p(m|x_1：t，z_1：t)·p(x_1：t|z_1：t，u_1：t-1)。

与此同时，本实施例基于Gmapping算法创建与畜禽舍对应的全局地图后，在步骤S1中具体基于巡检机器人行走的初始状态与目标状态，采用A-Star算法在全局地图上自动规划一条最短的全局路径；由此，在步骤S2中可进一步基于实时更新的巡检机器人的行走位姿，控制巡检机器人沿着最短的全局路径行走。

具体的，A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。A-Star算法中的距离估算值与实际值越接近，最终搜索速度越快。

A-Star算法的公式表示为：f(n)＝g(n)+h(n)；

式中，f(n)、g(n)、h(n)均为估价函数，f(n)表示巡检机器人在状态空间中从初始状态经由状态n到目标状态的代价估计，g(n)表示巡检机器人在状态空间中从初始状态到状态n的实际代价，h(n)表示巡检机器人从状态n到目标状态的最佳路径的估计代价。

对于上述路径搜索问题，初始状态、目标状态及状态n对应表示巡检机器人在全局地图中的节点位置，而代价表示距离。h(n)的选取保证找到最短路径(最优解)的条件，关键在于估价函数f(n)的选取，或者h(n)的选取。下面以估价函数d(n)表达状态n到目标状态的距离，则h(n)的选取大致分为如下三种情况：

(1)若h(n)＜d(n)，则表示搜索的点数多，搜索范围大，效率低，但能得到最优解。

(2)若h(n)＝d(n)，即距离估计h(n)等于最短距离，则搜索将严格沿着最短路径进行，此时的搜索效率是最高的。

(3)若h(n)>d(n)，则表示搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

由此，基于A-Star算法可在全局地图上自动规划一条最短的全局路径，以控制巡检机器人沿着最短的全局路径行走。

在控制巡检机器人行走时，巡检机器人不是全向移动，其移动状态分为前进与旋转，从而巡检机器人在两个相邻时刻Δt内的行走轨迹为一个圆弧轨迹，将该圆弧轨迹近似为直线轨迹，则巡检机器人以速度v，在两个相邻时刻Δt内沿坐标系x轴移动的距离为v·Δt，进而基于该直线轨迹与世界坐标系的x轴的夹角θ_t，只需将该段距离分别投影在世界坐标系的x轴和y轴，即可得到两个相邻时刻Δt内，巡检机器人在世界坐标系中坐标移动的位移Δx与Δy，由此可对巡检机器人建立如下公式所示的运动模型：

Δx＝νΔtcos(θ_t)；

Δy＝νΔtsin(θ_t)。

进一步的，通过上述建立的运动模型，基于巡检机器人的行走速度(包括前进速度与旋转速度)，即可推算巡检机器人的行走轨迹。由此，在此过程中需采用多组速度，以便于对巡检机器人的行走轨迹进行推算。在此，对于巡检机器人而言，应设定其行走的最大速度与最小速度。

由于巡检机器人的行走还受其自身驱动电机的性能的影响，即驱动电机的力矩有限，从而在转动中存在最大的加、减速限制，从而巡检机器人轨迹前行模拟的周期内，存在一个动态窗口，在该动态窗口内的速度是巡检机器人能够实际达到的速度，具体如下述公式所示：

V_d＝{(v，w)|v∈[v_c-v_bΔt，v_c+v_aΔt]，w∈[w_c-w_bΔt，w_c+w_aΔt]}；式中，V_d为动态窗口相应的速度集合，v，w为速度参变量，对应为巡检机器人的前进速度与旋转速度，v_c，w_c分别为巡检机器人当前的前进速度与旋转速度，v_a，w_a分别为巡检机器人前进与旋转的最大减速度，v_b，w_b分别为巡检机器人前进与旋转的最大加速度。

优选地，为了确保巡检机器人在沿着全局路径行走时，能够有效规避路径上的障碍物，如图1所示，本实施例中还包括：S3，在全局地图上进行局部路径规划，控制巡检机器人沿着规划的局部路径行走，对巡检机器人沿全局路径行走时遇到的障碍物进行规避。

具体的，本实施例采用超声波传感器来探测巡检机器人的行进前侧有无障碍物，在检测到巡检机器人的行进前侧有障碍物时，采用DWA算法在全局地图上规划巡检机器人行走的局部路径，采用如下公式所示的轨迹评价函数确定巡检机器人行走的局部路径：

G(v，w)＝σ·(α·heading(v，w)+β·dist(v，w)+γ·velocity(v，w))；其中，G(v,w)表示轨迹评价函数，heading(v,w)表示所述巡检机器人与障碍物的夹角，dist(v,w)表示所述巡检机器人距离与其行走轨迹相交的最近的障碍物的距离，velocity(v,w)表示所述巡检机器人的行走速度，α、β、γ依次为heading(v,w)、dist(v,w)及velocity(v,w)的权重，σ为轨迹平滑系数。

具体的，轨迹评价函数通过轨迹平滑系数σ可使得α、β、γ这三部分的权重更加平滑，使得规划的局部路径与障碍物之间保持一定的间距，且基于上述轨迹评价函数对DWA算法实施规划的局部路径进行评价，可获取一条最优的局部路径，以控制巡检机器人沿着局部路径行走，并在规避障碍物后，返回全局路径进行行走。

在此应指出的是，在遇到障碍物时，基于巡检机器人的安全考虑，为了确保巡检机器人能够在碰到障碍物前停下来，因此，巡检机器人在最大减速度V_a条件下，还应设置如下公式所示的约束条件：

图2为本实施例所示的对畜禽舍巡检机器人具体进行导航与定位的流程图，在对巡检机器人进行导航与定位时，可选择手动控制模式与自动控制模式。在自动控制模式下，向巡检机器人的控制模块导入上述实施例所示的基于激光雷达的SLAM而构建的全局地图，并启动里程测量元件进行里程计数。

接着，基于本领域所公知的电池电量检测芯片，以此来检测巡检机器人内电池的电量，在电池的电量低于预设阈值时，启动报警以进行充电警示，控制巡检机器人在全局地图上预设的充电位置寻找充电器，通过巡检机器人上的充电控制电机驱动自动充电模块与充电桩的充电部分结合，开始充电，在电池充满电后，再通过充电控制电机驱动自动充电模块缩回，并基于激光雷达获取巡检机器人行走的路径信息，控制巡检机器人沿全局路径行走。

在巡检机器人沿全局路径行走中遇到障碍物时，以如上实施例所示的方案，进行局部路径规划，以避开障碍物；与此同时，巡检机器人在沿着全局路径行走中巡检到生理信息异常的畜禽时，在全局地图上采用AMCL定位算法获取巡检机器人实时的位置信息，将该位置信息作为生理信息异常的畜禽的位置信息发送至云服务器，而巡检机器人在沿着全局路径行走中没有巡检到生理信息异常的畜禽时，继续保持当前的行走状态。

最后，按照上述对巡检机器人的行走规划方式，控制巡检机器人沿着全局路径继续行走，直至完成对畜禽舍的巡检作业。

优选地，如图3所示，本实施例还提供一种基于如上畜禽舍巡检机器人自动路径规划与定位方法的装置，包括：激光雷达1、惯性测量单元2、里程测量元件3和控制模块4；激光雷达1、惯性测量单元2及里程测量元件3分别连接控制模块4，控制模块4用于与巡检机器人的行走驱动系统5相连接；其中，激光雷达1用于采集畜禽舍当前的环境信息，惯性测量单元2用于采集巡检机器人行走时的三轴姿态角与加速度信息，里程测量元件3用于采集巡检机器人的行走里程信息，控制模块4用于基于激光雷达1、惯性测量单元2及里程测量元件3采集到的信息，通过行走驱动系统5对巡检机器人的行走进行导航和定位。

具体的，本实施例通过设置分别连接控制模块4的激光雷达1、惯性测量单元2、里程测量元件3，并将控制模块4与巡检机器人的行走驱动系统5相连接，则在控制巡检机器人行走时，激光雷达1获取畜禽舍当前的环境信息，控制模块4可基于本领域所公知的SLAM技术以此自动进行地图构建与路径规划；与此同时，可通过惯性测量单元2、里程测量元件3分别获取巡检机器人沿规划路径行走时的三轴姿态角与加速度信息及行走里程信息，获取巡检机器人实时的位置信息和姿态信息，从而实现对巡检机器人行走的导航和定位。

由此可见，本发明在对巡检机器人进行导航时，不需要配置辅助设备或标签，在畜禽舍内的行走路径没有预先规划的情况下，通过巡检机器人自身携带的传感设备，即可快速地在畜禽舍内对巡检机器人实施导航与定位，并能较好地控制巡检机器人在崎岖不平的路况下的行走姿态，以对巡检机器人实施精确地定位，其成本低廉，便于维护，有力地弥补传统的GPS导航及其它需要提前规划路径与铺设辅助设备的导航方式所存在的缺陷，适应巡检机器人在布局变动较频繁的畜禽舍内的行走。

在此应指出的是，激光雷达1采集的畜禽舍当前的环境信息包括畜禽舍内多个目标物的距离、高度、方位等参数；激光雷达可以为本领域所公知的TOF激光雷达，TOF激光雷达的最大探测距离大于10m。

控制模块4可采用本领域所公知的PLC控制器，行走驱动系统5包括本领域所公知的行走驱动电路及与巡检机器人各个行走轮相适配的驱动电机，从而控制模块4连接行走驱动电路，行走驱动电路连接巡检机器人上各个行走轮相应的驱动电机，以实现对巡检机器人行走姿态的控制。

优选地，本实施例中还包括：超声波传感器6；超声波传感器6通讯连接控制模块4，并用于探测所述巡检机器人行进前侧的障碍物。

具体的，超声波传感器6包括发射探头与接收探头，用于发射超声波，并在发射的超声波触碰到障碍物时，接收超声波反射的回波，从而超声波传感器6在检测到巡检机器人行进前侧的预设距离内有障碍物时，可即时向控制模块4反馈信号，以便控制模块4通过行走驱动系统5控制巡检机器人停止行走，并对巡检机器人的行走进行局部路径规划，从而防止对畜禽舍及其它障碍物产生直接碰撞。

最后应指出的是，在对巡检机器人进行导航时，还可配置磁力计7，磁力计7在本领域也称之为磁力仪或高斯计，用于检测畜禽舍当前环境的磁场，可以提供巡检机器人在现实世界中的东、南、西、北的朝向方位，从而可提高巡检机器人重定位的效率，并可配合惯性测量单元2，进一步提升对巡检机器人的导航与定位的准确性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，包括：S1，获取畜禽舍当前的环境信息，构建全局地图，在所述全局地图上规划巡检机器人行走的全局路径；

S2，基于所述巡检机器人行走时的三轴姿态角与加速度信息及行走里程信息，估算并更新所述巡检机器人实时的行走位姿，控制所述巡检机器人沿着所述全局路径行走。

2.根据权利要求1所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，还包括：S3，在所述全局地图上进行局部路径规划，控制所述巡检机器人沿着规划的局部路径行走，对所述巡检机器人沿所述全局路径行走时遇到的障碍物进行规避。

3.根据权利要求1所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，S1进一步包括：基于所述巡检机器人行走的初始状态与目标状态，采用A-Star算法在所述全局地图上自动规划一条最短的全局路径；

S2进一步包括：基于实时更新的所述巡检机器人的行走位姿，控制所述巡检机器人沿着最短的全局路径行走。

4.根据权利要求1至3任一所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，S2中所述估算并更新所述巡检机器人实时的行走位姿进一步包括：

基于所述巡检机器人行走时的三轴姿态角与加速度信息，获取所述巡检机器人行走时的预测位姿；将所述预测位姿作为初始位姿与所述巡检机器人在所述全局地图上的位姿进行扫描匹配；进行突变检测，获取扫描匹配位姿；将所述扫描匹配位姿与所述预测位姿进行融合，获取估算的所述巡检机器人实时的行走位姿，并对所述巡检机器人实时的行走位姿进行更新。

5.根据权利要求2所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，S3进一步包括：采用DWA算法在所述全局地图上规划所述巡检机器人行走的局部路径，采用如下公式所示的轨迹评价函数确定所述巡检机器人行走的局部路径：

G(v，w)＝σ·(α·heading(v，w)+β·dist(v，w)+γ·velocity(v，w))；

其中，G(v,w)表示轨迹评价函数，heading(v,w)表示所述巡检机器人与障碍物的夹角，dist(v,w)表示所述巡检机器人距离与其行走轨迹相交的最近的障碍物的距离，velocity(v,w)表示所述巡检机器人的行走速度，α、β、γ依次为heading(v,w)、dist(v,w)及velocity(v,w)的权重，σ为轨迹平滑系数。

6.根据权利要求1所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，S2进一步包括：所述巡检机器人在沿着所述全局路径行走中巡检到生理信息异常的畜禽时，在所述全局地图上采用AMCL定位算法获取所述巡检机器人实时的位置信息，将该位置信息作为生理信息异常的畜禽的位置信息发送至云服务器。

7.根据权利要求1所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，S2进一步包括：在所述巡检机器人沿着所述全局路径行走之前，检测所述巡检机器人内电池的电量，在所述电池的电量低于预设阈值时，控制所述巡检机器人行走至所述全局地图上预设的充电位置，以寻找充电器并进行充电，在所述电池充满电后，控制所述巡检机器人沿所述全局路径行走。

8.根据权利要求1所述的畜禽舍巡检机器人自动路径规划与定位方法，其特征在于，S1进一步包括：基于激光雷达获取畜禽舍当前的环境信息，基于Gmapping算法构建所述全局地图；

S2进一步包括：基于惯性测量单元获取所述巡检机器人行走时的三轴姿态角与加速度信息，基于里程测量元件获取所述巡检机器人行走时的行走里程信息。

9.一种基于权利要求1至8任一所述的畜禽舍巡检机器人自动路径规划与定位方法的装置，其特征在于，

包括：激光雷达、惯性测量单元、里程测量元件和控制模块；所述激光雷达、所述惯性测量单元及所述里程测量元件分别连接所述控制模块，所述控制模块用于与巡检机器人的行走驱动系统相连接；

其中，所述激光雷达用于采集畜禽舍当前的环境信息，所述惯性测量单元用于采集所述巡检机器人行走时的三轴姿态角与加速度信息，所述里程测量元件用于采集所述巡检机器人的行走里程信息，所述控制模块用于基于所述激光雷达、所述惯性测量单元及所述里程测量元件采集到的信息，通过所述行走驱动系统对所述巡检机器人的行走进行导航和定位。

10.根据权利要求9所述的装置，其特征在于，

还包括：超声波传感器；所述超声波传感器通讯连接所述控制模块，并用于探测所述巡检机器人行进前侧的障碍物。

Images