一種機(jī)器手的先進(jìn)控制算法研究

1、大 連 民 族 學(xué) 院 本 科 畢 業(yè) 設(shè) 計(jì)（論 文）一種機(jī)器手的先進(jìn)控制算法研究學(xué) 院（系）： 機(jī)電信息工程學(xué)院 專 業(yè)： 自 動(dòng) 化 學(xué) 生 姓 名： 孫 曉 雪 學(xué) 號(hào)： 07023316 指 導(dǎo) 教 師： 徐 國(guó) 凱 評(píng) 閱 教 師： 完 成 日 期： 2011年 月 日 大連民族學(xué)院一種機(jī)器手的先進(jìn)算法研究摘 要從機(jī)器人誕生到現(xiàn)在，機(jī)器人技術(shù)經(jīng)歷了一個(gè)長(zhǎng)期緩慢的發(fā)展過(guò)程。隨著計(jì)算機(jī)技術(shù)、微電子技術(shù)、網(wǎng)絡(luò)技術(shù)等的快速發(fā)展，機(jī)器人技術(shù)也得到了飛速發(fā)展。機(jī)器人的研究、制造和應(yīng)用程度，是一個(gè)國(guó)家或公司科技水平和經(jīng)濟(jì)實(shí)力的象征。由于關(guān)節(jié)機(jī)器人的動(dòng)作能力優(yōu)于其他類型機(jī)器人，故其控制問(wèn)題的研究和

2、開(kāi)發(fā)從經(jīng)濟(jì)上、技術(shù)上考慮都是十分必要的。本文主要研究關(guān)節(jié)機(jī)器人運(yùn)動(dòng)學(xué)及機(jī)器人圓弧插補(bǔ)軌跡控制問(wèn)題。引入齊次矩陣、齊次坐標(biāo)和齊次坐標(biāo)變換，對(duì)機(jī)器人進(jìn)行位姿分析，建立六自由度關(guān)節(jié)機(jī)器人坐標(biāo)系。根據(jù)rbt-6t/s02s教學(xué)機(jī)器人各連桿坐標(biāo)系間的齊次變換矩陣，建立了六自由度關(guān)節(jié)機(jī)器手正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)方程，并對(duì)六自由度關(guān)節(jié)機(jī)器人的逆運(yùn)動(dòng)學(xué)方程進(jìn)行了分析?；谧钚∑顖A弧插補(bǔ)原理,利用機(jī)器人逆運(yùn)動(dòng)學(xué)方程設(shè)計(jì)了機(jī)器人在空間內(nèi)的圓弧插補(bǔ)算法，得到空間內(nèi)機(jī)器手圓弧插補(bǔ)軌跡的控制過(guò)程。通過(guò)固定一個(gè)坐標(biāo)軸的方法設(shè)計(jì)了空間內(nèi)六自由度機(jī)器人圓弧插補(bǔ)流程。用vc+語(yǔ)言編寫(xiě)了六自由度關(guān)節(jié)機(jī)器人圓弧插補(bǔ)程序，并通過(guò)程序調(diào)

3、試進(jìn)行驗(yàn)證。調(diào)試證明這種機(jī)器人運(yùn)動(dòng)學(xué)算法是可行的。關(guān)鍵詞：關(guān)節(jié)機(jī)器人；坐標(biāo)變換；運(yùn)動(dòng)學(xué)；圓弧插補(bǔ)- i -abstractborn to now, from robot experienced a robot technology a slow process of development. along with the computer technology,microelectronic technology,network technology,fast development,the robot technology also got rapid development. robot

4、research,manufacture and application degree,is a country or a company technology level and economic strength of symbol.because of joint of the robot moves ability is better than other kinds of robot,so its control problem from research and development of the economic,technical considerations is very

5、 necessary.this essay mainly studies joint robot kinematics robot circular interpolation trajectory control problem. it leads into homogeneous matrix, homogeneous coordinate and homogeneous coordinate transactions, analysis robots pose, build 6 free of degree joint-robot coordinate system. according

6、 rbt-6t/s02s teaching robots various connecting rods coordinate systems homogeneous transformation matrix, build 6 free of degree joint-robot operations research, opposite-operations research equation ,and also analysis 6 free degree joint-robots opposite-operations research.based on the minimum dev

7、iation circular interpolation theory,using robot inverse kinematics model design arobot in the space inside the arc interpolation trajectory control process. through fixing a coordinate design 6 free of degree robot linear interpolation in the plane. with vc+ language compiling 6 free degree joint-r

8、obot linear interpolations order, and verify it through order debugging. according debugging can confirm the operations research is advisable. key words：joint-robot；coordinate transformation；kinematics；circular interpolation- iii -目 錄摘 要iabstractii1 緒論11.1 機(jī)器人的發(fā)展11.2 關(guān)節(jié)機(jī)器人22 機(jī)器人坐標(biāo)系的建立42.1 機(jī)器人位姿分析42.

9、1.1坐標(biāo)系號(hào)的分配方法42.1.2 用d-h方法確定各坐標(biāo)系的方位52.2 連桿坐標(biāo)系間的齊次變換矩陣62.2.1齊次變換62.3 連桿坐標(biāo)系間的齊次變換矩陣的表示方法93 機(jī)器人運(yùn)動(dòng)學(xué)113.1 機(jī)器人正運(yùn)動(dòng)學(xué)113.2 機(jī)器人逆運(yùn)動(dòng)學(xué)124 設(shè)計(jì)機(jī)器人圓弧插補(bǔ)算法144.1 最小偏差圓弧插補(bǔ)原理144.1.1插補(bǔ)過(guò)程144.1.2進(jìn)給判斷154.1.3簡(jiǎn)化修改154.1.4越象限調(diào)整155 調(diào)試分析195.1 空間內(nèi)機(jī)器人圓弧插補(bǔ)分析195.1.1機(jī)器人在空間內(nèi)圓弧插補(bǔ)195.1.2 圓弧插補(bǔ)分析205.2 調(diào)試分析20結(jié) 論23參 考 文 獻(xiàn)24附錄a 圓弧插補(bǔ)主程序25附錄b 動(dòng)態(tài)連

