CN115771153A - 一种多指标采集的畜禽健康巡检机器人控制方法及装备 - Google Patents

Abstract

本发明公开了一种多指标采集的畜禽健康巡检机器人控制方法及装备，涉及巡检机器人控制领域，该方法包括以下步骤：S1、对机器人进行初始检测；S2、根据设定的巡检时间和目标巡检点位，生成目标位置指令并对机器人发送目标位置指令；S3、根据步骤S2中的目标位置指令控制机器人到达目标巡检点位；S4、控制机器人在步骤S3中到达的目标巡检点位采集多指标数据；S5、判断是否巡检完设定的目标巡检点位；若是则巡检结束并返回充电桩，否则跳转到步骤S2。本发明能控制机器人实现自动定时定点完成数据采集的任务，并且能全面准确地采集包括深度、彩色、红外视频数据和温湿度、二氧化碳浓度、声音环境数据的多指标数据。

Description

一种多指标采集的畜禽健康巡检机器人控制方法及装备

技术领域

本发明涉及巡检机器人控制领域，具体涉及一种多指标采集的畜禽健康巡检机器人控制方法及装备。

背景技术

随着畜禽养殖业的不断发展，结构化、工厂化养殖开始不断兴起，生猪养殖行业楼房养猪规模不断扩大，禽类养殖场也开始进行更加结构化的家系栏养殖。结构化、工厂化养殖的最大好处就是节省土地面积、减少运输成本。工厂化养殖也存在一系列问题，例如由于畜禽养殖密度增大，疫病侵入时更容易造成大面积感染，人工无法实现24小时巡检等。由于畜禽健康巡检机器人能够更加安全，更加全面地采集畜禽生长的各类生理指标和环境指标，为畜禽健康监测提供大量的数据支持，所以结构化、工厂化养殖更加需要畜禽健康巡检机器人来完成畜禽健康监测的巡检任务。利用畜禽健康巡检机器人能够为工厂化畜禽养殖健康监测提供支撑，具有重要的意义。

在传统的畜禽养殖中，畜禽健康数据都由人工测量和记录，由于畜禽养殖的数量不断增长，人工测量和记录的方法费时、费力且容易出现记录错漏。现有技术中搭载了信息采集组件，但其仅能采集养殖对象的图像信息，缺少监测环境数据的感知设备，无法自动对监测畜禽生理指标的感知设备提供校准数据，也无法对畜禽生长环境状况进行监测。现有技术中搭载的感知设备无法自动定时定点到达设定位置，无法自动获取所需位置的畜禽生理数据和环境数据，不利于养殖场长时间监测畜禽健康状况。现有技术中使用电子标签来实现机器人对预设监测位置的准确定位，大量的电子标签需要对预设位置信息进行读写，且电子标签的部署需要大量的人工成本。在实际养殖场场景中，不仅需要采集各类体现畜禽健康状况的生理数据和环境数据，并且要求能够在设定时间段自动到达设定位置完成数据采集任务，提升效率并节省能耗。

发明内容

针对现有技术中的上述不足，本发明提供的一种多指标采集的畜禽健康巡检机器人控制方法及装备，能控制机器人实现自动定时定点完成多指标数据采集的任务。

为了达到上述发明目的，本发明采用的技术方案为：

一种多指标采集的畜禽健康巡检机器人控制方法，包括以下步骤：

S1、对机器人进行初始检测；

S2、根据设定的巡检时间和目标巡检点位，生成目标位置指令并对机器人发送目标位置指令；

S3、根据步骤S2中的目标位置指令控制机器人到达目标巡检点位；

S4、控制机器人在步骤S3中到达的目标巡检点位采集多指标数据；

S5、判断是否巡检完设定的目标巡检点位；若是则巡检结束并返回充电桩，否则跳转到步骤S2。

进一步地，步骤S1包括以下分步骤：

S11、判断机器人的电量是否充足；若是则进入步骤S2，否则向机器人发送充电指令并进入分步骤S12；

S12、判断机器人的激光数据是否异常；若是则报警，否则根据分步骤S11中的充电指令控制机器人返回充电桩。

进一步地，在分步骤S12中，判断机器人的激光数据是否异常包括以下步骤：

