[發(fā)明專利]空間六自由度運(yùn)動(dòng)的測(cè)量裝置及動(dòng)態(tài)測(cè)量方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201210203199.9
• 申請(qǐng)日: 2012-06-19
• 公開（公告）號(hào): CN102692201A
• 公開（公告）日: 2012-09-26
• 發(fā)明（設(shè)計(jì)）人: 謝志江;張高峰;尹兵;江登林;陳平;宋代平;雷鋼;孫小勇
• 申請(qǐng)（專利權(quán)）人: 重慶大學(xué);貴州航天天馬機(jī)電科技有限公司
• 主分類號(hào): G01B21/04
• 分類號(hào): G01B21/04;G01B21/22
• 代理公司: 重慶市前沿專利事務(wù)所 50211
• 代理人: 方洪

1.一種空間六自由度運(yùn)動(dòng)的測(cè)量裝置，其特征在于：在定平臺(tái)(1)上裝有六個(gè)虎克鉸(2)，這六個(gè)虎克鉸(2)圍成一個(gè)六角形；在所述定平臺(tái)(1)的上方設(shè)有動(dòng)平臺(tái)(10)，該動(dòng)平臺(tái)(10)上裝有六個(gè)球鉸(9)，六個(gè)球鉸(9)也圍成一個(gè)六角形，并且六個(gè)球鉸(9)與六個(gè)虎克鉸(2)一一對(duì)應(yīng)；所述六個(gè)球鉸(9)均通過支鏈與對(duì)應(yīng)的六個(gè)虎克鉸(2)相連接，該支鏈包括上支桿(8)、下支桿(3)和旋轉(zhuǎn)副(6)，所述上支桿(8)的上端與對(duì)應(yīng)的球鉸(9)連接，上支桿(8)的下端通過旋轉(zhuǎn)副(6)與下支桿(3)的上端鉸接，在旋轉(zhuǎn)副(6)上安裝有旋轉(zhuǎn)編碼器(5)，所述下支桿(3)的下端與對(duì)應(yīng)的虎克鉸(2)連接，同一支鏈上的旋轉(zhuǎn)副(6)位于該支鏈所對(duì)應(yīng)的球鉸(9)與虎克鉸(2)中心連線的外側(cè)，同一支鏈所對(duì)應(yīng)的球鉸(9)與虎克鉸(2)中心連線同對(duì)應(yīng)的旋轉(zhuǎn)副(6)位于一個(gè)平面內(nèi)，該支鏈只能在這個(gè)平面內(nèi)運(yùn)動(dòng)。

2.根據(jù)權(quán)利要求1所述的空間六自由度運(yùn)動(dòng)的測(cè)量裝置，其特征在于：所述上支桿(8)和下支桿(3)的結(jié)構(gòu)相同，均由螺紋套筒、螺桿和鎖定螺釘組成，螺桿的一端伸入螺紋套筒內(nèi)，兩者之間螺紋配合，并通過徑向穿設(shè)的鎖定螺釘頂緊；所述上支桿(8)的螺桿與球鉸(9)連接，上支桿(8)的螺紋套筒通過旋轉(zhuǎn)副(6)與下支桿(3)的螺紋套筒鉸接，下支桿(3)的螺桿與虎克鉸(2)連接。

3.根據(jù)權(quán)利要求2所述的空間六自由度運(yùn)動(dòng)的測(cè)量裝置，其特征在于：所述定平臺(tái)(1)為正六邊形，虎克鉸(2)安裝在定平臺(tái)(1)的六個(gè)角處；所述動(dòng)平臺(tái)(10)也為正六邊形，并位于定平臺(tái)(1)的正上方，所述球鉸(9)安裝在動(dòng)平臺(tái)(10)的六個(gè)角處。

4.一種利用權(quán)利要求1-3任一所述測(cè)量裝置進(jìn)行空間六自由度運(yùn)動(dòng)的動(dòng)態(tài)測(cè)量方法，其特征在于包括以下步驟：