10、接庫(kù)的使用27致 謝311 緒論機(jī)器人是自動(dòng)執(zhí)行工作的及其裝置。它既可以接受人類指揮，又可以運(yùn)行預(yù)先編排的程序，也可以根據(jù)以人工智能技術(shù)制定的原則性綱領(lǐng)行動(dòng)。它的任務(wù)是協(xié)助或取代人類工作的工作，例如生產(chǎn)業(yè)、建筑業(yè)，或是危險(xiǎn)的工作。人類社會(huì)的發(fā)展已離不開(kāi)機(jī)器人技術(shù)，而機(jī)器人技術(shù)的進(jìn)步又對(duì)推動(dòng)科技發(fā)展起著不可替代的作用。1.1 機(jī)器人的發(fā)展第一代：60年代初，美國(guó)unination公司成功研制第一臺(tái)數(shù)控機(jī)械手，它是具有記憶存儲(chǔ)能力的示教再現(xiàn)式機(jī)器人。第二代：70年代，出現(xiàn)了具有感覺(jué)傳感器的機(jī)器人，具有一定的自適應(yīng)能力。第三代：80年代，具有智能功能的機(jī)器人，具有靈活的思維功能。到了本世紀(jì)，隨著計(jì)算

11、機(jī)技術(shù)、微電子技術(shù)、網(wǎng)絡(luò)技術(shù)等的快速發(fā)展，機(jī)器人技術(shù)也得到了飛速發(fā)展。除了除了工業(yè)機(jī)器人水平不斷提高之外，各種用于非制造業(yè)的先進(jìn)機(jī)器人系統(tǒng)也有了長(zhǎng)足的進(jìn)展。1.2 關(guān)節(jié)機(jī)器人關(guān)節(jié)機(jī)器人由于結(jié)構(gòu)上的原因，其關(guān)節(jié)通常只有轉(zhuǎn)動(dòng)型和移動(dòng)型兩類。這些關(guān)節(jié)之中，凡單獨(dú)驅(qū)動(dòng)的稱為主動(dòng)關(guān)節(jié)，反之稱為從動(dòng)關(guān)節(jié)。單獨(dú)驅(qū)動(dòng)的主動(dòng)關(guān)節(jié)數(shù)目稱作關(guān)節(jié)機(jī)器人的自由度數(shù)目。本次畢業(yè)設(shè)計(jì)所使用的機(jī)器人為6自由度串聯(lián)關(guān)節(jié)式機(jī)器人rbt-6t/s02s教學(xué)機(jī)器人，其結(jié)構(gòu)如圖1.1所示。圖1.1 關(guān)節(jié)機(jī)器人結(jié)構(gòu)其軸線相互平行或垂直，采用串聯(lián)式開(kāi)鏈結(jié)構(gòu)，即機(jī)器人各連桿由旋轉(zhuǎn)關(guān)節(jié)或移動(dòng)關(guān)節(jié)串聯(lián)連接。采用交流伺服電機(jī)和步進(jìn)電機(jī)混合驅(qū)動(dòng)，連

12、桿的一端裝在固定的支座上（底座），另一端處于自由狀態(tài)，可安裝各種工具以實(shí)現(xiàn)機(jī)器人作業(yè)。關(guān)節(jié)的作用是使相互聯(lián)接的兩個(gè)連桿產(chǎn)生相對(duì)運(yùn)動(dòng)，關(guān)節(jié)的傳動(dòng)采用模塊化結(jié)構(gòu)，由錐齒輪、同步齒型帶和諧波減速器等多種傳動(dòng)結(jié)構(gòu)配合實(shí)現(xiàn)，主要傳動(dòng)部件采用可視化設(shè)計(jì)，是一個(gè)多輸入多輸出的動(dòng)力學(xué)復(fù)雜系統(tǒng)，具有高度的能動(dòng)性和靈活性。并通過(guò)windows環(huán)境下的軟件編程和運(yùn)動(dòng)控制卡實(shí)現(xiàn)對(duì)機(jī)器人的控制，使機(jī)器人能夠在工作空間內(nèi)任意位置精確定位。rbt-6t/s02s教學(xué)機(jī)器人的技術(shù)參數(shù)見(jiàn)表1.1。表1.1 rbt-6t/s02s教學(xué)機(jī)器人技術(shù)參數(shù)： 機(jī)構(gòu)形態(tài)串聯(lián)關(guān)節(jié)型自由度6負(fù)載能力3kg動(dòng)作范圍關(guān)節(jié)轉(zhuǎn)動(dòng)-150 +150關(guān)節(jié)

13、轉(zhuǎn)動(dòng)-150 -30關(guān)節(jié)轉(zhuǎn)動(dòng)+50 -70關(guān)節(jié)轉(zhuǎn)動(dòng)-150 +150關(guān)節(jié)轉(zhuǎn)動(dòng)-90 +90關(guān)節(jié)轉(zhuǎn)動(dòng)-180 +180最大速度關(guān)節(jié)轉(zhuǎn)動(dòng)60o /s關(guān)節(jié)轉(zhuǎn)動(dòng)90o /s關(guān)節(jié)轉(zhuǎn)動(dòng)60o /s關(guān)節(jié)轉(zhuǎn)動(dòng)60o /s關(guān)節(jié)轉(zhuǎn)動(dòng)60o /s關(guān)節(jié)轉(zhuǎn)動(dòng)120o /s電源容量單相 220v 50hz 8a2 機(jī)器人坐標(biāo)系的建立關(guān)節(jié)型機(jī)器人的機(jī)械本體實(shí)際上是一個(gè)由轉(zhuǎn)動(dòng)和移動(dòng)關(guān)節(jié)連接起來(lái)的開(kāi)鏈?zhǔn)竭B桿系統(tǒng)，每個(gè)獨(dú)立驅(qū)動(dòng)的關(guān)節(jié)決定著機(jī)器人的一個(gè)自由度。為了便于描述這些連桿之間的相互關(guān)系，在每一連桿關(guān)節(jié)上設(shè)立一個(gè)坐標(biāo)系，利用齊次變換就可以方便地描述這些坐標(biāo)系間的相對(duì)位置和姿態(tài)。2.1 機(jī)器人位姿分析2.1.1 用d-h方法確

