專利名稱：剛體空間位姿測(cè)量裝置及其測(cè)量方法
技術(shù)領(lǐng)域：
本發(fā)明屬于測(cè)控技術(shù)領(lǐng)域，涉及一種用于測(cè)量剛體空間六自由度位姿的方法及實(shí)現(xiàn)裝置。
背景技術(shù)：
在笛卡爾坐標(biāo)系中，剛體具有六個(gè)自由度。在機(jī)床、機(jī)構(gòu)、機(jī)器人等的工業(yè)控制中，為了實(shí)現(xiàn)高精度的定位，通?？刂茣?huì)采用閉環(huán)反饋控制，而這就對(duì)空間位姿的動(dòng)態(tài)測(cè)量提出了要求。目前采用的閉環(huán)反饋控制中，測(cè)量主要有兩種方式測(cè)量執(zhí)行機(jī)構(gòu)關(guān)節(jié)的位移(或角位移)進(jìn)行反饋或直接測(cè)量終端的位姿進(jìn)行反饋。
在測(cè)量執(zhí)行機(jī)構(gòu)關(guān)節(jié)位移(或角位移)進(jìn)行反饋的方案中，又可分為半閉環(huán)反饋與全閉環(huán)反饋兩種方法。半閉環(huán)反饋僅測(cè)量電機(jī)或絲杠的位移(或角位移)，忽略之后的傳動(dòng)系統(tǒng)誤差，方案的優(yōu)點(diǎn)是成本低，便于實(shí)現(xiàn)，缺點(diǎn)是忽略了機(jī)械傳動(dòng)系統(tǒng)的誤差，精度低；全閉環(huán)反饋需測(cè)量實(shí)際機(jī)構(gòu)關(guān)節(jié)的位移(或角位移)，常用的儀器是光柵尺等，這一方案的優(yōu)點(diǎn)是精度高，缺點(diǎn)是造價(jià)高，對(duì)環(huán)境有一定的要求。上述兩種方案存在的一個(gè)共同問題是，當(dāng)被測(cè)剛體位姿不能全部可控時(shí)，這種測(cè)量方案無(wú)法完成測(cè)量要求。
直接測(cè)量終端位姿的方法，通常是采用光學(xué)測(cè)量?jī)x器進(jìn)行非接觸式測(cè)量，如采用三臺(tái)光學(xué)儀器測(cè)量剛體上三個(gè)點(diǎn)的位置信息即可得到剛體的六自由度位姿信息。但是普通的光學(xué)測(cè)量設(shè)備實(shí)時(shí)性差，而可以滿足實(shí)時(shí)性要求時(shí)的光學(xué)測(cè)量設(shè)備，成本又成幾十倍的增加。另外光學(xué)測(cè)量?jī)x器對(duì)環(huán)境以及被測(cè)剛體的空間有一定的要求。
Stewart平臺(tái)機(jī)構(gòu)是最適宜用于實(shí)現(xiàn)空間六自由度運(yùn)動(dòng)的并聯(lián)機(jī)構(gòu)，它出現(xiàn)于二十世紀(jì)六十年代，通用Stewart平臺(tái)由一個(gè)固定平臺(tái)、一個(gè)運(yùn)動(dòng)平臺(tái)和聯(lián)接于固定平臺(tái)和運(yùn)動(dòng)平臺(tái)之間的六個(gè)執(zhí)行器組成。通過驅(qū)動(dòng)六個(gè)執(zhí)行器，使運(yùn)動(dòng)平臺(tái)實(shí)現(xiàn)空間六自由度運(yùn)動(dòng)。目前的Stewart平臺(tái)機(jī)構(gòu)都是用于主動(dòng)控制中。

