本發(fā)明涉及了一種機(jī)械臂控制方法，尤其是涉及了一種繩驅(qū)動(dòng)串聯(lián)關(guān)節(jié)型蛇形機(jī)械臂控制方法。

背景技術(shù)：

繩驅(qū)動(dòng)串聯(lián)關(guān)節(jié)型蛇形機(jī)械臂是一種新興的應(yīng)用于狹窄空間作業(yè)的仿生機(jī)械臂，通過(guò)繩索拉動(dòng)關(guān)節(jié)對(duì)機(jī)械臂進(jìn)行控制，由于驅(qū)動(dòng)機(jī)構(gòu)全部布置在基座上，機(jī)械臂上沒(méi)有任何驅(qū)動(dòng)機(jī)構(gòu)而變得輕便靈活。針對(duì)普通機(jī)器人難以開(kāi)展高效作業(yè)的狹窄封閉空間環(huán)境，機(jī)械臂能夠通過(guò)狹小的孔和裂縫等進(jìn)入封閉空間內(nèi)，然后進(jìn)行激光切割、修補(bǔ)、偵察內(nèi)部環(huán)境、構(gòu)建內(nèi)部三維場(chǎng)景等作業(yè)，例如在不拆卸外殼的前提下進(jìn)入飛機(jī)油箱進(jìn)行檢修。

由于蛇形機(jī)械臂的自由度數(shù)量遠(yuǎn)大于作業(yè)過(guò)程所需的自由度數(shù)量，因此屬于超冗余自由度機(jī)械臂，利用冗余的多個(gè)自由度能夠?qū)崿F(xiàn)靈活地進(jìn)行路徑規(guī)劃避開(kāi)障礙物，進(jìn)入狹窄的空間完成預(yù)定的作業(yè)任務(wù)。

相較于完全柔性的連續(xù)型機(jī)械臂，串聯(lián)關(guān)節(jié)型機(jī)械臂具有更高的控制精度，并且負(fù)載能力也大大提高，但完全柔性機(jī)械臂的控制方法均建立在等曲率光滑曲線(xiàn)等多個(gè)假設(shè)前提下，串聯(lián)關(guān)節(jié)型蛇形機(jī)械臂并不滿(mǎn)足條件；而目前大多數(shù)剛性機(jī)械臂的運(yùn)動(dòng)學(xué)模型均采用dh法進(jìn)行構(gòu)建，但對(duì)于具有一定柔性的蛇形機(jī)械臂，傳統(tǒng)的dh法并不適用。

技術(shù)實(shí)現(xiàn)要素：

為了解決背景技術(shù)中存在的問(wèn)題，從空間幾何關(guān)系出發(fā)進(jìn)行研究，本發(fā)明提出一種繩驅(qū)動(dòng)串聯(lián)關(guān)節(jié)型蛇形機(jī)械臂控制方法，構(gòu)建基于齊次變換矩陣的運(yùn)動(dòng)學(xué)模型并對(duì)機(jī)械臂進(jìn)行控制。

如圖1所示，本發(fā)明針對(duì)的機(jī)械臂由多個(gè)關(guān)節(jié)串聯(lián)而成，關(guān)節(jié)之間使用萬(wàn)向節(jié)連接，因而每個(gè)關(guān)節(jié)具有兩個(gè)彎曲自由度，則包含n個(gè)關(guān)節(jié)的機(jī)械臂總共有2n個(gè)自由度。一般在機(jī)械臂末端的工作空間需要三個(gè)移動(dòng)自由度，或再加上三個(gè)轉(zhuǎn)動(dòng)自由度，而通常使用的由6個(gè)關(guān)節(jié)串聯(lián)成的機(jī)械臂具有12個(gè)自由度，因此具備多個(gè)冗余自由度。