S121、判断连续两帧激光数据的变幅是否大于设定的激光数据变幅阈值；若是则判为数据存在异常，否则进入分步骤S122；

S122、判断当前帧激光数据是否在设定的激光数据最小阈值和激光数据最大阈值区间内；若是则进入分步骤S123，否则判为数据存在异常；

S123、判断当前帧激光数据与上一定位周期的最优估计位置值之差是否大于设定的实测误差阈值；若是则判为数据存在异常，否则判为数据不存在异常。

进一步地，步骤S3包括以下分步骤：

S31、根据机器人上一定位周期的最优估计位置、编码器读数和激光测距传感器读数，确定机器人当前定位周期的最优估计位置；

S32、判断分步骤S31中机器人当前定位周期的最优估计位置与目标位置的差值是否小于设定的目标误差阈值；若是则到达目标巡检点位，否则进入分步骤S33；

S33、判断机器人的激光数据是否存在异常；若是则报警，否则进入到分步骤S34；

S34、根据目标位置和上一定位周期的最优估计位置，确定当前速度控制周期控制电机的电流值；

S35、根据分步骤S34中当前速度控制周期电机的电流值控制机器人移动，更新编码器读数和激光测距传感器读数，并跳转到分步骤S31。

进一步地，S31包括以下分步骤：

S311、根据编码器读数确定当前定位周期电机编码器的累计距离；

S312、根据分步骤S311中当前定位周期电机编码器的累计距离，计算当前定位周期的先验估计位置，计算式为：

其中：

为当前定位周期的先验估计位置，

为上一定位周期的最优估计位置，

为当前定位周期电机编码器的累计距离；

S313、根据上一定位周期的后验估计方差和过程噪声方差计算当前定位周期的先验估计方差，计算公式为：

其中：

为当前定位周期的先验估计方差，

为上一定位周期的后验估计方差，

，

为上一定位周期的卡尔曼增益，

为上一定位周期的先验估计方差，Q为过程噪声方差；

S314、根据自适应测量噪声方差函数计算测量噪声方差，所述自适应测量噪声方差函数的解析式为：

其中：R为测量噪声方差，

为自适应测量噪声方差系数，Z _K 为当前定位周期的实际观测位置；

S315、根据分步骤S313中当前定位周期的先验估计方差和分步骤S314中的测量噪声方差，计算当前定位周期的卡尔曼增益，计算式为：

其中：

为当前定位周期的卡尔曼增益；

S316、根据分步骤S312中当前定位周期的先验估计位置、分步骤S315中当前定位周期的卡尔曼增益以及当前定位周期的实际观测位置，计算当前定位周期的最优估计位置，计算式为：

其中：

为当前定位周期的最优估计位置。

进一步地，步骤S34包括以下分步骤：

S341、根据目标位置和上一定位周期的最优估计位置，计算当前速度控制周期的机器人目标速度，计算式为：

其中：

为当前速度控制周期的机器人目标速度，

为位置比例系数，

为目标位置，

为上一定位周期的最优估计位置，k为当前周期；

S342、根据上一速度控制周期的反馈速度和分步骤S341中当前速度控制周期的机器人目标速度，计算当前速度控制周期控制电机的电流值，计算式为：

其中：

为当前速度控制周期控制电机的电流值，

为速度比例系数，

为当前速度控制周期的机器人目标速度差值，

，

为上一速度控制周期的反馈速度，

为积分系数，

为速度控制周期的机器人目标速度差值，i=0,1,2…k。

进一步地，步骤S4包括以下分步骤：

S41、通过多线程按时序采集包括深度视频数据、彩色视频数据、红外视频数据、温湿度数据、二氧化碳浓度数据和声音数据的多指标数据；

S42、选取分步骤S41中多指标数据的最晚起始时间作为当前位置数据的时间戳；

S43、截取当前位置数据在分步骤S42中的时间戳之后的多指标数据；

S44、将位置信息和分步骤S42中的时间戳添加到分步骤S43中的多指标数据上，并上传具有位置信息和时间戳的多指标数据。

一种应用于上述方法的多指标采集的畜禽健康巡检机器人控制装备，包括：

驱动轮电机组，所述驱动轮电机组包括驱动轮电机和工字钢轨道，所述驱动轮电机用于接收巡检控制模块的目标位置指令，驱动机器人在所述工字钢轨道上运动到达目标位置；

