CN106871859B - 一种刚体空间定轴转动角度检测方法 - Google Patents

Abstract

本发明公开了一种刚体空间定轴转动角度检测方法，提出了在全站仪测量定位原理，即通过利用全站仪在两个位置对同一个标志点的水平角和垂直角测量，得到相应标志点在全站仪坐标系下的坐标值的前提下，由标志点的坐标值构造一组矢量，利用双矢量定姿原理确定空间刚体的位姿，在此基础上，进行转动角度解算方法的推导和验证，为大运动范围下的刚体位姿参数测量提供了新的解决方法。

Description

一种刚体空间定轴转动角度检测方法

技术领域

本发明属于刚体转角测量技术领域，具体涉及一种刚体空间定轴转动角度检测方法的设计。

背景技术

刚体是指在任何外力作用下，其形状和大小完全不变的物体，是一种理想的质点系统。刚体最基本的运动形式是平动和转动，任何复杂的刚体运动都可以分解为平动与转动的叠加，这两种简单运动是工程中最常见的运动，也是研究复杂运动的基础。工程中某些物体的运动，例如：气缸内活塞的运动、车床上刀架的运动等属于刚体平移；而最常见的齿轮、机床的主轴、电机的转子等属于刚体绕定轴转动。

刚体运动的转角测量已被广泛用于工业、军事、航天、航海等许多领域，并且对测量精度的要求也越来越高。在工业控制和测量中，角度作为一个重要的参数，对其进行测量是十分必要的。比如在一些陆基站或海基站天线伺服系统中、在需要转台的工业生产中、在低转速的实时控制中，都少不了角度的测量。

工业上现有的主要的测角方法有：机械测角方法、电磁测角方法、光学测角方法以及光电测角方法等。机械式测角技术是研究最早的测角技术之一，它主要以多齿分度盘为代表。机械测角方法早期应用较多，其优点是简单，成本低。电磁分度式测角技术是利用电磁感应的原理来测量角度，主要是利用各种参数进行分度测角。电磁分度技术大量应用于进一步细分，使仪器的分度和测量范围扩大，分辨率提高，使用范围广泛。目前主要有圆磁栅测角和感应同步器测角两种。光学测角方法历来以其极高的测量准确度而受到人们的重视，光学测角方法的应用也越来越广泛。目前光学测角方法除众所周知的光学分度头法和多面梭体法外，常用的还有莫尔条纹法、平行干涉图法、圆光栅法、光学内反射法、激光干涉法等。在光电测角方法中，目前应用比较普遍的光电传感器有光学码盘式传感器、环形激光器和图像传感器，其具有高精度、高可靠性、实现简单、体积小、角灵敏度高等优点。

其中机械测角方法、电磁测角方法属于传统的角度测量方法，大多釆用手工接触测量的方式，测量的精度和稳定性受到外界环境影响较大。光学测角方法、光电测角方法由于测量过程中不与被测物体接触，不仅实现了转角的高精度非接触测量，而且其操作简单，系统可靠性高，便于在现场实施测量，并能够大大降低人力劳动。

针对定轴转动，转角检测方法可分为接触式检测和非接触式检测。例如在齿轮传动系统中，此时轴线固定且便于安装编码器等设备，应用接触式检测即可方便的检测转角；而例如在飞机舵面等转角检测中，轴线固定却不明确或者转角测量元件不便于安装，常规的安装编码器等检测方法不再适用，常见的检测方法为非接触式检测方法，例如视觉测量，目前大部分视觉测量方法都是基于双目立体视觉的测量技术。

一直以来，在多轴飞行器、运动机器人、机械臂和航天航空等领域中，刚体空间运动的姿态和位置参数测量是刚体运动分析和控制的重要内容。传统的空间刚体运动参数测量方法是在刚体上安装陀螺仪，利用陀螺仪测量刚体空间姿态转角，或结合陀螺仪和加速度计的数据融合方法求解刚体空间姿态信息，这种方式需要在刚体上安装陀螺仪等测量仪器，相应的测量成本及设备维护代价比较高且精度受限。而在一些情况下，例如刚体表面易受损或者转动轴线无法确定时，接触式测量方式有很大的局限性，因此在刚体运动参数测量方法中，非接触式测量由于测量装置不与被测表面直接接触，被测表面既不会被损伤，也不会对测量装置造成损害，同时避免了由于接触而引入的测量误差，而受到广泛的关注。