通過(guò)電機(jī)或其他驅(qū)動(dòng)機(jī)構(gòu)拉動(dòng)繩索對(duì)每個(gè)關(guān)節(jié)進(jìn)行驅(qū)動(dòng)，繩索的另一端固定在關(guān)節(jié)端面上，每個(gè)關(guān)節(jié)由至少三根并聯(lián)繩索驅(qū)動(dòng)，在萬(wàn)向節(jié)的限制下可以由繩索拉動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)。由于每個(gè)關(guān)節(jié)需要至少三根繩索驅(qū)動(dòng)，則n個(gè)關(guān)節(jié)至少需要總共3n根繩索驅(qū)動(dòng)。

以總計(jì)6個(gè)關(guān)節(jié)，每個(gè)關(guān)節(jié)由3根繩索驅(qū)動(dòng)為例，機(jī)械臂每個(gè)關(guān)節(jié)都有18個(gè)繩孔，對(duì)于與基座相連的第一個(gè)關(guān)節(jié)，其中只有3個(gè)繩孔是用于本節(jié)的3根驅(qū)動(dòng)繩的固定，其余15根繩索均穿過(guò)本節(jié)；第二個(gè)關(guān)節(jié)與由第一個(gè)關(guān)節(jié)穿出的3根繩索固定，其余12根繩索穿過(guò)本節(jié)，以此類(lèi)推，最后一個(gè)關(guān)節(jié)與穿過(guò)前面所有關(guān)節(jié)的最后3根繩索固定。

本發(fā)明采用的技術(shù)方案是：

本發(fā)明用給定各關(guān)節(jié)相對(duì)于上一關(guān)節(jié)的兩個(gè)空間姿態(tài)角度，求解所需要的繩索拉動(dòng)位移使機(jī)械臂運(yùn)動(dòng)到所有關(guān)節(jié)的空間姿態(tài)角度所確定的整體姿態(tài)。

所述的機(jī)械臂是由繩索驅(qū)動(dòng)的機(jī)械臂，機(jī)械臂由多個(gè)關(guān)節(jié)串聯(lián)而成，關(guān)節(jié)之間使用萬(wàn)向節(jié)連接，繩索連接到各個(gè)關(guān)節(jié)。具體地，所述機(jī)械臂可采用發(fā)明名稱(chēng)為《可檢測(cè)關(guān)節(jié)姿態(tài)的高冗余柔性機(jī)械臂裝置》，申請(qǐng)日為2017.6.21，申請(qǐng)?zhí)枮?01710475066.x的說(shuō)明書(shū)中記載的技術(shù)方案。

如圖7所示，方法包括：

1)先構(gòu)建絕對(duì)坐標(biāo)系xyz和每個(gè)關(guān)節(jié)的參考坐標(biāo)系x’y’z’；

2)對(duì)于機(jī)械臂各個(gè)關(guān)節(jié)在所需要的空間姿態(tài)角度下，計(jì)算機(jī)械臂每個(gè)關(guān)節(jié)上的空間點(diǎn)位于絕對(duì)坐標(biāo)系下的絕對(duì)坐標(biāo)(x0,y0,z0)，然后以絕對(duì)坐標(biāo)(x0,y0,z0)為依據(jù)計(jì)算相鄰兩個(gè)關(guān)節(jié)上各自一個(gè)空間點(diǎn)之間的距離，接著利用所有相鄰關(guān)節(jié)的空間點(diǎn)之間的距離計(jì)算每根繩索的總長(zhǎng)；

3)以此求出所有繩索各自的繩索總長(zhǎng)后，控制繩索拉到各自對(duì)應(yīng)的繩索總長(zhǎng)，從而控制機(jī)械臂運(yùn)動(dòng)到所需要的空間姿態(tài)角度，實(shí)現(xiàn)對(duì)機(jī)械臂空間姿態(tài)角度的控制。

所述的絕對(duì)坐標(biāo)系xyz具體是指：以機(jī)械臂的臂基座端面中心為坐標(biāo)系中心，沿垂直于臂基座端面的方向?yàn)閥方向，沿豎直方向?yàn)閦方向，再根據(jù)右手系沿水平方向?yàn)閤方向。

