CN105137983A - 双核高速两轮微微鼠冲刺控制器及其控制方法 - Google Patents
双核高速两轮微微鼠冲刺控制器及其控制方法 Download PDFInfo
- Publication number
- CN105137983A CN105137983A CN201510520975.1A CN201510520975A CN105137983A CN 105137983 A CN105137983 A CN 105137983A CN 201510520975 A CN201510520975 A CN 201510520975A CN 105137983 A CN105137983 A CN 105137983A
- Authority
- CN
- China
- Prior art keywords
- motor
- slight mouse
- spurt
- sensor
- stm32f407
- 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
Links
Landscapes
- Position Input By Displaying (AREA)
Abstract
本发明公开了双核高速两轮微微鼠冲刺控制器,包括电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块;所述电池装置分别电连接控制单元模块、电机X、电机Y和电机M,所述传感器装置和陀螺仪装置分别信号连接控制单元模块,所述控制单元模块分别信号连接电机X、Y和M。本发明通过位于底盘下的真空抽吸装置解决了微微鼠在行走中打滑的现象,同时根据微微鼠前进的速度以及地面情况自动调节真空抽吸直流电机M的伺服控制,使得微微鼠不在受制于迷宫地面路况。
Description
技术领域
本发明涉及微型迷宫冲刺机器人领域,尤其涉及一种双核高速两轮微微鼠冲刺控制器及其控制方法。
背景技术
微电脑鼠是使用嵌入式微控制器、传感器和机电运动部件构成的一种智能行走机器人,在国外已经竞赛了将近30年,其常采用两轮结构,两轮微电脑鼠二维结构如图1所示。
微电脑鼠可以在不同“迷宫”中自动记忆和选择路径,采用相应的算法,快速地到达所设定的目的地。其求解的迷宫之一示意如图2所示。
随着微电子技术、计算机控制技术的不断进步,国外专家在微电脑鼠求解迷宫的技术基础之上提出了一种更具有挑战性的迷宫机器人---微微鼠:为了增强迷宫复杂程度以及老鼠求解迷宫的难度,迷宫挡墙由原有的180mm变成了90mm,原有的迷宫由16*16格变成了32*32格,新的迷宫二维结构如图3所示。电源一旦打开,微微鼠全程完全依靠自身携带的传感器自动导航,并求解由1024个迷宫格组成的各种复杂迷宫,能够快速从起点找到一条到达设定目标点的最佳路径,然后以最快的速度冲刺到终点。作为一种自助导航智能机器人,因为通过无线装置可以向控制器输入迷宫信息,微微鼠或者微电脑鼠国际准则拒绝使用无线装置,为了能够得到微微鼠或者是微电脑鼠冲刺、冲刺后的信息,只能通过算法快速寄存并储存其行走信息,当完成任务后通过控制器的232串口或者是USB串口读取存储信息。
微微鼠在迷宫冲刺过程中要时刻判断周围的环境,然后传输参数到控制器,由控制器反复控制其在迷宫方格中精确的加速和减速进行运动。一只优秀的微微鼠必须具备良好的感知能力,有良好的行走能力,优秀的智能算法,否则将无法完成冲刺任务。微微鼠迷宫冲刺技术综合了多学科知识,对于提升在校学生的动手能力、团队协作能力和创新能力,促进学生课堂知识的消化和扩展学生的知识面都非常有帮助,并且微微鼠迷宫冲刺技术的开展可以培养大批相关领域的人才,进而促进相关领域的技术发展和产业化进程。
如果认为微微鼠只是微电脑鼠的简单拷贝,按照微电脑鼠技术来设计微微鼠,在实践中则会发现如下问题:
(1)基于轮式的微微鼠只能被动的适应迷宫地面的打滑程度,随着微微鼠冲刺速度的提高,其打滑概率也极大增加,导致求解迷宫失败。
(2)由于求解迷宫数目的大量增加,原有的微电脑鼠求解迷宫技术无法求解现有的复杂迷宫。
(3)由于微微鼠尺寸的大幅减少,如果微微鼠采用图1中的六组传感器技术探测冲刺时的迷宫,经常出现传感器相互干扰的状况,导致其读取迷宫信息失败。
(4)由于微电脑鼠冲刺伺服系统采用的都是比较低级的算法,使得微微鼠在迷宫当中的冲刺一般都要花费较长的时间,这使得在真正的大赛中无法取胜。
(5)由于迷宫挡墙尺寸的减少,使得微微鼠单格运行的距离减少,微微鼠冲刺过程中频繁的刹车和启动加重了单片机的工作量,单一的单片机无法满足微微鼠快速冲刺启动和停车的要求。
(6)对于两轮驱动的微微鼠来说一般要求驱动其冲刺的两个电机PWM控制信号要同步,受计算能力的限制单片机伺服系统很难满足这一条件,微微鼠在直道上行驶时不能准确的行走在中线上,在高速冲刺时很容易撞到迷宫挡墙,导致任务失败。
(7)由于受单片机容量和算法影响,微微鼠无法存储迷宫信息,当遇到掉电情况时所有的信息将消失,使得微微鼠无法完成冲刺任务。
(8)微微鼠在迷宫冲刺时,易于受到外界干扰,由于没有进行及时补偿导致微微鼠碰撞迷宫挡墙,最终无法完成冲刺任务。
(9)微微鼠在冲刺过程中,一旦遇到撞墙情况都会发生电机堵转情况,造成电机瞬间电流过大,严重时烧坏电机。
微微鼠求解迷宫是国际新兴的一门技术,由于微微鼠技术的难度较高以及迷宫设计的复杂性,导致国内还没有研发此机器人的单位。因此,需要设计一种满足初级者学习微微鼠求解迷宫的高速冲刺控制器。
发明内容
本发明的目的是解决现有技术的不足,提供一种双核高速两轮微微鼠冲刺控制器,以解决微微鼠在高速冲刺过程中打滑、传感器相互干扰、处理时间慢等问题。
本发明采用的技术方案是:基于STM32F407+FPGA芯片的全新控制模式,其程序框图如图4所示。全数字微微鼠两轴行走伺服控制系统以FPGA微处理器为核心,充分发挥FPGA数据处理速度较快的特点,使其全权处理三轴直流伺服系统的各种算法,把STM32F407从复杂的数据处理中解脱出来。其中伺服系统位置、速度、电流的给定值由微处理器STM32F407根据传感器S1、S2、S5、S6的导航值来生成,光电编码器反馈和电机的检测电流经FPGA内部算法解码后作为三闭环伺服控制的反馈值,经FPGA内部PID调节后生成控制三轴电机的PWM波。同时STM32F407实现部分的信号处理算法(直流电机转矩补偿等)和FPGA的控制逻辑,并响应中断,实现二者之间的数据通信和存储实时信号。
双核高速两轮微微鼠冲刺控制器,其特征是包括电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块;所述电池装置分别电连接控制单元模块、电机X、电机Y和电机M,所述传感器装置和陀螺仪装置分别信号连接控制单元模块,所述控制单元模块分别信号连接电机X、Y和M;所述电机X和电机Y位于两侧,电机M位于尾部,所述陀螺仪装置G位于中心位置;所述传感器装置包括位于两侧的红外传感器S1和S6,位于前端的红外传感器S2和S5,所述传感器S1、S6共同作用判断前方挡墙,传感器S2判断其左边挡墙的存在,传感器S5判断其右边挡墙的存在,同时S2和S5合作为直线运动提供导航依据;所述控制单元模块包括上位机程序模块和运动控制程序模块,所述上位机程序模块包括STM32F407处理器,所述运动控制程序模块包括FPGA处理器,所述FPGA处理器包括两轴行走伺服控制单元和单轴真空吸附伺服控制单元,所述STM32F407处理器电性连接FPGA处理器,所述两轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。
作为本发明的进一步改进,还包括有电压传感器V1,所述电压传感器V1电连接电池装置,信号连接控制单元模块的STM32F407处理器。
作为本发明的进一步改进,还包括光补偿传感器L1,所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。
作为本发明的进一步改进,还包括有电流传感器C1和C2,所述电流传感器C1和C2信号连接STM32F407处理器。
作为本发明的更进一步改进,所述FPGA处理器通过I/O端口与STM32F407处理器实时通讯,STM32F407处理器控制FPGA处理器开通和关断。
作为本发明的进一步改进,所述电机X和Y为高速永磁直流电机,所述电机M为微型直流电机。
作为本发明的进一步改进,所述红外传感器S1和S2信号发射方向间的夹角为75°~90°角,所述红外传感器S5和S6信号发射方向间的夹角为75°~90°角。
本发明采用的有益效果是:1、在运动过程中,充分考虑了电池在这个系统中的作用,基于STM32F407+FPGA控制器时刻都在对微微鼠的运行状态进行监测和运算,避免了大电流的产生,所以从根本上解决了大电流对锂离子电池的冲击,避免了由于大电流放电而引起的锂离子电池过度老化现象的发生。
2、由FPGA处理微微鼠数字伺服系统的行走伺服控制和真空吸附伺服控制,充分发挥FPGA控制方面的特长以及程序移植功能,使得控制比较简单,大大提高了运算速度,解决了单片机软件运行较慢的瓶颈,缩短了开发周期短,并且程序可移植能力强。
3、本发明基本实现全贴片元器件材料,实现了单板控制,不仅节省了控制板占用空间,而且有利于体积和重量的减轻,有利于提高微微鼠伺服系统的稳定性和动态性能。
4、由于本控制器采用FPGA处理两轮微微鼠全数字三轴伺服系统大量的数据与算法,把STM32F407从繁重的工作量中解脱出来,有效地防止了程序的“跑飞”,三轴两轮微微鼠全数字伺服系统抗干扰能力大大增强。
6、由FPGA输出PWM调制信号和方向信号,通过驱动电路可以直接驱动直流电机,不仅减轻了STM32F407的负担,简化了接口电路,而且省去了STM32F407内部编写位置、速度控制程序,以及各种PID算法的麻烦,使得系统的调试简单。
7、通过调节电机M可以有效调节真空吸盘对地面的吸附力,杜绝了微微鼠在行走时打滑现象的发生。
8、在微微鼠运行过程中,STM32F407会对直流电机X和电机Y的转矩进行在线辨识并利用直流电机力矩与电流的关系进行补偿,减少了电机转矩抖动对微微鼠快速冲刺伺服系统的影响。
9、在控制中,STM32F407根据传感器S1、S2、S5、S6的反馈值确定微微鼠快速冲刺时偏离中心位置的偏移量,然后根据不同的偏移量实时调整FPGA内部的PID参数,轻松实现分段P、PD、PID控制和非线性PID控制,使系统具有一定的自适应。
10、FPGA的中断命令可以很好的解决微微鼠在运行过程中遇到撞墙情况发生的电机堵转,利用中断命令可在检测到电机电流输出超出设定值时发出请求,然后STM32F407立即封锁FPGA的PWM输出,释放高速直流电机X和电机Y,从而有效地解决了堵转问题。
11、由于具有存储功能,这使得微微鼠掉电后可以轻易的调取已经探索好的迷宫信息,使二次探索的时间和路径大大降低。
附图说明
图1为原二轮六眼微电脑鼠二维图。
图2为微电脑鼠16*16迷宫示意图。
图3为微微鼠32*32迷宫示意图
图4为微微鼠伺服控制系统控制器程序框图。
图5为本发明示意图。
图6为本发明的原理框图。
图7为本发明的STM32F407处理器与FPGA的连接原理图。
图8为微微鼠前进示意图。
图9为微微鼠反向前进示意图。
图10为微微鼠右转示意图。
图11为微微鼠左转示意图。
具体实施方式
下面结合图1至图11,对本发明做进一步的说明。
STM32F4系列除引脚和软件兼容高性能的F2系列外,F4的主频(168MHz)高于F2系列(120MHz)、F1系类(78MHz),并支持单周期DSP指令和浮点单元、更大的SRAM容量(192KB,F2为128KB)、512KB-1MB的嵌入式闪存以及影像、网络接口和数据加密等更先进的外设。STM32F4系列基于最新的ARMCortexM4内核,在现有出色的STM32微控制器产品组合中新增了信号处理功能,并提高了运行速度;STM32F405x集成了定时器、3个ADC、2个DAC、串行接口、外存接口、实时时钟、CRC计算单元和模拟真随机数发生器在内的整套先进外设。STM32F407在STM32F405产品基础上增加了多个先进外设。这些性能使得F4系列可以较容易满足控制和信号处理功能混合的数字信号控制需求。高效的信号处理功能与Cortex-M4处理器系列的低能耗、低成本和易于使用的优点的组合,使得其可以为多轴电动机控制提供灵活解决方案。这些特点使得STM32F407相对于其它STM32F系类更适合微微鼠两轴伺服系统的信号处理。
FPGA是英文FieldProgrammableGateArray的缩写,即现场可编程门阵列,是在PAL、GAL、EPLD等可编程器件的基础上进一步发展的产物。它是作为专用集成电路(ASIC)领域中的一种半定制电路而出现的,即解决了定制电路的不足,又克服了原有可编程器件门电路数有限的缺点。
FPGA使得用户可以根据自己的设计需要,通过特定的布局布线工具对其内部进行重新组合连接,在最短的时间内设计出自己的专用集成电路,这样就减小成本、缩短开发周期。由于FPGA采用软件化的设计思想实现硬件电路的设计,这样就使得基于FPGA设计的系统具有良好的可复用和修改性,特别适合高速控制的多轴伺服系统使用。
如图所示,双核高速两轮微微鼠冲刺控制器,包括电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块;所述电池装置分别电连接控制单元模块、电机X、电机Y和电机M,所述传感器装置和陀螺仪装置分别信号连接控制单元模块,所述控制单元模块分别信号连接电机X、Y和M;所述电机X和电机Y位于两侧,电机M位于尾部,所述陀螺仪装置G位于中心位置;所述传感器装置包括位于两侧的红外传感器S1和S6,位于前端的红外传感器S2和S5,所述传感器S1、S6共同作用判断前方挡墙,传感器S2判断其左边挡墙的存在,传感器S5判断其右边挡墙的存在,同时S2和S5合作为直线运动提供导航依据;所述控制单元模块包括上位机程序模块和运动控制程序模块,所述上位机程序模块包括STM32F407处理器,所述运动控制程序模块包括FPGA处理器,所述FPGA处理器包括两轴行走伺服控制单元和单轴真空吸附伺服控制单元,所述STM32F407处理器电性连接FPGA处理器,所述两轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。
为了保护大电流对电池装置的冲击,还包括有电压传感器V1,所述电压传感器V1电连接电池装置,信号连接控制单元模块的STM32F407处理器。
为了减少和杜绝外界光源对传感器的干扰,从而影响整个系统的使用,还包括光补偿传感器L1,所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。
为了进一步使得本发明的伺服控制器精确控制微微鼠的前进和转向,还包括有电流传感器C1和C2,所述电流传感器C1和C2信号连接STM32F407处理器。
为进一步拓展两核处理器的处理能力,做到分工明确,工作不受干扰,所述FPGA处理器通过I/O端口与STM32F407处理器实时通讯,STM32F407处理器控制FPGA处理器开通和关断。
为了进一步提高微微鼠的性能,所述电机X和Y为高速永磁直流电机,所述电机M为微型直流电机。
为进一步消除红外传感器之间的干扰,提高传感器装置的感应精度,所述红外传感器S1和S2信号发射方向间的夹角为75°~90°角,所述红外传感器S5和S6信号发射方向间的夹角为75°~90°角。
将微微鼠放在迷宫起始点,在电源打开状态下,微微鼠先进入自锁状态,然后微微鼠鼠依靠前方、左右侧面红外蔽障传感器S1、S2、S5、S6根据实际导航环境传输参数给双核控制器中的STM32F407,然后STM32F407把实际导航环境转化为微微鼠两轴电机伺服系统控制的位置、速度和加速度参数指令值,然后与FPGA通讯,FPGA再结合光电编码器和电机电流的反馈,经其内部伺服控制算法处理两个独立电机的同步伺服控制,并把处理数据通讯给STM32F407。在整个运动过程中有陀螺仪G1实时进行位置检测和二次补偿,保证两轴微微鼠高速探测器的稳定性。
本发明是如下工作的:
双核两轮微微鼠冲刺控制器分为两部分:上位机系统和运动控制系统。其中上位机系统完成迷宫读取、坐标定位、在线输出等功能;运动控制系统完成微微鼠系统的伺服控制、数据存储、I/O控制等功能,其中工作量最大的两轴同步伺服控制单元和单轴真空吸附伺服控制单元交给FPGA处理,其余的包括上位机系统的完成交给STM32F407完成,这样就实现了STM32F407与FPGA的分工,同时二者之间也可以进行通讯,实时进行数据交换和调用。
1)在微微鼠未接到冲刺命令之前,传感器V1会首先检测电池电压,如果系统处于低压控制器将报警并提示充电。如果电压正常,微微鼠一般会处在起点坐标(0,0),并调出已经探索后的最优迷宫,控制器首先开启真空抽吸电机M,通过抽吸装置先对微型真空吸盘抽吸,使真空吸盘对地面具有一定的吸附力,控制器并实时检测,如果地面不干净,系统会自动调节电机M加大真空吸盘对地面的吸附力。当控制器发出的冲刺命令后,微微鼠会沿着最优路径开始快速向(F,F)、(F,10)、(10,F)、(10,10)冲刺。
2)微微鼠放在起点坐标(0,0),接到任务后为了防止放错冲刺方向,其前方的红外传感器S1、S6和会对冲刺路径上的前方环境进行判断,确定有无挡墙进入运动范围,如存在挡墙将向STM32F407发出中断请求,STM32F407会对中断做第一时间响应,然后禁止FPGA工作,封锁微微鼠的高速直流电机X和电机Y的PWM驱动信号,使微微鼠静止在原地,然后控制器二次判断迷宫确定前方信息,防止信息误判;如果没有挡墙进入前方的运动范围,微微鼠将开启复位电路,调取正常的迷宫信息,准备进行正常的冲刺。
3)在微微鼠沿着Y轴向前快速冲刺过程中如果迷宫信息中显示前方有Z格直线坐标下没有挡墙进入前方的运动范围,微微鼠将存储其现在的坐标(X,Y),并把向前运动Z格的位置参数并把此位置参数传输给STM32F407,由STM32F407根据冲刺控制器速度和加速度的要求,把Z格距离参数转化为两轴三闭环伺服系统的指令给定值传输给FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈生成驱动直流电机运动的PWM波,PWM波通过驱动桥放大后推动微微鼠向前冲刺。在微微鼠向前冲刺过程中,传感器S2和S5会对左右的迷宫挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据左右挡墙的迷宫信息确定其进入单墙导航模式、双墙导航模式或者是惯性导航模式。在微微鼠快速冲刺过程中陀螺仪实时记录其瞬时速度和位置,当微微鼠快速冲刺脱离了设定中心位置时,在新的采样周期内,STM32F407根据当前S2、S5的状态重新生成三闭环伺服系统的指令给定值并传输给FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈微调电机的PWM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置。当微微鼠在陀螺仪G1的控制下完成冲刺到达新地址时,微处理器将更新其坐标为(X,Y+Z),在Y+Z<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续执行新的冲刺命令,如果是则通知控制器已经冲刺到终点,然后置返航探索标志为1,微微鼠准备返程探索;
4)在微微鼠沿着Y轴反向向前快速冲刺过程中如果迷宫信息中显示前方有Z格直线坐标下没有挡墙进入前方的运动范围,微微鼠将存储其现在的坐标(X,Y),微微鼠沿着Y轴反向向前快速冲刺原理与微微鼠沿着Y轴正向向前快速冲刺原理相似,当微微鼠在陀螺仪G1的控制下完成冲刺到达新地址时,微处理器将更新其坐标为(X,Y-Z),在Y-Z<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续执行新的冲刺命令,如果是则通知控制器已经冲刺到终点,然后置返航探索标志为1,微微鼠准备返程探索;
5)在微微鼠沿着Y轴向前运动过程中如果有挡墙进入前方的运动范围,并且此时迷宫信息中左方有挡墙时,微微鼠将存储此时坐标(X,Y),然后进入图10所示的曲线运动轨迹,在右冲刺转弯时,STM32F407首先把行走直线很短的距离DashTurn_R90_Leading按照各种冲刺条件不同的要求转化为速度参数以及加速度参数传输给控制左右轮的FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈生成驱动直流电机运动的PWM波,PWM波通过驱动桥放大后推动微微鼠向前冲刺。在微微鼠向前冲刺过程中,传感器S2对左迷宫挡墙进行检测,并记录储存当前迷宫挡墙信息,微微鼠根据左挡墙的迷宫信息确定其进入单墙导航模式。在微微鼠快速冲刺过程中陀螺仪实时记录其瞬时速度和位置,当微微鼠快速冲刺脱离了设定中心位置时,在新的采样周期内,STM32F407根据当前S2的状态重新生成三闭环伺服系统的指令给定值并传输给FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈微调电机的PWM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置。当到达既定目标时,传感器参考值R90_FrontWallRef开始工作,防止外界干扰开始做误差补偿。误差补偿结束后开始调整直流电机X和直流电机Y速度使其完成弧度ARC的运动曲线,STM32F407首先把弧度ARC按照各种冲刺条件不同的要求转化为速度参数以及加速度参数传输给控制左右轮的FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈生成驱动直流电机运动的PWM波,PWM波通过驱动桥放大后推动微微鼠完成转弯冲刺。在微微鼠转弯冲刺过程中,传感器S1、S2、S5、S6关闭,系统进入陀螺仪惯性导航模式。在微微鼠快速冲刺过程中陀螺仪实时记录其瞬时速度和位置,并通过积分与设定位置的速度和角度对比,通过FPGA内部的调节器进行调整,当微微鼠快速冲刺脱离了设定位置时,在新的采样周期内,系统按照偏差大小微调电机的PWM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其完成弧度ARC;当到达既定目标后,系统开启传感器S2,控制器把直线行走很短的距离DashTurn_R90_Passing按照各种冲刺条件不同的要求转化为速度参数以及加速度参数传输给控制左右轮的FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈生成驱动直流电机运动的PWM波,PWM波通过驱动桥放大后推动微微鼠向前冲刺。在微微鼠向前冲刺过程中,传感器S3对左迷宫挡墙进行检测,并记录储存当前迷宫挡墙信息,微微鼠根据左挡墙的迷宫信息确定其进入单墙导航模式。在微微鼠快速冲刺过程中陀螺仪实时记录其瞬时加速度、速度和位置,当微微鼠快速冲刺脱离了设定中心位置时,在新的采样周期内,STM32F407根据当前S2的状态重新生成三闭环伺服系统的指令给定值并传输给FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈微调电机的PWM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置。当到达既定目标后完成整个右转弯的轨迹曲线运动。此时将更新其坐标为(X+1,Y),在X+1<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续执行新的冲刺命令,如果是则通知控制器已经冲刺到终点,然后置返航探索标志为1,微微鼠准备返程探索;
6)在微微鼠沿着Y轴向前运动过程中如果有挡墙进入前方的运动范围,并且此时迷宫信息中左方无挡墙而有右方有挡墙时,微微鼠将存储此时坐标(X,Y),微微鼠进入左转冲刺状态,微微鼠左转快速冲刺原理与微微鼠右转快速冲刺原理相似,通过三段法完成整个左转弯轨迹曲线运动,此时将更新其坐标为(X-1,Y),在X-1<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续执行新的冲刺命令,如果是则通知控制器已经冲刺到终点,然后置返航探索标志为1,微微鼠准备返程探索;
7)当微微鼠冲刺到达(F,F)、(F,10)、(10,F)、(10,10)后会准备冲刺后的返程探索以便搜寻更优的路径,控制器会调出其已经存储的迷宫信息,然后计算出可能存在的其它最佳路径,然后返程开始进入其中认为最优的一条。
8)为了能够实现微微鼠准确的坐标计算功能,在冲刺过程中,微微鼠左右的传感器S2和S5会时刻对周围的迷宫挡墙和柱子进行探测,如果S2或者S5发现传感器信号发生了跃变,则说明微微鼠进入了迷宫挡墙和柱子的交接点,老鼠准备离开当前的迷宫格子,STM32F407会根据微微鼠当前运行的距离进行精确补偿,此方法可以彻底消除微微鼠在已经冲刺路径中行驶累计的误差,为微微鼠准确冲刺奠定了基础。
9)为了能够减少光源对微微鼠冲刺的干扰,本发明加入了光电传感器了L1,此传感器会在微微鼠冲刺阶段对周围的异常光源进行读取,并自动送给控制器做实时补偿,消除了外界光源对冲刺的干扰。
10)在微微鼠运行过程中,STM32F407会对直流电机X和电机Y的转矩进行在线辨识,当电机的转矩受到外界干扰出现较大抖动时,控制器会利用直流电机力矩与电流的关系进行时候补偿,减少了电机转矩抖动对微微鼠高速冲刺的影响。
11)当微电脑完成整个冲刺过程到达(F,F)、(F,10)、(10,F)、(10,10),微微鼠会置探索标志为1,微微鼠返程探索回到起始点(0,0),STM32F407将控制FPGA使得微微鼠在起始坐标(0,0)中心点停车,然后重新调整FPGA的PWM波输出,使得电机X和电机Y以相反的方向运动,并在陀螺仪的控制下,原地旋转180度,然后停车1秒,二次调取迷宫信息,然后根据算法算出优化迷宫信息后的最优冲刺路径,然后置冲刺标志为1,系统进入二次快速冲刺阶段。然后按照冲刺----探索---冲刺,完成多次的冲刺,以达到快速冲刺的目的。
本领域技术人员应当知晓,本发明的保护方案不仅限于上述的实施例,还可以在上述实施例的基础上进行各种排列组合与变换,在不违背本发明精神的前提下,对本发明进行的各种变换均落在本发明的保护范围内。
Claims (9)
1.双核高速两轮微微鼠冲刺控制器,其特征是包括电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块;
所述电池装置分别电连接控制单元模块、电机X、电机Y和电机M,所述传感器装置和陀螺仪装置分别信号连接控制单元模块,所述控制单元模块分别信号连接电机X、Y和M;
所述电机X和电机Y位于两侧,电机M位于尾部,所述陀螺仪装置G位于中心位置;
所述传感器装置包括位于两侧的红外传感器S1和S6,位于前端的红外传感器S2和S5,所述传感器S1、S6共同作用判断前方挡墙,传感器S2判断其左边挡墙的存在,传感器S5判断其右边挡墙的存在,同时S2和S5合作为直线运动提供导航依据;
所述控制单元模块包括上位机程序模块和运动控制程序模块,所述上位机程序模块包括STM32F407处理器,所述运动控制程序模块包括FPGA处理器,所述FPGA处理器包括两轴行走伺服控制单元和单轴真空吸附伺服控制单元,所述STM32F407处理器电性连接FPGA处理器,所述两轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。
2.根据权利要求1所述的双核高速两轮微微鼠冲刺控制器,其特征是还包括有电压传感器V1,所述电压传感器V1电连接电池装置,信号连接控制单元模块的STM32F407处理器。
3.根据权利要求1所述的双核高速两轮微微鼠冲刺控制器,其特征是还包括光补偿传感器L1,所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。
4.根据权利要求1所述的双核高速两轮微微鼠冲刺控制器,其特征是还包括有电流传感器C1和C2,所述电流传感器C1和C2信号连接STM32F407处理器。
5.根据权利要求1至4中任意一项所述的双核高速两轮微微鼠冲刺控制器,其特征是所述FPGA处理器通过I/O端口与STM32F407处理器实时通讯,STM32F407处理器控制FPGA处理器开通和关断。
6.根据权利要求5所述的双核高速两轮微微鼠冲刺控制器,其特征是所述电机X和Y为高速永磁直流电机,所述电机M为微型直流电机。
7.根据权利要求1至4中任意一项所述的双核高速两轮微微鼠冲刺控制器,其特征是所述红外传感器S1和S2信号发射方向间的夹角为75°~90°角,所述红外传感器S5和S6信号发射方向间的夹角为75°~90°角。
8.一种如权利要求1所述的基于STM32F407和FPGA的双核两轮微微鼠冲刺控制方法,其特征是具体步骤如下:
1)检测电池电压,处于低压控制器将报警并提示充电;如果电压正常,则并调出已经探索后的最优迷宫;
2)冲刺方向检测,红外传感器S1、S6和会对冲刺路径上的前方环境进行判断,确定有无挡墙进入运动范围,如存在挡墙将向STM32F407发出中断请求,禁止FPGA工作,封锁微微鼠的高速直流电机X和电机Y的PWM驱动信号,使微微鼠静止在原地,然后二次判断迷宫确定前方信息,防止信息误判;
3)迷宫冲刺,微微鼠沿着X和Y轴快速冲刺,控制器判断其坐标是否为终点,如果不是将继续执行新的冲刺命令,如果是则通知控制器已经冲刺到终点,然后置返航探索标志为1,微微鼠准备返程探索;
4)返程冲刺,到达终点后会准备冲刺后的返程探索以便搜寻更优的路径,控制器会调出其已经存储的迷宫信息,然后计算出可能存在的其它最佳路径,然后返程开始进入其中认为最优的一条;
5)迷宫返程,在微微鼠进入迷宫返程探索时,其导航的传感器S1、S2、S5、S6将工作,并把反射回来的光电信号送给STM32F407,经STM32F407判断后送给FPGA,由FPGA运算后与STM32F407进行通讯,然后由控制器送控制信号给导航的电机X和电机Y进行确定:如果进入已经搜索的区域将进行快速前进,如果是未知返回区域则采用正常速度搜索,并时刻更新其坐标(X,Y),并判断其坐标是不是起点,如果是的话置返航探索标志为0,微微鼠进入冲刺阶段,并置冲刺标志为1;
6)二次冲刺,返程探索回到起始点,STM32F407将控制FPGA使得微微鼠在起始坐标中心点停车,然后重新调整FPGA的PWM波输出,使得电机X和电机Y以相反的方向运动,并在陀螺仪的控制下,原地旋转180度,然后停车1秒,二次调取迷宫信息,然后根据算法算出优化迷宫信息后的最优冲刺路径,然后置冲刺标志为1,系统进入二次快速冲刺阶段。
9.如权利要求8所述的基于STM32F407和FPGA的双核两轮微微鼠冲刺控制方法,其特征是迷宫冲刺阶段,传感器S2和S5会对左右的迷宫挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据左右挡墙的迷宫信息确定其进入单墙导航模式、双墙导航模式或者是惯性导航模式;在微微鼠快速冲刺过程中陀螺仪实时记录其瞬时加速度、速度和位置,当微微鼠快速冲刺脱离了设定中心位置时,在新的采样周期内,STM32F407根据当前S2、S5的状态重新生成三闭环伺服系统的指令给定值并传输给FPGA,然后FPGA再结合光电编码器以及电流传感器C1、C2的反馈微调电机的PWM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510520975.1A CN105137983A (zh) | 2015-08-24 | 2015-08-24 | 双核高速两轮微微鼠冲刺控制器及其控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510520975.1A CN105137983A (zh) | 2015-08-24 | 2015-08-24 | 双核高速两轮微微鼠冲刺控制器及其控制方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN105137983A true CN105137983A (zh) | 2015-12-09 |
Family
ID=54723358
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510520975.1A Pending CN105137983A (zh) | 2015-08-24 | 2015-08-24 | 双核高速两轮微微鼠冲刺控制器及其控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105137983A (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105911935A (zh) * | 2016-07-01 | 2016-08-31 | 江苏若博机器人科技有限公司 | 一种双核三轴四轮变结构高速微微鼠冲刺控制器 |
CN105929832A (zh) * | 2016-07-01 | 2016-09-07 | 江苏若博机器人科技有限公司 | 一种双核三轴四轮变结构高速微微鼠全数字伺服控制器 |
CN105974923A (zh) * | 2016-07-01 | 2016-09-28 | 江苏若博机器人科技有限公司 | 一种双核三轴四轮变结构高速微微鼠探索控制器 |
CN106950958A (zh) * | 2017-03-28 | 2017-07-14 | 歌尔科技有限公司 | 直线控制方法、装置及双轮机器人 |
CN107742997A (zh) * | 2017-10-20 | 2018-02-27 | 北京航天发射技术研究所 | 双轴转位机构控制系统、控制方法及捷联惯组自标定方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20060238156A1 (en) * | 2005-04-25 | 2006-10-26 | Lg Electronics Inc. | Self-moving robot capable of correcting movement errors and method for correcting movement errors of the same |
CN201886351U (zh) * | 2010-10-25 | 2011-06-29 | 陕西科技大学 | 一种用于迷宫冲刺的电脑鼠 |
CN103472830A (zh) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | 基于双核两轮微电脑鼠超快速探索控制器 |
CN103472837A (zh) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | 一种基于双核两轮微电脑鼠快速冲刺控制器 |
CN204086988U (zh) * | 2014-09-05 | 2015-01-07 | 西安建筑科技大学 | 一种用于迷宫解算的迷宫机器鼠系统 |
-
2015
- 2015-08-24 CN CN201510520975.1A patent/CN105137983A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20060238156A1 (en) * | 2005-04-25 | 2006-10-26 | Lg Electronics Inc. | Self-moving robot capable of correcting movement errors and method for correcting movement errors of the same |
CN201886351U (zh) * | 2010-10-25 | 2011-06-29 | 陕西科技大学 | 一种用于迷宫冲刺的电脑鼠 |
CN103472830A (zh) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | 基于双核两轮微电脑鼠超快速探索控制器 |
CN103472837A (zh) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | 一种基于双核两轮微电脑鼠快速冲刺控制器 |
CN204086988U (zh) * | 2014-09-05 | 2015-01-07 | 西安建筑科技大学 | 一种用于迷宫解算的迷宫机器鼠系统 |
Non-Patent Citations (1)
Title |
---|
PETER HARRISON: "Minos 2015 Presentations", 《MICROMOUSE ONLINE》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105911935A (zh) * | 2016-07-01 | 2016-08-31 | 江苏若博机器人科技有限公司 | 一种双核三轴四轮变结构高速微微鼠冲刺控制器 |
CN105929832A (zh) * | 2016-07-01 | 2016-09-07 | 江苏若博机器人科技有限公司 | 一种双核三轴四轮变结构高速微微鼠全数字伺服控制器 |
CN105974923A (zh) * | 2016-07-01 | 2016-09-28 | 江苏若博机器人科技有限公司 | 一种双核三轴四轮变结构高速微微鼠探索控制器 |
CN106950958A (zh) * | 2017-03-28 | 2017-07-14 | 歌尔科技有限公司 | 直线控制方法、装置及双轮机器人 |
CN107742997A (zh) * | 2017-10-20 | 2018-02-27 | 北京航天发射技术研究所 | 双轴转位机构控制系统、控制方法及捷联惯组自标定方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105334853A (zh) | 双核高速四轮微微鼠冲刺控制器 | |
CN105137975A (zh) | 六轮双核全自动高速灭火机器人伺服控制器 | |
CN105116918A (zh) | 一种双核两轮中速灭火机器人伺服控制器 | |
CN105159325A (zh) | 一种基于stm32f407和fpga两轮高速灭火机器人伺服控制器 | |
CN105137983A (zh) | 双核高速两轮微微鼠冲刺控制器及其控制方法 | |
CN105005249A (zh) | 一种全自动四轮双核高速灭火机器人伺服控制器 | |
CN105045289A (zh) | 一种单轮全自动高速灭火机器人伺服控制系统 | |
CN105045288A (zh) | 一种单轮全自动中速灭火机器人伺服控制系统 | |
CN105116897A (zh) | 双核高速四轮微微鼠全数字导航伺服控制器 | |
CN105137974A (zh) | 六轮双核全自动中速灭火机器人伺服控制器 | |
CN105320146A (zh) | 一种单轮双核中速灭火机器人伺服控制系统 | |
CN105116891A (zh) | 双核中速两轮微微鼠冲刺控制器及其控制方法 | |
CN105137988A (zh) | 单核低速两轮微微鼠全数字导航伺服控制器及控制方法 | |
CN105005308A (zh) | 基于stm32f407六轮灭火机器人伺服控制器 | |
CN105137979A (zh) | 双核中速六轮微微鼠冲刺控制器及其控制方法 | |
CN105204502A (zh) | 双核中速六轮微微鼠探索控制器及其控制方法 | |
CN105116898A (zh) | 单核低速两轮微微鼠冲刺控制器 | |
CN105159323A (zh) | 六轮单核全自动低速灭火机器人伺服控制器 | |
CN105137991A (zh) | 单核低速两轮微微鼠探索控制器 | |
CN105068558A (zh) | 一种单轮双核高速灭火机器人伺服控制系统 | |
CN105137978A (zh) | 双核高速四轮微微鼠探索控制器 | |
CN105159324A (zh) | 六轮双核中速灭火机器人伺服控制器 | |
CN105138017A (zh) | 基于两轮中速全自动灭火机器人的双核伺服控制系统 | |
CN105320147A (zh) | 一种单轮双核中低速灭火机器人伺服控制系统 | |
CN105137982A (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: 20151209 |