激光测距组，所述激光测距组包括两个激光测距传感器和两个激光反射挡板，所述激光测距传感器用于测量得到机器人当前定位周期的观测位置信息并传输至巡检控制模块，所述激光反射挡板用于反射激光测距传感器发射的激光光束；

视频数据采集模组，所述视频数据采集模组包括深度摄像头、彩色摄像头和红外摄像头，所述深度摄像头用于接收巡检控制模块的采集指令采集深度视频数据，并将深度视频数据传输至巡检控制模块，所述彩色摄像头用于接收巡检控制模块的采集指令采集彩色视频数据，并将彩色视频数据传输至巡检控制模块，所述红外摄像头用于接收巡检控制模块的采集指令采集红外视频数据，并将红外视频数据传输至巡检控制模块；

环境数据采集模组，所述环境数据采集模组包括温湿度传感器、二氧化碳传感器和声音采集器，所述温湿度传感器用于接收巡检控制模块的采集指令采集环境的温度和湿度数据，并将温度和湿度数据传输至巡检控制模块，所述二氧化碳传感器用于接收巡检控制模块的采集指令采集环境中的二氧化碳浓度数据，并将二氧化碳浓度数据传输至巡检控制模块，所述声音采集器用于接收巡检控制模块的采集指令采集环境中的声音数据，并将声音数据传输至巡检控制模块；

巡检控制模块，所述巡检控制模块用于对机器人进行初始检测，根据设定的巡检时间和目标巡检点位生成目标位置指令，并向驱动轮电机组发送目标位置指令控制机器人到达目标位置，接收激光测距组传输的机器人当前定位周期的观测位置信息并检验激光数据是否存在异常，生成采集指令发送至视频采集模组，并接收视频数据采集模组传输的多指标数据的视频数据，生成采集指令发送至环境采集模组，接收环境数据采集模组传输的多指标数据的环境数据。

本发明的有益效果为：

（1）本发明能够自动完成按照设定的时间和位置进行视频和环境数据的采集任务，使用自己设计的视频数据和环境数据分段方法对数据进行处理和存储，相较以往的数据采集方法，不仅能够提供全面准确的数据，并且能够在无人条件下自主运行，突破时间限制，提升效率，使畜禽健康监测更加全面；

（2）本发明采用基于自适应卡尔曼滤波算法的多传感器融合定位方法，利用本发明设计的自适应测量噪声方差函数自适应调整卡尔曼增益，融合机器人上一定位周期的位置信息、当前定位周期施加控制量后的编码器读数和激光测距传感器读数，计算得到当前定位周期机器人的最优估计位置，保证机器人定位更加精准，采集数据更加准确可用；

（3）本发明采用基于反馈机制的速度自适应控制方法，通过上一定位周期的最优估计位置与目标位置的误差计算当前速度控制周期机器人的目标速度，通过上一速度控制周期机器人的速度与目标速度的误差计算当前速度控制周期控制电机的电流值，在保证效率的同时，减少功率损耗，能够快速准确地到达设定巡检位置，可更节能高效地完成巡检任务；

（4）本发明中的机器人巡检控制方法能够更好地适应畜禽养殖场的数据采集和巡检需求，并且能够对异常情况做出处理和反馈；

（5）本发明的视频数据采集设备的安装结构设置为可调节角度，能够更好地适应不同场景不同角度的视频数据采集需求，保证各个设备采集的数据的一致性。

附图说明

图1为一种多指标采集的畜禽健康巡检机器人控制方法流程图；

图2为一种多指标采集的畜禽健康巡检机器人控制方法的多传感器融合定位子模块流程图；

图3为一种多指标采集的畜禽健康巡检机器人控制方法的速度自适应控制子模块流程图；

图4为一种多指标采集的畜禽健康巡检机器人控制方法的数据采集处理子模块流程图；

图5为一种应用于上述方法的多指标采集的畜禽健康巡检机器人控制装备图；

图6为一种应用于上述方法的多指标采集的畜禽健康巡检机器人控制装备的数据采集设备安装结构示意图；