所述的參考坐標(biāo)系x’y’z’具體是指：以機(jī)械臂的關(guān)節(jié)端面中心為坐標(biāo)系中心，垂直于關(guān)節(jié)端面的方向?yàn)閥’方向，沿豎直方向?yàn)閦’方向，再根據(jù)右手系沿水平方向?yàn)閤’方向。

所述方法具體為：

2.1)對(duì)于每個(gè)關(guān)節(jié)上每根繩的穿繩處，用第n個(gè)關(guān)節(jié)上穿繩處所在的空間點(diǎn)相對(duì)于自身關(guān)節(jié)的參考坐標(biāo)系{n}的參考坐標(biāo)位置(xn,yn,zn)，再采用以下公式計(jì)算得到穿繩處所在的空間點(diǎn)在絕對(duì)坐標(biāo)系下的絕對(duì)坐標(biāo)位置(x0,y0,z0)：

其中，tn表示機(jī)械臂第n個(gè)關(guān)節(jié)的齊次變換矩陣，n表示關(guān)節(jié)的序數(shù)；

2.2)將兩個(gè)關(guān)節(jié)間的繩索視為一條直線(xiàn)段，對(duì)于同一根繩索，計(jì)算相鄰兩個(gè)關(guān)節(jié)鄰接端面的各自穿繩處的空間點(diǎn)之間的距離si-1,i，作為繩索在兩個(gè)關(guān)節(jié)間的長(zhǎng)度：

其中，xi為第i關(guān)節(jié)端面上的穿繩處的空間點(diǎn)的絕對(duì)坐標(biāo)的x坐標(biāo)分量，xi-1為第i-1關(guān)節(jié)端面上的穿繩處的空間點(diǎn)的絕對(duì)坐標(biāo)的x坐標(biāo)分量，yi為第i關(guān)節(jié)端面上的穿繩處的空間點(diǎn)的絕對(duì)坐標(biāo)的y坐標(biāo)分量，yi-1為第i-1關(guān)節(jié)端面上的穿繩處的空間點(diǎn)的絕對(duì)坐標(biāo)的y坐標(biāo)分量，zi為第i關(guān)節(jié)端面上的穿繩處的空間點(diǎn)的絕對(duì)坐標(biāo)的z坐標(biāo)分量，zi-1為第i-1關(guān)節(jié)端面上的穿繩處的空間點(diǎn)的絕對(duì)坐標(biāo)的z坐標(biāo)分量；

2.3)將所有的關(guān)節(jié)間繩索長(zhǎng)度相加，再加上所有的穿過(guò)關(guān)節(jié)的繩索長(zhǎng)度(即關(guān)節(jié)兩個(gè)端面的距離l)，就得到了每根繩索的總長(zhǎng)，具體采用以下公式計(jì)算驅(qū)動(dòng)第n關(guān)節(jié)的繩索的總長(zhǎng)：

其中，si-1,i為第i-1關(guān)節(jié)和第i關(guān)節(jié)間的繩索長(zhǎng)度，l表示每個(gè)關(guān)節(jié)前后兩個(gè)端面的距離。

所述的tn表示機(jī)械臂第n個(gè)關(guān)節(jié)的齊次變換矩陣具體是采用以下公式計(jì)算：

tn＝tn-1×td×rxn×rzn×td×tl

式中，td表示沿y軸平移距離d的平移齊次變換矩陣，d為關(guān)節(jié)端面到所緊鄰連接的萬(wàn)向節(jié)中心的距離，rxn表示第n個(gè)關(guān)節(jié)旋轉(zhuǎn)俯仰角θ的旋轉(zhuǎn)齊次變換矩陣，rzn表示第n個(gè)關(guān)節(jié)旋轉(zhuǎn)偏轉(zhuǎn)角的旋轉(zhuǎn)齊次變換矩陣，tl表示沿y軸平移距離l的平移齊次變換矩陣，l表示每個(gè)關(guān)節(jié)前后兩個(gè)端面的距離。

