CN105137987A - 双核中速两轮微微鼠全数字导航伺服控制器及其控制方法 - Google Patents

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的控制器中引入精密运动控制专用芯片LM629，形成基于STM32F407+专用运动控制芯片的全新双核控制器，此控制器充分考虑电池在这个系统的作用，把导航控制器中工作量最大的两轴行走伺服系统以及单轴真空抽吸伺服系统由STM32F407和LM629配合处理，充分发挥STM32F407数据处理速度较快和LM629伺服调节器的特点，而人机界面、路径优化、轨迹参数、加速度、速度和位置数据存储和计算、I/O控制等功能交给STM32F407完成，这样就实现了STM32F407与LM629的分工，同时二者之间也可以进行通讯，实时进行数据交换和调用。

双核中速两轮微微鼠全数字导航伺服控制器，包括底盘，所述底盘上设有电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块，所述底盘两侧连接运动轮；所述电池装置电连接控制单元模块，所述传感器装置和陀螺仪装置分别信号连接控制单元模块，所述控制单元模块分别信号连接电机X、Y和M；所述电机X和电机Y分别传动联接运动轮，所述电机M传动联接真空抽吸装置，所述抽真空装置连接真空吸盘，所述真空吸盘位于底盘底部；所述陀螺仪装置G位于底盘的中心位置；所述传感器装置包括位于底盘两侧的红外传感器S1和S6，位于底盘前端的红外传感器S2和S5，所述传感器S1、S6共同作用判断前方挡墙，传感器S2判断其左边挡墙的存在，传感器S5判断其右边挡墙的存在，同时S2和S5合作为直线运动提供导航依据；所述控制单元模块包括上位机程序模块和运动控制程序模块，所述上位机程序模块包括STM32F407处理器，所述运动控制程序模块包括LM629处理器，所述LM629处理器包括两轴行走伺服控制单元和单轴真空吸附伺服控制单元，所述STM32F407处理器电性连接LM629处理器，所述两轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。

双核中速两轮微微鼠全数字导航伺服控制方法，其步骤如下：1）电压检测：检测电池电压、传感器电路和时钟电路；2）底盘真空抽吸：STM32F407控制器首先开启真空抽吸直流电机M，通过真空抽吸装置先对真空吸盘抽吸，使真空吸盘对地面具有一定的吸附力，STM32F407控制器实时检测；3）迷宫行走：判断周围的环境并送给STM32F407，STM32F407把这些环境参数转化为微微鼠左右轮要运行的位置、速度和加速度指令值，并把这些值传输给；然后由结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，二者之间的差值经内部的PID调节器调节后生成控制电机运行的实际速度-时间运动梯形图，这个梯形包含的面积就是微微鼠两个电机X和电机Y要运行的距离Y。STM32F407然后根据这个梯形图生成驱动两轴直流电机的PWM波，并通过驱动桥驱动两个独立直流电机X和电机Y向前运动；4）调整纠错：光补偿传感器L1会时刻对外界干扰光源进行采集，然后传输给STM32F407，STM32F407会根据L1的数值自动补偿外界干扰，减少了外界干扰光源对微微鼠快速探索和冲刺时的干扰；5）坐标检测：装在微微鼠上的高速永磁直流电机X和电机Y上的光电编码器的位置信号A脉冲、B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给LM629寄存器，记录电机的绝对位置，然后控制器会把这些值换算成微微鼠在迷宫中的具体位置；6）路径计算：STM32F407根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置数据等给LM629作为伺服系统的参考值，LM629结合光电编码器的反馈和电流传感器的反馈生成新的偏差，此偏差经LM629内部调节器调节后生成驱动电机运动的PWM波，在电机运动过程中陀螺仪G1实时记录微微鼠新的运动状态，并反馈给控制器，在新的采样周期到来后，重新调整微微鼠两轴三闭环伺服系统的加速度、速度和位置指令。

作为本发明的进一步改进，还包括有电压传感器V1，所述电压传感器V1电连接电池装置，信号连接控制单元模块的STM32F407处理器。

作为本发明的进一步改进，还包括光补偿传感器L1，所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。

作为本发明的进一步改进，还包括有电流传感器C1和C2，所述电流传感器C1和C2信号连接STM32F407处理器。

