基于室内电子地图的饲喂机器人及其精确定位方法
【专利摘要】一种基于室内电子地图的饲喂机器人及其精确定位方法，该饲喂机器人由上位控制计算机，Xbee无线收发装置，饲喂机器人控制器三部分组成；在上位控制计算机上建立饲喂管理系统和与畜舍实际几何尺寸相符合的室内电子地图；该电子地图负责管理饲喂机器人的行走路径，行走速度，饲喂的顺序关系，并将这些信息通过Xbee无线收发装置，发送给相应的饲喂机器人控制器；饲喂机器人控制器收到这些信息后，按规定的饲喂量、运行路径、运行速度运行，并将运行过程中的各种实时数据及时反馈给上位控制计算机，对数据库数据进行更新。本发明实现了饲喂机器人的无人驾驶、路径规划，并实现了饲喂机器人在运行过程中的精确定位。
【专利说明】
基于室内电子地图的饲喂机器人及其精确定位方法
技术领域
[0001] 本发明涉及畜牧饲喂技术领域，具体指一种基于室内电子地图的饲喂机器人及其 精确定位方法。
【背景技术】
[0002] 饲喂机械在牛场饲喂逐渐普及的背景下，提高饲喂机械的自动化、信息化，智能 化，降低工人的劳动强度，减少饲料浪费，提高劳动效率是促进我国畜牧行业进一步发展的 重要举措。
[0003] 中国专利CN 201110442498.3公开了一种以单片机为控制核心的精确饲喂机控制 系统。该饲喂机由双模行进机构、精确投料机构、单片机自动识别控制系统和上位机信息管 理系统组成。该专利技术没有涉及精确定位功能和电子地图导航功能，因而无法实现自主 路径规划和无人驾驶行走。
[0004] 中国专利CN200810239151.7公开了一种以单片机为控制核心的精确饲喂机控制 系统。该系统利用无线射频识别技术对进行个体识别，以计算机为信息管理平台，以单片机 为数据处理和控制平台，实现精细化饲喂。该技术的缺点是通过人工来推动饲喂设备的行 进，劳动强度大;该专利技术没有涉及精确定位功能和电子地图导航功能，因而无法实现自 主路径规划和无人驾驶行走。
[0005] 欧洲专利EP1661454公开了一种动物饲喂装置，该装置首先对牛的身份进行识别， 然后传送给控制单元，控制单元通过控制牛颈枷的开与关控制牛的进食时间的多少。该专 利技术没有涉及精确定位功能和电子地图导航功能，因而无法实现自主路径规划和无人驾 驶行走。
【发明内容】
[0006] 本发明的目的之一是提供一种基于室内电子地图导航的饲喂机器人，可实现无人 驾驶行走； 本发明的目的之二是提供一种基于室内电子地图的饲喂机器人精确定位方法，使饲喂 机器人能在室内实现精确定位。
[0007] 为达到上述目的，本发明采用的技术方案为： 一种基于室内电子地图的饲喂机器人，该饲喂机器人由上位控制计算机，Xbee无线收 发装置，饲喂机器人控制器三部分组成；在上位控制计算机上建立饲喂管理系统和与畜舍 实际几何尺寸相符合的室内电子地图；该电子地图负责管理饲喂机器人的行走路径，行走 速度，饲喂的顺序关系，并将这些信息通过Xbee无线收发装置，发送给相应的饲喂机器人控 制器;饲喂机器人控制器收到这些信息后，按规定的饲喂量、运行路径、运行速度运行，并将 运行过程中的各种实时数据及时反馈给上位控制计算机，对数据库数据进行更新。
[0008] 所述室内电子地图由地图编辑管理器、地图定位管理器、地图执行管理器三部分 组成;所述地图编辑管理器是根据畜舍的实际几何尺寸建立相对应的电子地图；根据饲喂 顺序建立饲喂路径的拓扑关系；对饲喂机器人的运行速度进行管理;所述地图定位管理器 是对饲喂机器人进行实时定位;地图执行管理器是根据饲喂机器人的当前位置，计算其下 一步的行走距离，运行方向，运行速度，并将这些数据生成控制命令，控制饲喂机器人按规 划好的路径精确运动。
[0009] -种基于室内电子地图的饲喂机器人的精确定位方法，具体步骤如下： a:在畜舍设置若干红外路标； b:饲喂机器人的前轮、后轮均通过减速机和直流伺服电机连接，该直流伺服电机尾部 安装有旋转编码器;饲喂机器人前轮的编码器用以测量其运行过程中的角度，饲喂机器人 后轮的编码器用以测量其运行过程中的运行距离；前、后轮的编码器信号均送入饲喂机器 人控制器用以计算饲喂机器人相对于起始原点的X轴和Y轴坐标； c:旋转红外测距传感器和陀螺仪安装在饲喂机器人的前顶部；用以测量饲喂机器人相 对于红外路标的绝对位置;红外测距传感器和陀螺仪的信号经饲喂机器人控制器运算后， 转换成饲喂机器人相对于起始原点的极坐标位置； d:通过和前后轮连接的直流伺服电机编码器测量出的脉冲数，经饲喂机器人控制器计 算出的饲喂机器人当前的位置相对于起始原点的位置姿态为&; e:通过旋转红外测距传感器和陀螺仪测量出红外路标距饲喂机器人的极坐标 f:将步骤d中饲喂机器人的当前位置姿态转换为极坐标期望值; g:对步骤e中测量的极坐标值和步骤f?中期望的极坐标值进行比对匹配，￥ -#|， g呈正态分布;通过修正脉冲当量、间隙补偿值、线性划分的区间点，找出方差最小的一组 参数，使定位精度达到最优； h:将步骤g中找到的最佳脉冲当量、间隙补偿值、线性划分的区间点，即g方差最小的 参数用扩展卡尔曼波滤波器产生饲喂机器人 、，一纖删 ______達；:，一个^
;从而克服了下一个控制节拍导航不 确定性的问题，最终实现由于测量误差、系统误差、累计误差导致无法精确定位导航的问 题。
[0010] 本发明实现了饲喂机器人的无人驾驶、路径规划，并通过纠正测量误差、系统误 差、累计误差实现了饲喂机器人在运行过程中的精确定位，解决了在导航过程中的导航不 确定、无法按期望路径规划行驶的问题。
【附图说明】
[0011] 图1为本发明的控制系统框架图。
【具体实施方式】
[0012] 下面结合附图对本发明进一步详细说明。
[0013] 如图1，一种基于室内电子地图的饲喂机器人，该饲喂机器人由上位控制计算机， Xbee无线收发装置，饲喂机器人控制器三部分组成;在上位控制计算机上建立饲喂管理系 统和与畜舍实际几何尺寸相符合的电子地图；该电子地图负责管理饲喂机器人的行走路 径，行走速度，饲喂的顺序关系，并将这些信息通过Xbee无线收发装置，发送给相应的饲喂 机器人控制器;饲喂机器人控制器收到这些信息后，按规定的饲喂量、运行路径、运行速度 运行，并将运行过程中的各种实时数据及时反馈给上位控制计算机，对数据库数据进行更 新。
[0014] 所述上位控制计算机采用研华工业控制计算机，主板选用A頂B-784，i 5处理器，4G 内存，32位windows7操作系统。电子地图是在ArcGIS 10.1 desktop软件的基础上用C#进行 二次开发，二次开发的主要内容包括:建立基准面变换，建立SQL数据库，对XY图层饲喂机器 人运动进行时间属性的访问，通过时间滑块实时对饲喂机器人的位置信息进行更新，在SQL 数据中建立用户列表和用户权限，对饲喂机器人的行走路径，行走速度，饲喂的顺序的界面 程序进行开发。
[0015] 所述电子地图由地图编辑管理器、地图定位管理器、地图执行管理器三部分组成。 所述地图编辑管理器是根据畜舍的实际几何尺寸建立相对应的电子地图；根据饲喂顺序建 立饲喂路径的拓扑关系；对饲喂机器人的运行速度进行管理;所述地图定位管理器是对饲 喂机器人进行实时定位;地图执行管理器是根据饲喂机器人的当前位置，计算其下一步的 行走距离，运行方向，运行速度，并将这些数据生成控制命令，控制饲喂机器人按规划好的 路径运动。
[0016] 所述饲喂机器人控制器采用Coretex-A9处理器，2GB内存，16GB eMMC Flash，10.1 寸电容触摸屏。饲喂机器人控制器的外围功能模块有旋转红外测距，条形码识别，直流伺服 电动机驱动控制，精确投料控制，蓄电池电量管理，Xbee无线收发等组成。该控制系统通过 室内电子地图、无线导航定位，解决了奶牛饲喂作业中饲喂机器人的无人驾驶、室内导航定 位、精确饲喂的问题。
[0017] 上位控制计算机和Xbee无线收发装置通过串口连接，饲喂机器人控制器与Xbee无 线收发装置也通过串口连接，Xbee-PRO无线收发装置室内通信100米，室外通信距离1500 米，发射功率100毫瓦，接收灵敏度-10dBm，数据传输率25KB/S，使用直接序列扩频通信抗 干扰能力强。
[0018]在饲喂过程中，定位误差的形成原因有测量误差、系统误差、累计误差。测量误差 是由于直流伺服电机后部的旋转编码器在测量过程中产生的，该编码器属于增量型脉冲编 码器，运行过程中的机械间隙、振动、滑动、电磁干扰都能引起测量误差。系统误差是由于饲 喂的实际坐标是通过脉冲当量或红外线发出到接受的时间差计算得出的，由于脉冲当量和 时间差本身就存在误差，从而形成系统误差。累计误差是由于在计算机中不停地进行叠加 运算形成的。
[0019] 由于上述测量误差、系统误差、累计误差存在而导致饲喂机器人在室内自动行驶 过程中无法精确定位，致引起导航路径的不确定的问题通过以下方法解决： a:在畜舍内按长度方向每20米，宽度方向小于20米的矩形的4个顶点设置若干红外路 标； b:饲喂机器人的前轮、后轮均和直流伺服电机的减速机连接，直流伺服电机尾部安装 有旋转编码器。饲喂机器人前轮的编码器用以测量其运行过程中的角度，饲喂机器人后轮 的编码器用以测量其运行过程中的运行距离。前后轮的编码器信号均送入饲喂机器人控制 器用以计算饲喂机器人相对于起始原点的X轴和Y轴坐标。
[0020] C:旋转红外测距传感器和陀螺仪安装在饲喂机器人的前底部；用以测量饲喂机器 人相对于路标的绝对位置。红外测距传感器和陀螺仪的信号经饲喂机器人控制器运算后， 转换成饲喂机器人相对于起始原点的极坐标位置。
[0021] d:通过和前后轮连接的直流伺服电机编码器测量出的脉冲数，经饲喂机器人控制 器计算出的饲喂机器人当前的位置相对于起始原点的位置姿态为％。
f於1 灣P懸纖A儀會爵置纖應.一 r j, .KJ r , i i!為'" t ' \ ^ X、>、\ \ < ";鋈从上一 !'.參'I :__襲_截_麵11_:量：
[0022] 上式中，_^是右后轮的运动距离，％仏是左后轮的运动距离，b是饲喂机两后轮 之间的距离。通过该公式可以看出饲喂机器人任意时刻的位置姿势等于前一时刻的位置姿 态加运动增量。所有的测量误差、系统误差、累计误差通过反复的叠加运算，计算出算饲喂 机器人的当前位置，这种通过旋转编码器测量而计算出的当前位置和实际位置偏差很大。
[0023] e:通过旋转红外测距传感器和陀螺仪测量出红外路标距饲喂机器人的极坐标_，
，:a|是极坐标的夹角，是极坐标的半径。由于安装在牛舍内的红外路标是固定不 动的，因此用红外测距传感器测量的饲喂机距红外路标的距离只受测量精度的影响，没有 系统误差和累计。
[0024] f:将步骤d中饲喂机器人当前的位置姿态:％，换算成极坐标期望值，
[0025」g :对步骤e中测量的极坐标值和步骤f中期望的极坐标值进行比对匹配 轉-呈正态分布。通过修正脉冲当量、间隙补偿值、线性划分的区间点，找出方 差最小的一组参数，使定位精度达到最优； h:将步骤g中找到的最佳脉冲当量、间隙补偿值、线性划分的区间点，即方差最小的一 组参数用扩展卡尔曼波滤波器产生饲喂机器人 丨:极| .，一翁輩_塞身^ LfcJ
義:_:_:_了'下一令幾_1贊 拍导航不确定性的问题，最终实现由于测量误差、系统误差、累计误差导致无法精确定 位导航的问题。
【主权项】
1. 一种基于室内电子地图的饲喂机器人，其特征在于：该饲喂机器人由上位控制计算 机，Xbee无线收发装置，饲喂机器人控制器三部分组成;在上位控制计算机上建立饲喂管理 系统和与畜舍实际几何尺寸相符合的室内电子地图；该电子地图负责管理饲喂机器人的行 走路径，行走速度，饲喂的顺序关系，并将这些信息通过Xbee无线收发装置，发送给相应的 饲喂机器人控制器;饲喂机器人控制器收到这些信息后，按规定的饲喂量、运行路径、运行 速度运行，并将运行过程中的各种实时数据及时反馈给上位控制计算机，对数据库数据进 行更新。2. 根据权利要求1所述的一种基于室内电子地图的饲喂机器人，其特征在于:所述室内 电子地图由地图编辑管理器、地图定位管理器、地图执行管理器三部分组成;所述地图编辑 管理器是根据畜舍的实际几何尺寸建立相对应的电子地图；根据饲喂顺序建立饲喂路径的 拓扑关系；对饲喂机器人的运行速度进行管理;所述地图定位管理器是对饲喂机器人进行 实时定位;地图执行管理器是根据饲喂机器人的当前位置，计算其下一步的行走距离，运行 方向，运行速度，并将这些数据生成控制命令，控制饲喂机器人按规划好的路径精确运动。3. 根据权利要求2所述的一种基于室内电子地图的饲喂机器人的精确定位方法，其特 征在于:具体步骤如下： a:在畜舍设置若干红外路标； b:饲喂机器人的前轮、后轮均通过减速机和直流伺服电机连接，该直流伺服电机尾部 安装有旋转编码器;饲喂机器人前轮的编码器用以测量其运行过程中的角度，饲喂机器人 后轮的编码器用以测量其运行过程中的运行距离；前、后轮的编码器信号均送入饲喂机器 人控制器用以计算饲喂机器人相对于起始原点的X轴和Y轴坐标； c:旋转红外测距传感器和陀螺仪安装在饲喂机器人的前顶部；用以测量饲喂机器人相 对于红外路标的绝对位置;红外测距传感器和陀螺仪的信号经饲喂机器人控制器运算后， 转换成饲喂机器人相对于起始原点的极坐标位置； d:通过和前后轮连接的直流伺服电机编码器测量出的脉冲数，经饲喂机器人控制器计 算出的饲喂机器人当前的位置相对于起始原点的位置姿态为 e:通过旋转红外测距传感器和陀螺仪测量出红外路标距饲喂机器人的极坐标7;; f:将步骤d中饲喂机器人的当前位置姿态转换为极坐标期望值 g:对步骤e中测量的极坐标值和步骤f中期望的极坐标值进行比对匹配，S#呈正态分布;通过修正脉冲当量、间隙补偿值、线性划分的区间点，找出方差最小的一组 参数，使定位精度达到最优； h:将步骤g中找到的最佳脉冲当量、间隙补偿值、线性划分的区间点，即#方差最小的 参数用扩展卡尔曼波滤波器产生饲喂机器人 T-%廳___顧獄養::下一令鑛_:1':___簾續露;从而克服了下一个控制节拍导航不 确定性的问题，最终实现由于测量误差、系统误差、累计误差导致无法精确定位导航的问 题。
【文档编号】G05D1/02GK105929826SQ201610330119
【公开日】2016年9月7日
【申请日】2016年5月18日
【发明人】杨军平, 袁虎成, 贺成柱, 张剑琴, 薛红睿, 王季, 吕凤玉, 李京默
【申请人】甘肃省机械科学研究院