一种双核三轴四轮变结构高速微微鼠全数字伺服控制器的制造方法

一种双核三轴四轮变结构高速微微鼠全数字伺服控制器的制造方法
【专利摘要】本发明公开了一种双核三轴四轮变结构高速微微鼠全数字伺服控制器，包括微微鼠壳体、车轮、第一红外传感器、第二红外传感器、第三红外传感器、第四红外传感器、第五红外传感器、第六红外传感器、第一高速直流伺服电机、第二高速直流伺服电机、真空吸附电机、第一磁电编码器、第二磁电编码器、运动传感器以及采集传感器，还包括控制板，所述的控制板采用双核控制器，包括ARM和FPGA，所述的ARM与STM32F407进行通信连接。通过上述方式，本发明提高了双核微微鼠全数字伺服系统的稳定性，有效防止了微微鼠在高速迷宫探索时的地面打滑，避免了微微鼠远远偏离中心位置现象的发生，提高了其快速求解时的稳定性。
【专利说明】
一种双核三轴四轮变结构高速微微鼠全数字伺服控制器
技术领域
[0001 ]本发明涉及一种双核三轴四轮高速微微鼠（PIC0M0USE)全数字伺服控制系统，属 于微型迷宫机器人领域。
【背景技术】
[0002] 随着微电子技术、计算机控制技术的不断进步，国外专家在微电脑鼠求解迷宫的 技术基础之上提出了一种更具有挑战性的迷宫机器人一微微鼠，其常用二维结构如图1所 示。为增强迷宫复杂程度以及老鼠求解迷宫的难度，迷宫挡墙由原有的180mm变成了 90mm， 原有的迷宫由16*16格变成了32*32格，新的迷宫二维结构如图2所示。电源一旦打开，微微 鼠全程完全依靠自身携带的传感器自动导航，并求解由1024个迷宫格组成的各种复杂迷 宫，能够快速从起点找到一条到达设定目标点的最佳路径，然后以最快的速度冲刺到终点。
[0003] 微微鼠作为一种新型的迷宫机器人技术，每年世界上有诸多国家和地区在展开这 种技术的竞争，并具有不同的竞赛规则，微微鼠在整个迷宫中的行走分为两部分:探索和冲 刺，相应花费的时间为探索时间TS和冲刺时间TD以及由于违规而出现的加罚时间TP，微微 鼠的最终性能HME由TS、TD和TP来决定，其中最具有代表性的是日本、美国、英国和新加坡。
[0004] 日本规则如下：TBffi=TD; 美国规则如下:HME=TS/30+TD+TP，其中TP为微微鼠出现故障的加罚时间； 英国规则如下:HME=TS/30+TD+TP，其中TP为微微鼠出现故障的加罚时间； 新加坡规则如下:TIME=TS/60+TD+TP，其中TP为微微鼠出现故障的加罚时间。
[0005] 从上面的国际规则可以看出，微微鼠求解迷宫在微微鼠的整个运动中占有非常重 要的位置，一旦微微鼠求解迷宫失败，微微鼠的整个功能也就无法实现。
[0006] 微微鼠在迷宫中行走过程中要时刻判断周围的环境，然后传输参数到控制器，由 控制器反复控制其在迷宫方格中精确的加速和减速运动。一只优秀的微微鼠成功求解迷宫 必须具备良好的感知能力，有良好的行走能力，优秀的智能算法，否则将无法完成任务。如 果采用现有的简易算法和结构实现微微鼠求解迷宫，在实践中发现： (1) 由于求解迷宫数目的大量增加，且迷宫探测设置点不在是原有的迷宫中心，而是迷 宫中的任意一格，使得原有的简易微微鼠求解迷宫技术无法求解现有的复杂迷宫； (2) 由于微微鼠尺寸的大幅减少，如果微微鼠采用图1中的六组传感器技术探索复杂迷 宫，在一些对探索有时间要求的国际规则中，受单核处理器处理速度影响，经常会出现探索 时间较长的现象发生，最终导致微微鼠竞争失败； (3) -些简易微微鼠样机伺服系统采用比较低级的单核芯片和算法，使得微微鼠在迷 宫当中的行走一般都要花费较长的时间，不仅消耗了大量电池的能量，而且在真正的大赛 中也无法取胜； (4) 由于迷宫挡墙尺寸的减少，使得微微鼠单格行走运行的距离减少，微微鼠在求解迷 宫过程中的频繁刹车和启动加重了单核控制器的工作量，单核伺服控制器无法满足微微鼠 快速启动和停车的要求； (5) 对于两轮驱动的微微鼠来说一般要求驱动其运动的两个电机PWM控制信号要同步， 受计算能力的限制单核伺服系统很难满足这一条件，微微鼠在直道上行驶时不能准确的行 走在中线上，在尚速行走时很容易撞到迷宫挡墙，导致任务失败； (6) 由于受单核控制器容量和算法影响，微微鼠无法存储迷宫信息，当遇到掉电情况时 所有的信息将消失，这使得整个探索过程要重新开始； (7) 微微鼠在迷宫行走时，基于单核控制易于受到外界干扰，由于没有进行及时补偿导 致微微鼠碰撞迷宫挡墙，最终无法完成任务； (8) 两轮微微鼠系统在加速行走时由于重心后移，使得老鼠前部轻飘，即使在良好的路 面上微微鼠也会打滑，有可能导致撞墙的现象出现，不利于高速微微鼠的发展； (9) 两轮微微鼠系统如果设计不当造成重心前偏，将导致驱动轮上承受的正压力减小， 这时微微鼠系统更加容易打滑，也更容易走偏，导致导航失败； (10) 两轮微微鼠系统如果设计不当造成重心侧偏将导致两个驱动轮承受的正压力不 同，在快速启动时两轮打滑程度不一致，瞬间就偏离轨迹，转弯时，其中正压力小的轮子可 能打滑，导致转弯困难； (11) 由于传统的微微鼠多采用光电编码器实现老鼠的速度和位置的反馈，由于光电编 码器的体积较大，使得微微鼠的体积相对较大，无法实现微微鼠伺服系统的微型化发展； (12) 由于比赛场地的灰尘较大，特别是迷宫经过多次比赛后，迷宫地板上吸附的灰尘 较大，使得快速行驶的微微鼠很容易打滑，导致迷宫信息错误，最终微微鼠无法完成探索和 冲刺的任务； (13) 由于传统微微鼠伺服系统采用的集成驱动芯片体积较大，微微鼠的体积无法微型 化且重量较大，在相同功率直流伺服电机驱动下无法取得足够大的加速度，系统的加速性 能较弱； (14) 基于单核控制的微微鼠全数字伺服系统既要处理各种光电传感器信号和迷宫信 息，而且还要处理微微鼠的多轴伺服控制，使得处理器的工作量较大，极大了影响了微微鼠 速度的提高和稳定性。
[0007] 微微鼠求解迷宫是国际新兴的一门技术，由于微微鼠技术的难度较高以及迷宫设 计的复杂性，导致国内还没有研发此机器人的单位。因此，需要借助现有的先进控制技术以 及先进控制芯片设计一种双核多轴多轮防滑的微微鼠求解迷宫的全数字伺服控制器。

