CN105005249A - 一种全自动四轮双核高速灭火机器人伺服控制器 - Google Patents

一种全自动四轮双核高速灭火机器人伺服控制器 Download PDF

Info

Publication number
CN105005249A
CN105005249A CN201510519612.6A CN201510519612A CN105005249A CN 105005249 A CN105005249 A CN 105005249A CN 201510519612 A CN201510519612 A CN 201510519612A CN 105005249 A CN105005249 A CN 105005249A
Authority
CN
China
Prior art keywords
sensor
motor
firefighting robot
fpga chip
wheel
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201510519612.6A
Other languages
English (en)
Inventor
张好明
刘增量
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
TONGLING UNIVERSITY
Original Assignee
TONGLING UNIVERSITY
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by TONGLING UNIVERSITY filed Critical TONGLING UNIVERSITY
Priority to CN201510519612.6A priority Critical patent/CN105005249A/zh
Publication of CN105005249A publication Critical patent/CN105005249A/zh
Pending legal-status Critical Current

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00Programme-control systems
    • G05B19/02Programme-control systems electric
    • G05B19/04Programme control other than numerical control, i.e. in sequence controllers or logic controllers
    • G05B19/042Programme control other than numerical control, i.e. in sequence controllers or logic controllers using digital processors
    • G05B19/0421Multiprocessor system
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/12Target-seeking control

Abstract

本发明公开了一种全自动四轮双核高速灭火机器人伺服控制器,包括:供电装置、STM32F407芯片、FPGA芯片、传感组件、图像采集组件、三轴加速度计传感器、陀螺仪、方向传感器和电机组件,所述供电装置电性连接所述STM32F407芯片和所述FPGA芯片,所述STM32F407芯片通讯连接所述FPGA芯片且使能所述FPGA芯片。通过上述方式,本发明全自动四轮双核高速灭火机器人伺服控制器通过STM32F407芯片和FPGA芯片形成全自动四轮双核高速灭火机器人伺服控制器,且通过真空吸附装置对地面形成一定的吸附能力,彻底解决灭火机器人行走打滑,结构新颖、控制简单、速度提升、节省空间、调速平稳、抗干扰能力强、稳定性高,在全自动四轮双核高速灭火机器人伺服控制器的普及上有着广泛的市场前景。

Description