作为本发明的更进一步改进，所述LM629处理器信号连接电机X和电机Y，所述电机X和Y分别传动联接微微鼠的两轮。

作为本发明的进一步改进，所述电机X和Y为高速永磁直流电机，所述电机M为微型直流电机。

作为本发明的进一步改进，所述红外传感器S1和S2信号发射方向间的夹角为75°～90°角，所述红外传感器S5和S6信号发射方向间的夹角为75°～90°角。

作为本发明的更进一步改进，所述迷宫行走的运动过程中，传感器S2和S5会对左右的挡墙进行判断，并记录储存当前迷宫挡墙信息，微微鼠根据前进方向左右挡墙的迷宫信息进入双墙导航模式、单墙导航模式或者惯性导航模式。

本发明采用的有益效果是：1：在运动过程中，充分考虑了电池在这个系统中的作用，基于STM32F407+LM629控制器时刻都在对微微鼠冲刺过程中的运行状态进行监测和运算，并由传感器C1、C2实时检测电机的电流，并且时刻显示锂离子电池的SOC，有利于了解电池的能量状态，当电池能量状态较低时，可以在冲刺前提前换掉电池，从而减少了电池对高速冲刺的误干扰。

2：由STM32F407+LM629处理微微鼠高速冲刺时两只直流电机的伺服控制，充分发挥STM32F407处理数据较快以及LM629作为伺服控制器的特点，使得两轴伺服系统控制比较简单，并大大提高了运算速度，解决了单片机软件运行较慢的瓶颈，缩短了开发周期短，而且程序可移植能力强。

3：本发明基本实现全贴片元器件材料，实现了单板控制，不仅降低了微微鼠的体积，而且也降低了其重心，有利于其快速冲刺。

4：陀螺仪G的加入使得微微鼠的速度大小和方向可以独立控制，有利于提高微微鼠快速冲刺时的稳定性和动态性能。

5：陀螺仪G的加入可以实时检测微微鼠冲刺时的运动角度并实时补偿，使得微微鼠更容易实现曲线轨迹的转动。

6：通过调节电机M可以有效调节真空吸盘对地面的吸附力，有效减少了微微鼠在行走时打滑现象的发生。

7：由于本控制器采用LM629处理大量的数据与算法，并充分考虑了周围的干扰源，并把STM32F407从繁重的工作量中解脱出来，有效地防止了微微鼠快速冲刺控制器两轴行走伺服以及单轴真空抽吸伺服系统程序的“跑飞”，抗干扰能力大大增强。

8：在微微鼠快速导航过程中，STM32F407会对高速永磁直流电机X和电机Y的转矩进行在线辨识，并利用直流电机力矩与电流的关系进行补偿，减少了电机转矩抖动对微微鼠在复杂迷宫下快速导航时的影响。

9：由于具有存储功能，这使得微微鼠可以轻易调取已经探索好的迷宫信息，可以优化二次冲刺的路径，降低冲刺时间。

附图说明

图1为原二轮六眼微电脑鼠二维图。

图2为微电脑鼠16*16迷宫示意图。

图3为微微鼠32*32迷宫示意图

图4为微微鼠导航伺服控制器程序框图。

图5为本发明示意图。

图6为本发明的原理框图。

图7为本发明的STM32F407处理器与LM629的连接原理图。

图8为微微鼠导航前进示意图。

图9为微微鼠导航反向前进示意图。

具体实施方式

下面结合图1至图9，对本发明做进一步的说明。

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系类更适合微微鼠两轴伺服系统的信号处理。

LM629是Nationalsemiconductor生产的一款用于精密运动控制的专用芯片，有24脚和28脚二种表面安装式封装，在一个芯片内集成了数字式运动控制的全部功能，使得设计一个快速、准确的运动控制系统的任务变得轻松、容易，这个特点使得LM629特别适合伺服运动控制。

对于本发明在电源打开状态下，微微鼠先进入自锁状态，控制器开启真空抽吸电机M，微微鼠靠前方红外传感器S1、S6和左右侧面红外传感器S2、S5探测未知迷宫信息，迷宫实际导航环境经红外接收器TSL262转换后输送给控制器。STM32F407先把这些环境参数转化为微微鼠左右轮要运行的位置、速度和加速度指令值，然后光电编码器的反馈和电机的电流被内部的位置解码器求解后作为位置、速度和加速度的反馈值，偏差经内部的PID调节后生成驱动两轴直流电机的PWM波，最终经驱动桥驱动两个独立电机行驶，在整个运动过程中有陀螺仪G1实时进行位置检测和补偿，并通过电流传感器C1、C2进行电流检测，实现两轴高速直流电机的同步伺服控制。