14、定各坐標(biāo)系的方位（1）機(jī)器人坐標(biāo)系建立的方法常用的是d-h方法，這種方法嚴(yán)格定義了每個(gè)關(guān)節(jié)的坐標(biāo)系，并對(duì)連桿和關(guān)節(jié)定義了4個(gè)參數(shù)，如圖2.1所示。圖2.1 機(jī)器人轉(zhuǎn)動(dòng)關(guān)節(jié)4個(gè)特征參數(shù)示意圖首先建立連桿i-1和i的參考坐標(biāo)系i，原點(diǎn)位于關(guān)節(jié)i-1和關(guān)節(jié)i兩軸線的公共法線與關(guān)節(jié)i軸線的交點(diǎn)上，參考坐標(biāo)系i的z軸就是關(guān)節(jié)i的軸線，xi軸沿關(guān)節(jié)i和關(guān)節(jié)i-1兩軸線的公共法線，方向由關(guān)節(jié)i指向關(guān)節(jié)i+1。對(duì)于如上建立的連桿坐標(biāo)系，共有4個(gè)特征參數(shù)來(lái)描述，其中用兩個(gè)參數(shù)用來(lái)描述一個(gè)連桿，即公共法線距離和垂直于所在平面內(nèi)兩軸的夾角；另外兩個(gè)參數(shù)來(lái)表示相鄰兩桿的關(guān)系，即兩連桿的相對(duì)位置和兩連桿法線的夾角。除第

15、一個(gè)和最后一個(gè)連桿外，每一個(gè)連桿兩端各有一轉(zhuǎn)動(dòng)軸線，每一個(gè)連桿兩端的軸線各有一條法線，分別為前、后相鄰連桿的公共法線。這兩法線間的距離即為。我們稱為連桿長(zhǎng)度，為連桿扭角，為兩連桿距離，為兩連桿夾角。另有一種特殊情況，即連桿i的兩端軸線平行。在這種情況下，由于兩平行軸線的公垂線存在多值，故無(wú)法確定連桿i的坐標(biāo)原點(diǎn)。這時(shí)，連桿i的坐標(biāo)原點(diǎn)由確定。（2）如圖2.3和圖2.4分別為rbt系列機(jī)器人運(yùn)動(dòng)結(jié)構(gòu)簡(jiǎn)圖及其用d-h方法建立的坐標(biāo)系。圖2.4 d-h方法坐標(biāo)系的建立圖2.3 rbt系列機(jī)器人運(yùn)動(dòng)結(jié)構(gòu)簡(jiǎn)圖（3）表2.1是rbt機(jī)器人的參數(shù)表2.1 rbt系列機(jī)器人的參數(shù)桿件變量為轉(zhuǎn)角n偏距dn(mm

16、)扭角n桿長(zhǎng)an(mm)11(0)387.5-90022(-90)0023033(0)0-9010744(0)26890055(0)0-90066(0)165.5002.2 連桿坐標(biāo)系間的齊次變換矩陣2.2.1齊次變換（1）齊次坐標(biāo)表示將一個(gè)n維空間的點(diǎn)用n+1維坐標(biāo)表示，則該n+1維坐標(biāo)即為n維坐標(biāo)的齊次坐標(biāo)。一般情況下w稱為該齊次坐標(biāo)中的比例因子，當(dāng)w=1時(shí)，其表示方法稱為齊次坐標(biāo)的規(guī)格化形式，當(dāng)n=3時(shí) p= 1 （2.1）（2）點(diǎn)在空間直角坐標(biāo)系中繞坐標(biāo)軸的旋轉(zhuǎn)變換如圖2.5所示，空間某一點(diǎn)a，坐標(biāo)為（，），當(dāng)它繞z軸旋轉(zhuǎn)角后至a點(diǎn)，坐標(biāo)為（，）。xyzzxy0a（，）圖2.5 點(diǎn)的旋

17、轉(zhuǎn)變換a（，）（3）點(diǎn)和a點(diǎn)的齊次坐標(biāo)分別為 1和 1，因此a點(diǎn)的旋轉(zhuǎn)齊次變換過(guò)程為 （2.2）簡(jiǎn)寫(xiě)為=rot(z,)a式中：rot(z,)表示齊次坐標(biāo)變換時(shí)繞z軸的轉(zhuǎn)動(dòng)齊次變換矩陣，又稱旋轉(zhuǎn)算子。 rot(z,)= （2.3） 式中：c=;=,下同。 同理，可寫(xiě)出繞x軸轉(zhuǎn)動(dòng)的旋轉(zhuǎn)算子和繞y軸轉(zhuǎn)動(dòng)的旋轉(zhuǎn)算子：rot(x,)= （2.4）rot(y,)= （2.5）（4）平移的齊次變換如圖2.6所示，空間某一點(diǎn)a，坐標(biāo)為（，），當(dāng)它平移至后，坐標(biāo)為（，）。xyzzxy0圖2.6 點(diǎn)的平移變換a（，）a（，）所以有 （2.6）也可以簡(jiǎn)寫(xiě)為a=trans(x, y, z)a式中：trans(x, y

18、, z)表示坐標(biāo)變換的平移算子，且trans(x, y, z)= （2.7）式中：第四列元素（x, y, z）分別表示沿坐標(biāo)軸x、y、z的移動(dòng)量。2.3 連桿坐標(biāo)系間的齊次變換矩陣的表示方法用表示機(jī)器人連桿n坐標(biāo)系的坐標(biāo)變換成n-1坐標(biāo)系的坐標(biāo)的齊次坐標(biāo)變換矩陣，通常把上標(biāo)省略，寫(xiě)成。對(duì)于n個(gè)關(guān)節(jié)的機(jī)器人，前一個(gè)關(guān)節(jié)向后一個(gè)關(guān)節(jié)的坐標(biāo)齊次變換矩陣分別為，也就是，其中，（）表示桿件1上的1號(hào)坐標(biāo)系到機(jī)座的0號(hào)坐標(biāo)系的齊次坐標(biāo)變換矩陣。一旦對(duì)全部連桿規(guī)定坐標(biāo)系之后，我們就能夠按照下列順序由兩個(gè)旋轉(zhuǎn)和兩個(gè)平移來(lái)建立相鄰兩連桿i-1與連桿i之間的相對(duì)關(guān)系：繞軸旋轉(zhuǎn)角，使軸轉(zhuǎn)到與同一平面內(nèi)。沿軸平移一段