發(fā)明內(nèi)容
本發(fā)明的目的在于克服傳統(tǒng)位置及姿態(tài)測(cè)量裝置的不足之處，提出一種接觸式的低成本、高精度的剛體空間位置和姿態(tài)的測(cè)量裝置及測(cè)量方法。本發(fā)明采用拉線式編碼器取代通用Stewart平臺(tái)機(jī)構(gòu)中的六個(gè)執(zhí)行器，使主動(dòng)控制變?yōu)楸粍?dòng)測(cè)量，從而獲得被測(cè)運(yùn)動(dòng)剛體的位姿信息。
本發(fā)明公開的剛體空間位姿測(cè)量裝置，其特征在于該測(cè)量裝置包括測(cè)量執(zhí)行機(jī)構(gòu)、數(shù)據(jù)采集裝置以及存儲(chǔ)計(jì)算程序的計(jì)算機(jī)；所述測(cè)量執(zhí)行機(jī)構(gòu)包括一個(gè)固定平臺(tái)及固定在所述固定平臺(tái)上的6個(gè)球鉸，一個(gè)與運(yùn)動(dòng)剛體固連的運(yùn)動(dòng)平臺(tái)及固定在所述運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸，兩端分別與固定平臺(tái)上的6個(gè)球鉸和運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸相連的六個(gè)拉線式編碼器；所述數(shù)據(jù)采集裝置包括分別與拉線式編碼器輸出端相連的六個(gè)計(jì)數(shù)器，與所述各計(jì)數(shù)器輸出端連接的多路復(fù)用器，以及與所述多路復(fù)用器相連的數(shù)據(jù)發(fā)送電路；所述數(shù)據(jù)發(fā)送電路與所述計(jì)算機(jī)相應(yīng)輸入端口連接；所述各球鉸在各自平臺(tái)的位置坐標(biāo)滿足測(cè)量執(zhí)行機(jī)構(gòu)雅克比矩陣Jp的條件數(shù)小于等于10，Jp=l→1T(Rp1×l→1)T|p1|......l→6T(Rp6×l→6)T|p6|∈R6×6,]]>式中 分別是沿每個(gè)拉線式編碼器長(zhǎng)度方向上的單位向量，p1～p6是運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系p中的坐標(biāo)，R為運(yùn)動(dòng)平臺(tái)坐標(biāo)系p關(guān)于固定平臺(tái)坐標(biāo)系P的方向余弦陣，按照依次繞固定平臺(tái)坐標(biāo)系P的x，y，z軸旋轉(zhuǎn)角度α，β，γ的卡爾丹角表示方法，具體表達(dá)式為R=cosβcosγ-cosβsinγsinβsinαsinβcosγ+cosαsinγ-sinαsinβsinγ+cosαcosγ-sinαcosβ-cosαsinβcosγ+sinαsinγcosαcosβsinγ+sinαcosγcosαcosβ.]]>本發(fā)明的一種改進(jìn)為所述的固定在運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸直接固定在運(yùn)動(dòng)剛體上，省去所述運(yùn)動(dòng)平臺(tái)。
本發(fā)明所述拉線式編碼器是數(shù)字式拉線式編碼器。
本發(fā)明的一種改進(jìn)為所述拉線式編碼器是模擬式拉線式編碼器，該剛體空間位姿測(cè)量裝置還包括一個(gè)A/D采集卡，所述A/D采集卡的輸入端與所述模擬式拉線式編碼器的輸出端相連，輸出端分別與六個(gè)計(jì)數(shù)器相連。
本發(fā)明還公開了一種所述測(cè)量裝置進(jìn)行剛體空間位姿測(cè)量的方法，其特征在于該測(cè)量方法包括如下步驟1)系統(tǒng)初始化，即確定各球鉸在各自平臺(tái)坐標(biāo)系中的坐標(biāo)，拉線式編碼器的初始長(zhǎng)度以及被測(cè)剛體在固定平臺(tái)坐標(biāo)系中的初始坐標(biāo)，并輸入所述計(jì)算機(jī)；2)當(dāng)被測(cè)剛體運(yùn)動(dòng)時(shí)，利用數(shù)據(jù)采集裝置采集拉線式編碼器的長(zhǎng)度變化信號(hào)，并傳送到計(jì)算機(jī)中；3)以被測(cè)剛體在固定平臺(tái)坐標(biāo)系中的初始坐標(biāo)為方程組初始值，計(jì)算機(jī)中的程序求解下述迭代方程組，得到被測(cè)剛體的空間姿態(tài)信息及位置信息Li-|R·pi+T-Pi|＝0，i＝1，…，6；其中，L1～L6為6個(gè)拉線式編碼器得到的測(cè)量長(zhǎng)度，Pi是固定平臺(tái)上的6個(gè)球鉸在固定平臺(tái)坐標(biāo)系P中的坐標(biāo)，pi是運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系p中的坐標(biāo)，R為運(yùn)動(dòng)平臺(tái)坐標(biāo)系p關(guān)于固定平臺(tái)坐標(biāo)系P的方向余弦陣，包含了待求的空間姿態(tài)信息，T＝[x y z]T是運(yùn)動(dòng)平臺(tái)坐標(biāo)系p的原點(diǎn)在固定平臺(tái)坐標(biāo)系P中的位置，包含了待求的空間位置信息；4)將當(dāng)前運(yùn)動(dòng)剛體的位姿作為迭代方程組的初始值，實(shí)現(xiàn)連續(xù)測(cè)量。
本發(fā)明相比已有的技術(shù)具有如下的優(yōu)點(diǎn)第一，本發(fā)明裝置采用Stewart機(jī)構(gòu)的連接方式，實(shí)現(xiàn)結(jié)構(gòu)簡(jiǎn)單。
第二，本裝置直接測(cè)量被測(cè)剛體的位姿，同時(shí)系統(tǒng)實(shí)時(shí)性較高，所以能夠?qū)崿F(xiàn)對(duì)被測(cè)剛體的全閉環(huán)實(shí)時(shí)控制。
第三，拉線式編碼器成本低，使用可靠，精度可達(dá)1微米，能滿足一般精度的測(cè)量要求。
第四，通過合理的球鉸位置選擇，使機(jī)構(gòu)對(duì)姿態(tài)比較敏感，測(cè)量精度得到提高。
第五，測(cè)量時(shí)操作簡(jiǎn)單、快捷，對(duì)環(huán)境及被測(cè)剛體的空間要求不高，適用于多種場(chǎng)合。


