本发明涉及角度传感领域，尤其涉及一种机械手关节角度测量系统、平台及测量方法。
背景技术
目前，在对机械手的控制过程中，对手指角度的传感是实现控制的基础。目前存在若干以声、光、电、磁等为原理的角度传感装置，但大多价格高昂，而且装置复杂、体积较大，不利于机械设计以及装配，为设计带来很大难度，同时也让设备变得冗杂。
惯性传感器是一种用来测量系统加速度、角速度的传感器，一般由加速度传感器和陀螺仪构成。其中，加速度传感器测量得到的加速度由重力加速度与物体的绝对加速度构成。将加速度传感器固连在被测量系统上，在静止的情况下，可以利用加速度传感器测量得到重力在传感器各轴方向上的分量。一个系统的空间取向由方位角，横滚角以及俯仰角构成仅仅使用重力传感器可以得到系统的俯仰角和横滚角，但是不能够得到方位角信息，因此不能得到系统的全部空间取向信息。磁场传感器是基于霍尔效应等电磁感应效应的测量传感器所在位置磁场的传感器。在地球表面的绝大多数地方，存在着稳定的地磁场，其强度约为0.5-0.6高斯。在一个很小的局部，地磁场可以被认为为匀强磁场，因此其和重力场相同，也可以作为系统空间取向的一个绝对参考。通过测量重力场和磁场信息，可以计算得到系统的全部空间取向信息。
对于机械手的关节，原则上，通过测量关节两杆连体坐标系下的重力场和磁场信息，能够计算两杆的全部空间取向，进而计算任意两者之间的相对角度，但计算过程复杂，需要耗费较长的时间，若考虑到传感器采集的原始数据存在误差，需要对数据进行预处理，耗费的时间将更长。
因此，本领域的技术人员致力于开发一种机械手关节角度测量系统、平台及测量方法，该系统为基于加速度-角速度-磁场传感器的机械手关节角度测量系统，该平台为应用上述加速度-角速度-磁场传感器的机械手关节角度测量系统的机械手平台。本发明测量系统十分便于小型化，可以安装在被测量关节上的绝大多数位置，十分灵活，避免了传统的角度传感装置(如编码器)安装在关节处时与电机、轴承等装置相干涉的问题，利用本发明测量系统的测量平台可大大降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，可靠性更好。本发明还提供了一种利用校准后的重力场和磁场信息，不解算每杆的位型而直接计算机械手关节两杆间角度的方法。与传统方法相比，本发明大大降低了测量成本，显著减小了测量系统的体积，提高了解算速度，缩短了数据处理时间。
技术实现要素：
有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是在机械手关节角度测量中如何让读取数据的测量系统小型化，从而降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，减小成本，方便安装，以及如何更加简便地解算所读取的数据从而使得提高解算速度，解算耗时缩短。
为实现上述目的，本发明提供了一种机械手关节角度测量系统、平台及测量方法，本发明测量系统是基于加速度-角速度-磁场传感器的机械手关节角度测量系统，该平台为应用上述加速度-角速度-磁场传感器的机械手关节角度测量系统的机械手平台。
本发明测量系统包括多个测量单元，所述测量单元固连于被测机械手关节的相关联杆件物体上，优选的，被测机械手关节的相关联两个杆件物体上，固连所述测量单元的个数为2个；每个所述测量单元包含一块加速度-角速度-磁场传感器和若干贴片电阻、滤波电路、印刷电路板以及fpc排线组件，所述传感器、所述电阻、所述滤波电路以及所述fpc排线组件载于所述印刷电路板上，优选的，所述印刷电路板整体尺寸为9×22×3mm，所述fpc排线组件包括fpc插座和fpc排线，优选的，所述加速度-角速度-磁场传感器为invensensempu9250九轴加速度-角速度-磁场传感器；所述印刷电路板通过fpc排线组件供电并与外部通信；所述fpc排线的接口包括电源正极、电源负极和i2c通信接口的sda、scl线，优选的，所述i2c通信接口为philipsi2c通信接口；所述电阻包括上拉电阻和下拉电阻，所述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的低通滤波器进行滤波，得到多组重力场与磁场的基础数据，对这些数据利用椭球拟合得到椭球方程，以磁场数据为例：
其中bx、by、bz为所述测量单元滤波后的所述基础数据，bx0、by0、bz0为球心在三轴上的坐标，a、b、c分别为椭球的三根半轴长度，重力场数据拟合同样采用上述方法进行椭球拟合。
更进一步地，所述步骤6具体为，以所述步骤2获得的拟合椭球方程作为本发明角度测量系统的校准模型，在测量过程中利用此模型对所述步骤5得出的重力场与磁场的基础数据进行校准得到真实数据，校准表达式如下所示：
其中bx’、by’、bz’为校准后磁场的真实数据，重力场数据同样采用上述方法进行校准。
更进一步地，所述步骤7还包括：
步骤7.1、利用所述步骤6的校准算法处理后得到的真实数据分别为：
其中b1、b2为处理后磁场的真实数据，g1、g2为处理后重力场的真实数据；
步骤7.2、每次测量后，首先对以上数据进行归一化，公式如下：
步骤7.3、利用下式可以求解出α角：
步骤7.4、通过两个所述测量单元可以求解出两个α角，结果取两者平均数即可，同时，从绝对坐标系角度计算，测量值理论上有如下数学形式：
由于测量单元固连坐标系是右手坐标系，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片invensensempu9250九轴加速度-角速度-磁场传感器芯片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的低通滤波器对传感器测量的数据进行滤波，得到重力场与磁场的基础数据，对这些基础数据利用椭球拟合得到椭球方程，以磁场数据为例：
其中bx、by、bz为传感器读取经过滤波后的基础数据，bx0、by0、bz0为球心在三轴上的坐标，a、b、c分别为椭球的三根半轴长度，此方程即为本系统的校准模型。重力场拟合同样采用上述方法进行椭球拟合，在测量过程中需要利用此模型对滤波后的基础数据数据进行校准得到真实数据，校准表达式如下所示：
其中bx’、by’、bz’为校准后磁场的真实数据，重力场数据同样采用上述方法进行校准。
本方法利用处理后的数据、空间几何关系以及右手系的定义，解算出机械手关节两杆之间的夹角。以图1所示的两个测量单元的z轴之间的夹角θ为例，具体实现方式如下：
首先在空间中任意一点建立如图6所示的绝对坐标系oxyz。其中z轴方向为重力加速度的反方向，x轴的方向为磁场在水平方向的投影方向，其中重力与磁场方向的夹角为α。此坐标系方向完全由重力场方向与磁场方向决定，与测量单元的空间取向无关。此坐标系在磁场垂直于水平面试无法定义，但是在地球极地以外的绝大部分区域磁场均明显不垂直于水平面，因此此定义方式有效。在较小的移动范围内，地磁场与重力场可以认为为匀强场，因此按照此方法定义在较小范围内的绝对坐标系的空间取向相同。
如图7所示，根据测量单元的几何关系，建立固连在两个测量单元上的坐标系o1x1y1z1，o2x2y2z2。如图8所示，每一个传感器测量得到的重力与磁场的原始数据在其固连坐标系上分量，再利用上述的滤波和校准处理后得到的真实数据分别为：
其中b1、b2为处理后磁场的真实数据，g1、g2为处理后重力场的真实数据。
每次测量后，首先对以上数据进行归一化：
利用下式可以求解出α角：
通过两个测量单元可以求解出两个α角，结果取两者平均数即可。同时，从绝对坐标系角度计算，测量值理论上有如下数学形式：
其中pi(j)是向量j在向量i方向上的投影，解算得到
由于测量单元固连坐标系是右手坐标系，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的重力场和磁场数据，利用前文所述的数据校准和角度解算方法，可实现对三个机械手指与机械手掌的相对角度的实时测量。
该基于加速度-角速度-磁场传感器的机械手关节角度测量系统、平台及测量方法能够有效的运用于机械手关节中，测量系统十分便于小型化，可以安装在被测量机械手关节上的绝大多数位置，十分灵活，避免了传统的角度传感装置(如编码器)安装在关节处时与电机、轴承等装置相干涉的问题，可大大降低传感系统的复杂性和机械设计的难度，使得机械手结构简单，可靠性更好，并且本发明提供了一种利用校准后的重力场和磁场信息，不解算每杆的位型而直接计算机械手关节两杆间角度的方法。与传统方法相比，本发明大大降低了测量成本，显著减小了测量系统的体积，提高了解算速度，缩短了数据处理时间。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。