CN108917589B - 一种机械手关节角度测量系统、平台及测量方法 - Google Patents

一种机械手关节角度测量系统、平台及测量方法 Download PDF

Info

Publication number
CN108917589B
CN108917589B CN201810791427.6A CN201810791427A CN108917589B CN 108917589 B CN108917589 B CN 108917589B CN 201810791427 A CN201810791427 A CN 201810791427A CN 108917589 B CN108917589 B CN 108917589B
Authority
CN
China
Prior art keywords
magnetic field
measuring
data
angle
sensor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810791427.6A
Other languages
English (en)
Other versions
CN108917589A (zh
Inventor
苟铭浩
黄宇秋
王晰
张沛晗
盛鑫军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN201810791427.6A priority Critical patent/CN108917589B/zh
Publication of CN108917589A publication Critical patent/CN108917589A/zh
Application granted granted Critical
Publication of CN108917589B publication Critical patent/CN108917589B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B7/00—Measuring arrangements characterised by the use of electric or magnetic techniques
    • G01B7/30—Measuring arrangements characterised by the use of electric or magnetic techniques for measuring angles or tapers; for testing the alignment of axes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
    • G01B21/22—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring angles or tapers; for testing the alignment of axes

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manipulator (AREA)
  • Measurement Of Length, Angles, Or The Like Using Electric Or Magnetic Means (AREA)

Abstract

本发明公开了一种机械手关节角度测量系统、平台及测量方法，涉及角度传感领域，测量系统包括多个固连于被测物体上的测量单元，每个测量单元包含一块加速度‑角速度‑磁场传感器和若干贴片电阻、滤波电路、印刷电路板以及FPC排线组件，上述器件载于印刷电路板上，通过FPC排线组件供电并与外部通信；本发明根据测量单元读取磁场与重力场数据，通过数据拟合、校准、计算得出两个测量单元间的角度；本发明测量系统可安装在被测关节绝大多数位置，避免传统角度传感装置安装时与电机、轴承等装置相互干涉问题，降低传感系统的复杂性和机械设计的难度，使机械手结构简单，可靠性更好，降低测量成本，减小测量系统的体积。

Description

一种机械手关节角度测量系统、平台及测量方法
技术领域
本发明涉及角度传感领域，尤其涉及一种机械手关节角度测量系统、平台及测量方法。
背景技术
目前，在对机械手的控制过程中，对手指角度的传感是实现控制的基础。目前存在若干以声、光、电、磁等为原理的角度传感装置，但大多价格高昂，而且装置复杂、体积较大，不利于机械设计以及装配，为设计带来很大难度，同时也让设备变得冗杂。
惯性传感器是一种用来测量系统加速度、角速度的传感器，一般由加速度传感器和陀螺仪构成。其中，加速度传感器测量得到的加速度由重力加速度与物体的绝对加速度构成。将加速度传感器固连在被测量系统上，在静止的情况下，可以利用加速度传感器测量得到重力在传感器各轴方向上的分量。一个系统的空间取向由方位角，横滚角以及俯仰角构成仅仅使用重力传感器可以得到系统的俯仰角和横滚角，但是不能够得到方位角信息，因此不能得到系统的全部空间取向信息。磁场传感器是基于霍尔效应等电磁感应效应的测量传感器所在位置磁场的传感器。在地球表面的绝大多数地方，存在着稳定的地磁场，其强度约为0.5-0.6高斯。在一个很小的局部，地磁场可以被认为为匀强磁场，因此其和重力场相同，也可以作为系统空间取向的一个绝对参考。通过测量重力场和磁场信息，可以计算得到系统的全部空间取向信息。
对于机械手的关节，原则上，通过测量关节两杆连体坐标系下的重力场和磁场信息，能够计算两杆的全部空间取向，进而计算任意两者之间的相对角度，但计算过程复杂，需要耗费较长的时间，若考虑到传感器采集的原始数据存在误差，需要对数据进行预处理，耗费的时间将更长。
因此，本领域的技术人员致力于开发一种机械手关节角度测量系统、平台及测量方法，该系统为基于加速度-角速度-磁场传感器的机械手关节角度测量系统，该平台为应用上述加速度-角速度-磁场传感器的机械手关节角度测量系统的机械手平台。本发明测量系统十分便于小型化，可以安装在被测量关节上的绝大多数位置，十分灵活，避免了传统的角度传感装置(如编码器)安装在关节处时与电机、轴承等装置相干涉的问题，利用本发明测量系统的测量平台可大大降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，可靠性更好。本发明还提供了一种利用校准后的重力场和磁场信息，不解算每杆的位型而直接计算机械手关节两杆间角度的方法。与传统方法相比，本发明大大降低了测量成本，显著减小了测量系统的体积，提高了解算速度，缩短了数据处理时间。
发明内容
有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是在机械手关节角度测量中如何让读取数据的测量系统小型化，从而降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，减小成本，方便安装，以及如何更加简便地解算所读取的数据从而使得提高解算速度，解算耗时缩短。
为实现上述目的，本发明提供了一种机械手关节角度测量系统、平台及测量方法，本发明测量系统是基于加速度-角速度-磁场传感器的机械手关节角度测量系统，该平台为应用上述加速度-角速度-磁场传感器的机械手关节角度测量系统的机械手平台。
本发明测量系统包括多个测量单元，所述测量单元固连于被测机械手关节的相关联杆件物体上，优选的，被测机械手关节的相关联两个杆件物体上，固连所述测量单元的个数为2个；每个所述测量单元包含一块加速度-角速度-磁场传感器和若干贴片电阻、滤波电路、印刷电路板以及FPC排线组件，所述传感器、所述电阻、所述滤波电路以及所述FPC排线组件载于所述印刷电路板上，优选的，所述印刷电路板整体尺寸为9×22×3mm，所述FPC排线组件包括FPC插座和FPC排线，优选的，所述加速度-角速度-磁场传感器为InvenSenseMPU9250九轴加速度-角速度-磁场传感器；所述印刷电路板通过FPC排线组件供电并与外部通信；所述FPC排线的接口包括电源正极、电源负极和I2C通信接口的SDA、SCL线，优选的，所述I2C通信接口为Philips I2C通信接口；所述电阻包括上拉电阻和下拉电阻，所述FPC排线的SDA、SCL通过所述印刷电路板分别与所述传感器对应引脚相连，所述传感器的SDA、SCL、nCS引脚分别通过所述上拉电阻与所述电源正极相连，所述传感器的SDO、FSYNC引脚通过所述下拉电阻与所述电源负极相连，优选的，所述上拉电阻和下拉电阻的电阻值为10kΩ，所述滤波电路连接在所述电源正极与所述电源负极之间。
进一步地，所述传感器在所述I2C通信接口总线上始终处于被选择状态，通过改变SCL和SDA的电平实现与其通信。
进一步地，所述测量系统的最高采样频率为100Hz。
进一步地，所述滤波电路包括若干个0.1μF、0.01μF电容。
本发明机械手平台，包括驱动电路板、机械手组件；所述机械手组件包括手指组件和手掌组件；所述驱动电路板包括测量单元、单片机和电机驱动芯片，优选的，所述单片机为STM32F103ZET6、所述电机驱动芯片为L298P，所述测量单元有多个，分别固连于所述手指组件的指尖和所述手掌组件上，所述驱动电路板通过所述FPC排线连接所述机械手组件上的测量单元。
更进一步地，所述手指组件包括电机、传动机构；所述传动机构包括四杆机构、D形轴、主动齿轮、从动齿轮、涡轮、蜗杆，所述四杆机构包括主动杆、固定杆和2条从动杆，所述电机固定在所述D形轴上，通过所述D形轴带动所述主动齿轮，所述主动齿轮带动所述从动齿轮转动，所述从动齿轮通过所述D形轴带动所述蜗杆转动，所述蜗杆带动所述涡轮转动所述蜗轮与所述四杆机构中的所述主动杆固连，通过上述传动机构实现对机械手指的传动，由于蜗轮蜗杆的自锁，当电机断电后，确保所述机械手指不会发生转动。
本发明的测量系统及平台的技术效果是：十分便于小型化，可以安装在被测量关节上的绝大多数位置，十分灵活，避免了传统的角度传感装置(如编码器)安装在关节处时与电机、轴承等装置相干涉的问题，可大大降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，可靠性更好，减小了测量系统的体积、降低了成本、安装更加便利。
本发明的测量方法包括：
步骤1、测量过程前，建立坐标系：采用绝对坐标系记录，即在空间中任意一点建立绝对坐标系OXYZ，其中Z轴方向为重力加速度的反方向，X轴的方向为磁场在水平方向的投影方向，其中重力与磁场方向的夹角为α，此坐标系方向完全由重力场方向与磁场方向决定，与测量单元的空间取向无关；
步骤2、进行椭球模型拟合数据：在测量过程开始前，对每个传感器测得的重力场、磁场分布进行椭球拟合，以拟合的椭球模型为基准进行后续的测量过程；
步骤3、测量过程中，当需对某状态下机械手关节角度进行测量时，以测量两个测量单元的角度为例，根据所述测量单元的几何关系，建立固连在所述两个测量单元上的坐标系O1X1Y1Z1，O2X2Y2Z2；
步骤4、得到原始数据：通过当前状态下测量单元的加速度-角速度-磁场传感器分别测量重力场和磁场数据的原始数据；
步骤5、滤波原始数据：对重力场和磁场数据的原始数据使用截止频率为10Hz的低通滤波器进行滤波，得到重力场与磁场的基础数据；
步骤6、椭球模型校准数据：根据步骤2得到的零点和灵敏度偏差，对步骤5得到的重力场与磁场的基础数据进行反向修正，得到加速度和磁场的真实数据；
步骤7、利用空间几何关系以及右手坐标系计算得出所述两个测量单元间的角度；
步骤8、重复步骤3至步骤7可以测量得出不同状态下的测量单元的角度。
进一步地，所述步骤2具体为，在测量过程开始前，对每个传感器测得的重力场、磁场分布进行椭球拟合，在匀强磁场(地磁场)中不停地旋转所述加速度-角速度-磁场传感器，使所述加速度-角速度-磁场传感器尽可能到达各种取向，读取多组重力场和磁场数据的原始数据，然后对多组重力场和磁场数据的原始数据使用截止频率为10Hz的低通滤波器进行滤波，得到多组重力场与磁场的基础数据，对这些数据利用椭球拟合得到椭球方程，以磁场数据为例：
Figure BDA0001735004950000041
其中Bx、By、Bz为所述测量单元滤波后的所述基础数据，Bx0、By0、Bz0为球心在三轴上的坐标，a、b、c分别为椭球的三根半轴长度，重力场数据拟合同样采用上述方法进行椭球拟合。
更进一步地，所述步骤6具体为，以所述步骤2获得的拟合椭球方程作为本发明角度测量系统的校准模型，在测量过程中利用此模型对所述步骤5得出的重力场与磁场的基础数据进行校准得到真实数据，校准表达式如下所示：
Figure BDA0001735004950000042
其中Bx’、By’、Bz’为校准后磁场的真实数据，重力场数据同样采用上述方法进行校准。
更进一步地，所述步骤7还包括：
步骤7.1、利用所述步骤6的校准算法处理后得到的真实数据分别为：
Figure BDA0001735004950000043
Figure BDA0001735004950000044
其中B1、B2为处理后磁场的真实数据，g1、g2为处理后重力场的真实数据；
步骤7.2、每次测量后，首先对以上数据进行归一化，公式如下：
Figure BDA0001735004950000045
步骤7.3、利用下式可以求解出α角：
Figure BDA0001735004950000046
步骤7.4、通过两个所述测量单元可以求解出两个α角，结果取两者平均数即可，同时，从绝对坐标系角度计算，测量值理论上有如下数学形式：
Figure BDA0001735004950000051
Figure BDA0001735004950000052
Figure BDA0001735004950000053
由于测量单元固连坐标系是右手坐标系，zi＝xi×yi(i＝1,2)，因此有：
Py(zi)＝Pz(xi)Px(yi)-Px(xi)Pz(yi)，(i＝1，2)
步骤7.5、计算得出所述两个测量单元z轴之间的夹角θ为：
θ＝cos<z1,z2>＝z1·z2＝Px(z1)Px(z2)+Py(z1)Py(z2)+Pz(z1)Pz(z2)
本发明的测量方法的技术效果是：利用校准结果对原始数据进行反向修正，得到真实的重力场和磁场数据，与传统的加速度-角速度-磁场传感器的角度解算方法中解算每杆的位型不同，直接通过空间几何关系以及右手坐标系计算机械手关节两杆间角度，通过该种简单、快速的算法，能够求解出两个(或多个)测量单元任意两轴之间的夹角，从而获得各物体间的相对角度，提高了解算速度，缩短了数据处理时间。
以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。
附图说明
图1是本发明机械手关节角度测量系统一个较佳实施例的工作原理示意图；
图2是本发明机械手关节角度测量系统一个较佳实施例的单个测量单元的结构示意图；
图3是本发明机械手关节角度测量系统一个较佳实施例的单个测量单元的电路图；
图4是本发明机械手关节角度测量方法一个较佳实施例的的工作流程示意图；
图5是本发明机械手关节角度测量方法一个较佳实施例实际情况下重力场/磁场在传感器坐标系中的偏心椭球分布示意图；
图6是本发明机械手关节角度测量方法一个较佳实施例重力场、磁场在绝对空间坐标系中的取向示意图；
图7是本发明机械手关节角度测量方法一个较佳实施例建立的固连在每一个测量单元上的坐标系示意图；
图8是本发明机械手关节角度测量方法一个较佳实施例每一个测量单元对重力场、磁场在绝对空间坐标系中的取向示意图；
图9是本发明机械手关节角度测量方法一个较佳实施例两个测量单元固连坐标系以及绝对坐标系间的关系示意图；
图10是本发明机械手平台单个手指外形示意图；
图11是本发明机械手平台单个手指结构示意图；
图12是本发明机械手平台整体外形示意图；
图13是本发明机械手关节角度测量系统的测量单元在本发明机械手平台中的安装位置的一个较佳实施例示意图；
图14是本发明机械手平台的驱动电路板正面示意图；
图15是本发明机械手平台的驱动电路板反面示意图。
其中：1-FPC排线，2-FPC插座，3-MPU9250传感器芯片，4-印刷电路板，5-电机，6-固定杆，7-主动杆，8-从动杆1，9-从动杆2，10-主动齿轮，11-从动齿轮，12-蜗杆，13-蜗轮，14-驱动电路板(含测量单元)，15-测量单元，16-STM32F103ZET6单片机，17-MPU9250传感器，18-FPC插座，19-L298P电机驱动芯片，20-滤波电路，21-上拉电阻，22-下拉电阻。
具体实施方式
以下参考说明书附图介绍本发明的一个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。
在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。
图1是本发明机械手关节角度测量系统一个较佳实施例的工作原理示意图。本机械手关节角度测量系统采用2片InvenSense MPU9250九轴加速度-角速度-磁场传感器芯片3完成角度测量工作，工作时将载有传感器芯片3的测量单元15固连于被测物体上，可测量2个物体在相距不远的近地面附近任何位置时的相对角度。
图2是本发明机械手关节角度测量系统一个较佳实施例的单个测量单元15的结构示意图。每个测量单元15包含一块MPU9250传感器3和若干贴片电阻、滤波电路20，通过FPC插座2和FPC排线1供电并与外部通信，以上器件载于印刷电路板4上，印刷电路板4整体尺寸约为9×22×3mm。贴片电阻,包括上拉电阻21和下拉电阻22，FPC排线的接口包括电源正极、电源负极和I2C通信接口的SDA、SCL线。
图3是本发明机械手关节角度测量系统一个较佳实施例的单个测量单元15的电路图。通过印刷电路板4，FPC排线1的SDA、SCL分别与传感器芯片3的对应引脚相连。传感器芯片3的SDA、SCL、nCS引脚分别通过10kΩ上拉电阻21与电源正极相连，SDO、FSYNC引脚通过10kΩ下拉电阻22与电源负极相接，另外，电源正负极之间还通过若干0.1μF、0.01μF电容组成滤波电路20相连以滤波。其他接线如图3所示，不再赘述。
此时，传感器芯片3在I2C通信接口总线上始终处于被选择状态，通过改变SCL和SDA的电平即可实现与其通信，读出重力场与磁场的原始数据。
图4为本发明机械手关节角度测量方法的工作流程示意图。对于两个物体，本测量系统通过固连在其上的测量单元，测量得到重力场与磁场在传感器坐标系中三轴方向的分量，并对原始数据进行滤波和校准。利用处理后的真实数据，本系统通过一种简单、快速的算法，能够求解出两个测量单元任意两轴之间的夹角，从而获得各物体间的相对角度，具体实施如下：
MPU9250传感器测量的重力场与磁场的原始数据存在高频噪声，本装置测量时使用截止频率为10Hz的低通滤波器对传感器测量的数据进行滤波，以减小高频噪声对数据的影响，得到重力场与磁场的基础数据。
当测量单元经历各种可能的空间取向时，理论上重力(或磁场)矢量的终点在测量单元座标系中的坐标分布于一个球心位于原点的球上。但实际上由于加速度传感器、磁场传感器的零点和灵敏度均存在偏差，单个测量单元在不同空间取向上测量得到的重力场与磁场在三维空间中的分布呈现一个如图5所示偏心椭球。因此在进行解算之前，需要对每个传感器进行椭球校准，即预先对传感器在不同取向上测得的重力场和磁场的原始数据进行滤波得到基础数据，对基础数据椭球拟合，得到零点与灵敏度偏差，在测量过程时根据拟合结果对滤波后的基础数据进行反向修正，得到重力场和磁场的真实数据。
在测量过程开始前，对每个传感器测得的重力场、磁场分布进行椭球拟合，在匀强磁场(地磁场)中不停地旋转所述加速度-角速度-磁场传感器，使加速度-角速度-磁场传感器尽可能到达各种取向，读取多组重力场与磁场的原始数据，使用截止频率为10Hz的低通滤波器对传感器测量的数据进行滤波，得到重力场与磁场的基础数据，对这些基础数据利用椭球拟合得到椭球方程，以磁场数据为例：
Figure BDA0001735004950000081
其中Bx、By、Bz为传感器读取经过滤波后的基础数据，Bx0、By0、Bz0为球心在三轴上的坐标，a、b、c分别为椭球的三根半轴长度，此方程即为本系统的校准模型。重力场拟合同样采用上述方法进行椭球拟合，在测量过程中需要利用此模型对滤波后的基础数据数据进行校准得到真实数据，校准表达式如下所示：
Figure BDA0001735004950000082
其中Bx’、By’、Bz’为校准后磁场的真实数据，重力场数据同样采用上述方法进行校准。
本方法利用处理后的数据、空间几何关系以及右手系的定义，解算出机械手关节两杆之间的夹角。以图1所示的两个测量单元的z轴之间的夹角θ为例，具体实现方式如下：
首先在空间中任意一点建立如图6所示的绝对坐标系OXYZ。其中Z轴方向为重力加速度的反方向，X轴的方向为磁场在水平方向的投影方向，其中重力与磁场方向的夹角为α。此坐标系方向完全由重力场方向与磁场方向决定，与测量单元的空间取向无关。此坐标系在磁场垂直于水平面试无法定义，但是在地球极地以外的绝大部分区域磁场均明显不垂直于水平面，因此此定义方式有效。在较小的移动范围内，地磁场与重力场可以认为为匀强场，因此按照此方法定义在较小范围内的绝对坐标系的空间取向相同。
如图7所示，根据测量单元的几何关系，建立固连在两个测量单元上的坐标系O1X1Y1Z1，O2X2Y2Z2。如图8所示，每一个传感器测量得到的重力与磁场的原始数据在其固连坐标系上分量，再利用上述的滤波和校准处理后得到的真实数据分别为：
Figure BDA0001735004950000083
Figure BDA0001735004950000084
其中B1、B2为处理后磁场的真实数据，g1、g2为处理后重力场的真实数据。
每次测量后，首先对以上数据进行归一化：
Figure BDA0001735004950000091
利用下式可以求解出α角：
Figure BDA0001735004950000092
通过两个测量单元可以求解出两个α角，结果取两者平均数即可。同时，从绝对坐标系角度计算，测量值理论上有如下数学形式：
Figure BDA0001735004950000093
其中Pi(j)是向量j在向量i方向上的投影，解算得到
Figure BDA0001735004950000094
Figure BDA0001735004950000095
Figure BDA0001735004950000096
由于测量单元固连坐标系是右手坐标系，zi＝xi×yi(i＝1,2)，因此有：
Py(zi)＝Pz(xi)Px(yi)-Px(xi)Pz(yi),(i＝1,2)
因此可以得到如图9所示的两测量单元z轴之间的夹角为：
θ＝cos<z1,z2>＝z1·z2＝Px(z1)Px(z2)+Py(z1)Py(z2)+Pz(z1)Pz(z2)
下面为本发明机械手关节角度测量系统应用于机械手平台的一个较佳实施例的原理简述，该实施例中测量系统有多个测量单元。
如图10、图11所示，机械手指的动力由电机5提供。电机5通过D形轴带动主动齿轮10，主动齿轮10带动从动齿轮11转动。从动齿轮11通过D形轴带动蜗杆12转动，蜗杆12带动蜗轮13转动。蜗轮13与四杆机构中的7固连。6、7、8、9构成四杆机构，其中6为固定杆，7为主动杆，8、9为从动杆。通过上述传动过程实现了对机械手指的传动。由于蜗轮13及蜗杆12的自锁，当电机5断电后，机械手指不会发生转动。
如图12所示，机械手平台由三个机械手指和一个机械手掌构成。
如图13所示，测量单元分别分布于三个机械手指的指尖和机械手掌上。通过与传感器17读数解算出机械手指上测量单元15相对于机械手掌上测量单元14的角度后，利用四杆机构的几何关系可以求出主动杆7以及从动杆8、9相对于机械手掌的角度。
如图14和图15所示，机械手平台的驱动电路板14由测量单元15、STM32F103ZET6单片机16、L298P电机驱动芯片19构成。MPU9250传感器17提供机械手掌处重力场与磁场信息，STM32F103ZET6单片机16进行数据的处理与信号控制，L298P芯片19实现对电机5的控制，驱动电路板14通过三个FPC插座18用于连接三个机械手指上的测量单元17。
机械手平台工作时，驱动电路板14通过FPC排线1与三个机械手指上的测量单元15连接，单片机16通过4个I2C通信接口采集分别位于机械手掌和三个机械手指上的测量单元15的重力场和磁场数据，利用前文所述的数据校准和角度解算方法，可实现对三个机械手指与机械手掌的相对角度的实时测量。
该基于加速度-角速度-磁场传感器的机械手关节角度测量系统、平台及测量方法能够有效的运用于机械手关节中，测量系统十分便于小型化，可以安装在被测量机械手关节上的绝大多数位置，十分灵活，避免了传统的角度传感装置(如编码器)安装在关节处时与电机、轴承等装置相干涉的问题，可大大降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，可靠性更好，并且本发明提供了一种利用校准后的重力场和磁场信息，不解算每杆的位型而直接计算机械手关节两杆间角度的方法。与传统方法相比，本发明大大降低了测量成本，显著减小了测量系统的体积，提高了解算速度，缩短了数据处理时间。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (8)

1.一种机械手关节角度测量系统，其特征在于，包括多个测量单元，所述测量单元设置于被测机械手关节的相关联杆件物体上；每个所述测量单元包含一块加速度-角速度-磁场传感器和若干贴片电阻、滤波电路、印刷电路板以及FPC排线组件，所述传感器、所述电阻、所述滤波电路以及所述FPC排线组件载于所述印刷电路板上，所述FPC排线组件包括FPC插座和FPC排线，所述FPC排线的接口包括电源正极、电源负极和I2C通信接口的SDA、SCL线，所述印刷电路板通过FPC排线组件供电并与外部通信，所述电阻包括上拉电阻和下拉电阻，所述FPC排线的SDA、SCL通过所述印刷电路板分别与所述传感器对应引脚相连，所述传感器的SDA、SCL、nCS引脚分别通过所述上拉电阻与所述电源正极相连，所述传感器的SDO、FSYNC引脚通过所述下拉电阻与所述电源负极相连，所述滤波电路连接在所述电源正极与所述电源负极之间，所述传感器在所述I2C通信接口总线上始终处于被选择状态，通过改变SCL和SDA的电平实现与其通信，所述测量系统最高采样频率为100Hz。
2.如权利要求1所述的机械手关节角度测量系统，其特征在于，所述上拉电阻和下拉电阻的电阻值为10kΩ，所述滤波电路由若干0.1μF、0.01μF电容组成。
3.一种应用权1～权2中任一项所述测量系统的机械手平台，其特征在于，包括驱动电路板、机械手组件；所述机械手组件包括手指组件和手掌组件；所述驱动电路板包括测量单元、单片机和电机驱动芯片；所述测量单元有多个，分别固连于所述手指组件的指尖和所述手掌组件上，所述驱动电路板通过所述FPC排线连接所述机械手组件上的测量单元。
4.如权利要求3所述的机械手平台，其特征在于，所述手指组件包括电机、传动机构；所述传动机构包括四杆机构、D形轴、主动齿轮、从动齿轮、蜗轮、蜗杆，所述四杆机构包括主动杆、固定杆和2条从动杆，所述电机固定在所述D形轴上，通过所述D形轴带动所述主动齿轮，所述主动齿轮带动所述从动齿轮转动，所述从动齿轮通过所述D形轴带动所述蜗杆转动，所述蜗杆带动所述蜗轮转动，所述蜗轮与所述四杆机构中的所述主动杆固连。
5.一种应用权1～权2中任一项所述测量系统的机械手关节角度测量方法，其特征在于，所述方法包括以下步骤：
步骤1、测量过程前，建立坐标系：采用绝对坐标系记录，即在空间中任意一点建立绝对坐标系OXYZ，其中Z轴方向为重力加速度的反方向，X轴的方向为磁场在水平方向的投影方向，其中重力与磁场方向的夹角为α，此坐标系方向完全由重力场方向与磁场方向决定，与测量单元的空间取向无关；
步骤2、进行椭球模型拟合数据：在测量过程开始前，对每个传感器测得的重力场、磁场分布进行椭球拟合，以拟合的椭球模型为基准进行后续的测量过程；
步骤3、测量过程中，当需对某状态下机械手关节角度进行测量时，以测量两个测量单元的角度为例，根据所述测量单元的几何关系，建立固连在所述两个测量单元上的坐标系O1X1Y1Z1，O2X2Y2Z2；
步骤4、得到原始数据：通过当前状态下测量单元的加速度-角速度-磁场传感器分别测量重力场和磁场数据的原始数据；
步骤5、滤波原始数据：对重力场和磁场数据的原始数据使用截止频率为10Hz的低通滤波器进行滤波，得到重力场与磁场的基础数据；
步骤6、椭球模型校准数据：根据步骤2得到的零点和灵敏度偏差，对步骤5得到的重力场与磁场的基础数据进行反向修正，得到重力场和磁场的真实数据；
步骤7、利用空间几何关系以及右手坐标系计算得出所述两个测量单元间的角度；
步骤8、重复步骤3至步骤7可以测量得出不同状态下的测量单元的角度。
6.如权利要求5所述的机械手关节角度测量方法，其特征在于，所述步骤2具体为，在测量过程开始前，对每个传感器测得的重力场、磁场分布进行椭球拟合，在匀强磁场中不停地旋转所述加速度-角速度-磁场传感器，使所述加速度-角速度-磁场传感器尽可能到达各种取向，读取多组重力场和磁场数据的原始数据，然后对多组重力场和磁场数据的原始数据使用截止频率为10Hz的低通滤波器进行滤波，得到多组重力场与磁场的基础数据，对这些数据利用椭球拟合得到椭球方程，以磁场数据为例：
Figure FDA0002595601510000021
其中Bx、By、Bz为所述测量单元滤波后的所述基础数据，Bx0、By0、Bz0为球心在三轴上的坐标，a、b、c分别为椭球的三根半轴长度，重力场数据拟合同样采用上述方法进行椭球拟合。
7.如权利要求6所述的机械手关节角度测量方法，其特征在于，所述步骤6中，以所述步骤2获得的拟合椭球方程作为本发明角度测量系统的校准模型，在测量过程中利用此模型对所述步骤5得出的重力场与磁场的基础数据进行校准得到真实数据，校准表达式如下所示：
Figure FDA0002595601510000022
其中Bx’、By’、Bz’为校准后磁场的真实数据，重力场数据同样采用上述方法进行校准。
8.如权利要求7所述的机械手关节角度测量方法，其特征在于，所述步骤7还包括：
步骤7.1、利用所述步骤6的校准算法处理后得到的真实数据分别为：
Figure FDA0002595601510000031
Figure FDA0002595601510000032
其中B1、B2为处理后磁场的真实数据，g1、g2为处理后重力场的真实数据；
步骤7.2、每次测量后，首先对以上数据进行归一化，公式如下：
Figure FDA0002595601510000033
步骤7.3、利用下式可以求解出α角：
Figure FDA0002595601510000034
步骤7.4、通过两个所述测量单元可以求解出两个α角，结果取两者平均数即可，同时，从绝对坐标系角度计算，测量值理论上有如下数学形式：
Figure FDA0002595601510000035
Figure FDA0002595601510000036
Figure FDA0002595601510000037
由于测量单元固连坐标系是右手坐标系，zi＝xi×yi(i＝1,2)，因此有：
Py(zi)＝Pz(xi)Px(yi)-Px(xi)Pz(yi)，(i＝1，2)
步骤7.5、计算得出所述两个测量单元z轴之间的夹角θ为：
θ＝cos<z1,z2>＝z1·z2＝Px(z1)Px(z2)+Py(z1)Py(z2)+Pz(z1)Pz(z2)。
CN201810791427.6A 2018-07-18 2018-07-18 一种机械手关节角度测量系统、平台及测量方法 Active CN108917589B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810791427.6A CN108917589B (zh) 2018-07-18 2018-07-18 一种机械手关节角度测量系统、平台及测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810791427.6A CN108917589B (zh) 2018-07-18 2018-07-18 一种机械手关节角度测量系统、平台及测量方法

Publications (2)

Publication Number Publication Date
CN108917589A CN108917589A (zh) 2018-11-30
CN108917589B true CN108917589B (zh) 2020-09-08

Family

ID=64414827

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810791427.6A Active CN108917589B (zh) 2018-07-18 2018-07-18 一种机械手关节角度测量系统、平台及测量方法

Country Status (1)

Country Link
CN (1) CN108917589B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113776486A (zh) * 2021-09-10 2021-12-10 广州国显科技有限公司 平行度测量装置及平行度测量方法
CN113561184B (zh) * 2021-09-18 2021-11-26 佛山隆深机器人有限公司 一种机械臂外置角度反馈装置及机械臂
CN115325990B (zh) * 2022-09-01 2024-07-26 西安交通大学 一种消除偏心影响的旋转梁角度监测系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104723347A (zh) * 2015-03-20 2015-06-24 上海理工大学 机械手
CN106445130A (zh) * 2016-09-19 2017-02-22 武汉元生创新科技有限公司 一种用于手势识别的动作捕捉手套及其校准方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4980318B2 (ja) * 2008-08-28 2012-07-18 Ｎｔｎ株式会社 等速自在継手用作動角センサ
CN102567638B (zh) * 2011-12-29 2018-08-24 无锡微感科技有限公司 一种基于微型传感器的交互式上肢康复系统
CN103776451B (zh) * 2014-03-04 2016-11-09 哈尔滨工业大学 一种基于mems的高精度三维姿态惯性测量系统以及测量方法
CN104799982B (zh) * 2015-05-14 2016-10-19 上海交通大学 基于连续体差动机构的单电机欠驱动假肢手
CN206422919U (zh) * 2016-10-25 2017-08-18 华南理工大学 一种基于无线蓝牙的身体跌倒检测装置
CN106646481B (zh) * 2016-11-09 2023-09-01 国网安徽省电力公司铜陵供电公司 一种用于输电线路的无人机测距装置及其测距方法
CN107131865A (zh) * 2017-06-13 2017-09-05 广州辛群科技有限公司 角度检测装置
CN107595290A (zh) * 2017-08-08 2018-01-19 刘超军 一种关节角度测量方法及系统
CN108279773B (zh) * 2018-01-12 2021-04-09 上海大学 一种基于marg传感器和磁场定位技术的数据手套

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104723347A (zh) * 2015-03-20 2015-06-24 上海理工大学 机械手
CN106445130A (zh) * 2016-09-19 2017-02-22 武汉元生创新科技有限公司 一种用于手势识别的动作捕捉手套及其校准方法

Also Published As

Publication number Publication date
CN108917589A (zh) 2018-11-30

Similar Documents

Publication Publication Date Title
CN108917589B (zh) 一种机械手关节角度测量系统、平台及测量方法
EP3769094B1 (en) Contactless current measurement using magnetic sensors
CN108061855B (zh) 一种基于mems传感器的球形电机转子位置检测方法
EP3224576B1 (en) Absolute rotary encoder
CN109813336B (zh) 惯性测量单元标定方法
KR100533106B1 (ko) 지자계 센서의 자세 오차 보상장치 및 방법
CN107561462B (zh) 基于变化磁场的胶囊全姿态测定系统及空间磁场检测装置
CN109483596B (zh) 一种机器人工具端负载的测试方法
CN104182614A (zh) 六自由度机械臂姿态监测系统及方法
CN107607114B (zh) 一种数字陀螺稳定平台在线频率特性软测试方法
CN109382817A (zh) 校准系统和校准方法
JP2003502681A (ja) 角度センサのオフセット補償方法
CN113870366B (zh) 基于位姿传感器的三维扫描系统的标定方法及其标定系统
Xu et al. A robust incremental-quaternion-based angle and axis estimation algorithm of a single-axis rotation using MARG sensors
JPWO2016143581A1 (ja) 球乗りロボット
US11307052B2 (en) Signal processing circuit, position detection device, and magnetic sensor system
CN110307860A (zh) 一种差分式霍尔多圈位置绝对位置传感器及其检测方法
TWI407102B (zh) 三維空間運動感測方法
CN106217374B (zh) 一种智能机械臂的控制方法、装置及系统
CN207456462U (zh) 一种微惯性测量单元和惯性测量单元检测设备
CN109623878A (zh) 一种用于仿人灵巧手手腕关节的传感系统的自标定方法
CN110189359B (zh) 一种基于分类学习的快速智能跟踪方法和装置
CN209793788U (zh) 一种用于仿人灵巧手手腕关节的传感系统
TW201331731A (zh) 一種差動感測裝置及使用其之自動定位平台
CN108007458A (zh) 一种捷联惯性原件连接装置及其用于测量机械臂角度方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant