自适应PID 控制的自动推料机器人的设计

沈 治

（常州工业职业技术学院智能控制学院，江苏 常州 213164）

1 引言

在传统的奶牛养殖过程中，草料铺设在牛栏旁，奶牛在进食的过程中，只会吃最靠近的草料，同时在进食的过程中，将草料拱到外侧，最终奶牛只能吃到部分草料，另外部分草料会因为时间堆积而变质废弃[1]。目前的解决方式一种是人工推料，将外侧草料，人工推入奶牛食用范围，这样需要安排较多人工及设备用于推料，但是夜间拢料不及时会导致奶牛产奶量低[2]；二是采用机械臂拢料，但是由于机械臂上不安装传感器，会导致碰撞奶牛头部造成奶牛损伤，而且机械臂上的传动螺杆会缠上草料，不利于清理和维护[3]。

这里设设计的推料机器人可以自动完成草料推料过程，同时设置充电桩，能够形成24 小时不间断推料，大大减少了人工以及在推料过中的劳动强度，提高了奶牛的产奶量。本系统已经成功应用于安徽某奶牛养殖场，系统设计合理、运行可靠，在草料推料的过程中实现了全自动化和智能化。

2 系统控制要求

一间牛舍，长378m，宽90.6m 使用2 台推料机器人，按照预设路径推料，如图1 所示。

图1 牛舍示意图Fig.1 Sketch of a cowshed

自动推料机器人采用边往前行走边旋转拢料的方式，可以在有限的空间内将草料推入奶牛的食用范围，如图2 所示；推料机器人高度60cm，防止草料漫过推料机器人顶度或者进入旋转部件夹缝中；有效直径是85cm，自重200kg；推料机器人行进速度是0.75m/s，旋转部件的旋转速度是7.16 圈/min（43°/s），要求3min 推料一次。

图2 推料机器人工作方式Fig.2 Working Mode of Feeding Robot

3 机械结构设计

该推料机器人，包括外框及支架，支架上方设有配重且支架上还连接驱动电机及前轮，驱动电机输出轴连接行走轮；还包括控制装置、减震装置及电力装置，控制装置包括中央控制器，中央控制器分别通信连接编码器、超声波传感器、角速度传感器、压磁式传感器及磁感应传感器，中央控制器还电连接驱动电机及手动控制器，减震装置包括功率增能器及保护环，功率增能器通信连接中央控制器并安装在保护环内，压磁式传感器也设于保护环内，保护环安装在外框外侧。使推料机可以在无人的情况下工作，让奶牛可以在一整天都能吃到新鲜的饲料，同时也大幅减少饲料的浪费。如图3 所示。

图3 机械结构示意图Fig.3 Schematic Diagram of Crawling Mechanism

4 电气控制系统设计

4.1 运动模型构建

搭建推料机器人移动平台，配置多种传感器。该平台采用三轮的运动结构，左右两轮通过差速来驱动平台行走，前轮来实现支撑和变向[4]。简化模型后，平台通过左右二轮来表示，假设该移动平台质量均匀，则重心位于底座中心，忽略地面和轮子的摩擦力，建立的自动推料机器人的运动模型，如图4 所示。

图4 运动模型Fig.4 Motion Model

图中x-o-y 构成了大地坐标系，在自动推草机器人当前坐标中心建立机器人坐标系xR-o-yR。在大地坐标系中，t 时刻机器人的姿态表示为（xt，yt，θt），其中（xt，yt）表示在t 时刻机器人的位置，θt表示t 时刻机器人的姿态，这里表示为机器人前进方向与大地坐标系中x 轴的夹角。d 为两时刻左右两轮的位移之差。vL和vR分别表示机器人左右驱动轮的速度，l 表示左、右轮的轴距，r 表示机器人作圆弧运动的半径。

从t 到t+1 时刻，自动拢草机器人的姿态发生了变化，变化量表示为（Δx，Δy，Δθ），根据上图，得到如下：

