CN114237238A - 一种自主导航的智能投料饲喂机器人及其投料饲喂方法 - Google Patents

Abstract

本发明公开了一种自主导航的智能投料饲喂机器人及其投料饲喂方法，其包括行走组件，行走组件包括行走架，行走架前端的左右两侧连接有至少一个提供动力的行走轮，行走架后端的下部连接有用于转向及提供动力的转向轮；投料组件，投料组件包括固定连接在行走架上的储料箱，储料箱上方连接有角度可调的投料臂，投料臂伸出行走架外的外周开有朝上设置的投料口，储料箱的前侧固定连接有激光雷达，投料臂伸出储料箱外的一侧连接有用于识别饲料桶桶口的摄像头；本发明能按照规划好的路径行驶且在行驶过程中能实时检测出动态障碍物并自动避障，识别出饲料桶桶口位置后自动投料。

Description

一种自主导航的智能投料饲喂机器人及其投料饲喂方法

技术领域

本发明涉及农业养殖技术领域，特别是一种自主导航的智能投料饲喂机器人及其投料饲喂方法。

背景技术

目前，我国农业养殖机械化程度普遍较低，智能化养殖装备较为稀少且落后，对于非机构化的养殖场而言，智能化技术在非机构化的养殖场的应用几乎为空白；对于一些较大的养殖场而言，需要人工在养殖场大范围的奔走运料和投料，投料模式较为粗放，投料过程劳动强度大，作业环境较差。

为了解决传统养殖时劳动强度大的技术难题，使用投料机器人进行自动投料。现有技术中，投料机器人多采用基于超声波传感器的避障方案或基于双目视觉传感器的避障方案，但是由于超声波传感器发出的超声波属于机械波，其容易衰减和受到干扰，从而导致测量精度低，并且超声波传感器测量的数据少，不利于投料机器人避障，而双目视觉传感器采集的是图像信息，其数据处理模块的计算量和数据传输量也比较大，从而导致投料机器人相应功耗高，需要配备大功率电源和高性能计算机，实用成本高，同时双目视觉传感器受光线影响大，在光线暗的场景无法使用。

发明内容

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述和/或现有的农业养殖中存在的问题，提出了本发明。

因此，本发明的目的是提供一种自主导航的智能投料饲喂机器人及其投料饲喂方法，其通过搭载的激光雷达装置实时扫描投料机器人周围环境和监测投料机器人与障碍物之间的距离，通过搭载的工业电脑对测得数据进行处理，按照规划好的路径行驶且在行驶过程中能实时检测出动态障碍物并自动避障，识别出饲料桶桶口位置后自动投料。

为解决上述技术问题，本发明提供如下技术方案：一种自主导航的智能投料饲喂机器人，其包括，

行走组件，所述行走组件包括行走架，所述行走架前端的左右两侧连接有至少一个提供动力的行走轮，行走架后端的下部连接有用于转向及提供动力的转向轮；

投料组件，所述投料组件包括固定连接在行走架上的储料箱，所述储料箱上方连接有角度可调的投料臂，所述投料臂伸出行走架外的外周开有朝上设置的投料口，储料箱的前侧固定连接有激光雷达，投料臂伸出储料箱外的一侧连接有用于识别饲料桶桶口的摄像头。

作为本发明所述自主导航的智能投料饲喂机器人的一种优选方案，其中：所述行走架上连接有工业电脑、控制箱和信号箱，所述信号箱用于接收饲料控制板的信号，所述工业电脑用于接收信号箱发送过来的饲料信息和摄像头传递的图像并对其进行处理，并将接收及处理后的信号发送给控制箱，控制箱接收工业电脑处理后的信号并发送控制指令，控制投料组件的投料和行走组件的行走。

作为本发明所述投料饲喂方法的一种优选方案，其中：当信号箱接收到不足的信号后，自主导航前往饲料不足的饲料桶投料，实现自主导航的具体步骤为，

进行养殖场环境地图的构建；

激光雷达所采集的点云通过欧式聚类算法进行饲料桶的聚类识别；

饲料桶饲料不足时，通过饲料控制板向工业电脑发送饲料不足信号，工业电脑收到某个饲料桶的信号后，通过路径规划算法实现行走组件到饲料桶的行走；

摄像头识别饲料桶桶口位置，经工业电脑处理完摄像头传递的图像后，将识别出的桶口位置数据传至控制箱，控制箱接收到桶口的位置信息，发送控制指令使投料臂转动至桶口位置后进行饲料输送。

作为本发明所述投料饲喂方法的一种优选方案，其中：聚类识别结束后，计算行走组件行驶到饲料桶的最佳投料点，确定最佳投料点的具体步骤如下，

经过投影得到一个圆形区域，求出行走组件与圆形区域圆心的距离；

计算行走组件到圆形区域的两个切点，行走组件当前的位置P到饲料桶的中心位置Q的单位向量为，

使R向量顺时针旋转α，则有切线PQ方向的单位向量ω，由以下公式得，

ω＝(cosβ,sinβ)＝[(cosθcosα-sinθsinα),(cosθsinα+sinθcosα)]＝(x,y)；

x₁＝(x+x₀)L

y₁＝(y+y₀)L；