其中附图标记为：1、驱动轮电机，2、第一激光测距传感器，3、视频数据采集模组，4、第二激光测距传感器，5、工字钢轨道，6、第一激光反射挡板，7、第二激光反射挡板，8、环境数据采集模组，9、巡检控制模块，10、声音采集器，11、温湿度传感器，12、二氧化碳传感器，13、深度摄像头，14、彩色摄像头，15、红外摄像头。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图1所示，一种多指标采集的畜禽健康巡检机器人控制方法，包括步骤S1-S5：

S1、对机器人进行初始检测。

在本发明的一个可选实施例中，当巡检控制模块开始执行后，巡检控制模块会监测机器人自身电量是否充足，若电量不足，巡检控制模块会下发充电指令。然后，巡检控制模块会检查激光数据是否异常，若无异常则控制机器人返回充电桩充电，若出现异常则证明激光测距传感器检测到机器人运动方向存在异物或者激光测距传感器出现损坏，此时巡检控制模块将报警并反馈到云服务器，等待管理员检修后重新运行机器人。

步骤S1包括以下分步骤：

S11、判断机器人的电量是否充足；若是则进入步骤S2，否则向机器人发送充电指令并进入分步骤S12。

S12、判断机器人的激光数据是否异常；若是则报警，否则根据分步骤S11中的充电指令控制机器人返回充电桩。

判断机器人的激光数据是否异常包括以下步骤：

S121、判断连续两帧激光数据的变幅是否大于设定的激光数据变幅阈值；若是则判为数据存在异常，否则进入分步骤S122。

S122、判断当前帧激光数据是否在设定的激光数据最小阈值和激光数据最大阈值区间内；若是则进入分步骤S123，否则判为数据存在异常。

S123、判断当前帧激光数据与上一定位周期的最优估计位置值之差是否大于设定的实测误差阈值；若是则判为数据存在异常，否则判为数据不存在异常。

S2、根据设定的巡检时间和目标巡检点位，生成目标位置指令并对机器人发送目标位置指令。

在本发明的一个可选实施例中，根据设定的巡检时间和目标巡检点位，巡检控制模块生成目标位置指令并向驱动轮电机组发送目标位置指令。

S3、根据步骤S2中的目标位置指令控制机器人到达目标巡检点位。

在本发明的一个可选实施例中，驱动轮电机组接收到目标位置指令，在到达设定巡检时间后，驱动机器人开始执行巡检任务。

在机器人执行巡检任务过程中，本发明采用基于自适应卡尔曼滤波算法的多传感器融合定位方法，如图2所示，提供了一种多指标采集的畜禽健康巡检机器人控制方法的多传感器融合定位子模块流程图，利用本发明设计的自适应测量噪声方差函数自适应调整卡尔曼增益，融合机器人上一定位周期的位置信息、当前定位周期施加控制量后的编码器读数和激光测距传感器读数，不断更新当前定位周期机器人的最优估计位置。

同时，本发明采用基于反馈机制的速度自适应控制方法，如图3所示，提供了一种多指标采集的畜禽健康巡检机器人控制方法的速度自适应控制子模块流程图，通过上一定位周期机器人的最优估计位置与目标位置的误差计算当前速度控制周期机器人的目标速度，通过上一速度控制周期机器人的速度与目标速度的误差计算当前速度控制周期控制电机的电流值，然后利用得到的电流值驱动机器人到达目标巡检点位。

步骤S3包括以下分步骤：

S31、根据机器人上一定位周期的最优估计位置、编码器读数和激光测距传感器读数，确定机器人当前定位周期的最优估计位置。

S31包括以下分步骤：

S311、根据编码器读数确定当前定位周期电机编码器的累计距离。

S312、根据分步骤S311中当前定位周期电机编码器的累计距离，计算当前定位周期的先验估计位置，计算式为：

其中：

为当前定位周期的先验估计位置，

为上一定位周期的最优估计位置，

为当前定位周期电机编码器的累计距离。

S313、根据上一定位周期的后验估计方差和过程噪声方差计算当前定位周期的先验估计方差，计算公式为：

其中：

为当前定位周期的先验估计方差，

为上一定位周期的后验估计方差，

，

为上一定位周期的卡尔曼增益，

为上一定位周期的先验估计方差，Q为过程噪声方差。

S314、根据自适应测量噪声方差函数计算测量噪声方差，所述自适应测量噪声方差函数的解析式为：