式中：vx—在当前坐标系x 方向上的速度；vy—在当前坐标系y 方向上的速度，根据以上关系，得到自动拢草机器人在t+1 时刻姿态：

自动推料机器人用式（3）来表示在x 方向上的速度：

机器人在作圆弧运动时，在采样时间内角度的变化很小，根据图中的关系，得到：

结合以上关系，得到机器人的角速度和圆弧运动的半径：

在实际工作时，能够控制的变量是电机的转速n，设定左右两轮直径d 为：

通过上面我们可以得到自动拢草机器人的运动方程：

式中：ωt—噪声，该噪声服从高期分布（0，Q），自动推料机器人的速度满足：

此时得到自动推料机器人的运动方程。由上式可知，车轮的直径d 和两轮之间的轴距l 为已知量，因此只要控制驱动左、右轮旋转的转速就可以控制自动推料机器人的线速度、角速度和转弯半径的控制，以实现对拢草机器人行走姿态的实时控制。

4.2 系统控制框图

该推料机器人系统控制框图，如图5 所示。

图5 系统框图Fig.5 System Block Diagram

当推料机器人沿饲喂通道直线行驶时，机器人旋转推料装置将草料推至围栏处，通过磁感应传感器感应事先埋设在地下的磁点来确认当前的位置，在行驶过程中通过角度传感器来控制前进方向，行驶动力来源于左右两个驱动电机，每个驱动电机配一个编码器，由中央控制器控制每个驱动电机的输出速度，形成两轮的差速来控制转弯的角度。通过超声波感应器来感应前进路径方向上的障碍物来选择停止或者前进，通过安装在机器人底部压磁式传感器来检测所遇阻力的大小来实时调整旋转推力机构所输出的推力，并在机器人四周安装了防护环，在发生和障碍物碰撞的极端情况下，起到一定的缓冲保护作用。

4.3 导航形式和轨迹跟踪策略

推料机器人的行走利用磁带导引的工作原理，在行走的路线上预埋磁点，不需要通过机器人本身的行走来识别区域地图，通过磁感应传感器来确定偏离中心的程度，角度传感器将值反馈给控制器，控制器采用自适应的PID 控制策略来调整行走姿态。自适应PID 控制是指采用自适应控制思想与常规PID 控制器相结合形成的自校正PID 控制技术[5]，通过输出反馈，调整参数适应控制器的变化，弥补经典PID 控制器的时变、非线性系统中的误差，是十分理想的控制方法[6]。

根据图4 和公式（10）知道，根据预埋的磁点，我们提前知道该点的曲率，提前让控制器实行角度的控制，ex和eθ表达了推料机器人当前状态（包括位姿与角度），根据实践经验，自动推料机器人的控制系统采用比例单元kP和kD微分单元进行控制即可，故两个控制器的PID 系数都可以表示为比例和微分的线性组合，如：

在确定了表达式之后，根据实践经验，在高速时为了保证推料机器人的稳定性，可以适当的牺牲跟踪精度，而在低速时则需要尽量减少误差。推料机器人在自适应PID 控制下的轨迹跟踪的仿真图，如图6 所示。

图6 机器人行走仿真轨迹Fig.6 Robot Walking Simulation Rrajectory

这种控制方案可以十分简单直观地完成推料机器人的直线行走的角度纠编控制和曲线时的轨迹跟踪，并且能够配合不同路径上的不同速度，可以精确的设置到送料围栏的最小距离。

4.4 控制流程

采用西门子1215C DC/DC/DC 的PLC 作为主控制器，作为晶体管输出的PLC 可以输出高速和PWM 脉冲[7]，配以SM1223（DI16/DQ16）和SM1234（4AI/4AQ），作为后续功能扩展。