1)將被測(cè)運(yùn)動(dòng)物體剛性固定在動(dòng)平臺(tái)(10)上，定平臺(tái)(1)剛性固定在相對(duì)靜止不動(dòng)的物體上，六個(gè)球鉸(9)分別編號(hào)為S₁、S₂、S₃、S₄、S₅、S₆，六個(gè)虎克鉸(2)分別編號(hào)為U₁、U₂、U₃、U₄、U₅、U₆；在被測(cè)運(yùn)動(dòng)物體上設(shè)定空間直角正交體坐標(biāo)系o-xyz，在動(dòng)平臺(tái)上球鉸(9)所圍成六角形的幾何中心設(shè)定空間直角正交體坐標(biāo)系動(dòng)坐標(biāo)系S-X'Y'Z'，S-X'Y'Z'與動(dòng)平臺(tái)固連，其原點(diǎn)S與動(dòng)平臺(tái)的形心重合，Z′軸垂直于動(dòng)平臺(tái)向上，X′軸與S₁S₆垂直，Y′軸平行于S₁S₆，在定平臺(tái)上虎克鉸(2)所圍成六角形的幾何中心設(shè)定空間直角正交體坐標(biāo)系O-XYZ，其原點(diǎn)O與定平臺(tái)的形心重合，Z軸垂直向上，Y軸與U₁U₆垂直，X軸平行于U₁U₆；

2)被測(cè)運(yùn)動(dòng)物體空間六自由度的運(yùn)動(dòng)帶動(dòng)動(dòng)平臺(tái)(10)運(yùn)動(dòng)及旋轉(zhuǎn)編碼器(5)轉(zhuǎn)動(dòng)，旋轉(zhuǎn)編碼器(5)測(cè)量旋轉(zhuǎn)副(6)的上支桿(8)與下支桿(3)的相對(duì)運(yùn)動(dòng)的角度時(shí)間曲線θ_i(t)，i=1，2，3，4，5，6；

3)由旋轉(zhuǎn)編碼器(5)的角度時(shí)間曲線正解，得出被測(cè)運(yùn)動(dòng)物體空間六自由度的運(yùn)動(dòng)規(guī)律。

5.根據(jù)權(quán)利要求4所述空間六自由度運(yùn)動(dòng)的動(dòng)態(tài)測(cè)量方法，其特征在于：

步驟3)包含如下過程：

a、運(yùn)動(dòng)分解：

動(dòng)平臺(tái)的運(yùn)動(dòng)可分解為隨S-X'Y'Z'坐標(biāo)原點(diǎn)S沿O-XYZ三個(gè)坐標(biāo)軸方向上的平移(X(t)，Y(t)，Z(t))^T，以及繞坐標(biāo)軸的旋轉(zhuǎn)(α(t)，β(t)，γ(t))^T；被測(cè)運(yùn)動(dòng)物體可分解為隨o-xyz坐標(biāo)原點(diǎn)o沿S-X'Y'Z'三個(gè)坐標(biāo)軸方向上的平移(x(t)，y(t)，z(t))^T，以及繞坐標(biāo)軸的旋轉(zhuǎn)(θ_x(t)，θ_y(t)，θ_z(t))^T；

b、坐標(biāo)變換：

S_i(i=1，2，3，4，5，6)表示動(dòng)平臺(tái)上的各球鉸點(diǎn)，U_i(i＝1，2，3，4，5，6)表示定動(dòng)平臺(tái)上的各虎克鉸點(diǎn)；S_i(i＝1，2，3，4，5，6)在固定空間直角坐標(biāo)系O-XYZ和動(dòng)空間直角坐標(biāo)系S-X'Y'Z'中的坐標(biāo)向量表示分別為S(S_iX，S_iY，S_iZ)^T、S(S_iX′，S_iY′，S_iZ′)^T；U_i(i＝1，2，3，4，5，6)在固定空間直角坐標(biāo)系O-XYZ中的坐標(biāo)向量表示分別為U(U_iX，U_iY，U_iZ)^T，l_i表示桿S_iU_i的長(zhǎng)度(i＝1，2，3，4，5，6)；靜坐標(biāo)向量S(S_iX，S_iY，S_iZ)^T和坐標(biāo)向量S(S_iX′，S_iY′，S_iZ′)^T有如下變換公式：

