CN108015742B - 一种基于3d霍尔遥感的远程交互机械臂系统及实现方法 - Google Patents

Abstract

本发明提出一种基于3D霍尔遥感的远程交互机械臂系统，包括主控端和受控端；所述主控端包括霍尔遥感机构、遥感控制模块、第一无线通信模块；所述受控端包括第二无线通信模块、机械臂和机械臂控制机构；所述霍尔遥感机构包括遥感结构件、3D磁力计和磁体；所述3D磁力计与遥感结构件相邻并与遥感控制模块相连；所述磁体固定于遥感结构件的遥感点位置；所述受控端的机械臂处设有与遥感控制模块连接的舵机；当遥感结构件的遥感点位置发生变化时，遥感控制模块把遥感点的位置变化数据发送至机械臂控制机构，使机械臂执行与遥感点运动相匹配的动作；本发明在克服传统遥感各项缺陷的同时，为机械臂远程控制提供更丰富的人机交互解决方案。

Description

一种基于3D霍尔遥感的远程交互机械臂系统及实现方法

技术领域

本发明涉及机械控制领域，尤其是一种基于3D霍尔遥感的远程交互机械臂系统及实现方法。

背景技术

目前应用机械臂控制工程中的遥感设备主要有开关式和电位器式，另有部分应用选用传感器为操作核心。开关式遥感用四个方向上的开关感测遥感的动作，操作模式以及输出模式单一，无法适应机械臂复杂的操作需求。

电位器遥感原理类同滑阻，即两个滑阻分别置于X和Y轴向，移动遥感即改变探针位置，产生相应电压变量输出。该结构由机械部件直接接触阻体，具有功耗高，滑动噪声大，耐潮湿性能弱等缺点，并且随着遥感工作时间增加，电刷和阻体间易产生磨损，造成阻值偏移且降低工作寿命。

部分遥感设备选用2D角度传感器或位移传感器设计，其输出通常为模拟信号，在输入控制系统前需通过外部模数转换电路，精度受到影响并且增大了电路的面积和设计的复杂性，不利于高集成化的设计应用。同时，此类2D器件无法侦测垂直方向上的遥感操作，不利于后续的机械臂功能拓展开发。

发明内容

本发明提出一种基于3D霍尔遥感的远程交互机械臂系统及实现方法，在克服传统遥感各项缺陷的同时，为机械臂远程控制提供更丰富的人机交互解决方案。

本发明采用以下技术方案。

一种基于3D霍尔遥感的远程交互机械臂系统，所述机械臂系统包括主控端和受控端；所述主控端包括霍尔遥感机构、遥感控制模块、第一无线通信模块；所述受控端包括第二无线通信模块、机械臂和机械臂控制机构；所述第一无线通信模块与第二无线通信模块以无线方式连接；所述霍尔遥感机构包括遥感结构件、3D磁力计和磁体；所述3D磁力计与遥感结构件相邻并与遥感控制模块相连；所述磁体固定于遥感结构件的遥感点位置；所述受控端的机械臂处设有与遥感控制模块连接的舵机；当遥感结构件的遥感点位置发生变化时，遥感控制模块通过3D磁力计测量遥感点的空间位置变化，并把遥感点的位置变化数据发送至机械臂控制机构，使机械臂在舵机驱动下执行与遥感点运动相匹配的动作。

所述遥感控制模块把遥感点的位置变化数据分解为X、Y、Z轴方向上的变化量后，把变化量发送至机械臂控制机构。

所述磁体为径向均匀充磁的NS极均分磁体；所述3D磁力计为能在X、Y、Z轴方向进行霍尔遥感测量的磁力计；所述3D磁力计位于X、Y、Z轴的坐标系原点处；X、Y、Z轴为3D磁力计在霍尔遥感时的三维坐标。

所述机械臂处的舵机设于机械臂的机械关节处，当机械臂控制模块接收到遥感控制模块发来的X、Y、Z轴方向上的变化量后，机械臂控制模块控制舵机驱动机械臂关节，使机械臂按X、Y、Z轴变化量进行动作。

所述机械臂末端设有机械爪；机械臂处设有四个舵机；三个舵机位于机械关节处，一个舵机位于机械爪处。

遥感控制模块内置DATAZ阈值范围，所述DATAZ阈值范围为磁体在3D磁力计上方时的操作阈值；在DATAZ超出阈值时，机械臂执行舵机转角锁定作业或机械爪抓取作业。

所述遥感结构件包括转台、遥感杆；所述遥感杆的末端固定磁体；遥感杆可滑动地穿置于转台处；当遥感杆在转台处向下滑移使磁体的DATAZ超出阈值时，机械臂舵机使机械关节的转角被锁定，机械爪执行抓取作业。

所述的机械臂系统，其远程交互的实现方法包括以下步骤；

A1、使遥感结构件上磁体体置处于初始位置，使受控端的机械臂上的机械爪位置处于初始位置，使第一无线通信模块和第二无线通信模块同步以达到数据收发速率及频段一致；

A2、驱动遥感结构件使磁体相对3D磁力计运动；

A3、以3D磁力计所在位置为坐标原点，3D磁力计对磁体所在位置进行探测，并把磁体所在位置的X轴坐标换算为三维分量数据DATAX，把磁体所在位置的Y轴坐标换算为三维分量数据DATAY，把磁体所在位置的Z轴坐标换算为三维分量数据DATAZ；并按预设频率探测磁体所在位置；

A4、遥感控制模块通过第一无线通信模块把磁体所在位置发送给受控端；受控端按接收到的三维分量数据驱动机械臂；使机械爪按三维分量数据进行移动；

在步骤A4中，三维分量数据DATAX、DATAY、DATAZ用于四个舵机的转角控制；当DATAZ超出阈值范围时，机械臂执行舵机转角锁定或机械爪抓取。

当机械爪执行抓取作业时，按以下步骤进行，

B1、转动、滑移遥感杆，使机械臂同步移动至待抓取目标处；

B2、遥感杆向下滑移使磁体坐标分量的DATAZ超出阈值范围，机械臂舵机锁定机械关节的转角，机械爪执行抓取作业并获取目标物；

B3、遥感杆上移使磁体坐标分量的DATAZ回到阈值范围内，然后遥感杆再次下移使磁体坐标分量的DATAZ超出阈值范围，由于DATAZ再次超出阈值范围且机械爪已完成抓取作业，舵机解锁机械关节；

B4、遥感杆再次上移使磁体坐标分量的DATAZ回到阈值范围内，机械臂再次随遥感杆的移动而运动；把抓取的目标物移至存放地。

本发明采用了新式的霍尔遥感技术，相比于传感遥感功耗更低，体积更小，电路设计简易，侦测模块无机械磨损，使用寿命长不易受环境影响。其可实时输出三维空间内的16位X，Y，Z的磁场强度分量数据，为后端机械臂系统提供更精准，更多样的控制模式。本发明可良好适用于多种需精确控制的领域，如工业流水线，飞控系统等。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

附图1是本发明的系统流程示意图；

附图2是本发明主控端的遥感结构件、3D磁力计和磁体的示意图；

附图3是磁体位移时的矢量改变示意图；

附图4是磁体位移时的矢量变化与3D磁力计位置关系示意图；

附图5是3D磁力计及其外部电路原理图；

附图6是3D磁力计与遥感控制模块的通信程序流程示意图；

附图7是无线通信模块的电路示意图；

附图8是无线通信模块的另一电路示意图；

附图9是本发明的受控端的机械臂结构示意图；

图中：1-遥感杆；2-转台；3-磁体；4-3D磁力计；5-机械关节；6-机械手。

具体实施方式

