CN105005308A - 基于stm32f407六轮灭火机器人伺服控制器 - Google Patents

Abstract

本发明公开了一种基于STM32F407六轮灭火机器人伺服控制器，所述的处理器采用STM32F407，其中，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机采用直流无刷电机，所述的第四电机采用直流电机。通过上述方式，本发明采用六轮结构替代了原有的两轮和四轮结构，由于采用六轮驱动技术，灭火机器人前后中轮都有动力，可按探索地面和周围环境状态不同而将需求扭矩按不同比例分布在前后所有的轮子上，以提高灭火机器人的行驶能力，真空吸附技术的引入彻底消除了灭火机器人行走打滑现象的发生，有效提高了灭火机器人搜寻房间位置的精确性。

Description

基于STM32F407六轮灭火机器人伺服控制器

技术领域

本发明涉多轴机器人的等领域，涉及一种六轮灭火机器人自动控制系统，尤其涉及一种基于STM32F407六轮灭火机器人伺服控制器。

背景技术

灭火机器人是一中模拟现实生活中人类发现有害火源并能够自动熄灭火源的一种新型智能机器人。一般情况下，比赛型灭火机器人能够在一间平面结构房子模型里运动，在操作规则指导下以最短的时间找到代表火源的一根蜡烛并将它熄灭。模拟现实家庭中机器人处理火警的过程。蜡烛代表家里燃起的火源，机器人必须找到并熄灭火源。蜡烛火焰的底部将离地面15～20cm高。蜡烛是直径1-2cm的白蜡烛。蜡烛火焰的确切高度和尺寸是不确定的、变化的，而且由蜡烛条件和周围的环境所决定。蜡烛将随机地放在比赛场地的一个房间里，比赛开始后不管火焰具体是什么尺寸，都要求机器人能发现蜡烛。

在真正的比赛中，为了加大比赛难度，比赛场地被分为n*n格的标准模式，最常采用的是8*8格的均匀模式，其比赛场地二维结构如图1所示，灭火机器人将在64格房间里寻找火源并熄灭。在图1的二维搜寻火源地图中，墙的材料是木质一般且可以反光，每块挡墙的长度为60cm长，高度在27-34cm。比赛场地地面是光滑的，场地的地板是黑色的，场地上的任意缝隙都刷成黑色，场地的缝隙不超过5mm。一些机器人可能用泡沫，粉末或者其他的物质来熄灭蜡烛的火焰。由于每一个机器人比赛后清洗场地的好坏直接影响到地面情况，故地面不保证在整个比赛过程中都保持绝对黑色。一旦启动，灭火机器人必须在没有人的干预下自己控制导航，而非人工控制，为了考验灭火机器人在搜寻火源过程中的稳定性，其不可以碰撞或接触墙壁，否则将被受到处罚。

一台完整的灭火机器人大致分为以下几个部分：

1）电机：执行电机是灭火机器人的动力源，它根据微处理器的指令来执行灭火机器人在二维平面上行走的相关动作；

2）算法：算法是灭火机器人的灵魂。灭火机器人必须采用一定的智能算法才能准确快速的从一个房间格到达另外一个房间的运动，然后发现火源，并开启自身携带的干冰控制器，扑灭火源；

3）微处理器：微处理器是灭火机器人的核心部分，是灭火机器人的大脑。灭火机器人所有的信息，包括房间墙壁信息，火源位置信息，和电机状态信息等都需要经过微处理器处理并做出相应的判断。

灭火机器人结合了多学科知识，对于提升在校学生的动手能力、团队协作能力和创新能力，促进学生课堂知识的消化和扩展学生的知识面都非常有帮助。国内研发此机器人的单位较多，但是研发的机器人比较落后，研发的灭火机器人结构如图2，长时间运行发现存在着很多安全问题，即：

（1）作为灭火机器人的执行机构采用的多是步进电机，经常会遇到丢失脉冲造成电机失步现象发生，导致对位置的记忆出现错误，灭火机器人无法寻求到火源，或者是灭火后机器人无法回到起始点；

（2）由于采用步进电机，使得机体发热比较严重，有的时候需要进行加装散热装置，使得机器人整体重量增加；

（3）由于采用步进电机，使得系统运转的机械噪声大大增加，不利于环境保护；

（4）由于采用步进电机，其电机本体一般都是多相结构，控制电路需要采用多个功率管，使得控制电路相对比较复杂，并且增加了控制器价格；

（5）由于采用步进电机，使得系统一般不适合在速度较高的场合运行，高速运动时容易产生振动，有时候可能会接触墙壁，导致寻找火源失败；

（6）由于灭火机器人要频繁的刹车和启动，加重了单片机的工作量，单一的单片机无法满足灭火机器人快速启动和停止的要求；

（7）相对采用的都是一些体积比较大的插件元器件，使得灭火机器人控制系统占用较大的空间，重量相对都比较重；

（8）由于受周围环境不稳定因素干扰，单片机控制器经常会出现异常，引起灭火机器人失控，抗干扰能力较差；

（9）对于两轮灭火机器人寻找火源过程来说，一般要求其两个电机的PWM控制信号要同步，由于受单片机计算能力的限制，单一单片机伺服系统很难满足这一条件，使得灭火机器人行走导航很难控制，特别是对于快速行走时情况更糟糕；