在非接触式测量方法中，视觉测量的应用研究最为广泛，视觉测量对复杂环境、复杂运动有很好的适应性和较高的测量精度。其中，在地上大型建筑和地下隧道施工等精密工程测量或变形监测领域中，全站型电子测距仪被广泛的使用。全站型电子测距仪是一种集光、机、电为一体的高技术测量仪器，是集水平角、垂直角、距离(斜距、平距)、高差测量功能于一体的测绘仪器系统。与光学经纬仪比较，电子经纬仪将光学度盘换为光电扫描度盘，将人工光学测微读数代之以自动记录和显示读数，使测角操作简单化，且可避免读数误差的产生，其一次安置仪器就可完成该测站上全部测量工作。

发明内容

本发明的目的是为了解决刚体空间转动轴位姿难以确定及刚体表面复杂的情况下，刚体转动角度传统接触式测量方法存在种种弊端的问题，提出了一种刚体空间定轴转动角度检测方法。

本发明的技术方案为：一种刚体空间定轴转动角度检测方法，包括以下步骤：

S1、刚体表面标志点定位；

S2、定义世界坐标系与刚体转动起止位置坐标系；

S3、构造与表征刚体运动的矢量；

S4、计算刚体转动角度。

进一步地，步骤S1具体为：

在刚体表面上设置三个标志点，采用两个设置于不同位置的全站仪分别对每个标志点进行测量，得到各标志点的水平角、垂直角，并根据全站仪的测量定位原理，由各标志点的水平角、垂直角得到该标志点的三维坐标值。

进一步地，步骤S2具体为：

以全站仪特征点测量数据的坐标系OXwYwZw作为世界坐标系；设刚体转动的初始位置和最终位置分别为m、n位置，以刚体转动轴作为Z轴分别建立n位置的刚体坐标系O1XnYnZn以及m位置的刚体坐标系O1XmYmZm；n位置的刚体坐标系O1XnYnZn相对于m位置的刚体坐标系O1XmYmZm存在角度为θ的转动量。

进一步地，步骤S3具体为：

在刚体坐标系O1XmYmZm中，以刚体表面的三个标志点作为特征点p_mi,i＝1,2,3，对应世界坐标系OXwYwZw中的特征点p_wi,i＝1,2,3；由点p_m1连接点p_m2、p_m3，得到矢量L_m1、L_m2，相对应的由点p_w1连接点p_w2、p_w3，得到矢量L_w1、L_w2；

设世界坐标系OXwYwZw和刚体坐标系O1XmYmZm的转化关系为：

式中

表示刚体坐标系O1XmYmZm相对于世界坐标系OXwYwZw的旋转矩阵，T为刚体坐标系O1XmYmZm的原点在世界坐标系OXwYwZw中的三维坐标值；

相应的特征点p_mi和p_wi的转化关系是：

由世界坐标系OXwYwZw中的矢量构造方式有：

L_w1＝p_w2-p_w1,L_w2＝p_w3-p_w1 (3)

同理得刚体坐标系O1XmYmZm中有：

L_m1＝p_m2-p_m1,L_m2＝p_m3-p_m1 (4)

由公式(2)(3)(4)得：

构造所需的第三个矢量，选用两个矢量L_w1与L_w2进行叉乘，且满足如下关系：

定义矢量J_w和J_m：

J_w＝L_w1*L_w2,J_m＝L_m1*L_m2 (7)

由公式(6)(7)得：

定义矩阵D_w和D_m：

D_w＝[L_w1 L_w2 J_w],D_m＝[L_m1 L_m2 J_m] (9)

由公式(5)(8)(9)得：

由矩阵D_w和D_m的构造过程可知，两个矩阵均为满秩矩阵，因此存在可逆矩阵，则由公式(10)得：

同理可知，刚体在n位置时，刚体坐标系O1XnYnZn相对于世界坐标系OXwYwZw的旋转矩阵

为：

其中：

D_w′＝[L_{w′ 1}L_w′2 J_w′],D_n＝[L_n1 L_n2 J_n] (13)

