專利名稱：一種用于五軸五環(huán)并聯(lián)運動機構(gòu)運動控制的逆解數(shù)學(xué)算法的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機械技術(shù)領(lǐng)域，具體涉及一種用于五軸五環(huán)并聯(lián)運動機構(gòu)運動控制的逆解數(shù)學(xué)算法。
背景技術(shù)：
隨著工業(yè)現(xiàn)代化的發(fā)展，對機床的技術(shù)指標提出了更高的要求，新一代機床的發(fā)展趨勢是進一步滿足超精密、超高速、激光和細微加工等新工藝提出的高性能和高集成度要求。近年來，并聯(lián)運動機床的出現(xiàn)就順應(yīng)了此趨勢。并聯(lián)運動機床是以空間并聯(lián)機構(gòu)為基礎(chǔ)，充分利用計算機數(shù)字控制潛力，以軟件取代部分硬件，以電氣裝置和電子器件取代部分機械傳動，使將近兩個世紀來的以笛卡爾坐標直線位移為基礎(chǔ)的機床機構(gòu)和運動學(xué)原理發(fā)生了根本變化。
從機床布局上來看，并聯(lián)運動機床與傳統(tǒng)數(shù)控機床的區(qū)別主要表現(xiàn)在傳統(tǒng)機床布局的基本特點是以床身、立柱、橫梁等作為支承部件，主軸部件和工作臺的滑板沿支承部件上的直線導(dǎo)軌移動，按照X、Y、Z坐標運動疊加的串聯(lián)運動學(xué)原理，形成加工表面軌跡。
并聯(lián)運動機床布局的基本特點是，以機床框架為固定平臺和若干桿件組成空間并聯(lián)機構(gòu)，主軸部件安裝在并聯(lián)機構(gòu)的動平臺上，改變桿件的長度或移動桿的支點，按照并聯(lián)運動學(xué)原理形成刀頭點的加工表面軌跡。并聯(lián)運動機床具有剛度強、動態(tài)性能好、機床模塊化程度高、易于重構(gòu)以及機械結(jié)構(gòu)簡單等優(yōu)點，它的出現(xiàn)引起國際學(xué)術(shù)界和工程界的極大關(guān)注，一些發(fā)達國家政府和研究機構(gòu)紛紛投入大量人力、物力，研究和開發(fā)可加工復(fù)雜曲面的并聯(lián)機床。并聯(lián)運動機床結(jié)構(gòu)成為新一代機床結(jié)構(gòu)的發(fā)展方向。并聯(lián)運動機床與傳統(tǒng)機床比較，具有以下優(yōu)點1)運動部件質(zhì)量小，運動慣性?。?)高運動速度和高加速度，適合高速加工；3)主要部件具有重復(fù)性，通用程度高；4)容易通過預(yù)加載荷，提高機床部件的剛度；5)通過控制系統(tǒng)可以實現(xiàn)運動精度的補償。
并聯(lián)運動機床在國內(nèi)外的發(fā)展歷程將近十年，1994年，在美國芝加哥國際機床展覽會上，美國Giddings&Lewis公司首次展出了Variax型并聯(lián)運動機床，引起轟動。它是一臺以Stewart平臺為基礎(chǔ)的5坐標立式加工中心，標志著機床設(shè)計開始采用并聯(lián)機構(gòu)，是機床結(jié)構(gòu)重大改革的里程碑。1997年在德國漢諾威國際機床博覽會(EMO97)和1999年巴黎國際機床博覽會(EMO99)上有很多并聯(lián)機床參展，其中德國Mikromat公司生產(chǎn)的6X型高速立式加工中心，美國Ingersoll公司生產(chǎn)的HOH-600型臥式并聯(lián)加工中心，英國Geodetic公司生產(chǎn)的EvolutionG500和G1000兩款并聯(lián)加工中心，瑞士聯(lián)邦技術(shù)學(xué)院研制的HexaGlide并聯(lián)機床，日本Toyada公司生產(chǎn)的基于銑削加工線的六條腿機床，瑞典NeosRobotics公司生產(chǎn)的Tricept845和600型并聯(lián)機床等。俄羅斯Lapic公司將Stewart平臺并聯(lián)機構(gòu)用于TM-1000型精密加工中心和КИМ-750型3坐標測量機。美國Hexel公司推出低價位的Tornado型5坐標加工中心和P2000銑床工作臺，以及6自由度的定位平臺和微型機器人等一系列產(chǎn)品。
我國的并聯(lián)機床領(lǐng)域起步不晚，燕山大學(xué)于1991年研制出我國第一臺并聯(lián)機器人樣機，并在此基礎(chǔ)上作了很多理論研究。清華大學(xué)、哈爾濱工業(yè)大學(xué)、北京理工大學(xué)等都先后推出基于Stewart平臺的虛擬軸機床的樣機。繼美國于1994年推出世界第一臺VARIAX虛擬軸機床之后，我國也于1996年由清華大學(xué)推出了國內(nèi)第一臺VAMTIY虛擬軸機床.隨后很多單位開始了并聯(lián)機床的研制。東北大學(xué)于1998年成功研制了五軸聯(lián)動三桿并聯(lián)機床DSX5-70。大連機床廠研制了五軸聯(lián)動并聯(lián)機床DCB510。并聯(lián)機床因其諸多優(yōu)點很可能成為能夠適應(yīng)21世紀靈活多變環(huán)境的新一代高速、高柔性、高經(jīng)濟性的數(shù)控加工設(shè)備，具有廣泛的市場前景。
并聯(lián)運動機床具有2個共同的缺點第一、有效工作空間與機床體積之比較小。第二、由于運動平臺受約束鉸鏈工作角度范圍的局限，運動平臺偏轉(zhuǎn)角小，雖然有6個自由度，但不能進行五面加工。
在世界各國著名的機床公司相繼推出的新產(chǎn)品中，都針對并聯(lián)機床的缺點進行變型改進，新一代并聯(lián)運動機床開始進入了實用階段。其中德國弗朗霍夫機床和壓力加工研究所(Fraunhofer Institut fuer Werzeugmaschenen und Umformtechnik)與德國Metrom公司(www.metrom.com)合作研制成功采用5桿并聯(lián)機構(gòu)和5環(huán)驅(qū)動的主軸部件，在并聯(lián)運動機構(gòu)理論上有所突破，總體配置設(shè)計是德國Michael Schwaar博士的發(fā)明，用5個轉(zhuǎn)環(huán)替代固定安裝的約束鉸鏈，從而實現(xiàn)主軸部件的偏轉(zhuǎn)角大于90°，能夠真正實現(xiàn)5軸聯(lián)動、5面加工。德國Michael Schwaar博士在2001年發(fā)明以上五軸五環(huán)并聯(lián)機構(gòu)，并將其開發(fā)成產(chǎn)品，在德國申請了專利，其逆解的數(shù)學(xué)模型沒有報道，在國內(nèi)的各類期刊及公開場合也沒有關(guān)于逆解數(shù)學(xué)模型的報道。