如图1-9所示，一种基于3D霍尔遥感的远程交互机械臂系统，所述机械臂系统包括主控端和受控端；所述主控端包括霍尔遥感机构、遥感控制模块、第一无线通信模块；所述受控端包括第二无线通信模块、机械臂和机械臂控制机构；所述第一无线通信模块与第二无线通信模块以无线方式连接；所述霍尔遥感机构包括遥感结构件、3D磁力计4和磁体3；所述3D磁力计与遥感结构件相邻并与遥感控制模块相连；所述磁体3固定于遥感结构件的遥感点位置；所述受控端的机械臂处设有与遥感控制模块连接的舵机；当遥感结构件的遥感点位置发生变化时，遥感控制模块通过3D磁力计测量遥感点的空间位置变化，并把遥感点的位置变化数据发送至机械臂控制机构，使机械臂在舵机驱动下执行与遥感点运动相匹配的动作。

所述遥感控制模块把遥感点的位置变化数据分解为X、Y、Z轴方向上的变化量后，把变化量发送至机械臂控制机构。

所述磁体3为径向均匀充磁的NS极均分磁体；所述3D磁力计为能在X、Y、Z轴方向进行霍尔遥感测量的磁力计；所述3D磁力计位于X、Y、Z轴的坐标系原点处；X、Y、Z轴为3D磁力计在霍尔遥感时的三维坐标。

所述机械臂处的舵机设于机械臂的机械关节5处，当机械臂控制模块接收到遥感控制模块发来的X、Y、Z轴方向上的变化量后，机械臂控制模块控制舵机驱动机械臂关节5，使机械臂按X、Y、Z轴变化量进行动作。

所述机械臂末端设有机械爪6；机械臂处设有四个舵机；三个舵机位于机械关节处5，一个舵机位于机械爪处。

遥感控制模块内置DATAZ阈值范围，所述DATAZ阈值范围为磁体在3D磁力计上方时的操作阈值；在DATAZ超出阈值时，机械臂执行舵机转角锁定作业或机械爪抓取作业。

所述遥感结构件包括转台2、遥感杆1；所述遥感杆1的末端固定磁体3；遥感杆1可滑动地穿置于转台2处；当遥感杆在转台处向下滑移使磁体的DATAZ超出阈值时，机械臂舵机使机械关节的转角被锁定，机械爪执行抓取作业。

所述的机械臂系统，其远程交互的实现方法包括以下步骤；

A1、使遥感结构件上磁体体置处于初始位置，使受控端的机械臂上的机械爪位置处于初始位置，使第一无线通信模块和第二无线通信模块同步以达到数据收发速率及频段一致；

A2、驱动遥感结构件使磁体相对3D磁力计运动；

A3、以3D磁力计所在位置为坐标原点，3D磁力计对磁体所在位置进行探测，并把磁体所在位置的X轴坐标换算为三维分量数据DATAX，把磁体所在位置的Y轴坐标换算为三维分量数据DATAY，把磁体所在位置的Z轴坐标换算为三维分量数据DATAZ；并按预设频率探测磁体所在位置；

A4、遥感控制模块通过第一无线通信模块把磁体所在位置发送给受控端；受控端按接收到的三维分量数据驱动机械臂；使机械爪按三维分量数据进行移动；

在步骤A4中，三维分量数据DATAX、DATAY、DATAZ用于四个舵机的转角控制；当DATAZ超出阈值范围时，机械臂执行舵机转角锁定或机械爪抓取。

当机械爪执行抓取作业时，按以下步骤进行，

B1、转动、滑移遥感杆，使机械臂同步移动至待抓取目标处；

B2、遥感杆向下滑移使磁体坐标分量的DATAZ超出阈值范围，机械臂舵机锁定机械关节的转角，机械爪执行抓取作业并获取目标物；

B3、遥感杆上移使磁体坐标分量的DATAZ回到阈值范围内，然后遥感杆再次下移使磁体坐标分量的DATAZ超出阈值范围，由于DATAZ再次超出阈值范围且机械爪已完成抓取作业，舵机解锁机械关节；

B4、遥感杆再次上移使磁体坐标分量的DATAZ回到阈值范围内，机械臂再次随遥感杆的移动而运动；把抓取的目标物移至存放地。

实施例1：

受控端的机械臂采用亚克力材料制作结构件，以舵机驱动机械关节进而实现机械臂的各项动作。