其中：R为测量噪声方差，

为自适应测量噪声方差系数，Z _K 为当前定位周期的实际观测位置。

S315、根据分步骤S313中当前定位周期的先验估计方差和分步骤S314中的测量噪声方差，计算当前定位周期的卡尔曼增益，计算式为：

其中：

为当前定位周期的卡尔曼增益。

S316、根据分步骤S312中当前定位周期的先验估计位置、分步骤S315中当前定位周期的卡尔曼增益以及当前定位周期的实际观测位置，计算当前定位周期的最优估计位置，计算式为：

其中：

为当前定位周期的最优估计位置。

S32、判断分步骤S31中机器人当前定位周期的最优估计位置与目标位置的差值是否小于设定的目标误差阈值；若是则到达目标巡检点位，否则进入分步骤S33。

S33、判断机器人的激光数据是否存在异常；若是则报警，否则进入到分步骤S34。

S34、根据目标位置和上一定位周期的最优估计位置，确定当前速度控制周期控制电机的电流值。

步骤S34包括以下分步骤：

S341、根据目标位置和上一定位周期的最优估计位置，计算当前速度控制周期的机器人目标速度，计算式为：

其中：

为当前速度控制周期的机器人目标速度，

为位置比例系数，

为目标位置，

为上一定位周期的最优估计位置，k为当前周期。

S342、根据上一速度控制周期的反馈速度和分步骤S341中当前速度控制周期的机器人目标速度，计算当前速度控制周期控制电机的电流值，计算式为：

其中：

为当前速度控制周期控制电机的电流值，

为速度比例系数，

为当前速度控制周期的机器人目标速度差值，

，

为上一速度控制周期的反馈速度，

为积分系数，

为速度控制周期的机器人目标速度差值，i=0,1,2…k。

S35、根据分步骤S34中当前速度控制周期电机的电流值控制机器人移动，更新编码器读数和激光测距传感器读数，并跳转到分步骤S31。

S4、控制机器人在步骤S3中到达的目标巡检点位采集多指标数据。

在本发明的一个可选实施例中，判定机器人到达目标巡检点位后，巡检控制模块生成采集指令并将采集指令发送至包括深度摄像头、彩色摄像头和红外摄像头的视频数据采集模组，以及包括温湿度传感器、二氧化碳传感器和声音采集器的环境数据采集模组。视频数据采集模组和环境数据采集模组接收到巡检控制模块的采集指令开始采集视频数据和环境数据，并将采集到的视频数据和环境数据传输至巡检控制模块。

如图4所示，提供了一种多指标采集的畜禽健康巡检机器人控制方法的数据采集处理子模块流程图。当机器人到达设定巡检位置后，巡检控制模块通过采集指令控制数据采集处理子模块，即控制彩色摄像头、红外摄像头、深度摄像头、温湿度传感器、二氧化碳传感器和声音采集器通过多线程按照时间顺序执行数据采集指令。

具体地，当所有采集设备在当前采集位置的已采集数据时长达到60秒，并且数据时间长度同时覆盖时长达到30秒，本发明选取数据采集最晚起始时间作为当前位置数据时间戳，截取当前位置数据时间戳后30秒的视频数据和环境数据，并为各段视频数据和环境数据添加时间戳和位置信息，保证多个设备采集的多项数据在时间上的一致性，随后上传处理后的视频数据和环境数据。

步骤S4包括以下分步骤：

S41、通过多线程按时序采集包括深度视频数据、彩色视频数据、红外视频数据、温湿度数据、二氧化碳浓度数据和声音数据的多指标数据。

S42、选取分步骤S41中多指标数据的最晚起始时间作为当前位置数据的时间戳。

S43、截取当前位置数据在分步骤S42中的时间戳之后的多指标数据。

S44、将位置信息和分步骤S42中的时间戳添加到分步骤S43中的多指标数据上，并上传具有位置信息和时间戳的多指标数据。

S5、判断是否巡检完设定的目标巡检点位；若是则巡检结束并返回充电桩，否则跳转到步骤S2。

如图5所示，一种应用于上述方法的多指标采集的畜禽健康巡检机器人控制装备，包括驱动轮电机组、激光测距组、视频数据采集模组、环境数据采集模组和巡检控制模块，具体如下：