所述的平移齊次變換矩陣td計(jì)算為：

所述的第n個(gè)關(guān)節(jié)的旋轉(zhuǎn)齊次變換矩陣rxn計(jì)算為：

所述的第n個(gè)關(guān)節(jié)的旋轉(zhuǎn)齊次變換矩陣rzn計(jì)算為：

沿y軸平移距離l的平移齊次變換矩陣tl為：

式中，θ表示關(guān)節(jié)繞x軸的相對(duì)旋轉(zhuǎn)角度，表示關(guān)節(jié)繞z軸的相對(duì)旋轉(zhuǎn)角度。

所述的平移齊次變換矩陣tl、平移齊次變換矩陣td、旋轉(zhuǎn)齊次變換矩陣rxn和旋轉(zhuǎn)齊次變換矩陣rzn代入齊次變換矩陣tn后獲得每個(gè)關(guān)節(jié)的齊次變換矩陣計(jì)算公式。例如，機(jī)械臂第一個(gè)關(guān)節(jié)的齊次變換矩陣t1計(jì)算為：

式中，θ表示關(guān)節(jié)繞x軸的相對(duì)旋轉(zhuǎn)角度，表示關(guān)節(jié)繞z軸的相對(duì)旋轉(zhuǎn)角度。

本發(fā)明的有益效果是：

由于機(jī)械臂關(guān)節(jié)的相互耦合作用，在控制前端的某些關(guān)節(jié)運(yùn)動(dòng)時(shí)必須要同時(shí)考慮末端其他關(guān)節(jié)被動(dòng)受到的影響，即使末端關(guān)節(jié)沒(méi)有相對(duì)前端關(guān)節(jié)運(yùn)動(dòng)(即末端關(guān)節(jié)的空間姿態(tài)角度沒(méi)有發(fā)生變化)，驅(qū)動(dòng)末端關(guān)節(jié)的繩索的長(zhǎng)度也會(huì)發(fā)生變化，需要進(jìn)行計(jì)算。采用本發(fā)明的控制方法能夠?qū)崿F(xiàn)解耦，不需要考慮前后關(guān)節(jié)的相互作用，分別獨(dú)立求解出驅(qū)動(dòng)各關(guān)節(jié)的繩索長(zhǎng)度的變化，從而獲得繩索的總長(zhǎng)，實(shí)現(xiàn)對(duì)機(jī)械臂整體姿態(tài)的控制。

目前大多數(shù)的控制方法僅針對(duì)柔性的連續(xù)型機(jī)械臂，對(duì)于具有一定剛性的串聯(lián)關(guān)節(jié)型蛇形機(jī)械臂均模仿對(duì)連續(xù)型機(jī)械臂的控制方法，采用類(lèi)似的運(yùn)動(dòng)學(xué)模型進(jìn)行研究。傳統(tǒng)的連續(xù)型機(jī)械臂采用彎曲角度和旋轉(zhuǎn)角度來(lái)建立運(yùn)動(dòng)學(xué)模型描述關(guān)節(jié)的空間姿態(tài)，該運(yùn)動(dòng)學(xué)模型是基于一系列假設(shè)的前提條件的，包括彎曲的曲率相等，忽略軸向變形等，在實(shí)際情況中難以保證條件全部滿(mǎn)足，因此必然會(huì)導(dǎo)致誤差。而本發(fā)明的方法不需要滿(mǎn)足這些前提條件，直接精確地描述了關(guān)節(jié)的空間姿態(tài)，因此具有更高的精度。