主控端选用3D微功耗磁力计MLX90393为核心设计一款新型霍尔遥感，该MLX90393芯片可侦测遥感操作时的三维磁场强度分量，测量数据采用I2C通信方式输出数字信号，分辨率高达16位，测量精度高且无需复杂的外部电路；测磁灵敏度可通过芯片寄存器设置，对磁体和结构件具有更好的适应性。芯片可切换测量模式和待机模式，测量模式下电流为2mA，待机模式下仅为2.4μA，极大的降低了功耗。芯片选用3×3mm的QFN封装，体积小，利于集成化设计。并且磁体不与磁力计直接接触，无机械磨损，不易受环境影响，使用寿命长。

受控端包括遥感结构件，3D微功耗磁力计MLX90393和磁体；霍尔遥感未操作时，磁体与MLX90393同轴放置，以保证X和Y分量数据的正负数据区间一致。磁体选用径向充磁且充磁应尽量均匀，即N和S极均分磁体，以保证磁场均匀对称。调整磁体与MLX90393距离，使X和Y分量正方向最大数据不高于0x8000,负方向最小数据不低于0x8000，以保证正负数据区间无交集。

结构件与磁体的安装如图2所示，遥感可实现360度全角度摆动，本次设计中任意摆动方向上的极限偏移角度，即ϴ最大为25度。摆动遥感即导致磁体会发生相应的位移，则磁矢量改变，其在三轴方向上的磁场分量即会发生变化，磁矢量分解如图3所示，且满足公式：

。

霍尔遥感的核心为3D微功耗磁力计MLX90393,基于Triaxis技术设计，能够精确侦测X，Y，Z轴上的磁场强度数据，分别转换为16位二进制数据存储。通过路径的线性位移，角度检测和3D定位方面的应用，可完成多种形式的人机界面（HMI）设计。该芯片具有三种测量模式：突发模式（Burst mode）,连续单次测量模式（Single Measurement mode）和变化唤醒模式（Wake Up on Change mode），可通过上位机程序下达指令以设定测量模式。本设计选用连续单次测量模式，则指令字节CMD byte=0011_1111。该指令被MLX90393接收的同时，即进行一次测量，完成测量后数据存储于MLX90393的寄存器中。

本发明中，安放好磁体后其生成的磁矢量B与MLX90393位置关系示意图如图4所示，MLX90393分别侦测得到三个磁场分量，B_X，B_Y和B_Z，并将其分别转换为16位二进制数据存储，待收到遥感控制模块上位机的数据读取命令（Read Measurement）后，向上位机发送数据。磁力计极其外部电路原理图如图5所示。

遥感控制模块由51单片机及其最小系统构成。51单片机完成与MLX90393的I2C通信，回收三维磁场分量数据以实现对遥感动作的定位。通过设置MLX90393的端口CS=1，确定通信方式为I2C；设置器件地址端口A1=0,A0=0；硬件地址确定为I2C_ADDR[6:0]=0001_100。51单片机与MLX90393通信程序流程图如图6所示。其中，各项子程序功能设计如下：

（1）Start_I2c：由51单片机开启I2C通信。

（2）Stop_I2c：由51单片机结束I2C通信。

（3）SendByte：51单片机发送一字节数据，由高位到低位逐位发送。

（4）RcvByte：51单片机发送足够的时钟脉冲以读取一字节数据。

（5）NoAck_I2c：非应答位，由51单片机在完成接收全部数据时发送。

（6）Ack：当51单片机拉高SCL端时，由MLX90393拉高SDA，则表明上一字节被成功接收，反之，接收失败。

完成上述通信，51单片机即可获取三维磁场分量数据。本发明选用径向磁体，表面磁场强度1500高斯，调整图4中磁体与芯片间距4mm。以MLX90393为原点构建坐标系，经系统验证，本发明的遥感定位设计原理如下：

（1）0x0000≤DATAX≤0x7000,遥感处于X轴正方向，坐标X=DATAX。

（2）0x9000≤DATAX≤0xFFFF,遥感处于X轴负方向，坐标X=-(0xFFFF-DATAX)。

（3）0x0000≤DATAY≤0x7000,遥感处于Y轴正方向，坐标Y=DATAY。