【发明内容】

[0008] 本发明主要解决的技术问题是提供一种双核三轴四轮变结构高速微微鼠全数字 伺服控制器，为克服单核控制器不能满足微微鼠快速行走的稳定性和快速性的要求，舍弃 了国产微微鼠所采用的单一单片机工作模式，在吸收国外先进控制思想的前提下，自主发 明了基于ARM( STM32F405 )+FPGA(A3P250)的全新双核控制模式，控制板以FPGA为处理核心， 以磁电编码器为反馈元件，通过其内部三闭环伺服控制实现三轴直流伺服的同步控制， STM32F405从复杂的工作当中解脱出来，实现部分的信号处理算法和FPGA的控制逻辑，并响 应中断，实现数据通信和存储实时信号。
[0009] 为解决上述技术问题，本发明采用的一个技术方案是：提供了一种双核三轴四轮 变结构高速微微鼠全数字伺服控制器，包括微微鼠壳体、车轮、第一红外传感器、第二红外 传感器、第三红外传感器、第四红外传感器、第五红外传感器、第六红外传感器、第一高速直 流伺服电机、第二高速直流伺服电机、真空吸附电机、第一磁电编码器、第二磁电编码器、运 动传感器以及采集传感器，四个所述的车轮分别两两设置在微微鼠壳体的左右两侧边，所 述的第一红外传感器和第六红外传感器分别设置在微微鼠壳体的左右两侧边并位于车轮 的前端，所述的第二红外传感器和第五红外传感器设置在微微鼠壳体的前端，所述的第三 红外传感器斜向设置在第一红外传感器和第二红外传感器之间，所述的第四红外传感器斜 向设置在第五红外传感器和第六红外传感器之间，所述的第一高速直流伺服电机和第二高 速直流伺服电机分别安装在微微鼠壳体的左右两边并位于两个车轮之间的位置，所述的真 空吸附电机设置在第一高速直流伺服电机和第二高速直流伺服电机上方的中间位置，所述 的第一磁电编码器和第二磁电编码器分别设置在第一高速直流伺服电机和第二高速直流 伺服电机的下方，所述的运动传感器和采集传感器依次设置在真空吸附电机的下方，还包 括控制板，所述的控制板设置在微微鼠壳体内，所述的控制板采用双核控制器，包括ARM和 FPGA，所述的ARM与FPGA进行通信连接。
[0010]在本发明一个较佳实施例中，所述的第三传感器和第四传感器斜向设置时与Y轴 之间的夹角大小为<tf<6〇0。
[0011]在本发明一个较佳实施例中，所述的ARM采用STM32F405控制器，所述的FPGA采用 A3P250控制器。
[0012] 在本发明一个较佳实施例中，所述的第一磁电编码器和第二磁电编码器均采用基 于磁电传感器AS5040H的编码器。
[0013] 在本发明一个较佳实施例中，所述的车轮上均设置有真空吸盘。
[0014] 在本发明一个较佳实施例中，所述的双核三轴四轮变结构高速微微鼠全数字伺服 控制器还包括电源装置，由所述的电源装置单独提供电流驱动所述的控制板，所述的控制 板分别发出第一控制信号、第二控制信号和第三控制信号，由所述的第一控制信号、第二控 制信号和第三控制信号分别控制所述的第二高速直流伺服电机、第一高速直流伺服电机和 真空吸附电机的信号合成之后再控制微微鼠的运动。
[0015] 在本发明一个较佳实施例中，所述的电源装置采用锂离子电池。
[0016] 在本发明一个较佳实施例中，所述的双核三轴四轮变结构高速微微鼠全数字伺服 控制器还设置有上位机程序和运动控制程序，所述的上位机程序还包括迷宫探知、迷宫存 储和数据输出，所述的运动控制程序还包括基于ARM+FPGA三轴全数字直流伺服控制、迷宫 定位和模式选择。
[0017] 在本发明一个较佳实施例中，所述的基于ARM+FPGA三轴全数字直流伺服控制还包 括相互连接的四轮微微鼠全数字直流伺服控制和基于单轴直流电机吸附摩擦伺服控制，所 述的四轮微微鼠全数字直流伺服控制包括位置模块、速度模块和加速度模块。
[0018] 本发明的有益效果是:本发明的双核三轴四轮变结构高速微微鼠全数字伺服控制 器，提高了双核微微鼠全数字伺服系统的稳定性，真空吸附电机通过真空抽吸装置不停抽 吸微型真空吸盘内的空气，使微型真空吸盘的内外压力不一样，产生一定的负压，使其对带 有灰尘的迷宫地面产生一定的吸附力，有效防止了微微鼠在高速迷宫探索时的地面打滑； 运动传感器可以测量出微微鼠的偏航率，当微微鼠在求解迷宫姿态发生变化超过设定阀值 时，在一个新的采样周期FPGA控制器立即对其位置补偿，避免了微微鼠远远偏离中心位置 现象的发生，提高了其快速求解时的稳定性。
【附图说明】
[0019] 为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使 用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于 本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它 的附图，其中： 图1为二轮驱动微微鼠二维图； 图2为微微鼠32*32迷宫示意图； 图3基于A3906SESTR-T两轴双核微微鼠全数字伺服系统连接示意图； 图4为双核三轴四轮变结构微微鼠二维原理图； 图5为基于STM32F405+A3P250双核三轴四轮变结构微微鼠原理框图； 图6为基于STM32F405+A3P250双核三轴四轮变结构微微鼠伺服控制程序框图； 图7为微微鼠导航模式。
【具体实施方式】
[0020] 下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施 例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通 技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范 围。
[0021]如图4所示，本发明实施例包括： 一种双核三轴四轮变结构高速微微鼠全数字伺服控制器，包括微微鼠壳体、车轮、第一 红外传感器S1、第二红外传感器S2、第三红外传感器S3、第四红外传感器S4、第五红外传感 器S5、第六红外传感器S6、第一高速直流伺服电机Y、第二高速直流伺服电机X、真空吸附电 机M、第一磁电编码器M1、第二磁电编码器M2、运动传感器G1以及采集传感器L1，四个所述的 车轮分别两两设置在微微鼠壳体的左右两侧边，所述的第一红外传感器S1和第六红外传感 器S6分别设置在微微鼠壳体的左右两侧边并位于车轮的前端，所述的第二红外传感器S2和 第五红外传感器S5设置在微微鼠壳体的前端，所述的第三红外传感器S3斜向设置在第一红 外传感器S1和第二红外传感器S2之间，所述的第四红外传感器S4斜向设置在第五红外传感 器S5和第六红外传感器S6之间，所述的第一高速直流伺服电机Y和第二高速直流伺服电机X 分别安装在微微鼠壳体的左右两边并位于两个车轮之间的位置，所述的真空吸附电机MS 置在第一高速直流伺服电机Y和第二高速直流伺服电机X上方的中间位置，所述的第一磁电 编码器Ml和第二磁电编码器M2分别设置在第一高速直流伺服电机Y和第二高速直流伺服电 机X的下方，所述的运动传感器G1和采集传感器L1依次设置在真空吸附电机M的下方。其中， 所述的车轮包括X轮、Y轮、R轮和Z轮。本实施例中，还包括控制板，所述的控制板设置在微微 鼠壳体内，所述的控制板采用双核控制器，包括ARM和FPGA，所述的ARM与FPGA进行通信连 接。
[0022]上述中，所述的第三传感器S1和第四传感器S4斜向设置时与Y轴之间的夹角大小 为:450:<:沒:<600。
[0023] 在本实施例中，所述的第一磁电编码器Ml和第二磁电编码器M2均采用基于磁电传 感器AS5040H的编码器;所述的车轮上均设置有真空吸盘，吸附性能好。
[0024]如图5所示，所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器还包括电 源装置，由所述的电源装置单独提供电流驱动所述的控制板，所述的控制板分别发出第一 控制信号、第二控制信号和第三控制信号，由所述的第一控制信号、第二控制信号和第三控 制信号分别控制所述的第二高速直流伺服电机、第一高速直流伺服电机和真空吸附电机的 信号合成之后再控制微微鼠的运动。
[0025]上述中，所述的电源装置采用锂离子电池。
[0026] STM32F4系列除引脚和软件兼容高性能的F2系列外，F4的主频（168MHz)高于F2系 列（120MHz)，并支持单周期DSP指令和浮点单元、更大的SRAM容量（192 KB，F2为128 KB)、 512KB-1MB的嵌入式闪存以及影像、网络接口和数据加密等更先进的外设。STM32F4系列基 于最新的STM32F405 Cortex M4内核，在现有出色的STM32微控制器产品组合中新增了信 号处理功能，并提高了运行速度;STM32F405X集成了定时器、3个ADC、2个DAC、串行接口、外 存接口、实时时钟、CRC计算单元和模拟真随机数发生器在内的整套先进外设，这些性能使 得F4系列可以较容易满足控制和信号处理功能混合的数字信号控制需求。高效的信号处理 功能与C 〇rteX-M4处理器系列的低能耗、低成本和易于使用的优点的组合，使得其可以为微 微鼠多种传感器信号处理提供了可靠的依据。
[0027] FPGA是英文Field Programmable Gate Array的缩写，即现场可编程门阵列，是在 PAL、GAL、EPLD等可编程器件的基础上进一步发展的产物。它是作为专用集成电路(ASIC)领 域中的一种半定制电路而出现的，即解决了定制电路的不足，又克服了原有可编程器件门 电路数有限的缺点。FPGA采用软件化的设计思想实现硬件电路的设计，这样就使得基于 FPGA设计的系统具有良好的可复用和修改性。A3P250是ACTEL公司设计的一种基于非易失 性Flash技术的FPGA器件。器件采用了精细颗粒架构VersaTi le，具有250K系统门结构，采用 了130 nm的工艺技术，内核电压1.5 V，A3P250是反熔丝的，抗辐射、耐高低温、功耗低、速 度快，应用较广，上述特点使得A3P250特别适合用于由于高性能的多轴直流伺服驱动控制， 特别适合应用于本发明中的三轴同步直流伺服控制，直接把STM32F405从复杂的伺服控制 中解脱出来。
[0028] 本发明为了减少双核微微鼠高速全数字伺服控制系统的体积，舍弃了传统的两轴 直流伺服电机H型驱动桥L6207D，而采用体积更小、电压更小的两轴直流伺服电机驱动桥 A3906SESTR-T，A3906SESTR-T是一种单双路直流电机驱动芯片，A3906旨在用于低电压步 进电动机、单路及双路直流电机的脉宽控制(PWM)，可在各通道输出高达1 A的电流，工作 电压范围为2.5至9 VJ3906SESTR-T内置固定关断时间PWM计时器，根据对芯片外围 采样电阻的选择，设置峰值电流。过流输出标记用于通知控制器电机电流已经达到峰值，可 用于过流保护，上述特点使得A3906SESTR-T特别适合应用于微微鼠的两轴行走伺服系统 中，A3906SESTR-T与微微鼠两轴直流伺服电机的连接图如图3所示，其中INI，IN2，IN3，IN4 和SLEEP信号来自于伺服控制器，控制器通过调整INI，IN2，IN3，IN4的信号来调整其输出信 号0UT1A，0UT1B，0UT2A，0UT2B，继而实现直流伺服电机的四象限运动。
[0029] 如图6所示，所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器还设置有 上位机程序和运动控制程序，所述的上位机程序还包括迷宫探知、迷宫存储和数据输出，所 述的运动控制程序还包括基于ARM+FPGA三轴全数字直流伺服控制、迷宫定位和模式选择。
[0030] 其中，所述的基于ARM+FPGA三轴全数字直流伺服控制还包括相互连接的四轮微微 鼠全数字直流伺服控制和基于单轴直流电机吸附摩擦伺服控制，所述的四轮微微鼠全数字 直流伺服控制包括位置模块、速度模块和加速度模块。
[0031] 为了提高双核微微鼠高速全数字伺服系统的稳定性，增加微微鼠与地面的接触面 积，减少微微鼠重心的前移、后移或者是侧移，本发明借助齿轮机械结构实现两轴四轮的功 能，左右每轴电机的转轴上配备一个小的机械齿轮，此齿轮和前后两个轮子的大齿轮机械 连接，其中大齿轮的齿为每周60,小齿轮的齿为每周15,通过这样的机械连接使得每个轮子 都变成了动力轮。
[0032] 为了进一步提高双核微微鼠高速全数字伺服系统的稳定性，防止微微鼠在高速探 索时由于地面灰尘较多而导致行走打滑，本发明在微微鼠伺服硬件系统中加入了微型直流 电机M，在微微鼠运动过程中，电机M通过真空抽吸装置不停抽吸微型真空吸盘内的空气，使 微型真空吸盘的内外压力不一样，产生一定的负压，使其对带有灰尘的迷宫地面产生一定 的吸附力，有效防止了微微鼠在高速迷宫探索时的地面打滑。
[0033]为了进一步提高双核微微鼠在求解迷宫时的稳定性，本发明在双核微微鼠伺服硬 件系统中加入了高性能MEMS运动传感器LY3200ALH，LY3200ALH可以测量出微微鼠的偏航 率，LY3200ALH参数时刻被STM32F405控制器记录并计算，当微微鼠在求解迷宫姿态发生变 化超过设定阀值时，在一个新的采样周期FPGA控制器立即对其位置补偿，避免了微微鼠远 远偏离中心位置现象的发生，提高了其快速求解时的稳定性。
[0034] 为了更好的采集迷宫信息和减少红外传感器占用的体积，本发明采用红外传感器 SFH4350代替了传统使用的0PE9954A，红外传感器31、32、33、34、35、36的红外光经侧边挡墙 反馈后会被对应的红外接收器BPW85A接收，然后BPW85A的反馈值经控制器计算后作为当前 位置的反馈，控制器通过这些反馈值调整微微鼠的姿态。
[0035] 为了减少光电编码器占用的提价，并减少灰尘对光电编码器的影响，本发明采用 基于磁电传感器AS5040H的编码器M1、M2替代了传统的光电编码器C1和C2,此传感器可以有 效测量出两轴直流伺服电机运动时的速度和位移，为微微鼠快速探索、冲刺的三闭环伺服 控制提供了可靠依据。
[0036] 在实验中发现，采用四组传感器可以提高传感器的采样频率，有利于提高微微鼠 的速度，但是四组传感器如果补偿做的不好将有可能导致微微鼠求解的是一个错误的迷 宫；如果采用六组传感器探测未知迷宫，求解迷宫一般不会出现错误，但是过多的传感器组 合影响了采样频率，不利于微微鼠速度的提高;为了兼顾不同的国际规则以及迷宫探索的 准确性，本发明自主研发了基于六组传感器自由组合变结构探测迷宫的全新控制模式，所 发明的三轴四轮微微鼠二维结构如图4所示，在图4中，为了更好的探测迷宫，传感器S3和S4 与Y轴之间的夹角大小为:妨^没乂佛 0，在此区间，传感器配合工作状态最佳。对于探索无 时间要求的国际规则，通过软件开启六组传感器探索模式，传感器S1、S6共同作用判断前方 挡墙，传感器S2、S3判断其左边挡墙的存在，传感器S4、S5判断其右边挡墙的存在，同时S2、 S3和S4、S5合作为微微鼠直线运动提供导航依据;对于探索有时间要求的国际规则，通过软 件开启四组传感器模式，传感器S1、S6共同作用判断前方挡墙，传感器S3判断其左边挡墙的 存在，传感器S4判断其右边挡墙的存在，同时S3和S4合作为微微鼠直线运动提供导航依据。 红外传感器SI、S3、S4、S6的红外光经侧边挡墙反馈后会被对应的红外接收器BPW85A接收， 然后BPW85A的反馈值经控制器计算后作为当前位置的反馈，然后控制器通过这些反馈值调 整微微鼠的姿态。
[0037]综上，本发明为克服单核控制器不能满足微微鼠快速行走的稳定性和快速性的要 求，舍弃了国产微微鼠所采用的单一单片机工作模式，在吸收国外先进控制思想的前提下， 自主发明了基于ARM(STM32F405)+FPGA(A3P250)的全新双核控制模式，其原理框图如图5所 示:控制板以FPGA为处理核心，以磁电编码器为反馈元件，通过其内部三闭环伺服控制实现 三轴直流伺服的同步控制。STM32F405从复杂的工作当中解脱出来，实现部分的信号处理算 法和FPGA的控制逻辑，并响应中断，实现数据通信和存储实时信号。
[0038]参照图4,具体实施步骤是： 对于本文设计的双核STM32F405+FPGA控制器，在电源打开状态下，微微鼠先进入自锁 状态，控制器开启真空抽吸电机M，自动调整微微鼠与地面的摩擦力系数。微微鼠依靠前方、 左右侧面蔽障传感器31、33、34、36(或31、32、33、34、35、36)根据实际导航环境传输参数给 控制器中的STM32F405，STM32F405把这些环境参数转化为微微鼠两轴直流伺服电机要运行 的距离、速度和加速度参考值，STM32F405然后与A3P250通讯，A3P250根据这些指令参数再 结合电机磁电编码器Ml和M2的反馈生成两轴电机X和电机Y的同步PWM波，A3P250调整两轴 直流伺服电机驱动芯片A3906SESTR-T的输出信号，A3906SESTR-T驱动两轴电机X和Y前进， 电机X和电机Y运行状态经磁电编码器Ml和M2反馈给A3P250,经A3P250处理后调整 A3906SESTR-T的管脚输出状态，继而改变微微鼠的运动状态。A3P250实时通过各种传感器 对微微鼠行走速度、地面等状况进行检测，并通过调节电机M的伺服控制来有效调节真空吸 盘对地面的吸附力，增加了微微鼠快速行走时的稳定性。
[0039]参照图3、图4、图5、图6和图7，其具体的功能实现如下： 1) 为了能够驱动三轴四轮微微鼠进行快速运动并且能够实现程序的快速移植，本控制 系统引入了 FPGA(A3P250)，但是通过I/O 口与STM32F405连接并实时通讯，由STM32F405通过 中断控制其开通和关断； 2) 打开电源开关瞬间，STM32F405会对电池电压进行检测，如果低压的话，将禁止FPGA 工作，电机不能自锁，同时电压传感器VI将工作，并发出报警信号； 3) 如果电压正常，系统将检测传感器电路和时钟电路，如果传感器电路和时钟电路出 现故障，系统将自动复位，重新检测，如有问题，将报警。如果系统正常工作，控制器首先开 启真空抽吸电机M，通过抽吸装置先对微型真空吸盘抽吸，使真空吸盘对地面具有一定的吸 附力，控制器并实时检测，如果地面不干净，系统会自动调节电机M加大真空吸盘对地面的 吸附力； 4) 在微电脑运动过程中，传感器31、32、33、34、35、36(六个独立的红外发射管3?財350 发出的红外光经六个独立的红外接收器BPW85A接受后转化为周围迷宫的信息)判断周围的 环境并送给STM32F405,控制器会根据规则选择开启四组传感器模式或是六组传感器模式， STM32F405把这些环境参数转化为微微鼠前后左右四轮要运行的距离、速度和加速度指令 值，STM32F405然后与A3P250通讯，A3P250根据各种探索和冲刺的不同要求，再结合磁电编 码器Ml和M2的速度和位移反馈，经其内部伺服控制算法生成控制电机X和电机Y的PWM波以 及方向和速度-时间运动梯形图，这个梯形包含的面积就是微微鼠直流伺服电机X和直流伺 服电机Y要运行的距离，A3P250调整A3906SESTR-T的管脚输出状态驱动电机X和电机Y按照 梯形图规律运动，在微微鼠前进过程中，A3P250通过磁电传感器Ml和M2实时对电机速度、微 微鼠行走距离、地面等状况进行检测，并通过调节电机M的伺服控制来有效调节真空吸盘对 地面的吸附力，并把处理数据通讯给STM32F405; 5) 在微微鼠运动过程中如果发现迷宫求解出现死循环将向STM32F405和A3P250发出中 断请求，STM32F405和A3P250会对中断做第一时间响应，如果STM32F405的中断响应没有来 得及处理，A3P250会直接释放微微鼠的电机X、电机Y的驱动信号，使老鼠静止在原地； 6) 电机X、电机Y相对应的磁电编码器会输出其位置信号A和位置信号B，磁电编码器的 位置信号A脉冲和B脉冲逻辑状态每变化一次，A3P250内的位置寄存器会根据左右轮的运行 方向加1或者是减1; 7) 等溢出中断来临，磁电编码器Ml和M2的位置信号被A3P250处理后，就产生一个INDEX 信号给STM32F405寄存器，记录电机的绝对位置，然后换算成微微鼠在迷宫中的具体位置， 并储存当前迷宫信息； 8) STM32F405根据微微鼠在迷宫的具体位置，送相应的加速度、速度和位置更新指令给 A3P250的梯形图发生器作为参考值，由梯形图发生器结合电机参数反馈信息，通过A3P250 内部的三环数字PID控制器生成功率驱动电机需要的PWM波信号和电机正反转信号，用来实 现微微鼠系统电机X、电机Y的伺服控制； 9) STM32F405根据实际外围磁电传感器Ml和M2传感信号确定电机X、电机Y正反转信号， 从而实现电机的正反转控制； 10) 微微鼠一旦加速、减速、路面灰尘较多时，A3P250会自动使能真空吸附装置，开启电 机M的伺服控制，微微鼠自然切换到三轴四轮驱动状态，增强了微微鼠的附着力和操控性； 11) 在微微鼠向前运动过程中，传感器S3 (或S2、S3)和S4(或S4、S5 )会对左右的挡墙进 行判断，STM32F405记录储存当前迷宫挡墙信息，微微鼠根据前进方向左右挡墙的迷宫信息 进入单墙导航模式或者是双墙导航模式：当传感器S3(或S2、S3)和S4(或S4、S5)探测到左右 均有挡墙时，全数字微微鼠进入双墙导航模式，此时传感器S3(或S2、S3)和S4(或S4、S5)会 把探测到的迷宫信息输入到全数字伺服控制器，控制器把红外实时探测的值与预设定值相 比较。当微微鼠快速行走受到外界干扰脱离了设定中心位置时，探测值将与设定值产生较 大偏差，此时传感器G1 (LY3200ALH)记录下当前微微鼠的瞬时偏航率，A3P250全数字伺服控 制器开始进行实时补偿并通过A3906SESTR-T微调两轴伺服电机X和Y的PWM波输入，通过此 方式可以重新调整微微鼠的姿态，使其重新回到设定中心位置；当传感器S3(或S2、S3)探测 到左侧面有挡墙而S4(或S4、S5)探测到右侧无挡墙时，全数字微微鼠进入左墙导航模式，此 时传感器S3(或S2、S3)会把探测到的迷宫信息输入到全数字伺服控制器，控制器会把实时 探测的值与预设定值想比较，当微微鼠快速行走时受到外界干扰脱离了设定中心位置时， 探测值将与设定值产生一定偏差，此时传感器G1 (LY3200ALH)记录下当前微微鼠的瞬时偏 航率，A3P250全数字伺服控制器开始进行实时补偿并通过A3906SESTR-T微调两轴伺服电机 X和Y的PWM波输入通过此方式可以重新调整微微鼠的姿态，使其重新回到设定中心位置;当 传感器S4(或S4、S5)探测到右侧面有挡墙而S3(或S2、S3)探测到左侧无挡墙时时，全数字微 微鼠进入右墙导航模式，此时传感器S4(或S4、S5)会把探测到的迷宫信息输入到全数字伺 服控制器，控制器会把实时探测的值与预设定值想比较，当微微鼠快速行走时受到外界干 扰脱离了设定中心位置时，探测值将与设定值产生一定偏差，此时传感器G1(LY3200ALH)记 录下当前微微鼠的瞬时偏航率，A3P250全数字伺服控制器开始进行实时补偿并通过 A3906SESTR-T微调两轴伺服电机X和Y的P丽波输入通过此方式可以重新调整微微鼠的姿 态，使其重新回到设定中心位置； 12) 当双核三轴四轮微微鼠在传感器G1 (LY3200ALH)的控制下运动到新地址时， STM32F405微处理器将更新其坐标，并判断其坐标是不是(X1，Y1)，如果不是将继续更新其 坐标，如果是将通知控制器已到达目标，然后置返航探索标志为1，微微鼠准备返程探索； 13) 为了能够实现微微鼠在探索和冲刺时准确的坐标计算，微微鼠左右的传感器S3(或 S2、S3)和S4(或S4、S5)会时刻对周围的迷宫挡墙和柱子进行探测，如果S3(或S2、S3)或者S4 (或S4、S5)发现传感器信号发生了较大数值的跃变，则说明微微鼠进入了从有迷宫挡墙到 无迷宫挡墙(或者是从无迷宫挡墙到有迷宫挡墙)状态的变化，STM32F405会根据微微鼠当 前运行状态并结合磁电编码器Ml和M2的输出进行精确补偿，彻底消除微微鼠在复杂迷宫中 已经累计的误差； 14) 如果微微鼠在运行过程中遇到故障撞墙时，电机X、电机Y的电流将增大，当超过设 定值时，A3906SESTR-T的电流采集电路将工作，A3P250将拉低A3906SESTR-T的输入信号， A3P250立即控制A3906SESTR-T停止工作，进而释放电机X、电机Y，从而有效地解决了堵转问 题； 15) 微微鼠在运行过程会时刻检测电池电压，当系统出现低压时，传感器VI将开启并发 出报警提示，有效地保护了锂离子电池； 16) 在微微鼠运动过程中，如果伺服系统转矩出现了脉动干扰，A3P250会根据当前状态 对电流环加以补偿，快速调整电流环的PID参数，使得系统快速稳定下来，防止转矩脉动对 伺服系统性能的影响； 17) 在微微鼠整个运动过程中，传感器L1会时刻对外界干扰光源进行采集，然后传输给 STM32F405，STM32F405会根据L1的自动补偿外界干扰，减少了外界干扰光源对微微鼠快速 探索和冲刺伺服系统的干扰； 18) 当微微鼠在传感器Gl (LY3200ALH)的控制下回到坐标起点(0，0)，STM32F405会根据 微微鼠原有的状态更新其信息：如果是第一次返程探索回来，则控制器会根据已有的探测 迷宫信息采用改进洪水算法求解出最佳路径；如果是冲刺返程探索回来，则控制器会根据 更新的探测迷宫信息采用改进洪水算法二次求解出最佳路径，然后A3P250根据冲刺速度的 大小自动调整电机M，改变真空吸盘对地面的吸附力，满足快速冲刺时的摩擦需要。
[0040]本发明具有的有益效果是： 1:本发明中的电阻和电容均采用0402封装替代了原有的0603封装，可以更好的减少微 微鼠的体积，有利于微微鼠的微型化发展； 2:本发明STM32F405采用BGA封装替代了原有的LQFP176封装，使得芯片占有的体积更 小，有利于减少微微鼠伺服控制系统的体积，且BGA封装更利于微微鼠伺服控制器芯片的散 执. ， 3:为了充分提高双核微微鼠伺服系统的稳定性和行驶能力，并兼顾两轮驱动的优点， 本发明舍弃了原有的多动力实时四驱结构，通过齿轮机械结构实现两轴四轮功能，既减少 了微微鼠控制器驱动动力电机的数目，又通过齿轮实现了多轮的功能，提高微微鼠的求解 迷宫时的行走稳定性； 4:由于STM32F405集成了新的DSP和FPU指令，168MHz的高速处理性能提高了数字信号 控制器的执行速度和代码效率，使得控制器处理传感器信号实时性能增加； 5:根据需要实现吸附功能。在微微鼠求解迷宫过程中，一旦遇到路面灰尘较多或加速 状况时，FPGA会根据两轴电机的磁电编码器反馈立即开启吸附电机M的伺服控制，微微鼠系 统自然切换到三轴四轮驱动状态，增强了微微鼠的附着力和操控性； 6:由于采用两轴四轮驱动结构，增加了微微鼠与地面的接触面积，减少了两轮微微鼠 由于机械结构造成的失速问题的发生，使得微微鼠具有更好的行走功能； 7:在此微微鼠伺服系统中引入了高性能MEMS运动传感器LY3200ALH，实现了微微鼠在 求解迷宫时的瞬时旋转速度的检测，并利用反馈实现全程导航的实时校正，有利于提高微 微鼠高速行走时的稳定性和动态性能； 8:微微鼠转向时，为了保证旋转的稳定性和准确性，通过传感器LY3200ALH的实时反 馈，基于STM32F405+A3P250双核伺服控制器对微微鼠的转弯实时校正，提高了微微鼠求解 迷宫信息的正确性； 9:由FPGA输出PWM调制信号和方向信号，通过驱动电路可以直接驱动三轴直流伺服电 机，不仅减轻了 STM32F405的负担，简化了接口电路，而且省去了 STM32F405内部编写位置、 速度控制程序，以及各种PID算法的麻烦，使得系统的调试简单； 10:根据国际规则的不同，控制器通过软件可以实现四组传感器和六组传感器的变结 构切换，提高了微微鼠求解迷宫的技术，有利于提高系统的运算速度； 11:由于采用磁电编码器技术替代了传统的光电编码器技术，使得微微鼠的体积可以 更小，有利于微微鼠微型化的发展； 12:由于采用磁电编码器替代了传统的光电编码器，使得灰尘对编码器的数据采集影 响大大降低，提高了微微鼠高速行走速度和位移的精确性，也保证了微微鼠求解迷宫的准 确性； 13:由于本控制器采用FPGA处理微微鼠探索、冲刺的各种算法，有效地防止了程序的 "跑飞"，抗干扰能力大大增强； 14:在微微鼠运行过程中，FPGA会对高速直流伺服电机X、电机Y和电机M的转矩进行在 线辨识并利用电机力矩与电流的关系进行补偿，减少了电机转矩抖动对微微鼠快速探索、 快速冲刺的影响； 15:通过调节电机M可以有效调节真空吸盘对地面的吸附力，消除了微微鼠在高速探 索、高速冲刺时打滑现象的发生； 16:由于具有存储功能，这使得微微鼠掉电后可以轻易的调取已经探索好的迷宫信息， 使二次探索的时间和路径大大降低； 17:在控制中，FPGA可以根据实际周围迷宫情况调整控制器内部的PID参数，轻松实现 分段P、H)、PID控制和非线性PID控制，使系统具有一定的自适应； 18:在运动过程中，充分考虑了电池在这个系统中的作用，基于STM32F405+FPGA控制器 时刻都在对微微鼠的运行状态进行监测和运算，避免了大电流的产生，所以从根本上解决 了大电流对锂离子电池的冲击，避免了由于大电流放电而引起的锂离子电池过度老化现象 的发生； 19:驱动桥A3906SESTR-T的电流采集功能可以很好的解决微微鼠在运行过程中遇到撞 墙等情况发生的电机堵转，当输出超出设定值时，A3906SESTR-T的电流采集电路立即工作， 直流电机X、电机Y的驱动信号被拉低，从而有效地解决了堵转问题。
[0041]综上所述，本发明的双核三轴四轮变结构高速微微鼠全数字伺服控制器，提高了 双核微微鼠全数字伺服系统的稳定性，真空吸附电机通过真空抽吸装置不停抽吸微型真空 吸盘内的空气，使微型真空吸盘的内外压力不一样，产生一定的负压，使其对带有灰尘的迷 宫地面产生一定的吸附力，有效防止了微微鼠在高速迷宫探索时的地面打滑;运动传感器 可以测量出微微鼠的偏航率，当微微鼠在求解迷宫姿态发生变化超过设定阀值时，在一个 新的采样周期FPGA控制器立即对其位置补偿，避免了微微鼠远远偏离中心位置现象的发 生，提尚了其快速求解时的稳定性。
[0042]以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发 明说明书内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领 域，均同理包括在本发明的专利保护范围内。
【主权项】
1. 一种双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征在于，包括微微鼠 壳体、车轮、第一红外传感器、第二红外传感器、第三红外传感器、第四红外传感器、第五红 外传感器、第六红外传感器、第一高速直流伺服电机、第二高速直流伺服电机、真空吸附电 机、第一磁电编码器、第二磁电编码器、运动传感器以及采集传感器，四个所述的车轮分别 两两设置在微微鼠壳体的左右两侧边，所述的第一红外传感器和第六红外传感器分别设置 在微微鼠壳体的左右两侧边并位于车轮的前端，所述的第二红外传感器和第五红外传感器 设置在微微鼠壳体的前端，所述的第三红外传感器斜向设置在第一红外传感器和第二红外 传感器之间，所述的第四红外传感器斜向设置在第五红外传感器和第六红外传感器之间， 所述的第一高速直流伺服电机和第二高速直流伺服电机分别安装在微微鼠壳体的左右两 边并位于两个车轮之间的位置，所述的真空吸附电机设置在第一高速直流伺服电机和第二 高速直流伺服电机上方的中间位置，所述的第一磁电编码器和第二磁电编码器分别设置在 第一高速直流伺服电机和第二高速直流伺服电机的下方，所述的运动传感器和采集传感器 依次设置在真空吸附电机的下方，还包括控制板，所述的控制板设置在微微鼠壳体内，所述 的控制板采用双核控制器，包括ARM和FPGA，所述的ARM与FPGA进行通信连接。2. 根据权利要求1所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的第三传感器和第四传感器斜向设置时与Y轴之间的夹角大小为 ::45e<￡U60e。3. 根据权利要求1所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的ARM采用STM32F405控制器，所述的FPGA采用A3P250控制器。4. 根据权利要求1所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的第一磁电编码器和第二磁电编码器均采用基于磁电传感器AS5040H的编码器。5. 根据权利要求1所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的车轮上均设置有真空吸盘。6. 根据权利要求1所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器还包括电源装置，由所述 的电源装置单独提供电流驱动所述的控制板，所述的控制板分别发出第一控制信号、第二 控制信号和第三控制信号，由所述的第一控制信号、第二控制信号和第三控制信号分别控 制所述的第二高速直流伺服电机、第一高速直流伺服电机和真空吸附电机的信号合成之后 再控制微微鼠的运动。7. 根据权利要求6所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的电源装置采用锂离子电池。8. 根据权利要求1所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器还设置有上位机程序和运 动控制程序，所述的上位机程序还包括迷宫探知、迷宫存储和数据输出，所述的运动控制程 序还包括基于ARM+FPGA三轴全数字直流伺服控制、迷宫定位和模式选择。9. 根据权利要求8所述的双核三轴四轮变结构高速微微鼠全数字伺服控制器，其特征 在于，所述的基于ARM+FPGA三轴全数字直流伺服控制还包括相互连接的四轮微微鼠全数字 直流伺服控制和基于单轴直流电机吸附摩擦伺服控制，所述的四轮微微鼠全数字直流伺服 控制包括位置模块、速度模块和加速度模块。
【文档编号】G05D1/02GK105929832SQ201610506443
【公开日】2016年9月7日
【申请日】2016年7月1日
【发明人】张好明, 陈阳
【申请人】江苏若博机器人科技有限公司