采用彎曲角度和旋轉(zhuǎn)角度來(lái)描述空間姿態(tài)有許多弊端。彎曲角度是指旋轉(zhuǎn)后的關(guān)節(jié)軸線(xiàn)與在關(guān)節(jié)處于沒(méi)有任何旋轉(zhuǎn)的初始位置時(shí)的關(guān)節(jié)軸線(xiàn)的夾角，而旋轉(zhuǎn)角度則是表示軸線(xiàn)彎曲朝向的角度，例如一棵樹(shù)被風(fēng)吹動(dòng)時(shí)，樹(shù)的軸線(xiàn)與地面的夾角即為彎曲角度，而樹(shù)彎曲的朝向可以是0°到360°，這就是旋轉(zhuǎn)角度。采用這兩種角度描述空間姿態(tài)時(shí)并不是一一對(duì)應(yīng)的，兩對(duì)數(shù)值不同的角度卻可以描述相同的空間姿態(tài)，例如彎曲角度為45°，旋轉(zhuǎn)角度為0°時(shí)描述的空間姿態(tài)與彎曲角度為135°，旋轉(zhuǎn)角度為180°時(shí)描述的空間姿態(tài)完全相同，這樣會(huì)導(dǎo)致對(duì)空間姿態(tài)進(jìn)行求解時(shí)解不唯一的情況，使問(wèn)題變得復(fù)雜。

此外采用這兩種角度描述空間姿態(tài)時(shí)，在機(jī)械臂連續(xù)運(yùn)動(dòng)的過(guò)程中可能會(huì)出現(xiàn)角度值突變的情況，例如在彎曲角度為45°，旋轉(zhuǎn)角度為0°時(shí)，旋轉(zhuǎn)角度不變的同時(shí)彎曲角度連續(xù)增加到135°，這樣由于前述的多對(duì)角度對(duì)應(yīng)同一空間姿態(tài)的情況，彎曲角度增加到90°后的瞬間，旋轉(zhuǎn)角度可能由0°突變?yōu)?80°，這樣的不連續(xù)性會(huì)大大增加控制的難度。

相比之下本發(fā)明采用俯仰角度和偏轉(zhuǎn)角度來(lái)描述空間姿態(tài)不僅可以在角度變化連續(xù)的基礎(chǔ)上唯一準(zhǔn)確地確定空間姿態(tài)，而且更加簡(jiǎn)單直觀(guān)，符合正常人的認(rèn)知而便于理解，在空間姿態(tài)描述更精確的同時(shí)也更易于求解計(jì)算。

附圖說(shuō)明

圖1為繩驅(qū)動(dòng)串聯(lián)關(guān)節(jié)型蛇形機(jī)械臂示意圖；

圖2為絕對(duì)坐標(biāo)系和參考坐標(biāo)系示意圖；

圖3為關(guān)節(jié)旋轉(zhuǎn)俯仰角θ后的姿態(tài)示意圖；

圖4為關(guān)節(jié)先旋轉(zhuǎn)俯仰角θ再旋轉(zhuǎn)偏轉(zhuǎn)角后的姿態(tài)示意圖；

圖5為機(jī)械臂的整體目標(biāo)空間姿態(tài)示意圖；

圖6為繩索及繩孔位置示意圖；

圖7為本發(fā)明方法流程圖。

具體實(shí)施方式

下面結(jié)合附圖及具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳細(xì)說(shuō)明。

本發(fā)明方法的原理如下：

每個(gè)關(guān)節(jié)的總長(zhǎng)為l+2d，其中l(wèi)為關(guān)節(jié)前后兩個(gè)端面的距離，d為關(guān)節(jié)端面到所緊鄰連接的萬(wàn)向節(jié)中心的距離。

建立以下基于幾何關(guān)系的運(yùn)動(dòng)學(xué)模型：

a)構(gòu)建絕對(duì)坐標(biāo)系，機(jī)械臂的各個(gè)關(guān)節(jié)在初始狀態(tài)下沿垂直于臂基座的同一直線(xiàn)布置時(shí)，第一個(gè)萬(wàn)向節(jié)的中心位于y軸上，與原點(diǎn)的距離為d，如圖2所示。