—种全自动四轮双核高速灭火机器人伺服控制器
技术领域
[0001] 本发明涉及机器人控制器领域,特别是涉及一种全自动四轮双核高速灭火机器人伺服控制器。
背景技术
[0002] 灭火机器人是一中模拟现实生活中人类发现有害火源并能够自动熄灭火源的一种新型智能机器人。一般情况下,比赛型灭火机器人能够在一间平面结构房子模型里运动,在操作规则指导下以最短的时间找到代表火源的一根蜡烛并将它熄灭。模拟现实家庭中机器人处理火警的过程。蜡烛代表家里燃起的火源,机器人必须找到并熄灭火源。蜡烛火焰的底部将离地面15〜20cm高。蜡烛是直径l-2cm的白蜡烛。蜡烛火焰的确切高度和尺寸是不确定的、变化的,而且由蜡烛条件和周围的环境所决定。蜡烛将随机地放在比赛场地的一个房间里,比赛开始后不管火焰具体是什么尺寸,都要求机器人能发现蜡烛。
[0003] 在真正的比赛中,为了加大比赛难度,比赛场地被分为n*n格的标准模式,最常采用的是8*8格的均匀模式,其比赛场地二维结构如图2所示,灭火机器人将在64格房间里寻找火源并熄灭。在图2的二维搜寻火源地图中,墙的材料是木质一般且可以反光,每块挡墙的长度为60cm长,高度在27-34cm。比赛场地地面是光滑的,场地的地板是黑色的。场地上的任意缝隙都刷成黑色。场地的缝隙不超过5_。一些机器人可能用泡沫,粉末或者其他的物质来熄灭蜡烛的火焰。由于每一个机器人比赛后清洗场地的好坏直接影响到地面情况,故地面不保证在整个比赛过程中都保持绝对黑色。一旦启动,灭火机器人必须在没有人的干预下自己控制导航,而非人工控制,为了考验灭火机器人在搜寻火源过程中的稳定性,其不可以碰撞或接触墙壁,否则将被受到处罚。
[0004] 一台完整的灭火机器人大致分为以下几个部分:
O电机:执行电机是灭火机器人的动力源,它根据微处理器的指令来执行灭火机器人在二维平面上行走的相关动作。
[0005] 2)算法:算法是灭火机器人的灵魂。灭火机器人必须采用一定的智能算法才能准确快速的从一个房间到达另外一个房间的运动,然后发现火源,并开启自身携带的干冰控制器,扑灭火源。
[0006] 3)微处理器:微处理器是灭火机器人的核心部分,是灭火机器人的大脑。灭火机器人所有的信息,包括房间墙壁信息,火源位置信息,和电机状态信息等都需要经过微处理器处理并做出相应的判断。
[0007] 灭火机器人结合了多学科知识,对于提升在校学生的动手能力、团队协作能力和创新能力,促进学生课堂知识的消化和扩展学生的知识面都非常有帮助。国内研发此机器人的单位较多,但是研发的机器人比较落后,研发的灭火机器人结构如图1,长时间运行发现存在着很多安全问题,即:
(I)作为灭火机器人的执行机构采用的多是步进电机,经常会遇到丢失脉冲造成电机失步现象发生,导致对位置的记忆出现错误,灭火机器人无法寻求到火源,或者是灭火后机器人无法回到起始点,
(2)由于采用步进电机,使得机体发热比较严重,有的时候需要进行加装散热装置,使得机器人整体重量增加,
(3)由于采用步进电机,使得系统运转的机械噪声大大增加,不利于环境保护,
(4 )由于采用步进电机,使得系统一般不适合在速度较高的场合运行,高速运动时容易产生振动,有时候可能会接触墙壁,导致寻找火源失败,
(5)由于灭火机器人要频繁的刹车和启动,加重了单片机的工作量,单一的单片机无法满足灭火机器人快速启动和停止的要求,
(6)相对采用的都是一些体积比较大的插件元器件,使得灭火机器人控制系统占用较大的空间,重量相对都比较重,
(7)由于受周围环境不稳定因素干扰,单片机控制器经常会出现异常,引起灭火机器人失控,抗干扰能力较差,
(8)对于两轮灭火机器人寻找火源过程来说,一般要求其两个电机的PWM控制信号要同步,由于受单片机计算能力的限制,单一单片机伺服系统很难满足这一条件,使得灭火机器人行走导航很难控制,特别是对于快速行走时情况更糟糕,
(9)在实际灭火过程中,火源未必处在房间格的中心,导致灭火机器人的行走方向与火源之间有一个夹角,导致灭火消费了大量的干冰,有时候可能会无法熄灭火源,
(10)在实际灭火过程中,由于蜡烛的燃烧,其高度也在发生变化,这与现实中的火源也非常相似,但是一般的灭火机器人携带的干冰灭火器的喷嘴高度是固定的,导致无法有效的扑灭火源,
(11)在实际灭火过程中,普通的光源探测传感器会可能收到外界光源的干扰,导致灭火探寻失败,无法完成任务。
发明内容
[0008] 本发明主要解决的技术问题是提供一种全自动四轮双核高速灭火机器人伺服控制器,通过采用STM32F407+FPGA芯片的全新双核控制器,同时引入真空吸附技术和加速度计传感器、陀螺仪和方向传感器,进一步提高其行走时的稳定性和精准性,充分考虑了电池在这个系统的作用,把控制系统中工作量最大的六轴伺服系统交给FPGA芯片处理,充分发挥FPGA芯片数据处理速度相对较快的特点,把STM32F407从复杂的六轴伺服控制中解脱出来,实现人机界面、房间读取、房间存储、坐标定位等简单功能,在全自动四轮双核高速灭火机器人伺服控制器的普及上有着广泛的市场前景。
[0009] 为解决上述技术问题,本发明提供一种全自动四轮双核高速灭火机器人伺服控制器,包括:供电装置、STM32F407芯片、FPGA芯片、传感组件、图像采集组件、三轴加速度计传感器、陀螺仪、方向传感器和电机组件,
所述供电装置电性连接所述STM32F407芯片和所述FPGA芯片,所述STM32F407芯片通讯连接所述FPGA芯片且使能所述FPGA芯片,所述传感组件、所述图像采集组件、所述三轴加速度计传感器、所述陀螺仪和所述方向传感器均通讯连接所述FPGA芯片,所述FPGA芯片通过所述三轴加速度计传感器、所述陀螺仪和所述方向传感器反馈的灭火机器人的加速度、速度和角度信息得出偏差以便进行位置补偿,所述FPGA芯片的输出端通讯连接并驱动控制所述电机组件,所述电机组件包括微型电机和升降电机,所述微型电机连接有真空抽吸装置和微型真空吸盘,所述微型电机通过所述真空抽吸装置抽吸所述微型真空吸盘内的空气,使所述微型真空吸盘产生内外压力差形成负压,以增加吸附能力、防止打滑,所述升降电机连接有灭火装置,
所述传感组件包括光电传感器,所述图像采集模块通讯连接所述光电传感器的输出端,当所述光电传感器感应到火源信号后触发开启所述图像采集模块。
[0010] 在本发明一个较佳实施例中,所述供电装置为锂电池。
[0011] 在本发明一个较佳实施例中,所述传感组件还包括第一传感器、第二传感器、第三传感器、第四传感器、第五传感器和第六传感器,所述第一传感器、第二传感器、第三传感器、第四传感器、第五传感器、第六传感器均为超声波传感器。
[0012] 在本发明一个较佳实施例中,所述第一传感器与所述第六传感器配合使用感应判断前方的障碍,所述第二传感器与所述第三传感器配合使用感应判断左方的障碍,所述第四传感器与所述第五传感器配合使用感应判断右方的障碍,所述第二传感器与所述第三传感器在不同位置精确测量到房间左侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,所述第四传感器与所述第五传感器在不同位置精确测量到房间右侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,并将所述信号传送至所述FPGA芯片进行精确补偿。
[0013] 在本发明一个较佳实施例中,所述电机组件还包括前左轮电机、前右轮电机、后左轮电机和后右轮电机,所述前左轮电机、前右轮电机、后左轮电机和后右轮电机分别连接控制所述灭火机器人的前左轮、前右轮、后左轮和后右轮的运动。
[0014] 在本发明一个较佳实施例中,所述前左轮电机、前右轮电机、后左轮电机、后右轮电机、微型电机和升降电机均为直流无刷电机。
[0015] 在本发明一个较佳实施例中,还包括光电编码器,所述光电编码器分别设置在所述前左轮电机、前右轮电机、后左轮电机、后右轮电机、微型电机和升降电机上。
[0016] 在本发明一个较佳实施例中,所述电机组件与所述FPGA芯片之间设置有信号放大装置。
[0017] 在本发明一个较佳实施例中,所述FPGA芯片驱动所述电机组件的信号均为PffM波控制信号。
[0018] 在本发明一个较佳实施例中,所述FPGA芯片的内部还设置有上位机系统和运动控制系统,所述上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块和在线输出模块,所述运动控制系统包括基于FPGA六轴同步直流无刷和直流混合伺服控制模块、坐标定位模块、I/O控制模块和图像采集模块,其中,所述基于FPGA六轴同步直流无刷和直流混合伺服控制模块包括基于四轴直流无刷电机灭火机器人搜寻伺服控制模块、单轴真空吸盘吸附伺服控制模块和灭火器单轴升降伺服控制模块。
[0019] 本发明的有益效果是:本发明全自动四轮双核高速灭火机器人伺服控制器具有结构新颖、控制简单、速度提升、节省空间、调速平稳、抗干扰能力强、稳定性高等优点,在全自动四轮双核高速灭火机器人伺服控制器的普及上有着广泛的市场前景。
附图说明
[0020] 为了更清楚地说明本发明实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图,其中:
图1是现有技术基于单片机控制的两轮灭火机器人的原理图;
图2是本发明的全自动四轮双核高速灭火机器人伺服控制器一较佳实施例的房间示意图;
图3是本发明的全自动四轮双核高速灭火机器人一较佳实施例的结构示意图;
图4是本发明的全自动四轮双核高速灭火机器人一较佳实施例的原理图;
图5是本发明的全自动四轮双核高速灭火机器人伺服控制器一较佳实施例的程序图; 图6是本发明的全自动四轮双核高速灭火机器人一较佳实施例的运行方向示意图;
图7是本发明的全自动四轮双核高速灭火机器人一较佳实施例的右转示意图;
图8是本发明的全自动四轮双核高速灭火机器人一较佳实施例的左转示意图。
具体实施方式
[0021] 下面将对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本发明的一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
[0022] 请参阅图2-图8,本发明实施例包括:
一种全自动四轮双核高速灭火机器人伺服控制器,包括:供电装置、STM32F407芯片、FPGA芯片、传感组件、图像采集组件、三轴加速度计传感器、陀螺仪、方向传感器和电机组件,
所述供电装置电性连接所述STM32F407芯片和所述FPGA芯片,所述STM32F407芯片通讯连接所述FPGA芯片且使能所述FPGA芯片,所述传感组件、所述图像采集组件、所述三轴加速度计传感器、所述陀螺仪和所述方向传感器均通讯连接所述FPGA芯片,所述FPGA芯片通过所述三轴加速度计传感器、所述陀螺仪和所述方向传感器反馈的灭火机器人的加速度、速度和角度信息得出偏差以便进行位置补偿,所述FPGA芯片的输出端通讯连接并驱动控制所述电机组件,所述电机组件包括微型电机和升降电机,所述微型电机连接有真空抽吸装置和微型真空吸盘,所述微型电机通过所述真空抽吸装置抽吸所述微型真空吸盘内的空气,使所述微型真空吸盘产生内外压力差形成负压,以增加吸附能力、防止打滑,所述升降电机连接有灭火装置,
所述传感组件包括光电传感器,所述图像采集模块通讯连接所述光电传感器的输出端,当所述光电传感器感应到火源信号后触发开启所述图像采集模块。
[0023] 优选地,所述供电装置为锂电池。
[0024] 优选地,所述传感组件还包括第一传感器、第二传感器、第三传感器、第四传感器、第五传感器和第六传感器,所述第一传感器、第二传感器、第三传感器、第四传感器、第五传感器、第六传感器均为超声波传感器。
[0025] 优选地,所述第一传感器与所述第六传感器配合使用感应判断前方的障碍,所述第二传感器与所述第三传感器配合使用感应判断左方的障碍,所述第四传感器与所述第五传感器配合使用感应判断右方的障碍,所述第二传感器与所述第三传感器在不同位置精确测量到房间左侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,所述第四传感器与所述第五传感器在不同位置精确测量到房间右侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,并将所述信号传送至所述FPGA芯片进行精确补偿。
[0026] 优选地,所述电机组件还包括前左轮电机、前右轮电机、后左轮电机和后右轮电机,所述前左轮电机、前右轮电机、后左轮电机和后右轮电机分别连接控制所述灭火机器人的前左轮、前右轮、后左轮和后右轮的运动。
[0027] 优选地,所述前左轮电机、前右轮电机、后左轮电机、后右轮电机、微型电机和升降电机均为直流无刷电机。
[0028] 优选地,还包括光电编码器,所述光电编码器分别设置在所述前左轮电机、前右轮电机、后左轮电机、后右轮电机、微型电机和升降电机上。
[0029] 优选地,所述电机组件与所述FPGA芯片之间设置有信号放大装置。
[0030] 优选地,所述FPGA芯片驱动所述电机组件的信号均为PffM波控制信号。
[0031] 优选地,所述FPGA芯片的内部还设置有上位机系统和运动控制系统,所述上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块和在线输出模块,所述运动控制系统包括基于FPGA六轴同步直流无刷和直流混合伺服控制模块、坐标定位模块、I/O控制模块和图像采集模块,其中,所述基于FPGA六轴同步直流无刷和直流混合伺服控制模块包括基于四轴直流无刷电机灭火机器人搜寻伺服控制模块、单轴真空吸盘吸附伺服控制模块和灭火器单轴升降伺服控制模块。
[0032] STM32F4系列除引脚和软件兼容高性能的F2系列外,F4的主频(168MHz)高于F2系列(120MHz),并支持单周期DSP指令和浮点单元、更大的SRAM容量(192 KB, F2为128KB)、512KB-1MB的嵌入式闪存以及影像、网络接口和数据加密等更先进的外设。STM32F4系列基于最新的ARM Cortex M4内核,在现有出色的STM32微控制器产品组合中新增了信号处理功能,并提高了运行速度;STM32F405x集成了定时器、3个ADC、2个DAC、串行接口、夕卜存接口、实时时钟、CRC计算单元和模拟真随机数发生器在内的整套先进外设。STM32F407在STM32F405产品基础上增加了多个先进外设。这些性能使得F4系列可以较容易满足控制和信号处理功能混合的数字信号控制需求。高效的信号处理功能与CorteX-M4处理器系列的低能耗、低成本和易于使用的优点的组合,使得其可以为多轴电动机控制提供灵活解决方案。这些特点使得STM32F407特别适合多轴灭火机器人伺服系统的信号处理。
[0033] FPGA芯片是英文Field Programmable Gate Array的缩写,即现场可编程门阵列,是在PAL、GAL、EPLD等可编程器件的基础上进一步发展的产物。它是作为专用集成电路(ASIC)领域中的一种半定制电路而出现的,即解决了定制电路的不足,又克服了原有可编程器件门电路数有限的缺点。
[0034] FPGA芯片采用了逻辑单元阵列LCA (Logic Cell Array)这样一个新概念,内部包括可配置逻辑模块 CLB (Configurable Logic Block)、输出输入模块 1B (Input OutputBlock)和内部连线(Interconnect)三个部分。FPGA芯片的基本特点主要有:
I)采用FPGA芯片设计ASIC电路,用户不需要投片生产,就能得到合用的芯片。
[0035] 2) FPGA芯片可做其它全定制或半定制ASIC电路的中试样片。
[0036] 3) FPGA芯片内部有丰富的触发器和I / O引脚。
[0037] 4) FPGA芯片是ASIC电路中设计周期最短、开发费用最低、风险最小的器件之一。
[0038] 5) FPGA芯片采用高速CHMOS工艺,功耗低,可以与CMOS、TTL电平兼容。
[0039] 可以说,FPGA芯片是小批量系统提高系统集成度、可靠性的最佳选择之一。
[0040] 上述特点使得用户可以根据自己的设计需要,通过特定的布局布线工具对其内部进行重新组合连接,在最短的时间内设计出自己的专用集成电路,这样就减小成本、缩短开发周期。由于FPGA芯片采用软件化的设计思想实现硬件电路的设计,这样就使得基于FPGA芯片设计的系统具有良好的可复用和修改性。这种全新的设计思想已经逐渐应用在高性能的直流电机和交流电机驱动控制上,并快速发展。
[0041] 为了能够精确探寻房间并最终成功找到火源,本发明采用六组传感器探测房间模式,所发明的灭火机器人二维结构如图3所示:传感器S1、S6共同作用判断前方挡墙,传感器S2和S3共同合作判断其左边挡墙的存在,传感器S4和S5共同合作判断其右边挡墙的存在,同时S2、S3、S4、S5合作为灭火机器人直线运动提供导航依据。由于在比赛现场,太阳光有可能直射进来,这样红外传感器就不能在使用,本发明采用超声波传感器代替了红外传感器。在这种结构设置中,S2和S3可以在不同位置精确测量到房间左侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,S4和S5可以在不同位置精确测量到房间右侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,这个位置的传感器信号阶跃变化可以被控制器捕捉到,然后在此位置可以对灭火机器人进行精确补偿,这对于灭火机器人求解房间找到火源并返回到起点计算至关重要,如果没有此智能补偿的话,灭火机器人有可能在复杂房间中的累计误差足以使其无法求解此房间地图,导致无法回到房间起点。
[0042] 为了提高四轮灭火机器人在寻找火源过程中行走导航的稳定性,本发明在灭火机器人伺服硬件系统中加入了三轴加速度计传感器Al、陀螺仪Gl和方向传感器D1。在灭火机器人行走房间期间全程开启加速度计传感器Al、陀螺仪Gl和方向传感器Dl,加速度计传感器Al、陀螺仪Gl和方向传感器Dl用来测量灭火机器人三个前进方向的加速度、速度和角度。控制器根据测得的角度信号求出偏差。当灭火机器人的姿态发生变化超过设定阀值时,在一个新的采样周期控制器就立即对其位置补偿,避免灭火机器人远远偏离中心位置而出现撞墙现象的发生,提高了其快速行走导航的稳定性。
[0043] 为了提高灭火机器人全数字伺服系统的稳定性,防止灭火机器人在高速行走时打滑导致灭火机器人房间信息错误,本发明在灭火机器人伺服硬件系统中加入了微型直流电机M,在灭火机器人运动过程中,电机M通过真空抽吸装置不停抽吸微型真空吸盘内的空气,使微型真空吸盘的内外压力不一样,产生一定的负压,使其对房间地面产生一定的吸附能力,即使房间地板受到了上一组选手的破坏产生了一定变化,灭火机器人也不会受到影响,有效防止了灭火机器人在高速行走时的地面打滑。
[0044] 为了能够准确的采集火源信,为灭火机器人提供正确的火源信息号并熄灭火源,本发明在普通的光电采集基础上,加入了图像采集系统,如果普通光电传感器采集到了火源信号,控制器开启图像采集系统,然后控制器控制两个行走伺服电机运动方向相反,灭火机器人原地旋转一个角度,使得图像采集系统能够精确的采集火源信息;火源信息确定后,控制器根据图像采集结果控制另外一台升降直流电机E工作,根据火源高度自动升高或降低干冰灭火器的高度,使干冰喷洒高度刚好对准火源中心。
[0045] 本发明为克服单片机不能满足两轴灭火机器人行走的稳定性,进一步提高灭火机器人行走的速度,舍弃了国产灭火机器人所采用的单一单片机工作模式,在吸收国外先进控制思想的前提下,自主发明了基于STM32F407+FPGA芯片的四轮双核全新控制模式。控制板以FPGA芯片为处理核心,实现四轴直流无刷电机和两轴直流电机的六轴伺服控制的数字信号实时处理,并响应各种中断,实现数据信号的实时存储。双核控制器把STM32F407从复杂的工作当中解脱出来,实现房间信息读取、房间存储、I/O控制等简单部分的信号处理,并响应FPGA芯片中断,实现二者之间的数据通信和存储实时信号。
[0046] 为达上述目的,本发明采取以下技术方案,为了提高运算速度,保证四轮灭火机器人系统的稳定性和可靠性,本发明在基于STM32F407的控制器中舍弃了专用精密运动控制专用芯片,而引入FPGA芯片,形成基于STM32F407+FPGA芯片的全新双核控制器。双核控制器同时引入真空吸附技术和加速度计传感器、陀螺仪和方向传感器,进一步提高其行走时的稳定性和精准性。此控制器充分考虑电池在这个系统的作用,把控制系统中工作量最大的六轴伺服系统交给FPGA芯片处理,充分发挥FPGA芯片数据处理速度相对较快的特点,把STM32F407从复杂的六轴伺服控制中解脱出来,实现人机界面、房间读取、房间存储、坐标定位等简单功能。
[0047] 参照图5,具体实施步骤是:
对于本文设计的STM32F407+FPGA芯片控制器,在电源打开状态下,灭火机器人先进入自锁状态。控制器首先开启真空抽吸电机M,通过抽吸装置先对微型真空吸盘抽吸,使真空吸盘对地面具有一定的吸附能力。灭火机器人通过超声波传感器S1、S6判断前方运动环境,实际导航环境被转化为控制参数传输给FPGA芯片,FPGA芯片把这些环境参数转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4和电机光电编码器的反馈生成控制直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动,加速度计Al和光电编码器实时把测得的信号反馈给FPGA芯片,由FPGA芯片二次校正迷宫机器人的姿态。灭火机器人在运动过程中,FPGA芯片根据灭火机器人运动速度自动通过电机M调节真空装置对地面的吸附能力,增加有效摩擦,防止灭火机器人快速行走打滑,STM32F407实时储存房间信息。当控制器发现火源后,图像采集系统开启。控制器在图像采集帮助下,自动调整灭火器喷嘴与火源的角度,同时升降电机E自动调节灭火器的高度,然后自动开启干冰灭火器电磁阀,通过喷洒干冰灭火,灭火完毕后STM32F407立即调出灭火机器人已经储存的房间信息,通过洪水算法找出返程最短路径,并开启加速模式迅速回到起点等待一下条寻求火源命令。
[0048] 参照图4、图5,图6、图7、图8,其具体的功能实现如下:
I)为了能够驱动两轮灭火机器人运动并解决高速探寻房间打滑问题,本控制系统引入了 FPGA芯片,由其产生六路控制直流无刷电机和直流电机的PffM波控制信号,FPGA芯片通过I/O 口与STM32F407进入实时通讯,由STM32F407控制其开通和关断。
[0049] 2)打开电源瞬间,STM32F407会对电池电压进行检测,如果低压的话,FPGA芯片将封锁直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM波控制信号,电机不能启动,同时电压传感器Vl将工作,并发出报警信号。如果系统电压正常,FPGA芯片将使能真空抽吸电机M的PffM波控制信号,通过抽吸装置先对微型真空吸盘抽吸,使真空吸盘对地面具有一定的吸附能力,满足灭火机器人探寻火源的速度要求。
[0050] 3)在灭火机器人未接到探索命令之前,它一般会在起点坐标(0,0)等待控制器发出的探索命令,一旦接到任务后,会沿着起点开始为了寻找火源而进行全宫探索。
[0051] 4)灭火机器人放在起点坐标(0,0),一般情况下,灭火机器人按照图6中北的方向(计算机编程代码为O)放置,接到任务后其前方的传感器S1、S6和会对前方的环境进行判断,确定有没有挡墙进入运动范围,如存在挡墙将向STM32F407发出存储命令,STM32F407会对中断做第一时间响应,然后FPGA芯片禁止后驱的两轮PffM波控制信号输出,并同时使能中驱两路的PWM波控制信号输出,通过调整FPGA芯片的输出控制直流无刷电机X正转,直流无刷电机Y反转,灭火机器人在加速度传感器Al、陀螺仪Gl和方向传感器Dl的控制下向右旋转90度,灭火机器人首先沿着X轴正向(东的方向,计算机编程代码为2)搜寻火源。
[0052] 5)在灭火机器人运动过程中,装在直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R上的光电编码器会输出其位置信号A和位置信号B并反馈给FPGA芯片,光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次,FPGA芯片的位置寄存器会根据直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的运行方向加I或者是减I ;
6)在灭火机器人运动过程中,装在直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R上的光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时,就产生一个INDEX信号给FPGA芯片寄存器,记录电机的绝对位置,然后换算成灭火机器人在房间中的具体位置。
[0053] 7)为了能够实现灭火机器人准确的坐标计算功能,灭火机器人左右的传感器S2、S3和S4、S5会时刻对运动方向左右的房间挡墙和柱子进行探测,如果S2、S3或者S4、S5发现传感器信号发生了跃变,则说明灭火机器人进入了从有房间挡墙到无房间挡墙(或者是从无房间挡墙到有房间挡墙)状态的变化,STM32F407会根据灭火机器人当前运行状态精确补偿,彻底消除灭火机器人在复杂房间中探寻火源时已经累计的误差。
[0054] 8)在灭火机器人沿着任何一个方向向前运动,在任何一个方格的中心如果确定没有挡墙进入前方的运动范围,则灭火机器人将存储其坐标(X,Y),并把向前运动一格的位置参数送给FPGA芯片,FPGA芯片把向前一格参数转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2和电机光电编码器的反馈生成控制直流无刷电机X和直流无刷电机Y的PWM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y向前运动。在灭火机器人沿着当前房间格向前探索过程中,传感器S2、S3和S4、S5会对左右的挡墙进行判断,并记录储存当前搜寻房间挡墙信息,灭火机器人根据前进方向左右挡墙的房间信息进入单墙导航模式或者是双墙导航模式,然后再结合设定的左右挡墙导航阀值,加速度计Al、陀螺仪Gl和方向传感器Dl记录灭火机器人实时的加速度、速度和位置信号并送给控制器,FPGA芯片记录储存灭火机器人的瞬时加速度、速度和位置信息,当灭火机器人快速探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差由FPGA芯片转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X和直流无刷电机Y的PWM控制信号,控制信号经驱动桥放大后驱动电机X和电机Y向前运动。通过此方式可以精确调整灭火机器人的姿态,使其重新回到设定中心位置。如果灭火机器人搜寻火源过程中出现失速或者房间地面灰尘较多的情况,FPGA芯片会调节M加大灭火机器人与地面的摩擦并使能后驱的两轮PWM波控制信号,控制器开启直流无刷电机Z和直流无刷电机R,系统进入四轮驱动状态,FPGA芯片根据时间和速度要求,把剩余的距离D转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4、电机光电编码器的反馈,生成直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。灭火机器人在四驱状态下依旧按照原有的导航模式前进,加速度计Al、陀螺仪Gl和方向传感器Dl记录灭火机器人实时的加速度、速度和位置信号并送给控制器,FPGA芯片记录储存灭火机器人的瞬时加速度、速度和位置信息,当灭火机器人四轮探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差由FPGA芯片转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动电机X和电机Y向前运动。通过此方式可以精确调整灭火机器人的姿态,使其重新回到设定中心位置;当灭火机器人在加速度计Al的控制下运动一格距离到达新地址时,微处理器将更新其坐标信息。
[0055] 如果在坐标(X,Y)时的方向为北,在更新其坐标为(X,Y+1),新坐标方向依旧为北;如果在坐标(X,Y)时的方向为东,在更新其坐标为(Χ+1,Y),新坐标方向依旧为东;如果在坐标(Χ,γ)时的方向为南,在更新其坐标为(X,Υ-1),新坐标方向依旧为南;如果在坐标(X,Y)时的方向为西,在更新其坐标为(x-l,Y),新坐标方向依旧为西;
9)在灭火机器人沿着当前方向向前运动过程中如果传感器SI和S6判断前方有挡墙进入运动范围,并且此时传感器S2、S3、S4、S5分别判断左右都有挡墙时,灭火机器人将存储此时坐标(X,Y),根据传感器SI和S6的反馈计算出向前运动停车的位置参数YS1,由FPGA芯片根据探索控制器速度和加速度要求把向前停车距离参数转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Ζ、直流无刷电机R要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4和电机光电编码器的反馈生成控制直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。在灭火机器人沿着当前房间格向前探索过程中,传感器S2、S3和S4、S5对左右挡墙进行判断,并记录储存当前搜寻房间挡墙信息,灭火机器人根据前进方向左右挡墙的房间信息进入双墙导航模式,然后再结合设定的左右挡墙导航阀值,加速度计Al、陀螺仪Gl和方向传感器Dl记录灭火机器人实时的加速度、速度和位置信号并送给控制器,FPGA芯片记录储存灭火机器人的瞬时加速度、速度和位置信息,当灭火机器人快速停车脱离了设定中心位置时,微处理器根据离开中心位置的偏差由FPGA芯片转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。通过此方式可以精确调整灭火机器人的姿态,使其重新回到设定中心位置。灭火机器人实现在设置停车点停车。FPGA芯片重新调整灭火机器人四个电机的PffM波输出,首先禁止后轮的两路PWM波工作,同时使能中驱的两轮PffM波输出,使中驱直流无刷电机X和直流无刷电机Y运动方向相反,灭火机器人原地在加速计传感器Al、陀螺仪Gl和方向传感器Dl的控制下实现精确的原地180度转向,然后灭火机器人沿着原先相反的方向运动。
[0056] 如果在坐标(X,Y)时的方向为北,则更新其坐标为(X,Y),新坐标方向为南;如果在坐标(Χ,γ)时的方向为东,则更新其坐标为(XI,Y),新坐标方向为西;如果在坐标(X,Y)时的方向为南,则更新其坐标为(X,Y),新坐标方向为北;如果在坐标(X,Y)时的方向为西,则更新其坐标为(X,Y),新坐标方向为东;
10)在灭火机器人沿着当前方向向前运动过程中如果有挡墙进入前方的运动范围,并且此时传感器S2和S3判断左边有挡墙,而S4、S5判断右方没有挡墙时,灭火机器人将存储此时坐标(X,Y),然后灭火机器人将按照图7的曲线行走;为了充分发挥FPGA芯片的快速处理数据能力,舍弃了基于MC58113和LM629的三段法转入法,为了使转弯更逼近于实际情况,本发明采用五段法完成转弯:转入前直线校正R90_Leading,转入弧度校正ARClUfA弧度ARC12,转出弧度校正ARC13,转出直线校正R90_Passing。
[0057] 在右转弯时,FPGA芯片首先把行走直线很短的距离R90_Leading按照控制器不同搜寻速度和加速度要求生成灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4和电机光电编码器的反馈生成控制直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PWM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。在灭火机器人沿着当前房间格向前探索过程中,传感器S2、S3会对左挡墙进行判断,并记录储存当前搜寻房间挡墙信息,灭火机器人根据前进方向左挡墙的房间信息进入单左墙导航模式,然后再结合设定的左挡墙导航阀值,加速度计Al、陀螺仪Gl和方向传感器Dl记录灭火机器人实时的加速度、速度和位置信号并送给控制器,FPGA芯片记录储存灭火机器人的瞬时加速度、速度和位置信息,当灭火机器人快速探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差由FPGA芯片转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。通过此方式可以精确调整灭火机器人的姿态,使其重新回到设定中心位置。
[0058] 当到达既定目标时,传感器参考值R90_FrontWallRef开始工作,防止外界干扰开始做误差补偿。误差补偿结束后,控制器释放后驱的两个直流无刷电机,通过中驱的两个直流无刷电机进行转弯。控制器开始调整直流无刷电机X和直流无刷电机Y速度使其完成弧度ARC的运动曲线.FPGA芯片首先把弧度ARCll按照控制器不同搜寻速度和加速度要求转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2和电机光电编码器的反馈生成控制电机X和电机Y的PffM控制信号,PWM波控制信号通过驱动桥放大后推动灭火机器人完成转弯搜寻。在灭火机器人转弯搜寻过程中,传感器S2、S3、S4、S5无法为系统提供位置参考,系统依靠方向传感器DI进行位置修正。在灭火机器人快速搜寻转弯过程中方向传感器Dl实时记录其瞬时角度,控制器然后与设定位置的角度对比,当灭火机器人快速搜寻脱离了设定位置时,在新的采样周期内,FPGA芯片把偏差大小转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X和直流无刷电机Y的PffM控制信号,两轴伺服系统开始进行实时补偿来调整灭火机器人的姿态,使其完成弧度ARCll ;
当完成ARCll转入前校正后,FPGA芯片首先把弧度ARC12按照控制器不同搜寻速度和加速度要求转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2和电机光电编码器的反馈生成控制电机X和电机Y的PWM控制信号,PWM波控制信号通过驱动桥放大后推动灭火机器人完成转弯搜寻。在灭火机器人转弯搜寻过程中,系统依靠方向传感器Dl进行位置修正。在灭火机器人快速搜寻转弯过程中方向传感器Dl实时记录其瞬时角度,控制器然后与设定位置的角度对比,当灭火机器人快速搜寻脱离了设定位置时,在新的采样周期内,FPGA芯片把偏差大小转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X和直流无刷电机Y的PffM控制信号,两轴伺服系统开始进行实时补偿来调整灭火机器人的姿态,使其完成弧度ARC12 ;
当完成弧度ARC12转弯后,灭火机器人为弧度转出做准备。FPGA芯片把弧度ARC13按照控制器不同搜寻速度和加速度要求转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2和电机光电编码器的反馈生成控制电机X和电机Y的PWM控制信号,PffM波控制信号通过驱动桥放大后推动灭火机器人完成转弯搜寻。在灭火机器人转弯转出过程中,系统依靠方向传感器Dl进行位置修正。在灭火机器人快速搜寻转弯过程中方向传感器Dl实时记录其瞬时角度,控制器然后与设定位置的角度对比,当灭火机器人快速搜寻脱离了设定位置时,在新的采样周期内,FPGA芯片把偏差大小转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器Cl、C2、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X和直流无刷电机Y的PffM控制信号,两轴伺服系统开始进行实时补偿来调整灭火机器人的姿态,使其完成弧度 ARC13 ;
当到达既定目标后,系统依靠传感器S2、S3开始导航,控制器把直线行走很短的距离R90_Passing按照控制器不同搜寻速度和加速度要求生成灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4和电机光电编码器的反馈生成控制直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。在灭火机器人沿着当前房间格向前探索过程中,传感器S2、S3会对左挡墙进行判断,并记录储存当前搜寻房间挡墙信息,灭火机器人根据前进方向左挡墙的房间信息进入单左墙导航模式,然后再结合设定的左挡墙导航阀值,加速度计Al、陀螺仪Gl和方向传感器Dl记录灭火机器人实时的加速度、速度和位置信号并送给控制器,FPGA芯片记录储存灭火机器人的瞬时加速度、速度和位置信息,当灭火机器人快速探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差由FPGA芯片转化为灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的新的位置、速度和加速度指令值,FPGA芯片再结合电流传感器C1~C4、电机光电编码器、加速计传感器Al、陀螺仪Gl和方向传感器Dl的反馈,微调直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R的PffM控制信号,控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动。通过此方式可以精确调整灭火机器人的姿态,使其重新回到设定中心位置。当到达既定目标后完成整个右转弯的轨迹曲线运动,然后控制其开始更新其坐标和方向。
[0059] 如果在坐标(X,Y)时的方向为北,则更新其坐标为(X+1,Y),新坐标方向为东;如果在坐标(X,Y)时的方向为东,则更新其坐标为(X,Y-1),新坐标方向为南;如果在坐标(X,Y)时的方向为南,则更新其坐标为(X-1,Y),新坐标方向为西;如果在坐标(X,Y)时的方向为西,则更新其坐标为(X,Y+1),新坐标方向为北;
11)在灭火机器人沿着当前方向向前运动过程中如果有挡墙进入前方的运动范围,并且此时左右的传感器S2和S3判断左边无挡墙,而S4、S5判断右方有挡墙时,灭火机器人将存储此时坐标(X,Y),然后灭火机器人将按照图8的曲线行走;为了充分发挥FPGA芯片的快速处理数据能力,舍弃了基于MC58113或者是LM629的三段法转入法,为了使转弯更逼近于实际情况,本发明采用五段法完成转弯:转入前直线校正L90_Leading,转入弧度校正ARC21,转入弧度ARC22,转出弧度校正ARC23,转出直线校正L90_Passing。
[0060] 在左转弯时,控制器控制电机行走规律与右转类似,当通过五段法到达既定目标后完成整个左转弯的轨迹曲线运动,然后控制器按照图6的设置开始更新其坐标和方向。
[0061] 如果在坐标(X,Y)时的方向为北,则更新其坐标为(X-1,Y),新坐标方向为西;如果在坐标(X,Y)时的方向为东,则更新其坐标为(X,Y+1),新坐标方向为北;如果在坐标(X,Y)时的方向为南,则更新其坐标为(X+1,Y),新坐标方向为东;如果在坐标(X,Y)时的方向为西,则更新其坐标为(X,Y-1 ),新坐标方向为南;
12)当灭火机器人到达一个新的房间格后,光电传感器S7开始工作,对新坐标下的光源进行判断,如果光电传感器捕捉到信号将通知STM32F407发现目标,STM32F407会开启图像采集系统进行光源分析,经确定火源无误后,图像采集系统再次工作:首先控制器根据图像采集系统确定干冰灭火器喷嘴与蜡烛之间的角度,STM32F407按照旋转角度、角加速度要求,把此角度转化为角度、角速度、角加速度指令值,然后与FPGA芯片通讯,FPGA芯片结合电机电流Cl、C2和光电编码器的反馈,自动调整直流无刷电机X和电机Y的PWM波控制信号,使得电机X正转,电机Y反转,在加速度计Al、陀螺仪Gl和方向传感器Dl控制下灭火机器人开始原地旋转调整喷嘴与蜡烛之间的角度,当到达设定角度后,控制器使得灭火机器人原地自锁;
STM32F407根据图像采集系统确定喷嘴与火源之间的高度差,STM32F407按照距离、速度、加速度要求,把此高度差转化为位置、速度、加速度指令值,然后与FPGA芯片通讯,FPGA芯片结合电机E的电和光电编码器的反馈,自动调整直流电机E的PffM波控制信号,然后驱动直流电机E工作,使得灭火器的喷嘴高度与蜡烛火源高度一致,控制器开启携带的干冰灭火器的电磁阀,开始对蜡烛进行喷洒干冰直至光源消失,图像采集系统二次判断火源,确定灭火完成后,控制器关闭图像采集系统;STM32F407通过FPGA芯片自动调整直流无刷电机X和电机Y的PWM波控制信号,使得电机X反转,电机Y正转,在加速度计Al、陀螺仪Gl和方向传感器Dl控制下灭火机器人开始原地旋转,并恢复到刚入宫的位置;
如果光电传感器S7没有捕捉到新坐标下的光源,灭火机器人将离开目前房间格,将继续搜寻并更新其坐标;
13)当灭火机器人搜到光源,并喷洒干冰完成灭火后,灭火机器人会停在目标点,然后控制器调出灭火机器人已经搜索的路径,并舍弃未搜寻的目标,通过洪水算法找出已经搜寻过房间的最佳路径,然后灭火机器人按照此路径快速回到搜寻起点;
14)在灭火机器人行走过程总,,控制器并实时检测灭火机器人的行走速度,并根据地面情况系统会自动调节电机M加大真空吸盘对地面的吸附能力。
[0062] 15)在灭火机器人房间搜寻过程中,FPGA芯片会对高速直流无刷电机X、直流无刷电机Y、直流电机Z、之路电机R、直流电机M、直流电机E的转矩进行在线辨识,由于系统采用的是三闭环伺服控制,如果转矩出现脉动,控制器会利用直流电机力矩与电流的关系对干扰进行线性补偿,有效减少了电机转矩抖动对灭火机器人快速探索时导航的影响,增加了其抗干扰能力。
[0063] 16)当灭火机器人完成整个返程过程回到起始点(0,0),STM32F407将控制FPGA芯片使得灭火机器人中心点停车,并重新调整FPGA芯片驱动信号,使得直流无刷电机X和直流无刷电机Y以相反的方向运动,灭火机器人加速度计传感器Al的控制下,原地旋转180度,原地自锁,等待下一个搜寻命令。
[0064] 本发明全自动四轮双核高速灭火机器人伺服控制器的有益效果是:
1、在运动过程中,充分考虑了电池在这个系统中的作用,基于STM32F407+FPGA芯片控制器时刻都在对灭火机器人的运行状态进行监测和运算,并且在电池提供电源的过程中,电流传感器C1~C6时刻对电池的电流进行观测并送给控制器进行保护,避免了大电流的产生,所以从根本上解决了大电流对锂离子电池的冲击,避免了由于大电流放电而引起的锂离子电池过度老化现象的发生;
2、用直流无刷电机替代了步进电机,使得电机无机械摩擦,无磨损,无电火花,且免维护,而且直流无刷电机的效率高,功率和转矩密度高,使得系统的效率更高;
3、由FPGA芯片处理灭火机器人的直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R和两轴直流电机M和直流电机E的伺服控制,使得控制比较简单,大大提高了运算速度,解决了单片机软件运行较慢的瓶颈,缩短了开发周期短,并且程序可移植能力强;
4、本发明基本实现全贴片元器件材料,实现了单板控制,不仅节省了控制板占用空间,而且有利于灭火机器人体积和重量的减轻;
5、由于采用直流无刷电机,使得系统带载能力更强,调速范围比较宽,调速比较平稳;
6、由于本控制器采用FPGA芯片处理六轴伺服大量的数据与算法,有效地防止了程序的“跑飞”,抗干扰能力大大增强; 7、在灭火机器人实际运动过程中,FPGA芯片可以根据机器人外围运行情况适时调整其内部伺服控制的PID参数,实现分段P、H)、PID控制和非线性PID控制,使系统满足中低速运行时速度的切换;
8、在此四轮灭火机器人系统引入了三轴加速度计Al、陀螺仪Gl和方向传感器D1,可以准确得到灭火机器人的加速度、速度和位置信息,实现了灭火机器人在房间探索时的瞬时加速度、速度和位置的直接检测,并利用直接反馈实现全程导航和二次补偿,有利于提高灭火机器人的稳定性和动态性能;
9、在灭火机器人运行过程中,FPGA芯片会对直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、单轴吸附控制直流电机M和灭火器升降电机E的转矩进行在线辨识并利用电机力矩与电流的关系进行补偿,减少了电机转矩抖动对灭火机器人快速探寻火源的影响;
10、通过调节直流电机M可以有效调节真空吸盘对地面的吸附力,消除了灭火机器人在高速探寻火源时打滑现象的发生;
11、由FPGA芯片根据探索火源的速度、加速度要求把外界偏差转化为各个电机控制的位置、速度和加速度给定,再结合光电编码器和电流传感器的反馈输出PWM调制信号和方向信号,通过驱动电路可以直接驱动电机,极大的提高了运算速度;
12、图像采集的加入可以有效捕捉火源,减少外界干扰对机器人的误操作;
13、调节直流无刷电机X和直流无刷电机Y的运动方向相反,通过方向传感器Dl可以得到灭火机器人旋转的角度,使得机器人携带的灭火器与火源处于一条直线上,可以有效熄灭火源;
14、通过调整直流电机E的伺服控制可以调整灭火器的高度,使得灭火器喷嘴与火源中心高度一致,有利于有效扑灭火源。
[0065] 以上所述仅为本发明的实施例,并非因此限制本发明的专利范围,凡是利用本发明说明书内容所作的等效结构或等效流程变换,或直接或间接运用在其它相关的技术领域,均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种全自动四轮双核高速灭火机器人伺服控制器,其特征在于,包括:供电装置、STM32F407芯片、FPGA芯片、传感组件、图像采集组件、三轴加速度计传感器、陀螺仪、方向传感器和电机组件, 所述供电装置电性连接所述STM32F407芯片和所述FPGA芯片,所述STM32F407芯片通讯连接所述FPGA芯片且使能所述FPGA芯片,所述传感组件、所述图像采集组件、所述三轴加速度计传感器、所述陀螺仪和所述方向传感器均通讯连接所述FPGA芯片,所述FPGA芯片通过所述三轴加速度计传感器、所述陀螺仪和所述方向传感器反馈的灭火机器人的加速度、速度和角度信息得出偏差以便进行位置补偿,所述FPGA芯片的输出端通讯连接并驱动控制所述电机组件,所述电机组件包括微型电机和升降电机,所述微型电机连接有真空抽吸装置和微型真空吸盘,所述微型电机通过所述真空抽吸装置抽吸所述微型真空吸盘内的空气,使所述微型真空吸盘产生内外压力差形成负压,以增加吸附能力、防止打滑,所述升降电机连接有灭火装置, 所述传感组件包括光电传感器,所述图像采集模块通讯连接所述光电传感器的输出端,当所述光电传感器感应到火源信号后触发开启所述图像采集模块。
2.根据权利要求1所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述供电装置为锂电池。
3.根据权利要求1所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述传感组件还包括第一传感器、第二传感器、第三传感器、第四传感器、第五传感器和第六传感器,所述第一传感器、第二传感器、第三传感器、第四传感器、第五传感器、第六传感器均为超声波传感器。
4.根据权利要求3所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述第一传感器与所述第六传感器配合使用感应判断前方的障碍,所述第二传感器与所述第三传感器配合使用感应判断左方的障碍,所述第四传感器与所述第五传感器配合使用感应判断右方的障碍,所述第二传感器与所述第三传感器在不同位置精确测量到房间左侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,所述第四传感器与所述第五传感器在不同位置精确测量到房间右侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化,并将所述信号传送至所述FPGA芯片进行精确补偿。
5.根据权利要求1所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述电机组件还包括前左轮电机、前右轮电机、后左轮电机和后右轮电机,所述前左轮电机、前右轮电机、后左轮电机和后右轮电机分别连接控制所述灭火机器人的前左轮、前右轮、后左轮和后右轮的运动。
6.根据权利要求5所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述前左轮电机、前右轮电机、后左轮电机、后右轮电机、微型电机和升降电机均为直流无刷电机。
7.根据权利要求5所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,还包括光电编码器,所述光电编码器分别设置在所述前左轮电机、前右轮电机、后左轮电机、后右轮电机、微型电机和升降电机上。
8.根据权利要求1所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述电机组件与所述FPGA芯片之间设置有信号放大装置。
9.根据权利要求1所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述FPGA芯片驱动所述电机组件的信号均为PffM波控制信号。
10.根据权利要求1所述的全自动四轮双核高速灭火机器人伺服控制器,其特征在于,所述FPGA芯片的内部还设置有上位机系统和运动控制系统,所述上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块和在线输出模块,所述运动控制系统包括基于FPGA六轴同步直流无刷和直流混合伺服控制模块、坐标定位模块、I/O控制模块和图像采集模块,其中,所述基于FPGA六轴同步直流无刷和直流混合伺服控制模块包括基于四轴直流无刷电机灭火机器人搜寻伺服控制模块、单轴真空吸盘吸附伺服控制模块和灭火器单轴升降伺服控制模块。
CN201510519612.6A 2015-08-24 2015-08-24 一种全自动四轮双核高速灭火机器人伺服控制器 Pending CN105005249A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510519612.6A CN105005249A (zh) 2015-08-24 2015-08-24 一种全自动四轮双核高速灭火机器人伺服控制器

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510519612.6A CN105005249A (zh) 2015-08-24 2015-08-24 一种全自动四轮双核高速灭火机器人伺服控制器