（10）由于采用两个动力轮驱动，为了满足快速搜寻火源时的加速和减速，使得单个驱动电机的功率较大，不仅占用的空间较大，而且有时候在一些相对需求能量较低的状态下造成“大马拉小车”的现象出现，不利于灭火机器人系统能源的节省。

因此，需要对现有的基于单片机控制的两轮灭火机器人控制器进行重新设计，寻求一种经济适用的能够在现实中的使用六轮灭火机器人伺服系统。

发明内容

本发明主要解决的技术问题是提供一种基于STM32F407六轮灭火机器人伺服控制器，能够更好的提高灭火机器人对复杂环境的适应能力，采用六轮结构替代了原有的两轮和四轮结构，兼顾两轮中置转向的优点，采用前驱+中驱+后驱的六轮驱动结构，中置驱动的两个直流无刷电机功率较大，前置和后置驱动的四个直流无刷电机功率较小，只有在动力需求较高时才启动，起到助力作用，由于采用六轮驱动技术，灭火机器人前后中轮都有动力，可按探索地面和周围环境状态不同而将需求扭矩按不同比例分布在前后所有的轮子上，以提高灭火机器人的行驶能力。

为解决上述技术问题，本发明采用的一个技术方案是：提供了一种基于STM32F407六轮灭火机器人伺服控制器，包括电池、处理器、第一电机、第二电机、第三电机、第四电机、第五电机、第六电机、第七电机以及灭火机器人，所述的电池单独提供电流驱动所述的处理器，所述的处理器采用STM32F407，所述的处理器分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号分别控制所述的第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机的信号合成之后再控制灭火机器人的运动，其中，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机采用直流无刷电机，所述的第四电机采用直流电机。

在本发明一个较佳实施例中，所述的电池采用锂离子电池。

在本发明一个较佳实施例中，所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号均为PWM波控制信号。

在本发明一个较佳实施例中，所述的处理器的内部还设置有上位机系统和运动控制系统，所述的上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块以及在线输出模块，所述的运动控制系统包括七轴混合伺服控制模块、坐标定位模块以及I/O控制模块，其中，所述的七轴混合伺服控制模块包括六轴直流无刷电机灭火机器人搜寻伺服控制模块和单轴真空吸盘吸附伺服控制模块。

在本发明一个较佳实施例中，所述的六轮灭火机器人伺服控制器还包括超声波传感器、电流传感器、光电传感器、电压传感器以及加速度计传感器，所述的超声波传感器、电流传感器、光电传感器、电压传感器以及加速度计传感器均与处理器通讯连接。

在本发明一个较佳实施例中，所述的超声波传感器的数量为6个、电流传感器的数量为7个、光电传感器、电压传感器以及加速度计传感器的数量均为1个。

在本发明一个较佳实施例中，所述的六轮灭火机器人伺服控制器还包括光电编码器，所述的光电编码器分别安装在第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机上。

本发明的有益效果是：本发明的基于STM32F407六轮灭火机器人伺服控制器，为克服单片机不能满足两轮灭火机器人行走的稳定性和快速性的要求，舍弃了国产自动灭火机器人所采用的基于单片机控制两轮的工作模式，在吸收国外先进控制思想的前提下，自主研发了基于STM32F407的全新六轮驱动控制模式，控制板以STM32F407为处理核心，实现六轴直流无刷电机和单轴直流电机的七轴混合伺服控制数字信号实时处理，并响应各种中断，实现数据信号的实时存储，同时真空吸附技术的引入彻底消除了灭火机器人行走打滑现象的发生，有效提高了灭火机器人搜寻房间位置的精确性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图，其中：

图1为灭火机器人房间示意图；

图2为基于单片机控制的两轮灭火机器人原理图；

图3为基于STM32F407六轮灭火机器人结构图；

图4为基于STM32F407六轮灭火机器人原理图。；

图5为基于STM32F407六轮灭火机器人伺服程序框图；

图6为灭火机器人运行方向示意图；

图7为右转示意图；

图8为左转示意图。

具体实施方式

下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

如图4所示，本实施例包括：

一种基于STM32F407六轮灭火机器人伺服控制器，包括电池、处理器、第一电机、第二电机、第三电机、第四电机、第五电机、第六电机、第七电机以及灭火机器人，所述的电池单独提供电流驱动所述的处理器，所述的处理器采用STM32F407，所述的处理器分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号分别控制所述的第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机的信号合成之后再控制灭火机器人的运动，其中，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机均采用直流无刷电机，所述的第四电机采用直流电机。本实施例中，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机分别对应标记为直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W，第四电机标记为直流电机M。

如图3所示，所述的六轮灭火机器人伺服控制器还包括超声波传感器、电流传感器、光电传感器、电压传感器以及加速度计传感器，所述的超声波传感器、电流传感器、光电传感器、电压传感器以及加速度计传感器均与处理器通讯连接。其中，所述的超声波传感器的数量为6个，本实施例中超声波传感器标记为S1、S2、S3、S4、S5和S6；电流传感器的数量为7个，本实施例中电流传感器标记为C1、C2、C3、C4、C5、C6和C7；光电传感器、电压传感器以及加速度计传感器的数量均为1个，本实施例中光电传感器标记为S7，电压传感器标记为V1，加速度计传感器标记为A1。