19、距離，把移到與同一直線上。沿軸平移距離，把連桿i-l的坐標(biāo)系移到使其原點(diǎn)與連桿n的坐標(biāo)系原點(diǎn)重合的地方。繞軸旋轉(zhuǎn)角，使轉(zhuǎn)到與同一直線上。這種關(guān)系可由表示連桿i對(duì)連桿i-1相對(duì)位置的四個(gè)齊次變換來(lái)描述，并叫做矩陣。此關(guān)系式為 （2.8）展開(kāi)上式可得 （2.9）當(dāng)機(jī)械手各連桿的坐標(biāo)系被規(guī)定之后，就能夠列出各連桿的常量參數(shù)。 對(duì)于跟在旋轉(zhuǎn)關(guān)節(jié)i后的連桿，這些參數(shù)為，和。對(duì)于跟在棱柱聯(lián)軸節(jié)i后的連桿來(lái)說(shuō)，這些參數(shù)為和。然后，角的正弦值和余弦值也可計(jì)算出來(lái)。這樣，矩陣就成為關(guān)節(jié)變量的函數(shù)（對(duì)于旋轉(zhuǎn)關(guān)節(jié)）或變量d的函數(shù)（對(duì)于棱柱聯(lián)軸節(jié)）。一旦求得這些數(shù)據(jù)之后，就能夠確定六個(gè)變換矩陣的值。283 機(jī)器人運(yùn)動(dòng)

20、學(xué)3.1 機(jī)器人正運(yùn)動(dòng)學(xué)機(jī)器人運(yùn)動(dòng)學(xué)只涉及到物體的運(yùn)動(dòng)規(guī)律，不考慮產(chǎn)生運(yùn)動(dòng)的力和力矩。機(jī)器人正運(yùn)動(dòng)學(xué)所研究的內(nèi)容是：給定機(jī)器人各關(guān)節(jié)的角度或位移，求解計(jì)算機(jī)器人末端執(zhí)行器相對(duì)于參考坐標(biāo)系的位置和姿態(tài)問(wèn)題。根據(jù)前面章節(jié)介紹的內(nèi)容，研究機(jī)器人運(yùn)動(dòng)學(xué)首先應(yīng)建立機(jī)器人各桿件的構(gòu)件坐標(biāo)系，從而得出齊次坐標(biāo)變換矩陣。能描述連桿坐標(biāo)系之間相對(duì)平移和旋轉(zhuǎn)的齊次變換。描述第一個(gè)連桿對(duì)于機(jī)身的位姿，描述第二個(gè)連桿坐標(biāo)系相對(duì)于第一個(gè)連桿坐標(biāo)系的位姿。如果已知一點(diǎn)在最末一個(gè)坐標(biāo)系（如n坐標(biāo)系）的坐標(biāo)，要把它表示成前一個(gè)坐標(biāo)系（如n-1坐標(biāo)系）的坐標(biāo)，那么齊次坐標(biāo)變換為。依此類推，可知此點(diǎn)到基坐標(biāo)系的齊次齊次坐標(biāo)變換矩

21、陣為：=對(duì)于六自由度機(jī)器人，其末端執(zhí)行器坐標(biāo)系（即連桿坐標(biāo)6）的坐標(biāo)相對(duì)于連桿i-1坐標(biāo)系的齊次變換矩陣，用表示，即=機(jī)器人末端執(zhí)行器相對(duì)于機(jī)身坐標(biāo)系的齊次變換矩陣為=式中：常寫(xiě)成。各連桿變換矩陣相乘，可得到機(jī)器人末端執(zhí)行器的位姿方程（正運(yùn)動(dòng)學(xué)方程）為：= （3.1）式中各參數(shù)見(jiàn)表3.1其中：向矢量處于手爪入物體的方向上，稱之為接近矢量，y向矢量的方向從一個(gè)指尖指向另一個(gè)指尖，處于規(guī)定手爪方向上，稱為方向矢量；最后一個(gè)矢量叫法線矢量，它與矢量和矢量一起構(gòu)成一個(gè)右手矢量集合，并由矢量的叉乘所規(guī)定：。標(biāo)3-1 rbt系列機(jī)器人的正運(yùn)動(dòng)學(xué)的參數(shù)參數(shù)計(jì)算公式nxnynzoxoyozaxayazpxpy