Publications (1)

Publication Number Publication Date
CN105005249A true CN105005249A (zh) 2015-10-28

Family

ID=54377962

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510519612.6A Pending CN105005249A (zh) 2015-08-24 2015-08-24 一种全自动四轮双核高速灭火机器人伺服控制器

Country Status (1)

Country Link
CN (1) CN105005249A (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105573316A (zh) * 2015-12-01 2016-05-11 武汉科技大学 一种自主编队移动群体机器人
CN106074048A (zh) * 2016-07-28 2016-11-09 江苏若博机器人科技有限公司 一种双核三轮驱动高速机器人电动病床控制器
CN106137621A (zh) * 2016-07-28 2016-11-23 江苏若博机器人科技有限公司 一种单核三轮驱动多功能电动病床
CN106176085A (zh) * 2016-07-28 2016-12-07 江苏若博机器人科技有限公司 一种无线双核三轮驱动高速机器人电动病床控制器
CN106647755A (zh) * 2016-12-21 2017-05-10 上海芮魅智能科技有限公司 一种智能化实时构建清扫地图的扫地机器人
CN106737563A (zh) * 2016-11-30 2017-05-31 山东康威通信技术股份有限公司 一种轻型轨道机器人四驱运动控制系统及其方法
CN107465367A (zh) * 2017-08-14 2017-12-12 广东工业大学 一种电机控制的方法及装置
CN108614574A (zh) * 2018-06-11 2018-10-02 湖北工业大学 一种厘米级卫星定位智能无人农机控制系统及方法
CN111405250A (zh) * 2020-03-24 2020-07-10 北京海益同展信息科技有限公司 视频流数据处理方法、装置、机器人及存储介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH11276618A (ja) * 1998-03-31 1999-10-12 Nohmi Bosai Ltd 消火ロボット及びその管理方法
CN201583652U (zh) * 2009-12-03 2010-09-15 河海大学 光源识别传感器阵列模块
CN101884830A (zh) * 2010-05-28 2010-11-17 中国科学技术大学 一种灭火机器人系统
CN201711969U (zh) * 2010-07-09 2011-01-19 华南理工大学 一种自带真空源的负压吸附模块
CN202223800U (zh) * 2011-09-28 2012-05-23 西安建筑科技大学 一种可视火灾探测与人员疏导的消防联动装置
CN103198445A (zh) * 2013-02-21 2013-07-10 南京理工大学 高速红外目标检测与跟踪图像处理装置及其处理方法
KR20130098592A (ko) * 2012-02-28 2013-09-05 한서대학교 산학협력단 자율 주행 로봇 및 그 제어 방법
CN103529832A (zh) * 2013-09-24 2014-01-22 苏州工业园区职业技术学院 基于双核四轮高速微电脑鼠对角线冲刺系统
CN203697010U (zh) * 2013-12-27 2014-07-09 上海维纽机器人有限公司 随动机器人电控系统
CN203825465U (zh) * 2014-04-29 2014-09-10 南昌航空大学 一种比赛用搜救机器人
CN204447051U (zh) * 2015-01-13 2015-07-08 南昌航空大学 一种比赛用新型灭火机器人

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH11276618A (ja) * 1998-03-31 1999-10-12 Nohmi Bosai Ltd 消火ロボット及びその管理方法
CN201583652U (zh) * 2009-12-03 2010-09-15 河海大学 光源识别传感器阵列模块
CN101884830A (zh) * 2010-05-28 2010-11-17 中国科学技术大学 一种灭火机器人系统
CN201711969U (zh) * 2010-07-09 2011-01-19 华南理工大学 一种自带真空源的负压吸附模块
CN202223800U (zh) * 2011-09-28 2012-05-23 西安建筑科技大学 一种可视火灾探测与人员疏导的消防联动装置
KR20130098592A (ko) * 2012-02-28 2013-09-05 한서대학교 산학협력단 자율 주행 로봇 및 그 제어 방법
CN103198445A (zh) * 2013-02-21 2013-07-10 南京理工大学 高速红外目标检测与跟踪图像处理装置及其处理方法
CN103529832A (zh) * 2013-09-24 2014-01-22 苏州工业园区职业技术学院 基于双核四轮高速微电脑鼠对角线冲刺系统
CN203697010U (zh) * 2013-12-27 2014-07-09 上海维纽机器人有限公司 随动机器人电控系统
CN203825465U (zh) * 2014-04-29 2014-09-10 南昌航空大学 一种比赛用搜救机器人
CN204447051U (zh) * 2015-01-13 2015-07-08 南昌航空大学 一种比赛用新型灭火机器人

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
张飞飞: "基于STM32的智能灭火机器人设计", 《甘肃科技》 *
李 疆: "基于DSP与FPGA的机器人运动控制系统设计", 《机械与电子》 *
李莉: "基于ARM和FPGA的机器人运动控制器的实现", 《计算机测量与控制》 *
王珊珊: "轮式移动机器人控制系统设计", 《中国优秀硕士学位论文全文数据库》 *
陈建斌: "基于ARM与FPGA的移动机器人控制系统设计", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105573316A (zh) * 2015-12-01 2016-05-11 武汉科技大学 一种自主编队移动群体机器人
CN105573316B (zh) * 2015-12-01 2019-05-03 武汉科技大学 一种自主编队移动群体机器人
CN106074048A (zh) * 2016-07-28 2016-11-09 江苏若博机器人科技有限公司 一种双核三轮驱动高速机器人电动病床控制器
CN106176085A (zh) * 2016-07-28 2016-12-07 江苏若博机器人科技有限公司 一种无线双核三轮驱动高速机器人电动病床控制器
CN106137621A (zh) * 2016-07-28 2016-11-23 江苏若博机器人科技有限公司 一种单核三轮驱动多功能电动病床
CN106737563A (zh) * 2016-11-30 2017-05-31 山东康威通信技术股份有限公司 一种轻型轨道机器人四驱运动控制系统及其方法
CN106737563B (zh) * 2016-11-30 2019-05-17 山东康威通信技术股份有限公司 一种轻型轨道机器人四驱运动控制系统及其方法
CN106647755A (zh) * 2016-12-21 2017-05-10 上海芮魅智能科技有限公司 一种智能化实时构建清扫地图的扫地机器人
CN107465367A (zh) * 2017-08-14 2017-12-12 广东工业大学 一种电机控制的方法及装置
CN108614574A (zh) * 2018-06-11 2018-10-02 湖北工业大学 一种厘米级卫星定位智能无人农机控制系统及方法
CN111405250A (zh) * 2020-03-24 2020-07-10 北京海益同展信息科技有限公司 视频流数据处理方法、装置、机器人及存储介质

Similar Documents

Publication Publication Date Title
CN105137975A (zh) 六轮双核全自动高速灭火机器人伺服控制器
CN105005249A (zh) 一种全自动四轮双核高速灭火机器人伺服控制器
CN105159325A (zh) 一种基于stm32f407和fpga两轮高速灭火机器人伺服控制器
CN105169610A (zh) 基于两轮高速全自动灭火机器人的双核伺服控制系统
CN105116918A (zh) 一种双核两轮中速灭火机器人伺服控制器
CN105334853A (zh) 双核高速四轮微微鼠冲刺控制器
CN105045289A (zh) 一种单轮全自动高速灭火机器人伺服控制系统
CN105137974A (zh) 六轮双核全自动中速灭火机器人伺服控制器
CN105045288A (zh) 一种单轮全自动中速灭火机器人伺服控制系统
CN105320146A (zh) 一种单轮双核中速灭火机器人伺服控制系统
CN105005308A (zh) 基于stm32f407六轮灭火机器人伺服控制器
CN105159323A (zh) 六轮单核全自动低速灭火机器人伺服控制器
CN105116897A (zh) 双核高速四轮微微鼠全数字导航伺服控制器
CN105137983A (zh) 双核高速两轮微微鼠冲刺控制器及其控制方法
CN103472840A (zh) 基于arm9四轮微电脑鼠探索控制器
CN105068558A (zh) 一种单轮双核高速灭火机器人伺服控制系统
CN105159324A (zh) 六轮双核中速灭火机器人伺服控制器
CN205055285U (zh) 基于两轮高速全自动灭火机器人的双核伺服控制系统
CN105116898A (zh) 单核低速两轮微微鼠冲刺控制器
CN205055284U (zh) 基于两轮中速全自动灭火机器人的双核伺服控制系统
CN105005318A (zh) 一种全自动四轮低速灭火机器人伺服控制器
CN105116891A (zh) 双核中速两轮微微鼠冲刺控制器及其控制方法
CN105137978A (zh) 双核高速四轮微微鼠探索控制器
CN203643838U (zh) 基于双核四轮微电脑鼠及其快速冲刺控制器
CN105137977A (zh) 一种四轮双核高速灭火机器人伺服控制器

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20151028