如图5所示，所述的处理器的内部还设置有上位机系统和运动控制系统，所述的上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块以及在线输出模块，所述的运动控制系统包括七轴混合伺服控制模块、坐标定位模块以及I/O控制模块，其中，所述的七轴混合伺服控制模块包括六轴直流无刷电机灭火机器人搜寻伺服控制模块和单轴真空吸盘吸附伺服控制模块。

STM32F4系列除引脚和软件兼容高性能的F2系列外，F4的主频(168MHz)高于F2系列(120MHz)，并支持单周期DSP指令和浮点单元、更大的SRAM容量(192 KB，F2为128 KB)、512KB-1MB的嵌入式闪存以及影像、网络接口和数据加密等更先进的外设。STM32F4系列基于最新的ARM Cortex M4内核， 在现有出色的STM32微控制器产品组合中新增了信号处理功能，并提高了运行速度；STM32F405x集成了定时器、3个ADC、2个DAC、串行接口、外存接口、实时时钟、CRC计算单元和模拟真随机数发生器在内的整套先进外设。STM32F407在STM32F405产品基础上增加了多个先进外设。这些性能使得F4系列可以较容易满足控制和信号处理功能混合的数字信号控制需求。高效的信号处理功能与Cortex-M4处理器系列的低能耗、低成本和易于使用的优点的组合，使得其可以为多轴电动机控制提供灵活解决方案。这些特点使得STM32F407特别适合多轴灭火机器人伺服系统的信号处理。

为了能够更好的提高灭火机器人对复杂环境的适应能力，本发明采用六轮结构替代了原有的两轮和四轮结构：为了兼顾两轮中置转向的优点，本发明采用前驱+中驱+后驱的六轮驱动结构：中置驱动的两个直流无刷电机功率较大，前置和后置驱动的四个直流无刷电机功率较小，只有在动力需求较高时才启动，起到助力作用。由于采用六轮驱动技术，灭火机器人前后中轮都有动力，可按探索地面和周围环境状态不同而将需求扭矩按不同比例分布在前后所有的轮子上，以提高灭火机器人的行驶能力。

具体地：

为了能够精确探寻房间寻找火源，本发明采用六组传感器探测房间模式，所发明的灭火机器人结构如图3所示：超声波传感器S1、S6共同作用判断前方挡墙，声波传感器S2和S3共同合作判断其左边挡墙的存在，传感器S4和 S5共同合作判断其右边挡墙的存在，同时声波传感器S2、S3、S4、S5合作为灭火机器人直线运动提供导航依据。由于在比赛现场，太阳光有可能直射进来，这样红外传感器就不能在使用，本发明采用超声波传感器代替了红外传感器。在这种结构设置中，声波传感器S2和S3可以在不同位置精确测量到房间左侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化，声波传感器S4和S5可以在不同位置精确测量到房间右侧从有挡墙到无挡墙或者无挡墙到有挡墙的变化，这个位置的传感器信号阶跃变化可以被控制器捕捉到，然后在此位置可以对灭火机器人进行精确补偿，这对于灭火机器人求解房间找到火源并返回到起点计算至关重要，如果没有此智能补偿的话，灭火机器人有可能在复杂房间中的累计误差足以使其无法求解此房间地图，导致无法回到房间起点。

为了提高六轮灭火机器人在寻找火源过程中行走导航的稳定性，本发明在灭火机器人伺服硬件系统中加入了加速度计传感器A1。在灭火机器人行走房间期间全程开启加速度计传感器A1，加速度计传感器A1用来测量灭火机器人三个前进方向的加速度。控制器根据测得的加速度计的加速度信号利用积分和二次积分得到其旋转角速度和角度。当灭火机器人的姿态发生变化超过设定阀值时，在一个新的采样周期控制器就立即对其位置补偿，避免灭火机器人远远偏离中心位置而出现撞墙现象的发生，提高了其快速行走导航的稳定性。

为了提高六轮灭火机器人全数字伺服系统的稳定性，防止灭火机器人在高速行走时打滑导致灭火机器人房间信息错误，本发明在灭火机器人伺服硬件系统中加入了直流电机M，在灭火机器人运动过程中，直流电机M通过真空抽吸装置不停抽吸微型真空吸盘内的空气，使微型真空吸盘的内外压力不一样，产生一定的负压，使其对房间地面产生一定的吸附能力，即使房间地板受到了上一组选手的破坏产生了一定变化，灭火机器人也不会受到影响，有效防止了灭火机器人在高速行走时的地面打滑。

本发明为克服单片机不能满足两轮灭火机器人行走的稳定性和快速性的要求，舍弃了国产自动灭火机器人所采用的基于单片机控制两轮的工作模式，在吸收国外先进控制思想的前提下，自主发明了基于STM32F407的全新六轮驱动控制模式。控制板以STM32F407为处理核心，实现六轴直流无刷电机和单轴直流电机的七轴混合伺服控制数字信号实时处理，并响应各种中断，实现数据信号的实时存储。