双核中速两轮微微鼠全数字导航伺服控制器，包括底盘，所述底盘上设有电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块，所述底盘两侧连接运动轮；所述电池装置电连接控制单元模块，所述传感器装置和陀螺仪装置分别信号连接控制单元模块，所述控制单元模块分别信号连接电机X、Y和M；所述电机X和电机Y分别传动联接运动轮，所述电机M传动联接真空抽吸装置，所述抽真空装置连接真空吸盘，所述真空吸盘位于底盘底部；所述陀螺仪装置G位于底盘的中心位置；所述传感器装置包括位于底盘两侧的红外传感器S1和S6，位于底盘前端的红外传感器S2和S5，所述传感器S1、S6共同作用判断前方挡墙，传感器S2判断其左边挡墙的存在，传感器S5判断其右边挡墙的存在，同时S2和S5合作为直线运动提供导航依据；所述控制单元模块包括上位机程序模块和运动控制程序模块，所述上位机程序模块包括STM32F407处理器，所述运动控制程序模块包括LM629处理器，所述LM629处理器包括两轴行走伺服控制单元和单轴真空吸附伺服控制单元，所述STM32F407处理器电性连接LM629处理器，所述两轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。

为了保护大电流对电池装置的冲击，还包括有电压传感器V1，所述电压传感器V1电连接电池装置，信号连接控制单元模块的STM32F407处理器。

为了减少和杜绝外界光源对传感器的干扰，从而影响整个系统的使用，还包括光补偿传感器L1，所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。

为了进一步使得本发明的伺服控制器精确控制微微鼠的前进和转向，还包括有电流传感器C1和C2，所述电流传感器C1和C2信号连接STM32F407处理器。

为进一步拓展两核处理器的处理能力，做到分工明确，工作不受干扰，所述LM629处理器信号连接电机X和电机Y，所述电机X和Y分别传动联接微微鼠的两轮。

为了进一步提高微微鼠的性能，所述电机X和Y为高速永磁直流电机，所述电机M为微型直流电机。

为进一步消除红外传感器之间的干扰，提高传感器装置的感应精度，所述红外传感器S1和S2信号发射方向间的夹角为75°～90°角，所述红外传感器S5和S6信号发射方向间的夹角为75°～90°角。

微微鼠在迷宫导航时导航伺服控制器以LM629微处理器为核心，其中伺服系统位置、速度、电流的给定值由微处理器STM32F407根据传感器S1、S2、S5、S6的导航值来生成，光电编码器的输出经LM629内部解码器解码后作为闭环控制的反馈值，经其内部PID调节器调节后生成控制电机的PWM波。同时STM32F407实现导航时部分的信号处理算法（直流电机转矩补偿等）和LM629的控制逻辑，并响应中断，实现数据通信和存储实时信号。

本发明是如下工作的：

1）为了能够驱动两轮微微鼠进行运动，本控制系统引入了两片LM629，但是通过I/O口与STM32F407进入实时通讯，由STM32F407控制其开通和关断。在开通阶段，STM32F407通过传感器S1、S2、S5、S6的反馈计算出LM629的目标位置值、速度和加速度值。在关断阶段，STM32F407禁止LM629工作，从而封锁驱动两只独立电机的PWM波输出。

2）打开电源瞬间，电压传感器V1会对电池电压进行检测，如果伺服系统处于低压状态，传感器将与STM32F407进行通讯，控制器禁止工作，驱动电机运转的PWM波被封锁，高速永磁直流电机X和电机Y不能启动，控制器并发出并报警信号。。

3）如果电压正常，系统将检测传感器电路和时钟电路，如果传感器电路和时钟电路出现故障，系统将自动复位，重新检测，如有问题，将报警。