例如第一關(guān)節(jié)，以第一關(guān)節(jié)軸線(xiàn)與y軸重合時(shí)為初始位置，構(gòu)建固連在第一關(guān)節(jié)上的參考坐標(biāo)系。設(shè)第一關(guān)節(jié)首先繞x軸旋轉(zhuǎn)了一個(gè)角度θ，由于萬(wàn)向節(jié)的約束，等效于繞通過(guò)第一個(gè)萬(wàn)向節(jié)中心且平行于x軸的軸線(xiàn)旋轉(zhuǎn)，此時(shí)其自身的參考坐標(biāo)系也繞x軸旋轉(zhuǎn)了相同角度θ而變成如圖3所示。

然后在此基礎(chǔ)上再繞其自身的變化后的參考坐標(biāo)系的z軸旋轉(zhuǎn)一個(gè)角度，由于萬(wàn)向節(jié)的約束，等效于繞通過(guò)第一個(gè)萬(wàn)向節(jié)中心且平行于變化后的z軸的軸線(xiàn)旋轉(zhuǎn)，這樣就獲得了第一關(guān)節(jié)在任意轉(zhuǎn)動(dòng)后的一般姿態(tài)，如圖4所示。

為了從絕對(duì)坐標(biāo)系變換到第一關(guān)節(jié)轉(zhuǎn)動(dòng)后的參考坐標(biāo)系，首先需要將絕對(duì)坐標(biāo)系先沿y軸平移距離d，然后繞變化后的x軸旋轉(zhuǎn)角度θ，再繞變化后的z軸旋轉(zhuǎn)角度，最后繞變化后的y軸平移距離d+l。

沿y軸平移距離d的平移齊次變換矩陣為：

繞x軸旋轉(zhuǎn)俯仰角θ的旋轉(zhuǎn)齊次變換矩陣為：

繞z軸旋轉(zhuǎn)偏轉(zhuǎn)角的旋轉(zhuǎn)齊次變換矩陣為：

沿y軸平移距離l的平移齊次變換矩陣tl為：

依次進(jìn)行上述變換，即連續(xù)右乘各齊次變換矩陣，獲得從絕對(duì)坐標(biāo)系變換到第一關(guān)節(jié)轉(zhuǎn)動(dòng)后的參考坐標(biāo)系的齊次變換矩陣

其中包含了第一關(guān)節(jié)的轉(zhuǎn)動(dòng)角度θ和兩個(gè)變量。

對(duì)于從絕對(duì)坐標(biāo)系變換到第二關(guān)節(jié)的參考坐標(biāo)系，只需要在t1變換的基礎(chǔ)上將變量改為第二關(guān)節(jié)的轉(zhuǎn)動(dòng)角度θ2和此時(shí)旋轉(zhuǎn)齊次變換矩陣變?yōu)閞x2和rz2再重復(fù)一樣的變換步驟即可得到：

t2＝t1×td×rx2×rz2×td×tl

改記第一關(guān)節(jié)的轉(zhuǎn)動(dòng)角度為θ1和則t2中包含了θ1、θ2和四個(gè)變量。

以此類(lèi)推，可以獲得機(jī)械臂各關(guān)節(jié)的齊次變換矩陣t1、t2、…、tn：

tn＝tn-1×td×rxn×rzn×td×tl

其中包含了θ1、θ2、…、θn、共2n個(gè)變量。

在獲得各關(guān)節(jié)的齊次變換矩陣后，就可以計(jì)算經(jīng)過(guò)任意角度的轉(zhuǎn)動(dòng)后關(guān)節(jié)上穿繩處所在空間點(diǎn)的絕對(duì)坐標(biāo)，空間點(diǎn)相對(duì)第n關(guān)節(jié)參考坐標(biāo)系{n}的坐標(biāo)為(xn,yn,zn)，則空間點(diǎn)在絕對(duì)坐標(biāo)系下的絕對(duì)坐標(biāo)(x0,y0,z0)可由下式求出：