为达上述目的，本发明采取以下技术方案， 为了提高运算速度，保证六轮灭火机器人系统的稳定性和可靠性，本发明在基于STM32F407的控制器中引入真空吸附技术来增加灭火机器人与地方的摩擦系数防止打滑，同时引入加速度计传感器对灭火机器人进行二次姿态校正。此控制器充分考虑电池在这个系统的作用，把控制系统中工作量最大的七轴伺服系统交给STM32F407处理，充分发挥STM32F407数据处理速度相对较快的特点，从而快速实现灭火机器人人机界面、房间读取、房间存储、坐标定位、I/O控制、火源发现及灭火等功能。

在电源打开状态下，灭火机器人先进入自锁状态。灭火机器人通过超声波传感器S1、S6判断前方运动环境，实际导航环境被转化为控制参数传输给STM32F407，STM32F407把这些环境参数转化为灭火机器人在指定方向下直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W要运行的距离、速度和加速度参数指令值，然后STM32F407结合电流传感器C1-C6和光电编码器的反馈生成控制直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W的六路PWM控制信号，控制信号经驱动桥放大后驱动直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W向前运动，STM32F407实时记录加速度计传感器A1和光电编码器的信号反馈，当灭火机器人偏离位置时，微处理器根据这些信号二次校正灭火机器人的姿态。灭火机器人在运动过程中，STM32F407根据灭火机器人运动速度自动通过直流电机M调节真空装置对地面的吸附能力，增加有效摩擦，防止灭火机器人快速行走打滑，并实时储存房间信息，并根据实际功率需求，实时切换到两驱、四驱或六驱状态。当控制器确定发现火源后自动开启干冰灭火器电磁阀，通过喷洒干冰灭火，灭火完毕后STM32F407立即调出灭火机器人已经储存的房间信息，通过洪水算法找出返程最短路径，并开启六轮加速模式迅速回到起点等待一下条寻求火源命令。

参照图4、图5，图6、图7、图8，其具体的功能实现如下：

1）打开电源瞬间，STM32F407会对电池电压进行检测，如果低压的话，STM32F407将封锁直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W的六路PWM波控制信号，直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W不能启动，同时电压传感器V1将工作，并发出报警信号，如果系统电压正常，控制器首先开启真空抽吸的直流电机M，通过抽吸装置先对微型真空吸盘抽吸，使真空吸盘对地面具有一定的吸附力，控制器并实时检测，如果地面不干净，系统会自动调节电机M加大真空吸盘对地面的吸附力；

2）在灭火机器人未接到搜寻火源命令之前，它一般会在起点坐标（0，0）等待控制器发出的搜寻命令，一旦接到任务后，会沿着起点开始为了寻找火源进行全宫探索；

3）灭火机器人放在起点坐标（0，0），一般情况下，灭火机器人按照图6中北的方向（计算机编程代码为0）放置，接到任务后其前方的超声波传感器S1、S6和会对前方的环境进行判断，确定有没有挡墙进入运动范围，如存在挡墙将向STM32F407发出存储命令，STM32F407会对中断做第一时间响应，然后禁止前后四轮直流无刷电机的PWM波控制信号输出，并使能中驱两路的PWM波控制信号，控制中驱的两台直流无刷电机X正转和直流无刷电机Y反转，释放前后驱的四台直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W，灭火机器人在加速度计传感器A1的控制下向右旋转90度，灭火机器人首先沿着X轴正向（东的方向，计算机编程代码为2）搜寻火源；

4）在灭火机器人运动过程中，装在直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W和直流电机M上的光电编码器会输出其位置信号A和位置信号B并反馈给STM32F407，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，STM32F407的位置寄存器会根据直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W和直流电机M的运行方向加1或者是减1；

5）在灭火机器人运动过程中，装在直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W和直流电机M上的光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给STM32F407寄存器，记录直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W和直流电机M的绝对位置，然后换算成灭火机器人在房间中的具体位置；

6）为了能够实现灭火机器人准确的坐标计算功能，灭火机器人左右的超声波传感器S2、S3和S4、S5会时刻对运动方向左右的房间挡墙和柱子进行探测，如果超声波传感器S2、S3或者超声波传感器S4、S5发现传感器信号发生了跃变，则说明灭火机器人进入了从有房间挡墙到无房间挡墙（或者是从无房间挡墙到有房间挡墙）状态的变化，STM32F407会根据灭火机器人当前运行状态精确补偿，彻底消除灭火机器人在复杂房间中探寻火源时已经累计的误差；

