一种可无线控制的双核四轮驱动UWB定位割草机器人及其控
制方法
技术领域
本发明属于割草机器人领域,尤其是一种可无线控制的双核四轮驱动UWB定位割草机器人及其控制方法。
背景技术
割草机器人是一种能够自主行走割草的机器人。通常用于家庭、公园、园林、小区、高尔夫球场的草地修剪维护。由于割草机器人能够自动行走、自动割草、无需人工操作,能够减轻人力,提高工作效率,并能保持割草高度与质量的稳定。
我国在割草机器人领域的研究开发相对滞后,整体水平不高,目前的割草机器人一般由机身、行走机构、切割机构、控制系统所组成,在工作前需要预埋边界线,割草机器人通过使用电磁传感器检测边界线上电流信号强弱来判断与边界间距离,基于单片机的控制器控制两台步进电机的来调整控制割草机的行走路径。对于此类割草机器人,智能化程度较低,实际使用存在着以下不足:现有的割草机器人采用步进电机,会遇到丢失脉冲造成电机失步现象发生,导致对割草位置的计算出现错误,割草机器人丢失实际位置。也会使得机体发热比较严重,有的时候需要进行加装散热装置,使得机器人整体重量增加,不利于机器人爬坡割草。甚至在系统运转的机械噪声大大增加,不利于环境保护。最后一点是割草机器人系统一般不适合高速运行,容易产生振动。
目前的割草机器人设计通常采用单轮驱动或双轮驱动,虽然单轮驱动的割草机器人可以很好的满足对速度和方向的解耦,但是单轮驱动的行走电机功率较大,有时候会造成大马拉小车的现象出现。由于单轮驱动的割草机器人动力与地面的动力接触点只有一个,造成人为很难精确控制其移动的方向,轻微的一点干扰就能造成较大的方向改变。双轮驱动可以削弱单轮驱动的部分缺点,但是割草机器人在爬坡或遇到地面坑洼的时候需要通过电机过载来满足功率要求,长时间的运行会伤害电机的性能,造成系统的可靠性大幅度降低。割草机器人在许多紧急状态下需要快速加速和以较高的速度运行,在这种条件下系统需求的功率较大,满足正常行驶的两轮电机功率无法满足加速要求,系统无法满足紧急状况下的功率要求。
现有的设计中割草机器人使用单核控制如图1,需要同时处理路径规划、导航控制、电机控制等工作,运算量大,导致运算速度慢,控制频率低精度差。由于割草机器人在运行过程中频繁的刹车和启动,加重了单核控制器的工作量,单核控制器无法满足割草机器人快速启动和停止的要求。由于受周围环境不稳定因素干扰,单核割草机器人控制器经常会出现异常,引起割草机器人在行驶过程中失控,抗干扰能力较差。虽然基于专用伺服控制芯片可以生成多轴电机的PWM控制信号,但是需要主控制器与专用芯片通讯后输入控制参数才可以实现,造成整体运算速度降低;受专用伺服控制芯片内部伺服程序的影响,一般情况下伺服控制PID参数不能实时更改,满足不了割草机器人实时快速伺服控制系统的要求;割草机器人运动模式采用简单的直线行走遇到边界转向的方式,缺少全局的路径规划,不够智能。割草机器人在割草行走过程中盲目行走,导致路径重复,浪费能源,续航能力短。割草机器人无法记录已割区域,会出现反复割同一区域现象,割草效率低。割草机器人使用定时工作模式,无法区别已割与未割区域,在结束割草作业后,往往有部分区域没有割,产生漏割现象。边界线需要人工安装,安装繁琐,工作量比较大。边界线预埋后,如果割草区域发生变化,很难修改。边界线常年暴露在室外,容易因腐蚀、氧化、动物破坏而损坏。割草机器人只能通过感应边界线确定是否出界,而无法得到自身精确位置。割下来的草比较大,仍遗留在草坪上,需要再次人工清理,费事费工。
此外,割草机器人需要人工在本地使用按键启动,如果人员不在家中则无法进行割草作业。割草机器人的所有运行信息在本机处理,外界无法得知运行状态与割草进度。如果割草机器人发生故障或其他紧急情况,需要人员到现场查看与操控,不利于割草自动化的发展。
发明内容
为了解决现有技术中的不足,本发明所采用的技术方案如下:
一种可无线控制的双核四轮驱动UWB定位割草机器人,包括割草机器人本体、充电定位站和UWB辅助定位基站;割草机器人本体包括有机身、行走电机、减速器、主动轮、割草电机、割刀、碰撞杆、UWB定位标签、WIFI无线传输模块、充电对接器、控制器、电池;所述控制器包括DSP处理器和FPGA处理器;
所述充电定位站包括有充电系统、UWB定位基站,充电定位基站固定在草地上,能够给割草机器人提供自动充电,并设有雨棚,能在下雨时保护割草机器人电子设备。
所述UWB辅助定位基站安装在草地上固定位置。两个辅助定位基站与充电定位基站通过UWB通信,既可以形成UWB定位系统,通过三角定位法算法得出安装有UWB定位标签的割草机器人位置。可以增加UWB辅助定位基站数量来提高定位精度。
进一步,所述DSP处理器的输入端分别连接UWB定位标签、倾斜传感器、碰撞传感器、雨水传感器、陀螺仪、控制面板和WIFI无线传输模块,所述DSP处理器的输出端连接FPGA处理器,所述FPGA处理器的输出端分别连接行走电机和割草电机;所述直流无刷伺服电机将速度、位置信号再传输至FPGA处理器;
进一步,所述行走电机为直流无刷伺服电机,直流无刷伺服电机的输出依次连接减速器和主动轮所述割草电机为直流无刷电机,直流无刷电机的输出端与割刀相连,所述割刀有双层刀片且割刀设置在车身底部,能够将草分段切割,粉碎切割下的草。
进一步,所述WIFI无线传输模块与控制总站之间通过信号连接。
进一步,所述UWB定位标签设有两个,第一UWB定位标签设置于机身后部中间,第二UWB定位标签设置于机身前部中间;所述第一UWB定位标签位于第二UWB定位标签的正后方,第一UWB定位标签和第二UWB定位标签中线的连线始终为机身中轴线,第一 UWB定位标签和第二UWB定位标签的安装高度一致。
进一步,所述UWB定位基站与第一UWB辅助定位基站、第二UWB辅助定位基站通过UWB通信构成机身坐标定位系统;在机身坐标定位系统中,主控单元采用三角定位算法分别获得第一UWB定位标签的绝对坐标、第二UWB定位标签的绝对坐标以及机身的绝对坐标。
进一步,所述充电对接器安装于机身前部,且所述充电对接器连接电池,为电池进行充电;所述电池分别为DSP处理器和FPGA处理器提供电能。
一种可无线控制的双核四轮驱动UWB定位割草机器人控制方法,包括以下步骤:
S1,需输入权限密码,割草机器人启动工作。
S2,割草机器人初始化,检测各模块工作是否正常,检测电池电压是否过低若电压过低,将提示电量低无法工作,并进入充电模式。
S3,检测UWB定位程序是否正常,如果UWB定位出现丢失,即进入停机自锁模式。
S4,查询控制面板按键及其标志位。这个过程中用户可以使用控制面板或者WIFI无线传输模块和割草机器人交互,设定基础参数;割草机器人将存储相关信息在主存储器中,并将影响相关的标志位。
S5,查询是否需要出充电站,如果割草机器人在充电站中,并且用户需要割草机器人出充电站,割草机器人将执行出充电站程序。DSP会自动断开连接线与交流电源的连接,割草机器人转为蓄电池供电状态。
S6,查询是否执行割草任务,如果需要执行割草任务,割草机器人将进入割草任务工作模式,否者将进入下个循环;控制所述割草任务运行的方法为:
S6.1,查询割草区域地图,并划分割草区域栅格。
S6.2,检测电池电量是否不足,如果电量不足将执行回充电站程序。
S6.3,检测UWB定位程序是否正常,如果UWB定位出现丢失,即进入停机自锁模式,割草机器人将锁定在原地,不再移动,直到UWB定位恢复正常。
S6.4,根据当前位置与栅格地图,通过全覆盖路径算法进行路径规划。
S6.5,根据规划的路径,前进到下一栅格,这一过程中DSP把直流无刷行走电机和转向电机要运转的距离SX转化为加速度、速度和位置参考指令值,然后DSP再结合行走电机和转向电机的磁电传感器的反馈生成驱动直流无刷伺服行走电机和转向电机的驱动信号,驱动信号经功率桥放大后驱动直流无刷伺服行走电机和转向电机以相反的方向运动,在运动过程磁电传感器实时反馈电机的运行参数给FPGA,根据反馈参数二次微调行走电机和转向电机的PWM控制信号,来进行闭环控制,使得割草机器人按照规划路径行走。
S6.6,记录割草机器人的实际路径,并标记当前栅格为已割。
S6.7,查询是否全部栅格都标注为已割后,如果是则代表割草任务完成,割草机将执行返回充电站程序,如果否则进入下个循环。
S7,特殊情况通过中断服务程序进行,如倾斜传感器、碰撞传感器、雨水传感器将会影响中断标志位;如果中断标志位使能,程序将保存现场,进入中断服务程序。
S8,进入中断服务程序后,将检查相关标志位。如果倾斜传感器标志位使能,则代表割草机器人已经翻倒,此时DSP和FPGA经内部伺服控制程序调整直流无刷伺服电机U、 V、X、Y的PWM输出,立刻停止割刀电机与行走电机的运行,并复位软件,防止事故发生。
S9,如果碰撞传感器标志位使能,则说明前方有障碍物,此时将执行避障程序。
S10,如果雨水传感器标志位使能,则说明已经下雨,此时潮湿的草地已经不适合割草工作,割草机器人将执行返回充电站程序。
S11,在割草机器人返回充电站后,割草机器人上的充电对接器将与充电定位站上的充电系统对接。
进一步,执行避障程序的控制方法为:DSP和FPGA经内部伺服控制程序调整直流无刷伺服电机U、V、X、Y的PWM输出,控制割草机器人在安全范围内停车,割草机器人将后退一段距离,并向右转绕行障碍物。在割草机器人运动过程中,磁电传感器会时刻检测直流无刷伺服电机U、V、X、Y的运动速度和位移,并反馈给FPGA,由FPGA二次调整直流无刷电机U、V、X、Y的PWM波控制信号以满足实际需求。在绕过障碍物后割草机器人将继续之前割草工作。
进一步,执行返回充电站程序的控制方法DSP根据机器人规划的返回充电站路径,把直流无刷伺服电机U、V、X、Y要运转的距离SX转化为加速度、速度和位置参考指令值,然后DSP再结合电机U、V、X、Y的磁电传感器的反馈生成驱动直流无刷伺服电机U、V、 X、Y的驱动信号,驱动信号经功率桥放大后驱动直流无刷伺服电机U、V、X、Y以相反的方向运动,在运动过程中磁电传感器实时反馈电机的运行参数给FPGA,根据反馈参数二次微调电机U、V、X、Y的PWM控制信号,来进行闭环控制,使得割草机器人按照规划路径行走。
进一步,S11中与充电定位站上的充电系统对接的方法为:DSP(TMS320F28335)会自动断开连接线与蓄电池的连接,割草机器人转为交流供电状态,交流电源对系统中的蓄电池充电。此时割草机器人入停机自锁模式,割草机器人将锁定在充电站,即使在外力影响下也不再移动,保证充电过程的安全稳定。
本发明的有益效果:
1、本发明行走电机采用直流无刷伺服电机替代了步进电机,不会有步进电机的失步现象,割草机器人的位置控制更加精准。同时直流无刷伺服电机没有碳刷的机械接触,能够减少工作噪音,提高电机寿命。同时能实现停机自锁功能,在紧急情况下即使有外力或斜坡上也能够使得割草机器人停在固定位置,提高了安全性。
2、本发明使用DSP+FPGA双核处理器协同工作设计,DSP处理上层路径规划、导航控制,FPGA处理电机控制等底层工作,大幅提高运算速度与控制精度,使得割草机器人行走得更稳定,路径更加准确。由FPGA(QL1P100)处理多轴直流无刷伺服电机的全数字伺服控制,大大提高了运算速度,解决了单单片机运行较慢的瓶颈,缩短了开发周期短,并且程序可移植能力强。采用FPGA(QL1P100)处理大量的多轴伺服数据与算法,并充分考虑了周围的干扰源,把DSP(TMS320F28335)从复杂的计算中解脱出来,有效地防止了程序的“跑飞”,抗干扰能力大大增强。由FPGA直接生产多轴直流无刷电机驱动信号,不需要 DSP向其输入任何参数,使得系统的处理速度加快,有利于系统高速运行。FPGA控制器根据外围环境实时调整伺服控制系统的PID参数,满足电动病床不同状况下快速伺服控制系统调整的要求。
3、本发明采用四轮独立四驱结构,即使遇到大角度斜坡与地面坑洼,电机也不需要过载来满足功率要求,具有极强的地形适应能力。在紧急状态下,四个电机能够保证极端加减速的功率需求,提高了系统的安全性。
4、本发明采用WIFI无线传输模块,割草机器人能够接入互联网,即使人员不在家中,也可以通过远程控制来进行割草作业。本发明通过WIFI无线模块与控制总站相连,割草区域地图、割草模式都可以远程进行设置,并且割草机器人的运行状态与割草进度能够在远程总站上一目了然,摆脱了本地的限制,操作与控制起来更加方便。如果割草机器人发生故障或其他紧急情况,不需要人员到现场,可以远程查看故障原因与远程处理。
5、本发明的割草机器人采用了智能割草任务程序,可以根据工作区域地图进行全局路径规划,而不是进行简单随机行走模式。本发明的割草机器人在割草行走过程路径更加合理,很少出现重复路径,能够减少电量消耗,提高续航能力。
6、本发明的割草机器人记录已割区域,如果一个区域已经割过,不会再次去割,提高割草效率。本发明的割草机器人可以通过记录区分已割与未割区域,在割草作业中,如果有部分区域漏割,会重新规划路径进行补割。
7、本发明采用UWB无线定位系统,摆脱了有线的限制,无需人工预埋金属边界线,省时省力。如果割草区域有变化,本发明可以方便的调整割草区域地图,不需要挖地重新布线。由于采用无线定位,本发明不会存在因为边界金属线的损坏而导致割草机器人跑出割草区域或无法工作情况,稳定性与可靠性大幅度提高。本发明可以实时获取割草机器人在草坪上的具体位置,可以实现割草机器人的精准控制。本发明采用的UWB定位可以达到厘米计精度,相较于GPS、Zigbee等无线定位方式,精度更高,成本更低。
8、本发明配备了倾斜传感器,在割草机器人倾倒情况下,能够立刻关闭割刀与行走电机,避免发生事故。本发明配备了碰撞传感器,能够在碰到障碍物后,自动绕行。本发明配备了雨水传感器,在下雨时能够使得割草机器人自动返回充电站避雨。
9、本发明采用双层割刀设计,能够将草分段切割并粉碎,使得切割下的草很小,能够直接作为天然肥料留在草地上,不再需要人工的二次清理,环保又节省人力。
附图说明
图1为传统割草机器人控制原理图;
图2为本发明智能无线双核四轮驱动UWB定位割草机器人结构图;
图3为本发明智能无线双核四轮驱动UWB定位割草机器人控制原理图;
图4为本发明智能无线双核四轮驱动UWB定位割草机器人UWB定位原理图;
图5为本发明智能无线双核四轮驱动UWB定位割草机器人程序框图;
图6为本发明智能无线双核四轮驱动UWB定位割草机器人智能割草任务程序框图;
图中,1、机身,2、行走电机,3、减速器,4、主动轮,6、割草电机,7、割刀,8、碰撞杆,9、UWB定位标签,9a、第一UWB定位标签,9b、第二UWB定位标签,10、充电对接器,11、控制器,12、电池,13、充电定位站,14、UWB定位基站,15、充电系统,16、UWB辅助定位基站,16a、第一UWB辅助定位基站,16b、第二UWB辅助定位基站,40、割草机器人。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅用于解释本发明,并不用于限定本发明。
如图2所示,本发明所提出的一种可无线控制的双核四轮驱动UWB定位割草机器人,在机身1前后两端的车底部分别设有1对行走电机2,每个行走电机2的输出端依次连接减速器3和主动轮4;在本实施例中,行走电机2设有四个,为直流无刷伺服电机,主动轮4有四个。行走电机2内有磁电编码器,与FPGA处理器相连,提供电机速度与位置信息。四个主动轮4中两个安装于机身前部左右两侧,另外两个安装于机身1后部左右两侧。可在主动轮4外加装橡胶履带,分别连接左侧前后主动轮与右侧前后主动轮4,即可将四轮差速驱动转化为履带驱动,进一步提高越障能力。
在机身1上设有割草电机6,割草电机6采用直流无刷电机,割草电机6与割刀7相连,割刀7设置在车身1中部的底面,割刀7采用双层刀片,能够将草分段切割,粉碎切割下的草。行走电机2和割草电机6分别与驱动器相连,驱动器连接FPGA处理器,通过驱动器提供电机驱动信号。
在机身1的前端分别设置两个碰撞杆8,分别是第一碰撞杆8a、第二碰撞杆8b;在两个碰撞杆8内有设有碰撞传感器,碰撞传感器与DSP处理器相连,提供外界碰撞信号。
在机身1的前端还设有充电对接器10,充电对接器10与电池12相连。在割草机器人本体与充电定位站13对接后,充电对接器10与充电定位站中的充电系统连接,为电池12 进行充电。电池12分别与DSP处理器、FPGA处理器、驱动器相连,且提供主控与电机的能源动力。
在机身1上设有UWB定位标签9,机身1后部中间布置有第一UWB定位标签9a,机身1前部中间布置有第二UWB定位标签9b;第一UWB定位标签9a位于第二UWB定位标签9b的正后方,第一UWB定位标签9a和第二UWB定位标签9b中线的连线始终为机身1的中轴线,第一UWB定位标签9a和第二UWB定位标签9b的安装高度一致,且第一 UWB定位标签9a和第二UWB定位标签9b分别连接DSP处理器。
在割草区域内固定设置第一UWB辅助定位基站16a、第二UWB辅助定位基站16b和充电定位站13;充电定位站13能够给割草机器人提供自动充电服务,并设有雨棚,能在下雨时保护割草机器人电子设备。且充电定位站13包括有充电系统15和UWB定位基站 14;UWB定位基站14与第一UWB辅助定位基站16a、第二UWB辅助定位基站16b通过 UWB通信构成机身坐标定位系统;在机身坐标定位系统中,主控单元采用三角定位算法分别获得第一UWB定位标签的绝对坐标、第二UWB定位标签的绝对坐标以及机身的绝对坐标;UWB定位基站14与第一UWB定位标签、第二UWB定位标签通过UWB通信构成机身正面定位系统;在机身正面定位系统中,DSP处理器计算第一UWB定位标签和第二UWB 定位标签连线与坐标系中X轴正向的夹角为机身正面方向角,并以第一UWB定位标签指向第二UWB定位标签的方向为机身正面朝向。
如图3,控制器包括DSP处理器和FPGA处理器,其中,倾斜传感器、雨水传感器与DSP处理器连接,传输外界信号。陀螺仪与DSP处理器连接,提供转向角度信息。控制面板与DSP处理器连接,与用户进行交互。UWB定位标签与DSP处理器连接,提供定位信息给DSP处理器。DSP处理器与所述FPGA处理器相连接,FPGA处理器与电机连接,并发送电机的PWM控制信号给电机。WIFI无线传输模块与DSP处理器相连接,传输无线控制信号。WIFI无线传输模块与控制总站之间通过信号连接。
驱动器与行走电机、割草电机相连,提供电机驱动信号。所述电池与单核主控、驱动器相连,提供主控与电机的能源动力。所述电池与充电对接器10相连。在割草机器人本体与充电定位站对接后,充电对接器10将与充电定位站中的充电系统连接,为电池进行充电。所述充电对接器10安装于机身前部。
参照图4,在机身坐标定位系统中,DSP处理器采用三角定位算法分别获得第一UWB定位标签9a的绝对坐标、第二UWB定位标签9b的绝对坐标以及机身的绝对坐标。算法描述如下:
设UWB定位基站14坐标点为A(Xa,Ya),第一UWB辅助定位基站16a坐标点为 B(Xb,Yb),第二UWB辅助定位基站16b坐标点为C(Xc,Yc),第一UWB定位标签9a坐标点为L1(X1,Y1)和第二UWB定位标签9b坐标点为L2(X2,Y2)。
对于第一UWB定位标签9a,根据UWB三角定位原理,可获得L1点的坐标(X1,Y1)满足以下方程组:
上式中,v为脉冲的传播速度,ta1为脉冲从UWB定位基站14到第一UWB定位标签 9a的传播时间,tb1为脉冲从第一UWB辅助定位基站16a到第一UWB定位标签9a的传播时间,tc1为脉冲从第二UWB辅助定位基站16b到第一UWB定位标签9a的传播时间。通过求解上述方程组可获得第一UWB定位标签9a的坐标(X1,Y1)。
对于第二UWB定位标签9b,根据UWB三角定位原理,可获得L2点的坐标(X2,Y2)满足以下方程组:
上式中,v为脉冲的传播速度,ta2为脉冲从UWB定位基站14到第一UWB定位标签 9a的传播时间,tb2为脉冲从第一UWB辅助定位基站16a到第一UWB定位标签9a的传播时间,tc2为脉冲从第二UWB辅助定位基站16b到第一UWB定位标签9a的传播时间。通过求解上述方程组可获得第一UWB定位标签9a的坐标(X2,Y2)。
计算第一UWB定位标签9a与第二UWB定位标签9b之间连线的中心点L的坐标为
该坐标即为机身1的坐标。
在机身正面定位系统中,控制器11计算第一UWB定位标签9a和第二UWB定位标签9b连线与坐标系中X轴正向的夹角θ为机身正面方向角,并以第一UWB定位标签9a指向第二UWB定位标签9b的方向为机身正面朝向。结合图2对算法描述如下:
设L1与L2连线与X轴正向的夹角为θ,则θ可通过以下公式计算:
对于本文设计的基于DSP+FPGA割草机器人,在电源打开状态下,操作面板先工作,如果确实需要启动割草机器人,使用人员需输入权限密码,割草机器人才可能启动工作,否则割草机器人就待在原地等待权限开启命令。在正常运动状态下,割草机器人通过各种传感器读取外部环境比反馈参数给DSP处理器,由DSP处理器处理后,发送参数给 FPGA(QL1P100),由FPGA(QL1P100)生成四轴差速行驶的直流无刷伺服电机的同步控制 PWM信号,PWM波信号经驱动器放大后驱动直流无刷电机U、V、X、Y向前运动,其运动速度和位移被相对应的磁电编码器反馈给FPGA(QL1P100),由FPGA(QL1P100)根据运行状态参数二次调整四轴同步PWM控制信号以满足实际工作需求。割草机器人在运行过程中,操作面板在线存储并输出当前状态,使得数据直观显示。
参照图5,割草机机器人的程序运行包括以下步骤:
S1,为了防止误操作,本发明采用启动权限保护,当确定需要启动割草机器人时,需输入权限密码,割草机器人才可能启动工作,否则割草机器人就待在原地等待权限开启命令。
S2,割草机器人启动后将先进行初始化。这此过程中将检测各模块工作是否正常,如果有异常情况将发出相关报警,提示人员处理。割草机器人将检测电池电压是否过低,如果电压过低,将提示电量低无法工作,并进入充电模式,交流电源对系统中的蓄电池充电,保证割草机器人有足够的能源完成任务。
S3,初始化后将进入主程序循环。首先将检测UWB定位程序是否正常,如果UWB定位出现丢失,即进入停机自锁模式,割草机器人将锁定在原地,不再移动,直到UWB定位恢复正常。这样能够保证割草机器人不会出现乱跑的现象,确保了安全。
S4,查询控制面板按键及其标志位。这个过程中用户可以使用控制面板和割草机器人交互,比如设定草地地图,设定割草模式,调节割草高度,设置割草任务等等。割草机器人将存储相关信息在主存储器中,并将影响相关的标志位。
S5,查询是否需要出充电站,如果割草机器人在充电站中,并且用户需要割草机器人出充电站,割草机器人将执行出充电站程序。DSP(TMS320F28335)会自动断开连接线与交流电源的连接,割草机器人转为蓄电池供电状态。
S6,查询是否执行割草任务,如果需要执行割草任务,割草机器人将进入割草任务工作模式,否者将进入下个循环。其中,如图6控制所述割草任务运行的方法为:
S6.1,查询割草区域地图,并划分割草区域栅格。
S6.2,检测电池电量是否不足,如果电量不足将执行回充电站程序。
S6.3,检测UWB定位程序是否正常,如果UWB定位出现丢失,即进入停机自锁模式,割草机器人将锁定在原地,不再移动,直到UWB定位恢复正常。这样能够保证割草机器人不会出现乱跑的现象,确保了安全。
S6.4,根据当前位置与栅格地图,通过全覆盖路径算法进行路径规划。
S6.5,根据规划的路径,前进到下一栅格。这一过程中DSP(TMS320F28335)把直流无刷行走电机和转向电机要运转的距离SX转化为加速度、速度和位置参考指令值,然后DSP(TMS320F28335)再结合行走电机和转向电机的磁电传感器的反馈生成驱动直流无刷伺服行走电机和转向电机的驱动信号,驱动信号经功率桥放大后驱动直流无刷伺服行走电机和转向电机以相反的方向运动,在运动过程磁电传感器实时反馈电机的运行参数给 FPGA(QL1P100),根据反馈参数二次微调行走电机和转向电机的PWM控制信号,来进行闭环控制,使得割草机器人按照规划路径行走。
S6.6,记录割草机器人的实际路径,并标记当前栅格为已割。
S6.7,查询是否全部栅格都标注为已割后,如果是则代表割草任务完成,割草机将执行返回充电站程序,如果否则进入下个循环。
S7,特殊情况通过中断服务程序进行,如倾斜传感器、碰撞传感器、雨水传感器将会影响中断标志位。如果中断标志位使能,程序将保存现场,进入中断服务程序。
S8,进入中断服务程序后,将检查相关标志位。如果倾斜传感器标志位使能,则代表割草机器人已经翻倒,此时DSP(TMS320F28335)和FPGA(QL1P100)经内部伺服控制程序调整直流无刷伺服电机U、V、X、Y的PWM输出,立刻停止割刀电机与行走电机的运行,并复位软件,防止事故发生。
S9,如果碰撞传感器标志位使能,则说明前方有障碍物,此时将执行避障程序,DSP(TMS320F28335)和FPGA(QL1P100)经内部伺服控制程序调整直流无刷伺服电机U、V、 X、Y的PWM输出,控制割草机器人在安全范围内停车,割草机器人将后退一段距离,并向右转绕行障碍物。在割草机器人运动过程中,磁电传感器会时刻检测直流无刷伺服电机 U、V、X、Y的运动速度和位移,并反馈给FPGA(QL1P100),由FPGA(QL1P100)二次调整直流无刷电机U、V、X、Y的PWM波控制信号以满足实际需求。在绕过障碍物后割草机器人将继续之前割草工作。
S10,如果雨水传感器标志位使能,则说明已经下雨,此时潮湿的草地已经不适合割草工作,割草机器人将执行返回充电站程序。DSP(TMS320F28335)根据机器人规划的返回充电站路径,把直流无刷伺服电机U、V、X、Y要运转的距离SX转化为加速度、速度和位置参考指令值,然后DSP(TMS320F28335)再结合电机U、V、X、Y的磁电传感器的反馈生成驱动直流无刷伺服电机U、V、X、Y的驱动信号,驱动信号经功率桥放大后驱动直流无刷伺服电机U、V、X、Y以相反的方向运动,在运动过程中磁电传感器实时反馈电机的运行参数给FPGA(QL1P100),根据反馈参数二次微调电机U、V、X、Y的PWM控制信号,来进行闭环控制,使得割草机器人按照规划路径行走。
S11,在割草机器人返回充电站后,割草机器人上的充电对接器将与充电定位站上的充电系统对接。DSP(TMS320F28335)会自动断开连接线与蓄电池的连接,割草机器人转为交流供电状态,交流电源对系统中的蓄电池充电。此时割草机器人入停机自锁模式,割草机器人将锁定在充电站,即使在外力影响下也不再移动,保证充电过程的安全稳定。
以上实施例仅用于说明本发明的设计思想和特点,其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施,本发明的保护范围不限于上述实施例。所以,凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰,均在本发明的保护范围之内。