双核中速六轮全数字导航伺服控制器及其控制方法.pdf

本发明公开了双核中速六轮全数字导航伺服控制器，包括底盘，所述底盘上设有电池装置、传感器装置、辅助导航装置、真空抽吸装置和控制单元模块，所述底盘两侧连接运动轮；所述辅助导航装置包括方向传感器D1、陀螺仪G1和加速度计A1。本发明通过位于底盘下的真空抽吸装置解决了微微鼠在行走中打滑的现象，同时根据微微鼠前进的速度以及地面情况自动调节真空抽吸直流电机M的伺服控制，使得微微鼠不在受制于迷宫地面路况。

权利要求书
1.  双核中速六轮全数字导航伺服控制器，其特征是包括底盘，所述底盘上设有电池装置、传感器装置、辅助导航装置、真空抽吸装置和控制单元模块，所述底盘两侧连接运动轮；
所述辅助导航装置包括方向传感器D1、陀螺仪G1和加速度计A1；
所述电池装置电连接控制单元模块，所述传感器装置和陀螺仪装置分别信号连接控制单元模块，所述控制单元模块分别信号连接电机X、Y、Z、U、R和M；
所述电机X和电机Y分别位于中部两侧并传动联接运动轮，电机Z和电机R位于尾部两侧并传动联接运动轮，电机W和电机U位于前部两侧并传动联接运动轮；所述电机M传动联接真空抽吸装置，所述抽真空装置连接真空吸盘，所述真空吸盘位于底盘底部；
所述传感器装置包括位于底盘两侧的红外传感器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、C3、C4、C5和C6，所述电流传感器C1、C2、C3、C4、C5和C6信号连接STM32F407处理器。

5.  根据权利要求1至4中任意一项所述的双核中速六轮全数字导航伺服控制器，其特征是所述LM629处理器信号连接电机X、Y、Z、U、R和W。

6.  根据权利要求5所述的双核中速六轮全数字导航伺服控制器，其特征是所述电机X、Y、Z、R、U和W为高速永磁直流电机，所述电机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~C6的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，二者之间的差值经LM629内部的PID调节器调节后生成控制电机运行的实际速度-时间运动梯形图，这个梯形包含的面积就是微微鼠六个电机X、Y、Z、U、R和W要运行的距离。

9.  STM32F407然后根据这个梯形图生成驱动六轴直流电机的PWM波，并通过驱动桥驱动独立直流电机X、Y、Z、U、R和W向前运动；
4）调整纠错：光补偿传感器L1会时刻对外界干扰光源进行采集，然后传输给STM32F407，STM32F407会根据L1的数值自动补偿外界干扰，减少了外界干扰光源对微微鼠快速探索和冲刺时的干扰；
5）坐标检测：装在微微鼠上的高速永磁直流电机X、Y、Z、R、U和W上的光电编码器的位置信号A脉冲、B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给LM629寄存器，记录电机的绝对位置，然后控制器会把这些值换算成微微鼠在迷宫中的具体位置；
6）路径计算：STM32F407根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置数据等给LM629作为伺服系统的参考值，LM629结合光电编码器的反馈和电流传感器的反馈生成新的偏差，此偏差经LM629内部调节器调节后生成驱动电机运动的PWM波，在电机运动过程中陀螺仪G1实时记录微微鼠新的运动状态，并反馈给控制器，在新的采样周期到来后，重新调整微微鼠六轴闭环伺服系统的加速度、速度和位置指令。

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