在獲得空間點(diǎn)的絕對(duì)坐標(biāo)后，就可以計(jì)算兩點(diǎn)間的距離，作為si-1,i。

每一根繩索都會(huì)固定到它所驅(qū)動(dòng)的關(guān)節(jié)的端面上，并且穿過(guò)前一個(gè)關(guān)節(jié)的端面上相應(yīng)的繩孔，在兩個(gè)關(guān)節(jié)間的繩索可以視為一條直線(xiàn)段，直線(xiàn)段的兩個(gè)端點(diǎn)分別位于前后兩個(gè)關(guān)節(jié)的端面上已知的確定位置，因此可以通過(guò)求出端點(diǎn)的絕對(duì)坐標(biāo)來(lái)計(jì)算兩個(gè)端點(diǎn)的距離，從而獲得繩索在兩個(gè)關(guān)節(jié)間的長(zhǎng)度。

在求出每根繩索在每?jī)蓚€(gè)關(guān)節(jié)間的長(zhǎng)度后，將所有的關(guān)節(jié)間繩索長(zhǎng)度相加，再加上所有的穿過(guò)關(guān)節(jié)的繩索長(zhǎng)度(即關(guān)節(jié)兩個(gè)端面的距離l)，就得到了每根繩索的總長(zhǎng)。對(duì)于驅(qū)動(dòng)第n關(guān)節(jié)的繩索，其總長(zhǎng)為：

其中si-1,i為第i-1關(guān)節(jié)和第i關(guān)節(jié)間的繩索長(zhǎng)度，當(dāng)i＝1時(shí)si-1,i表示基座端面和第一關(guān)節(jié)間的繩索長(zhǎng)度。由于最后一個(gè)關(guān)節(jié)中沒(méi)有繩索穿過(guò)，驅(qū)動(dòng)該關(guān)節(jié)的繩索僅固定到該關(guān)節(jié)的前端面，因此只加上其余關(guān)節(jié)中穿過(guò)的繩索長(zhǎng)度(n-1)×l。由于每個(gè)關(guān)節(jié)至少由三根繩索驅(qū)動(dòng)，根據(jù)不同的繩孔位置，至少有三個(gè)ln，且值不一定相同。

設(shè)每個(gè)關(guān)節(jié)由m根繩索驅(qū)動(dòng)，則總共需要求出m×n個(gè)值。在求出所有繩索各自對(duì)應(yīng)的繩索總長(zhǎng)l1、l2、…、lm×n后，只需要控制相應(yīng)的繩索拉到相應(yīng)的總長(zhǎng)，就可以控制機(jī)械臂運(yùn)動(dòng)到由變量θ1、θ2、…、θn、所確定的整體姿態(tài)，實(shí)現(xiàn)對(duì)機(jī)械臂空間姿態(tài)角度的控制。

本發(fā)明的實(shí)施例如下：

以總計(jì)6個(gè)關(guān)節(jié)，每個(gè)關(guān)節(jié)由3根繩索驅(qū)動(dòng)為例，機(jī)械臂每個(gè)關(guān)節(jié)都有18個(gè)繩孔，繩孔位于每個(gè)關(guān)節(jié)的兩個(gè)端面上，以關(guān)節(jié)軸線(xiàn)與端面的交點(diǎn)為圓心，等角度均布于半徑r＝0.042m的圓周上，如圖6所示。機(jī)械臂每個(gè)關(guān)節(jié)兩個(gè)端面的距離l＝0.147m，關(guān)節(jié)端面到萬(wàn)向節(jié)中心的距離d＝0.019m。

并設(shè)機(jī)械臂需要運(yùn)動(dòng)到θ1＝30°，θ2＝-30°，θ3＝-30°，θ4＝30°，θ5＝30°，θ6＝-30°，所有關(guān)節(jié)角度均為0°的姿態(tài)，如圖5所示。