7）在灭火机器人沿着任何一个方向向前运动，在任何一个方格的中心如果确定没有挡墙进入前方的运动范围，则灭火机器人将存储其坐标（X，Y），并把向前运动一格的位置参数送给STM32F407，STM32F407根据搜寻控制器速度和加速度要求以及光电编码器的反馈，生成速度-时间运动梯形图，这个梯形包含的面积就是灭火机器人直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R要运行的一格距离，STM32F407根据这个梯形图生成驱动四轴直流无刷电机的PWM波控制信号，然后STM32F407使能驱动芯片驱动四个独立直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R向前运动，在沿着当前房间格向前探索过程中，超声波传感器S2、S3和S4、S5会对左右的挡墙进行判断，并记录储存当前搜寻房间挡墙信息，灭火机器人根据前进方向左右挡墙的房间信息进入单墙导航模式或者是双墙导航模式，然后再结合设定的左右挡墙导航阀值，加速度计传感器A1记录灭火机器人实时的加速度信号并送给控制器，控制器分别通过积分和二次积分得到瞬时角速度和角度，记录灭火机器人的瞬时加速度、速度和位置信息，当灭火机器人快速探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速计传感器A1开始进行实时补偿，微调直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置；如果灭火机器人搜寻火源过程中出现失速或者房间地面灰尘较多的情况，STM32F407会调节直流电机M加大灭火机器人与地面的摩擦并使能前中后驱的六轮PWM波控制信号，控制器开启直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W，系统进入六轮驱动状态，STM32F407根据时间和速度要求，把剩余的距离D转化为六轴直流无刷电机的PWM波，使直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W以相同的速度前进，灭火机器人在六驱状态下依旧按照原有的导航模式前进，加速度计传感器A1实时记录灭火机器人的瞬时运动加速度，通过积分和二次积分得到其速度和位置信号，当灭火机器人六轮探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速度计传感器A1开始进行实时补偿，微调六轴直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置；当灭火机器人在加速度计传感器A1的控制下运动一格距离到达新地址时，微处理器将更新其坐标信息；

如果在坐标（X，Y）时的方向为北，在更新其坐标为（X，Y+1），新坐标方向依旧为北；如果在坐标（X，Y）时的方向为东，在更新其坐标为（X+1，Y），新坐标方向依旧为东；如果在坐标（X，Y）时的方向为南，在更新其坐标为（X，Y-1），新坐标方向依旧为南；如果在坐标（X，Y）时的方向为西，在更新其坐标为（X-1，Y），新坐标方向依旧为西；

光电传感器S7开始工作，对新坐标下的光源进行判断，如果光电传感器捕捉到信号将通知控制器发现目标，控制器会让光电传感器禁止工作2秒工作，2秒后重新开启光电传感器，如果再次捕捉到光电信号，控制器会开启携带的干冰灭火器的电磁阀，开始对蜡烛进行喷洒干冰直至光源消失，然后控制器调出灭火机器人已经搜索的路径，并舍弃未搜寻的目标，通过洪水算法找出已经搜寻过房间的最佳路径，然后灭火机器人按照此路径快速回到搜寻起点；

如果光电传感器S7没有捕捉到新坐标下的光源，灭火机器人将离开目前房间格，将继续搜寻并更新其坐标；

8）在灭火机器人沿着当前方向向前运动过程中如果超声波传感器S1和S6判断前方有挡墙进入运动范围，并且此时超声波传感器S2、S3、S4和S5分别判断左右都有挡墙时，灭火机器人将存储此时坐标（X，Y），根据传感器S1和S6的反馈计算出向前运动停车的位置参数YS1，由STM32F407根据探索控制器速度和加速度要求生成速度-时间运动梯形图，这个梯形包含的面积就是灭火机器人六个直流无刷电机要停车的距离Y1，STM32F407根据这个梯形图再结合光电编码器和电流传感器C1-C6反馈生成驱动六轴直流无刷电机的PWM波控制信号，然后STM32F407使能驱动芯片驱动六个独立直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W向前运动，控制器实现六轮同步减速状态，在向前停车过程中，超声波传感器S2、S3、S4、S5实时对左右的挡墙进行判断，并反馈当前房间挡墙信息，灭火机器人进入双墙导航模式，然后再结合设定的左右挡墙导航阀值，加速度计传感器A1记录灭火机器人实时的加速度信号并送给控制器，控制器分别通过积分和二次积分得到瞬时角速度和角度，记录灭火机器人的瞬时加速度、速度和位置信息，当灭火机器人快速探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速计传感器A1开始进行实时补偿，微调六轴直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置，灭火机器人实现在设置停车点停车，STM32F407禁止前后驱四路的直流无刷电机PWM波输出，同时使能中驱的两路直流无刷电机PWM波输出，调整灭火机器人直流无刷电机X、直流无刷电机Y的PWM波输出，使得两个永磁直流无刷电机运动方向相反，灭火机器人原地在加速计传感器A1的控制下实现精确的原地180度转向，然后灭火机器人沿着原先相反的方向运动；

如果在坐标（X，Y）时的方向为北，则更新其坐标为（X，Y），新坐标方向为南；如果在坐标（X，Y）时的方向为东，则更新其坐标为（X1，Y），新坐标方向为西；如果在坐标（X,Y）时的方向为南，则更新其坐标为（X，Y），新坐标方向为北；如果在坐标（X，Y）时的方向为西，则更新其坐标为（X，Y），新坐标方向为东；

9）在灭火机器人沿着当前方向向前运动过程中如果有挡墙进入前方的运动范围，并且此时左右的超声波传感器S2和S3判断左边有挡墙，而超声波传感器S4、S5判断右方没有挡墙时，灭火机器人将存储此时坐标（X，Y），然后灭火机器人将按照图7的曲线行走；

