于两轮高速全自动灭火机器人的双核伺服控制系统的具体实施步骤是:
[0072]对于本文设计的STM32F407+FPGA控制器，在电源打开状态下，灭火机器人先进入自锁状态。控制器首先开启真空抽吸电机M，通过抽吸装置先对微型真空吸盘抽吸，使真空吸盘对地面具有一定的吸附能力。灭火机器人通过超声波传感器Sl、S6判断前方运动环境，实际导航环境被转化为控制参数传输给FPGA，FPGA把这些环境参数转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的位置、速度和加速度指令值，FPGA再结合电流传感器Cl、C2和电机光电编码器的反馈生成控制直流无刷电机X和直流无刷电机Y的PWM控制信号，控制信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y向前运动，加速度计A1、陀螺仪G1和方向传感器D1和光电编码器实时把测得的信号反馈给FPGA，由FPGA二次校正灭火机器人的姿态。灭火机器人在运动过程中，FPGA根据灭火机器人运动速度自动通过电机Μ调节真空装置对地面的吸附能力，增加有效摩擦，防止灭火机器人快速行走打滑，STM32F407实时储存房间信息。当控制器发现火源后，图像采集系统开启。控制器在图像采集帮助下，自动调整灭火器喷嘴与火源的角度，同时升降电机Ε自动调节灭火器的高度，然后自动开启干冰灭火器电磁阀，通过喷洒干冰灭火，灭火完毕后STM32F407立即调出灭火机器人已经储存的房间信息，通过洪水算法找出返程最短路径，并开启加速模式迅速回到起点等待一下条寻求火源命令。
[0073]基于两轮高速全自动灭火机器人的双核伺服控制系统的具体的功能实现如下:
[0074]1)为了能够驱动两轮灭火机器人运动并解决高速探寻房间打滑问题，本控制系统引入了 FPGA，由其产生四路控制直流无刷电机和直流电机的PWM波控制信号，FPGA通过1/0 口与STM32F407进入实时通讯，由STM32F407控制其开通和关断。
[0075]2)打开电源瞬间，STM32F407会对电池电压进行检测，如果低压的话，FPGA将封锁直流无刷电机X和直流无刷电机Y的PffM波控制信号，电机不能启动，同时电压传感器VI将工作，并发出报警信号。如果系统电压正常，FPGA将开启真空抽吸电机Μ的PWM波控制信号，通过抽吸装置先对微型真空吸盘抽吸，使真空吸盘对地面具有一定的吸附能力，满足灭火机器人探寻火源的速度要求。
[0076]3)在灭火机器人未接到探索命令之前，它一般会在起点坐标(0，0)等待控制器发出的探索命令，一旦接到任务后，会沿着起点开始为了寻找火源而进行全宫探索。
[0077]4)灭火机器人放在起点坐标(0，0)，一般情况下，灭火机器人按照图6中北的方向(计算机编程代码为0)放置，接到任务后其前方的传感器Sl、S6和会对前方的环境进行判断，确定有没有挡墙进入运动范围，如存在挡墙将向STM32F407发出存储命令，STM32F407会对中断做第一时间响应，然后通过调整FPGA的输出控制直流无刷电机X正转，直流无刷电机Υ反转，灭火机器人在加速度传感器Α1、陀螺仪G1和方向传感器D1的控制下向右旋转90度，灭火机器人首先沿着X轴正向(东的方向，计算机编程代码为2)搜寻火源。
[0078]5)在灭火机器人运动过程中，装在直流无刷电机X和直流无刷电机Υ上的光电编码器会输出其位置信号Α和位置信号B，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，FPGA的位置寄存器会根据直流无刷电机X和直流无刷电机Y的运行方向加1或者是减1 ;
[0079]6)在灭火机器人运动过程中，装在直流无刷电机X和直流无刷电机Y上的光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给FPGA寄存器，记录电机的绝对位置，然后换算成灭火机器人在房间中的具体位置。
[0080]7)为了能够实现灭火机器人准确的坐标计算功能，灭火机器人左右的传感器S2、S3和S4、S5会时刻对运动方向左右的房间挡墙和柱子进行探测，如果S2、S3或者S4、S5发现传感器信号发生了跃变，则说明灭火机器人进入了从有房间挡墙到无房间挡墙(或者是从无房间挡墙到有房间挡墙)状态的变化，STM32F407会根据灭火机器人当前运行状态精确补偿，彻底消除灭火机器人在复杂房间中探寻火源时已经累计的误差。
[0081]8)在灭火机器人沿着任何一个方向向前运动，在任何一个方格的中心如果确定没有挡墙进入前方的运动范围，则灭火机器人将存储其坐标(X，Y)，并把向前运动一格的位置参数送给FPGA，FPGA把向前一格参数转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的位置、速度和加速度指令值，FPGA再结合电流传感器C1、C2和电机光电编码器的反馈生成控制直流无刷电机X和直流无刷电机Y的PWM控制信号，控制信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y向前运动。在灭火机器人沿着当前房间格向前探索过程中，传感器S2、S3和S4、S5会对左右的挡墙进行判断，并记录储存当前搜寻房间挡墙信息，灭火机器人根据前进方向左右挡墙的房间信息进入单墙导航模式或者是双墙导航模式，然后再结合设定的左右挡墙导航阀值，加速度计A1、陀螺仪G1和方向传感器D1记录灭火机器人实时的加速度、速度和角度信号并送给FPGA，FPGA实时记录储存灭火机器人的瞬时加速度、速度和位置信息，当灭火机器人快速探索脱离了设定中心位置时，微处理器根据离开中心位置的偏差由FPGA转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的新的位置、速度和加速度指令值，FPGA再结合电流传感器Cl、C2、电机光电编码器、加速计传感器A1、陀螺仪G1和方向传感器D1的反馈，微调直流无刷电机X和直流无刷电机Y的PWM控制信号，控制信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y向前运动。通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置。当灭火机器人在加速度计A1的控制下精确运动一格到达新地址时，微处理器将更新其坐标。
[0082]如果在坐标(X，Y)时的方向为北，在更新其坐标为(Χ，Υ+1)，新坐标方向依旧为北；如果在坐标(Χ，Υ)时的方向为东，在更新其坐标为(Χ+1，Υ)，新坐标方向依旧为东；如果在坐标(Χ，Υ)时的方向为南，在更新其坐标为(X，Υ-1)，新坐标方向依旧为南；如果在坐标(Χ，Υ)时的方向为西，在更新其坐标为(Χ_1，Υ)，新坐标方向依旧为西；
[0083]9)在灭火机器人沿着当前方向向前运动过程中如果传感器S1和S6判断前方有挡墙进入运动范围，并且此时传感器S2、S3、S4、S5分别判断左右都有挡墙时，灭火机器人将存储此时坐标(X，Υ)，根据传感器S1和S6的反馈计算出向前运动停车的位置参数YS1，由FPGA根据探索控制器速度和加速度把向前停车距离参数转化为灭火机器人直流无刷电机X和直流无刷电机Υ要运行的位置、速度和加速度指令值，FPGA再结合电流传感器C1、C2和电机光电编码器的反馈生成控制直流无刷电机X和直流无刷电机Y的PWM控制信号，控制信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y向前运动。在灭火机器人沿着当前房间格向前探索过程中，传感器S2、S3和S4、S5对左右挡墙进行判断，STM32F407实时记录储存当前搜寻房间挡墙信息，灭火机器人根据前进方向左右挡墙的房间信息进入双墙导航模式，然后再结合设定的左右挡墙导航阀值，加速度计A1、陀螺仪G1和方向传感器D1记录灭火机器人实时的加速度、速度和角度信号并送给FPGA，FPGA实时记录储存灭火机器人的瞬时加速度、速度和位置信息，当灭火机器人快速停车脱离了设定中心位置时，微处理器根据离开中心位置的偏差由FPGA转化为灭火机器人直流无刷电机X和直流无刷电机Y要运行的新的位置、速度和加速度指令值，FPGA再结合电流传感器C1、C2、电机光电编码器、加速计传感器A1、陀螺仪G1和方向传感器D1的反馈，微调直流无刷电机X和直流无刷电机Y的PWM控制信号，控制信号经驱动桥放大后驱动直流无刷电机X和直流无刷电机Y向前运动。通过此方式可以精确调整灭火机器人的姿态，使其重新回到设定中心位置。灭火机器人实现在设置停车点停车。STM32F407通过FPGA重新调整灭火机器人两个直流无刷电机的PWM波输出，使得两个永磁直流无刷电机运动方向相反，灭火机器人原地在加速计传感器A1的控制下实现精确的原地180度转向，然后灭火机器人沿着原先相反的方向运动。
[0084]如果在坐标(X，Y)时的方向为北，则更新其坐标为(Χ，Υ)，新坐标方向为南；如果在坐标(Χ，γ)时的方向为东，则更新其坐标为(XI，Υ)，新坐标方向为西；如果在坐标(Χ，Υ)时的方向为南，则更新其坐标为(Χ，γ)，新坐标方向为北；如果在坐标(Χ，γ)时的方向为西，则更新其坐标为(Χ，γ)，新坐标方向为东；
[0085]10)在灭火机器人沿着当前方向向前运动过程中如果有挡墙进入前方的运动范围，并且此时传感器S2和S3判断左边有挡墙，而S4、S5判断右方没有挡墙时，灭火机器人将存储此时坐标(X，Y)，然后灭火机器人将按照图7的曲线行走；为了充分发挥FPGA的快速处理数据能力，舍弃了基于MC58113和LM629的三段法转入法，为了使转弯更逼近于实际情况，本实用新型采用五段法完成转弯:转入前直线校正R90_Leading，转入弧度校正ARC11，转入弧度ARC12，转出弧度校正ARC13，转