發(fā)明內(nèi)容
本發(fā)明的目的在于提出一種用于五軸五環(huán)并聯(lián)運動機構(gòu)運動控制的逆解數(shù)學(xué)算法。
并聯(lián)機床由于其自身的結(jié)構(gòu)特點，不存在沿固定方向?qū)虻膶?dǎo)軌，數(shù)控加工所需的刀具運動軸X，Y，Z，A，B，C等并不真正存在，不能對其進行直接控制，在任何時刻并聯(lián)機構(gòu)的運動都必須是五桿聯(lián)動，故五個伺服驅(qū)動系統(tǒng)之間具有強耦合，非線性的關(guān)系。
機構(gòu)運動學(xué)分析的最基本任務(wù)是建立輸入與輸出構(gòu)件之間位置關(guān)系的數(shù)學(xué)模型，并聯(lián)機床的運動學(xué)數(shù)學(xué)模型包括運動學(xué)方程的正解和逆解問題，它是并聯(lián)機床的速度分析、加速度分析、受力分析、誤差分析及動力學(xué)分析的基礎(chǔ)。正解問題是指已知各驅(qū)動副的長度(或轉(zhuǎn)角)，求活動平臺位姿；逆解問題是指已知活動平臺位姿，求各驅(qū)動副的長度(或轉(zhuǎn)角)。以機床本身加工狀況來說，工件的加工軌跡是已知條件，即活動平臺位姿是已知的，從而求得各驅(qū)動副的長度(或轉(zhuǎn)角)，因此對于機床，逆解更為實用。
并聯(lián)運動機構(gòu)用5個轉(zhuǎn)環(huán)替代固定安裝的約束鉸鏈，機構(gòu)運動學(xué)數(shù)學(xué)模型和以前的各種并聯(lián)運動機構(gòu)相比發(fā)生了極大的變化，即五個約束鉸鏈的位置由已知變?yōu)槲粗景l(fā)明提出的用于五軸五環(huán)并聯(lián)運動機構(gòu)的運動控制逆解數(shù)學(xué)算法，是采用數(shù)學(xué)中的矢量法，求得該并聯(lián)運動機構(gòu)運動控制逆解的數(shù)學(xué)模型。
本發(fā)明提出的用于五軸五環(huán)并聯(lián)運動機構(gòu)運動控制的逆解數(shù)學(xué)算法，其具體步驟如下(1)建立坐標系將五軸五環(huán)并聯(lián)機構(gòu)放入坐標系(如圖1所示)，以機床工作臺作為靜平臺，靜坐標系原點O位于靜平臺中心，Bi點為機架上的固定點，是Qi點在靜坐標系中的垂直投影，則得Bi＝(B cos βi，B sin βi，0)T，i＝1、2…5，其中，B為靜平臺半徑，βi為Bi點與x軸成的夾角；以五環(huán)圍住的主軸頭部為動平臺，主軸頭部的頂部中心O’為動坐標系原點，動坐標系中的Z’軸和主軸頭部軸心線重合，Qi點則是桿件固聯(lián)機架的點，Qi＝(B cos βi，B sin βi，qi)T，其中，qi為鉸接點的高度；桿與環(huán)的鉸接處設(shè)為Ai點，則Ai點在動平臺上的坐標為Ai＝(A cos αi，A sin αi，Hi)T其中，A為動平臺的半徑，Hi為每個環(huán)到動坐標系x′-y′面的距離，αi為Ai點與動坐標系的x軸成的夾角，顯然，這是個不定變量，因為它是在無限制轉(zhuǎn)動環(huán)上的點；(2)選取其中一個支鏈由于五軸五環(huán)并聯(lián)機構(gòu)每個支鏈基本相同，故取出一個支鏈分析R點代表刀尖(如圖2所示)，距動平臺主軸底部中心處正好為刀的伸出長度，
R＝(x0，y0，z0)T；(3)將步驟(2)中選取的單支鏈由空間矢量轉(zhuǎn)化成平面矢量與圖2所對應(yīng)的字母用小寫字母表示(如圖3所示)，即Ai點用a表示，因為a處的鉸鏈是一自由度的轉(zhuǎn)動副，根據(jù)機構(gòu)學(xué)原理可判定，矢量qa和矢量o′r必相交，而矢量o′a的模為動平臺主軸頭部半徑，由空間幾何學(xué)可證明出三矢量qa、o′r和o′a共面；這樣單支鏈由空間矢量問題轉(zhuǎn)化成平面矢量問題；(4)減少一個未知數(shù)以加工一個平面為例，且設(shè)定刀的軸心線與加工表面垂直，其方程為hx0(x0-x1)-hy0(y0-y1)-hz0(z0-z1)＝0，該平面的法向矢量即(hx0，hy0，hz0)，x1，y1，z1是已知常數(shù)，令法向為單位矢量，則有hx02+hy02+hz02＝1，這樣在三個未知數(shù)中就可以用其中兩個表示另一個了，減少了一個未知數(shù)；hx0，hy0，hz0，x0，y0，z0是方程中的函數(shù)變量，法向矢量(hx0，hy0，hz0)決定了動平臺主軸刀位的姿態(tài)，而x0，y0，z0變量決定了動平臺主軸刀尖的位置；(5)從步驟(4)中得到刀位矢量為o′r＝Hi×(hx0，hy0，hz0)，其中，Hi第i個環(huán)中心到刀尖軌跡的距離，即可得出o′點的坐標；(6)用數(shù)學(xué)軟件Matlab求得桿件長度與刀具位姿的函數(shù)關(guān)系，即矢量qα的長度求得Li；(A)由o′點坐標和q點坐標可得矢量qo′；(B)由矢量qo′和矢量o′r可得qo′r平面法矢量n1，矢量n1為矢量qo′和矢量o′r的叉乘；(C)由矢量n1和o′r可得矢量o′α方向的某一矢量n2，矢量n2為矢量o′α和矢量o′r的叉乘；(D)單位化矢量n2，再乘以環(huán)半徑A，得到矢量o′α；(E)得到a坐標；(F)得到Li通項Li＝((Bxi-A×(hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-x0-Hi×hx0)^2+(Byi-A×((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-y0-Hi×hy0)^2+(qi-A×(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-z0-Hi×(1-hx0^2-hy0^2)^(1/2))^2)^(1/2)。
本發(fā)明的有益效果本發(fā)明所作的逆解數(shù)學(xué)算法，是用于五軸五環(huán)并聯(lián)機床運動控制的。當五根桿環(huán)住的主軸中心刀尖需走出既定路線時，五根桿應(yīng)按照怎樣的模式伸縮，這需要用數(shù)學(xué)函數(shù)定義出刀尖軌跡與桿長的關(guān)系式。本發(fā)明所作的數(shù)學(xué)模型即為這種關(guān)系式。因此這是研制并聯(lián)機床的關(guān)鍵技術(shù)。同時也是并聯(lián)機床速度分析、加速度分析、工作空間分析、運動學(xué)性能評價、數(shù)控系統(tǒng)算法、運動誤差及動力學(xué)研究的基礎(chǔ)。本發(fā)明所作的逆解數(shù)學(xué)算法針對五軸五環(huán)并聯(lián)機床，它對于我國跟上世界并聯(lián)機床發(fā)展步伐是很有意義的。


圖1為五軸五環(huán)坐標系示意圖。其中，(a)動平臺、靜平臺及坐標系的相對位置。
(b)動平臺(機床主軸)在動坐標系中的投影。
(c)靜平臺在靜坐標系中的投影。
圖2為單支鏈坐標示意圖。
圖3為支鏈矢量圖。
圖4為實施例1中X方向的位移量。
圖5為Y方向的位移量。
具體實施例方式
下面結(jié)合實施例進一步說明本發(fā)明。
實施例1Q4坐標，(-630.03，363.75，900)；H4＝75；Q5坐標，(-472.98，-273.07，1444.92)；H5＝250；由Qi點坐標可得Bi＝(B cos βi，B sin βi，0)T的坐標B1＝(Bx1，By1，0)＝(472.98，-273.07，0)B2＝(Bx2，By20)＝(630.03，363.75，0)B3＝(Bx3，By30)＝(0，510.09，0)B4＝(Bx4，By40)＝(-630.03，363.75，0)Li通項公式是個參數(shù)化公式，即只要是五軸五環(huán)并聯(lián)機構(gòu)，機構(gòu)的結(jié)構(gòu)尺寸是可以任意改變的。結(jié)構(gòu)尺寸包括動平臺半徑A，桿件固聯(lián)機架的Qi點，Qi＝(B cos βi，B sin βi，qi)T；每個環(huán)到動坐標系x′-y′面的距離Hi。
通過機械三維設(shè)計軟件SolidWorks建模，做出了虛擬的五軸五環(huán)并聯(lián)機床的實體模型，根據(jù)實體模型中的結(jié)構(gòu)尺寸，確定出了常量，表示如下A＝123.87Q1坐標，(472.98，-273.07，1444.92)；H1＝330；Q2坐標，(630.03，363.75，900)；H2＝25；Q3坐標，(0，510.09，1444.92)；H3＝170；B5＝(Bx5，By50)＝(-472.98，-273.07，0)q1＝1444.92q2＝900q3＝1444.92
q4＝900q5＝1444.92將Qi，Bi，qi，Hi的常量代入Li，則桿長方程Li＝f(hx0，hy0，hz0，x0，y0，z0)顯然，桿長方程為桿長和加工表面數(shù)學(xué)方程之間的函數(shù)關(guān)系。
常量代入得各桿長方程L1＝((472.98-123.87×(hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))/((hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))^2)^(1/2)-1.×x0-330.×hx0)^2+(-273.07-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))/((hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))^2)^(1/2)-1.×y0-330.×hy0)^2+(1444.92-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))/((hy0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07))-1.×hx0×(hx0×(y0+330.×hy0+273.07)-1.×hy0×(x0+330.×hx0-472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+330.×hx0-472.98)-1.×hx0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+330.×hy0+273.07)))^2)^(1/2)-1.×z0-330.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)L2＝((630.03-123.87×(hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))/((hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))^2)^(1/2)-1.×x0-25.×hx0)^2+(363.75-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))/((hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))^2)^(1/2)-1.×y0-25.×hy0)^2+(900.-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))/((hy0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75))-1.×hx0×(hx0×(y0+25.×hy0-363.75)-1.×hy0×(x0+25.×hx0-630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+25.×hx0-630.03)-1.×hx0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+25.×hy0-363.75)))^2)^(1/2)-1.×z0-25.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)L3＝((-123.87×(hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))/((hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))^2)^(1/2)-1.×x0-170.×hx0)^2+(510.09-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))/((hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))^2)^(1/2)-1.×y0-170.×hy0)^2+(1444.92-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))/((hy0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09))-1.×hx0×(hx0×(y0+170.×hy0-510.09)-1.×hy0×(x0+170.×hx0)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+170.×hx0)-1.×hx0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+170.×hy0-510.09)))^2)^(1/2)-1.×z0-170.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)L4＝
((-630.03-123.87×(hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))/((hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)1.×hy0×(x0+75.×hx0+630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))^2)^(1/2)-1.×x0-75.×hx0)^2+(363.75-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03)))/((hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))^2)^(1/2)-1.×y0-75.×hy0)^2+(900.-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))/((hy0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75))-1.×hx0×(hx0×(y0+75.×hy0-363.75)-1.×hy0×(x0+75.×hx0+630.03)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+75.×hx0+630.03)-1.×hx0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.))-1.×hy0×(hy0×(z0+75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-900.)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+75.×hy0-363.75)))^2)^(1/2)-1.×z0-75.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)L5＝((-472.98-123.87×(hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))/((hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))^2)^(1/2)-1.×x0-250.×hx0)^2+(-273.07-123.87×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))/((hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))^2)^(1/2)-1.×y0-250.×hy0)^2+(1444.92-123.87×(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))/((hy0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98))-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)))^2+((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07))-1.×hx0×(hx0×(y0+250.×hy0+273.07)-1.×hy0×(x0+250.×hx0+472.98)))^2+(hx0×((1.-1.×hx0^2-1.×hy0^2)^(1/2)×(x0+250.×hx0+472.98)-1.×hx0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92))-1.×hy0×(hy0×(z0+250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)-1444.92)-1.×(1.-1.×hx0^2-1.×hy0^2)^(1/2)×(y0+250.×hy0+273.07)))^2)^(1/2)-1.×z0-250.×(1.-1.×hx0^2-1.×hy0^2)^(1/2))^2)^(1/2)3.驗證過程先定出讓刀尖走的軌跡，然后對每根桿輸入以上的桿長函數(shù)，運行后看看刀尖軌跡是不是走的既定軌跡。我們驗證了圓，空間斜線，水平線等，得到的結(jié)果是正確的。
舉例為了使主軸在XOY平面上走出一個圓心在(-100，0，0)的圓，代入以下方程組x0=100×COS(time)-100y0=100×SIN(time)z0=0]]>代入后將此方程組方程分別代入五根桿長函數(shù)，在Adams中對應(yīng)的五根桿處輸入桿長函數(shù)，經(jīng)過仿真，可記錄主軸刀尖的位移軌跡。圓方程組代入桿長函數(shù)，得到L1＝SQRT((472.98-(100×COS(time)-100)-123.87×(472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))×(472.98-(100×COS(time)-100)-123.87×(472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))+(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))×(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(472.98-(100×COS(time)-100))×(472.98-(100×COS(time)-100))))+664094.6064)-917.83L2＝SQRT((630.03-(100×COS(time)-100)-123.87×(630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))×(630.03-(100×COS(time)-100)-123.87×(630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))+(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))×(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(630.03-(100×COS(time)-100))×(630.03-(100×COS(time)-100))))+330625)-833.66L3＝SQRT((-(100×COS(time)-100)-123.87×(-(100×COS(time)-100))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))×(-(100×COS(time)-100)-123.87×(-(100×COS(time)-100))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))+(510.09-100×SIN(time)-123.87×(510.09-100×SIN(time))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))×(510.09-100×SIN(time)-123.87×(510.09-100×SIN(time))/SQRT((510.09-100×SIN(time))×(510.09-100×SIN(time))+(-(100×COS(time)-100))×(-(100×COS(time)-100))))+950469.0064)-1048.63L4＝SQRT((-630.03-(100×COS(time)-100)-123.87×(-630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))×(-630.03-(100×COS(time)-100)-123.87×(-630.03-(100×COS(time)-100))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))+(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))×(363.75-100×SIN(time)-123.87×(363.75-100×SIN(time))/SQRT((363.75-100×SIN(time))×(363.75-100×SIN(time))+(-630.03-(100×COS(time)-100))×(-630.03-(100×COS(time)-100))))+275625)-799.99L5＝SQRT((-472.98-(100×COS(time)-100)-123.87×(-472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))×(-472.98-(100×COS(time)-100)-123.87×(-472.98-(100×COS(time)-100))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))+(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))×(-273.07-100×SIN(time)-123.87×(-273.07-100×SIN(time))/SQRT((-273.07-100×SIN(time))×(-273.07-100×SIN(time))+(-472.98-(100×COS(time)-100))×(-472.98-(100×COS(time)-100))))+800881.8064)-989.5453.輸出結(jié)果主軸刀尖的位移軌跡記錄由Adams輸出，如圖4和圖5所示。確實為圓。
權(quán)利要求
1.一種用于五軸五環(huán)并聯(lián)運動機構(gòu)運動控制的逆解數(shù)學(xué)算法，其特征在于具體步驟如下(1)建立坐標系將五軸五環(huán)并聯(lián)機構(gòu)放入坐標系，以機床工作臺作為靜平臺，靜坐標系原點O位于靜平臺中心，Bi點為機架上的固定點，是Qi點在靜坐標系中的垂直投影，則得Bi＝(Bcosβi，Bsinβi，0)T，i＝1、2…5，其中，B為靜平臺半徑，βi為Bi點與x軸成的夾角；以五環(huán)圍住的主軸頭部為動平臺，主軸頭部的頂部中心O’為動坐標系原點，動坐標系中的Z’軸和主軸頭部軸心線重合，Qi點為桿件固聯(lián)機架的點，則得Qi＝(Bcosβi，Bsinβi，qi)T，其中，qi為鉸接點的高度；桿與環(huán)的鉸接處設(shè)為Ai點，則Ai點在動平臺上的坐標為Ai＝(Acosαi，Asinαi，Hi)T其中，A為動平臺的半徑，Hi為每個環(huán)到動坐標系x′-y′‘面的距離，αi為Ai點與動坐標系的x軸成的夾角，顯然，這是個不定變量，因為它是在無限制轉(zhuǎn)動環(huán)上的點；(2)選取其中一個支鏈由于五軸五環(huán)并聯(lián)機構(gòu)每個支鏈基本相同，故取出一個支鏈分析R點代表刀尖，距動平臺主軸底部中心處正好為刀的伸出長度，R＝(x0，y0，z0)T；(3)將步驟(2)中選取的單支鏈由空間矢量轉(zhuǎn)化成平面矢量Ai點用a表示，a處的鉸鏈為一自由度的轉(zhuǎn)動副，根據(jù)機構(gòu)學(xué)原理判定，矢量qa和矢量o′r必相交，而矢量o′a的模為動平臺主軸頭部半徑，由空間幾何學(xué)證明三矢量qa、o′r和o′a共面；則單支鏈由空間矢量問題轉(zhuǎn)化成平面矢量問題；(4)減少一個未知數(shù)加工一個平面，且設(shè)定刀的軸心線與加工表面垂直，其方程為hx0(x0-x1)-hy0(y0-y1)-hz0(z0-z1)＝0該平面的法向矢量即(hx0，hy0，hz0)，x1，y1，z1是已知常數(shù)，令法向為單位矢量，則有hx02+hy02+hz02＝1，這樣在三個未知數(shù)中就減少了一個未知數(shù)；hx0，hy0，hz0，x0，y0，z0是方程中的函數(shù)變量，法向矢量(hx0，hy0，hz0)決定動平臺主軸刀位的姿態(tài)，x0，y0，z0變量決定動平臺主軸刀尖的位置；(5)從步驟(4)中得到刀位矢量為o′r＝Hi×(hx0，hy0，hz0)，其中，Hi第i個環(huán)中心到刀尖軌跡的距離，即可得出o′點的坐標；(6)用數(shù)學(xué)軟件Matlab求得桿件長度與刀具位姿的函數(shù)關(guān)系，即矢量qα的長度求得Li；(A)由o′點坐標和q點坐標可得矢量qo′；(B)由矢量qo′和矢量o′r可得qo′r平面法矢量n1，矢量n1為矢量qo′和矢量o′r的叉乘；(C)由矢量n1和o′r可得矢量o′α方向的某一矢量n2，矢量n2為矢量o′α和矢量o′r的叉乘；(D)單位化矢量n2，再乘以環(huán)半徑A，得到矢量o′α；(E)得到a坐標；(F)得到Li通項Li＝((Bxi-A×(hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-x0-Hi×hx0)^2+(Byi-A×((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-y0-Hi×hy0)^2+(qi-A×(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))/((hy0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi))-(1-hx0^2-hy0^2)^(1/2)×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)))^2+((1-hx0^2-hy0^2)^(1/2)×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi))-hx0×(hx0×(y0+Hi×hy0-Byi)-hy0×(x0+Hi×hx0-Bxi)))^2+(hx0×((1-hx0^2-hy0^2)^(1/2)×(x0+Hi×hx0-Bxi)-hx0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi))-hy0×(hy0×(z0+Hi×(1-hx0^2-hy0^2)^(1/2)-qi)-(1-hx0^2-hy0^2)^(1/2)×(y0+Hi×hy0-Byi)))^2)^(1/2)-z0-Hi×(1-hx0^2-hy0^2)^(1/2))^2)^(1/2)。
全文摘要
本發(fā)明屬于機械技術(shù)領(lǐng)域，具體涉及一種用于五軸五環(huán)并聯(lián)運動機構(gòu)的逆解數(shù)學(xué)算法。其具體步驟為建立坐標系，選取其中一個支鏈，將選取的單支鏈由空間矢量轉(zhuǎn)化成平面矢量，求得刀位矢量，然后根據(jù)矢量關(guān)系求得矢量qα。最后通過數(shù)學(xué)軟件Matlab求得桿件長度與刀具位姿和刀尖位置的函數(shù)關(guān)系，桿長L
文檔編號G05B19/18GK1900864SQ20061002918
公開日2007年1月24日 申請日期2006年7月20日 優(yōu)先權(quán)日2006年7月20日
發(fā)明者謝紅, 張曙, 姚晶晶 申請人:同濟大學(xué)