【正文】 極限位置仿真與模型修改 上體豎直行走 。并且初定運(yùn)動(dòng)速度為 20i n/s。故為了模 擬實(shí)際運(yùn)動(dòng)，又添加了模擬地面的一個(gè)軌道，并且模型中規(guī)定左腳上的兩個(gè)點(diǎn)槽連接到這個(gè)軌道上。如圖 中設(shè)置了右腿髖關(guān)節(jié)繞 y 軸旋轉(zhuǎn)的關(guān)節(jié)驅(qū)動(dòng)，把 MATLAB 中對(duì)應(yīng)計(jì)算得到的數(shù)據(jù)表格導(dǎo)入 Pro/E 中進(jìn)行仿真。特別指出的是，對(duì)應(yīng)髖關(guān)節(jié)的繞 y 軸轉(zhuǎn)動(dòng)的關(guān)節(jié)需要在初始狀態(tài)時(shí)加上 ?180 176。 圖 56 運(yùn)動(dòng)分析 根據(jù)如圖 56 中銷釘?shù)姆较?，和第四章中定義的轉(zhuǎn)動(dòng)軸的轉(zhuǎn)動(dòng)方向按照右手定則確定而最終完成定義。首先，需要為已經(jīng)組裝好的機(jī)器人腿部結(jié)構(gòu)添加必要的腿部驅(qū)動(dòng)。圖 為機(jī)器人腿部組裝圖。并導(dǎo)入第一個(gè)零件 BODY 并已缺省模式放置于場(chǎng)景中。 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 36 圖 52 KGJ 零件工程圖 圖 53 零件工程 大腿長(zhǎng)度為 280 mm,如圖 所示，其連接的旋轉(zhuǎn)軸已經(jīng)固接于兩端。如圖 所示。右下角為對(duì)應(yīng)的 3D 立體圖。 機(jī)器人腿部結(jié)構(gòu)三維建模 在開始建模之前，先考察想達(dá)到的計(jì)算結(jié)果，建立模型，然后在三個(gè)關(guān)節(jié)上加上相應(yīng)的約束以及關(guān)節(jié)角度，看計(jì)算的結(jié)果 是否可以達(dá)到預(yù)想的結(jié)果。同時(shí)產(chǎn)生的角度時(shí)間 數(shù)據(jù)可在第五章中的運(yùn)動(dòng)仿真使用。運(yùn)動(dòng)學(xué)、理想運(yùn)動(dòng)模式生成程序等，并對(duì)實(shí)結(jié)果進(jìn)行了分析和研究。 其中機(jī)器人數(shù)據(jù)模型采用二叉樹的結(jié)構(gòu)，并運(yùn)用遞歸算法精簡(jiǎn)程序。 小結(jié) 本章中完成了 雙足機(jī)器人的數(shù)學(xué)建模的程序編寫，計(jì)算了機(jī)器人正運(yùn)動(dòng)學(xué)，運(yùn)用兩種方法計(jì)算了機(jī)器人逆運(yùn)動(dòng)學(xué)，并編程檢驗(yàn)了正逆運(yùn)動(dòng)學(xué)計(jì)算結(jié)果。具體影響運(yùn)動(dòng)的效果，還需要在仿真實(shí)驗(yàn)中驗(yàn)證。 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 33 測(cè)試數(shù)據(jù)列表，在程序中改變機(jī)器人 BODY 對(duì)地高度、機(jī)器人上體繞 z 軸扭轉(zhuǎn)角度、步長(zhǎng) 300。 圖 417 兩腳距離調(diào)整 qds=Dcos? 式 425 向前運(yùn)動(dòng)步驟又可以保持分解到三個(gè)繞 y 軸運(yùn)動(dòng)的自由度上。具體計(jì)算和測(cè)試如下。這樣的控制是失 效的，突變使得實(shí)際執(zhí)行機(jī)構(gòu)無法完成任務(wù)，甚至可能損壞機(jī)器人或造成事故。 以下是幾組測(cè)定數(shù)據(jù)的對(duì)比： 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 32 圖 416 第一種補(bǔ)償方式與關(guān)節(jié)旋轉(zhuǎn)角度關(guān)系 仿真 2 是沒有超出極限位置時(shí)候右腿各個(gè)關(guān)節(jié)的時(shí)間角度圖，從圖中可以看出，每個(gè)圖中數(shù)據(jù)都是連續(xù)的，因而滿足運(yùn)動(dòng)學(xué)要求。與之前的測(cè)定一樣，不斷繪制各個(gè)關(guān)節(jié)的時(shí)間角度關(guān)系圖，當(dāng)圖形出現(xiàn)跳躍數(shù)據(jù)時(shí)，就說明此時(shí)輸入的數(shù)據(jù)不能滿足計(jì)算要求，再更正，直到逼算出能夠正確計(jì)算運(yùn)動(dòng)結(jié)果的角度值。具體計(jì)算和測(cè)試如下。這兩種方式都分別進(jìn)行了計(jì)算并測(cè)試或計(jì)算出了轉(zhuǎn)動(dòng)極限角度。由于上體的轉(zhuǎn)角，使得在 xy 平面投影中，髖關(guān)節(jié)與踝關(guān)節(jié)存在一定的距離，需要計(jì)算補(bǔ)償才能滿足步長(zhǎng)要求。 其中 H 為 BODY 桿件對(duì)地高度， L 為步長(zhǎng)，對(duì)應(yīng)有關(guān)系式： L=2 22500 H? 式 423 之后對(duì)對(duì)應(yīng)狀態(tài)進(jìn)行計(jì)算： GUI 程 序中變換數(shù)值并對(duì)右腿主要運(yùn)動(dòng)的三個(gè)關(guān)節(jié)繪制時(shí)間角度曲線。其運(yùn)動(dòng)關(guān)節(jié)主要運(yùn)用了 xz 平面中的 6 個(gè)自由度完成行走。 shujushengcheng 函數(shù) 將計(jì)算結(jié)果分別存儲(chǔ)到 q1~q12 變量中，分別對(duì)應(yīng) 12 個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度，并且儲(chǔ)存為 PROE 中能夠輸入的數(shù)據(jù)形式。 在整個(gè)運(yùn)動(dòng)過程，主要讓其完成下三個(gè)步驟： 1）、由直立狀態(tài)下蹲到雙腿彎曲狀態(tài)，用 step_start_ final 函數(shù)計(jì)算； 2）、計(jì)算走一步（四個(gè)周期）的角度值，用 step_new 函數(shù)計(jì)算； 3）、由雙腿彎曲狀態(tài)轉(zhuǎn)換為直 立狀態(tài)，用 step_start_ final 函數(shù)計(jì)算。對(duì)于 aba 的設(shè)定具體按照人體在步行時(shí)的四種狀態(tài)區(qū)分的，這四種狀態(tài)分別是： 運(yùn)動(dòng)過程中左腳在前，并且運(yùn)動(dòng)趨勢(shì)為兩腳在邁步的狀態(tài)（兩腳遠(yuǎn)離身體中心軸線）； 運(yùn)動(dòng)過程中左腳在前，并且運(yùn)動(dòng)趨勢(shì)為兩腳為并步狀態(tài)（兩腳向身體中心軸線靠近）； 運(yùn)動(dòng)過程中右腳在前，并且運(yùn)動(dòng)趨勢(shì)為兩腳在邁步的狀態(tài)（兩腳遠(yuǎn)離身體中心軸線）； 運(yùn)動(dòng)過程中右腳在前，并且運(yùn)動(dòng)趨勢(shì)為兩腳為并步狀態(tài)（兩腳向身體中心軸線靠近）。在循環(huán)過程中將計(jì)算出來的數(shù)據(jù)記錄到全局變量 times 和 datas 中。以上詳細(xì)主函數(shù)流程圖見下圖 (a)。最后，為初始數(shù)據(jù)修改一些內(nèi)容，并將計(jì)算結(jié)果的弧度值，全部轉(zhuǎn)換為角度值。 是為了在行走過程中為了保持一定的高度，需要雙腿彎曲以保證雙腿行走時(shí)的邁步范圍。根據(jù)以上設(shè)定，完成程序編寫。 模擬步行數(shù)據(jù)計(jì)算與參數(shù)分析 預(yù)先給定模擬步行的約束條件，在模擬機(jī)器人步行運(yùn)動(dòng)時(shí)，確定步行過程中兩腳之間最大的距離為： l； BODY 高度為： h。正運(yùn)動(dòng)學(xué)計(jì)算中首先需要輸入已知參數(shù)：主體位姿和六個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度。關(guān)于測(cè)試正逆運(yùn)動(dòng)學(xué)推導(dǎo)程序的 GUI 程序，如下圖所示 圖 411 正逆運(yùn)動(dòng)學(xué)測(cè)試界面 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 27 其中，程序中對(duì)應(yīng)計(jì)算的是機(jī)器人右腿的正逆運(yùn)動(dòng)學(xué)參數(shù)。根據(jù)式 35式 362 完成雅克比矩陣函數(shù) Jacobian 的編寫。然后定義從身體坐標(biāo)系原點(diǎn)到髖關(guān)節(jié)距離為 D，大腿長(zhǎng)度為 A，小腿長(zhǎng)度為 B，詳細(xì)圖示如下： P 2=p 1+R 00 D 式 47 圖 410 腿部關(guān)節(jié)逆運(yùn)動(dòng)計(jì)算 在足部踝關(guān)節(jié)的局部坐標(biāo)系中，髖關(guān)節(jié)的位置為： r =R7（ p2p7） = RyRyRx 式 48 如上圖膝關(guān)節(jié)，故 C 可以求出： C= 222 RzRyRx ?? 式 49 這樣在三角形 ABC 中，運(yùn)用余弦定理有： C2 =A2 +B2 2ABcos（ q?? ） 式 410 根據(jù)式 410 可導(dǎo)出膝關(guān) 節(jié)角度： q=cos 1? { AB CBA 2 222 ?? }+? 式 411 由正玄定理：)sin( qC ??= aAsin 式 412 由此 a=sin1? ｛ C qA )sin( ?? ｝ 式 413 根據(jù)矢量分解，踝關(guān)節(jié)角 q=atan2（ Ry， Rz） q’=atan2｛ Rx， sign（ Rz） 22 RzRy ? ｝ a sign（） 函數(shù)，含義為取符號(hào)函數(shù)，即當(dāng) x 為正時(shí)返回 +1，當(dāng) x 為負(fù)時(shí)返回 1，當(dāng) x 為零時(shí)返回 0。就是在給定端部桿件的位姿之后求出全部關(guān)節(jié)角度，可以看作正運(yùn)動(dòng)學(xué)的逆運(yùn)算。然后給出各個(gè)關(guān)節(jié)的關(guān)節(jié)角（ LINK(*).q），之后在主程序中運(yùn)行 ForwardKinematics(1)就能完成全部桿件的位姿計(jì)算。 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 24 圖 49 單個(gè)連桿位姿 連桿 ?i 相對(duì)于母桿的齊次變換矩陣為： Tj=????????1000^ bjqjaje 式 42 之后研究如圖 兩連桿連接情況。 正運(yùn)動(dòng)學(xué)計(jì)算 正運(yùn)動(dòng)學(xué)如下圖所示，為單個(gè)連桿的齊次變換矩陣的推導(dǎo)。關(guān)節(jié)軸矢量為三維單位矢量，是根據(jù)機(jī)器人關(guān)節(jié)的轉(zhuǎn)動(dòng)方向已經(jīng)完成規(guī)定的，對(duì)應(yīng)關(guān)節(jié)數(shù)據(jù)已經(jīng)分配在 LINK 結(jié)構(gòu)體數(shù)組中。調(diào)用對(duì)應(yīng)函數(shù)時(shí) 需注意對(duì)應(yīng)的傳遞值和返回值的形式，如函數(shù) a_0 中傳遞值為三維矢量，返回值為反對(duì)稱矩陣，函數(shù) a_1 反之 。其中函數(shù) a_0 完成操作符“∧”的定義，函數(shù) a_1 完成操作符“∨”的定義。 三維矢量與反 對(duì)稱矩陣 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 23 生成的反對(duì)稱矩陣是在原有軸矢量的數(shù)據(jù)基礎(chǔ)之上生成的特殊形式，其操作符在第三章 節(jié)中已經(jīng)定義，對(duì)應(yīng)的變換定義式為式 332。首先需要對(duì)有 12 自由度的仿人機(jī)器人腿部的連桿進(jìn)行編號(hào)，每個(gè)自由度驅(qū)動(dòng)裝置必須滿足運(yùn)動(dòng)： 圖 45 關(guān)節(jié)標(biāo)號(hào)分配 各個(gè)局部各個(gè)局部坐標(biāo)系之間的相對(duì)位置關(guān)系是由關(guān)節(jié)軸矢量 ai 和相對(duì)位置矢量 bi 來確定，如下圖，結(jié)構(gòu)體中添加對(duì)應(yīng)變量：關(guān)節(jié)軸矢量 ai →“ a”，其中，關(guān)節(jié)軸矢量是指相對(duì)于母連桿轉(zhuǎn)動(dòng)的單位角速度矢量，其旋轉(zhuǎn)方向是由右手法則來定義，膝關(guān)節(jié)關(guān)節(jié)軸矢量為： a 5=a 11=??????????010 相對(duì)位置 bi 表示的是一個(gè)局部坐標(biāo)系的原點(diǎn)在其母 連桿坐標(biāo)系中的位置。 數(shù)學(xué)模型的構(gòu)造 構(gòu)造數(shù)學(xué)模型是為步行機(jī)器人對(duì)應(yīng) 它的分解部分給他們賦予標(biāo)號(hào)，然后為每一個(gè)分解部分構(gòu)造一個(gè)擁有大量數(shù)據(jù)的結(jié)構(gòu)體變量，再構(gòu)造結(jié)構(gòu)體數(shù)組來完成整個(gè)機(jī)器人數(shù)字模型的建立。這樣如圖 的樹形結(jié)構(gòu)就可以完成整個(gè)結(jié)構(gòu)樹結(jié)點(diǎn)的訪問計(jì)算，并且不會(huì)無窮循環(huán)，提供了可靠的遞歸出口。其實(shí)這種方式也對(duì)應(yīng)二叉樹的遍歷方式，以下用前序遍歷法編寫顯示各個(gè)連桿名稱的函數(shù)。編程時(shí)，將每一個(gè)結(jié)構(gòu)定義為對(duì)應(yīng)的名稱。它的肢體可以抽象為桿件，聯(lián)動(dòng)運(yùn)動(dòng)的部分是連接桿件的關(guān)節(jié)部分，為了建立合理的數(shù)據(jù)結(jié)構(gòu)先將機(jī)器人做如下分解。 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 19 第四章 雙足機(jī)器人建模 雙足機(jī)器人數(shù)據(jù)結(jié)構(gòu)的建立 數(shù)據(jù)選擇 雙足機(jī)器人的本體結(jié)構(gòu)可以理解為是由很多桿件和關(guān)節(jié)連接構(gòu)成的，最重要的問題就是簡(jiǎn)化數(shù)據(jù)編寫和計(jì)算，進(jìn)一步對(duì)機(jī)器人進(jìn)行有效的分解，是本章研究的要點(diǎn)。并且在正運(yùn)動(dòng)學(xué)已經(jīng)計(jì)算完成的情況下完成后續(xù)的推導(dǎo)。 本節(jié)利用機(jī)器人逆運(yùn)動(dòng)學(xué)來推導(dǎo)雅克比矩陣。由機(jī)器人雅克比矩陣可以推導(dǎo)出關(guān)節(jié)力矩、機(jī)器人與外部外力之間的關(guān)系。p 按照矢量關(guān)系有 39。旋轉(zhuǎn)所在平面交旋轉(zhuǎn)軸于 Q 點(diǎn)。剛體上的一點(diǎn) P 所對(duì)應(yīng)的位置矢量為 p ，經(jīng) 過 ? 角度的旋轉(zhuǎn)，轉(zhuǎn)動(dòng)至 p’ 點(diǎn)（位置矢量為 p 39。如圖 所示有 Vt= v?? V⊥ =v v ∥ =(E?? )v 式 336 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 17 圖 37 某點(diǎn) p 繞定軸轉(zhuǎn) 并且定點(diǎn)運(yùn)動(dòng)的剛體的任何位移，均可以以通過該定點(diǎn)的一個(gè)軸的旋轉(zhuǎn)運(yùn)動(dòng)來表示，這是理論力學(xué)中的歐拉定理闡述的。 在公式 334 兩邊同時(shí)右乘 R 可得： R =w ^R 式 335 規(guī)定角度的方向時(shí)，是以角速度矢量垂直平面為基準(zhǔn)平面，并按照右手法則確定的 。 下面來為了計(jì)算方便來定義一種運(yùn)算，由三維矢量導(dǎo)出對(duì)應(yīng)的反對(duì)稱矩陣要操作符“∧”表示，由反對(duì)稱矩陣導(dǎo)出對(duì)應(yīng)的三維矢量用操作符“∨”表示。所以為了表示方便現(xiàn)在需要定義一種新的計(jì)算。p =v ，故： 39。p =R’R p 式 328 上公式描述了用局部坐標(biāo)系中的點(diǎn)的位置來表示世界坐標(biāo)系中該點(diǎn)的速度的 計(jì)算過程。p = 39。w 39。v =Rv 由叉積定義可知，轉(zhuǎn)動(dòng)前后的關(guān)系為： v =w p 39。p =R 39。當(dāng)角速度矢量、位置矢量和速度矢量在旋轉(zhuǎn)矩陣的作用下轉(zhuǎn)動(dòng)時(shí)有： 39。w =39。的方向轉(zhuǎn)，這時(shí)拇指指向的方向就是速度的方向。在轉(zhuǎn)動(dòng)的肢體上選擇一個(gè)研究點(diǎn)，并用矢量的方式表示該點(diǎn)，如圖 ，而這一點(diǎn)的速度就可以 用角速度矢量與該點(diǎn)矢量的叉積來表示。由式 320 可以發(fā)現(xiàn)，當(dāng)轉(zhuǎn) 動(dòng)軸線定為坐標(biāo)軸時(shí)，就可以將角速度矢量做歸一化處理，將速度矢量分解為一個(gè)單位速度矢量和轉(zhuǎn)速標(biāo)量的乘積。 圖 35 繞 z軸轉(zhuǎn)動(dòng)三維物體 以上也用矢量的形式表示： w =??????????100 式 320 這樣就定義了角速度矢量，其中每個(gè)元素的單位都是 rad/s。 角速度矢量 在三維空間中表示轉(zhuǎn)動(dòng)速度時(shí)，可以表示成為繞某個(gè)軸 線的轉(zhuǎn)動(dòng)。 =??????????100010001 =E 所以經(jīng)過以上推算，旋轉(zhuǎn)矩陣的逆矩陣等于其轉(zhuǎn)置。39。39。39。39。39。這三個(gè)矢量是兩兩相互垂直的，故他們正交： Eiej=ji ji ??01 式 317 用矩陣的形式重新表示時(shí)： 太原工業(yè)學(xué)院畢業(yè)設(shè)計(jì) 14 R’ R=??????????