侯云涛，尧李慧，蔡晓华，王 强

(1.黑龙江省农业机械工程科学研究院，哈尔滨 150081；2.黑龙江八一农垦大学，黑龙江 大庆 163319；3.中国农业机械化科学研究院呼和浩特分院，呼和浩特 010010)



自动清粪机器人路径规划方法的研究与实现

侯云涛1，尧李慧2，蔡晓华1，王 强3

(1.黑龙江省农业机械工程科学研究院，哈尔滨 150081；2.黑龙江八一农垦大学，黑龙江 大庆 163319；3.中国农业机械化科学研究院呼和浩特分院，呼和浩特 010010)

自动清粪机器人的工作环境复杂，不仅有静态障碍物(如牛栏)，且动态障碍物众多(如人和牲畜等)。为此，提出了一种基于行为方法的路径规划法。该方法非常适合于动态环境下机器人的运动规划，其原理简单、计算快捷、容易实现。根据该算法，确立了一套完整的软件系统和设计方案，并在此基础上制造出了样机。经过样机在奶牛养殖场的工作和测试，验证了该方法的实时性、准确性和可操作性。

自动清粪机器人；行为方法；路径规划算法；动态环境

0 引言

随着养殖业中大规模化奶牛养殖场的日益兴起，如何有效地对奶牛粪便进行收集及处理，日益成为一个严峻棘手的问题。如不能有效处理奶牛粪便，不仅影响奶牛的出奶量和品质，还会给奶牛及养殖场附近居民的健康带来极大威胁。清理奶牛粪污的工作在规模化牛场用人力完成是不可能的，因此迫切需要开发一种自走式智能化牛舍清粪机器人，能够自动化、全天候地工作，并且行走速度适中、噪音极小，对牛群的日常饲养和奶牛起居不造成任何影响和伤害，替代传统繁重的人工清洁工作。

由于清粪机器人工作环境复杂，障碍物众多(如牛栏、奶牛等)，且许多奶牛不时在牛舍中走动，对于机器人来说，无疑于众多的移动障碍物。因此，如何设计出最优的行走路径，有效规避牛舍中的障碍物，成为清粪机器人控制算法的关键问题。

路径规划问题按照机器人的工作条件，可以分为两种工作模型：一是基于静止障碍物的规化模型,养牛场的全部地面障碍信息都是预知的;二是基于传感器的路径规划算法,养牛场的地面障碍信息是全部未知或部分未知的[1]。清粪机器人的工作条件无疑属于第2种。

对于养牛场的地面工作障碍信息部分未知或者完全未知的情况，机器人利用装配的传感器实时检测周边环境，控制系统利用传感器接收到的数据决策机器人的运动轨迹。局部路径规划的常用方法有人工势场法及模糊逻辑等[2]。

本文采用一种基于行为的方法，可以实现机器人的局部最优路径规划，行为的综合结果用来指导机器人的运动，非常适用于时变未知环境下的路径规划。经过样机的现场测试表明，本文方法的准确性和实时性极高。

1 自动清粪机器人的运动轨迹模型

本文设计的自动清粪机器人的两个后轮作为完全独立的驱动轮，分别由2个直流伺服电机控制，利用转速差实现转向。其具有自重轻、承载大、机构简单、驱动和控制相对方便、行走速度快、机动灵活及工作效率高等优点[3]。

机器人装配有以下3种传感器：

1)惯性测量部件，用来检测机器人的转角；

2)超声波传感器，检测机器人与障碍物的距离；

3)电流检测传感器，用来检测驱动电机的电流，一旦电流值超过正常值，即可判断机器人前端接触到障碍物。

根据Muri和Neuman提出的轮式移动机器人运动学建模理论，两轮驱动的自动清粪机器人作为几何中心和质心不重合的约束非理想的简化模型，如图1所示[4]。简化后的清粪机器人的运行机构由两个同轴的驱动后轮和1个刮粪板组成，三点确定一个平面，刮粪板起到万向从动轮转向的作用。清粪机器人的两个后轮分别由两个独立的直流伺服电机驱动，前方刮板看作是支撑轮，在后方两个驱动轮的作用下，机器人可以沿目标方向向前移动。(xM,yM)为机器人质量中心M在XOY世界坐标系中的横纵坐标；θ为清洁机器人运动方向与世界坐标系X轴的夹角；C为清洁机器人连接两个驱动轮的轮轴的几何中心，(xC，yC)为清洁机器人的平面投影几何中心C在世界坐标系中的横纵坐标；r为两个驱动轮半径，2b为两驱动轮之间的距离；d为M、C之间的距离。