4）微微鼠数字导航伺服系统初始化正常后，系统将对进行检测。对于基于的系统来说，“忙”状态的检测是整个伺服系统设计的首要部分，在处理器向写命令或者读写数字后，“忙”状态位会被立刻置位，此时，会忽略一切命令数据传输，直至“忙”状态被复位，所以在每次运动之前先检测此状态位，判断是否为“忙”，如果是“忙”要进行软件复位，使系统可以进行数据通讯。

5）对于基于的系统来说，复位也是伺服系统操作中重要的一个环节，复位后，查看的状态字，如果不等于84H或者C4H，说明硬件复位失败，必须重新复位，否则不可以正常工作。

6）如果一切正常，控制器首先通过LM629开启真空抽吸电机M，通过抽吸装置先对微型真空吸盘抽吸，使真空吸盘对地面具有一定的吸附力，控制器并实时检测，如果地面不干净，系统会自动调节电机M加大真空吸盘对地面的吸附力。

7）在微微鼠运动过程中，传感器S1、S2、S5、S6（四个独立的红外发射管OPE5594A发出的红外光经接收器TSL262接受后转化为周围迷宫的信息）判断周围的环境并送给STM32F407，STM32F407把这些环境参数转化为微微鼠左右轮要运行的位置、速度和加速度指令值，并把这些值传输给；然后由结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，二者之间的差值经内部的PID调节器调节后生成控制电机运行的实际速度-时间运动梯形图，这个梯形包含的面积就是微微鼠两个电机X和电机Y要运行的距离Y。STM32F407然后根据这个梯形图生成驱动两轴直流电机的PWM波，并通过驱动桥驱动两个独立直流电机X和电机Y向前运动。

在运动过程中，传感器S2和S5会对左右的挡墙进行判断，并记录储存当前迷宫挡墙信息，微微鼠根据前进方向左右挡墙的迷宫信息进入双墙导航模式、单墙导航模式或者惯性导航模式：当传感器S2、S5探测到左右均有挡墙时，全数字微微鼠进入双墙导航模式，此时传感器S2、S5会把探测到的迷宫信息输入到全数字伺服控制器，控制器把红外实时探测的值与预设定值相比较。当微微鼠快速行走时受到外界干扰脱离了设定中心位置时，探测值将与设定值产生较大偏差，在新的采样周期到来后，STM32F407把这个差值转化为新的三闭环伺服系统的指令给定值，然后由结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，全数字伺服控制器通过内部的PID调节器开始进行实时补偿并微调电机的PWM波输入，此时微微鼠在陀螺仪的控制下完成姿态的调整，使其重新回到设定中心位置；当传感器S2探测到左侧面有挡墙而S5探测到右侧无挡墙时，全数字微微鼠进入左墙导航模式，此时传感器S2会把探测到的迷宫信息输入到全数字伺服控制器，控制器会把实时探测的值与预设定值想比较，当微微鼠快速行走时受到外界干扰脱离了设定中心位置时，探测值将与设定值产生一定偏差，在新的采样周期到来后，STM32F407把这个差值转化为新的三闭环伺服系统的指令给定值，然后由结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，全数字伺服控制器通过内部的PID调节器开始进行实时补偿并微调电机的PWM波输入，此时微微鼠在陀螺仪的控制下完成姿态的调整，使其重新回到设定中心位置；当传感器S5探测到右侧面有挡墙而S2探测到左侧无挡墙时时，全数字微微鼠进入右墙导航模式，此时传感器S5会把探测到的迷宫信息输入到全数字伺服控制器，控制器会把实时探测的值与预设定值想比较，当微微鼠快速行走时受到外界干扰脱离了设定中心位置时，探测值将与设定值产生一定偏差，在新的采样周期到来后，STM32F407把这个差值转化为新的三闭环伺服系统的指令给定值，然后由结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，全数字伺服控制器通过内部的PID调节器开始进行实时补偿并微调电机的PWM波输入，此时微微鼠在陀螺仪的控制下完成姿态的调整，使其重新回到设定中心位置；当传感器S2、S5探测到左右均无迷宫挡墙时，全数字微微鼠进入惯性导航模式，此时传感器S2、S5无法为微微鼠伺服系统提供位置参考。一旦进入此状态，陀螺仪G1就显得非常重要，实时测量微微鼠的速度，并输送给控制器，控制器会自动进行积分运算得到其瞬时位置数据，并依靠开始的位置信息，推算现有的位置信息，并把现有的三闭环伺服参数输送给，然后结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，全数字伺服控制器通过内部的PID调节器开始进行实时补偿并微调电机的PWM波输入使得微微鼠顺利通过无红外导航的迷宫，直至红外传感器S2、S5开启为止。