（4）0x9000≤DATAY≤0xFFFF,遥感处于Y轴负方向，坐标Y=-(0xFFFF-DATAY)。

（5）结构件约束遥感操作仅可使磁体处于芯片上方，则0x6500≤DATAZ≤0x7500,坐标Z=DATAZ。

其中，DATAX,DATAY,DATAZ为磁力计MLX90393发送至51单片机的16位三维磁场分量数据。X,Y,Z为霍尔遥感的三维坐标。

实施例2：

实施例1中的第一无线通信模块、第二无线通信模块以TI公司的CC2530和CC2591为核心构成。CC2530是应用于2.4G IEEE802.15.4的片上系统（SoC）解决方案，以低功耗8051微控制器为内核。采用6×6mm的QFN封装，体积小且外部电路简易，可适应多种应用的2.4G通信RF收发器，典型应用电路如图7所示。CC2591作为RF前端功放，增大信号的输出功率，提升信号发射距离。同时利用其中的低噪放大器，增强该模块信号接收的灵敏度，降低误码率。其采用4×4mm的QFN封装，体积小，利于集成。CC2591与CC2530的电路连接如图8所示。

本发明需二个无线通信模块，分别作为通信A端和通信B端以实现半双工通信。通信A端与遥感控制模块连接，由51单片机开启定时器T1作为波特率发生器，晶振11.0592MHZ，波特率调整为9600，利用串口将三维磁场数据发送至CC2530，经由CC2591和天线，将数据无线发出。通信B端与机械臂控制模块连接，将三维磁场数据通过串口发送至52单片机。52单片机同样需要开启定时器T1,选用晶振11.0592MHZ，波特率9600，以实现数据收发速率一致，避免误码现象。

实施例3：

实施例2中，受控端的机械臂控制机构内的机械臂控制模块由52单片机及其最小系统构成。由于该模块需完成通过无线模块接收三维磁场强度数据，进行数据处理并输出用于驱动机械臂舵机的信号，则选用具有三个定时器的52单片机（T1作波特率发生器，T0,T2输出舵机控制信号）。舵机内部基准源生成周期20ms，脉宽1.5ms的方波信号，将该信号的有效电压与输入信号的有效电压比较生成压差，由压差决定电机正反转及转角大小。输入信号的脉宽应为0.5ms至2.5ms，对应0度至180度。根据上述舵机原理和遥感原理，设计X数据和Y数据分别控制机械臂上的二个舵机，以X数据控制一个舵机为例，设计原理如下：

初始化定时子程序，给变量赋初始值tempX=1500，单步计时周期约为1us，即生成1.5ms脉宽的方波信号，使舵机处于中间（90度）位置。根据上述霍尔遥感的X分量数据区间可知，机械臂在X轴方向上的转角分辨率A满足：

则：

则52单片机生成的方波脉宽满足tempX us即tempX/1000 ms。舵机将根据霍尔遥感实时输出的X分量数据，转动相应的转角。Y分量同理。

按压下霍尔遥感，则Z分量数据偏离原操作区间0x6500≤DATAZ≤0x7500。52单片机设定阈值，侦测到Z分量数据偏离上述区间则锁定舵机控制信号，即锁定两个舵机转角。再次侦测到Z分量数据偏离，则解开锁定。

机械臂结构如图9所示，包含四个舵机，分别处于四个机械关节处。采用本发明中两组霍尔遥感，即可完成四个舵机的转角精确控制。

当磁体移至磁体坐标超出Z分量阈值范围时，Z分量数据用于完成舵机转角锁定和机械爪抓取功能。

Claims (3)