图1 自动清粪机器人运动模型Fig.1 Motion model of the barn cleaner

设机器人平台质量中心 M 的前进速度为 VM，它垂直于平面的车轮轴，在世界坐标系中坐标轴上的分量分别为

(1)

(2)

由图 1 可知：清洁机器人质量中心 M 和平面投影几何中心 C有如下位置关系，即

xc=xM-dcosθ

(3)

yc=yM-dsinθ

(4)

对式(3)和式(4)求导可得

(5)

(6)

将式(1)、式(2)分别代入式(5)和式(6)，经过运算可以得平面投影几何中心C的速度关系式为

(7)

(8)

(9)

由式(1)、式(5)、式(8)和式(9)可得

(10)

(11)

当不考虑驱动轮的滑动时，由式(9) ～ 式(11)可以得到机器人的运动学方程为

(12)

由于机器人的质量中心 M 和平面投影几何中心 C并不重合，在两个后置驱动轮满足与地面无滑动、纯滚动的运行条件下，清粪机器人所受的非完整约束关系为

(13)

可将式(13)改写成如下形式，即

(14)

(15)

速度V(t)为

V(t)=[v，ω]T

(16)

其中，v表示机器人平面投影几何中心C在世界坐标系下的线速度;ω表示C 的笛卡儿坐标系下角速度。

2 基本行为集

针对清粪机器人的复杂作业环境，本文为清粪机器人设计了3种基本行为：follow_wall行为、change_direction行为和move_to_alone行为。其中，follow_wall行为的功能是：根据机器人当前探测到的环境信息，通过超声波传感器及惯性测量部件以避开环境中的静态障碍物，并利用机器人距离静态障碍的距离及惯性测量检测出的转角，使机器人按照预先设定的轨迹行走。change_direction行为的功能是：当人或者奶牛突然出现在预先设定的轨迹上时，控制系统根据电流传感器检测到的驱动电机电流值，可以使机器人迅速停止运动，以免造成伤害，之后能够绕开障碍物，并且使机器人回到预先设定的轨迹。move_to_alone行为的功能是：当环境中没有可以利用测距的静态障碍物，并且尚未碰撞到移动障碍物时，那么机器人将进行直行动作，由于纠偏措施较少，所以该行为进行的时间不能过长，以免偏离预定轨迹。

3种行为之间的优先级关系如图2所示。其中，位于高层的行为抑制下层的行为，S表示抑制，即change_direction行为抑制follow_wall行为，follow_wall行为抑制move_to_alone行为。在某时刻，只有确定的唯一种行为对机器人起作用。

图2 基于行为控制的结构Fig.2 The control structure based on the behavior

2.1 Change_direction行为

当机器人在预设轨迹上行驶、碰撞到突然进入到轨迹中的人或牲畜等动态障碍物时，通过电流传感器可以检测到驱动电机的电流瞬间增大，机器人会立即停止前进，并在向后行驶一定的距离后，开始旋转；之后开启此行为模式，沿着障碍物的边界开始行驶。开启该模式时，follow_wall行为和Move_to_alone行为均被锁死。

以机器人当前位置R点为原点、当前运动方向为X轴，建立极坐标系，如图3所示[5]。

图3 Change_direction行为示意图Fig.3 Change_direction behavior diagram

机器人下一步运行的运动方向应当满足

(17)

(18)