当微微鼠在陀螺仪的控制下运动到新地址时，微处理器将更新其坐标，并判断其坐标是不是（F，F）、（F，10）、（10，F）、（10，10）其中的一个，如果不是将继续更新其坐标，如果是的话通知控制器已到达目标，然后置返航探索标志为1，微微鼠准备返程探索；

8）在导航过程中如果微微鼠发现迷宫求解出现死循环将向STM32F407发出中断请求，STM32F407会对中断做第一时间响应，然后禁止工作；如果STM32F407的中断响应没有来得及处理，微微鼠的X电机和Y电机将原地自锁；

9）装在微微鼠上的高速永磁直流电机X和电机Y上的光电编码器会分别输出其位置信号A和位置信号B。如果位置信号A超前于位置信号B，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，LM629内的位置寄存器加1；如果位置信号A滞后于位置信号B，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，LM629内的位置寄存器减1。

10）装在微微鼠上的高速永磁直流电机X和电机Y上的光电编码器的位置信号A脉冲、B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给LM629寄存器，记录电机的绝对位置，然后控制器会把这些值换算成微微鼠在迷宫中的具体位置。

11）STM32F407根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置数据等给LM629作为伺服系统的参考值，LM629结合光电编码器的反馈和电流传感器的反馈生成新的偏差，此偏差经LM629内部调节器调节后生成驱动电机运动的PWM波，在电机运动过程中陀螺仪G1实时记录微微鼠新的运动状态，并反馈给控制器，在新的采样周期到来后，重新调整微微鼠两轴三闭环伺服系统的加速度、速度和位置指令。

12）为了能够实现微微鼠在导航时准确的坐标计算，微微鼠左右的传感器S2和S5会时刻对周围的迷宫挡墙和柱子进行探测，如果S2或者S5发现传感器信号发生了跃变，则说明微微鼠进入了迷宫挡墙和柱子的交接点，老鼠准备离开当前的迷宫格子，STM32F407会根据微微鼠当前运行的距离进行精确补偿，此方法可以彻底消除微微鼠在迷宫中已经累计的误差，为微微鼠求解复杂迷宫奠定了基础。

13）如果微微鼠在导航过程中遇到故障撞墙时，电机的电流将增大，当超过设定值时，LM629的中断命令LPES将会向控制器发出中断请求，此时控制器会立即控制LM629停止工作，从而有效地解决了堵转问题。

14）微微鼠在导航过程会时刻检测电池电压，当系统出现低压时，传感器V1将开启并发出报警提示，有效地保护了锂离子电池。

15）在微微鼠导航过程中，STM32F407会对电机的转矩进行在线辨识，当电机的转矩受到外界干扰出现较大抖动时，控制器把此干扰当作新的电流环输入，重新调整的电流环输入，然后迅速调整其PID参数，并利用电机力矩与电流的关系进行实时快速补偿，减少了电机转矩抖动对微微鼠快速探索和冲刺的影响。

本领域技术人员应当知晓，本发明的保护方案不仅限于上述的实施例，还可以在上述实施例的基础上进行各种排列组合与变换，在不违背本发明精神的前提下，对本发明进行的各种变换均落在本发明的保护范围内。

Claims (10)

1.双核中速两轮微微鼠全数字导航伺服控制器，其特征是包括底盘，所述底盘上设有电池装置、传感器装置、陀螺仪装置G、真空抽吸装置和控制单元模块，所述底盘两侧连接运动轮；

所述电池装置电连接控制单元模块，所述传感器装置和陀螺仪装置分别信号连接控制单元模块，所述控制单元模块分别信号连接电机X、Y和M；

所述电机X和电机Y分别传动联接运动轮，所述电机M传动联接真空抽吸装置，所述抽真空装置连接真空吸盘，所述真空吸盘位于底盘底部；

所述陀螺仪装置G位于底盘的中心位置；