控制流程，如图7 所示。设备在机械安装完成后，将其放置在地面上进行测试，此时通过HMI 进入测试模式，在测试模式下可进行路径选择、速度选择、前后移动和定点移动功能，同时设置好kP和kD的相关参数，此时参数将被锁定，不允许进行修改，如果进行修改，选择登录修改界面，输入正确的帐号和密码，即可修改。测试完成后，代表设备完好、行走正常，此时选择进入自动运行界面，在此画面中能够实时监控到机器人当前的位置、行走速度和当前的状态，在运行过程中，如果出现报警信号紧急停止（前方障碍或阻力太大导致旋转推料电机过载），待工作人员处理后，完成余下工作。

4.5 HMI 监控画面设计

监控画面设计采用MCGS 的触摸屏，可以利用函数以及脚本设计功能复杂的画面[8]，本系统我们设计了测试界面，如图8 所示。运行界面，如图9 所示。多个子画面弹出窗口，如图10 所示。通过测试界面能够完成机器人点动行走、路径设置、速度设置、PID 参数设置，在自动运行界面里能够实时显示推料机器人当前速度、位置、电量、时间，利用自适应的PID 控制策略来完成机器人的行走过程，如遇紧急异常情况，会进行声光报警及窗口提示。

图7 控制流程Fig7 Control Flow

图8 测试界面Fig.8 Test Interface

图9 自动控制界面Fig.9 Automatic Control Interface

在自动控制界面上，可以实时地对当前推料机器人所在的位置（如A-16 指的是A 区的第16 个点）、当前的电量、速度进行显示，同时在下方的报警条中能够进行当前阻力过大后的报警显示，同时为了简化画面，又设置了“电量提示、登录窗口“子窗口，进行一些人机的交互以及故障现象提示，方便操作人员排故检修。程序脚本如下所示，效果如图11 所示。

IF 设备0_ 读写M0030=1 THEN

! OpenSubWnd（电量过低提示，150，120，224，120，0）

ENDIF // 按照触摸屏上的位置和大小打开“电量过低提示”子窗口

IF ! strComp（帐号，"hqjd"）=0 AND ! strComp（密码，"hqj3d"）=0

THEN

设备0_ 读写M010=1

用户窗口.故障定时界面.Close（）

! CloseSubWnd（故障定时界面）

ENDIF //如果帐号和密码设置正确，则关闭"故障定时界面"，同时

M10=1

图10 多个子窗口界面Fig.10 Multiple Subwindow Interfaces

5 系统运行情况

本系统从2018.12 月投入使用，系统运行稳定，大大减少了人工的消耗，实物如图11 所示。以一间牛舍为例，现将人工和机器人推料相关的技术指标对比，如表1 所示。通过对比发现，如果每个牛舍能够安排2 台推料机器人，在不充电的情况下，单个推料机器人可以工作（4～6）个小时，如果采用交替工作的方式，可以实现牛舍拢料的全天候作业，人工减少了86%，而推料次数增加了4 倍，草料需求减少了28%，频繁的推食可以刺激奶牛更频繁的进食，这对奶牛的健康、生育、生产都有积极的影响，同时大大的降低了奶牛养殖企业的劳动需求和草料成本。

图11 实物图Fig.11 Physical Map

表1 人工推料与机器人推料Tab.1 Artificial and Robot Feeding

6 结论

采用现代的控制技术，利用自适应PID 策略完成推料机器人的行走纠编控制，最大偏移距离为15.8mm，表明该机器人具有良好的路径保持能力。通过软件可以设置推送频率、每组送料类型、对于每一条拢料路径可以设置到（30～60）cm 的送料围栏的最小距离。根据这些数据，软件确定正确的阻力，产生（250～1280）N的推力。根据在某一点进食，推料机器人会自动纠正到围栏的最佳距离，解决了人工多、工作劳动强度大、夜间无法及时推料的缺陷，实现了推料过程中的全自动化和智能化。目前系统运行可靠，在畜牧围栏养殖行业中得到了推广。