P的坐标为(x₀,y₀)，C坐标为(a,b)，Q为切点，Q的坐标为(x₁,y₁)，L为切线，c为P点到C点的长度，α为旋转角度，θ为PC与x轴的夹角，β为PQ与 x轴夹角。

作为本发明所述投料饲喂方法的一种优选方案，其中：所述路径规划算法包括以下步骤，

根据已建地图进行全局路径规划；

通过模拟退火算法实现多目标路径规划；

在行驶过程中通过动态避障算法进行实时避障。

使用自主导航的智能投料饲喂机器人进行投料的方法，其中：m个饲料桶响应饲料不足的信号后，所述全局路径规划的方法包括以下步骤，

确定初始温度T_emax、终止温度T_emin及降温速度r(0<r<1)，确定起始点S和m个目标点，组成(S,T₁,T₂,…,T_m,S)；

将(S,T₁,T₂,…,T_m,S)组成m个排列组合的解空间I₀＝{(S,T₁,T₂,…,T_m,S)、 (S,T₁,T₂,...,T_m,T_m-1,S)…}；

随机生成一个初始解I_i(I_i∈I)通过二变换方法产生的解，随机交换路径解 I_i＝(S,T₁,...,T_p-1,T_p,T_p+1,...,T_q-1,T_q,T_q+1,T_m,S)中的两个元素T_p，T_q的新路径解为I_i’＝ (S,T₁,...,T_p-1,T_q,T_p+1,...,T_q-1,T_p,T_q+1,T_m,S)；

将路径的长度设置为目标函数，由以下两公式确定变换后路径长度的变换值，