22、pz表中，下同。3.2 機(jī)器人逆運(yùn)動(dòng)學(xué)機(jī)器人的運(yùn)動(dòng)學(xué)反解存在的區(qū)域稱為機(jī)器人的工作空間，求解機(jī)器人逆解的目的也在于要求出機(jī)器人的工作空間。工作空間是操作臂的末端能夠到達(dá)的空間范圍，即末端能夠到達(dá)的目標(biāo)點(diǎn)集合。值得指出的是，工作空間應(yīng)該嚴(yán)格地區(qū)分為兩類：（1）靈活（工作）空間：指機(jī)器人手爪能夠以任意方位到達(dá)的目標(biāo)點(diǎn)集合。因此，在靈活空間的每個(gè)點(diǎn)上，手部的指向可任意規(guī)定。 （2）可達(dá)（工作）空間：指機(jī)器人手爪至少在一個(gè)方位上能夠到達(dá)的目標(biāo)點(diǎn)集合。機(jī)器人操作臂運(yùn)動(dòng)學(xué)反解的數(shù)目決定于關(guān)節(jié)數(shù)目和連桿參數(shù)（對(duì)于旋轉(zhuǎn)關(guān)節(jié)操作臂指的是，）和關(guān)節(jié)變量的活動(dòng)范圍。求解6自由度機(jī)器人的過(guò)程如下：求解的變量為。t=

23、（3.2）式中各參數(shù)見(jiàn)表3-1整理矩陣各項(xiàng)可得： （3.3） （3.4） （3.5）注：其中,根據(jù)上述已知條件求出相應(yīng)的變量。在解機(jī)器人逆運(yùn)動(dòng)學(xué)方程時(shí)，碰到的另一問(wèn)題是解不是唯一（稱為多重解）。在工作空間中任何點(diǎn)，機(jī)械手能以任意方位到達(dá)，并且有兩種可能的形位，即運(yùn)動(dòng)學(xué)方程可能有兩組解。造成機(jī)器人運(yùn)動(dòng)學(xué)逆解具有多解的原因是由于解反三角形函數(shù)方程產(chǎn)生的。對(duì)于一個(gè)真實(shí)的機(jī)器人，只有一組解與實(shí)際情況對(duì)應(yīng)，為此必須做出判斷，以選擇合適的解。通常采用以下的方法剔除多解：（1）根據(jù)關(guān)節(jié)運(yùn)動(dòng)空間來(lái)選擇合適的解。（2）選擇一個(gè)最接近的解。（3）根據(jù)避障要求選擇合適的解。（4）逐級(jí)剔除多余解。4 設(shè)計(jì)機(jī)器人圓弧插

24、補(bǔ)算法4.1 最小偏差圓弧插補(bǔ)原理最小偏差插補(bǔ)法是將加工曲線中的一段圓弧上任意加工的動(dòng)態(tài)坐標(biāo)到圓心的距離與理想圓弧半徑r的差值作為偏差函數(shù)來(lái)反映實(shí)際加工偏差，即f（x,y）=x2+y2_r2。根據(jù)偏差函數(shù)便可知道加工點(diǎn)，看這點(diǎn)在給定軌跡的上方或下與理想圓弧的相對(duì)位置。在進(jìn)給之前，先判定向x坐標(biāo)方向或y坐標(biāo)方向和對(duì)角線坐標(biāo)（x和y同時(shí)進(jìn)給）方向進(jìn)給一步的偏差，選擇偏差較小的方向進(jìn)給，形成合適的加工進(jìn)給軌跡。4.1.1插補(bǔ)過(guò)程關(guān)于圓弧的最小偏差插補(bǔ)運(yùn)算，每進(jìn)給一步需要進(jìn)行四個(gè)工作步驟，其工作流程如下：（1）偏差判別根據(jù)偏差值確定加工點(diǎn)在圓弧外側(cè)（圓弧上）或內(nèi)側(cè)。（2）坐標(biāo)進(jìn)給根據(jù)偏差判別結(jié)果，決定

25、向x坐標(biāo)方向、y坐標(biāo)方向或?qū)蔷€坐標(biāo)（x和y同時(shí)進(jìn)給）方向進(jìn)給。（3）偏差計(jì)算對(duì)進(jìn)給后的新加工點(diǎn)計(jì)算出能反映偏離加工圓弧位置情況的新偏差，提供下一步做判別的依據(jù)。 （4）終點(diǎn)判斷計(jì)算偏差同時(shí)，還要進(jìn)行終點(diǎn)判斷，以確定是否到達(dá)終點(diǎn)。4.1.2進(jìn)給判斷 以第一象限逆向圓弧nr1情況為例，進(jìn)行圓弧加工的進(jìn)給判別規(guī)則的討論。如圖1所示。圖4.1 nr1圓弧進(jìn)給方向?qū)τ谌我饧庸c(diǎn)（x,y）,當(dāng)xy時(shí)，若向y方向進(jìn)給一步，則新加工點(diǎn)的偏差函數(shù)為f（y）=x2+(y+1)2-r2=f+2y+1若向?qū)蔷€方向進(jìn)給一步，則新加工點(diǎn)的偏差函數(shù)為f(x,y)=(x-1) 2+(y+1)2-r2=f-2x+2 （4.

26、1）令f(x,y)=f(y)+f(x,y )=2f-2x+4y+3 f(x,y)稱為點(diǎn)（x,y）的偏差比較函數(shù)。若f(x,y) 0，則f(x,y) | f（y）|，應(yīng)選偏差較小的x,y方向進(jìn)給；若f(x,y)0，則f(x,y)| f（y）|，應(yīng)選偏差較小的y方向進(jìn)給。因此，對(duì)于任意點(diǎn)n（xn,yn）（xnyn）,若fn0（應(yīng)進(jìn)給x,y），其新加工點(diǎn)的偏差比較函數(shù)為fn+l=2fn+l-2xn+l+4yn+l+3=fn-4xn+4yn+10 （4.2）若fn0（應(yīng)進(jìn)給y），其新加工點(diǎn)的偏差比較函數(shù)為fn+l=2fn+l-2xn+l+4yn+l+3=fn+4xn+4yn+6 （4.3）初始狀態(tài)時(shí)f