1.一种基于3D霍尔遥感的远程交互机械臂系统，其特征在于：所述机械臂系统包括主控端和受控端；所述主控端包括霍尔遥感机构、遥感控制模块、第一无线通信模块；所述受控端包括第二无线通信模块、机械臂和机械臂控制机构；所述第一无线通信模块与第二无线通信模块以无线方式连接；所述霍尔遥感机构包括遥感结构件、3D磁力计和磁体；所述3D磁力计与遥感结构件相邻并与遥感控制模块相连；所述磁体固定于遥感结构件的遥感点位置；所述受控端的机械臂处设有与遥感控制模块连接的舵机；当遥感结构件的遥感点位置发生变化时，遥感控制模块通过3D磁力计测量遥感点的空间位置变化，并把遥感点的位置变化数据发送至机械臂控制机构，使机械臂在舵机驱动下执行与遥感点运动相匹配的动作；

所述遥感控制模块把遥感点的位置变化数据分解为X、Y、Z轴方向上的变化量后，把变化量发送至机械臂控制机构；

所述磁体为径向均匀充磁的NS极均分磁体；所述3D磁力计为能在X、Y、Z轴方向进行霍尔遥感测量的磁力计；所述3D磁力计位于X、Y、Z轴的坐标系原点处；X、Y、Z轴为3D磁力计在霍尔遥感时的三维坐标；

所述机械臂处的舵机设于机械臂的机械关节处，当机械臂控制模块接收到遥感控制模块发来的X、Y、Z轴方向上的变化量后，机械臂控制模块控制舵机驱动机械臂关节，使机械臂按X、Y、Z轴变化量进行动作；

所述机械臂末端设有机械爪；机械臂处设有四个舵机；三个舵机位于机械关节处，一个舵机位于机械爪处；

遥感控制模块内置DATAZ阈值范围，所述DATAZ阈值范围为磁体在3D磁力计上方时的操作阈值；在DATAZ超出阈值时，机械臂执行舵机转角锁定作业或机械爪抓取作业；

所述遥感结构件包括转台、遥感杆；所述遥感杆的末端固定磁体；遥感杆可滑动地穿置于转台处；当遥感杆在转台处向下滑移使磁体的DATAZ超出阈值时，机械臂舵机使机械关节的转角被锁定，机械爪执行抓取作业；

一种基于3D霍尔遥感的机械臂远程交互实现方法，以上所述的机械臂系统，其远程交互的实现方法包括以下步骤；

A1、使遥感结构件上磁体置处于初始位置，使受控端的机械臂上的机械爪位置处于初始位置，使第一无线通信模块和第二无线通信模块同步以达到数据收发速率及频段一致；

A2、驱动遥感结构件使磁体相对3D磁力计运动；

A3、以3D磁力计所在位置为坐标原点，3D磁力计对磁体所在位置进行探测，并把磁体所在位置的X轴坐标换算为三维分量数据DATAX，把磁体所在位置的Y轴坐标换算为三维分量数据DATAY，把磁体所在位置的Z轴坐标换算为三维分量数据DATAZ；并按预设频率探测磁体所在位置；

A4、遥感控制模块通过第一无线通信模块把磁体所在位置发送给受控端；受控端按接收到的三维分量数据驱动机械臂；使机械爪按三维分量数据进行移动。

2.根据权利要求1所述的一种基于3D霍尔遥感的远程交互机械臂系统，其特征在于：在步骤A4中，三维分量数据DATAX、DATAY、DATAZ用于四个舵机的转角控制；当DATAZ超出阈值范围时，机械臂执行舵机转角锁定或机械爪抓取。

3.根据权利要求2所述的一种基于3D霍尔遥感的远程交互机械臂系统，其特征在于：当机械爪执行抓取作业时，按以下步骤进行，

B1、转动、滑移遥感杆，使机械臂同步移动至待抓取目标处；

B2、遥感杆向下滑移使磁体坐标分量的DATAZ超出阈值范围，机械臂舵机锁定机械关节的转角，机械爪执行抓取作业并获取目标物；

B3、遥感杆上移使磁体坐标分量的DATAZ回到阈值范围内，然后遥感杆再次下移使磁体坐标分量的DATAZ超出阈值范围，由于DATAZ再次超出阈值范围且机械爪已完成抓取作业，舵机解锁机械关节；

B4、遥感杆再次上移使磁体坐标分量的DATAZ回到阈值范围内，机械臂再次随遥感杆的移动而运动；把抓取的目标物移至存放地。

Images