Stewart平臺(tái)姿態(tài)測(cè)量裝置及測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種Stewart平臺(tái)姿態(tài)測(cè)量裝置及測(cè)量方法，尤其涉及一種采用全加 速度傳感器實(shí)現(xiàn)六自由度Stewart平臺(tái)形心位置、旋轉(zhuǎn)角度和旋轉(zhuǎn)方向的姿態(tài)參數(shù)測(cè)量的 測(cè)量裝置及測(cè)量方法，屬于運(yùn)動(dòng)模擬器姿態(tài)測(cè)量領(lǐng)域。
【背景技術(shù)】
[0002] Stewart平臺(tái)又稱為并聯(lián)平臺(tái)機(jī)構(gòu)，它是由動(dòng)平臺(tái)、靜平臺(tái)、鉸鏈和六個(gè)驅(qū)動(dòng)桿組 成，可以實(shí)現(xiàn)6個(gè)自由度的轉(zhuǎn)動(dòng)。由于這種機(jī)構(gòu)具有結(jié)構(gòu)簡(jiǎn)單、動(dòng)態(tài)性能好、剛度大、承載能 力強(qiáng)等優(yōu)點(diǎn)，在機(jī)器人、機(jī)床制造行業(yè)、汽車運(yùn)動(dòng)模擬器、航天運(yùn)載工具模擬器、空間對(duì)接機(jī) 構(gòu)、潛艇救援對(duì)接器及高速鐵路無(wú)砟軌道板等方面具有極高的應(yīng)用價(jià)值。
[0003] Stewart平臺(tái)的位置姿態(tài)控制精度是衡量其工作質(zhì)量?jī)?yōu)劣和性能高低的主要指 標(biāo)，成為并聯(lián)驅(qū)動(dòng)平臺(tái)應(yīng)用研宄中的關(guān)鍵技術(shù)之一，平臺(tái)控制算法的核心內(nèi)容是位置正解 和位置反解，即已知?jiǎng)悠脚_(tái)位姿求解驅(qū)動(dòng)桿長(zhǎng)（反解），或已知各驅(qū)動(dòng)桿長(zhǎng)求解動(dòng)平臺(tái)位姿 (正解）。測(cè)量出動(dòng)平臺(tái)位置姿態(tài)六參數(shù)（x，y，z，a，0， y)T，即可求解反解；相反已知桿長(zhǎng) 變化可正解出位姿6參數(shù)。一般來(lái)說(shuō)正解多采用數(shù)值分析法，比如牛頓一辛卜森法迭代求 解計(jì)算量大、速度慢、不能閉環(huán)控制，相比之下反解速度快可實(shí)現(xiàn)實(shí)時(shí)控制。
[0004] 目前對(duì)平臺(tái)位置和姿態(tài)的精確控制常用方法主要有兩種。一是機(jī)電法，通過(guò)測(cè)量 電機(jī)的角位移或油缸的伸縮線位移進(jìn)行反饋控制，該方案的優(yōu)點(diǎn)是成本低，便于實(shí)現(xiàn)，缺點(diǎn) 是半閉環(huán)控制，精度低；二是光電法，通過(guò)單個(gè)或兩個(gè)攝像頭獲取三維定位信息，其優(yōu)點(diǎn)是 精度高，但是造價(jià)高，可定位范圍小，對(duì)應(yīng)用現(xiàn)場(chǎng)也有較高要求。這兩種方法都具有較強(qiáng)的 局限性，由于位置和姿態(tài)的精確控制是并聯(lián)平臺(tái)機(jī)構(gòu)得以實(shí)現(xiàn)其應(yīng)用價(jià)值的必要前提，因 而對(duì)其進(jìn)行高精度的動(dòng)態(tài)位姿測(cè)量就具有十分重要的意義，基于實(shí)時(shí)控制的強(qiáng)烈需求，本 發(fā)明提出一種基于無(wú)陀螺捷聯(lián)慣導(dǎo)系統(tǒng)測(cè)量方法，采用4只三軸加速度傳感器直接測(cè)量 Stewart平臺(tái)位姿參數(shù)的新方法。
[0005] 無(wú)陀螺捷聯(lián)慣導(dǎo)是指慣性測(cè)量組合中不使用陀螺儀，而利用線加速度計(jì)得到慣性 測(cè)量的全部參數(shù)，適用于戰(zhàn)術(shù)導(dǎo)彈、智能炮彈中等利用加速度計(jì)代替陀螺實(shí)現(xiàn)制導(dǎo)的領(lǐng)域。 此理念國(guó)外早在1965年就提出，但此后近20年時(shí)間里一直停頓，主要原因是陀螺技術(shù)的飛 速發(fā)展。進(jìn)入20世紀(jì)90年代，隨著計(jì)算機(jī)技術(shù)以及MEMS水平的提高，中國(guó)國(guó)內(nèi)外對(duì)采用全 加速度計(jì)制作慣性測(cè)量的研宄方案越來(lái)越重視，使其重獲新生。1982年Shmuel J.Merhav 提出采用旋轉(zhuǎn)或振動(dòng)加速度計(jì)組成無(wú)陀螺慣性測(cè)量組件，提出從加速度計(jì)的輸出信號(hào)中分 離線加速度和角速度的方法；1991年Algrain斷言最少需要六個(gè)加速度計(jì)可測(cè)量物體的線 加速度和角加速度；1999年Lee求出了利用6個(gè)加速度計(jì)測(cè)量物體旋轉(zhuǎn)運(yùn)動(dòng)的解法，并將 卡爾曼濾波應(yīng)用在導(dǎo)航系統(tǒng)中；2000年Kirill詳細(xì)研宄了基于六加速度計(jì)配置的算法。國(guó) 內(nèi)的研宄剛剛起步，主要進(jìn)行此方向研宄的有哈爾濱工程大學(xué)、哈爾濱工業(yè)大學(xué)、北京理工 大學(xué)等少數(shù)高校，但目前研宄內(nèi)容主要涉及到不同應(yīng)用領(lǐng)域加速度計(jì)配置方案的研宄，及 相應(yīng)角速度的優(yōu)化算法等，還沒(méi)有具體應(yīng)用方面的報(bào)道。通過(guò)調(diào)研，國(guó)內(nèi)外學(xué)者在無(wú)陀螺捷 聯(lián)慣導(dǎo)加速度計(jì)方案配置方面，主要采用的是六加速度配置方案，細(xì)分又有好幾種，但都是 由加速度計(jì)輸出的比力中解出載體角加速度，再經(jīng)積分得到角速度。主要缺點(diǎn)有些是由于 角速度項(xiàng)由平方根計(jì)算得到，無(wú)法確定角速度方向；或者計(jì)算量大，且角速度的誤差隨時(shí)間 累積，或者對(duì)安裝的精度要求太高，實(shí)際應(yīng)用困難，由于以上原因六加速度計(jì)方式不是很理 想的方式。九加速度計(jì)配置方案是一種改進(jìn)方式，利用加速度計(jì)輸出的冗余信息改進(jìn)角速 度解算算法從而提高精度，通過(guò)對(duì)角加速度積分確定角速度符號(hào)消除角速度符號(hào)的不確定 性，對(duì)角速度平方項(xiàng)開(kāi)方確定數(shù)值抑制迭代誤差，但具體算法為見(jiàn)公布，且從文獻(xiàn)上看傳感 器的安裝位置配置不適合應(yīng)用于Stewart這類動(dòng)平臺(tái)上。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明的目的就在于為了解決上述問(wèn)題而提供一種采用全加速度傳感器實(shí)現(xiàn) Stewart平臺(tái)姿態(tài)測(cè)量裝置及測(cè)量方法。
[0007] 本發(fā)明通過(guò)以下技術(shù)方案來(lái)實(shí)現(xiàn)上述目的：
[0008] -種Stewart平臺(tái)姿態(tài)測(cè)量裝置，所述Stewart平臺(tái)包括動(dòng)平臺(tái)和靜平臺(tái)，所述動(dòng) 平臺(tái)和所述靜平臺(tái)之間通過(guò)六個(gè)球鉸分別與六根可伸縮的連桿相連；設(shè)動(dòng)平臺(tái)坐標(biāo)系{0J 的三個(gè)坐標(biāo)軸軸向分別為X、Y、Z，其坐標(biāo)原點(diǎn)0 b是所述動(dòng)平臺(tái)的6個(gè)球鉸的機(jī)械絞點(diǎn)組成 的圓周的圓心，設(shè)靜平臺(tái)坐標(biāo)系1^}的坐標(biāo)原點(diǎn)仏是所述靜平臺(tái)的6個(gè)球鉸的機(jī)械絞點(diǎn)組 成的圓周的圓心；在所述動(dòng)平臺(tái)上安裝有四個(gè)墊塊，在四個(gè)所述墊塊上一一對(duì)應(yīng)地安裝有 四個(gè)三軸向加速度傳感器，其中，所述第一三軸向加速度傳感器和所述第四三軸向加速度 傳感器分別安裝于對(duì)應(yīng)的所述墊塊的上表面，所述第二三軸向加速度傳感器和所述第三三 軸向加速度傳感器分別安裝于對(duì)應(yīng)的所述墊塊的側(cè)表面；四個(gè)所述三軸向加速度傳感器的 三個(gè)敏感軸方向均為x、y、z，其中，第一三軸向加速度傳感器與第四三軸向加速度傳感器 以坐標(biāo)原點(diǎn)〇 b對(duì)稱并分別位于X軸的兩側(cè)，第二三軸向加速度傳感器與第三三軸向加速度 傳感器以X軸為對(duì)稱軸對(duì)稱安裝，所述第一三軸向加速度傳感器的x、y、z軸分別與所述動(dòng) 平臺(tái)的X、Y、Z軸平行，所述第一三軸向加速度傳感器、所述第二三軸向加速度傳感器、所述 第三三軸向加速度傳感器、所述第四三軸向加速度傳感器的包含安裝方向的坐標(biāo)分別為： (X^、Zj)、（_X2、5^2、乙2)、（_叉2、_y"2、乙2)、（_Xi、y"i、Zj) 〇
[0009] 具體地，所述三軸向加速度傳感器為靈敏度為lOOmv/g、量程為50g、精度為 0.0 OOlg、頻響范圍為0. 5-4. 5KHz的低頻三軸向加速度傳感器。
[0010] 一種Stewart平臺(tái)姿態(tài)測(cè)量裝置采用的測(cè)量方法，包括以下步驟：
[0011] (1)將四個(gè)所述三軸向加速度傳感器的輸出簡(jiǎn)化成Ov b2……b12)，其中（bp b2、 b3)是第一三軸向加速度傳感器的x、y、z三個(gè)方向輸出值，（b4、b 5、b6)是第二三軸向加速 度傳感器的x、y、z三個(gè)方向輸出值，（b7、b8、b9)是第三三軸向加速度傳感器的x、y、z三個(gè) 方向輸出值，（b 1Q、bn、b12)是第四三軸向加速度傳感器的x、y、z三個(gè)方向輸出值；
[0012] (2)根據(jù)下面公式⑴推導(dǎo)求解：
[0013]
【主權(quán)項(xiàng)】
1. 一種Stewart平臺(tái)姿態(tài)測(cè)量裝置，所述Stewart平臺(tái)包括動(dòng)平臺(tái)和靜平臺(tái)，所述動(dòng)平 臺(tái)和所述靜平臺(tái)之間通過(guò)六個(gè)球較分別與六根可伸縮的連桿相連；其特征在于；設(shè)動(dòng)平臺(tái) 坐標(biāo)系{化}的S個(gè)坐標(biāo)軸軸向分別為X、Y、Z，其坐標(biāo)原點(diǎn)化是所述動(dòng)平臺(tái)的6個(gè)球較的機(jī) 械絞點(diǎn)組成的圓周的圓屯、，設(shè)靜平臺(tái)坐標(biāo)系{〇i}的坐標(biāo)原點(diǎn)〇1是所述靜平臺(tái)的6個(gè)球較的 機(jī)械絞點(diǎn)組成的圓周的圓屯、；在所述動(dòng)平臺(tái)上安裝有四個(gè)墊塊，在四個(gè)所述墊塊上一一對(duì) 應(yīng)地安裝有四個(gè)=軸向加速度傳感器，其中，所述第一=軸向加速度傳感器和所述第四= 軸向加速度傳感器分別安裝于對(duì)應(yīng)的所述墊塊的上表面，所述第二=軸向加速度傳感器和 所述第==軸向加速度傳感器分別安裝于對(duì)應(yīng)的所述墊塊的側(cè)表面；四個(gè)所述=軸向加速 度傳感器的=個(gè)敏感軸方向均為X、y、Z，其中，第一=軸向加速度傳感器與第四=軸向加速 度傳感器W坐標(biāo)原點(diǎn)〇b對(duì)稱并分別位于X軸的兩側(cè)，第二=軸向加速度傳感器與第==軸 向加速度傳感器WX軸為對(duì)稱軸對(duì)稱安裝，所述第一=軸向加速度傳感器的X、y、Z軸分別 與所述動(dòng)平臺(tái)的X、Y、Z軸平行，所述第一=軸向加速度傳感器、所述第二=軸向加速度傳 感器、所述第==軸向加速度傳感器、所述第四=軸向加速度傳感器的包含安裝方向的坐 標(biāo)分別為；（Xi、-Yi、Zi)、（-X2、Y2、Z2)、（-&、-y]、Z2)、（-Xi、Yi、Zi)。
2. 根據(jù)權(quán)利要求1所述的Stewart平臺(tái)姿態(tài)測(cè)量裝置，其特征在于：所述=軸向加速 度傳感器為靈敏度為lOOmv/g、量程為50g、精度為0.OOOlg、頻響范圍為0. 5-4. 5KHZ的低頻 =軸向加速度傳感器。
3. -種如權(quán)利要求1或2所述的Stewart平臺(tái)姿態(tài)測(cè)量裝置采用的測(cè)量方法，其特征 在于；包括W下步驟： (1) 將四個(gè)所述S軸向加速度傳感器的輸出簡(jiǎn)化成化i，b2……612)，其中化1、62、63)是 第一S軸向加速度傳感器的x、y、zS個(gè)方向輸出值，化4、be、be)是第二S軸向加速度傳感 器的x、y、z立個(gè)方向輸出值，（b,、b8、bg)是第立立軸向加速度傳感器的x、y、z立個(gè)方向輸 出值，化K)、bii、b。）是第四S軸向加速度傳感器的x、y、zS個(gè)方向輸出值； (2) 根據(jù)下面公式（1)推導(dǎo)求解：
式（1)中，未知變量ai、32、a]、a*、as、ag、a?、as、ag、aio、a。、a。分別為 對(duì)應(yīng)的A,、Ay、Az、Wy、W,.，、c4、紅巧、￡t^、w|、Wji.Wz、^^,￡也，求解式（1)可推導(dǎo)出平 臺(tái)旋轉(zhuǎn)參數(shù)，如W下公式（2)~（13)所示，其中，Ay、Ay、A,分別是動(dòng)平臺(tái)的坐標(biāo)原點(diǎn)〇b相 對(duì)于靜平臺(tái)的坐標(biāo)原點(diǎn)OI在x、y、zS個(gè)方向的加速度分量，叫^、itir、￡4分別是動(dòng)平 臺(tái)旋轉(zhuǎn)角速度在X、y、Z立個(gè)方向的角加速度，通過(guò)該角加速度可快速判斷出旋轉(zhuǎn)角方向， 分別是動(dòng)平臺(tái)旋轉(zhuǎn)角速度在x、y、z=個(gè)方向的幅值平方，通過(guò)該幅值平 方可快速計(jì)算出旋轉(zhuǎn)角度大小，《,?,、《,?,、《,?,為誤差分析參數(shù)，由《4、（^3-、^也7、 ;m|、；wf、WXWy、《XWZ、《yWZ九個(gè)變量準(zhǔn)確獲得在X、y、ZS個(gè)方向的動(dòng)平臺(tái)旋轉(zhuǎn) 角速度《y、的幅值和旋轉(zhuǎn)方向：
上面公式中的Xi、Yi、Zi、X2、y2、Z2分別為對(duì)應(yīng)s軸向加速度傳感器相對(duì)于動(dòng)平臺(tái)的坐 標(biāo)原點(diǎn)化的距離，可自定義其數(shù)值； 通過(guò)上述公式（2)-(13)求得Ay、Ay、A,、即可獲得動(dòng)平臺(tái)坐標(biāo)系相對(duì)于靜 平臺(tái)坐標(biāo)系的位置和姿態(tài)參數(shù)，即動(dòng)平臺(tái)的坐標(biāo)原點(diǎn)化即形屯、在靜平臺(tái)坐標(biāo)系中的位置坐 標(biāo)P= (X，y，Z)和旋轉(zhuǎn)參數(shù)搖擺角a、縱搖角P及偏擺角丫。
【專利摘要】本發(fā)明公開(kāi)了一種Stewart平臺(tái)姿態(tài)測(cè)量裝置，動(dòng)平臺(tái)坐標(biāo)系的三個(gè)坐標(biāo)軸軸向分別為X、Y、Z，在Stewart平臺(tái)的動(dòng)平臺(tái)上通過(guò)四個(gè)墊塊安裝有四個(gè)三軸向加速度傳感器，其中兩個(gè)三軸向加速度傳感器以動(dòng)平臺(tái)坐標(biāo)系的坐標(biāo)原點(diǎn)Ob對(duì)稱并分別位于X軸的兩側(cè)，另外兩個(gè)三軸向加速度傳感器以X軸為對(duì)稱軸對(duì)稱安裝。本發(fā)明還公開(kāi)了一種Stewart平臺(tái)姿態(tài)測(cè)量裝置采用的測(cè)量方法，通過(guò)公式求得動(dòng)平臺(tái)的坐標(biāo)原點(diǎn)Ob相對(duì)于靜平臺(tái)的坐標(biāo)原點(diǎn)OI的加速度分量和動(dòng)平臺(tái)旋轉(zhuǎn)角速度，即可獲得動(dòng)平臺(tái)坐標(biāo)系相對(duì)于靜平臺(tái)坐標(biāo)系的位置和姿態(tài)參數(shù)。通過(guò)本發(fā)明無(wú)需復(fù)雜計(jì)算就可以直接獲得Stewart平臺(tái)的形心坐標(biāo)、旋轉(zhuǎn)角度和旋轉(zhuǎn)方向參數(shù)。
【IPC分類】G01C21-16, G01B21-00
【公開(kāi)號(hào)】CN104848818
【申請(qǐng)?zhí)枴緾N201510282513
【發(fā)明人】張毅, 張 榮, 周繼昆, 陳穎
【申請(qǐng)人】中國(guó)工程物理研究院總體工程研究所
【公開(kāi)日】2015年8月19日
【申請(qǐng)日】2015年5月28日