27、0=0，則f0=-2xq+4yq+3 (xq,yq為圓弧起點(diǎn)坐標(biāo)) （4.4）對(duì)于圖1任意加工點(diǎn)（x,y），當(dāng)xy時(shí)，向x方向進(jìn)給，f(x)= (x-1)2+y2-r2=f-2x+1；向?qū)蔷€方向進(jìn)給，其f(x,y )= (x-1)2+(y+1)2- r2=f-2x+2y+2。偏差比較函數(shù)為f(x,y)=f(x)+f(x,y )=2f-4x+2y+3 （4.5）對(duì)任意點(diǎn)n(xn,yn) (xnyn)，若fn0（應(yīng)進(jìn)給x,y），其新加工點(diǎn)的偏差比較函數(shù)為fn+l=2fn+l-4xn+l+2yn+l+3=fn-4xn+4yn+10（同式4.2）若fn0（應(yīng)進(jìn)給x），其新加工點(diǎn)的偏差比較函數(shù)為fn+

28、l=2fn+l-4xn+l+2yn+l+3=fn-4xn+6 （4.6）初始狀態(tài)時(shí)f0=0，則f0=-4xq+2yq+3 （4.7） 4.1.3簡(jiǎn)化修改為便于編程實(shí)現(xiàn)加工軌跡的連續(xù)跟蹤，將偏差比較函數(shù)進(jìn)行簡(jiǎn)化修改。當(dāng)加工點(diǎn)n(xn,yn)處于xnyn時(shí)，令x=xq-yq，y=yq式（4.4）簡(jiǎn)化為f0=-2x+2y+3 （4.8）若fn0，進(jìn)給x,y，式（4.2）簡(jiǎn)化為fn+l=fn-4x+10y=y+1 （4.9）x=x-2若fn0，進(jìn)給y，式（4.3）簡(jiǎn)化為fn+l=fn+4x+6y=y+1 （4.10）x=x-1同理，當(dāng)加工點(diǎn)n(xn,yn)處于xnyn時(shí)，令x=xq,y=yq,式（4.

29、7）f0可簡(jiǎn)化為f0=-2x+2y+3（同式（4.8）。若fn0，進(jìn)給x,y，式（4.2）簡(jiǎn)化為fn+l=fn+4y+10x=x-1 （4.11）y=y+2若fn0，進(jìn)給x,式（4.6）簡(jiǎn)化為fn+l=fn-4x+6x=x-1 （4.12）y=y+1在圖1的圓弧加工過(guò)程中，當(dāng)加工點(diǎn)由同一象限的xy過(guò)渡到xy區(qū)間時(shí)，進(jìn)給方向及偏差函數(shù)都發(fā)生了改變，插補(bǔ)計(jì)算公式fxy=fxy-2x-2y需要修改為x=yy=0 （4.13）f=f-4y由于圓弧所在象限不同，順逆不同，則進(jìn)給方向、偏差函數(shù)及插補(bǔ)計(jì)算公式也隨之不同。在一個(gè)坐標(biāo)平面內(nèi)，四個(gè)象限的圓弧可分別用圖2表示順圓插補(bǔ)進(jìn)給方向，用圖3表示逆圓插補(bǔ)進(jìn)給

30、方向。各象限圓弧的插補(bǔ)計(jì)算公式推導(dǎo)歸納如表1所示。 圖4.2 逆圓（sr）進(jìn)給方向 圖4.3 逆圓（nr）進(jìn)給方向表1 各象限圓弧插補(bǔ)計(jì)算公式4.1.4越象限調(diào)整在曲線加工中，可能經(jīng)常有一段圓弧連續(xù)跨越幾個(gè)象限的情況，而圓弧插補(bǔ)的初值僅由圓弧起點(diǎn)所在象限決定，因此當(dāng)圓弧從某一象限延續(xù)到相鄰象限時(shí)，進(jìn)給方向及插補(bǔ)計(jì)算公式等需進(jìn)行越象限調(diào)整。以圓弧nr3向圓弧nr4過(guò)渡為例當(dāng)|x|y|時(shí),f nr3=2f+4x-2y+3, f nr4=2f+4x+2y+3，則f nr4= f nr3+4y。令x=|xq|，y=|yq|-|xq|,則偏差比較函數(shù)過(guò)象限修改為f=f-4y,表2列出了從圓弧nr3的起點(diǎn)

31、（-3，-4）向圓弧nr4的終點(diǎn)（3，-4）過(guò)渡的插補(bǔ)計(jì)算實(shí)例。因此當(dāng)圓弧從nr1向nr2過(guò)渡；從nr3向nr4過(guò)渡；從sr4向sr3過(guò)渡；從sr2向sr1過(guò)渡，且當(dāng)|x|y|時(shí)，令x=|xq|，y=|yq|-|xq|,則偏差比較函數(shù)的過(guò)象限修改為f=f-4y。而當(dāng)圓弧從nr2向nr3過(guò)渡；從nr4向nr1過(guò)渡；從sr1向sr4過(guò)渡；從sr3向sr2過(guò)渡，且當(dāng)|x|y|時(shí)，令x=|xq|-|yq|，y=|yq|,則偏差比較函數(shù)的過(guò)象限修改為f=f-4x。 表2 越象限過(guò)渡插補(bǔ)計(jì)算實(shí)例5 調(diào)試分析5.1 平面內(nèi)機(jī)器人圓弧插補(bǔ)分析5.1.1機(jī)器人在平面內(nèi)圓弧插補(bǔ)流程圖如圖5.1所示yn運(yùn)算開(kāi)始判斷