在实际的刚体转动过程中，最终位置n相对初始位置m只存在定轴转动，且转动角度为θ，因此n位置的刚体坐标系O1XnYnZn相对于m位置的刚体坐标系O1XmYmZm的旋转矩阵

为：

由公式(11)(12)(14)得：

矢量模在坐标变换中具有不变性，因此存在如下关系：

|L_wi|＝|L_mi|＝|L_ni|,i＝1,2 (16)。

进一步地，步骤S4具体为：

由坐标系的建立方式可知，特征点相对于刚体坐标系是固定的，因此特征点在n位置和m位置的坐标数值上是相同的，即：

p_ni＝p_mi,i＝1,2,3 (17)

由于各特征点的坐标相同，则有：

D_m＝D_n (18)

结合公式(11)(12)(15)(18)推导出如下关系式：

由矩阵构造过程可知，D_n是非奇异矩阵，则由公式(19)可知：

与

是相似矩阵；由相似矩阵的性质可知：相似矩阵具有相同的迹；而

是由全站仪测量得到的点坐标构造出来，是已知矩阵，令：

则有：

其中

由相似矩阵性质得：

2cosθ+1＝a₁₁+a₂₂+a₃₃ (22)

由公式(22)可知，θ由

的迹求得，令a＝a₁₁+a₂₂+a₃₃，得到刚体转角θ的表达式为：

本发明的有益效果是：本发明采用非接触式测量方式并基于双矢量定姿原理，可依据全站仪测得的刚体上标志点的两组坐标值，便捷的求解出刚体定轴转动的角度，为大运动范围下的刚体位姿参数测量提供了新的解决方法。

附图说明

图1为本发明提供的一种刚体空间定轴转动角度检测方法流程图。

图2为本发明实施例的全站仪测量定位原理示意图。

图3为本发明实施例的定义坐标系示意图。

图4为本发明实施例的特征点与矢量构造示意图。

具体实施方式

下面结合附图对本发明的实施例作进一步的说明。

本发明提供了一种刚体空间定轴转动角度检测方法，如图1所示，包括以下步骤：

S1、刚体表面标志点定位。

如图2所示，在刚体表面上设置三个标志点p₁、p₂和p₃，采用两个设置于不同位置的全站仪A、B分别对每个标志点进行测量，得到各标志点的水平角、垂直角，并根据全站仪的测量定位原理，由各标志点的水平角、垂直角得到该标志点的三维坐标值。

S2、定义世界坐标系与刚体转动起止位置坐标系。

如图3所示，以全站仪特征点测量数据的坐标系OXwYwZw作为世界坐标系；设刚体转动的初始位置和最终位置分别为m、n位置，以刚体转动轴作为Z轴分别建立n位置的刚体坐标系O1XnYnZn以及m位置的刚体坐标系O1XmYmZm；n位置的刚体坐标系O1XnYnZn相对于m位置的刚体坐标系O1XmYmZm存在角度为θ的转动量。由于刚体在空间的任意方位，总可以通过绕坐标轴的三次有限转动而达到，设刚体的初始位置坐标系O1XmYmZm相对于世界坐标系OXwYwZw存在回转角，俯仰角，偏转角的旋转量(用欧拉角表征姿态)。

S3、构造与表征刚体运动的矢量。

如图4所示，在刚体坐标系O1XmYmZm中，以刚体表面的三个标志点作为特征点p_mi,i＝1,2,3，对应世界坐标系OXwYwZw中的特征点p_wi,i＝1,2,3；由点p_m1连接点p_m2、p_m3，得到矢量L_m1、L_m2，相对应的由点p_w1连接点p_w2、p_w3，得到矢量L_w1、L_w2。

设世界坐标系OXwYwZw和刚体坐标系O1XmYmZm的转化关系为：

式中

表示刚体坐标系O1XmYmZm相对于世界坐标系OXwYwZw的旋转矩阵，T为刚体坐标系O1XmYmZm的原点在世界坐标系OXwYwZw中的三维坐标值。

相应的特征点p_mi和p_wi的转化关系是：

由世界坐标系OXwYwZw中的矢量构造方式有：