驱动轮电机组，所述驱动轮电机组包括驱动轮电机1和工字钢轨道5，所述驱动轮电机1用于接收巡检控制模块的目标位置指令，驱动机器人在所述工字钢轨道5上运动到达目标位置。

在本发明的一个可选实施例中，到达设定的巡检时间后，巡检控制模块生成目标位置指令并发送至驱动轮电机组。驱动轮电机组接收到目标位置指令，驱动机器人在工字钢轨道上移动，最终将机器人送达到目标位置。当巡检完所有目标位置后，驱动轮电机组驱动机器人返回充电桩进行充电。

激光测距组，所述激光测距组包括第一激光测距传感器2、第二激光测距传感器4、第一激光反射挡板6和第二激光反射挡板7，所述第一激光测距传感器2和第二激光测距传感器4用于测量得到机器人当前定位周期的观测位置信息并传输至巡检控制模块，所述第一激光反射挡板6和第二激光反射挡板7用于反射激光测距传感器发射的激光光束。

在本发明的一个可选实施例中，激光测距组通过两个激光测距传感器和两个激光反射挡板，能够实时测量当前定位周期的实际观测位置，并将实际观测位置信息传输至巡检控制模块，巡检控制模块可根据实际观测位置信息进一步计算得到机器人的最优估计位置，实现机器人的精确定位。

视频数据采集模组3，所述视频数据采集模组包括深度摄像头、彩色摄像头和红外摄像头，所述深度摄像头用于接收巡检控制模块的采集指令采集深度视频数据，并将深度视频数据传输至巡检控制模块，所述彩色摄像头用于接收巡检控制模块的采集指令采集彩色视频数据，并将彩色视频数据传输至巡检控制模块，所述红外摄像头用于接收巡检控制模块的采集指令采集红外视频数据，并将红外视频数据传输至巡检控制模块。

如图6所示，提供了一种应用于上述方法的多指标采集的畜禽健康巡检机器人控制装备的数据采集设备安装结构示意图。彩色摄像头14安装于机器人底部的视频数据采集模组的中间，其摄像头可绕安装轴旋转，旋转范围为-45°~45°。红外摄像头15安装于机器人底部的视频数据采集模组的一侧，其摄像头可绕安装轴旋转，旋转范围为-30°~30°。深度摄像头13安装于机器人底部的视频数据采集模组的一侧，其摄像头可绕安装轴旋转，旋转范围为-30°~30°。

在本发明的一个可选实施例中，当机器人到达设定巡检位置后，巡检控制模块通过采集指令控制视频数据采集模组，即控制彩色摄像头、红外摄像头、深度摄像头通过多线程按照时间顺序执行数据采集指令。

具体地，当彩色摄像头、红外摄像头、深度摄像头在当前采集位置的已采集数据时长达到60秒，并且数据时间长度同时覆盖时长达到30秒，本发明选取数据采集最晚起始时间作为当前位置视频数据的时间戳，截取当前位置数据时间戳后30秒的视频数据，并为各段视频数据添加时间戳和位置信息，保证多个设备采集的多项数据在时间上的一致性，随后上传处理后的视频数据和环境数据。

环境数据采集模组8，所述环境数据采集模组包括温湿度传感器、二氧化碳传感器和声音采集器，所述温湿度传感器用于接收巡检控制模块的采集指令采集环境的温度和湿度数据，并将温度和湿度数据传输至巡检控制模块，所述二氧化碳传感器用于接收巡检控制模块的采集指令采集环境中的二氧化碳浓度数据，并将二氧化碳浓度数据传输至巡检控制模块，所述声音采集器用于接收巡检控制模块的采集指令采集环境中的声音数据，并将声音数据传输至巡检控制模块。

如图6所示，提供了一种应用于上述方法的多指标采集的畜禽健康巡检机器人控制装备的数据采集设备安装结构示意图。温湿度传感器11安装于机器人底部，伸出的探头可采集环境的温度和湿度数据。二氧化碳传感器12安装于机器人底部，伸出的探头可采集环境中的二氧化碳浓度数据。声音采集器10安装于机器人底部，伸出的麦克风可采集环境中的声音数据。

在本发明的一个可选实施例中，当机器人到达设定巡检位置后，巡检控制模块通过采集指令控制环境数据采集模组，即控制温湿度传感器、二氧化碳传感器和声音采集器通过多线程按照时间顺序执行采集指令。

具体地，当温湿度传感器、二氧化碳传感器和声音采集器在当前采集位置的已采集数据时长达到60秒，并且数据时间长度同时覆盖时长达到30秒，本发明选取数据采集最晚起始时间作为当前位置环境数据的时间戳，截取当前位置数据时间戳后30秒的环境数据，并为各段环境数据添加时间戳和位置信息，保证多个设备采集的多项数据在时间上的一致性，随后上传处理后的环境数据。

巡检控制模块9，所述巡检控制模块用于对机器人进行初始检测，根据设定的巡检时间和目标巡检点位生成目标位置指令，并向驱动轮电机组发送目标位置指令控制机器人到达目标位置，接收激光测距组传输的机器人当前定位周期的观测位置信息并检验激光数据是否存在异常，生成采集指令发送至视频采集模组，并接收视频数据采集模组传输的多指标数据的视频数据，生成采集指令发送至环境采集模组，接收环境数据采集模组传输的多指标数据的环境数据。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (8)

1.一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，包括以下步骤：

S1、对机器人进行初始检测；

S2、根据设定的巡检时间和目标巡检点位，生成目标位置指令并对机器人发送目标位置指令；

S3、根据步骤S2中的目标位置指令控制机器人到达目标巡检点位；

S4、控制机器人在步骤S3中到达的目标巡检点位采集多指标数据；

S5、判断是否巡检完设定的目标巡检点位；若是则巡检结束并返回充电桩，否则跳转到步骤S2。

2.根据权利要求1所述的一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，步骤S1包括以下分步骤：

S11、判断机器人的电量是否充足；若是则进入步骤S2，否则向机器人发送充电指令并进入分步骤S12；

S12、判断机器人的激光数据是否异常；若是则报警，否则根据分步骤S11中的充电指令控制机器人返回充电桩。

3.根据权利要求2所述的一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，在分步骤S12中，判断机器人的激光数据是否异常包括以下步骤：

S121、判断连续两帧激光数据的变幅是否大于设定的激光数据变幅阈值；若是则判为数据存在异常，否则进入分步骤S122；

S122、判断当前帧激光数据是否在设定的激光数据最小阈值和激光数据最大阈值区间内；若是则进入分步骤S123，否则判为数据存在异常；

S123、判断当前帧激光数据与上一定位周期的最优估计位置值之差是否大于设定的实测误差阈值；若是则判为数据存在异常，否则判为数据不存在异常。

4.根据权利要求1所述的一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，步骤S3包括以下分步骤：

S31、根据机器人上一定位周期的最优估计位置、编码器读数和激光测距传感器读数，确定机器人当前定位周期的最优估计位置；

S32、判断分步骤S31中机器人当前定位周期的最优估计位置与目标位置的差值是否小于设定的目标误差阈值；若是则到达目标巡检点位，否则进入分步骤S33；

S33、判断机器人的激光数据是否存在异常；若是则报警，否则进入到分步骤S34；

S34、根据目标位置和上一定位周期的最优估计位置，确定当前速度控制周期控制电机的电流值；

S35、根据分步骤S34中当前速度控制周期电机的电流值控制机器人移动，更新编码器读数和激光测距传感器读数，并跳转到分步骤S31。

5.根据权利要求4所述的一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，S31包括以下分步骤：

S311、根据编码器读数确定当前定位周期电机编码器的累计距离；

S312、根据分步骤S311中当前定位周期电机编码器的累计距离，计算当前定位周期的先验估计位置，计算式为：

其中：

为当前定位周期的先验估计位置，

为上一定位周期的最优估计位置，

为当前定位周期电机编码器的累计距离；

S313、根据上一定位周期的后验估计方差和过程噪声方差计算当前定位周期的先验估计方差，计算公式为：

其中：

为当前定位周期的先验估计方差，

为上一定位周期的后验估计方差，

，

为上一定位周期的卡尔曼增益，

为上一定位周期的先验估计方差，Q为过程噪声方差；

S314、根据自适应测量噪声方差函数计算测量噪声方差，所述自适应测量噪声方差函数的解析式为：

其中：R为测量噪声方差，