32、圓弧所在象限psdjfkdfdfdfdjkjasdfs13jp1ppppp機(jī)器人手運(yùn)行至起始點(diǎn)找出圓弧運(yùn)動(dòng)過(guò)程中變化快的一軸運(yùn)算結(jié)束偏差計(jì)算與判別y軸+增量yx軸+增量zn-1n-1是否為0？圖5.1 圓弧插補(bǔ)流程圖流程圖實(shí)現(xiàn)了了機(jī)器人平面內(nèi)圓弧插補(bǔ)。5.2 調(diào)試分析（1）對(duì)rbt-6t/s02s教學(xué)機(jī)器人進(jìn)行調(diào)試，首先給出調(diào)試的界面如圖5.2所示。圖5.2 平面圓弧線插補(bǔ)界面（2）第二象限內(nèi)圓弧插補(bǔ)界面如圖5.3所示。圖5.3 第二象限內(nèi)圓弧插補(bǔ)（3）跨象限圓弧如圖5.4所示。圖5.4 跨象限圓弧插補(bǔ) 通過(guò)對(duì)rbt-6t/s02s教學(xué)機(jī)器人調(diào)試結(jié)果可以看出，這種運(yùn)動(dòng)學(xué)算法是可行的。結(jié) 論畢業(yè)

33、設(shè)計(jì)已經(jīng)臨近尾聲，在這將近四個(gè)月的設(shè)計(jì)過(guò)程中，讓我感受到了解決問(wèn)題的快樂(lè)和面對(duì)困難的勇氣。從寫(xiě)作論文的過(guò)程中，我一點(diǎn)一滴的將自己所學(xué)的知識(shí)容入到我的畢業(yè)設(shè)計(jì)當(dāng)中，真正的第一次獨(dú)立的設(shè)計(jì)。從最開(kāi)始的確立題目、調(diào)研，查找資料以及對(duì)三個(gè)報(bào)告的撰寫(xiě)，真正的把理論與實(shí)際如此緊密的結(jié)合起來(lái)，應(yīng)用了自己所學(xué)的知識(shí)，如高等數(shù)學(xué)、自動(dòng)化控制理論和計(jì)算機(jī)等相關(guān)知識(shí)完整的串聯(lián)起來(lái)，把它們上升到技術(shù)的高度。在設(shè)計(jì)過(guò)程中，也經(jīng)常遇到從未接觸過(guò)的問(wèn)題，不理解的理論，我都會(huì)請(qǐng)教老師，查閱資料，逐漸的從了解到理解，從理解到應(yīng)用，最終解決困難。自動(dòng)化專業(yè)是一個(gè)涉及領(lǐng)域廣、知識(shí)結(jié)構(gòu)復(fù)雜的專業(yè)，這也是我喜歡這個(gè)專業(yè)的原因之一。面臨

34、各式各樣的問(wèn)題，陌生的理論體系，我們一步一步學(xué)會(huì)，弄懂，最后為我所用，在這過(guò)程中盡情享受學(xué)習(xí)知識(shí)，運(yùn)用知識(shí)的快感。相信在未來(lái)的科技時(shí)代，自動(dòng)化將發(fā)揮巨大的力量，機(jī)器人也會(huì)在人類的生活和生產(chǎn)中扮演重要的角色。參 考 文 獻(xiàn)1 徐國(guó)凱.運(yùn)動(dòng)系統(tǒng)自適應(yīng)控制器設(shè)計(jì).北京：中國(guó)科學(xué)技術(shù)出版社,2004.2 嚴(yán)學(xué)高.機(jī)器人原理.南京：東南大學(xué)出版社,1992. 3 王天然.機(jī)器人.北京：化學(xué)工業(yè)出版社,2002.4 蔡自興.機(jī)器人學(xué).北京：清華大學(xué)出版社,2000. 5 何廣平.關(guān)節(jié)型機(jī)器人.北京：化學(xué)工業(yè)出版社,2003.6 劉極峰.機(jī)器人技術(shù)基礎(chǔ).北京：高等教育出版社,2006.7 孫迪生.機(jī)器人控制

35、技術(shù).北京：機(jī)械工業(yè)出版社，2006.8 孟慶鑫.機(jī)器人技術(shù)基礎(chǔ).哈爾濱：哈爾濱工業(yè)出版社，2006. 9 張忠?guī)?visual c+2008專題應(yīng)用程序開(kāi)發(fā)實(shí)例精講.北京：電子工業(yè)出版社，2008.10 大熊 繁.機(jī)器人控制.北京：科技出版社，2002.11 楊化書(shū).工業(yè)機(jī)器人技術(shù)的應(yīng)用及發(fā)展j.黃河水利職業(yè)技術(shù)學(xué)院學(xué)報(bào)，2004年04期.12 楊春杰.多關(guān)節(jié)機(jī)械手設(shè)計(jì)與運(yùn)動(dòng)學(xué)研究.武漢：武漢理工大學(xué)出版社，2002.13 祁云嵩.vc+程序設(shè)計(jì)解析與訓(xùn)練.上海：華東理工大學(xué)出版社，2005.14 craig,johnj.introduction to robotics.北京：機(jī)械工業(yè)出版社

36、，2006.15 john p.wander.pipelined computation of manipulator modeling matricesj.joumal of robobics and automation.2001.6(6).:556-566.附錄a 圓弧插補(bǔ)主程序以下是某一象限的順時(shí)針圓弧插補(bǔ)代碼，其他象限類似：#define e(x,y) (x)*(x)+(y)*(y)-r_r /偏差計(jì)算 r_r為半徑的平方#define dt1/如果在第一象限case1:while(yx) /45-90，此段x值變化比y快k=orig_x+x; /orig_x為原點(diǎn)坐標(biāo)movetop

37、osition(k,orig_y+y); /移動(dòng)電機(jī)至目標(biāo)位置x+=dt; /優(yōu)先改變xf=e(x,y); /y不變時(shí) 所得點(diǎn)距圓周偏差，g=e(x,y-dt); /y變時(shí) 所得點(diǎn)距圓周偏差，if（abs_16(f)=abs_16(g)）y-=dt;/如果變時(shí)偏差小，則取改變yif(k=end_x)goto exit; /如果到達(dá)終點(diǎn)，則跳出循環(huán)體while(x=y) /0,45，此段y值變化比x快k=orig_y+y;movetoposition(k,orig_x+x); /移動(dòng)電機(jī)至目標(biāo)位置y-=dt; /優(yōu)先改變yf=e(x,y); /x不變時(shí) 所得點(diǎn)距圓周偏差，g=e(x+dt,y);

