CN105137981A - 单核低速四轮微微鼠探索控制器及其控制方法 - Google Patents
单核低速四轮微微鼠探索控制器及其控制方法 Download PDFInfo
- Publication number
- CN105137981A CN105137981A CN201510520971.3A CN201510520971A CN105137981A CN 105137981 A CN105137981 A CN 105137981A CN 201510520971 A CN201510520971 A CN 201510520971A CN 105137981 A CN105137981 A CN 105137981A
- Authority
- CN
- China
- Prior art keywords
- motor
- slight mouse
- mouse
- stm32f407
- sensor
- 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、真空抽吸装置和控制单元模块;微微鼠的四轮其中一对位于微微鼠的中部,另一对位于微微鼠的尾部;所述陀螺仪装置包括陀螺仪G1和加速度计A1,所述陀螺仪G1为三轴陀螺仪用于测量三个转动方向运动,所述加速度计A1为三轴加速度计用于测量三个平移运动的加速度。本发明通过位于底盘下的真空抽吸装置解决了微微鼠在探索过程中打滑的现象,同时根据微微鼠前进的速度以及地面情况自动调节真空抽吸直流电机M的伺服控制,使得微微鼠不在受制于迷宫地面路况。
Description
技术领域
本发明涉及微型迷宫探索机器人领域,尤其涉及一种单核低速四轮微微鼠探索控制器及其控制方法。
背景技术
微电脑鼠是使用嵌入式微控制器、传感器和机电运动部件构成的一种智能行走机器人,在国外已经竞赛了将近30年,其常采用两轮结构,两轮微电脑鼠二维结构如图1所示。
微电脑鼠可以在不同“迷宫”中自动记忆和选择路径,采用相应的算法,快速地到达所设定的目的地。其求解的迷宫之一示意如图2所示。
随着微电子技术、计算机控制技术的不断进步,国外专家在微电脑鼠求解迷宫的技术基础之上提出了一种更具有挑战性的迷宫机器人---微微鼠:为了增强迷宫复杂程度以及老鼠求解迷宫的难度,迷宫挡墙由原有的180mm变成了90mm,原有的迷宫由16*16格变成了32*32格,新的迷宫二维结构如图3所示。电源一旦打开,微微鼠全程完全依靠自身携带的传感器自动导航,并求解由1024个迷宫格组成的各种复杂迷宫,能够快速从起点找到一条到达设定目标点的最佳路径,然后以最快的速度冲刺到终点。作为一种自助导航智能机器人,因为通过无线装置可以向控制器输入迷宫信息,微微鼠或者微电脑鼠国际准则拒绝使用无线装置,为了能够得到微微鼠或者是微电脑鼠探索、冲刺后的信息,只能通过算法快速寄存并储存其行走信息,当完成任务后通过控制器的232串口或者是USB串口读取存储信息。
微微鼠在迷宫中探索过程中要时刻判断周围的环境,然后传输参数到控制器,由控制器反复控制其在迷宫方格中精确的进行加速和减速运动。一只优秀的微微鼠必须具备良好的感知能力,有良好的行走能力,优秀的智能算法,否则将无法完成任务。微微鼠迷宫探索技术综合了多学科知识,对于提升在校学生的动手能力、团队协作能力和创新能力,促进学生课堂知识的消化和扩展学生的知识面都非常有帮助,并且微微鼠迷宫技术的开展可以培养大批相关领域的人才,进而促进相关领域的技术发展和产业化进程。
如果认为微微鼠只是微电脑鼠的简单拷贝,按照微电脑鼠技术来设计微微鼠,在实践中则会发现如下问题:
(1)基于轮式的微微鼠只能被动的适应迷宫地面的打滑程度,随着微微鼠速度的提高,其打滑概率也极大增加,导致求解迷宫失败。
(2)由于求解迷宫数目的大量增加,原有的微电脑鼠求解迷宫技术无法求解现有的复杂迷宫。
(3)由于微微鼠尺寸的大幅减少,如果微微鼠采用图1中的六组传感器技术探测迷宫,经常出现传感器相互干扰的状况,导致其读取迷宫信息失败。
(4)由于微电脑鼠探索控制器采用的都是比较低级的算法,使得微微鼠在迷宫当中的探索一般都要花费较长的时间,这使得在真正的大赛中无法取胜。
(5)由于受单片机容量影响,现有的微微鼠基本上都只有两个动力驱动轮,采用两轮差速方式行驶,使得***对两轴的伺服要求较高,特别是直线导航时,要求速度和加速度要追求严格的一致,否则直线导航将会失败,导致微微鼠出现撞墙的现象发生;
(6)两轮微微鼠***在加速时由于重心后移,使得老鼠前部轻飘,即使在良好的路面上微微鼠也会打滑,有可能导致撞墙的现象出现,不利于高速微微鼠的发展。
(7)两轮微微鼠***在正常行驶时如果设计不当造成重心前偏,将导致驱动轮上承受的正压力减小,这时微微鼠***更加容易打滑,也更容易走偏,导致导航失败。
(8)两轮微微鼠***在正常行驶时如果设计不当造成重心侧偏将导致两个驱动轮承受的正压力不同,在快速启动时两轮打滑程度不一致,瞬间就偏离轨迹,转弯时,其中正压力小的轮子可能打滑,导致转弯困难。
(9)由于采用两个动力轮驱动,为了满足复杂状态下的加速和减速,使得单个驱动电机的功率较大,不仅占用的空间较大,而且有时候在一些相对需求能量较低的状态下造成“大马拉小车”的现象出现,不利于微微鼠本体微型化发展和微微鼠***能源的节省。
微微鼠求解迷宫是国际新兴的一门技术,由于微微鼠技术的难度较高以及迷宫设计的复杂性,导致国内还没有研发此机器人的单位。因此,需要设计一种满足初级者学习微微鼠求解迷宫的高速探索控制器。
发明内容
本发明的目的是解决现有技术的不足,提供一种单核低速四轮微微鼠探索控制器,以解决微微鼠在探索过程中打滑、传感器相互干扰、处理时间慢等问题。
本发明采用的技术方案是:单核低速四轮微微鼠探索控制器,包括传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块;微微鼠的四轮其中一对位于微微鼠的中部,另一对位于微微鼠的尾部;所述陀螺仪装置包括陀螺仪G1和加速度计A1,所述陀螺仪G1为三轴陀螺仪用于测量三个转动方向运动,所述加速度计A1为三轴加速度计用于测量三个平移运动的加速度;所述传感器装置和陀螺仪装置分别信号连接控制单元模块,所述控制单元模块分别信号连接电机X、Y、Z、R和M;所述传感器装置包括位于两侧的红外传感器S1和S6,位于前端的红外传感器S2和S5,所述传感器S1、S6共同作用判断前方挡墙,传感器S2判断其左边挡墙的存在,传感器S5判断其右边挡墙的存在,同时S2和S5合作为直线运动提供导航依据;所述控制单元模块包括上位机程序模块和运动控制程序模块,所述上位机程序模块包括STM32F407处理器,所述运动控制程序模块包括两片两轴驱动控制芯片L6207D,所述L6207D芯片处理四轴行走伺服控制和单轴真空吸附伺服控制,所述STM32F407处理器电性连接L6207D芯片,所述四轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。
单核低速四轮微微鼠探索控制方法,其包括如下步骤:1)启动信号确认,在起点坐标(0,0)等待控制器发出的探索命令,接到探索任务后,会沿着起点开始向终点(F,F)、(F,10)、(10,F)、(10,10)探索;2)障碍判断,前方的传感器S1、S6和会对前方的环境进行判断,确定有没有挡墙进入运动范围,如存在挡墙将向STM32F407发出中断请求,STM32F407会对中断做第一时间响应,然后禁止控制四轮的两片L6207D使能端ENA、ENB工作,封锁微微鼠的电机X、电机Y、电机Z和电机R的PWM驱动信号,使其静止在原地,然后二次判断迷宫确定前方信息,防止探索时信息误判;3)路径探索,在微微鼠沿着X轴和Y轴向前运动,在任何一个方格的中心如果确定没有挡墙进入前方的运动范围,则微微鼠将存储其坐标(X,Y),并把向前运动一格的位置参数传输给STM32F407,STM32F407根据探索控制器速度和加速度要求生成速度运动梯形图,这个速度-时间图形包含的面积就是微微鼠电机X和电机Y要运行的一格距离;STM32F407根据这个梯形图结合光电编码器、电流传感器C1、C2的反馈生成驱动两轴直流电机的PWM波,然后STM32F407使能L6207D,由L6207D驱动两个独立电机X和电机Y向前运动,电机X和电机Y的光电编码器会时刻记录已经移动的距离并输送给控制器,陀螺仪G1和加速计A1记录微微鼠探索时的瞬时运动加速度、速度和位置,当微微鼠快速探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差大小借助陀螺仪G1和加速度计A1开始进行实时补偿,微调电机的PWM波输入,通过此方式可以精确调整微微鼠在直道的姿态,使其重新回到设定中心位置;4)路径判断,当微微鼠到达(F,F)、(F,10)、(10,F)或(10,10)中的终点准备返程探索时,控制器会调出其已经存储的迷宫,然后根据快速迷宫算法计算出可能存在的最佳路径,返程开始进入其中认为最优的一条;5)返航,在微微鼠进入迷宫正常返航运行时,并其导航的传感器S1、S2、S5、S6将工作,并把反射回来的光电信号送给STM32F407,经STM32F407判断后使能L6207D,然后导航的电机X、电机Y、电机Z和电机R;如果进入已经搜索的区域将进行快速前进,如果是未知返回区域则采用正常速度搜索,控制器会实时检测电机X、电机Y、电机Z、电机R光电编码器的数值,并根据其速度大小自动调节电机M加大或者减少真空吸盘对地面的吸附力,并时刻更新其坐标(X,Y),并判断其坐标是不是(0,0),如果是的话置返航探索标志为0,微微鼠进入冲刺阶段,并置冲刺标志为1。
作为本发明的进一步改进,还包括有电压传感器V1,所述电压传感器V1电连接电池装置,信号连接控制单元模块的STM32F407处理器。
作为本发明的进一步改进,还包括光补偿传感器L1,所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。
作为本发明的进一步改进,还包括有电流传感器C1、C2、C3和C4,所述电流传感器C1、C2、C3和C4信号连接STM32F407处理器。
作为本发明的更进一步改进,所述STM32F407通过I/O口连接L6207D芯片,从而使能控制两轴驱动控制芯片L6207D。
作为本发明的进一步改进,所述电机X、Y、Z和R为高速永磁直流电机,所述电机M为微型直流电机。
作为本发明的进一步改进,所述红外传感器S1和S2信号发射方向间的夹角为75°~90°角,所述红外传感器S5和S6信号发射方向间的夹角为75°~90°角。
本发明采用的有益效果是:1:在探索过程中,充分考虑了电池在这个***中的作用,基于STM32F407+L6207D控制器时刻都在对微微鼠的运行状态进行监测和运算,由于L6207D内部集成了电流采集电路,时刻对电机的电流可以进行采集,从根本上避免了大电流的产生,所以解决了大电流对锂离子电池的冲击,避免了由于大电流放电而引起的锂离子电池过度老化现象的发生。
2:为了充分提高微微鼠***的稳定性和行驶能力,并兼顾两轮中置转向的优点,本发明采用四轮驱动结构:中置驱动的功率较大,后置驱动的两个个电机功率较小,只有在动力需求较高时才启动,起到助力作用。由于采用四轮驱动技术,微微鼠前后轮都有动力,可按迷宫地面和周围环境状态不同而将需求扭矩按不同比例分布在前后所有的轮子上,以提高微微鼠的行驶能力。
3:根据需要实现分时四驱。在正常行驶环境下,微微鼠一般会采用释放后轮,采用中置轮驱动的方式;而一旦遇到路面灰尘较多或加速状况时,STM32407会自动检测并立即将微微鼠需求扭矩分配给后置两个助力驱动轮,同时控制器改变电机M的伺服控制,微微鼠***自然切换到四轮驱动状态,增强了微微鼠的附着力和操控性。
4:由于采用中驱+后驱的复合四轮驱动方式,当需要加速行驶时,把动力分配到四个电机,一旦一个动力轮由于地面、机械结构等造成暂时离开地面,STM32407可以重新分配扭矩,把更多的扭矩分配在未失速的驱动轮上,使***迅速脱离不稳定状态,重新回到四轴动力平衡状态,使得微微鼠具有更好的直线行走功能。
5:微微鼠转向时,为了保证旋转的稳定性,采用中置的两驱动轮实现转弯,并释放后置的两助力驱动轮,并利用加速度计实时测量微微鼠的瞬时加速度,为微微鼠精确转弯提供反馈。
6:四轮微微鼠***在正常行驶时如果设计不当造成重心前偏,将导致后侧驱动轮上承受的正压力减小,STM32F407会自动调整后侧的动力分配,使***处于一种新的平衡状态,防止微微鼠打滑。
7:四轮微微鼠***在正常行驶时如果设计不当造成重心侧偏,将导致一侧驱动轮上承受的正压力减小,STM32F407会自动调整这一侧的动力分配,使***处于一种新的平衡状态,防止微微鼠打滑。
8:由STM32F407处理微微鼠的五只直流电机的独立伺服控制,使得控制比较简单,大大提高了运算速度,解决了单片机软件运行较慢的瓶颈,缩短了开发周期短,并且程序可移植能力强。
9:本发明基本实现全贴片元器件材料,不仅节省了控制板占用空间,而且有利于体积和重量的减轻,有利于提高微微鼠的稳定性和动态性能。
10:采用四组传感器探索迷宫技术替代原有的六组传感器探索迷宫技术不仅减少了***中各传感器组的干扰,并提高了迷宫挡墙采集频率,有利于提高微微鼠伺服控制器的运算速度。
11:由于本控制器采用STM32F407处理迷宫读取、探索算法,有效地防止了程序的“跑飞”,抗干扰能力大大增强。
12:在微微鼠探索过程中,控制器会对四轴电机的转矩进行在线辨识并利用电机力矩与电流的关系进行补偿,减少了电机转矩抖动对微微鼠探索行走时动态性能的影响。
13:在控制中,STM32F407可以根据实际周围迷宫情况调整控制器内部的PID参数,轻松实现分段P、PD、PID控制和非线性PID控制,使***具有一定的自适应。
14:L6207D的使能端可以很好的解决微微鼠在运行过程中遇到撞墙情况发生的电机堵转,利用中断命令可在输出超出设定值时,L6207D的电流采集电路立即发出禁止使能请求,STM32F407静止使能端,释放直流电机X、电机Y、电机Z和电机R,从而有效地解决了堵转问题。
15:通过调节电机M可以有效调节真空吸盘对地面的吸附力,有效增加微微鼠与地面的摩擦系数,有效减少微微鼠在高速行走时打滑现象的发生。
附图说明
图1为原二轮六眼微电脑鼠二维图。
图2为微电脑鼠16*16迷宫示意图。
图3为微微鼠32*32迷宫示意图
图4为微微鼠探索控制器程序框图。
图5为本发明示意图。
图6为本发明的原理框图。
图7为本发明的L6207D的封装示意图。
图8为微微鼠前进示意图。
图9为微微鼠反向前进示意图。
图10为微微鼠加速时速度-时间示意图。
图11为微微鼠减速时速度-时间示意图。
图12为微微鼠右转示意图。
图13为微微鼠左转示意图。
具体实施方式
下面结合图1至图13,对本发明做进一步的说明。
STM32F4系列除引脚和软件兼容高性能的F2系列外,F4的主频(168MHz)高于F2系列(120MHz),并支持单周期DSP指令和浮点单元、更大的SRAM容量(192KB,F2为128KB)、512KB-1MB的嵌入式闪存以及影像、网络接口和数据加密等更先进的外设。STM32F4系列基于最新的ARMCortexM4内核,在现有出色的STM32微控制器产品组合中新增了信号处理功能,并提高了运行速度;STM32F405x集成了定时器、3个ADC、2个DAC、串行接口、外存接口、实时时钟、CRC计算单元和模拟真随机数发生器在内的整套先进外设。STM32F407在STM32F405产品基础上增加了多个先进外设。这些性能使得F4系列可以较容易满足控制和信号处理功能混合的数字信号控制需求。高效的信号处理功能与Cortex-M4处理器系列的低能耗、低成本和易于使用的优点的组合,使得其可以为多轴电动机控制提供灵活解决方案。这些特点使得STM32F407特别适合微微鼠四轴伺服***的信号处理。
L6207D芯片采用双DMOS全桥设计,并在同一芯片上实现了隔离DMOS功率晶体管、CMOS和双极性电路技术,工作电压为8~52V,输出峰值电流为5.6A,工作频率高达100KHz,具有非耗散过流保护、热关断、欠压锁定等保护功能。在电机控制应用中,可以同时驱动两个直流电机。
其中一片L6207D的23脚和14脚EnA,EnB是使能控制端,分别控制永磁直流电机X和永磁直流电机Y的停转,另外一片L6207D的23脚和14脚EnA,EnB使能控制端,分别控制永磁直流电机Z和永磁直流电机R的停转。L6207D的1,2,11,12脚接STM32F407的输入脚,由STM32F407控制OUT1A,OUT2A和OUT1B,OUT2B的电平,通过不同的电平组合控制电机X和电机Y或者是电机Z和电机R的正反转。同时电流传感器把检测电流输入到SENSEA和SENSEB来对电机进行限流控制。
在实验中发现,由于微微鼠体积的大幅度降低,图1中传感器S2和S3以及S4和S5经常产生相互干扰;同时由于每组传感器采集迷宫挡墙参数都需要一定的时间,加重了采样周期,导致采样频率降低,延长了处理器STM32F407的处理时间。本发明为克服六组传感器探测迷宫的缺陷,自主研发了基于四组传感器探测迷宫的四轮微微鼠探索控制器,所发明的微微鼠二维结构如图5所示:传感器S1、S6共同作用判断前方挡墙,传感器S2判断其左边挡墙的存在,传感器S5判断其右边挡墙的存在,同时S2和S5合作为微微鼠直线运动提供导航依据。为了使用智能算法计算迷宫挡墙信息,在微微鼠校正阶段,微微鼠放在迷宫不同设定位置,红外传感器S1、S2、S5、S6的红外光经侧边挡墙反馈后会被对应的红外接收器TSL262接收,然后TSL262的接收值经控制器计算后作为当前位置的设定阀值,然后微微鼠在行走过程中通过与这些阀值比较得到其迷宫信息。在相同位置,图1中传感器S2的计算阀值明显比S3精确,传感器S5的计算阀值明显比S4精确,而且S2和S5还可以精确测量到迷宫从有挡墙到无挡墙以及无挡墙到有挡墙的变化,这个位置的传感器信号变化可以被控制器捕捉到,然后在此位置可以对微微鼠进行精确补偿,这对于复杂迷宫计算至关重要,如果没有此智能补偿的话,微微鼠在复杂迷宫中的累计误差足以使求解迷宫失败。因此本控制器采用特定的结构来求解迷宫,以确保迷宫的准确性。
本申请引入了四轮驱动技术,加大了微微鼠与地面的接触面积,并利用真空吸盘技术提高了其与地面的摩擦力系数,增加了其稳定性。然后,在基于STM32F407的控制器中引入多轴驱动集成专用芯片L6207D,形成基于STM32F407+多轴专用驱动芯片L6207D的全新控制器,此控制器充分考虑电池在这个***的作用,把控制***中工作量最大的五轴伺服***交给STM32F407处理,充分发挥STM32F407数据处理速度较快的特点,同时与多轴驱动集成专用芯片L6207D通过使能端实时进行通讯,进行数据交换和调用。
微微鼠放在迷宫起始点,微微鼠放在迷宫起始点,在电源打开状态下,微微鼠先进入自锁状态,控制器开启真空抽吸电机M,调整微微鼠与地面的吸附力,增加其与地面的摩擦力。微微鼠依靠前方、左右侧面蔽障红外传感器S1、S2、S5、S6根据实际导航环境传输参数给STM32F407,STM32F407处理后与L6207D通讯,然后由使能端控制两片L6207D的输出状态,进而处理探索期间四个独立电机的伺服控制,并把电机运行状态经光电编码器、电流传感器C1~C4、陀螺仪G1和加速计A1反馈给STM32F407,由STM32F407继续处理后续的运行状态。
为了进一步提高四轮微微鼠在探索迷宫时的稳定性,本发明在微微鼠探索伺服硬件***中加入了三轴陀螺仪G1和三轴加速度计A1,控制器舍弃了单一陀螺仪工作模式。在微微鼠探索迷宫期间全程开启陀螺仪和加速度计,三轴陀螺仪G1用来测量微微鼠三个转动方向运动,三轴加速度计用来测量微微鼠三个平移运动的加速度。控制器根据测得的加速度信号计算出微微鼠的速度和位置数据,利用陀螺仪G1短时测量准确的优势和加速度计A1长时稳定的特点,两者结合,得到即能短时稳定又能长时稳定的倾斜角度,用陀螺仪测量短时内角度变化,把加速度计传感器当作倾角传感器测量倾角,并在一个长时间范围内,迫使陀螺仪得到的倾角慢慢匹配加速度传感器得到的倾角。陀螺仪和加速度计时刻记录微微鼠探索迷宫时的瞬时参数并输送给控制器,当微微鼠在探索迷宫时姿态发生变化超过设定阀值时,在一个新的采样周期控制器就立即对其位置补偿,避免了微微鼠远远偏离中心位置现象的发生,提高了其快速探索时的稳定性。
为了进一步提高微微鼠探索控制器的稳定性,防止微微鼠在高速探索时打滑导致微微鼠迷宫信息错误,本发明在微微鼠探索控制器中加入了微型直流电机M,在微微鼠运动过程中,电机M通过真空抽吸装置不停抽吸微型真空吸盘内的空气,使微型真空吸盘的内外压力不一样,产生一定的负压,使其对迷宫地面产生一定的吸附力,有效防止了微微鼠在高速迷宫探索时的地面打滑。
本发明是如下工作的:
1)在微微鼠未接到探索命令之前,它一般会在起点坐标(0,0)等待控制器发出的探索命令,一旦接到任务后,控制器首先开启真空抽吸电机M,通过抽吸装置先对微型真空吸盘抽吸,使真空吸盘对地面具有一定的吸附力,控制器并实时检测,如果地面不干净,***会自动调节电机M加大真空吸盘对地面的吸附力。然后微微鼠会沿着起点开始向终点(F,F)、(F,10)、(10,F)、(10,10)探索。
2)微微鼠放在起点坐标(0,0),接到任务后其前方的传感器S1、S6和会对前方的环境进行判断,确定有没有挡墙进入运动范围,如存在挡墙将向STM32F407发出中断请求,STM32F407会对中断做第一时间响应,然后禁止控制四轮的两片L6207D使能端ENA、ENB工作,封锁微微鼠的电机X、电机Y、电机Z和电机R的PWM驱动信号,使其静止在原地,然后二次判断迷宫确定前方信息,防止探索时信息误判;
3)在微微鼠启动瞬间,按照图10所示速度-时间梯形图进行加速运动,为了满足快速加速要求,控制器首先通过真空抽吸电机M的伺服控制抽吸空气,增加真空吸盘对地面的摩擦,满足微微鼠加速要求。然后STM32F407会置两片L6207D的的23脚和14脚EnA,EnB为高电平,开启四轮驱动模式,后驱助力轮开始工作。STM32F407立即将微微鼠加速度转化为需求扭矩,然后按照内部算法分配部分扭矩给后置助力驱动电机Z、电机R和中驱动电机X、电机Y,当微微鼠的速度到达设定直线速度时,STM32F407会置后置助力轮控制芯片L6207D的的23脚和14脚EnA,EnB为低电平,***释放电机Z和电机R,***依靠电机X和电机Y进入中轮驱动状态,控制器根据现有速度和地面情况决定是否二次调节电机M的伺服控制。
4)在微微鼠沿着Y轴向前运动过程中,在任何一个方格的中心如果确定没有迷宫挡墙进入前方的运动范围,则微微鼠将存储其坐标(X,Y),控制器把向前运动一格的位置参数送给STM32F407,STM32F407根据探索控制器速度和加速度要求生成速度运动梯形图,这个速度-时间图形包含的面积就是微微鼠电机X和电机Y要运行的一格距离。STM32F407根据这个梯形图结合光电编码器、电流传感器C1、C2的反馈生成驱动两轴直流电机的PWM波,然后STM32F407使能L6207D,由L6207D驱动两个独立电机X和电机Y向前运动,电机X和电机Y的光电编码器会时刻记录已经移动的距离并输送给控制器;在微微鼠沿着当前迷宫格向前探索过程中,此时控制器会实时检测电机X和电机Y光电编码器的数值,并根据其速度大小自动调节电机M加大真空吸盘对地面的吸附力,然后传感器S2和S5会对左右的挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据前进方向左右挡墙的迷宫信息进入单墙导航模式或者是双墙导航模式,然后陀螺仪G1和加速计A1记录微微鼠探索时的瞬时运动加速度、速度和位置,当微微鼠快速探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差大小借助陀螺仪G1和加速度计A1开始进行实时补偿,微调电机的PWM波输入,通过此方式可以精确调整微微鼠在直道的姿态,使其重新回到设定中心位置;在未到达目标前,如果微微鼠高速探索出现失速或者迷宫地面灰尘较多的情况,STM32F407会调节M加大微微鼠与地面的摩擦并使能后驱芯片L6207D的使能端,重新开启电机Z和电机R,***进入四轮驱动状态,STM32F407根据时间和速度要求,把剩余的距离D1转化为四轴直流电机的PWM波,使电机X、电机Y、电机Z和电机R以相同的速度前进,微微鼠在四驱状态下,微微鼠依旧按照原有的单墙导航模式或者是双墙导航模式前进,陀螺仪G1和加速度计A1实时记录微微鼠的瞬时运动加速度、速度和位置,当微微鼠快速四轮探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差借助陀螺仪开始进行实时补偿,微调四轴电机的PWM波输入,通过此方式可以精确调整微微鼠的姿态,使其重新回到设定中心位置;当微微鼠在陀螺仪的控制下运动一格距离到达新地址时,微处理器将更新其坐标为(X,Y+1),在Y+1<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续更新其坐标,如果是的话通知控制器已经搜索到目标,然后置返航探索标志为1,微微鼠准备返程探索;
5)在微微鼠沿着Y轴向前运动过程中如果有迷宫挡墙进入前方的运动范围,并且此时左右的传感器S2、S5判断左右都有挡墙时,微微鼠将存储此时坐标(X,Y),根据传感器S1和S6的反馈计算出向前运动停车的位置参数,按后按照速度-时间图形运动,由STM32F407先调节M加大微微鼠与地面的摩擦并使能后驱助力驱动芯片L6207D,开启电机Z和电机R使***进入四轮驱动减速状态,STM32F407根据停车位置参数、探索速度和加速度要求,再结合光电编码器、电流传感器C1~C4的反馈生成驱动四轴直流电机X、电机Y、电机Z和电机R的PWM波,四个独立电机带动微微鼠向前减速运动,在向前停车过程中,传感器S2、S5实时对左右的挡墙进行判断,并反馈当前迷宫挡墙信息,微微鼠进入双墙导航模式,然后在陀螺仪G1和加速度计A1的帮助下,微微鼠实现在设置停车点停车。然后STM32F407使能中驱L6207D,禁止后驱L6207D工作,并调整OUT1A、OUT2A、OUT1B、OUT2B的电平,使得电机X和电机Y运动方向相反,微微鼠原地由陀螺仪G1和加速度计A1控制调转180度,然后准备沿着Y轴反向运动。
在微微鼠沿着Y轴反方向向前运动过程中,在任何一个方格的中心如果确定没有迷宫挡墙进入前方的运动范围,则微微鼠将存储其坐标(X,Y),微微鼠反向运动原理与正向相似,当当微微鼠在陀螺仪的控制下运动一格距离到达新地址时,微处理器将更新其坐标为(X,Y-1),在Y-1<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续更新其坐标,如果是的话通知控制器已经搜索到目标,然后置返航探索标志为1,微微鼠准备返程探索;
6)在微微鼠沿着Y轴向前运动过程中如果有迷宫挡墙进入前方的运动范围,并且此时左右传感器S2、S5判断左方有挡墙右方无挡墙时,微微鼠将存储此时坐标(X,Y),根据传感器S1和S6的反馈计算出向前运动停止在迷宫方格中心A的位置参数,按后按照速度-时间图形运动,由STM32F407根据探索速度和加速度要求生成速度运动梯形图,这个图形包含的面积就是微微鼠四个马达要停车的距离。STM32F407然后根据这个梯形图结合光电编码器、电流传感器C1~C4的反馈生成驱动四轴直流电机的PWM波,STM32F407使能两片L6207D,由L6207D驱动四个独立电机X、电机Y、电机Z和电机R向前减速运动并实现中心点停车。在向前停车过程中,伺服控制器会调节电机M加大微微鼠与地面的摩擦保证微微鼠准确停车,同时传感器S2实时对左挡墙进行判断,并反馈当前迷宫挡墙信息,微微鼠进入单墙导航模式,然后在陀螺仪G1和加速计A1的帮助下,微微鼠实现在设置停车点停车。然后STM32F407使能中驱的驱动芯片L6207D,并禁止后驱的L6207D,并调整L6207D的OUT1A、OUT2A、OUT1B、OUT2B的电平,使得电机X和电机Y运动方向相反,微微鼠原地由陀螺仪控制向右调转90度,然后沿着X轴正向运动。
在微微鼠X轴运动原理与沿着Y轴运动原理相似,当微微鼠在陀螺仪的控制下运动一格距离到达新地址时,微处理器将更新其坐标为(X+1,Y),在X+1<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续更新其坐标,如果是的话通知控制器已经搜索到目标,然后置返航探索标志为1,微微鼠准备返程探索;
7)在微微鼠沿着Y轴向前运动过程中如果有迷宫挡墙进入前方的运动范围,并且此时左右传感器S2、S5判断左方无挡墙而右方有挡墙时,微微鼠将存储此时坐标(X,Y),微微鼠左转运动原理与右转运动原理相似;当微微鼠在陀螺仪的控制下运动一格距离到达新地址时,微处理器将更新其坐标为(X-1,Y),在X-1<1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续更新其坐标,如果是的话通知控制器已经搜索到目标,然后置返航探索标志为1,微微鼠准备返程探索;
8)当微微鼠到达(F,F)、(F,10)、(10,F)、(10,10)准备返程探索时,控制器会调出其已经存储的迷宫,然后根据快速迷宫算法计算出可能存在的最佳路径,然后返程开始进入其中认为最优的一条。
9)在微微鼠进入迷宫正常返航运行时,并其导航的传感器S1、S2、S5、S6将工作,并把反射回来的光电信号送给STM32F407,经STM32F407判断后使能L6207D,然后导航的电机X、电机Y、电机Z和电机R:如果进入已经搜索的区域将进行快速前进,如果是未知返回区域则采用正常速度搜索,控制器会实时检测电机X、电机Y、电机Z、电机R光电编码器的数值,并根据其速度大小自动调节电机M加大或者减少真空吸盘对地面的吸附力,并时刻更新其坐标(X,Y),并判断其坐标是不是(0,0),如果是的话置返航探索标志为0,微微鼠进入冲刺阶段,并置冲刺标志为1。
10)为了能够实现微微鼠在探索时准确的坐标计算,微微鼠左右的传感器S2和S5会时刻对周围的迷宫挡墙和柱子进行探测,如果S2或者S5发现传感器信号发生了较大数值的跃变,则说明微微鼠进入了从有迷宫挡墙到无迷宫挡墙(或者是从无迷宫挡墙到有迷宫挡墙)状态的变化,STM32F407会根据微微鼠当前运行状态精确补偿,彻底消除微微鼠在复杂迷宫中探索时已经累计的误差。
11)如果微微鼠在探索过程中遇到故障撞墙时,电机X、电机Y、电机Z和电机R的电流将增大,当超过设定值时,L6207D的电流采集电路将工作,STM32F407将会向L6207D发出禁止使能中断,此时控制器会立即控制L6207D停止工作,进而释放电机X、电机Y、电机Z和电机R,从而有效地解决了堵转问题。
12)微微鼠在探索过程会时刻检测电池电压,当***出现低压时,传感器V1将开启并发出报警提示,有效地保护了锂离子电池。
13)在微微鼠整个运动过程中,传感器L1会时刻对外界干扰光源进行采集,然后传输给STM32F407,STM32F407会根据L1的自动补偿外界干扰,减少了外界干扰光源对微微鼠快速探索伺服***的干扰。
14)在微微鼠探索过程中,控制器会对高速直流电机X、电机Y、电机Z和电机R的转矩进行在线辨识,如果转矩出现脉动,控制器会利用直流电机力矩与电流的关系进行线性补偿,快速调整电流环的PID参数,使得***快速稳定下来,减少了电机转矩抖动对微微鼠快速探索时导航的影响。
15)当微微鼠完成整个探索过程回到起始点(0,0),STM32F407将控制两片L6207D使得微微微鼠中心点停车,然后重新调整两片L6207D的OUT1A、OUT2A、OUT1B和OUT2B的电平,使得电机X和电机Y以相反的方向运动,并在陀螺仪G1和加速度计A1的控制下原地旋转180度,然后停车1秒。控制器开始调取迷宫信息并根据改进蚁群算法(Improvedantcolonyalgorithm)计算出微微鼠探索的最优冲刺路径,然后置冲刺标志为1,***进入快速冲刺阶段。然后控制器根据冲刺速度的大小自动调整电机M,改变真空吸盘对地面的吸附力,满足快速冲刺时的摩擦需要。
本领域技术人员应当知晓,本发明的保护方案不仅限于上述的实施例,还可以在上述实施例的基础上进行各种排列组合与变换,在不违背本发明精神的前提下,对本发明进行的各种变换均落在本发明的保护范围内。
Claims (9)
1.单核低速四轮微微鼠探索控制器,其特征是包括传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块;微微鼠的四轮其中一对位于微微鼠的中部,另一对位于微微鼠的尾部;
所述陀螺仪装置包括陀螺仪G1和加速度计A1,所述陀螺仪G1为三轴陀螺仪用于测量三个转动方向运动,所述加速度计A1为三轴加速度计用于测量三个平移运动的加速度;
所述传感器装置和陀螺仪装置分别信号连接控制单元模块,所述控制单元模块分别信号连接电机X、Y、Z、R和M;
所述传感器装置包括位于两侧的红外传感器S1和S6,位于前端的红外传感器S2和S5,所述传感器S1、S6共同作用判断前方挡墙,传感器S2判断其左边挡墙的存在,传感器S5判断其右边挡墙的存在,同时S2和S5合作为直线运动提供导航依据;
所述控制单元模块包括上位机程序模块和运动控制程序模块,所述上位机程序模块包括STM32F407处理器,所述运动控制程序模块包括两片两轴驱动控制芯片L6207D,所述L6207D芯片处理四轴行走伺服控制和单轴真空吸附伺服控制,所述STM32F407处理器电性连接L6207D芯片,所述四轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。
2.根据权利要求1所述的单核低速四轮微微鼠探索控制器,其特征是还包括有电压传感器V1,所述电压传感器V1电连接电池装置,信号连接控制单元模块的STM32F407处理器。
3.根据权利要求1所述的单核低速四轮微微鼠探索控制器,其特征是还包括光补偿传感器L1,所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。
4.根据权利要求1所述的单核低速四轮微微鼠探索控制器,其特征是还包括有电流传感器C1、C2、C3和C4,所述电流传感器C1、C2、C3和C4信号连接STM32F407处理器。
5.根据权利要求1至4中任意一项所述的单核低速四轮微微鼠探索控制器,其特征是所述STM32F407通过I/O口连接L6207D芯片,从而使能控制两轴驱动控制芯片L6207D。
6.根据权利要求5所述的单核低速四轮微微鼠探索控制器,其特征是所述电机X、Y、Z和R为高速永磁直流电机,所述电机M为微型直流电机。
7.根据权利要求1至4中任意一项所述的单核低速四轮微微鼠探索控制器,其特征是所述红外传感器S1和S2信号发射方向间的夹角为75°~90°角,所述红外传感器S5和S6信号发射方向间的夹角为75°~90°角。
8.一种如权利要求1所述的单核低速四轮微微鼠探索控制方法,其特征是包括如下步骤:
1)启动信号确认,在起点坐标(0,0)等待控制器发出的探索命令,接到探索任务后,会沿着起点开始向终点(F,F)、(F,10)、(10,F)、(10,10)探索;
2)障碍判断,前方的传感器S1、S6和会对前方的环境进行判断,确定有没有挡墙进入运动范围,如存在挡墙将向STM32F407发出中断请求,STM32F407会对中断做第一时间响应,然后禁止控制四轮的两片L6207D使能端ENA、ENB工作,封锁微微鼠的电机X、电机Y、电机Z和电机R的PWM驱动信号,使其静止在原地,然后二次判断迷宫确定前方信息,防止探索时信息误判;
3)路径探索,在微微鼠沿着X轴和Y轴向前运动,在任何一个方格的中心如果确定没有挡墙进入前方的运动范围,则微微鼠将存储其坐标(X,Y),并把向前运动一格的位置参数传输给STM32F407,STM32F407根据探索控制器速度和加速度要求生成速度运动梯形图,这个速度-时间图形包含的面积就是微微鼠电机X和电机Y要运行的一格距离;STM32F407根据这个梯形图结合光电编码器、电流传感器C1、C2的反馈生成驱动两轴直流电机的PWM波,然后STM32F407使能L6207D,由L6207D驱动两个独立电机X和电机Y向前运动,电机X和电机Y的光电编码器会时刻记录已经移动的距离并输送给控制器,陀螺仪G1和加速计A1记录微微鼠探索时的瞬时运动加速度、速度和位置,当微微鼠快速探索脱离了设定中心位置时,微处理器根据离开中心位置的偏差大小借助陀螺仪G1和加速度计A1开始进行实时补偿,微调电机的PWM波输入,通过此方式可以精确调整微微鼠在直道的姿态,使其重新回到设定中心位置;
4)路径判断,当微微鼠到达(F,F)、(F,10)、(10,F)或(10,10)中的终点准备返程探索时,控制器会调出其已经存储的迷宫,然后根据快速迷宫算法计算出可能存在的最佳路径,返程开始进入其中认为最优的一条;
5)返航,在微微鼠进入迷宫正常返航运行时,并其导航的传感器S1、S2、S5、S6将工作,并把反射回来的光电信号送给STM32F407,经STM32F407判断后使能L6207D,然后导航的电机X、电机Y、电机Z和电机R;如果进入已经搜索的区域将进行快速前进,如果是未知返回区域则采用正常速度搜索,控制器会实时检测电机X、电机Y、电机Z、电机R光电编码器的数值,并根据其速度大小自动调节电机M加大或者减少真空吸盘对地面的吸附力,并时刻更新其坐标(X,Y),并判断其坐标是不是(0,0),如果是的话置返航探索标志为0,微微鼠进入冲刺阶段,并置冲刺标志为1。
9.如权利要求8所述的单核低速四轮微微鼠探索控制方法,其特征是所述步骤三路径探索中,当传感器S2和S5会对左右的挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据前进方向左右挡墙的迷宫信息进入双墙导航模式、单墙导航模式或者惯性导航模式。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510520971.3A CN105137981A (zh) | 2015-08-24 | 2015-08-24 | 单核低速四轮微微鼠探索控制器及其控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510520971.3A CN105137981A (zh) | 2015-08-24 | 2015-08-24 | 单核低速四轮微微鼠探索控制器及其控制方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN105137981A true CN105137981A (zh) | 2015-12-09 |
Family
ID=54723356
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510520971.3A Pending CN105137981A (zh) | 2015-08-24 | 2015-08-24 | 单核低速四轮微微鼠探索控制器及其控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105137981A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105892470A (zh) * | 2016-07-01 | 2016-08-24 | 江苏若博机器人科技有限公司 | 一种单核三轴四轮变结构微微鼠全数字伺服***控制器 |
CN105929833A (zh) * | 2016-07-01 | 2016-09-07 | 江苏若博机器人科技有限公司 | 一种单核三轴四轮变结构微微鼠冲刺控制器 |
Citations (4)
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 | 陕西科技大学 | 一种用于迷宫冲刺的电脑鼠 |
CN103472840A (zh) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | 基于arm9四轮微电脑鼠探索控制器 |
CN204086988U (zh) * | 2014-09-05 | 2015-01-07 | 西安建筑科技大学 | 一种用于迷宫解算的迷宫机器鼠*** |
-
2015
- 2015-08-24 CN CN201510520971.3A patent/CN105137981A/zh active Pending
Patent Citations (4)
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 | 陕西科技大学 | 一种用于迷宫冲刺的电脑鼠 |
CN103472840A (zh) * | 2013-09-16 | 2013-12-25 | 苏州工业园区职业技术学院 | 基于arm9四轮微电脑鼠探索控制器 |
CN204086988U (zh) * | 2014-09-05 | 2015-01-07 | 西安建筑科技大学 | 一种用于迷宫解算的迷宫机器鼠*** |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105892470A (zh) * | 2016-07-01 | 2016-08-24 | 江苏若博机器人科技有限公司 | 一种单核三轴四轮变结构微微鼠全数字伺服***控制器 |
CN105929833A (zh) * | 2016-07-01 | 2016-09-07 | 江苏若博机器人科技有限公司 | 一种单核三轴四轮变结构微微鼠冲刺控制器 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105334853A (zh) | 双核高速四轮微微鼠冲刺控制器 | |
CN105137975A (zh) | 六轮双核全自动高速灭火机器人伺服控制器 | |
CN105116897A (zh) | 双核高速四轮微微鼠全数字导航伺服控制器 | |
CN105005249A (zh) | 一种全自动四轮双核高速灭火机器人伺服控制器 | |
CN105045289A (zh) | 一种单轮全自动高速灭火机器人伺服控制*** | |
CN105137983A (zh) | 双核高速两轮微微鼠冲刺控制器及其控制方法 | |
CN105045288A (zh) | 一种单轮全自动中速灭火机器人伺服控制*** | |
CN105137974A (zh) | 六轮双核全自动中速灭火机器人伺服控制器 | |
CN105137988A (zh) | 单核低速两轮微微鼠全数字导航伺服控制器及控制方法 | |
CN105204502A (zh) | 双核中速六轮微微鼠探索控制器及其控制方法 | |
CN105005308A (zh) | 基于stm32f407六轮灭火机器人伺服控制器 | |
CN105116898A (zh) | 单核低速两轮微微鼠冲刺控制器 | |
CN105137979A (zh) | 双核中速六轮微微鼠冲刺控制器及其控制方法 | |
CN105116891A (zh) | 双核中速两轮微微鼠冲刺控制器及其控制方法 | |
CN105137981A (zh) | 单核低速四轮微微鼠探索控制器及其控制方法 | |
CN105159323A (zh) | 六轮单核全自动低速灭火机器人伺服控制器 | |
CN105137978A (zh) | 双核高速四轮微微鼠探索控制器 | |
CN105137991A (zh) | 单核低速两轮微微鼠探索控制器 | |
CN105320147A (zh) | 一种单轮双核中低速灭火机器人伺服控制*** | |
CN105302133A (zh) | 单核低速六轮微微鼠全数字导航伺服***控制器 | |
CN105137982A (zh) | 单核低速四轮微微鼠冲刺控制器及其控制方法 | |
CN105116896A (zh) | 单核低速六轮微微鼠探索控制器 | |
CN105159324A (zh) | 六轮双核中速灭火机器人伺服控制器 | |
CN105137985A (zh) | 双核中速四轮微微鼠探索控制器 | |
CN103472831B (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 |
Application publication date: 20151209 |
|
RJ01 | Rejection of invention patent application after publication |