双核高速两轮微微鼠探索控制器及其控制方法_3

文档序号:9396362阅读:来源:国知局
(10,F)或(10,10)中的终点准备返程探索时,控制器会调出其已经存储的迷宫,然后根据快速迷宫算法计算出可能存在的最佳路径,返程开始进入其中认为最优的一条;5)返航,在微微鼠进入迷宫正常返航运行时,并其导航的传感器S1、S2、S5、S6将工作,并把反射回来的光电信号送给STM32F407,经STM32F407判断后送给FPGA,由FPGA运算后与STM32F407进行通讯,然后由控制器送控制信号给导航的电机X和电机Y进行确定:如果进入已经搜索的区域将进行快速前进,如果是未知返回区域则采用正常速度搜索,并时刻更新其坐标(X,Y),并判断其坐标是不是(O,0),如果是的话置返航探索标志为O。
[0058]为进一步提高本发明的微微鼠的探索前进,当传感器S2和S5会对左右的挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据前进方向左右挡墙的迷宫信息进入双墙导航模式、单墙导航模式或者惯性导航模式。
[0059]本发明是如下工作的:
双核两轮微微鼠高速探索控制器分为两部分:上位机系统和运动控制系统。其中上位机系统完成迷宫读取、坐标定位、在线输出等功能;运动控制系统完成微微鼠系统的伺服控制、数据存储、I/o控制等功能,其中工作量最大的两轴同步伺服控制单元和单轴真空吸附伺服控制单元交给FPGA处理,其余的包括上位机系统的完成交给STM32F407完成,这样就实现了 STM32F407与FPGA的分工,同时二者之间也可以进行通讯,实时进行数据交换和调用。
[0060]I)在微微鼠未接到探索命令之前,它一般会在起点坐标(0,O)等待控制器发出的探索命令,如果一切正常,控制器首先开启真空抽吸电机M,通过抽吸装置先对微型真空吸盘抽吸,使真空吸盘对地面具有一定的吸附力,控制器并实时检测,如果地面不干净,系统会自动调节电机M加大真空吸盘对地面的吸附力。一旦接到探索任务后,会沿着起点开始向终点(F,F)、(F,10)、(10,F)、(10,10)探索。
[0061]2)微微鼠放在起点坐标(0,0),接到任务后其前方的传感器S1、S6和会对前方的环境进行判断,确定有没有挡墙进入运动范围,如存在挡墙将向STM32F407发出中断请求,STM32F407会对中断做第一时间响应,如果STM32F407的中断响应没有来得及处理,微微鼠的X马达和Y马达将原地自锁,然后STM32F407 二次判断迷宫确定前方信息,防止信息误判;
3 )在微微鼠沿着Y轴向前运动,在任何一个方格的中心如果确定没有挡墙进入前方的运动范围,则微微鼠将存储其坐标(X,Y),并把向前运动一格的位置参数传输给STM32F407,根据探索控制器速度和加速度的要求,由STM32F407生成两轴直流电机三闭环伺服系统的指令给定值并传输给FPGA,然后FPGA再结合光电编码器以及电流传感器Cl、C2的反馈生成驱动直流电机运动的PWM波,PWM波通过驱动桥放大后推动微微鼠向前运动。在微微鼠向前运动过程中,传感器S2和S5会对左右的迷宫挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据左右挡墙的迷宫信息确定其进入单墙导航模式、双墙导航模式或者是惯性导航模式。在微微鼠探索迷宫过程中陀螺仪实时记录其瞬时加速度、速度和位置,当微微鼠快速探索脱离了设定中心位置时,在新的采样周期内,STM32F407根据当前S2、S5的状态重新生成三闭环伺服系统的指令给定值并传输给FPGA,然后FPGA再结合光电编码器以及电流传感器Cl、C2的反馈微调电机的PffM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置。当微微鼠在陀螺仪Gl的控制下运动一格距离到达新地址时,微处理器将更新其坐标为(X,Y+1),在Y+1〈1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续探索并更新其坐标,如果是则通知控制器已经搜索到目标,然后置返航探索标志为1,微微鼠准备返程探索;
4)在微微鼠沿着Y轴向前运动过程中如果传感器SI和S6判断前方有挡墙进入运动范围,并且此时传感器S2、S5分别判断左右都有挡墙时,微微鼠将存储此时坐标(X,Y),控制器根据传感器SI和S6的反馈计算出向前运动停车的位置参数,根据探索控制器速度和加速度的要求,由STM32F407生成直流电机闭环伺服系统的指令给定值并传输给FPGA,FPGA再结合光电编码器以及电流传感器Cl、C2的反馈生成新的驱动两轴直流电机的PffM波,PffM波驱动两个独立直流电机X和电机Y向前运动。在向前运动过程中,传感器S2、S5实时对左右的挡墙进行判断,并反馈当前迷宫挡墙信息,微微鼠进入双墙导航减速模式,在陀螺仪的帮助下,微微鼠实现在设置停车点停车。STM32F407使能FPGA并调整微微鼠两个电机的PWM波输出,使得两个永磁直流电机运动方向相反,微微鼠原地在陀螺仪的控制下实现精确的原地180度转向,并沿着Y轴开始反向运动。
[0062]微微鼠沿着Y轴反向探索原理与微微鼠沿着Y轴正向探索原理相似,在微微鼠向前运动过程中,传感器S2和S5会对左右的挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据左右挡墙的迷宫信息确定其进入单墙模式、双墙导航模式或者是惯性导航模式,陀螺仪实时记录微微鼠的瞬时加速度、速度和位置信息。当微微鼠在陀螺仪的控制下运动一格距离到达新地址时,微处理器将更新其坐标为(X,Y-1),在Y-1〈1F的前提下,判断其坐标是不是(F,F)、(F,10)、(10,F)、(10,10)其中的一个,如果不是将继续向前探索并更新其坐标,如果是则通知控制器已经搜索到目标,然后置返航探索标志为1,微微鼠准备返程探索;
5)在微微鼠沿着Y轴向前运动过程中如果有挡墙进入前方的运动范围,并且此时左右的传感器S2、S5判断左方有挡墙而右方没有挡墙时,微微鼠将存储此时坐标(X,Y),根据传感器SI和S6的反馈计算出向前运动停车的位置参数,根据探索控制器速度和加速度的要求,由STM32F407生成两轴直流电机三闭环伺服系统的指令给定值并传输给FPGA,FPGA再结合光电编码器以及电流传感器Cl、C2的反馈生成驱动直流电机运动的PffM波,PffM波通过驱动桥放大后推动微微鼠向前运动。在微微鼠向前运动过程中,传感器S2会对左挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据左挡墙的迷宫信息确定进入单墙导航模式,然后陀螺仪实时记录微微鼠的瞬时加速度、速度和位置信息。当微微鼠快速探索脱离了设定中心位置时,传感器S2的值将改变,在新的采样周期内,STM32F407根据当前状态重新生成三闭环伺服系统的指令给定值传输给FPGA,然后FPGA再结合光电编码器以及电流传感器Cl、C2的反馈微调电机的PffM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置,直至微微鼠到达中心点准确停车。STM32F407使能FPGA并调整微微鼠两个电机的PffM波输出,使得两个永磁直流电机运动方向相反,微微鼠在陀螺仪精确控制下原地向右旋转90度,然后沿着X轴正向运动。
[0063]在微微鼠沿着X轴正向运动过程中,控制器把向前运动一格的位置参数传输给STM32F407,根据探索控制器速度和加速度的要求,由STM32F407生成两轴直流电机三闭环伺服系统的指令给定值并传输给FPGA,FPGA再结合光电编码器以及电流传感器Cl、C2的反馈生成驱动直流电机运动的PWM波,PWM波通过驱动桥放大后推动微微鼠向前运动。在微微鼠向前运动过程中,传感器S2和S5会对左右的挡墙进行判断,并记录储存当前迷宫挡墙信息,微微鼠根据左右挡墙的迷宫信息确定其进入单墙模式、双墙导航模式或者是惯性导航模式。陀螺仪实时记录微微鼠的瞬时加速度、速度和位置信息。当微微鼠快速探索脱离了设定中心位置时,在新的采样周期内,STM32F407根据当前S2、S5的状态重新生成三闭环伺服系统的指令给定值传输给FPGA,FPGA再结合光电编码器以及电流传感器Cl、C2的反馈微调电机的PWM波输入,两轴伺服系统开始进行实时补偿来调整微微鼠的姿态,使其重新回到设定中心位置。当微微鼠在陀螺仪的控制下运动一格距离到达新地址时,微处理器将
当前第3页1 2 3 4 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1