Δf＝f(I_i')-f(I_i)；

由Metropolis接受准则式计算出路径接收概率p，选出新一代解；如果Δf<0, 表示变换之后的路径长度小于变换前的解，则新解为I_i’；反之以概率p接受新解，Metropolis接受准则式为：

更新温度T_e，由以下降温函数进行降温

Te'＝Te×r；

判断降温后的温度T_e，是否到达温度T_emin，若条件满足，输出最优路径解，算法结束：否则返回随机生产初始解所在步骤；

其中，d(c_i,c_i+1)为，ΔL为，r为。

作为本发明所述投料饲喂方法的一种优选方案，其中：实现多目标路径规划具体步骤如下，

假设通过模拟退火算法所计算出的最佳路径解为I₀(I₀∈I)，I₀＝{(S,T₁,T₂，…，T_n,S)},最佳路径解中相邻目标点间路径规划算法通过全局路径规划算法，全局路径规划算法通过代价函数进行全局最优路径选取，代价函数的模型如下：

F(n)＝G(n)+exp[H(n)]*[H(n)+H(p)]；

路径节点搜索完毕后，行走路线节点集合中，删去中间节点，只保留起始点、拐点和目标点，将处理好的路径按照较小的步长进行分割，得到比之前更多的路径点；

假设去除中间节点后得到的路径节点集合为A＝{Q，a₁，a₂，....a_n’,T}，Q 和T为模拟退火算法求出的最佳路径接中相邻的两个目标点，a_n’为拐点，S 从a₁开始依次连接各个节点，如果与a_m’的连线中间无障碍物时，不予处理，当与a_m’的连线中间有障碍物时，将a_m’-1作为新的路径节点保存下来作为新的路径节点；

从a_m’-1开始重复上述步骤，得到新的路径点，最后从T开始从前往后再重复以上步骤更新路径点，得出新的路径点为B＝{Q，b₁,b₂,....b_k,T}；

其中，F(n)为从起始状态经由状态n到目标状态的估价函数；G(n)为在状态空间从起始状态到状态n的实际路径距离；H(n)为从状态n到目标状态规划的最小估计距离；H(p)为当前节点的父节点到目标节点的距离；exp[H(n)]为 [H(n)+H(p)]的权重。

作为本发明所述投料饲喂方法的一种优选方案，其中：所述动态避障算法包括以下步骤，

假设行走组件的运动轨迹是若干段圆弧组成；

通过选取速度范围内的多对线速度v_t和角速度ω_t，实现规划出下一段时间Δt的圆弧轨迹；

通过改进后的评价函数进行最优路径挑选。

作为本发明所述投料饲喂方法的一种优选方案，其中：规划圆弧轨迹的具体步骤如下，

建立行走组件在Δt时间内的运动模型：

运动模型建完后，根据速度推算出多组运动轨迹

通过改进的采样速度评价函数进行最优路径的选取，改进后的评价函数如下式所示，

G(v,w)＝α*Dheading(v,w)+β*dist_sta(v,w)+γ*vel(v,w)+δ*dist_dyna(v,w)；

方向角的偏差Dheading(v,ω)，通过以下公式进行评价

Dheading(v,ω)＝180°-|Φ-η|；

对于速度函数vel(v,ω)，速度越快评价函数得分越高，把当前线速度绝对值当作速度评价函数，如下公式所示

vel(v,ω)＝|v|；

将计算出来的评价函数的每一个项做归一化处理后进行相加，挑选评分最高的一条模拟轨迹；

其中，Dheading(v,ω)为距离投料机器人最近的全局路径节点的评价函数，是投料机器人模拟轨迹位置与最近全局路径节点的方向角偏差；dist_sta(v,ω)为速度对应全局路径上最近静态障碍物距离投料机器人的评价函数；vel(v,ω)为速度大小的评价函数；dist_dyna(v,ω)为速度对应轨迹上离局部动态未知障碍物最近距离的评价函数；α、β、γ、δ分别对应其评价函数的权重，v为机器人行走的线速度，w为机器人行走的角速度，设定J为全局路径，K为局部路径，Z 为最近的全局路径节点，Ф为机器人到Z点方向与水平方向的夹角，η为当前行驶方向与水平方向的夹角；对于距离函数dist_sta(v,ω)和dist_dyna(v,ω)，距离障碍物越远评分越高，如果在预测轨迹上没有障碍物，则将dist_dyna(v,ω)设置为一个常数。

作为本发明所述投料饲喂方法的一种优选方案，其中：推算多组模拟轨迹的具体步骤如下，

行走组件速度存在最大速度和最小速度的约束，如下式所示，

V₁＝{(v,w)|v∈[v_min,v_max]∩w∈[w_min,w_max]}；

紧急刹车的安全距离约束，如下式所示，

速度选取范围，如下式所示，

V＝V₁∩V₂；

其中，v_min、v_max分别是投料机器人线速度最小值、最大值，ω_min、ω_max分别是投料机器人角速度最小值、最大值；dist(x_t,y_t)表示投料机器人位姿为(x_t,y_t|,θ_t) 时，对应轨迹上离障碍物最近的距离，

分别为线速度和角速度的最大减速度，因此，在速度采样范围V下，可生成多组不同速度组(v,ω)的模拟轨迹。

本发明的有益效果：本发明通过搭载的激光雷达实时扫描投料机器人周围环境和监测投料机器人与障碍物之间的距离，机器人按照规划好的路径行驶且在行驶过程中能实时检测出动态障碍物并自动避障，通过改进的聚类方法提高机器人行走时实时检测的准确率，根据计算出的最佳投料点控制投料角度，减小投料臂转动的总角度。

附图说明

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

图1为本发明的立体结构图一。

图2为图1中A处的局部放大图。

图3为本发明的立体结构图二。

图4为图3中B处的局部放大图。

图5为本发明的立体结构图三。

图6为图5中C处的局部放大图。

图7为本发明的立体结构图四。

图8为图7中D处的局部放大图。

图9为投料机器人工作的流程图。

图10为控制箱内的主要电路原理图。

图11为投料臂的控制电路图。

图12为将饲料桶膨胀前的示意图。

图13为将饲料桶膨胀后的示意图。

图14为求最佳投料点的示意图。

图15为局部路径规划评价函数Dheading(v,ω)项计算示意图。

图16为投料机器人行走时动态障碍物聚类结果示意图。

图17为改进的分区聚类流程图。

其中，100行走组件，101行走架，102行走轮，103行走电机，104转向轮，105转向支架，106连接盘，107转向从动齿轮，108转向主动齿轮，109 转向电机，200投料组件，201激光雷达，202储料箱，203摄像头，204投料臂，204a投料口，205上输送电机，206传料罩，207连接件，208角度调节从动齿轮，209工业电脑，210控制箱，211信号箱，212饲料输送管，212a进料口，213饲料输送螺旋片，214竖直输送轴，215下输送螺旋叶片，216下输送轴，217上输送螺旋叶片，218上输送轴，219饲料输送主动链轮，220饲料输送电机，221饲料输送从动链轮，222下输送电机，223角度调节电机，224角度调节主动齿轮，225传料罩，300饲养物鹅，400饲料桶。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

实施例1

参照图1～图9，为本发明的第一个实施例，该实施例提供了一种自主导航的智能投料饲喂机器人及其投料饲喂方法，其能够完成颗粒从不同倾斜角度和高度位置下滑或滚动的接触参数标定实验。

一种自主导航的智能投料饲喂机器人，包括行走组件，所述行走组件包括行走架，所述行走架前端的左右两侧连接有至少一个提供动力的行走轮，行走架后端的下部连接有用于转向及提供动力的转向轮；

投料组件，所述投料组件包括固定连接在行走架上的储料箱，所述储料箱上方连接有角度可调的投料臂，所述投料臂伸出行走架外的外周开有朝上设置的投料口，储料箱的前侧固定连接有激光雷达，投料臂伸出储料箱外的一侧连接有用于识别饲料桶桶口的摄像头。

进一步的，所述行走架上连接有工业电脑、控制箱和信号箱，所述信号箱用于接收饲料控制板的信号，所述工业电脑用于接收信号箱发送过来的饲料信息和摄像头传递的图像并对其进行处理，并将接收及处理后的信号发送给控制箱，控制箱接收工业电脑处理后的信号并发送控制指令，控制投料组件的投料和行走组件的行走。

进一步的，行走组件还包括可转动地连接在行走架后端的转向从动齿轮，转向从动齿轮的下侧固定连接有连接盘，连接盘上固定连接有转向支架，转向支架外侧固定连接有行走电机，行走电机与转向轮连接，转向支架外的连接盘上固定连接有转向电机，转向电机上连接有转向轴，转向轴上连接有与转向主动齿轮啮合的转向从动齿轮。

进一步的，投料组件还包括固定在行走架前端的下输送电机，储料箱的下部可转动地连接有下输送轴，下输送电机与下输送轴传动连接，下输送轴上排布有若干下输送螺旋叶片，储料箱上固定连接有竖直设置的饲料输送管，饲料输送管上可转动地连接有竖直输送轴，行走架的下部固定连接有饲料输送电机，饲料输送电机上连接有饲料输送主动链轮，竖直输送轴向下伸出饲料输送管外的一端连接有饲料输送从动链轮，饲料输送主动链轮经链条与饲料输送从动链轮连接(附图中链条未画出，此为现有技术)，竖直输送轴上排布有若干饲料输送螺旋叶片，在储料箱内的饲料输送管上开有相对下输送轴设置的进料口，进料口的下缘不高于储料箱的内壁底部所在高度，饲料输送管的上部可转动地连接有角度调节从动齿轮，储料箱的上侧固定连接有支撑支架，支撑支架上固定连接有角度调节电机，角度调节电机上连接有与角度调节从动齿轮啮合的角度调节主动齿轮，角度调节从动齿轮上侧固定连接有连接件，连接件的上侧固定连接有转动套，竖直输送轴的上端可转动地连接在转动套的上端，转动套上开有传送口，转动套外侧固定有传料罩，传料罩的一端经传送口与转动套的内腔连通，传料罩远离转动套的一端固定有投料臂，传料罩的另一端与投料臂的内腔连通，投料臂的一端固定连接有上输送电机，摄像头固定连接在投料臂的另一端，投料臂上可转动地连接有上输送轴，上输送电机与上输送轴连接，上输送轴上排布有若干上输送螺旋叶片。

本发明中搭载着多线激光雷达，通过激光雷达采集养殖场的环境信息和机器人与障碍物之间的距离信息，将采集的数据通过串口通信实时传至工业电脑，工业电脑通过建图和导航方法进行三维环境地图构建，在工业电脑接收到饲料桶发出的饲料低于阈值信号后，控制行走组件进入导航模式，先根据已建地图进行全局路径规划，结合模拟退火算法实现多目标路径规划，在行驶过程中通过动态避障算法实现实时避障，计算出最佳投料点，行驶至饲料桶所在位置后，通过摄像头识别饲料桶桶口的位置，控制箱控制角度调节电机动作，使投料臂转动，投料口转动至桶口位置后，下输送电机和上输送电机动作，开始投料，投料完毕后，饲料控制板发送投料结束信号给信号箱，信号箱将投料结束信号发送给工业电脑，工业电脑将处理后的投料完毕信号传送给控制箱，控制箱控制下输送电机和上输送电机停止动作；准备下一次的投料；本发明通过行走组件完成自动行走，行走架上设置投料组件，实现自动投料，适用范围广。

实施例2

参照图10～图15，为本发明的第二个实施例，与第一个实施例的不同之处在于，其能实现投料机器人的自主行走及投料。

控制箱内设有转向电机驱动器、步进电机驱动器和前进、后退电机驱动器， PLC控制器和IMU惯性测量单元，转向电机驱动器控制转向电机的动作，步进电机驱动器控制角度调节电机的动作，前进、后退电机驱动器控制行走电机的动作，PLC控制器向各个电机驱动器发送控制指令。

使用投料机器人进行投料的方法，包括以下步骤：

(1)进行养殖场环境地图的构建，建图过程中采用惯性测量单元采集的航向角代替里程计的航向角，减少里程计因车轮打滑而产生的航向角误差，使得所建地图更加精确，同时在对于饲料桶的点云先通过体素格下降法对分割后的点云进行下采样，即将点云分为数个大小相等的体素栅格(边长为e的正方体)，计算栅格内所有点云重心，用重心代替整个栅格内的点云，再通过欧式聚类算法进行饲料桶的聚类；将地图中的障碍物和饲料桶边缘向外进行膨胀，防止投料机器人行进时发生碰撞；

(2)激光雷达所采集的点云通过欧式聚类算法进行饲料桶的聚类识别；

(3)饲料桶饲料不足时，通过饲料控制板向工业电脑发送饲料不足信号，工业电脑收到某个饲料桶的信号后，通过路径规划算法实现行走组件到饲料桶的行走；

(4)摄像头识别饲料桶桶口位置，经工业电脑处理完摄像头传递的图像后，将识别出的桶口位置数据传至控制箱，控制箱接收到桶口的位置信息，发送控制指令使投料臂转动至桶口位置后进行饲料输送。

进一步的，聚类识别结束后，计算行走组件行驶到饲料桶的最佳投料点，确定最佳投料点的具体步骤如下，

经过投影得到一个圆形区域，求出行走组件与圆形区域圆心的距离；

计算行走组件到圆形区域的两个切点，行走组件当前的位置P到饲料桶的中心位置Q的单位向量为，

使R向量顺时针旋转α，则有切线PQ方向的单位向量ω，由以下公式得，

ω＝(cosβ,sinβ)＝[(cosθcosα-sinθsinα),(cosθsinα+sinθcosα)]＝(x,y)；

x₁＝(x+x₀)L

y₁＝(y+y₀)L；

P的坐标为(x₀,y₀)，C坐标为(a,b)，Q为切点，Q的坐标为(x₁,y₁)，L为切线，c为P点到C点的长度，α为旋转角度，θ为PC与x轴的夹角，β为PQ与 x轴夹角(以养殖场的西南角为坐标原点，X轴方向为正东方向，Y轴方向为正北方向)。

进一步的，所述路径规划算法包括以下步骤，

(301)根据已建地图进行全局路径规划；

(302)通过模拟退火算法实现多目标路径规划；

(303)在行驶过程中通过动态避障算法进行实时避障。

进一步的，m个饲料桶响应饲料不足的信号后，所述全局路径规划的方法包括以下步骤，

(301a)确定初始温度T_emax、终止温度T_emin及降温速度r(0<r<1)，确定起始点S和m个目标点，组成(S,T₁,T₂,…,T_m,S)；

(301b)将(S,T₁,T₂,…,T_m,S)组成m个排列组合的解空间 I₀＝{(S,T₁,T₂,…,T_m,S)、(S,T₁,T₂,...,T_m,T_m-1,S)…}；

(301c)随机生成一个初始解I_i(I_i∈I)通过二变换方法产生的解，随机交换路径解I_i＝(S,T₁,...,T_p-1,T_p,T_p+1,...,T_q-1,T_q,T_q+1,T_m,S)中的两个元素T_p，T_q的新路径解为I_i’＝(S,T₁,...,T_p-1,T_q,T_p+1,...,T_q-1,T_p,T_q+1,T_m,S)；

(301d)将路径的长度设置为目标函数，由以下两公式确定变换后路径长度的变换值，

Δf＝f(I_i')-f(I_i)；

由Metropolis接受准则式计算出路径接收概率p，选出新一代解；如果Δf<0, 表示变换之后的路径长度小于变换前的解，则新解为I_i’；反之以概率p接受新解，Metropolis接受准则式为：

(301e)更新温度T_e’，由以下降温函数进行降温

Te'＝Te×r；

判断降温后的温度T_e’，是否到达温度T_emin，若条件满足，输出最优路径解，算法结束：否则返回随机生产初始解所在步骤；

其中，f(S,T₁,T₂,...,T_n,S)为路径长度目标函数，d(c_i,c_i+1)为c_i与c_i+1两点间的距离。

进一步的，实现多目标路径规划具体步骤如下，

假设通过模拟退火算法所计算出的最佳路径解为I₀(I₀∈I)，I₀＝{(S,T₁,T₂，…，T_n,S)},最佳路径解中相邻目标点间路径规划算法通过全局路径规划算法，全局路径规划算法通过代价函数进行全局最优路径选取，代价函数的模型如下：

F(n)＝G(n)+exp[H(n)]*[H(n)+H(p)]；

路径节点搜索完毕后，行走路线节点集合中，删去中间节点，只保留起始点、拐点和目标点，将处理好的路径按照较小的步长进行分割，得到比之前更多的路径点；

假设去除中间节点后得到的路径节点集合为A＝{Q，a₁，a₂，....a_n’,T}，Q 和T为模拟退火算法求出的最佳路径接中相邻的两个目标点，a_n’为拐点，S 从a₁开始依次连接各个节点，如果与a_m’的连线中间无障碍物时，不予处理，当与a_m’的连线中间有障碍物时，将a_m’-1作为新的路径节点保存下来作为新的路径节点；

从a_m’-1开始重复上述步骤，得到新的路径点，最后从T开始从前往后再重复以上步骤更新路径点，得出新的路径点为B＝{Q，b₁,b₂,....b_k,T}，由此可较好的缩短路径长度并且减小投料机器人移动时总的转动角度；

其中，F(n)为从起始状态经由状态n到目标状态的估价函数；G(n)为在状态空间从起始状态到状态n的实际路径距离；H(n)为从状态n到目标状态规划的最小估计距离；H(p)为当前节点的父节点到目标节点的距离；exp[H(n)]为 [H(n)+H(p)]的权重，由公式可以看出，当H(n)较大时，权重也比较大，此时路径节点迅速的朝目标节点靠近；当H(n)较小时，权重也变小，靠近目标节点时权重接近1，能够保证到达目标点，大大的减少了拓展点的搜索。

进一步的，所述动态避障算法包括以下步骤，

假设行走组件的运动轨迹是若干段圆弧组成；

通过选取速度范围内的多对线速度v_t和角速度ω_t，实现规划出下一段时间Δt的圆弧轨迹；

通过改进后的评价函数进行最优路径挑选。

进一步的，规划圆弧轨迹的具体步骤如下，

建立行走组件在Δt时间内的运动模型：

运动模型建完后，根据速度推算出多组运动轨迹，具体为，

行走组件速度存在最大速度和最小速度的约束，如下式所示，

V₁＝{(v,w)|v∈[v_min,v_max]∩w∈[w_min,w_max]}；

紧急刹车的安全距离约束，如下式所示，

速度选取范围，如下式所示，

V＝V₁∩V₂；

其中，v_min、v_max分别是投料机器人线速度最小值、最大值，ω_min、ω_max分别是投料机器人角速度最小值、最大值；dist(x_t,y_t)表示投料机器人位姿为(x_t,y_t|,θ_t) 时，对应轨迹上离障碍物最近的距离，

分别为线速度和角速度的最大减速度，因此，在速度采样范围V下，可生成多组不同速度组(v,ω)的模拟轨迹；

通过改进的采样速度评价函数进行最优路径的选取，改进后的评价函数如下式所示，

G(v,w)＝α*Dheading(v,w)+β*dist_sta(v,w)+γ*vel(v,w)+δ*dist_dyna(v,w)；

方向角的偏差Dheading(v,ω)，通过以下公式进行评价

Dheading(v,ω)＝180°-|Φ-η|；

对于速度函数vel(v,ω)，速度越快评价函数得分越高，把当前线速度绝对值当作速度评价函数，如下公式所示

vel(v,ω)＝|v|；

将计算出来的评价函数的每一个项做归一化处理后进行相加，挑选评分最高的一条模拟轨迹；

其中，Dheading(v,ω)为距离投料机器人最近的全局路径节点的评价函数，是投料机器人模拟轨迹位置与最近全局路径节点的方向角偏差；dist_sta(v,ω)为速度对应全局路径上最近静态障碍物距离投料机器人的评价函数；vel(v,ω)为速度大小的评价函数；dist_dyna(v,ω)为速度对应轨迹上离局部动态未知障碍物最近距离的评价函数；α、β、γ、δ分别对应其评价函数的权重，v为机器人行走的线速度，w为机器人行走的角速度，新引入的dist_dyna(v,ω)可以减少已知静态障碍物对投料机器人路径规划时的干扰，可实现实时动态避障，防止局部最优路径的生成；设定J为全局路径，K为局部路径，Z为最近的全局路径节点，Ф为机器人到Z点方向与水平方向的夹角，η为当前行驶方向与水平方向的夹角；对于距离函数dist_sta(v,ω)和dist_dyna(v,ω)，距离障碍物越远评分越高，如果在预测轨迹上没有障碍物，则将dist_dyna(v,ω)设置为一个常数。

进一步的，机器人在行驶过程中需要对养殖场所出现的动态障碍物如饲养物鹅进行识别，将点云按照不同半径区域进行分区，不同区域内的点云按照不同的阈值进行聚类，半径越大的区域对应的聚类阈值越大，解决偏远距离点云稀疏难以聚类的问题，提高机器人行走时实时检测的准确率，具体步骤如下：

S1：将实时采集的点云进行预处理，采用直通滤波获得兴趣区域，直通滤波可以指定坐标范围进行裁剪点云区域，保留兴趣范围内的点，兴趣区域范围为投料机器人行驶的正前方0度到180度，以确定实时检测的点云区域范围；通过统计滤波器去除明显离群点,统计滤波对每一个领域进行统计分析，计算它到所有临近点的平均距离，假设得到的结果是一个高斯分布，其形状是由均值和标准差决定，那么平均距离在标准范围之外的点，将会被当做离群点剔除；最后采用随机采样一致性算法(RANSAC算法)进行分割地面，即随机选取至少三个不共线的点，根据所设定的阈值计算点云数据集中其它点是否满足平面的数学模型，将满足的点设为局内点，记录模型局内点的数量；重复多次迭代，舍弃局内点小于阈值的模型，选用更好的模型替代现存模型，满足迭代退出条件后退出循环，得到最合理的解，从而分割出地面；

S2：完成点云的预处理之后，将采集的点云数据按照不同的半径进行区域的划分，可将点云分为以r₁、2r₁、3r₁为半径的同心圆点云区域，不同半径的点云区域设置不同的聚类阈值，分别对应阈值D₁、2D₁、3D₁，对于半径大于 3r₁的区域，聚类阈值取4D₁；

S3：依据所有点云在平面XOY的分布距离，对不同半径r₁对点云进行筛选并且进行分区，将筛选的不同区域的点云保留在点云空间G_f(f＝1,2,…)内；

S4：点云空间G_f＝{G_k}^N(k＝1,2,…)，G_k为点云空间里的点，G_k＝[x_k,y_k,z_k]^T，使用K-D树作为搜索机制，K-D树是一种空间划分树，就是把整个空间划分为特定的几个部分，然后在特定空间的部分内进行相关搜索操作；对于点云空间G_f里的点G_k，搜索距离点G_k最近的N个点，计算出这N个点云到G_k距离 D，将距离D小于D_s的所有点保留在点云类空间F内，并且记录所加入的点云数量；

S5：从类空间F中随机挑选点V_k，进行步骤四中相邻点距离进行计算，并判断与阈值D_s大小，保留满足条件的点于类空间F中；

S6：重复步骤S3和S4，根据所设定的聚类点云的最小数量的阈值N_min和最大数量阈值N_max进行聚类点云的判断，如果在这个范围内，表示障碍物类 O_b(b＝1,2,…)已经形成，否则结束循环；

S7：计算障碍物类O_b内点云在坐标系内X轴、Y轴、Z轴上点云坐标的最大值，在最大坐标外随机找点O_b+1，重复循环步骤S4到S6，形成新障碍物 O_b+1，直到障碍物类O_b数量不再增加，否则结束点云数据空间G_f聚类；

S8：引入另一分区点云空间G_f+1，进行步骤S4到步骤S7，完成该点云分区的聚类，循环直至所有分区点云聚类完成；

S9：通过计算障碍物三维尺寸和空间位置，采用长方体框进行标记，计算公式如下，

式中，n为障碍物类点云总数；x_a、y_a、z_a分别为障碍物点云X轴、Y轴、 Z轴的平均坐标，即障碍物的中心位置；x_max、y_max、z_max分别障碍物点云X轴、 Y轴、Z轴的最大坐标值；x_min、y_min、z_min分别障碍物点云X轴、Y轴、Z轴的最小坐标值；l、w、h分别是障碍物标记长方体的长、宽、高，由此可以实时框出养殖场的动态障碍物鹅，并且可以知道其三维尺寸信息以及中心位置；

S10：对聚类完成的障碍物设立高度阈值h_s,将阈值内的障碍物的Z轴最大坐标值z_max比较，Z轴最大坐标值z_max小于阈值h_s的聚类点云当做养殖场动态障碍物鹅，Z轴最大坐标值z_max小于阈值h_s的聚类点云不予考虑；在投料机器人行驶过程中，对所聚类的动态障碍物进行局部路径规划，对于其它聚类物如饲料桶之类，则不启用局部路径规划，以保证投料机器人顺利到达投料点。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种自主导航的智能投料饲喂机器人，其特征在于：其包括，

行走组件，所述行走组件包括行走架，所述行走架前端的左右两侧连接有至少一个提供动力的行走轮，行走架后端的下部连接有用于转向及提供动力的转向轮；

投料组件，所述投料组件包括固定连接在行走架上的储料箱，所述储料箱上方连接有角度可调的投料臂，所述投料臂伸出行走架外的外周开有朝上设置的投料口，储料箱的前侧固定连接有激光雷达，投料臂伸出储料箱外的一侧连接有用于识别饲料桶桶口的摄像头。

2.如权利要求1所述的自主导航的智能投料饲喂机器人，其特征在于：所述行走架上连接有工业电脑、控制箱和信号箱，所述信号箱用于接收饲料控制板的信号，所述工业电脑用于接收信号箱发送过来的饲料信息和摄像头传递的图像并对其进行处理，并将接收及处理后的信号发送给控制箱，控制箱接收工业电脑处理后的信号并发送控制指令，控制投料组件的投料和行走组件的行走。

3.使用权利要求1所述的自主导航的智能投料饲喂机器人进行投料的方法，其特征在于：当信号箱接收到不足的信号后，自主导航前往饲料不足的饲料桶投料，实现自主导航的具体步骤为，

进行养殖场环境地图的构建；

激光雷达所采集的点云通过欧式聚类算法进行饲料桶的聚类识别；

饲料桶饲料不足时，通过饲料控制板向工业电脑发送饲料不足信号，工业电脑收到某个饲料桶的信号后，通过路径规划算法实现行走组件到饲料桶的行走；

摄像头识别饲料桶桶口位置，经工业电脑处理完摄像头传递的图像后，将识别出的桶口位置数据传至控制箱，控制箱接收到桶口的位置信息，发送控制指令使投料臂转动至桶口位置后进行饲料输送。

4.如权利要求3所述的投料饲喂方法，其特征在于：聚类识别结束后，计算行走组件行驶到饲料桶的最佳投料点，确定最佳投料点的具体步骤如下，

经过投影得到一个圆形区域，求出行走组件与圆形区域圆心的距离；

计算行走组件到圆形区域的两个切点，行走组件当前的位置P到饲料桶的中心位置Q的单位向量为，

使R向量顺时针旋转α，则有切线PQ方向的单位向量ω，由以下公式得，

ω＝(cosβ,sinβ)＝[(cosθcosα-sinθsinα),(cosθsinα+sinθcosα)]＝(x,y)；

x₁＝(x+x₀)L

y₁＝(y+y₀)L；

P的坐标为(x₀,y₀)，C坐标为(a,b)，Q为切点，Q的坐标为(x₁,y₁)，L为切线，c为P点到C点的长度，α为旋转角度，θ为PC与x轴的夹角，β为PQ与x轴夹角。

5.如权利要求3所述的投料饲喂方法，其特征在于：所述路径规划算法包括以下步骤，

根据已建地图进行全局路径规划；

通过模拟退火算法实现多目标路径规划；

在行驶过程中通过动态避障算法进行实时避障。

6.如权利要求5所述的投料饲喂方法，其特征在于：当有m个饲料桶响应饲料不足的信号时，所述全局路径规划的方法包括以下步骤，

确定初始温度T_emax、终止温度T_emin及降温速度r(0<r<1)，确定起始点S和m个目标点，组成(S,T₁,T₂,…,T_m,S)；

将(S,T₁,T₂,…,T_m,S)组成m个排列组合的解空间I₀＝{(S,T₁,T₂,…,T_m,S)、(S,T₁,T₂,...,T_m,T_m-1,S)…}；

随机生成一个初始解I_i(I_i∈I)通过二变换方法产生的解，随机交换路径解I_i＝(S,T₁,...,T_p-1,T_p,T_p+1,...,T_q-1,T_q,T_q+1,T_m,S)中的两个元素T_p，T_q的新路径解为I_i’＝(S,T₁,...,T_p-1,T_q,T_p+1,...,T_q-1,T_p,T_q+1,T_m,S)；

将路径的长度设置为目标函数，由以下两公式确定变换后路径长度的变换值，

Δf＝f(I_i')-f(I_i)；

由Metropolis接受准则式计算出路径接收概率p，选出新一代解；如果Δf<0,表示变换之后的路径长度小于变换前的解，则新解为I_i’；反之以概率p接受新解，Metropolis接受准则式为：

更新温度T_e，由以下降温函数进行降温

Te'＝Te×r；

判断降温后的温度T_e，是否到达温度T_emin，若条件满足，输出最优路径解，算法结束：否则返回随机生产初始解所在步骤；

其中，f(S,T₁,T₂,...,T_n,S)为路径长度目标函数，d(c_i,c_i+1)为c_i与c_i+1两点间的距离。

7.如权利要求5所述的投料饲喂方法，其特征在于：实现多目标路径规划具体步骤如下，

假设通过模拟退火算法所计算出的最佳路径解为I₀(I₀∈I)，I₀＝{(S,T₁,T₂，…，T_n,S)},最佳路径解中相邻目标点间路径规划算法通过全局路径规划算法，全局路径规划算法通过代价函数进行全局最优路径选取，代价函数的模型如下：

F(n)＝G(n)+exp[H(n)]*[H(n)+H(p)]；

路径节点搜索完毕后，行走路线节点集合中，删去中间节点，只保留起始点、拐点和目标点，将处理好的路径按照较小的步长进行分割，得到比之前更多的路径点；

假设去除中间节点后得到的路径节点集合为A＝{Q，a₁，a₂，....a_n’,T}，Q和T为模拟退火算法求出的最佳路径接中相邻的两个目标点，a_n’为拐点，S从a₁开始依次连接各个节点，如果与a_m’的连线中间无障碍物时，不予处理，当与a_m’的连线中间有障碍物时，将a_m’-1作为新的路径节点保存下来作为新的路径节点；

从a_m’-1开始重复上述步骤，得到新的路径点，最后从T开始从前往后再重复以上步骤更新路径点，得出新的路径点为B＝{Q，b₁,b₂,....b_k,T}；

其中，F(n)为从起始状态经由状态n到目标状态的估价函数；G(n)为在状态空间从起始状态到状态n的实际路径距离；H(n)为从状态n到目标状态规划的最小估计距离；H(p)为当前节点的父节点到目标节点的距离；exp[H(n)]为[H(n)+H(p)]的权重。

8.如权利要求1～4任一项所述的投料饲喂方法，其特征在于：所述动态避障算法包括以下步骤，

假设行走组件的运动轨迹是若干段圆弧组成；

通过选取速度范围内的多对线速度vt和角速度ωt，实现规划出下一段时间Δt的圆弧轨迹；

通过改进后的评价函数进行最优路径挑选。

9.如权利要求8所述的投料饲喂方法，其特征在于：规划圆弧轨迹的具体步骤如下，

建立行走组件在Δt时间内的运动模型：

运动模型建完后，根据速度推算出多组运动轨迹

通过改进的采样速度评价函数进行最优路径的选取，改进后的评价函数如下式所示，

G(v,w)＝α*Dheading(v,w)+β*dist_sta(v,w)+γ*vel(v,w)+δ*dist_dyna(v,w)；

方向角的偏差Dheading(v,ω)，通过以下公式进行评价

Dheading(v,ω)＝180°-|Φ-η|；

对于速度函数vel(v,ω)，速度越快评价函数得分越高，把当前线速度绝对值当作速度评价函数，如下公式所示

vel(v,ω)＝|v|；

将计算出来的评价函数的每一个项做归一化处理后进行相加，挑选评分最高的一条模拟轨迹；

其中，Dheading(v,ω)为距离投料机器人最近的全局路径节点的评价函数，是投料机器人模拟轨迹位置与最近全局路径节点的方向角偏差；dist_sta(v,ω)为速度对应全局路径上最近静态障碍物距离投料机器人的评价函数；vel(v,ω)为速度大小的评价函数；dist_dyna(v,ω)为速度对应轨迹上离局部动态未知障碍物最近距离的评价函数；α、β、γ、δ分别对应其评价函数的权重，v为机器人行走的线速度，w为机器人行走的角速度，设定J为全局路径，K为局部路径，Z为最近的全局路径节点，Ф为机器人到Z点方向与水平方向的夹角，η为当前行驶方向与水平方向的夹角；对于距离函数dist_sta(v,ω)和dist_dyna(v,ω)，距离障碍物越远评分越高，如果在预测轨迹上没有障碍物，则将dist_dyna(v,ω)设置为一个常数。

10.如权利要求8所述的投料饲喂方法，其特征在于：推算多组模拟轨迹的具体步骤如下，

行走组件速度存在最大速度和最小速度的约束，如下式所示，

V₁＝{(v,w)|v∈[v_min,v_max]∩w∈[w_min,w_max]}；

紧急刹车的安全距离约束，如下式所示，

速度选取范围，如下式所示，

V＝V₁∩V₂；

其中，v_min、v_max分别是投料机器人线速度最小值、最大值，ω_min、ω_max分别是投料机器人角速度最小值、最大值；dist(x_t,y_t)表示投料机器人位姿为(x_t,y_t|,θ_t)时，对应轨迹上离障碍物最近的距离，

分别为线速度的最大减速度和角速度的最大减速度，因此，在速度采样范围V下，可生成多组不同速度组(v,ω)的模拟轨迹。

Images