所述传感器装置包括位于底盘两侧的红外传感器S1和S6，位于底盘前端的红外传感器S2和S5，所述传感器S1、S6共同作用判断前方挡墙，传感器S2判断其左边挡墙的存在，传感器S5判断其右边挡墙的存在，同时S2和S5合作为直线运动提供导航依据；

所述控制单元模块包括上位机程序模块和运动控制程序模块，所述上位机程序模块包括STM32F407处理器，所述运动控制程序模块包括LM629处理器，所述LM629处理器包括两轴行走伺服控制单元和单轴真空吸附伺服控制单元，所述STM32F407处理器电性连接LM629处理器，所述两轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。

2.根据权利要求1所述的双核中速两轮微微鼠全数字导航伺服控制器，其特征是还包括有电压传感器V1，所述电压传感器V1电连接电池装置，信号连接控制单元模块的STM32F407处理器。

3.根据权利要求1所述的双核中速两轮微微鼠全数字导航伺服控制器，其特征是还包括光补偿传感器L1，所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。

4.根据权利要求1所述的双核中速两轮微微鼠全数字导航伺服控制器，其特征是还包括有电流传感器C1和C2，所述电流传感器C1和C2信号连接STM32F407处理器。

5.根据权利要求1至4中任意一项所述的双核中速两轮微微鼠全数字导航伺服控制器，其特征是所述LM629处理器信号连接电机X和电机Y，所述电机X和Y分别传动联接微微鼠的两轮。

6.根据权利要求5所述的双核中速两轮微微鼠全数字导航伺服控制器，其特征是所述电机X和Y为高速永磁直流电机，所述电机M为微型直流电机。

7.根据权利要求1至4中任意一项所述的双核中速两轮微微鼠全数字导航伺服控制器，其特征是所述红外传感器S1和S2信号发射方向间的夹角为75°～90°角，所述红外传感器S5和S6信号发射方向间的夹角为75°～90°角。

8.一种如权利要求1所述的双核中速两轮微微鼠全数字导航伺服控制方法，其特征是步骤如下：

1）电压检测：检测电池电压、传感器电路和时钟电路；

2）底盘真空抽吸：STM32F407控制器首先开启真空抽吸直流电机M，通过真空抽吸装置先对真空吸盘抽吸，使真空吸盘对地面具有一定的吸附力，STM32F407控制器实时检测；

3）迷宫行走：判断周围的环境并送给STM32F407，STM32F407把这些环境参数转化为微微鼠左右轮要运行的位置、速度和加速度指令值，并把这些值传输给LM629；然后由LM629结合光电编码器的反馈和电流传感器C1、C2的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，二者之间的差值经LM629内部的PID调节器调节后生成控制电机运行的实际速度-时间运动梯形图，这个梯形包含的面积就是微微鼠两个电机X和电机Y要运行的距离Y。

9.STM32F407然后根据这个梯形图生成驱动两轴直流电机的PWM波，并通过驱动桥驱动两个独立直流电机X和电机Y向前运动；

4）调整纠错：光补偿传感器L1会时刻对外界干扰光源进行采集，然后传输给STM32F407，STM32F407会根据L1的数值自动补偿外界干扰，减少了外界干扰光源对微微鼠快速探索和冲刺时的干扰；

5）坐标检测：装在微微鼠上的高速永磁直流电机X和电机Y上的光电编码器的位置信号A脉冲、B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给LM629寄存器，记录电机的绝对位置，然后控制器会把这些值换算成微微鼠在迷宫中的具体位置；

6）路径计算：STM32F407根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置数据等给LM629作为伺服系统的参考值，LM629结合光电编码器的反馈和电流传感器的反馈生成新的偏差，此偏差经LM629内部调节器调节后生成驱动电机运动的PWM波，在电机运动过程中陀螺仪G1实时记录微微鼠新的运动状态，并反馈给控制器，在新的采样周期到来后，重新调整微微鼠两轴三闭环伺服系统的加速度、速度和位置指令。

10.根据权利要求8所述的双核中速两轮微微鼠全数字导航伺服控制方法，其特征是所述迷宫行走的运动过程中，传感器S2和S5会对左右的挡墙进行判断，并记录储存当前迷宫挡墙信息，微微鼠根据前进方向左右挡墙的迷宫信息进入双墙导航模式、单墙导航模式或者惯性导航模式。