L_w1＝p_w2-p_w1,L_w2＝p_w3-p_w1 (3)

同理得刚体坐标系O1XmYmZm中有：

L_m1＝p_m2-p_m1,L_m2＝p_m3-p_m1 (4)

由公式(2)(3)(4)得：

显然，在同一坐标系下，由同一点出发构造的两个矢量是不平行的，为了构造所需的第三个矢量，选用两个矢量L_w1与L_w2进行叉乘，且满足如下关系：

为了方便表述，定义矢量J_w和J_m：

J_w＝L_w1*L_w2,J_m＝L_m1*L_m2 (7)

由公式(6)(7)得：

定义矩阵D_w和D_m：

D_w＝[L_w1 L_w2 J_w],D_m＝[L_m1 L_m2 J_m] (9)

由公式(5)(8)(9)得：

由矩阵D_w和D_m的构造过程可知，两个矩阵均为满秩矩阵，因此存在可逆矩阵，则由公式(10)得：

同理可知，刚体在n位置时，刚体坐标系O1XnYnZn相对于世界坐标系OXwYwZw的旋转矩阵

为：

其中：

D_w′＝[L_w′1 L_w′2 J_w′],D_n＝[L_n1 L_n2 J_n] (13)

在实际的刚体转动过程中，最终位置n相对初始位置m只存在定轴转动，且转动角度为θ，因此n位置的刚体坐标系O1XnYnZn相对于m位置的刚体坐标系O1XmYmZm的旋转矩阵

为：

由公式(11)(12)(14)得：

矢量模在坐标变换中具有不变性，因此存在如下关系：

|L_wi|＝|L_mi|＝|L_ni|,i＝1,2 (16)

由公式(15)(16)可知，存在6个坐标未知量和1个转动角度未知量，共7个未知量；由矩阵每个元素对应关系和矢量模相等的关系，共可列出13个关系式，直接求解较为复杂，因此本发明实施例中采用矩阵变换求解转动角θ。

S4、计算刚体转动角度。

由坐标系的建立方式可知，特征点相对于刚体坐标系是固定的，因此特征点在n位置和m位置的坐标数值上是相同的，即：

p_ni＝p_mi,i＝1,2,3 (17)

由于各特征点的坐标相同，则有：

D_m＝D_n (18)

结合公式(11)(12)(15)(18)推导出如下关系式：

由矩阵构造过程可知，D_n是非奇异矩阵，则由公式(19)可知：

与

是相似矩阵；由相似矩阵的性质可知：相似矩阵具有相同的迹；而

是由全站仪测量得到的点坐标构造出来，是已知矩阵，令：

则有：

其中

由相似矩阵性质得：

2cosθ+1＝a₁₁+a₂₂+a₃₃ (22)

由公式(22)可知，θ由

的迹求得，令a＝a₁₁+a₂₂+a₃₃，得到刚体转角θ的表达式为：

下面根据实际中的应用情况，对公式(23)的准确性进行验证：

(1)给定刚体坐标系与世界坐标系下的位姿关系、在刚体坐标系下三个点的坐标以及刚体的转角θ₀。

(2)依据坐标变换关系，计算得到两组世界坐标系下的三个点的坐标。

(3)依据理论推导过程，构造相关矩阵，由公式(23)得到转动角θ。

(4)判别θ和给定转角θ₀是否一致。

验证结果如下表所示，结果证明本发明提供的刚体空间定轴转动角度检测方法可行。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (3)

1.一种刚体空间定轴转动角度检测方法，其特征在于，包括以下步骤：

S1、刚体表面标志点定位；所述步骤S1具体为：

在刚体表面上设置三个标志点，采用两个设置于不同位置的全站仪分别对每个标志点进行测量，得到各标志点的水平角、垂直角，并根据全站仪的测量定位原理，由各标志点的水平角、垂直角得到该标志点的三维坐标值；

S2、定义世界坐标系与刚体转动起止位置坐标系；所述步骤S2具体为：

以全站仪特征点测量数据的坐标系OXwYwZw作为世界坐标系；设刚体转动的初始位置和最终位置分别为m、n位置，以刚体转动轴作为Z轴分别建立n位置的刚体坐标系O1XnYnZn以及m位置的刚体坐标系O1XmYmZm；n位置的刚体坐标系O1XnYnZn相对于m位置的刚体坐标系O1XmYmZm存在角度为θ的转动量；