为自适应测量噪声方差系数，Z _K 为当前定位周期的实际观测位置；

S315、根据分步骤S313中当前定位周期的先验估计方差和分步骤S314中的测量噪声方差，计算当前定位周期的卡尔曼增益，计算式为：

其中：

为当前定位周期的卡尔曼增益；

S316、根据分步骤S312中当前定位周期的先验估计位置、分步骤S315中当前定位周期的卡尔曼增益以及当前定位周期的实际观测位置，计算当前定位周期的最优估计位置，计算式为：

其中：

为当前定位周期的最优估计位置。

6.根据权利要求4所述的一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，步骤S34包括以下分步骤：

S341、根据目标位置和上一定位周期的最优估计位置，计算当前速度控制周期的机器人目标速度，计算式为：

其中：

为当前速度控制周期的机器人目标速度，

为位置比例系数，

为目标位置，

为上一定位周期的最优估计位置，k为当前周期；

S342、根据上一速度控制周期的反馈速度和分步骤S341中当前速度控制周期的机器人目标速度，计算当前速度控制周期控制电机的电流值，计算式为：

其中：

为当前速度控制周期控制电机的电流值，

为速度比例系数，

为当前速度控制周期的机器人目标速度差值，

，

为上一速度控制周期的反馈速度，

为积分系数，

为速度控制周期的机器人目标速度差值，i=0,1,2…k。

7.根据权利要求1所述的一种多指标采集的畜禽健康巡检机器人控制方法，其特征在于，步骤S4包括以下分步骤：

S41、通过多线程按时序采集包括深度视频数据、彩色视频数据、红外视频数据、温湿度数据、二氧化碳浓度数据和声音数据的多指标数据；

S42、选取分步骤S41中多指标数据的最晚起始时间作为当前位置数据的时间戳；

S43、截取当前位置数据在分步骤S42中的时间戳之后的多指标数据；

S44、将位置信息和分步骤S42中的时间戳添加到分步骤S43中的多指标数据上，并上传具有位置信息和时间戳的多指标数据。

8.一种应用权利要求1-7任一所述方法的多指标采集的畜禽健康巡检机器人控制装备，其特征在于，包括：

驱动轮电机组，所述驱动轮电机组包括驱动轮电机和工字钢轨道，所述驱动轮电机用于接收巡检控制模块的目标位置指令，驱动机器人在所述工字钢轨道上运动到达目标位置；

激光测距组，所述激光测距组包括两个激光测距传感器和两个激光反射挡板，所述激光测距传感器用于测量得到机器人当前定位周期的观测位置信息并传输至巡检控制模块，所述激光反射挡板用于反射激光测距传感器发射的激光光束；

视频数据采集模组，所述视频数据采集模组包括深度摄像头、彩色摄像头和红外摄像头，所述深度摄像头用于接收巡检控制模块的采集指令采集深度视频数据，并将深度视频数据传输至巡检控制模块，所述彩色摄像头用于接收巡检控制模块的采集指令采集彩色视频数据，并将彩色视频数据传输至巡检控制模块，所述红外摄像头用于接收巡检控制模块的采集指令采集红外视频数据，并将红外视频数据传输至巡检控制模块；

环境数据采集模组，所述环境数据采集模组包括温湿度传感器、二氧化碳传感器和声音采集器，所述温湿度传感器用于接收巡检控制模块的采集指令采集环境的温度和湿度数据，并将温度和湿度数据传输至巡检控制模块，所述二氧化碳传感器用于接收巡检控制模块的采集指令采集环境中的二氧化碳浓度数据，并将二氧化碳浓度数据传输至巡检控制模块，所述声音采集器用于接收巡检控制模块的采集指令采集环境中的声音数据，并将声音数据传输至巡检控制模块；

巡检控制模块，所述巡检控制模块用于对机器人进行初始检测，根据设定的巡检时间和目标巡检点位生成目标位置指令，并向驱动轮电机组发送目标位置指令控制机器人到达目标位置，接收激光测距组传输的机器人当前定位周期的观测位置信息并检验激光数据是否存在异常，生成采集指令发送至视频采集模组，并接收视频数据采集模组传输的多指标数据的视频数据，生成采集指令发送至环境采集模组，接收环境数据采集模组传输的多指标数据的环境数据。

Images