说明书双核中速六轮全数字导航伺服控制器及其控制方法
技术领域
本发明涉及微型迷宫导航机器人领域，尤其涉及一种双核中速六轮全数字导航伺服控制器及其控制方法。
背景技术
微电脑鼠是使用嵌入式微控制器、传感器和机电运动部件构成的一种智能行走机器人，在国外已经竞赛了将近30年，其常采用两轮结构，两轮微电脑鼠二维结构如图1所示。
微电脑鼠可以在不同“迷宫”中自动记忆和选择路径，采用相应的算法，快速地到达所设定的目的地。其求解的迷宫之一示意如图2所示。
随着微电子技术、计算机控制技术的不断进步，国外专家在微电脑鼠求解迷宫的技术基础之上提出了一种更具有挑战性的迷宫机器人---微微鼠：为了增强迷宫复杂程度以及老鼠求解迷宫的难度，迷宫挡墙由原有的180mm变成了90mm，原有的迷宫由16*16格变成了32*32格，新的迷宫二维结构如图3所示。电源一旦打开，微微鼠全程完全依靠自身携带的传感器自动导航，并求解由1024个迷宫格组成的各种复杂迷宫，能够快速从起点找到一条到达设定目标点的最佳路径，然后以最快的速度冲刺到终点。作为一种自助导航智能机器人，因为通过无线装置可以向控制器输入迷宫信息，微微鼠或者微电脑鼠国际准则拒绝使用无线装置，为了能够得到微微鼠或者是微电脑鼠探索、冲刺后的信息，只能通过算法快速寄存并储存其导航信息，当完成任务后通过控制器的232串口或者是USB串口读取存储信息。
微微鼠在迷宫中导航过程中要时刻判断周围的环境，然后传输参数到控制器，由控制器反复控制其在迷宫方格中精确的加速和减速进行运动。一只优秀的微微鼠必须具备良好的感知能力，有良好的行走能力，优秀的智能算法，否则将无法完成任务。微微鼠技术综合了多学科知识，对于提升在校学生的动手能力、团队协作能力和创新能力，促进学生课堂知识的消化和扩展学生的知识面都非常有帮助，并且微微鼠走迷宫技术的开展可以培养大批相关领域的人才，进而促进相关领域的技术发展和产业化进程。
如果认为微微鼠只是微电脑鼠的简单拷贝，按照微电脑鼠技术来设计微微鼠，在实践中则会发现如下问题：
（1）基于轮式的微微鼠只能被动的适应迷宫地面的打滑程度，随着微微鼠速度的提高，其打滑概率也极大增加，导致求解迷宫失败。
（2）由于求解迷宫数目的大量增加，原有的微电脑鼠求解迷宫技术无法求解现有的复杂迷宫。
（3）由于微微鼠尺寸的大幅减少，如果微微鼠采用图1中的六组传感器技术探测迷宫，经常出现传感器相互干扰的状况，导致其读取迷宫信息失败。
（4）由于微电脑鼠伺服系统采用的都是比较低级的算法，使得微微鼠在迷宫当中的导航一般都要花费较长的时间，这使得在真正的大赛中无法取胜。
（5）由于迷宫挡墙尺寸的减少，使得微微鼠单格运行的距离减少，微微鼠频繁的刹车和启动加重了单片机的工作量，单一的单片机无法满足微微鼠快速启动和停车的要求。
（6）对于两轮驱动的微微鼠来说一般要求驱动其运动的两个电机PWM控制信号要同步，受计算能力的限制单一单片机伺服系统很难满足这一条件，微微鼠在直道上行驶时不能准确的行走在中线上，在高速导航时很容易撞到迷宫挡墙，导致任务失败。
（7）由于受单片机容量和算法影响，微微鼠无法存储迷宫信息，当遇到掉电情况时所有的信息将消失，这使得整个探索过程要重新开始。
（8）两轮微微鼠系统在加速导航时由于重心后移，使得老鼠前部轻飘，即使在良好的路面上微微鼠也会打滑，有可能导致撞墙的现象出现，不利于高速微微鼠的发展。
（9）两轮微微鼠系统在正常导航时行驶时如果设计不当造成重心前偏，将导致驱动轮上承受的正压力减小，这时微微鼠系统更加容易打滑，也更容易走偏，导致导航失败。
（10）两轮微微鼠系统在正常导航行驶时如果设计不当造成重心侧偏将导致两个驱动轮承受的正压力不同，在快速启动时两轮打滑程度不一致，瞬间就偏离轨迹，转弯时，其中正压力小的轮子可能打滑，导致转弯困难。
（11）由于采用两个动力轮驱动，为了满足复杂状态下的加速和减速导航，使得单个驱动电机的功率较大，不仅占用的空间较大，而且有时候在一些相对需求能量较低的状态下造成“大马拉小车”的现象出现，不利于微微鼠本体微型化发展和微微鼠系统能源的节省。
（12）如果采用前驱+后驱的全时四驱，虽然导航时动态性能较好，但是全时四驱顾名思义随时随地都保持四驱状态，但是其耗能较高。
（13）如果采用前驱+后驱的分时四驱探索方式，无论是采用前驱或者是后驱时都具有一定的弱点，在转弯探索时角度不是过大，就是不够，转弯动态性能较差；
（14）如果采用中驱+后驱的四轮驱动方式进行导航，通过释放后驱两轮采用中驱两轮转弯，虽然微微鼠转弯探索性能有所提高，电机的效率也得到一定优化，但是在高速导航时会造成重心后偏，需要软件对此进行保护。
（15）无论是两轮驱动或者是四轮驱动，在高速导航遇到迷宫接缝处具有一定的高度差时，导航动态性能都会收到严重影响。
（16）基于轮式的微微鼠在导航阶段只能被动的适应迷宫地面的打滑程度，随着微微鼠速度的提高，其打滑概率也极大增加，导致求解迷宫失败。
微微鼠求解迷宫是国际新兴的一门技术，由于微微鼠技术的难度较高以及迷宫设计的复杂性，导致国内还没有研发此机器人的单位。因此，需要设计一种满足初级者学习微微鼠求解迷宫的全数字伺服控制器。
发明内容
本发明的目的是解决现有技术的不足，提供一种双核中速六轮全数字导航伺服控制器，以解决微微鼠在导航过程中打滑、传感器相互干扰、处理时间慢等问题。
本发明采用的技术方案是：为了提高运算速度，保证微微鼠数字导航伺服控制器的稳定性和可靠性，本发明在基于STM32F407的控制器中引入精密运动控制专用芯片LM629，形成基于STM32F407+专用运动控制芯片的全新双核控制器，此控制器充分考虑电池在这个系统的作用，把导航伺服控制器中工作量最大的六轴行走伺服系统以及单轴真空抽吸伺服系统由STM32F407和LM629配合处理，充分发挥STM32F407数据处理速度较快和LM629伺服调节器的特点，而人机界面、路径优化、轨迹参数、加速度、速度和位置数据存储和计算、I/O控制等功能交给STM32F407完成，这样就实现了STM32F407与LM629的分工，同时二者之间也可以进行通讯，实时进行数据交换和调用。
本发明采用的有益效果是：1、在导航过程中，充分考虑了电池在这个系统中的作用，基于STM32F407+LM629控制器时刻都在对微微鼠的运行状态进行监测和运算，同时电流传感器C1~C6实时在对电流进行检测并反馈给控制器进行保护，避免了大电流的产生，并且时刻显示锂离子电池的SOC，有利于了解电池的能量状态，由于本发明采用永磁直流电机，电机的加速度和电机的电流呈一定比例关系，所以当电池SOC较低时，为了保证导航时能量的要求，可以在导航前提前换掉电池，从而减少了电池因为放电电流过高对高速导航的误干扰。
2、为了充分提高多轮微微鼠导航伺服控制器的稳定性和行驶能力，并兼顾两轮中置转向的优点，并保证微微鼠的重心位置有利于各种运动，本发明采用六轮驱动结构：中置驱动的功率较大，后置和前置驱动的四个个电机功率较小，只有在动力需求较高时才启动，起到助力作用。由于采用六轮驱动技术，微微鼠前后中六个动力轮都有动力，可按迷宫地面和周围环境状态不同而将需求扭矩按不同比例分布在前后所有的轮子上，提高了微微鼠高速导航对复杂迷宫的适应能力。
3、导航时根据需要实现两驱、四驱和六驱的功能。在正常导航速度下，微微鼠一般会采用释放前后四轮，采用中置两轮驱动的方式；而一旦需要稍微提速，此时的加速度较小，STM32407会根据加速度大小并立即使能中驱后驱的四片LM629工作，将微微鼠需求扭矩部分分配给后置两个助力驱动轮，同时控制器改变电机M的伺服控制，微微鼠系统自然切换到四轮驱动导航状态。而一旦需要快速提速或者是地面灰尘较多时，STM32407会根据加速度大小并使能前驱中驱后驱的六片LM629，工作并立即将微微鼠需求扭矩分配给前中后置六个动力驱动轮，同时控制器改变电机M的伺服控制，微微鼠系统自然切换到六轮驱动状态，不仅满足快速导航时动力需求，而且还增加了微微鼠导航时的平衡性，增强了微微鼠的附着力和操控性。
4、由于采用多轮驱动的复合驱动方式，当需要加速时，把动力分配到四个或者是六个电机，一旦一个动力轮由于地面、机械结构等造成暂时离开地面，STM32407可以重新分配扭矩并使能相应的LM629，把更多的扭矩分配在未失速的驱动轮上，使系统迅速脱离不稳定状态，重新回到四轴动力或者是六轴动力平衡状态，使得微微鼠具有更好的直线导航调整能力。
5、微微鼠导航转向时，为了保证旋转的稳定性，控制器使能中驱的两片LM629并禁止前后四片LM629，微微鼠采用中置的两驱动轮实现转弯，并释放前后置的四个助力驱动轮，利用方向传感器D1、陀螺仪G1、加速度计A1实时测量微微鼠的瞬时角度、角速度和加速度，为微微鼠精确转弯提供可靠准确的反馈。
6、多轮驱动方式下微微鼠导航伺服控制器，在正常导航时如果设计不当造成重心偏移，将导致一侧驱动轮上承受的正压力减小，STM32F407会自动调整这一侧的动力分配，然后使能相应的LM629调整电机的伺服控制使微微鼠处于一种新的平衡状态，防止微微鼠打滑。
7、由LM629处理六轮微微鼠高速导航时的六只直流电机的伺服控制和单轴真空吸附电机的伺服控制，使得控制比较简单，大大提高了运算速度，解决了单片机软件运行较慢的瓶颈，缩短了开发周期短，并且程序可移植能力强。
8、在此微微鼠导航伺服控制器中引入了方向传感器D1、三轴陀螺仪G1和三轴加速度计A1，实现了微微鼠在迷宫导航时的瞬时加速度、速度、角度的检测，不仅省去了利用陀螺仪进行积分得到角度的运算，而且还减少了利用陀螺仪进行积分得到角度的误差，控制器利用方向传感器D1、三轴陀螺仪G1和三轴加速度计A1的直接反馈实现全程导航和二次补偿，有利于提高微微鼠高速导航时的稳定性和动态性能。
9、采用四组传感器迷宫导航技术替代原有的六组传感器迷宫导航技术不仅减少了系统中各传感器组的干扰，并提高了迷宫挡墙采集频率，有利于提高导航伺服控制器的运算速度。
10、由于本控制器采用STM32F407处理迷宫读取和更新以及LM629处理导航时七轴伺服控制算法，有效地防止了程序的“跑飞”，抗干扰能力大大增强。
11、在微微鼠快速导航过程中，控制器会对七轴直流电机的转矩进行在线辨识并利用电机力矩与电流的关系进行补偿，减少了电机转矩抖动对微微鼠快速导航的影响。
12、通过调节电机M可以有效调节真空吸盘对地面的吸附力，并通过检测微微鼠实际的运动速度控制器自动调节电机M实时改变真空吸盘对地面的吸附能力，满足实际需求，彻底消除了了微微鼠在高速导航时打滑现象的发生，微微鼠也从高速导航时被动适应迷宫地面转为不在受其影响，对高速导航有质的影响。
13、由于具有存储功能，这使得微微鼠可以轻易调取已经探索好的迷宫信息，可以优化二次冲刺的路径，降低导航时间。
附图说明
图1为原二轮六眼微电脑鼠二维图。
图2为微电脑鼠16*16迷宫示意图。
图3为微微鼠32*32迷宫示意图
图4为微微鼠导航伺服控制器程序框图。
图5为本发明示意图。
图6为本发明的原理框图。
图7为本发明的STM32F407处理器与LM629的连接原理图。
图8为微微鼠前进示意图。
图9为MX118示意图。
具体实施方式
下面结合图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脚二种表面安装式封装，在一个芯片内集成了数字式运动控制的全部功能，使得设计一个快速、准确的运动控制系统的任务变得轻松、容易。
为了减少驱动电路所占的空间，本发明采用MX118新芯片。为了能够对电机的电流能够进行检测保护，每轴电机配备一个0.01欧姆的贴片电阻传感器来进行电流采样，并把采样结果反馈给STM32F407，为微微鼠的电流环控制提供反馈。MX118是为低电压下工作的系统而设计的单通道直流电机驱动集成电路。它具有H桥驱动器，为对空间敏感的系统设计提供理想选择，内置功率MOSFET开关，可实现对负载直流电机正转、反转、刹车以及待机四个功能的控制，刹车功能能使负载电机快速停止转动。MX118内置温度保护功能，当低内阻的负载电机堵转或者输出短路时，MX118输出电流瞬间增大，电路功耗急剧上升，芯片温度急剧升高，当芯片温度超过内部温度保护电路设置得最高温度点(典型值150℃)后，内部电路关断MX118内置的功率开关管，切断负载电流，避免温度过高造成塑料封装冒烟、起火等安全隐患。MX118常用封装为SOT23-6，MX118管脚分布和其真值表如图9所示。
在实验中发现，由于微微鼠体积的大幅度降低，图1中传感器S2和S3以及S4和S5经常产生相互干扰；同时由于每组传感器采集迷宫挡墙参数都需要一定的时间，加重了采样周期，导致采样频率降低，延长了处理器STM32F407的处理时间。本发明为克服六组传感器探测迷宫的缺陷，自主研发了基于四组传感器探测迷宫的导航伺服控制器，微微鼠二维结构如图5所示：传感器S1、S6共同作用判断前方挡墙，同时传感器S1和S6也可以为微微鼠各种运动提供加速或者是减速判据，有效提高其运动速度；传感器S2判断其左边挡墙的存在，传感器S5判断其右边挡墙的存在，同时S2和S5合作为微微鼠直线运动提供导航依据。为了使用智能算法计算迷宫挡墙信息，在微微鼠校正阶段，微微鼠被人为放在迷宫不同设定位置，红外传感器S1、S2、S5、S6的红外光经侧边挡墙反馈后会被对应的红外接收器TSL262接收，然后TSL262的接收值经控制器计算后作为当前位置的设定阀值，然后微微鼠在导航过程中通过与这些阀值比较得到其迷宫信息。在相同位置，图1中传感器S2的计算阀值明显比S3精确，传感器S5的计算阀值明显比S4精确，而且S2和S5还可以精确测量到迷宫从有挡墙到无挡墙以及无挡墙到有挡墙的变化，这个位置的传感器信号变化可以被控制器捕捉到，然后在此位置可以对微微鼠进行精确补偿，这对于复杂迷宫计算至关重要，如果没有此智能补偿的话，微微鼠在复杂迷宫中的累计误差足以使求解迷宫失败。因此本控制器采用图6中的结构来求解迷宫，以确保迷宫的准确性。
为了进一步提高六轮微微鼠在导航时的稳定性，由于陀螺仪G1通过积分其角速度得到微微鼠偏离中心的角度，在长期运动中会造成积分误差，本发明了为了能够精确测量微微鼠偏离中心的角度，在微微鼠导航伺服控制器中加入了方向传感器D1，三轴陀螺仪G1和三轴加速度计A1，控制器舍弃了单一陀螺仪工作模式。在微微鼠迷宫导航期间全程开启方向传感器D1、陀螺仪G1和加速度计A1，方向传感器D1实时测量微微鼠偏离中心的角度，三轴陀螺仪G1准确测量微微鼠三个转动方向的角度运动，三轴加速度计用来测量微微鼠三个平移运动的加速度。方向传感器D1、陀螺仪G1和加速度计A1时刻准确记录微微鼠的瞬时参数并输送给控制器，当微微鼠的姿态发生变化超过设定阀值时，在一个新的采样周期就立即对其位置补偿，避免了微微鼠远远偏离中心位置现象的发生，提高了其快速导航时的稳定性；同时方向传感器D1也可以为微微鼠校正阶段提供一定的帮助，在早期设置微微鼠姿态阀值时，由于没有方向传感器辅助，微微鼠的放置方向一旦有一定偏差，造成微微鼠稍微有点偏头，通过红外测量作为最后给定的参数阀值就一定错误，微微鼠在实际运动中即使姿态正确也要被控制器调整到错误的设定姿态，方向传感器D1的加入避免了这种不必要的失误，也为微微鼠的精确导航提供了可靠判据；
为了进一步提高微微鼠数字导航控制器的稳定性，防止微微鼠在高速导航时打滑导致微微鼠迷宫信息错误，本发明在微微鼠导航伺服控制器中加入了微型直流电机M，在微微鼠运动过程中，电机M通过真空抽吸装置不停抽吸微型真空吸盘内的空气，使微型真空吸盘的内外压力不一样，产生一定的负压，使其对迷宫地面产生一定的吸附能力，彻底消除了微微鼠在高速导航时的地面打滑。
综上，本发明为克服单片机不能满足微微鼠求解复杂迷宫的要求，自主研发了基于STM32F407+LM629的全数字六轮双核微微鼠伺服控制器，其原理如图6所示：控制板以LM629为伺服处理核心，LM629通过数据线和控制线与STM32F407口相连，STM32F407把外界环境转化后向LM629发送位置、速度、加速度等指令值，LM629再结合光电编码器、电流传感器C1~C6的反馈生成六轴伺服系统的偏差信号，控制板以LM629为处理核心来产生七轴伺服系统PWM波控制信号，经驱动桥MX118放大后驱动微微鼠快速前进。在微微鼠运动过程中，控制器自动实现真空抽吸电机M的伺服控制，有效调整微微鼠与地面的吸附能力，增加其与地面的摩擦系数。
双核中速六轮全数字导航伺服控制器，包括底盘，所述底盘上设有电池装置、传感器装置、辅助导航装置、真空抽吸装置和控制单元模块，所述底盘两侧连接运动轮；所述辅助导航装置包括方向传感器D1、陀螺仪G1和加速度计A1；所述电池装置电连接控制单元模块，所述传感器装置和陀螺仪装置分别信号连接控制单元模块，所述控制单元模块分别信号连接电机X、Y、Z、U、R和M；所述电机X和电机Y分别位于中部两侧并传动联接运动轮，电机Z和电机R位于尾部两侧并传动联接运动轮，电机W和电机U位于前部两侧并传动联接运动轮；所述电机M传动联接真空抽吸装置，所述抽真空装置连接真空吸盘，所述真空吸盘位于底盘底部；所述传感器装置包括位于底盘两侧的红外传感器S1和S6，位于底盘前端的红外传感器S2和S5，所述传感器S1、S6共同作用判断前方挡墙，传感器S2判断其左边挡墙的存在，传感器S5判断其右边挡墙的存在，同时S2和S5合作为直线运动提供导航依据；所述控制单元模块包括上位机程序模块和运动控制程序模块，所述上位机程序模块包括STM32F407处理器，所述运动控制程序模块包括LM629处理器，所述LM629处理器包括六轴行走伺服控制单元和单轴真空吸附伺服控制单元，所述STM32F407处理器电性连接LM629处理器，所述六轴行走伺服控制单元信号连接单轴真空吸附伺服控制单元。
为了保护大电流对电池装置的冲击，还包括有电压传感器V1，所述电压传感器V1电连接电池装置，信号连接控制单元模块的STM32F407处理器。
为了减少和杜绝外界光源对传感器的干扰，从而影响整个系统的使用，还包括光补偿传感器L1，所述光补偿传感器L1信号连接控制单元模块的STM32F407处理器。
为了进一步使得本发明的伺服控制器精确控制微微鼠的前进和转向，还包括有电流传感器C1、C2、C3、C4、C5和C6，所述电流传感器C1、C2、C3、C4、C5和C6信号连接STM32F407处理器。
为进一步拓展两核处理器的处理能力，做到分工明确，工作不受干扰，所述LM629处理器信号连接电机X、Y、Z、U、R和W。
为了进一步提高微微鼠的性能，所述电机X、Y、Z、R、U和W为高速永磁直流电机，所述电机M为微型直流电机。
为进一步消除红外传感器之间的干扰，提高传感器装置的感应精度，所述红外传感器S1和S2信号发射方向间的夹角为75°～90°角，所述红外传感器S5和S6信号发射方向间的夹角为75°～90°角。
双核中速六轮全数字导航伺服控制方法，其步骤如下：1）电压检测：检测电池电压、传感器电路和时钟电路；2）底盘真空抽吸：STM32F407控制器首先开启真空抽吸直流电机M，通过真空抽吸装置先对真空吸盘抽吸，使真空吸盘对地面具有一定的吸附力，STM32F407控制器实时检测；3）迷宫行走：判断周围的环境并送给STM32F407，STM32F407把这些环境参数转化为微微鼠左右轮要运行的位置、速度和加速度指令值，并把这些值传输给LM629；然后由LM629结合光电编码器的反馈和电流传感器C1~C6的反馈经其内部位置解码器生成三闭环伺服系统的指令反馈值，二者之间的差值经LM629内部的PID调节器调节后生成控制电机运行的实际速度-时间运动梯形图，这个梯形包含的面积就是微微鼠六个电机X、Y、Z、U、R和W要运行的距离。STM32F407然后根据这个梯形图生成驱动六轴直流电机的PWM波，并通过驱动桥驱动独立直流电机X、Y、Z、U、R和W向前运动；4）调整纠错：光补偿传感器L1会时刻对外界干扰光源进行采集，然后传输给STM32F407，STM32F407会根据L1的数值自动补偿外界干扰，减少了外界干扰光源对微微鼠快速探索和冲刺时的干扰；5）坐标检测：装在微微鼠上的高速永磁直流电机X、Y、Z、R、U和W上的光电编码器的位置信号A脉冲、B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给LM629寄存器，记录电机的绝对位置，然后控制器会把这些值换算成微微鼠在迷宫中的具体位置；6）路径计算：STM32F407根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置数据等给LM629作为伺服系统的参考值，LM629结合光电编码器的反馈和电流传感器的反馈生成新的偏差，此偏差经LM629内部调节器调节后生成驱动电机运动的PWM波，在电机运动过程中陀螺仪G1实时记录微微鼠新的运动状态，并反馈给控制器，在新的采样周期到来后，重新调整微微鼠六轴闭环伺服系统的加速度、速度和位置指令。
微微鼠在迷宫导航时七轴伺服控制器以LM629微处理器为核心，其中伺服系统位置、速度、电流的给定值由微处理器STM32F407根据传感器S1、S2、S5、S6的导航值来生成，光电编码器的输出经LM629内部解码器解码后作为闭环控制的反馈值，经其内部PID调节器调节后生成控制电机的PWM波。同时STM32F407实现导航时部分的信号处理算法（直流电机转矩补偿等）和LM629的控制逻辑，并响应中断，实现数据通信和存储实时信号。
本发明是如下工作的：
1）为了能够实现分时两驱、四驱和六驱的功能，驱动七轴微微鼠进行运动，本控制系统引入了七片LM629，但是通过I/O口与STM32F407连接，由STM32F407通过使能LM629控制七个独立电机的开通和关断。
2）打开电源瞬间，STM32F407会对电池电压进行检测，如果低压的话，将禁止七片LM629工作，封锁直流电机X、电机Y、电机Z、电机R、电机U、电机W和电机M的PWM控制信号，微微鼠不能启动，同时电压传感器V1将工作，并发出报警信号。如果系统电压正常，控制器首先控制开启真空抽吸电机M的LM629工作，通过抽吸装置先对微型真空吸盘抽吸，使真空吸盘对地面具有一定的吸附能力，控制器并实时检测，如果地面不干净，系统会自动调节电机M加大真空吸盘对地面的吸附力。然后控制器解锁微微鼠，微微鼠参考方向传感器D1的反馈通过电机X和电机Y的差速运动自动调节其与运动方向的夹角直至二者之间夹角为零，完成姿态调整，彻底消除了微微鼠刚启动瞬间就出现偏头现象的发生。
3）在微电脑运动过程中，传感器S1、S2、S5、S6（四个独立的红外发射管OPE5594A发出的红外光经四个独立的红外接收器TSL262接受后转化为周围迷宫的信息）判断周围的环境并送给STM32F407，STM32F407把这些环境参数转化为微微鼠前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，由LM629再结合光电编码器和电流传感器C1~C6的反馈生成微微鼠速度-时间运动梯形图，这个梯形包含的面积就是微微鼠电机X、电机Y、电机Z、电机R、电机U、电机W要运行的距离Y。STM32F407然后根据这个梯形图的加速度和速度距离参数生成驱动六轴直流电机的PWM波，由MX118驱动六个独立电机运动，此时控制器实时检测电机X、电机Y、电机Z、电机R、电机U和电机W的光电编码器的数值，并根据其速度大小通过第七片MX118自动调节电机M，改变真空吸盘对地面的吸附能力。
4）在微微鼠运动过程中如果控制器发现迷宫求解出现死循环将向STM32F407发出中断请求，STM32F407会对中断做第一时间响应，禁止六片使能微微鼠行走的LM629工作，进而释放微微鼠的电机X、电机Y、电机Z、电机R、电机U和电机W。
5）装在电机X、电机Y、电机Z、电机R、电机U和电机W上的光电编码器会输出其位置信号A和位置信号B，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，STM32F407内的位置寄存器会根据左右轮的运行方向加1或者是减1；
6）光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给STM32F407寄存器，记录电机的绝对位置，然后换算成微微鼠在迷宫中的具体位置，并储存当前迷宫信息。
7）控制器根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置数据等给LM629的梯形图发生器作为参考值，由LM629再结合光电编码器、电流传感器C1~C6的反馈计算出微微鼠需要更新的实际加速度、速度和位置信号以及控制七轴电机运转的PWM控制信号，满足伺服系统的快速性要求。
8）STM32F407根据实际外围传感信号确定电机X（或者电机Y、电机Z、电机R、电机U、电机W和电机M）正反转信号，然后通过调整IN1和IN2的电平高低来实现电机的方向控制，进而实现微微鼠系统电机X（或者电机Y、电机Z、电机R、电机U、电机W和电机M）的方向控制。
9）当传感器S2、S5和方向传感器D1、陀螺仪G1、加速度计A1共同作用探测到微微鼠处于正常直线匀速行驶时，STM32F407一般会禁止前后助力四轮LM629工作，释放微微鼠的后轮电机Z和R和前轮电机U和电机W，采用电机X和电机Y中置驱动的方式。
10）微微鼠一旦加速、减速、路面灰尘较多时，STM32F407会根据控制器功率需求计算，自动使能前后四个助力使能芯片LM629工作，把微微鼠需求扭矩部分配给电机Z和电机R、电机U和电机W，微微鼠自然切换到四轮或者是六轮驱动状态，增强了微微鼠的附着力和操控性。
11)在微微鼠向前运动过程中，控制器实时使能LM629调节电机M的伺服控制来有效调节真空吸盘对地面的吸附力，改变微微鼠与地面的摩擦系数，同时传感器S2和S5会对左右的挡墙进行判断，并记录储存当前迷宫挡墙信息。微微鼠根据前进方向左右挡墙的迷宫信息进入双墙导航模式、单墙导航模式或者惯性导航模式：当传感器S2、S5探测到左右均有挡墙时，全数字微微鼠进入双墙导航模式，此时传感器S2、S5会把探测到的迷宫信息输入到全数字伺服控制器，控制器把红外实时探测的值与预设定值相比较。当微微鼠快速导航时受到外界干扰脱离了设定中心位置时，探测值将与设定值产生较大偏差，此时方向传感器D1、陀螺仪G1和加速度计A1记录下当前微微鼠的瞬时运动加速度、速度和偏移角度送给控制器，STM32F407综合以上数值，按照伺服控制器速度加速度要求转化为微微鼠前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，在新的采样周期到来后，LM629再结合光电编码器的反馈和电流传感器C1~C6的反馈生成微调电机的PWM波控制信号，经驱动桥MX118放大后，微微鼠在方向传感器D1、陀螺仪G1和加速度计A1的控制下完成姿态的调整，使其重新回到设定中心位置；当传感器S2探测到左侧面有挡墙而S5探测到右侧无挡墙时，全数字微微鼠进入左墙导航模式，此时传感器S2会把探测到的迷宫信息输入到全数字伺服控制器，控制器会把实时探测的值与预设定值想比较，当微微鼠快速导航时受到外界干扰脱离了设定中心位置时，探测值将与设定值产生较大偏差，此时方向传感器D1、陀螺仪G1和加速度计A1记录下当前微微鼠的瞬时运动加速度、速度和偏移角度送给控制器，STM32F407综合以上数值，按照伺服控制器速度加速度要求转化为微微鼠前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，在新的采样周期到来后，LM629再结合光电编码器的反馈和电流传感器C1~C6的反馈生成微调电机的PWM波控制信号，经驱动桥MX118放大后，微微鼠在方向传感器D1、陀螺仪G1和加速度计A1的控制下完成姿态的调整，使其重新回到设定中心位置；当传感器S5探测到右侧面有挡墙而S2探测到左侧无挡墙时时，全数字微微鼠进入右墙导航模式，此时传感器S5会把探测到的迷宫信息输入到全数字伺服控制器，控制器会把实时探测的值与预设定值想比较，当微微鼠快速导航时受到外界干扰脱离了设定中心位置时，探测值将与设定值产生较大偏差，此时方向传感器D1、陀螺仪G1和加速度计A1记录下当前微微鼠的瞬时运动加速度、速度和偏移角度送给控制器，STM32F407综合以上数值，按照伺服控制器速度加速度要求转化为微微鼠前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，在新的采样周期到来后，LM629再结合光电编码器的反馈和电流传感器C1~C6的反馈生成微调电机的PWM波控制信号，经驱动桥MX118放大后，微微鼠在方向传感器D1、陀螺仪G1和加速度计A1的控制下完成姿态的调整，使其重新回到设定中心位置；当传感器S2、S5探测到左右均无迷宫挡墙时，全数字微微鼠进入惯性导航模式，此时传感器S2、S5无法为微微鼠伺服系统提供位置参考。一旦进入此状态，方向传感器D1、陀螺仪G1和加速度计A1就显得非常重要，它实时测量微微鼠的加速度和角速度和偏移角度并输送给控制器，STM32F407把上述偏差按照伺服控制器速度加速度要求转化为微微鼠前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，在新的采样周期到来后，LM629再结合光电编码器的反馈和电流传感器C1~C6的反馈生成微调电机的PWM波控制信号，经驱动桥MX118放大后，微微鼠在方向传感器D1、陀螺仪G1和加速度计A1的控制下完成姿态的调整，使得微微鼠顺利通过无红外导航的迷宫，直至红外传感器S2、S5开启为止。
当微微鼠在方向传感器D1、陀螺仪G1和加速度计A1的控制下运动到新地址时，微处理器将更新其坐标，并判断其坐标是不是（F，F）、（F，10）、（10，F）、（10，10）其中的一个，如果不是将继续更新其坐标，如果是的话通知控制器已到达目标，然后置返航探索标志为1，微微鼠准备返程探索；
12）为了能够实现微微鼠在导航时准确的坐标计算，微微鼠左右的传感器S2和S5会时刻对周围的迷宫挡墙和柱子进行探测，如果S2或者S5发现传感器信号发生了较大数值的跃变，则说明微微鼠进入了从有迷宫挡墙到无迷宫挡墙（或者是从无迷宫挡墙到有迷宫挡墙）状态的变化，STM32F407会根据微微鼠当前运行状态精确补偿，彻底消除微微鼠在复杂迷宫中已经累计的误差。
13）当微微鼠转向时，为了保证旋转的稳定性，STM32F407立即使能前后四轮伺服调节芯片LM629，采用六轮同步伺服控制来实现转弯：在前进停车时，由STM32F407结合停车距离、停车时间生成前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，LM629再结合光电编码器反馈、电流传感器反馈、方向传感器、陀螺仪和加速度计的反馈生成六路PWM信息控制直流电机X、电机Y、电机R、电机Z、电机U和电机W，完成六轴伺服系统的同步停车控制，然后控制器禁止控制电机Z、电机R、电机U和电机W的四片LM629工作，并使能控制电机X和电机Y的两片LM629，然后微微鼠在方向传感器D1、陀螺仪G1和加速度计A1控制下原地旋转需要转弯的角度；在加速前进时，由STM32F407结合加速距离、加速时间生成前后左右六轮要运行的距离、速度和加速度指令值，然后与LM629通讯，LM629再结合光电编码器反馈、电流传感器反馈以及陀螺仪的反馈生成六路PWM信息控制直流电机X、电机Y、电机R、电机Z、电机U和电机W，完成六轴伺服系统的同步加速控制。
14）如果微微鼠在运行过程中遇到故障撞墙时，电机X、电机Y、电机Z、电机R、电机U和电机W的电流将增大，当超过设定值时，电流传感器C1~C6采集电路电压将超过设定值，STM32F407将禁止相应的LM629工作，封锁电机X、电机Y、电机Z、电机R、电机U和电机W的控制信号，进而释放电机X、电机Y、电机Z、电机R、电机U和电机W，从而有效地解决了堵转问题。
15）微微鼠在运行过程会时刻检测电池电压，当系统出现低压时，传感器V1将开启并发出报警提示，有效地保护了锂离子电池。
16）在微微鼠运动过程中，如果系统转矩出现了脉动干扰，STM32F407会根据当前状态对电流加以补偿，快速调整LM629内部PID调节器的PID参数，使得系统快速稳定下来，防止转矩脉动对伺服系统性能的影响。
17）在微微鼠整个运动过程中，传感器L1会时刻对外界干扰光源进行采集，然后传输给STM32F407，STM32F407会根据L1的自动补偿外界干扰，减少了外界干扰光源对微微鼠数字导航伺服控制器的干扰。
17)当微微鼠在陀螺仪的控制下回到坐标起点（0，0），控制器会根据微微鼠原有的状态更新其信息，如果是第一次返程探索回来，则控制器会根据已有的探测迷宫信息采用改进蚁群算法（Improvedantcolonyalgorithm）求解出最佳路径；如果是冲刺返程探索回来，则控制器会根据更新的探测迷宫信息采用改进蚁群算法（Improvedantcolonyalgorithm）二次求解出最佳路径，然后控制器根据冲刺速度的大小自动调整电机M，改变真空吸盘对地面的吸附力，满足快速导航时的摩擦需要。
本领域技术人员应当知晓，本发明的保护方案不仅限于上述的实施例，还可以在上述实施例的基础上进行各种排列组合与变换，在不违背本发明精神的前提下，对本发明进行的各种变换均落在本发明的保护范围内。