由已知量獲得各齊次變換矩陣：

例如對(duì)于第一關(guān)節(jié)有：

求出從絕對(duì)坐標(biāo)系變換到第一關(guān)節(jié)轉(zhuǎn)動(dòng)后的參考坐標(biāo)系的齊次變換矩陣：

然后求出從絕對(duì)坐標(biāo)系變換到第二關(guān)節(jié)轉(zhuǎn)動(dòng)后的參考坐標(biāo)系的齊次變換矩陣：

以此類(lèi)推，依次求出t3、t4、t5、t6。

如圖6所示，然后計(jì)算繩索端點(diǎn)的絕對(duì)坐標(biāo)，以靠近基座的端面為前端面，繩索一端與驅(qū)動(dòng)機(jī)構(gòu)固定，另一端從前一個(gè)關(guān)節(jié)的后端面穿出，與繩索驅(qū)動(dòng)的關(guān)節(jié)的前端面固定。驅(qū)動(dòng)第一關(guān)節(jié)的繩索從基座端面穿出，固定到第一關(guān)節(jié)的前端面上；驅(qū)動(dòng)第二關(guān)節(jié)的繩索穿過(guò)基座后再穿過(guò)第一關(guān)節(jié)，從第一關(guān)節(jié)的后端面穿出，固定到第二關(guān)節(jié)的前端面上，以此類(lèi)推。

從第一關(guān)節(jié)與機(jī)械臂基座間的繩索開(kāi)始。對(duì)于第一根繩索，它在基座端面上的端點(diǎn)的絕對(duì)坐標(biāo)為(0,0,r)，即(0,0,0.042)，由于基座的參考坐標(biāo)系就是絕對(duì)坐標(biāo)系，不需要再轉(zhuǎn)換；第一根繩索在第一關(guān)節(jié)前端面上的參考坐標(biāo)系的坐標(biāo)為(0,-l,r)，即(0,-0.147,0.042)，則其絕對(duì)坐標(biāo)(x0,y0,z0)為：

獲得兩端點(diǎn)的絕對(duì)坐標(biāo)后就可以求出第一根繩索在基座與第一關(guān)節(jié)間的長(zhǎng)度，然后以此類(lèi)推，求出所有繩索在基座與第一關(guān)節(jié)間的長(zhǎng)度，再以此類(lèi)推，向后方遞進(jìn)，逐級(jí)求出各關(guān)節(jié)間的繩索長(zhǎng)度。最后對(duì)每一根繩索，將各關(guān)節(jié)間的長(zhǎng)度再加上穿過(guò)關(guān)節(jié)的繩索長(zhǎng)度，就獲得了單根繩索的總長(zhǎng)。列出求出的18根繩索相應(yīng)的總長(zhǎng)(單位為m)如下：

l1＝0.0150，l2＝0.2204，l3＝0.4208，l4＝0.5878，l5＝0.7678，l6＝0.9552，l7＝0.0476，l8＝0.2204，l9＝0.3837，l10＝0.5878，l11＝0.7920，l12＝0.9552，l13＝0.0476，l14＝0.2204，l15＝0.4079，l16＝0.5878，l17＝0.7549，l18＝0.9552。

驅(qū)動(dòng)機(jī)構(gòu)拉動(dòng)繩索，使繩索穿出基座端面的長(zhǎng)度分別為以上相應(yīng)的值，即可控制機(jī)械臂運(yùn)動(dòng)到給定的姿態(tài)。

由以上實(shí)施例可見(jiàn)，本發(fā)明能夠精確唯一地描述機(jī)械臂的整體空間姿態(tài)，實(shí)現(xiàn)解耦，不需要解方程組就能夠分別獨(dú)立求解出驅(qū)動(dòng)各關(guān)節(jié)的繩索長(zhǎng)度的變化，運(yùn)算量小，效率高，實(shí)時(shí)性好。