方向为北；如果在坐标(X，Y)时的方向为南，则更新其坐标为(Χ+1，Y)，新坐标方向为东；如果在坐标(X，Y)时的方向为西，则更新其坐标为(X，Y-1)，新坐标方向为南；
光电传感器S7开始工作，对新坐标下的光源进行判断，如果光电传感器S7捕捉到信号将通知控制器发现目标，控制器会让光电传感器S7禁止工作2秒工作，2秒后重新开启光电传感器S7，如果再次捕捉到光电信号，控制器会开启灭火机器人携带的干冰容器的电磁阀，开始对蜡烛进行喷洒干冰直至光源消失；如果光电传感器S7没有捕捉到新坐标下的光源，灭火机器人将离开目前房间格，将继续搜寻并更新其坐标；
12)当灭火机器人搜到光源，并喷洒干冰完成灭火后，灭火机器人会停在目标点，然后控制器调出灭火机器人已经搜索的路径，并舍弃未搜寻的目标，通过洪水算法找出已经搜寻过房间的最佳路径，然后灭火机器人按照此路径快速回到搜寻起点；
13)在灭火机器人行走过程总，控制器并实时检测灭火机器人的行走速度，并根据地面情况系统会自动调节电机M加大真空吸盘对地面的吸附能力；
14)在灭火机器人房间搜寻过程中，FPGA会对高速直流无刷电机X、直流无刷电机Y、直流无刷电机Ζ、直流无刷电机R、直流无刷电机U、直流无刷电机W、直流电机M的转矩进行在线辨识，由于系统采用的是三闭环伺服控制，如果转矩出现脉动，FPGA会利用直流电机力矩与电流的关系对干扰进行线性补偿，有效减少了电机转矩抖动对灭火机器人快速探索时导航的影响，增加了其抗干扰能力。
[0035]15)当灭火机器人完成整个返程过程回到起始点(0，0)，STM32F407将控制FPGA使得灭火机器人中心点停车，并重新调整FPGA驱动信号，禁止前后四路PffM波控制信号输出并使能中驱的两路PWM波控制信号，驱动信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y以相反的方向运动，灭火机器人加速度计传感器Al、陀螺仪Gl和方向传感器Dl的控制下，原地旋转180度，FPGA使能所有的PffM波控制信号输出，并使灭火机器人原地自锁，等待下一个搜寻火源命令。
[0036]本发明具有的有益效果是:
1:在运动过程中，充分考虑了电池在这个系统中的作用，基于STM32F407+FPGA控制器时刻都在对灭火机器人的运行状态进行监测和运算，并且在电池提供电源的过程中，电流传感器C1-C7时刻对电池的电流进行观测并送给控制器进行保护，避免了大电流的产生，所以从根本上解决了大电流对锂离子电池的冲击，避免了由于大电流放电而引起的锂离子电池过度老化现象的发生；
2:用直流无刷电机替代了步进电机，使得电机无机械摩擦，无磨损，无电火花，且免维护，而且直流无刷电机的效率高，功率和转矩密度高，使得系统的效率更高；
3:由FPGA处理灭火机器人的六只直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W、单轴吸附控制的直流电机M的伺服控制，使得控制比较简单，大大提高了运算速度，解决了单片机软件运行较慢的瓶颈，缩短了开发周期短，并且程序可移植能力强；
4:本发明基本实现全贴片元器件材料，实现了单板控制，不仅节省了控制板占用空间，而且有利于灭火机器人体积和重量的减轻；
5:由于采用直流无刷电机，使得系统带载能力更强，调速范围比较宽，调速比较平稳；
6:由于本控制器采用FPGA处理七轴伺服大量的数据与算法，有效地防止了程序的“跑飞”，抗干扰能力大大增强；
7:在灭火机器人实际运动过程中，FPGA可以根据机器人外围运行情况适时调整其内部伺服控制的PID参数，实现分段P、ro、PID控制和非线性PID控制，使系统满足中低速运行时速度的切换；
8:在此六轮灭火机器人系统引入了三轴加速度计传感器Al、陀螺仪Gl和方向传感器D1，实现了灭火机器人在房间探索时的瞬时加速度、速度和角度的直接检测，并利用反馈实现全程导航和二次补偿，有利于提高灭火机器人的稳定性和动态性能；
9:在六轮灭火机器人运行过程中，FPGA会对高速直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W、和直流电机M的转矩进行在线辨识并利用电机力矩与电流的关系进行补偿，减少了电机转矩抖动对灭火机器人快速探寻火源的影响；
10:FPGA通过调节直流电机M的伺服控制可以有效调节真空吸盘对地面的吸附能力，消除了灭火机器人在高速探寻火源时打滑现象的发生；
11:由FPGA根据探索火源的速度、加速度要求把外界偏差转化为各个电机控制的位置、速度和加速度给定，再结合光电编码器和电流传感器的反馈输出PWM调制信号和方向信号，通过驱动电路可以直接驱动电机，极大的提高了运算速度；
12:根据灭火机器人搜寻功率需要实现分时多驱。在正常搜寻时，由于所求功率较小，STM32F407 一般会通过FPGA释放前后四轮，采用中置动力的两轮驱动的方式；而一旦遇到稍微提速时，STM32407会自动检测并立即通过使能FPGA后置两路PffM控制信号开启后置的两个助力直流无刷电机，同时通过FPGA改变直流电机M的伺服控制，灭火机器人系统自然切换到中驱+后驱的四轮驱动状态，增强了灭火机器人的搜寻时的可靠性；而一旦遇到路面灰尘较多或灭火机器人速度较高时，STM32407会自动检测并立即通过使能FPGA前后中置六路PWM控制信号，并开启前后置的四个助力直流无刷电机，同时通过FPGA改变电机M的伺服控制，灭火机器人系统自然切换到前驱+中驱+后驱的六轮驱动状态，进一步增强了灭火机器人的搜寻时的可靠性；
13:由于采用前驱+中驱+后驱的复合六轮驱动方式，当需要加速探寻或者是返回起点时，控制器把动力分配到六个直流无刷电机，一旦一个动力轮由于地面、机械结构等造成暂时离开地面，STM32407可以重新分配扭矩，把更多的扭矩通过FPGA使能相应的PffM波控制信号分配在未失速的驱动轮上，使系统迅速脱离不稳定状态而重新回到六轴动力平衡状态，使得灭火机器人具有更好的探寻行走功能；
14:灭火机器人转向时，为了保证旋转的稳定性，FPGA通过使能中置两路PffM波控制信号而禁止前后四路PWM波控制信号工作，采用中置的两个直流无刷电机实现搜寻转弯，并释放前后置的四个助力直流无刷电机。
[0037]以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本发明的专利保护范围内。
【主权项】
1.一种六轮双核高速灭火机器人伺服控制器，其特征在于，包括电池、处理器、第一电机、第二电机、第三电机、第四电机、第五电机、第六电机、第七电机以及灭火机器人，所述的电池单独提供电流驱动所述的处理器，所述的处理器分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号分别控制所述的第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机的信号合成之后再控制灭火机器人的运动，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机采用直流无刷电机，所述的第四电机采用直流电机，其中，所述的处理器采用双核处理器，包括STM32F407和FPGA，所述的FPGA与STM32F407进行通信连接。2.根据权利要求1所述的六轮双核高速灭火机器人伺服控制器，其特征在于，所述的电池采用锂离子电池。3.根据权利要求1所述的六轮双核高速灭火机器人伺服控制器，其特征在于，所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号均为PWM波控制信号。4.根据权利要求1所述的六轮双核高速灭火机器人伺服控制器，其特征在于，所述的处理器的内部还设置有上位机系统和运动控制系统，所述的上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块以及在线输出模块，所述的运动控制系统包括基于FPGA七轴同步混合伺服控制模块、坐标定位模块以及I/O控制模块，其中，所述的基于FPGA七轴同步混合伺服控制模块包括六轴直流无刷电机灭火机器人搜寻伺服控制模块和单轴真空吸盘吸附伺服控制模块。5.根据权利要求1所述的六轮双核高速灭火机器人伺服控制器，其特征在于，所述的六轮灭火机器人伺服控制器还包括超声波传感器、电流传感器、光电传感器、电压传感器、加速度计传感器、陀螺仪以及方向传感器，所述的超声波传感器、电流传感器、光电传感器、电压传感器、加速度计传感器、陀螺仪以及方向传感器均与处理器通讯连接。6.根据权利要求5所述的六轮双核高速灭火机器人伺服控制器，其特征在于，所述的超声波传感器的数量为6个、电流传感器的数量为7个、光电传感器、电压传感器、加速度计传感器、陀螺仪以及方向传感器的数量均为I个。7.根据权利要求1所述的六轮双核高速灭火机器人伺服控制器，其特征在于，所述的六轮灭火机器人伺服控制器还包括光电编码器，所述的光电编码器分别安装在第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机上。
【专利摘要】本发明公开了一种六轮双核高速灭火机器人伺服控制器，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机采用直流无刷电机，所述的第四电机采用直流电机，其中，所述的处理器采用双核处理器，包括STM32F407和FPGA，所述的FPGA与STM32F407进行通信连接。通过上述方式，本发明提供了基于STM32F407+FPGA的六轮双核全新控制模式，控制板以FPGA为处理核心，实现六轴直流无刷电机和单轴直流电机的七轴混合伺服控制的数字信号实时处理，真空吸附技术的引入彻底消除了灭火机器人行走打滑现象的发生，有效提高了灭火机器人搜寻房间位置的精确性。
【IPC分类】G05D1/02
【公开号】CN105045267
【申请号】CN201510519589
【发明人】张好明, 杨锐敏
【申请人】铜陵学院
【公开日】2015年11月11日
【申请日】2015年8月24日