38、 /x改變，所得點(diǎn)距圓周偏差，if(abs_16(f)=abs_16(g)x+=dt; /如果變時(shí)偏差小，則取改變xif(k=end_y)goto exit; /如果到達(dá)終點(diǎn)，則跳出循環(huán)體break;附錄b 動(dòng)態(tài)連接庫(kù)的使用7.1 配置文件配置文件用于配置機(jī)器人的各項(xiàng)參數(shù)，包括運(yùn)動(dòng)控制卡和各軸參數(shù)等，一個(gè)配置文件中可以定義多套機(jī)器人，一般用戶不需要修改而直接使用。7.2 運(yùn)動(dòng)控制函數(shù)由于配置文件可以定義多套機(jī)器人，除了初始化、關(guān)閉等函數(shù)外，所有函數(shù)都是針對(duì)當(dāng)前機(jī)器人的函數(shù)。1、 int s6s1_init(int robot_num,const char* filename,bool init

39、_card)；功能：初始化。參數(shù)：robot_num，機(jī)器人編號(hào) filename 初始化文件路徑名稱，如果填寫(xiě)null，系統(tǒng)會(huì)自動(dòng)使用動(dòng)態(tài)連接庫(kù)同一路徑下的s6s1cfg.ini文件init_card 是否初始化，為true會(huì)初始化相關(guān)運(yùn)動(dòng)控制卡，為false時(shí)表示在此之前其他使用控制卡的代碼已經(jīng)初始化了運(yùn)動(dòng)控制卡，此功能用于多個(gè)機(jī)器人跨卡開(kāi)發(fā)時(shí)，由第一個(gè)初始化的機(jī)器人進(jìn)行唯一一次初始化。2、 void s6s1_close()；功能：關(guān)閉3、 int s6s1_select(int robot_num)；功能：選擇當(dāng)前機(jī)器人參數(shù)：robot_num 機(jī)器人編號(hào)4、 int s6s1_sto

40、p()；功能：停止當(dāng)前機(jī)器人5、 int s6s1_stop_axis(int axis)；功能：停止當(dāng)前機(jī)器人的指定軸參數(shù)：axis要停止的軸（0-5）6、 void s6s1_reset()；功能：使當(dāng)前機(jī)器人軟件復(fù)位7、 int s6s1_moveall(double angle6,double speed6, bool acc)；功能：同時(shí)當(dāng)前機(jī)器人移動(dòng)所有軸參數(shù)：angle，長(zhǎng)度為6的雙精度數(shù)組，依次為各個(gè)軸的目的轉(zhuǎn)角，單位：度speed，長(zhǎng)度為6的雙精度數(shù)組，依次為各個(gè)軸的運(yùn)動(dòng)速度，單位：度/秒acc，啟動(dòng)時(shí)是否加速，為true時(shí)采用加速，為false時(shí)直接以指定速度運(yùn)行8、 in

41、t s6s1_moveone(int axes,double angle,double speed,bool by_speed,bool acc)；功能：移動(dòng)當(dāng)前機(jī)器人指定軸參數(shù)：axes， 指定的軸（0-5）angle，軸的目的轉(zhuǎn)角，單位：度speed，軸的運(yùn)動(dòng)速度，單位：度/秒by_speed，運(yùn)動(dòng)方式，為false時(shí)表示位置模式，為true時(shí)表示速度模式，angle參數(shù)被忽略acc，啟動(dòng)時(shí)是否加速，為true時(shí)采用加速，為false時(shí)直接以指定速度運(yùn)行。9、 int s6s1_homestep1()；功能：當(dāng)前機(jī)器人開(kāi)始復(fù)位第一步10、 int s6s1_homestep2(int ax

42、is)；功能：當(dāng)前機(jī)器人指定的軸開(kāi)始復(fù)位第二步參數(shù)：axis，指定的軸號(hào)（0-6）11、 int s6s1_hand(int outdata)；功能：控制當(dāng)前機(jī)器人的手爪張開(kāi)或閉合參數(shù)：outdata 0：閉合，1：張開(kāi)12、 void s6s1_set_critical(critical_section* pcriticalsection)；功能：設(shè)置臨界段參數(shù)，該函數(shù)用于系統(tǒng)集成時(shí)共同使用一個(gè)臨界段來(lái)確保線程的安全性，需要在s6s1_init之前調(diào)用，如果傳入的參數(shù)為null，或在s6s1_init之前沒(méi)有調(diào)用該函數(shù)，系統(tǒng)內(nèi)部會(huì)自動(dòng)創(chuàng)建一個(gè)臨界段對(duì)象維護(hù)線程安全，機(jī)器人單獨(dú)使用時(shí)不需要調(diào)用該函數(shù)參數(shù)：pcriticalsection 指向一個(gè)已經(jīng)初始化的臨界段對(duì)象的指針13、 int s6s1_get_state(int axis ,bool* ppel,bool* pnel,bool* porg)；功能：獲得當(dāng)前機(jī)器人指定軸的信號(hào)狀態(tài)參數(shù)：axis 指定軸號(hào)(0-5)ppel 指向一個(gè)bool型變量，函數(shù)返回時(shí)其內(nèi)容為+el信號(hào)是否有效（true為有效）pnel 指向一個(gè)bool型變量，函數(shù)返回時(shí)其內(nèi)容為-el信號(hào)是否有效（true為有效）porg 指向一個(gè)bool型變量，函數(shù)返回時(shí)其內(nèi)容為org信號(hào)是否有效（true為有效）14、 int s6s1_get_motio