圖1是本發(fā)明所述的剛體空間位姿測(cè)量裝置的系統(tǒng)構(gòu)成示意圖。
圖2是剛體空間位姿測(cè)量方法的流程圖。
圖3a是本發(fā)明的一個(gè)實(shí)施例的結(jié)構(gòu)原理框圖。
圖3b是本發(fā)明的另一個(gè)實(shí)施例的結(jié)構(gòu)原理框圖。
具體實(shí)施例方式
結(jié)合附圖具體描述本發(fā)明所述的運(yùn)動(dòng)剛體位姿的測(cè)量方法與測(cè)量裝置。
如圖1所述，本發(fā)明所述剛體空間位姿測(cè)量裝置包括測(cè)量執(zhí)行機(jī)構(gòu)、數(shù)據(jù)采集裝置以及存儲(chǔ)計(jì)算程序的計(jì)算機(jī)；所述測(cè)量執(zhí)行機(jī)構(gòu)包括固定平臺(tái)1，安裝在固定平臺(tái)上的六個(gè)球鉸101～106；運(yùn)動(dòng)平臺(tái)2，安裝在運(yùn)動(dòng)平臺(tái)上的六個(gè)球鉸201～206；相應(yīng)連接球鉸101～106與201～206的拉線式編碼器301～306；數(shù)據(jù)采集裝置4的輸入端與拉線式編碼器的輸出端連接，數(shù)據(jù)采集裝置4的輸出端與計(jì)算機(jī)5的相應(yīng)輸入端口連接。
固定平臺(tái)1，用來安裝六個(gè)球鉸101～106，使球鉸位置固定，在固定平臺(tái)上建立一個(gè)固定不變的固定平臺(tái)坐標(biāo)系，精確測(cè)定球鉸101～106在固定平臺(tái)坐標(biāo)系中的坐標(biāo)。在具體實(shí)施例中，固定平臺(tái)1的具體形式可為平板也可為框架結(jié)構(gòu)，根據(jù)被測(cè)剛體的運(yùn)動(dòng)范圍及角度，固定平臺(tái)可安裝在運(yùn)動(dòng)剛體的上方或者下方。
運(yùn)動(dòng)平臺(tái)2，用來安裝六個(gè)球鉸201～206，同時(shí)另一端與所需測(cè)量的運(yùn)動(dòng)剛體固連。在運(yùn)動(dòng)平臺(tái)上建立一個(gè)運(yùn)動(dòng)平臺(tái)坐標(biāo)系，精確測(cè)定球鉸201～206在運(yùn)動(dòng)平臺(tái)坐標(biāo)系中的坐標(biāo)。當(dāng)所需測(cè)量的剛體上比較容易安裝球鉸時(shí)，可省略掉運(yùn)動(dòng)平臺(tái)2，直接將六個(gè)球鉸201～206安裝在所需測(cè)量的剛體上，并在被測(cè)剛體上建立運(yùn)動(dòng)平臺(tái)坐標(biāo)系。本實(shí)施例中由于被測(cè)剛體面積較小不易安裝球鉸，故采用運(yùn)動(dòng)平臺(tái)2。
各球鉸在各自平臺(tái)的位置應(yīng)使被測(cè)剛體在運(yùn)動(dòng)范圍內(nèi)時(shí)，測(cè)量執(zhí)行機(jī)構(gòu)的雅克比矩陣條件數(shù)滿足要求。雅克比矩陣表示運(yùn)動(dòng)平臺(tái)的位姿誤差量和支鏈長(zhǎng)度誤差量間的映射關(guān)系，其條件數(shù)反映了所設(shè)計(jì)的機(jī)構(gòu)在空間中各個(gè)方向上精度的比例，是衡量測(cè)量精度的一個(gè)重要指標(biāo)。根據(jù)研究發(fā)現(xiàn)，當(dāng)雅克比矩陣的條件數(shù)大于10時(shí)，最終的測(cè)量精度將急劇下降，說明球鉸坐標(biāo)設(shè)計(jì)不合理，需重新設(shè)計(jì)球鉸位置。本發(fā)明所述剛體空間位姿測(cè)量裝置中的測(cè)量執(zhí)行機(jī)構(gòu)的雅克比矩陣的條件數(shù)小于等于10，優(yōu)選在4～5以內(nèi)。雅克比矩陣的具體表達(dá)式為Jp=l→1T(Rp1×l→1)T|p1|......l→6T(Rp6×l→6)T|p6|∈R6×6,]]>式中 分別是沿每個(gè)拉線式編碼器長(zhǎng)度方向上的單位向量，p1～p6是運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系p中的坐標(biāo)，R為運(yùn)動(dòng)平臺(tái)坐標(biāo)系p關(guān)于固定平臺(tái)坐標(biāo)系P的方向余弦陣，按照依次繞固定平臺(tái)坐標(biāo)系P的x，y，z軸旋轉(zhuǎn)角度α，β，γ的卡爾丹角表示方法，具體表達(dá)式為R=cosβcosγ-cosβsinγsinβsinαsinβcosγ+cosαsinγ-sinαsinβsinγ+cosαcosγ-sinαcosβ-cosαsinβcosγ+sinαsinγcosαcosβsinγ+sinαcosγcosαcosβ.]]>拉線式編碼器301～306，兩端分別與固定平臺(tái)球鉸和運(yùn)動(dòng)平臺(tái)球鉸相連，當(dāng)被測(cè)剛體運(yùn)動(dòng)時(shí)通過編碼器記錄下對(duì)應(yīng)球鉸間長(zhǎng)度變化的情況。數(shù)據(jù)采集裝置4將拉線式編碼器301～306的數(shù)據(jù)轉(zhuǎn)換成計(jì)算機(jī)可以接收的形式，統(tǒng)一將數(shù)據(jù)傳輸至存儲(chǔ)有位姿解算程序的計(jì)算機(jī)5中。
拉線式編碼器在輸出數(shù)據(jù)方式上可為數(shù)字式或模擬式。當(dāng)采用數(shù)字式的拉線式編碼器時(shí)，見圖3a，數(shù)據(jù)采集裝置4包括六個(gè)記錄編碼器變化量的計(jì)數(shù)器，將六路信號(hào)匯合成一路信號(hào)的多路復(fù)用器以及與多路復(fù)用器相連的將匯合后的信號(hào)發(fā)送給計(jì)算機(jī)的數(shù)據(jù)發(fā)送電路。數(shù)字式拉線式編碼器輸出端與六個(gè)計(jì)數(shù)器相連，各計(jì)數(shù)器輸出端與多路復(fù)用器連接，數(shù)據(jù)發(fā)送電路與所述計(jì)算機(jī)相應(yīng)輸入端口連接。當(dāng)采用模擬式拉線式編碼器時(shí)，見圖3b，該剛體空間位姿測(cè)量裝置還包括一個(gè)將模擬信號(hào)轉(zhuǎn)換為數(shù)字信號(hào)的A/D采集卡，所述A/D采集卡的輸入端與所述模擬式拉線式編碼器的輸出端相連，輸出端分別與數(shù)據(jù)采集裝置4中的六個(gè)計(jì)數(shù)器相連。
拉線式編碼器除輸出方式外，在計(jì)數(shù)方面具體可采用增量式或絕對(duì)式，電路特性上，可選擇電壓型、電流型等，在具體實(shí)施例中，可選用封裝完整的成品拉線式編碼器以及多路復(fù)用器，如HONTKO公司的數(shù)字增量式電壓型拉線式編碼器HLS-M-1005，74151多路復(fù)用器等。數(shù)據(jù)采集裝置的數(shù)據(jù)發(fā)送電路可選用通用的串口發(fā)送電路，并口發(fā)送電路等計(jì)算機(jī)可接收的方式。
存儲(chǔ)了位姿解算算法的計(jì)算機(jī)5，將拉線式編碼器的數(shù)據(jù)以及球鉸101～106在固定平臺(tái)坐標(biāo)系中的坐標(biāo)和201～206球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系中的坐標(biāo)結(jié)合起來，通過求解機(jī)構(gòu)的運(yùn)動(dòng)學(xué)方程得到運(yùn)動(dòng)平臺(tái)相對(duì)固定平臺(tái)的位置和姿態(tài)。
如圖2所示。本發(fā)明所述的剛體空間位姿測(cè)量方法的流程如下(1)系統(tǒng)初始化這部分主要對(duì)后續(xù)測(cè)量和計(jì)算中應(yīng)用到的參數(shù)進(jìn)行確定，其中包括球鉸101～106以及201～206分別在固定平臺(tái)坐標(biāo)系和運(yùn)動(dòng)平臺(tái)坐標(biāo)系中的坐標(biāo)，拉線式編碼器的初始長(zhǎng)度以及用于迭代求解方程的被測(cè)剛體初始情況下在固定平臺(tái)坐標(biāo)系中的坐標(biāo)，并將所述坐標(biāo)輸入計(jì)算機(jī)；(2)將拉線式編碼器測(cè)量的數(shù)據(jù)通過數(shù)據(jù)采集裝置傳輸?shù)接?jì)算機(jī)中。如果拉線式編碼器，為增量式拉線式編碼器，則需要將拉線式編碼器中的長(zhǎng)度變化量與拉線式編碼器的初始長(zhǎng)度相加得到各組球鉸間的距離。
(3)求解如下迭代方程組得到運(yùn)動(dòng)剛體的空間位置和空間姿態(tài)。
Li-|R·pi+T-Pi|＝0，i＝1，…，6式中T＝[x y z]T是坐標(biāo)系p的原點(diǎn)在坐標(biāo)系P中的位置，可以得到運(yùn)動(dòng)剛體的空間位置信息；R為運(yùn)動(dòng)平臺(tái)坐標(biāo)系p關(guān)于固定平臺(tái)坐標(biāo)系P的方向余弦陣，按照依次繞固定平臺(tái)坐標(biāo)系P的x，y，z軸旋轉(zhuǎn)有限角度α，β，γ的卡爾丹角表示方法，具體表達(dá)式為
R=cosβcosγ-cosβsinγsinβsinαsinβcosγ+cosαsinγ-sinαsinβsinγ+cosαcosγ-sinαcosβ-cosαsinβcosγ+sinαsinγcosαcosβsinγ+sinαcosγcosαcosβ]]>L1～L6為6個(gè)拉線式編碼器得到的測(cè)量長(zhǎng)度。Pi是固定平臺(tái)上的六個(gè)球鉸在固定平臺(tái)坐標(biāo)系P中的坐標(biāo)。pi是運(yùn)動(dòng)平臺(tái)上的六個(gè)球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系p中的坐標(biāo)。
在具體的實(shí)施中，上述迭代方程組可采用牛頓—拉斐遜法求解。下面為了表述方便，將本迭代求解算法公式闡述如下采用牛頓—拉斐遜法求解Stewart機(jī)構(gòu)運(yùn)動(dòng)學(xué)正解的方法。
機(jī)構(gòu)的運(yùn)動(dòng)學(xué)正解方程可表示為fi(x，y，z，α，β，γ)＝Li-|R·pi+T-Pi|＝0i＝1，2，…6該方程組為含有6個(gè)未知數(shù)的6個(gè)非線性的方程。令X＝(x，y，z，α，β，γ)，F(xiàn)(X)＝[f1(X)，f2(X)，f3(X)，f4(X)，f5(X)，f6(X)]T。X*為方程組的解。牛頓—拉斐遜法為一迭代算法。將方程Taylor展開，得到F(X+ΔX)＝F(X)+JδX+O(δX2)式中J為方程的雅克比矩陣，Jij=∂fi∂Xj.]]>忽略方程的二階量，并令F(X+ΔX)＝0有JδX＝-F(X)此為一線性方程組，解為δX＝-J-1F(X)。因此其迭代過程為Xnew＝Xold+δX。
(4)將當(dāng)前運(yùn)動(dòng)剛體的位姿替代迭代方程的初始值，為下一次測(cè)量做準(zhǔn)備，實(shí)現(xiàn)連續(xù)測(cè)量。
權(quán)利要求
1.剛體空間位姿測(cè)量裝置，其特征在于該測(cè)量裝置包括測(cè)量執(zhí)行機(jī)構(gòu)、數(shù)據(jù)采集裝置以及存儲(chǔ)計(jì)算程序的計(jì)算機(jī)；所述測(cè)量執(zhí)行機(jī)構(gòu)包括一個(gè)固定平臺(tái)及固定在所述固定平臺(tái)上的6個(gè)球鉸，一個(gè)與運(yùn)動(dòng)剛體固連的運(yùn)動(dòng)平臺(tái)及固定在所述運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸，兩端分別與固定平臺(tái)上的6個(gè)球鉸和運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸相連的六個(gè)拉線式編碼器；所述數(shù)據(jù)采集裝置包括分別與拉線式編碼器輸出端相連的六個(gè)計(jì)數(shù)器，與所述各計(jì)數(shù)器輸出端連接的多路復(fù)用器，以及與所述多路復(fù)用器相連的數(shù)據(jù)發(fā)送電路；所述數(shù)據(jù)發(fā)送電路與所述計(jì)算機(jī)相應(yīng)輸入端口連接；所述各球鉸在各自平臺(tái)的位置坐標(biāo)滿足測(cè)量執(zhí)行機(jī)構(gòu)雅克比矩陣Jp的條件數(shù)小于等于10，JP=l→1T(Rp1×l→1)T|p1|······l→6T(Rp6×l→6)T|p6|∈R6×6,]]>式中 分別是沿每個(gè)拉線式編碼器長(zhǎng)度方向上的單位向量，p1～p6是運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系P中的坐標(biāo)，R為運(yùn)動(dòng)平臺(tái)坐標(biāo)系P關(guān)于固定平臺(tái)坐標(biāo)系P的方向余弦陣，按照依次繞固定平臺(tái)坐標(biāo)系P的x，y，z軸旋轉(zhuǎn)角度α，β，γ的卡爾丹角表示方法，具體表達(dá)式為R=cosβcosγ-cosβsinγsinβsinαsinβcosγ+cosαsinγ-sinαsinβsinγ+cosαcosγ-sinαcosβ-cosαsinβcosγ+sinαsinγcosαsinβsinγ+sinαcosγcosαcosβ.]]>
2.根據(jù)權(quán)利要求1所述的剛體空間位姿測(cè)量裝置，其特征在于所述的固定在運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸直接固定在運(yùn)動(dòng)剛體上，省去所述運(yùn)動(dòng)平臺(tái)。
3.根據(jù)權(quán)利要求1或2所述的剛體空間位姿測(cè)量裝置，其特征在于所述拉線式編碼器是數(shù)字式拉線式編碼器。
4.根據(jù)權(quán)利要求1或2所述的剛體空間位姿測(cè)量裝置，其特征在于所述拉線式編碼器是模擬式拉線式編碼器，該剛體空間位姿測(cè)量裝置還包括一個(gè)A/D采集卡，所述A/D采集卡的輸入端與所述模擬式拉線式編碼器的輸出端相連，輸出端分別與六個(gè)計(jì)數(shù)器相連。
5.利用權(quán)利要求1所述測(cè)量裝置進(jìn)行剛體空間位姿測(cè)量的方法，其特征在于該測(cè)量方法包括如下步驟1)系統(tǒng)初始化，即確定各球鉸在各自平臺(tái)坐標(biāo)系中的坐標(biāo)，拉線式編碼器的初始長(zhǎng)度以及被測(cè)剛體在固定平臺(tái)坐標(biāo)系中的初始坐標(biāo)，并輸入所述計(jì)算機(jī)；2)當(dāng)被測(cè)剛體運(yùn)動(dòng)時(shí)，利用數(shù)據(jù)采集裝置采集拉線式編碼器的長(zhǎng)度變化信號(hào)，并傳送到計(jì)算機(jī)中；3)以被測(cè)剛體在固定平臺(tái)坐標(biāo)系中的初始坐標(biāo)為方程組初始值，計(jì)算機(jī)中的程序求解下述迭代方程組，得到被測(cè)剛體的空間姿態(tài)信息及位置信息Li-|R·pi+T-Pi|＝0，i＝1，…，6；其中，L1～L6為6個(gè)拉線式編碼器得到的測(cè)量長(zhǎng)度，Pi是固定平臺(tái)上的6個(gè)球鉸在固定平臺(tái)坐標(biāo)系P中的坐標(biāo)，pi是運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸在運(yùn)動(dòng)平臺(tái)坐標(biāo)系p中的坐標(biāo)，R為運(yùn)動(dòng)平臺(tái)坐標(biāo)系p關(guān)于固定平臺(tái)坐標(biāo)系P的方向余弦陣，包含了待求的空間姿態(tài)信息，T＝[x y z]T是運(yùn)動(dòng)平臺(tái)坐標(biāo)系p的原點(diǎn)在固定平臺(tái)坐標(biāo)系P中的位置，包含了待求的空間位置信息；4)將當(dāng)前運(yùn)動(dòng)剛體的位姿作為迭代方程組的初始值，實(shí)現(xiàn)連續(xù)測(cè)量。
全文摘要
剛體空間位姿測(cè)量裝置及其測(cè)量方法，屬于測(cè)控技術(shù)領(lǐng)域。為克服傳統(tǒng)位姿測(cè)量裝置的不足，本發(fā)明提出了一種接觸式的低成本、高精度的剛體空間位姿測(cè)量裝置，包括測(cè)量執(zhí)行機(jī)構(gòu)、數(shù)據(jù)采集裝置以及存儲(chǔ)計(jì)算程序的計(jì)算機(jī)；所述測(cè)量執(zhí)行機(jī)構(gòu)包括固定平臺(tái)及固定在固定平臺(tái)上的6個(gè)球鉸，與運(yùn)動(dòng)剛體固連的運(yùn)動(dòng)平臺(tái)及固定在運(yùn)動(dòng)平臺(tái)上的6個(gè)球鉸，兩端分別與固定平臺(tái)球鉸和運(yùn)動(dòng)平臺(tái)球鉸相連的六個(gè)拉線式編碼器。本發(fā)明還公開了一種剛體空間位姿測(cè)量方法，該方法根據(jù)上述測(cè)量裝置獲取的初始數(shù)據(jù)，利用迭代方程組得到被測(cè)剛體的空間位姿信息。本發(fā)明所述裝置結(jié)構(gòu)簡(jiǎn)單，系統(tǒng)實(shí)時(shí)性較高，能實(shí)現(xiàn)對(duì)被測(cè)剛體的全閉環(huán)實(shí)時(shí)控制。
文檔編號(hào)B23Q17/22GK1570556SQ20041000908
公開日2005年1月26日 申請(qǐng)日期2004年5月12日 優(yōu)先權(quán)日2004年5月12日
發(fā)明者段廣洪, 張輝, 周潛, 南仁東, 王啟明 申請(qǐng)人:清華大學(xué), 中國(guó)科學(xué)院國(guó)家天文臺(tái)