在右转弯时，STM32F407首先把行走直线很短的距离 R90_Leading按照控制器不同搜寻速度和加速度要求生成速度-时间运动梯形图，这个梯形包含的面积就是灭火机器人四个直流无刷电机X、直流无刷电机Y、四个直流无刷电机Z、直流无刷电机R要运动的距离。STM32F407根据这个梯形图再结合光电编码器和电机电流反馈生成驱动四轴直流无刷电机的PWM波，然后STM32F407使能驱动芯片驱动两个独立直流无刷电机X、直流无刷电机Y、四个直流无刷电机Z、直流无刷电机R向前运动，在向前运动过程中，超声波传感器S2、S3实时对左右的挡墙进行判断，并反馈当前房间挡墙信息，灭火机器人进入单左墙导航模式，然后再结合设定的左挡墙导航阀值，加速度计传感器A1记录灭火机器人实时的加速度信号并送给控制器，控制器分别通过积分和二次积分得到瞬时角速度和角度，记录灭火机器人的瞬时加速度、速度和位置信息，当灭火机器人快速探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速计传感器A1开始进行实时补偿，微调四个直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置；如果灭火机器人在校正位置阶段出现失速或者房间地面灰尘较多的情况，STM32F407会调节直流电机M加大灭火机器人与地面的摩擦并使能前后驱的四轮PWM波控制信号，控制器开启直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W，系统进入六轮驱动状态，STM32F407根据时间和速度要求，把剩余的距离D转化为六轴直流无刷电机的PWM波，使直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W以相同的速度前进，灭火机器人在六驱状态下依旧按照原有的左挡墙导航模式前进，加速度计传感器A1实时记录灭火机器人的瞬时运动加速度，通过积分和二次积分得到其速度和位置信号，当灭火机器人六轮探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速度计传感器A1开始进行实时补偿，微调六轴直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置；

当到达既定目标时，传感器参考值R90_FrontWallRef开始工作，防止外界干扰开始做误差补偿。误差补偿结束后控制器禁止前后四轮的直流无刷电机的PWM波控制信号工作，并使能中驱的两轮直流无刷直流电机的PWM波控制信号，控制器调整直流无刷电机X和直流无刷电机Y速度使其完成弧度ARC的运动曲线，STM32F407首先把弧度ARC按照控制器不同搜寻速度和加速度要求生成速度-时间运动梯形图，这个梯形包含的面积就是灭火机器人直流无刷电机X和直流无刷电机Y要运动的距离，然后STM32F407再结合光电编码器以及电流传感器C1、C2的反馈生成驱动直流无刷电机运动的PWM波，PWM波通过驱动桥放大后推动灭火机器人完成转弯搜寻。在灭火机器人转弯搜寻过程中，超声波传感器S2、S3、S4和S5无法为系统提供位置参考，系统依靠加速度计传感器A1进行位置修正。在灭火机器人快速搜寻转弯过程中加速度计传感器A1实时记录其瞬时加速度，控制器然后通过积分和二次积分得到其速度和位置信息，并通过与设定位置的速度和角度对比，当灭火机器人快速搜寻脱离了设定位置时，在新的采样周期内，系统按照偏差大小微调直流无刷电机X和直流无刷电机Y的PWM波输入，两轴伺服系统开始进行实时补偿来调整灭火机器人的姿态，使其完成弧度ARC；

当到达既定目标后，系统依靠超声波传感器S2、S3开始导航，控制器把直线行走很短的距离 R90_Passing按照控制器不同搜寻速度和加速度要求生成速度-时间运动梯形图，这个梯形包含的面积就是灭火机器人四个直流无刷电机X、直流无刷电机Y、四个直流无刷电机Z、直流无刷电机R要运动的距离，STM32F407根据这个梯形图再结合光电编码器和电机电流反馈生成驱动四轴直流无刷电机的PWM波，然后STM32F407使能驱动芯片驱动两个独立直流无刷电机X、直流无刷电机Y、四个直流无刷电机Z、直流无刷电机R向前运动，在向前运动过程中，超声波传感器S2、S3实时对左右的挡墙进行判断，并反馈当前房间挡墙信息，灭火机器人进入单左墙导航模式，然后再结合设定的左挡墙导航阀值，加速度计传感器A1记录灭火机器人实时的加速度信号并送给控制器，控制器分别通过积分和二次积分得到瞬时角速度和角度，记录灭火机器人的瞬时加速度、速度和位置信息，当灭火机器人快速探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速计传感器A1开始进行实时补偿，微调四个直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置；如果灭火机器人在校正位置阶段出现失速或者房间地面灰尘较多的情况，STM32F407会调节M加大灭火机器人与地面的摩擦并使能前后驱的四轮PWM波控制信号，控制器开启直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W，系统进入六轮驱动状态，STM32F407根据时间和速度要求，把剩余的距离D转化为六轴直流无刷电机的PWM波，使直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U和直流无刷电机W以相同的速度前进，灭火机器人在六驱状态下依旧按照原有的左挡墙导航模式前进，加速度计传感器A1实时记录灭火机器人的瞬时运动加速度，通过积分和二次积分得到其速度和位置信号，当灭火机器人六轮探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差借助加速度计传感器A1开始进行实时补偿，微调六轴直流无刷电机的PWM波输入，通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置；当到达既定目标后完成整个右转弯的轨迹曲线运动，然后控制其开始更新其坐标和方向；