其中，D0为一裕量，用来确保机器人与障碍物保持一定距离；θfw(j)在满足上式的条件下，使得Dfw((θfw(j))取得最小值，记为θfw(m)。当没有发现合适的θfw(j)时，机器人原地旋转θao(固定值)。

于是，change_direction行为规划的控制输入为

(19)

(20)

当机器人与障碍物的距离Dfw(θfw(j))大于某个定值时，控制系统通过惯性测量部件检测到的机器人的转角值，可以使得机器人通过旋转一定的角度来调整运行的方向，从而重新回到预设轨迹,开启follow_wall行为和Move_to_alone行为。

2.2 follow_wall行为

在机器人按照预先设计定的轨迹正常行驶，并且周边环境具有固定的、可供测距的静态障碍物存在时(如牛栏，水槽等)，该行为模式被激活。

在此行为模式下，机器人通过超声波传感器不断检测与周边静态障碍物的距离，控制系统通过判断距离是否符合预先的设定值，来判断运动轨迹是否偏离。当发生偏离时，就需要控制系统进行及时调整。调整过程中，可以通过超声波传感器测距来保证调整后机器人与周边静态障碍物的距离符合预先的设定，通过惯性测量部件测转角来保证调整后的运动方向符合预先的设定。

2.3 Move_to_alone行为

当机器人在正常的行走过程中，周边环境没有可供超声波传感器测距的静态障碍物存在时，该行为模式被激活。

在此行为模式下，只有惯性测量部件检测到的转角作为控制系统的纠偏输入，超声波传感器无法为控制系统提供有效的输入。所以，在规划机器人的行走路径时，要尽量减少该行为模式的出现次数。

3 机器人运行轨迹规划算法的优化实现

通过本文之前的说明，可以得出以下结论：本文设计的机器人路径规划算法中，Change_direction行为是机器人在预定行驶轨迹中遇到突发的动态障碍物时的处理方法；follow_wall行为和Move_to_alone行为是机器人在预定轨迹上正常行驶时的行为模式。

图4简易说明了follow_wall行为和Move_to_alone行为如何构成了机器人在没有动态障碍物的情况下的行驶轨迹。

图4 运动轨迹示意图Fig.4 Trajectory diagram

运动轨迹由以下路径组成：(1)沿墙或牛栏走(follow_wall行为)；(2)左转(45°)；(3)直行(Move_to_alone行为)；(4)右转(45°)；(5)沿墙或牛栏走(follow_wall行为)；(6)右转45°；(7)直行(Move_to_alone行为)；(8)左转45°；(9)沿墙或牛栏走(follow_wall行为)；(10)沿墙或牛栏走(follow_wall行为)；(11)左转10°，之后右转5°；(12)直行(Move_to_alone行为)；(13)左转180°；(14)直行(Move_to_alone行为)；(15)沿墙或牛栏走(follow_wall行为)；(16)沿墙或牛栏走(follow_wall行为)；(17)直行(Move_to_alone行为)；(18)左转90°；(19)沿墙或牛栏走(follow_wall行为)；(20)左转90°；(21)直行(Move_to_alone行为)；(22)右转10°；(23)沿墙或牛栏走(follow_wall

行为)。

4 结论

本文提出的路径规划方法非常适合于动态环境下(即不时出现移动障碍物情况下)的机器人的运动路线规划。本文在为机器人设计了3种基本行为后，根据机器人的工作目标、周边的工作环境等，可以通过进行在线调整来提高机器人适应环境的能力，包括改变行为的激活状态和运行状态、改变行为算法模型的接口参数及改变行为的综合决策判断模式等。最后，控制系统针对各个被激活的基本行为进行综合决策，驱动执行机构执行决策。

按照本文提出的路径规划方法进行了软件编程，并组装了测试样机。通过样机在奶牛养殖场的相关测试、运行，验证了本文提出方法的实时性、准确性和可操作性。

[1] 王菁华,张翠敏.智能机器人综合路径规划算法在Matlab中的实现[J].天津工程师范学院学报, 2006,16(3):47-50.

[2] 于奎龙,贾小平，曹有辉，等.基于混合算法的局部路径规划[J].装甲兵工程学院学报, 2008,22(2):43-45.

[3] 张倩.差动式移动机器人的运动性能分析[D].天津：河北工业大学，2013.

[4] 李江昊，李振波，陈佳品.毫米级全方位移动微型装配机器人设计[J].机器人,2008,30(1):63-69.

[5] 屈 鸿，黄利伟，柯星.动态环境下基于改进蚁群算法的机器人路径规划研究[J].电子科技大学学报,2015,44(2):260-265.

Research and Realization of Automatic Barn Cleaner Path Planning

Hou Yuntao1, Yao Lihui2, Cai Xiaohua1, Wang Qiang3

(1.Scientific Research Institute of Agricultural Mechanical Engineering in Heilongjiang，Harbin 150081, China；2.Heilongjiang BAYI Agriculture university，Daqing 163319, China; 3.Hohhot Branch of Chinese Academy of Agricultural Mechanization Sciences 010010, China)

The working environment of barn cleaner is complex,not only static obstacles,such as the stable,and dynamic obstacles, such as people and livestock,etc.Aimed at the situation ,this paper proposes a method of path planning algorithm based on behavior .This method is very suitable for robot in dynamic environment, its principle is simple, quick calculation, easy to implement.According to this algorithm, we established a complete set of software system and the design scheme,And on this basis, make the prototype.After the prototype in dairy cow farm work and test, this method was verified real-time, accuracy and operability.

barn cleaner; behavior method; path planning; dynamic environment

2016-07-21

“十二五”国家科技支撑计划项目(2014BAD08B10)

侯云涛(1984-),男，哈尔滨人，工程师，(E-mail)736126 975@qq.com。

蔡晓华(1968-),男，哈尔滨人，研究员级高级工程师，(E-mail) 12369757@qq.com。

S24；TP242

A

1003-188X(2017)06-0023-04