[發(fā)明專利]一種剛體空間定軸轉(zhuǎn)動(dòng)角度檢測(cè)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201710001869.1
• 申請(qǐng)日: 2017-01-03
• 公開(kāi)（公告）號(hào): CN106871859B
• 公開(kāi)（公告）日: 2020-07-03
• 發(fā)明（設(shè)計(jì)）人: 丁杰雄;司朝陽(yáng);羅欣;胡維博;李菲;畢丹煬;丁啟程
• 申請(qǐng)（專利權(quán)）人: 電子科技大學(xué)
• 主分類號(hào): G01C1/00
• 分類號(hào): G01C1/00
• 代理公司: 成都虹盛匯泉專利代理有限公司 51268
• 代理人: 王偉

1.一種剛體空間定軸轉(zhuǎn)動(dòng)角度檢測(cè)方法，其特征在于，包括以下步驟：

S1、剛體表面標(biāo)志點(diǎn)定位；所述步驟S1具體為：

在剛體表面上設(shè)置三個(gè)標(biāo)志點(diǎn)，采用兩個(gè)設(shè)置于不同位置的全站儀分別對(duì)每個(gè)標(biāo)志點(diǎn)進(jìn)行測(cè)量，得到各標(biāo)志點(diǎn)的水平角、垂直角，并根據(jù)全站儀的測(cè)量定位原理，由各標(biāo)志點(diǎn)的水平角、垂直角得到該標(biāo)志點(diǎn)的三維坐標(biāo)值；

S2、定義世界坐標(biāo)系與剛體轉(zhuǎn)動(dòng)起止位置坐標(biāo)系；所述步驟S2具體為：

以全站儀特征點(diǎn)測(cè)量數(shù)據(jù)的坐標(biāo)系OXwYwZw作為世界坐標(biāo)系；設(shè)剛體轉(zhuǎn)動(dòng)的初始位置和最終位置分別為m、n位置，以剛體轉(zhuǎn)動(dòng)軸作為Z軸分別建立n位置的剛體坐標(biāo)系O1XnYnZn以及m位置的剛體坐標(biāo)系O1XmYmZm；n位置的剛體坐標(biāo)系O1XnYnZn相對(duì)于m位置的剛體坐標(biāo)系O1XmYmZm存在角度為θ的轉(zhuǎn)動(dòng)量；

S3、構(gòu)造與表征剛體運(yùn)動(dòng)的矢量，得到剛體坐標(biāo)系O1XnYnZn相對(duì)于世界坐標(biāo)系OXwYwZw的旋轉(zhuǎn)矩陣表達(dá)式為：

S4、根據(jù)表達(dá)式得到計(jì)算剛體轉(zhuǎn)動(dòng)角度的θ表達(dá)式為：

其中，a＝a₁₁+a₂₂+a₃₃，x_ik為矩陣中的元素，y_kj為矩陣D_w＇中的元素，且矩陣與D_w＇為全站儀測(cè)量得到的點(diǎn)坐標(biāo)構(gòu)造得到的。

2.根據(jù)權(quán)利要求1所述的剛體空間定軸轉(zhuǎn)動(dòng)角度檢測(cè)方法，其特征在于，所述步驟S3具體為：

在剛體坐標(biāo)系O1XmYmZm中，以剛體表面的三個(gè)標(biāo)志點(diǎn)作為特征點(diǎn)p_mi,i＝1,2,3，對(duì)應(yīng)世界坐標(biāo)系OXwYwZw中的特征點(diǎn)p_wi,i＝1,2,3；由點(diǎn)p_m1連接點(diǎn)p_m2、p_m3，得到矢量L_m1、L_m2，相對(duì)應(yīng)的由點(diǎn)p_w1連接點(diǎn)p_w2、p_w3，得到矢量L_w1、L_w2；

設(shè)世界坐標(biāo)系OXwYwZw和剛體坐標(biāo)系O1XmYmZm的轉(zhuǎn)化關(guān)系為：

式中表示剛體坐標(biāo)系O1XmYmZm相對(duì)于世界坐標(biāo)系OXwYwZw的旋轉(zhuǎn)矩陣，T為剛體坐標(biāo)系O1XmYmZm的原點(diǎn)在世界坐標(biāo)系OXwYwZw中的三維坐標(biāo)值；

相應(yīng)的特征點(diǎn)p_mi和p_wi的轉(zhuǎn)化關(guān)系是：

由世界坐標(biāo)系OXwYwZw中的矢量構(gòu)造方式有：

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

同理得剛體坐標(biāo)系O1XmYmZm中有：

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

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

構(gòu)造所需的第三個(gè)矢量，選用兩個(gè)矢量L_w1與L_w2進(jìn)行叉乘，且滿足如下關(guān)系：

定義矢量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的構(gòu)造過(guò)程可知，兩個(gè)矩陣均為滿秩矩陣，因此存在可逆矩陣，則由公式(10)得：

同理可知，剛體在n位置時(shí)，剛體坐標(biāo)系O1XnYnZn相對(duì)于世界坐標(biāo)系OXwYwZw的旋轉(zhuǎn)矩陣為：

其中：

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

在實(shí)際的剛體轉(zhuǎn)動(dòng)過(guò)程中，最終位置n相對(duì)初始位置m只存在定軸轉(zhuǎn)動(dòng)，且轉(zhuǎn)動(dòng)角度為θ，因此n位置的剛體坐標(biāo)系O1XnYnZn相對(duì)于m位置的剛體坐標(biāo)系O1XmYmZm的旋轉(zhuǎn)矩陣為：

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

矢量模在坐標(biāo)變換中具有不變性，因此存在如下關(guān)系：

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

3.根據(jù)權(quán)利要求2所述的剛體空間定軸轉(zhuǎn)動(dòng)角度檢測(cè)方法，其特征在于，所述步驟S4具體為：

由坐標(biāo)系的建立方式可知，特征點(diǎn)相對(duì)于剛體坐標(biāo)系是固定的，因此特征點(diǎn)在n位置和m位置的坐標(biāo)數(shù)值上是相同的，即：

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

由于各特征點(diǎn)的坐標(biāo)相同，則有：

D_m＝D_n (18)

結(jié)合公式(11)(12)(15)(18)推導(dǎo)出如下關(guān)系式：

由矩陣構(gòu)造過(guò)程可知，D_n是非奇異矩陣，則由公式(19)可知：與是相似矩陣；由相似矩陣的性質(zhì)可知：相似矩陣具有相同的跡；而是由全站儀測(cè)量得到的點(diǎn)坐標(biāo)構(gòu)造出來(lái)，是已知矩陣，令：

則有：

其中由相似矩陣性質(zhì)得：

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

由公式(22)可知，θ由的跡求得，令a＝a₁₁+a₂₂+a₃₃，得到剛體轉(zhuǎn)角θ的表達(dá)式為：