SiXSiYSiZ=[T]×SiX′SiX′SiX′+XYZ,]]>i＝1，2，3，4，5，6

其中T為坐標(biāo)變換公式，這里有：

[T]=cosβcosγ-cosαsinγ+sinαsinβcosγsinαsinγ+cosαsinβcosγcosβsinγcosαcosγ+sinαsinβsinγ-sinαcosγ+cosαsinβsinγ-sinβsinαcosβcosαcosβ]]>

根據(jù)上下支桿旋轉(zhuǎn)角度的變化，利用機(jī)構(gòu)運(yùn)動(dòng)學(xué)算法解算出動(dòng)平臺(tái)的六自由度運(yùn)動(dòng)變化，得出動(dòng)平臺(tái)的空間位姿V＝(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T，通過坐標(biāo)變換得到被測(cè)運(yùn)動(dòng)物體的六自由度運(yùn)動(dòng)變化，確定被測(cè)物體的空間位姿W＝(x(t)，y(t)，z(t)，θ_x(t)，θ_y(t)，θ_z(t))^T；通過旋轉(zhuǎn)編碼器電機(jī)上的編碼記錄的旋轉(zhuǎn)角度，能夠精確地的得出上球鉸與下虎克鉸距離的變化Δl_i(t)(i＝1，2，3，4，5，6)；則上球鉸與下虎克鉸的距離可表示為：(i＝1，2，3，4，5，6)，其中l(wèi)_i(t)表示狀態(tài)角度為θ時(shí)上球鉸與下虎克鉸的距離，l_下為轉(zhuǎn)動(dòng)副中心到虎克鉸中心的距離，l_上為轉(zhuǎn)動(dòng)副中心到球鉸中心的距離，則動(dòng)平臺(tái)位姿(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T和六組球鉸及胡可鉸的距離l_i(t)的非線性約束方程式為：

F_i(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))=l_i²-[(S_ix-U_ix)²+(S_iy-U_iy)²+(S_iz-U_iz)²]＝0，(i＝1，2，3，4，5，6)

c、迭代解法：

令V＝(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T，F(xiàn)(V)=(f₁(V)，f₂(V)，f₃(V)，f₄(V)，f₅(V)，f₆(V))^T，選擇初始點(diǎn)V₀，F(xiàn)_i(V₀)(i＝1，2，3，4，5，6)在V₀附近進(jìn)行泰勒展開，取得其線性部分作為動(dòng)平臺(tái)姿態(tài)的牛頓迭帶公式：

V_k+1=V_k-[J(V_k)]^-1F(V_k)(k＝0，1，2，...)

其中

經(jīng)過多次迭代后，當(dāng)滿足|V_k+1-V_k|<E時(shí)終止迭代，此時(shí)的V_k+1便是滿足精度要求的動(dòng)平臺(tái)空間位姿V=(X(t)，Y(t)，Z(t)，α(t)，β(t)，γ(t))^T的值，再經(jīng)坐標(biāo)轉(zhuǎn)換矩陣的轉(zhuǎn)換、平移，可得被測(cè)運(yùn)動(dòng)物體的空間位姿W＝(x(t)、y(t)、z(t)，θ_x(t)、θ_y(t)、θ_z(t))^T；被測(cè)運(yùn)動(dòng)物體上的空間直角坐標(biāo)系相對(duì)動(dòng)平臺(tái)上坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣T′：

T′=cosθycosθz-cosθxsinθz+sinθxsinθycosθzsinθxsinθz+cosθxsinθycosθzcosθysinθzcosθxcosθz+sinθxsinθysinθz-sinθxcosθz+cosθxsinθysinθz-sinθysinθxcosθycosθxcosθy.]]>