[发明专利]一种刚体空间定轴转动角度检测方法

权利要求书

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₃₃，得到刚体转角θ的表达式为：