S3、构造与表征刚体运动的矢量，得到刚体坐标系O1XnYnZn相对于世界坐标系OXwYwZw的旋转矩阵

表达式为：

S4、根据

表达式得到计算刚体转动角度的θ表达式为：

其中，a＝a₁₁+a₂₂+a₃₃，

x_ik为矩阵中的元素，y_kj为矩阵D_w＇中的元素，且矩阵

与D_w＇为全站仪测量得到的点坐标构造得到的。

2.根据权利要求1所述的刚体空间定轴转动角度检测方法，其特征在于，所述步骤S3具体为：

在刚体坐标系O1XmYmZm中，以刚体表面的三个标志点作为特征点p_mi,i＝1,2,3，对应世界坐标系OXwYwZw中的特征点p_wi,i＝1,2,3；由点p_m1连接点p_m2、p_m3，得到矢量L_m1、L_m2，相对应的由点p_w1连接点p_w2、p_w3，得到矢量L_w1、L_w2；

设世界坐标系OXwYwZw和刚体坐标系O1XmYmZm的转化关系为：

式中

表示刚体坐标系O1XmYmZm相对于世界坐标系OXwYwZw的旋转矩阵，T为刚体坐标系O1XmYmZm的原点在世界坐标系OXwYwZw中的三维坐标值；

相应的特征点p_mi和p_wi的转化关系是：

由世界坐标系OXwYwZw中的矢量构造方式有：

L_w1＝p_w2-p_w1,L_w2＝p_w3-p_w1 (3)

同理得刚体坐标系O1XmYmZm中有：

L_m1＝p_m2-p_m1,L_m2＝p_m3-p_m1 (4)

由公式(2)(3)(4)得：

构造所需的第三个矢量，选用两个矢量L_w1与L_w2进行叉乘，且满足如下关系：

定义矢量J_w和J_m：

J_w＝L_w1*L_w2,J_m＝L_m1*L_m2 (7)

由公式(6)(7)得：

定义矩阵D_w和D_m：

D_w＝[L_w1 L_w2 J_w],D_m＝[L_m1 L_m2 J_m] (9)

由公式(5)(8)(9)得：

由矩阵D_w和D_m的构造过程可知，两个矩阵均为满秩矩阵，因此存在可逆矩阵，则由公式(10)得：

同理可知，刚体在n位置时，刚体坐标系O1XnYnZn相对于世界坐标系OXwYwZw的旋转矩阵

为：

其中：

D_w′＝[L_w′1 L_w′2 J_w′],D_n＝[L_n1 L_n2 J_n] (13)

在实际的刚体转动过程中，最终位置n相对初始位置m只存在定轴转动，且转动角度为θ，因此n位置的刚体坐标系O1XnYnZn相对于m位置的刚体坐标系O1XmYmZm的旋转矩阵

为：

由公式(11)(12)(14)得：

矢量模在坐标变换中具有不变性，因此存在如下关系：

|L_wi|＝|L_mi|＝|L_ni|,i＝1,2 (16)。

3.根据权利要求2所述的刚体空间定轴转动角度检测方法，其特征在于，所述步骤S4具体为：

由坐标系的建立方式可知，特征点相对于刚体坐标系是固定的，因此特征点在n位置和m位置的坐标数值上是相同的，即：

p_ni＝p_mi,i＝1,2,3 (17)

由于各特征点的坐标相同，则有：

D_m＝D_n (18)

结合公式(11)(12)(15)(18)推导出如下关系式：

由矩阵构造过程可知，D_n是非奇异矩阵，则由公式(19)可知：

与

是相似矩阵；由相似矩阵的性质可知：相似矩阵具有相同的迹；而

是由全站仪测量得到的点坐标构造出来，是已知矩阵，令：

则有：

其中

由相似矩阵性质得：

2cosθ+1＝a₁₁+a₂₂+a₃₃ (22)

由公式(22)可知，θ由

的迹求得，令a＝a₁₁+a₂₂+a₃₃，得到刚体转角θ的表达式为：

Images