本发明涉及机器人关节角度测量技术领域，尤其是涉及一种对偶激光式机器人关节角度测量方法及角度测量装置。
背景技术：
关节角度测量是机器人运动控制过程中的一个重要环节，工业生产和日常生活中许多场景都需要对关节角度量进行测量，例如多轴机器人的关节角度、自动旋转门窗的张合角度等。通常，各类测角传感器将机构运动中的角位移量转化为电信号，计算机收到数据后通过一定的数学运算求得该机构的关节角度值，从而确定关节运动位置以便为控制器提供反馈参考。
近年来随着计算机技术的发展和传感器性能的提升，角度测量技术取得了巨大的进步。目前，角度测量方法可以按照工作原理分为以下七类：基于电学的方法、基于光学的方法、光栅方法、光电编码器方法、电磁方法、惯性测量法、视觉测量法等。在机器人的关节角度测量中，一般采用光电式旋转编码器和磁性编码器进行测量。
1)磁性编码器
磁性编码器的工作原理是利用霍尔效应来进行角度测量。该传感器通过检测由关节转动所引起的霍尔元件周围磁场变化，属于非接触式测量传感器。受加工工艺限制，磁性编码器无法达到较高的测量精度，但其价格低廉，使用寿命较长，对环境依赖性低，被广泛用于工业生产中。
2)光电编码器
光电编码器又被称为光电码盘，同样属于非接触式测量传感器。该装置利用光的透过和阻挡来表示二进制测量信号，通过在码盘圆周上等距刻画码道来设定不同精度的角度测量值，测量精度优于磁性编码器。由于其具备较高的测量精度，光电式编码器被广泛应用于工业机器人的关节角度测量之中。但是由于加工制造难度较大，该类型传感器的高端产品价格相对偏高。
上述两类传感器均需要与关节转轴或电机轴之间通过联轴器固定或者齿轮啮合等方式直接接触，安装难度较大。此外，这两类传感器的加工工艺复杂，对电磁环境敏感，需要精准的标定、封装和校准操作，生产成本相对较高。
3)光学方法
随着激光技术的迅速发展，许多高精度的光学测角方法被开发出来。这些方法通常采用干涉法来保证测量精度，例如圆光栅测角法、激光干涉测角法等。这些方法可以达到非常高的测角精度，但是测量的动态范围较小，而且由于光学元器件需要精密加工以及光路复杂等原因，其造价较为高昂。近期，基于激光三角测距法的非接触式测角装置被设计出来，这一方法动态范围大，响应速度快，但其装置光路复杂，设备成本依然较高。
综上所述，已有的角度测量装置或安装难度较大，或是动态范围不够，或是成本较高，无法保证对机器人关节姿态测量的要求，无法适应复杂的生产应用场景。
技术实现要素：
本发明的目的在于克服上述现有技术存在的缺陷而提供一种对偶激光式机器人关节角度测量方法及角度测量装置。
机器人的末端关节在摆动过程中，会引起左右侧激光测距的变化，而这一距离变化与关节角度之间存在明确的几何关系，本发明基于这一原理实现。
本发明的目的可以通过以下技术方案来实现：
一种对偶激光式机器人关节角度测量方法，该方法以地平面作为参考面构建激光-平面测角模型，通过测量机器人关节处的对偶激光测距变化值及所述激光-平面测角模型，获得机器人关节的转动角度。
进一步地，该方法包括以下步骤：
1)在机器人上安装对偶激光测距仪，该对偶激光测距仪包括两个对偶设置的激光器，该激光器的出射激光与臂轴垂线方向的夹角根据测量需求设定；
2)在机器人关节运动过程中实时采集对偶激光测距仪左右两个激光测距值；
3)基于步骤2)的采集数据和所述激光-平面测角模型获得机器人关节的转动角度。
进一步地，所述转动角度的计算公式为：
其中，θ为转动角度，rl、rr分别为左右两个激光测距值，α为出射激光与臂轴垂线方向的夹角。
进一步地，以修订值
其中，δr0表示距离偏置参数。
本发明还提供一种实现所述的测量方法的对偶激光式机器人关节角度测量装置，包括：
对偶激光测距仪，安装于机器人上，且其中心轴与机器人的臂轴平行；
计算机，与所述对偶激光测距仪连接，在机器人关节运动过程中实时采集对偶激光测距仪左右两个激光测距值，并基于所述激光-平面测角模型获得机器人关节的转动角度。
进一步地，所述对偶激光测距仪包括v字型基座和对偶式固定在所述v字型基座上的左激光器和右激光器，所述v字型基座固定于机器人上。
进一步地，所述v字型基座上设置有用于与激光器和机器人连接的固定孔。
进一步地，所述v字型基座上设置有导线通过孔。
进一步地，所述计算机内存储有转动角度获取程序，该转动角度获取程序包括：
数据采集子程序，用于在机器人关节运动过程中实时采集左右两个激光测距值；
关节角度测量子程序，用于根据激光-平面测角模型获得机器人关节的转动角度。
进一步地，所述转动角度获取程序还包括：
测量结果可视化子程序，用于动态显示机器人关节的转动角度实时测量值。
与现有技术相比，本发明具有如下有益效果：
1、本发明设计基于地平面参考的“机械臂-激光”转动几何测角模型，并利用该模型将角度变化量转变为光学测量的距离变化量，进而利用测得的激光到地面的距离来反推出关节角，属于非接触式测量原理。
2、本发明采用对偶式激光测角法，即利用对偶激光测距对转动角度进行估计，一方面提高了角度测量精度，另一方面减少了模型参数，使得模型不依赖于关节距离地面高度和关节的大小形状。
3、与传统的旋转编码器相比，本发明对偶激光测角仪不需要直接安装在关节转轴上，安装位置不受限制，避免了旋转编码器在与转轴安装中同轴配准的困难，降低了装配难度。
4、本发明价格相对低廉，角度测量精度高，响应速度快，对周围环境和运动载体的依赖性低，可以很好的适用于室内机器人的工作环境。
5、本发明将待测角度量转化为距离测量量，并通过对偶激光测量法减少了单边模型的误差，有效提升了角度测量精度。在实验过程中，本发明的角度测量装置在激光安装倾角为45度时，角度测量误差为0.3度。实验结果表明，该装置可以准确、快速、可靠的对机器人关节角进行测量。
6、本发明的角度测量方法有较大的实际应用优势。
附图说明
图1为本发明基于激光测距的关节角度测量原理示意图；
图2为本发明对偶激光测角仪的结构示意图，其中，(2a)为正视图，(2b)为几何参数示意图，(2c)为俯视图；
图3为以地面为参考的机械臂-激光转动几何测角模型示意图；
图4为实验中对偶激光测角仪测量到的原始数据示意图；
图5为角度测量结果示意图；
图6为误差曲线示意图；
图7为误差统计直方图，其中，(7a)为对偶激光，(7b)为左侧激光，(7c)为右侧激光。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
实施例1
本实施例提供一种对偶激光式机器人关节角度测量方法，该方法以地平面作为参考面构建激光-平面测角模型，通过激光测距与关节转动角度之间的几何关系换算，将关节的转动角度变化转换到激光测距变化之中，实时测量时，通过测量机器人关节处的对偶激光测距变化值及激光-平面测角模型，获得机器人关节的转动角度。该方法包括以下步骤：
1)在机器人上安装对偶激光测距仪，该对偶激光测距仪包括两个对偶设置的激光器，该激光器的出射激光与臂轴垂线方向的夹角根据测量需求设定；
2)在机器人关节运动过程中实时采集对偶激光测距仪左右两个激光测距值；
3)基于步骤2)的采集数据和激光-平面测角模型获得机器人关节的转动角度。
上述方法以地平面作为参考面构建激光-平面测角模型，本实施例利用单连杆机构模型对机械臂末端关节的摆动过程进行抽象，可得如图3所示的几何模型。图中，l表示机械臂臂长，h为转轴距离地面高度，rl、rr分别为基于激光测距仪发出的测距激光束测得的左侧激光测距值和右侧激光测距值，机械臂摆动角度为θ，激光与垂直于臂轴的直线夹角为α(即激光倾角)。
由几何分析可得，左侧激光与地面夹角为(α θ)，右侧激光与地面夹角为(α-θ)，因此，可以得到激光测距与转动角度的关系表达式：
根据公式(1)(2)可知，机械臂摆角与激光测距之间存在明确的函数关系。因此，当模型中的几何参数h、l、α已知时(这些参数均容易通过测量手段事先获取)，只需测得任意一侧的激光距离值，既可反求出摆角值。将式(1)(2)展开可得：
(rlsinα l)cosθ rlcosαsinθ＝h(3)
(rrsinα l)cosθ-rrcosαsinθ＝h(4)
当左侧激光距离已知时，令a＝rlsinα l，b＝rlcosα，c＝h，可得以下角度计算公式：
当右侧激光距离已知时，令a＝rrsinα l,b＝rrcosα,c＝h,角度计算公式如下：
虽然公式(5)、(6)可以求解关节转动角度，但是这两个公式均依赖于外部几何参数(h和l)，这为测角装置的部署和使用造成了麻烦，因此，本测量方法进一步进行了改进。
根据以上几何模型，令d＝h-lcosθ，结合公式(1)(2)，则有如下公式成立：
rrsin(α-θ)＝rlsin(α θ)＝d(7)
由公式(1)(2)(7)可以发现，关节摆动角度只与左右侧激光的测量距离有关，而与d无关，即与关节转轴距离地面高度h和机械臂长度l无关。因此，通过化简整理公式(1)(2)(7)可得：
根据公式(8)，可以推导出基于对偶激光测角法公式，即：
通过公式(9)可以发现，对偶激光测角法里只涉及一个模型参数，即出射激光与臂轴垂线方向的夹角α，而这一角度值可以通过仪器设计的方式事先给出。这一结论避免了手工测量距地面高度等物理参数，使角度测量过程得到大大的简化。
考虑到实际的安装过程中激光器和机械臂的摆动轴之间存在一定的距离偏差以及激光器本身的尺寸大小，因此在理想测量模型的基础上需要增加一个距离偏置常数。即公式(9)中的左右激光测距值需要做以下调整：
其中，
上述方法基于地平面参考的“机械臂-激光”转动几何测角模型，并利用该模型将角度变化量转变为光学测量的距离变化量，进而利用测得的激光到地面的距离来反推出关节角度，角度测量精度高，参数少。
表1
本发明测量方法为光学方法，通过几何关系转换与激光测距的方式，将机器人关节转动的角位移变化量转化为距离激光距离变化量。该方法与其他现有测角方法的性能对比如表1所示。
实施例2
如图1所示，本实施提供一种对偶激光式机器人关节角度测量装置，包括对偶激光测距仪1和计算机2，其中，对偶激光测距仪1安装于机器人上，且其中心轴与机器人的臂轴平行；计算机2与对偶激光测距仪连接，在机器人关节运动过程中实时采集对偶激光测距仪左右两个激光测距值，并基于激光-平面测角模型获得机器人关节的转动角度。该角度测量装置的测量原理如实施例1。
对偶激光测距仪1可安装在机械臂轴向，如装于机器人末端或安装在与机械臂的侧面，只需保证与运动方向平行即可。
如图2所示，对偶激光测距仪包括v字型基座13和对偶式固定在v字型基座上的左激光器11和右激光器12，v字型基座13固定于机器人上。v字型基座13上设置有用于与激光器和机器人连接固定孔。v字型基座13上设置有导线通过孔。
本实施例通过螺钉和螺母来对激光测距仪进行固定。左激光器11和右激光器12分别安装于v字型基座13的v字的两臂上，且激光射出方向朝外。v字的两臂分别向侧边延升，以形成与机器人连接的平面。两臂和两个平面上均设有用于装配固定的固定孔，v字中间还设置有导线通过孔，以便于激光测距仪的供电和通信。
本实施例的对偶激光测距仪充分利用了几何设计上的对称性，使得提出的传感器结构变得简洁美观。
v字型基座13的等腰三角形顶角β＝2α，可通过控制该顶角的大小，调节不同的激光倾角，实现出射激光与臂轴垂线方向的不同夹角的设定。本实施例中，v字型基座的等腰三角形顶角设置为90°，即激光倾角为45°。
该装置中，激光器采用的是飞行时间法单点激光测距传感器，其性能参数如表2所示。激光测距仪与计算机之间通过串口通信协议进行通信，本装置中采用的是ttl转usb设备完成激光测距仪与计算机之间的数据通信。
表2
计算机2内存储有转动角度获取程序，该转动角度获取程序包括：
数据采集子程序21，用于在机器人关节运动过程中实时采集左右两个激光测距值；
关节角度测量子程序22，用于根据对偶激光测距仪发出的出射激光与臂轴垂线方向的夹角以及激光-平面测角模型获得机器人关节的转动角度。
在另一实施例中，转动角度获取程序还包括：测量结果可视化子程序23，用于动态显示机器人关节的转动角度实时测量值。
为了验证上述装置的正确性，本实施例在kinovajaco2七轴机械臂上进行了实验验证。jaco2机械臂的关节角度输出频率为100hz，编码器的角度测量分辨率为0.000142度，作为角度测量的参考基准。实验中采用的操作系统为ubuntu16.04，实验数据全部基于开源机器人操作系统(ros)平台进行采集。
在jaco2机械臂上的实验过程中，对偶激光测角仪原型设备安装于jaco2机器人的第七轴关节末端，测角仪的激光出射倾角设定为45度。运动中第六轴和第七轴关节之间保持固定，关节转动过程设定为第六轴关节的往复运动，即测量第六轴的关节角度。
首先将机械臂固定在工作台一侧，然后将机械臂的末端两关节拖动至地面上方，使其与地面垂直。此处可通过两侧激光测距结果对关节零点进行直观判断，当左右两侧激光测距值相等时，可认为到达零点位置，并对此处的第六轴关节角度值进行记录。在零点设定完成后，对机械臂第六轴设定往复运动任务。将机械臂设定为在零点左右侧做正负向(本实验规定面向机械臂时，以逆时针转动方向为正方向)往返摆动，摆动幅度为30°，共执行10个周期的运动任务。
实验结果如图4-图7所示。图4为激光测距传感器测量到的原始数据，即两侧激光的距离测量值。通过图线可以看出，激光测距呈对称性交替变化的趋势，这一表现侧面印证了激光测距模型公式(4)(5)的正确性。图5为角度测量结果。图中绿色图线为机械臂内置编码器测得的关节转动角度，由于编码器关节角度的零位定义与本测量装置不同，因此在减去零点处记录的关节角度后作为本传感器的角度测量参考值；蓝色图线为公式(5)的角度计算结果，红色图线为公式(6)的角度计算结果，这两个曲线均是仅依赖单侧激光求解得到的关节角度；粉色图线为利用对偶激光测角法公式(9)计算得到的结果。通过对比可以看出，四条曲线的变化趋势保持了高度一致。相比于单侧激光测角法而言，对偶激光测角法与编码器测得的角度值更加接近。
量化误差分析曲线可见图6和图7。通过图6看出，仅依赖单侧激光进行关节角度计算时，在激光器所在一侧的转动角度值测量误差较小，在其相反的一侧时角度误差测量较大。而对偶激光测角法克服了这一不足，即在两侧的角度测量都较小。通过图7可以看出，对偶激光测角法误差分布大致符合零均值高斯分布，而单侧激光测角法误差存在双峰，即在其中一侧有较大的误差存在。这一现象再次表明单侧激光在激光所在的一侧无法有效观测到转角变化，而对偶激光测角法则不存在这一问题。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由本发明所确定的保护范围内。
1.一种对偶激光式机器人关节角度测量方法，其特征在于，该方法以地平面作为参考面构建激光-平面测角模型，通过测量机器人关节处的对偶激光测距变化值及所述激光-平面测角模型，获得机器人关节的转动角度。
2.根据权利要求1所述的对偶激光式机器人关节角度测量方法，其特征在于，该方法包括以下步骤：
1)在机器人上安装对偶激光测距仪，该对偶激光测距仪包括两个对偶设置的激光器，该激光器的出射激光与臂轴垂线方向的夹角根据测量需求设定；
2)在机器人关节运动过程中实时采集对偶激光测距仪左右两个激光测距值；
3)基于步骤2)的采集数据和所述激光-平面测角模型获得机器人关节的转动角度。
3.根据权利要求2所述的对偶激光式机器人关节角度测量方法，其特征在于，所述转动角度的计算公式为：
其中，θ为转动角度，rl、rr分别为左右两个激光测距值，为出射激光与臂轴垂线方向的夹角。
4.根据权利要求3所述的对偶激光式机器人关节角度测量方法，其特征在于，以修订值
其中，δr0表示距离偏置参数。
5.一种实现如权利要求1所述的测量方法的对偶激光式机器人关节角度测量装置，其特征在于，包括：
对偶激光测距仪，安装于机器人上，且其中心轴与机器人的臂轴平行；
计算机，与所述对偶激光测距仪连接，在机器人关节运动过程中实时采集对偶激光测距仪左右两个激光测距值，并基于所述激光-平面测角模型获得机器人关节的转动角度。
6.根据权利要求5所述的对偶激光式机器人关节角度测量装置，其特征在于，所述对偶激光测距仪包括v字型基座和对偶式固定在所述v字型基座上的左激光器和右激光器，所述v字型基座固定于机器人上。
7.根据权利要求6所述的对偶激光式机器人关节角度测量装置，其特征在于，所述v字型基座上设置有用于与激光器和机器人连接的固定孔。
8.根据权利要求6所述的对偶激光式机器人关节角度测量装置，其特征在于，所述v字型基座上设置有导线通过孔。
9.根据权利要求5所述的对偶激光式机器人关节角度测量装置，其特征在于，所述计算机内存储有转动角度获取程序，该转动角度获取程序包括：
数据采集子程序，用于在机器人关节运动过程中实时采集左右两个激光测距值；
关节角度测量子程序，用于根据激光-平面测角模型获得机器人关节的转动角度。
10.根据权利要求6所述的对偶激光式机器人关节角度测量装置，其特征在于，所述转动角度获取程序还包括：
测量结果可视化子程序，用于动态显示机器人关节的转动角度实时测量值。
技术总结