如果在坐标（X，Y）时的方向为北，则更新其坐标为（X+1，Y），新坐标方向为东；如果在坐标（X，Y）时的方向为东，则更新其坐标为（X，Y-1），新坐标方向为南；如果在坐标（X，Y）时的方向为南，则更新其坐标为（X-1，Y），新坐标方向为西；如果在坐标（X，Y）时的方向为西，则更新其坐标为（X，Y+1），新坐标方向为北；

光电传感器S7开始工作，对新坐标下的光源进行判断，如果光电传感器S7捕捉到信号将通知控制器发现目标，控制器会让光电传感器S7禁止工作2秒工作，2秒后重新开启光电传感器S7，如果再次捕捉到光电信号，控制器会开启灭火机器人携带的干冰灭火器的电磁阀，开始对蜡烛进行喷洒干冰直至光源消失；如果光电传感器S7没有捕捉到新坐标下的光源，灭火机器人将离开目前房间格，将继续搜寻并更新其坐标；

10）在灭火机器人沿着当前方向向前运动过程中如果有挡墙进入前方的运动范围，并且此时左右的超声波传感器S2和S3判断左边无挡墙，而超声波传感器S4、S5判断右方有挡墙时，灭火机器人将存储此时坐标（X，Y），然后灭火机器人将按照图8的曲线行走；

在左转弯时，控制器控制电机行走规律与右转类似，当通过三段法到达既定目标后完成整个左转弯的轨迹曲线运动，控制器按照图6的设置开始更新其坐标和方向。

如果在坐标（X，Y）时的方向为北，则更新其坐标为（X-1，Y），新坐标方向为西；如果在坐标（X，Y）时的方向为东，则更新其坐标为（X，Y+1），新坐标方向为北；如果在坐标（X，Y）时的方向为南，则更新其坐标为（X+1，Y），新坐标方向为东；如果在坐标（X，Y）时的方向为西，则更新其坐标为（X，Y-1），新坐标方向为南；

光电传感器S7开始工作，对新坐标下的光源进行判断，如果光电传感器S7捕捉到信号将通知控制器发现目标，控制器会让光电传感器S7禁止工作2秒工作，2秒后重新开启光电传感器S7，如果再次捕捉到光电信号，控制器会开启携带的干冰灭火器的电磁阀，开始对蜡烛进行喷洒干冰直至光源消失；如果光电传感器S7没有捕捉到新坐标下的光源，灭火机器人将离开目前房间格，将继续搜寻并更新其坐标；

11）当灭火机器人搜到光源，并喷洒干冰完成灭火后，灭火机器人会停在目标点，然后控制器调出灭火机器人已经搜索的路径，并舍弃未搜寻的目标，通过洪水算法找出已经搜寻过房间的最佳路径，然后灭火机器人按照此路径快速回到搜寻起点，STM32F407会根据灭火机器人的速度自动调节直流电机M的伺服控制，自动改变真空吸盘对地面的吸附能力；

12）在灭火机器人在房间搜寻火源过程中，控制器会对高速直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W、直流电机M的转矩进行在线辨识，由于系统采用的是三闭环伺服控制，如果转矩出现脉动，控制器会利用直流电机力矩与电流的关系对干扰进行线性补偿，有效减少了电机转矩抖动对灭火机器人快速探索时导航的影响，增加了其抗干扰能力；

13）当灭火机器人完成整个返程过程回到起始点（0，0），STM32F407将控制灭火机器人中心点停车，并重新调整驱动功率桥的电平，使得直流无刷电机X和直流无刷电机Y以相反的方向运动，灭火机器人加速度计传感器A1的控制下，原地旋转180度，原地自锁，等待下一个搜寻命令。

本发明具有的有益效果是：

1：在运动过程中，充分考虑了电池在这个系统中的作用，基于STM32F407控制器时刻都在对灭火机器人的运行状态进行监测和运算，并且在电池提供电源的过程中，电流传感器C1-C7时刻对电池的电流进行观测并送给控制器进行保护，避免了大电流的产生，所以从根本上解决了大电流对锂离子电池的冲击，避免了由于大电流放电而引起的锂离子电池过度老化现象的发生；

2：用直流无刷电机替代了步进电机，使得电机无机械摩擦，无磨损，无电火花，且免维护，而且直流无刷电机的效率高，功率和转矩密度高，使得系统的效率更高；

3：由STM32F407处理灭火机器人的六只直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W、单轴吸附控制直流电机M的伺服控制，使得控制比较简单，大大提高了运算速度，解决了单片机软件运行较慢的瓶颈；

4：本发明基本实现全贴片元器件材料，为了增加系统的处理速度，系统采用两片STM32F407来实现各种算法，二者之间实时通讯，实现了单板控制，不仅节省了控制板占用空间，而且有利于灭火机器人体积和重量的减轻；

5：由于采用直流无刷电机，使得调速范围比较宽，调速比较平稳；

6：由于本控制器采用两片STM32F407处理六轴直流无刷电机和单轴直流电机伺服大量的数据与算法，有效地防止了程序的“跑飞”，抗干扰能力大大增强；

7：在灭火机器人实际运动过程中，STM32F407可以根据灭火机器人外围运行情况适时调整内部三闭环伺服控制的PID参数，实现分段P、PD、PID控制和非线性PID控制，使系统满足中低速运行时速度的切换；

8、在此灭火机器人系统引入了三轴加速度计传感器A1，通过积分和二次积分可以近似得到灭火机器人的角速度和角度信息，实现了灭火机器人在房间探索时的瞬时加速度、速度的间接检测，并利用反馈实现全程导航和二次补偿，有利于提高灭火机器人的稳定性和动态性能；

9：在灭火机器人运行过程中，STM32F407会对高速直流无刷电机X、直流无刷电机Y、直流无刷电机Z、直流无刷电机R、直流无刷电机U、直流无刷电机W和直流电机M的转矩进行在线辨识并利用电机力矩与电流的关系进行补偿，减少了电机转矩抖动对灭火机器人快速探寻火源的影响；

10：STM32F407通过调节直流电机M可以有效调节真空吸盘对地面的吸附能力，消除了灭火机器人在高速搜寻火源时打滑现象的发生；

11：根据灭火机器人搜寻功率需要实现分时多驱，在正常搜寻时，由于所求功率较小，灭火机器人一般会采用释放前后四轮，采用中置动力的两轮驱动的方式；而一旦遇到稍微提速时，STM32407会自动检测并立即将灭火机器人需求扭矩分配给后置两个助力直流无刷电机，同时控制器改变电机M的伺服控制，灭火机器人系统自然切换到中驱+后驱的四轮驱动状态，增强了灭火机器人的搜寻时的可靠性；而一旦遇到路面灰尘较多或灭火机器人速度较高时，STM32407会自动检测并立即将灭火机器人需求扭矩分配给前后置四个助力直流无刷电机，同时控制器改变直流电机M的伺服控制，灭火机器人系统自然切换到前驱+中驱+后驱的六轮驱动状态，进一步增强了灭火机器人搜寻时的可靠性；

12：由于采用前驱+中驱+后驱的复合六轮驱动方式，当需要加速探寻或者是返回起点时，控制器把动力分配到六个直流无刷电机，一旦一个动力轮由于地面、机械结构等造成暂时离开地面，STM32407可以重新分配扭矩，把更多的扭矩分配在未失速的驱动轮上，使系统迅速脱离不稳定状态，重新回到六轴动力平衡状态，使得灭火机器人具有更好的探寻行走功能；

13：灭火机器人转向时，为了保证旋转的稳定性，采用中置的两个直流无刷电机实现搜寻转弯，并释放前后置的四个助力直流无刷电机。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (7)

1.一种基于STM32F407六轮灭火机器人伺服控制器，其特征在于，包括电池、处理器、第一电机、第二电机、第三电机、第四电机、第五电机、第六电机、第七电机以及灭火机器人，所述的电池单独提供电流驱动所述的处理器，所述的处理器采用STM32F407，所述的处理器分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号分别控制所述的第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机的信号合成之后再控制灭火机器人的运动，其中，所述的第一电机、第二电机、第三电机、第五电机、第六电机和第七电机采用直流无刷电机，所述的第四电机采用直流电机。

2.根据权利要求1所述的基于STM32F407六轮灭火机器人伺服控制器，其特征在于，所述的电池采用锂离子电池。

3.根据权利要求1所述的基于STM32F407六轮灭火机器人伺服控制器，其特征在于，所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号和第七控制信号均为PWM波控制信号。

4.根据权利要求1所述的基于STM32F407六轮灭火机器人伺服控制器，其特征在于，所述的处理器的内部还设置有上位机系统和运动控制系统，所述的上位机系统包括房间探索模块、房间存储模块、路径读取模块、人机界面模块以及在线输出模块，所述的运动控制系统包括七轴混合伺服控制模块、坐标定位模块以及I/O控制模块，其中，所述的七轴混合伺服控制模块包括六轴直流无刷电机灭火机器人搜寻伺服控制模块和单轴真空吸盘吸附伺服控制模块。

5.根据权利要求1所述的基于STM32F407六轮灭火机器人伺服控制器，其特征在于，所述的六轮灭火机器人伺服控制器还包括超声波传感器、电流传感器、光电传感器、电压传感器以及加速度计传感器，所述的超声波传感器、电流传感器、光电传感器、电压传感器以及加速度计传感器均与处理器通讯连接。

6.根据权利要求5所述的基于STM32F407六轮灭火机器人伺服控制器，其特征在于，所述的超声波传感器的数量为6个、电流传感器的数量为7个、光电传感器、电压传感器以及加速度计传感器的数量均为1个。

7.根据权利要求1所述的基于STM32F407六轮灭火机器人伺服控制器，其特征在于，所述的六轮灭火机器人伺服控制器还包括光电编码器，所述的光电编码器分别安装在第一电机、第二电机、第三电机、第四电机、第五电机、第六电机和第七电机上。