本發(fā)明實(shí)施例涉及機(jī)器人控制技術(shù)，尤其涉及一種確定串聯(lián)機(jī)器人反解結(jié)果的方法及裝置。

背景技術(shù)：

仿生機(jī)器人已應(yīng)用到很多領(lǐng)域，其可以代替或部分代替人去完成危險(xiǎn)環(huán)境中的作業(yè)，如地震災(zāi)害或者醫(yī)療行業(yè)，醫(yī)療行業(yè)中可以將機(jī)器人尺寸做到足夠小進(jìn)入人手無(wú)法達(dá)到的空間，如人體自然腔道(如食道)完成手術(shù)或定點(diǎn)給藥。但是，有些環(huán)境不能讓機(jī)器人完全自主運(yùn)行，如醫(yī)療手術(shù)環(huán)境或者非常復(fù)雜的地震災(zāi)區(qū)環(huán)境等，需要有經(jīng)驗(yàn)的人來(lái)操控這些機(jī)器人完成相應(yīng)的任務(wù)，這就要求使用主從控制系統(tǒng)來(lái)操控機(jī)器人。

在主從控制系統(tǒng)中，一般需要有主操作手、從操作手和控制系統(tǒng)。主操作手一般由人來(lái)操作，從操作手一般為串聯(lián)機(jī)器人，在主從機(jī)器人的控制系統(tǒng)中，對(duì)主、從機(jī)器人之間的實(shí)時(shí)性要求高。對(duì)于高實(shí)時(shí)性，主從機(jī)器人的控制系統(tǒng)中電機(jī)、控制器的響應(yīng)時(shí)間、主從機(jī)器人間的通信時(shí)間很短，滿足實(shí)時(shí)性要求比較容易，難點(diǎn)在于從串聯(lián)機(jī)器人的反解時(shí)間。

在現(xiàn)有技術(shù)中，對(duì)于串聯(lián)機(jī)器人，一般采用主從異構(gòu)的方式來(lái)進(jìn)行主從操作，主從異構(gòu)機(jī)器人大多采用采用雅克比算法進(jìn)行控制，拾取主手機(jī)器人在三維空間內(nèi)速度變化，再對(duì)從機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，計(jì)算其雅克比矩陣，對(duì)矩陣求逆。雅克比矩陣控制算法適用于從機(jī)器人自由度(串聯(lián)機(jī)器人的關(guān)節(jié)個(gè)數(shù))可以簡(jiǎn)化為3或者6的情況，但位移累積誤差大，在奇異位置時(shí)，雅克比矩陣會(huì)失效，因而對(duì)于高度冗余的串聯(lián)機(jī)器人(12自由度或者更多)，這種方法不適用。此外，數(shù)值法中的循環(huán)坐標(biāo)下降法、牛頓法、擬牛頓法、或者這三者中的兩兩結(jié)合、神經(jīng)網(wǎng)絡(luò)等雖然可以被使用，但這些算法求反解的速度慢，有局限性、計(jì)算和存儲(chǔ)上開(kāi)銷(xiāo)大，因而對(duì)于串聯(lián)機(jī)器人不實(shí)用。

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

有鑒于此，本發(fā)明實(shí)施例提供一種確定串聯(lián)機(jī)器人反解結(jié)果的方法及裝置，可以快速而準(zhǔn)確地求出串聯(lián)機(jī)器人的反解結(jié)果。

第一方面，本發(fā)明實(shí)施例提供了一種確定串聯(lián)機(jī)器人反解結(jié)果的方法，包括：

獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在所述末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)；

隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量；

根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)；

根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)；

根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果。

第二方面，本發(fā)明實(shí)施例還提供了一種確定串聯(lián)機(jī)器人反解結(jié)果的裝置，包括：

目標(biāo)坐標(biāo)獲取模塊，用于獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在所述末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)；

隨機(jī)關(guān)節(jié)變量組產(chǎn)生模塊，與所述目標(biāo)坐標(biāo)獲取模塊相連，用于隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量；

隨機(jī)坐標(biāo)計(jì)算模塊，與所述隨機(jī)關(guān)節(jié)變量組產(chǎn)生模塊相連，用于根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)；

近似坐標(biāo)選取模塊，與所述隨機(jī)坐標(biāo)計(jì)算模塊相連，用于根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)；

反解結(jié)果確定模塊，與所述近似坐標(biāo)選取模塊相連，用于根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果。

本發(fā)明實(shí)施例通過(guò)隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組，計(jì)算末端關(guān)節(jié)的隨機(jī)坐標(biāo)，在各隨機(jī)坐標(biāo)中選取末端關(guān)節(jié)的近似坐標(biāo)，來(lái)確定目標(biāo)坐標(biāo)的反解結(jié)果。本發(fā)明實(shí)施例中的方法適用于任意自由度的串聯(lián)機(jī)器人，求反解結(jié)果速度快，反解結(jié)果的精度高，計(jì)算和存儲(chǔ)上開(kāi)銷(xiāo)小，沒(méi)有累積誤差。

附圖說(shuō)明

圖1是本發(fā)明實(shí)施例一提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖；

圖2是本發(fā)明實(shí)施例二提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖；

圖3是本發(fā)明實(shí)施例三提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖；

圖4是本發(fā)明實(shí)施例四提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖；

圖5是本發(fā)明實(shí)施例五提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的裝置結(jié)構(gòu)示意圖；

圖6是本發(fā)明實(shí)施例六提供的用于實(shí)施確定串聯(lián)機(jī)器人反解結(jié)果的方法的主從機(jī)器人的閉環(huán)控制系統(tǒng)示意圖；

圖7是本發(fā)明實(shí)施例六提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法中的各關(guān)節(jié)坐標(biāo)系示意圖。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)說(shuō)明。可以理解的是，此處所描述的具體實(shí)施例僅僅用于解釋本發(fā)明，而非對(duì)本發(fā)明的限定。另外還需要說(shuō)明的是，為了便于描述，附圖中僅示出了與本發(fā)明相關(guān)的部分而非全部結(jié)構(gòu)。

實(shí)施例一

圖1是本發(fā)明實(shí)施例一提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖，本實(shí)施例可適用于已知串聯(lián)機(jī)器人末端坐標(biāo)求解各關(guān)節(jié)變量的情況，該方法可以由確定串聯(lián)機(jī)器人反解結(jié)果的裝置來(lái)執(zhí)行，該裝置可以由軟件和/或硬件方式實(shí)現(xiàn)，該裝置可以集成在任何提供存儲(chǔ)和運(yùn)算功能的設(shè)備中，例如典型的是工業(yè)PC機(jī)等。該方法包括：

S101、獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在所述末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)。串聯(lián)機(jī)器人以串聯(lián)的方式連接有多個(gè)關(guān)節(jié)，最低端的關(guān)節(jié)為末端關(guān)節(jié)，位于該末端關(guān)節(jié)之上的關(guān)節(jié)相對(duì)于該關(guān)節(jié)都可以被稱為前端關(guān)節(jié)。

可以理解的是，在對(duì)機(jī)器人進(jìn)行控制時(shí)，控制結(jié)果是通過(guò)末端關(guān)節(jié)的位置以及姿態(tài)來(lái)體現(xiàn)的，但是為了使末端關(guān)節(jié)能夠達(dá)到一個(gè)預(yù)設(shè)的位置以及姿態(tài)，需要各前端關(guān)節(jié)的共同配合運(yùn)動(dòng)才能實(shí)現(xiàn)。

在本實(shí)施例中，所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)，具體是指與期望末端關(guān)節(jié)能夠達(dá)到的位置以及姿態(tài)的坐標(biāo)表示。在工業(yè)PC機(jī)獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)后，通過(guò)下面的步驟計(jì)算各個(gè)前端關(guān)節(jié)的關(guān)節(jié)變量(典型的位置以及姿態(tài))，實(shí)現(xiàn)最終能夠?qū)⑺瞿┒岁P(guān)節(jié)調(diào)整至該目標(biāo)坐標(biāo)之上。

S102、隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量。

每個(gè)關(guān)節(jié)都有對(duì)應(yīng)的關(guān)節(jié)變量，它表示該關(guān)節(jié)的運(yùn)動(dòng)情況，整個(gè)串聯(lián)機(jī)器人的各個(gè)關(guān)節(jié)的運(yùn)動(dòng)情況可以用關(guān)節(jié)變量組表示，關(guān)節(jié)變量組中包括各個(gè)前端關(guān)節(jié)的關(guān)節(jié)變量。當(dāng)隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組時(shí)，與之對(duì)應(yīng)的末端的位置和姿態(tài)也是隨機(jī)的，得到的末端的位置和姿態(tài)有可能是理想的，也有可能是不理想的，經(jīng)過(guò)篩選后才能得到需要的。隨機(jī)關(guān)節(jié)變量組可由隨機(jī)函數(shù)產(chǎn)生，隨機(jī)關(guān)節(jié)變量可根據(jù)關(guān)節(jié)的實(shí)際運(yùn)動(dòng)范圍限制在一定范圍。例如當(dāng)關(guān)節(jié)變量為關(guān)節(jié)角時(shí)，可通過(guò)公式θ_n＝θ_Min-n+θ_Max-n×Random()產(chǎn)生各個(gè)隨機(jī)關(guān)節(jié)角，Random()為一種隨機(jī)函數(shù)，用于產(chǎn)生一個(gè)隨機(jī)數(shù)。θ_n為第n個(gè)關(guān)節(jié)的關(guān)節(jié)角，θ_Min-n為第n個(gè)關(guān)節(jié)的最小關(guān)節(jié)角，θ_Max-n為第n個(gè)關(guān)節(jié)的最大關(guān)節(jié)角，并組成隨機(jī)關(guān)節(jié)角組(θ₁，θ₂，…，θ_N),n∈[1,N],N為隨機(jī)關(guān)節(jié)變量中包括的關(guān)節(jié)角總數(shù)。其與前端關(guān)節(jié)的總數(shù)量相一致

產(chǎn)生的隨機(jī)關(guān)節(jié)變量組的組數(shù)可根據(jù)求解的精確度設(shè)置，當(dāng)增大隨機(jī)關(guān)節(jié)變量組的組數(shù)時(shí)，能提高結(jié)果的精確度。

當(dāng)串聯(lián)機(jī)器人的關(guān)節(jié)數(shù)增加一個(gè)時(shí)，只是在產(chǎn)生的隨機(jī)關(guān)節(jié)變量組里增加一個(gè)關(guān)節(jié)變量，計(jì)算的復(fù)雜度只是線性增加，所以本實(shí)施例適用于任意關(guān)節(jié)數(shù)(自由度)的串聯(lián)機(jī)器人。

S103、根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)。

當(dāng)各個(gè)前端關(guān)節(jié)的關(guān)節(jié)變量確定了，末端關(guān)節(jié)的位置和姿態(tài)就確定了，也即確定了末端關(guān)節(jié)的坐標(biāo)。例如可建立基坐標(biāo)系和關(guān)節(jié)坐標(biāo)系，由關(guān)節(jié)之間的連接關(guān)系確定各個(gè)坐標(biāo)系之間的變換矩陣及末端關(guān)節(jié)到基坐標(biāo)系的坐標(biāo)變換矩陣，通過(guò)把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到坐標(biāo)變換矩陣，得到對(duì)應(yīng)的多個(gè)末端關(guān)節(jié)的隨機(jī)坐標(biāo)。

S104、根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)。

因?yàn)殡S機(jī)關(guān)節(jié)變量組有多組，所以對(duì)應(yīng)的末端關(guān)節(jié)的隨機(jī)坐標(biāo)有多個(gè)。隨機(jī)坐標(biāo)和目標(biāo)坐標(biāo)的差異有大有小，隨機(jī)關(guān)節(jié)變量組是隨機(jī)產(chǎn)生的，隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)相同的概率很小?？稍O(shè)置誤差閾值，將誤差閾值范圍內(nèi)的隨機(jī)坐標(biāo)作為末端關(guān)節(jié)的近似坐標(biāo)，便于做進(jìn)一步篩選。

因?yàn)檎`差閾值可人為設(shè)置，所以可以控制結(jié)果的誤差。本實(shí)施例中的結(jié)果是通過(guò)隨機(jī)的實(shí)驗(yàn)試出來(lái)的，而不是由計(jì)算得出，所以沒(méi)有累積誤差。

其中，選取的近似坐標(biāo)的數(shù)量可以為1個(gè)也可以為多個(gè)。其中，可以將與目標(biāo)坐標(biāo)之間的誤差值最小的一個(gè)隨機(jī)坐標(biāo)作為所述近似坐標(biāo)；也可以將與目標(biāo)坐標(biāo)之間的誤差值滿足預(yù)設(shè)誤差條件的多個(gè)隨機(jī)坐標(biāo)作為所述近似坐標(biāo)，本實(shí)施例對(duì)此并不進(jìn)行限制。

S105、根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果。

末端關(guān)節(jié)的近似坐標(biāo)有對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，當(dāng)各個(gè)關(guān)節(jié)按該隨機(jī)關(guān)節(jié)變量組中的關(guān)節(jié)變量進(jìn)行運(yùn)動(dòng)時(shí)，可使末端關(guān)節(jié)處于與目標(biāo)坐標(biāo)相接近的近似坐標(biāo)對(duì)應(yīng)的位置上。所以可將近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組作為目標(biāo)坐標(biāo)的反解結(jié)果。

本實(shí)施例通過(guò)隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組，計(jì)算末端關(guān)節(jié)的隨機(jī)坐標(biāo)，在各隨機(jī)坐標(biāo)中選取末端關(guān)節(jié)的近似坐標(biāo)，來(lái)確定目標(biāo)坐標(biāo)的反解結(jié)果。本實(shí)施例中的方法適用于任意自由度的串聯(lián)機(jī)器人，求反解結(jié)果速度快，反解結(jié)果的精度高，計(jì)算和存儲(chǔ)上開(kāi)銷(xiāo)小，沒(méi)有累積誤差。

實(shí)施例二

圖2是本發(fā)明實(shí)施例二提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖，實(shí)施例二以上述實(shí)施例為基礎(chǔ)，將根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)，優(yōu)化為，建立串聯(lián)機(jī)器人的基坐標(biāo)系、每一個(gè)關(guān)節(jié)的關(guān)節(jié)坐標(biāo)系，其中，i∈[1,n]；n為所述串聯(lián)機(jī)器人包括的關(guān)節(jié)總數(shù)，根據(jù)所述基坐標(biāo)系，以及所述關(guān)節(jié)坐標(biāo)系，確定各個(gè)坐標(biāo)系之間的變換矩陣，根據(jù)公式計(jì)算所述末端關(guān)節(jié)在所述基坐標(biāo)系中的變換矩陣，作為所述預(yù)設(shè)的坐標(biāo)變換矩陣，把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到預(yù)設(shè)的坐標(biāo)變換矩陣中，得到與所述末端關(guān)節(jié)對(duì)應(yīng)的多個(gè)隨機(jī)坐標(biāo)。

本實(shí)施例提供的確定串聯(lián)機(jī)器人反解結(jié)果的方法包括以下步驟：S201、S202、S203、S204、S205、S206、S207和S208。其中，S201、S202、S207和S208分別與實(shí)施例一中的S101、S102、S104和S105相同，相同的步驟不再贅述。

S201、獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在所述末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)。

S202、隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量。

S203、建立串聯(lián)機(jī)器人的基坐標(biāo)系、每一個(gè)關(guān)節(jié)的關(guān)節(jié)坐標(biāo)系，其中，i∈[1,n]；n為所述串聯(lián)機(jī)器人包括的關(guān)節(jié)總數(shù)。

例如可根據(jù)空間機(jī)器人的連桿參數(shù)，應(yīng)用D-H建模方法，建立串聯(lián)機(jī)器人的基坐標(biāo)系O₀(x₀,y₀,z₀)、每一個(gè)關(guān)節(jié)的關(guān)節(jié)坐標(biāo)系O_i(x_i,y_i,z_i)。每個(gè)關(guān)節(jié)坐標(biāo)系中的關(guān)節(jié)變量表示了該關(guān)節(jié)的運(yùn)動(dòng)情況。關(guān)節(jié)坐標(biāo)系中的關(guān)節(jié)變量可統(tǒng)一為用基坐標(biāo)表示。

S204、根據(jù)所述基坐標(biāo)系，以及所述關(guān)節(jié)坐標(biāo)系，確定各個(gè)坐標(biāo)系之間的變換矩陣。

關(guān)節(jié)之間的連接關(guān)系確定各個(gè)坐標(biāo)系之間的變換矩陣某個(gè)關(guān)節(jié)坐標(biāo)系的變量可用相鄰關(guān)節(jié)坐標(biāo)系表示，各個(gè)關(guān)節(jié)坐標(biāo)系經(jīng)幾次變換后都可轉(zhuǎn)換成用基坐標(biāo)系表示。

S205、根據(jù)公式計(jì)算所述末端關(guān)節(jié)在所述基坐標(biāo)系中的變換矩陣，作為所述預(yù)設(shè)的坐標(biāo)變換矩陣。

每組隨機(jī)關(guān)節(jié)變量組對(duì)應(yīng)著末端關(guān)節(jié)的隨機(jī)坐標(biāo)，這個(gè)對(duì)應(yīng)關(guān)系可以是末端關(guān)節(jié)在所述基坐標(biāo)系中的變換矩陣，串聯(lián)機(jī)器人確定后，各個(gè)關(guān)節(jié)之間的連接關(guān)系也就確定了，預(yù)設(shè)的坐標(biāo)變換矩陣就確定了。末端到基坐標(biāo)系O₀(x₀,y₀,z₀)的坐標(biāo)變換矩陣可由各個(gè)坐標(biāo)系之間的變換矩陣逐項(xiàng)右乘得到，

其中，d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z用于表示末端關(guān)節(jié)的姿態(tài)，p_x、p_y及p_z用于表示末端關(guān)節(jié)的位置；

S206、把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到預(yù)設(shè)的坐標(biāo)變換矩陣中，得到與所述末端關(guān)節(jié)對(duì)應(yīng)的多個(gè)隨機(jī)坐標(biāo)。

因?yàn)轭A(yù)設(shè)的坐標(biāo)變換矩陣可由各個(gè)坐標(biāo)系之間的變換矩陣逐項(xiàng)右乘得到，所以預(yù)設(shè)的坐標(biāo)變換矩陣中的d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z、p_x、p_y及p_z的取值是各個(gè)關(guān)節(jié)變量的函數(shù)，把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到預(yù)設(shè)的坐標(biāo)變換矩陣中，得到與所述末端關(guān)節(jié)對(duì)應(yīng)的多個(gè)隨機(jī)坐標(biāo)。隨機(jī)關(guān)節(jié)變量組表示各個(gè)關(guān)節(jié)的運(yùn)動(dòng)情況，預(yù)設(shè)的坐標(biāo)變換矩陣表示關(guān)節(jié)之間的連接關(guān)系，二者結(jié)合可得到對(duì)應(yīng)的隨機(jī)坐標(biāo)。接下來(lái)就可判斷得到的末端的隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)是否接近。

S207、根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)。

S208、根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果。

本實(shí)施例通過(guò)把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到預(yù)設(shè)的坐標(biāo)變換矩陣中，得到與所述末端關(guān)節(jié)對(duì)應(yīng)的多個(gè)隨機(jī)坐標(biāo)，通過(guò)檢驗(yàn)隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)的差異即可確定對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組是否可作為反解，簡(jiǎn)化了串聯(lián)機(jī)器人求反解的過(guò)程。

實(shí)施例三

圖3是本發(fā)明實(shí)施例三提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖，實(shí)施例三以上述實(shí)施例為基礎(chǔ)，將根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)，優(yōu)化為，計(jì)算所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)與所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值，獲取所述誤差值小于誤差閾值的末端關(guān)節(jié)的隨機(jī)坐標(biāo)，作為所述末端關(guān)節(jié)的近似坐標(biāo)。

本實(shí)施例提供的確定串聯(lián)機(jī)器人反解結(jié)果的方法包括以下步驟：S301、S302、S303、S304、S305和S306。其中，S301、S302、S303和S306分別與實(shí)施例一中的S101、S102、S103和S105相同，相同的步驟不再贅述。

S301、獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在所述末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)。

S302、隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量。

S303、根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)。

S304、計(jì)算所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)與所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值。

末端關(guān)節(jié)的隨機(jī)坐標(biāo)有多個(gè)，它們與末端關(guān)節(jié)的目標(biāo)坐標(biāo)之間的差異可用誤差值表示。末端的坐標(biāo)包含末端的位置和姿態(tài)，所以誤差值包括位置誤差和姿態(tài)誤差。因?yàn)槟┒说淖鴺?biāo)是關(guān)節(jié)變量的函數(shù)，且末端關(guān)節(jié)的目標(biāo)坐標(biāo)是已知量，所以誤差值是隨機(jī)關(guān)節(jié)變量的函數(shù)。

進(jìn)一步地，計(jì)算所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)與所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值，包括：

根據(jù)公式：ΔE＝ΔP+ΔR；以及

計(jì)算所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)與所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值ΔE。

其中，ΔP表示位置差異，ΔR表示姿態(tài)差異，ω_ρ和ω_γ為所述位置差異ΔP和所述姿態(tài)差異ΔR的權(quán)重，P_s為末端關(guān)節(jié)的目標(biāo)位置，p_sj為末端關(guān)節(jié)的目標(biāo)位置分量，d_sj為末端關(guān)節(jié)的目標(biāo)姿態(tài)，P_s、p_sj及d_sj可從所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)獲得，P_r為末端關(guān)節(jié)的隨機(jī)位置，p_rj為末端關(guān)節(jié)的隨機(jī)位置分量，d_rj為末端關(guān)節(jié)的隨機(jī)姿態(tài)，P_r、p_rj及d_rj可從所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)中獲得。

其中，ω_γ權(quán)重大小的選擇應(yīng)足以使姿態(tài)誤差與位置誤差同步收斂，一般應(yīng)該大于10⁴；如果機(jī)器人的運(yùn)動(dòng)狀態(tài)對(duì)末端位置的精度要求更高一些，則ω_γ的取值一般為0；若是對(duì)位置和姿態(tài)都需要滿足一定的精度，則ω_γ的取值一般為10⁶。

S305、獲取所述誤差值小于誤差閾值的末端關(guān)節(jié)的隨機(jī)坐標(biāo)，作為所述末端關(guān)節(jié)的近似坐標(biāo)。

當(dāng)末端關(guān)節(jié)的隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)的誤差值小于誤差閾值，說(shuō)明末端關(guān)節(jié)的隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)很近似，所以可將該末端關(guān)節(jié)的隨機(jī)坐標(biāo)作為末端關(guān)節(jié)的近似坐標(biāo)。

S306、根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果。

本實(shí)施例通過(guò)計(jì)算末端關(guān)節(jié)的目標(biāo)坐標(biāo)與末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值，可知與末端關(guān)節(jié)的隨機(jī)坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組能否作為反解，能方便且有效地篩選出反解結(jié)果。

實(shí)施例四

圖4是本發(fā)明實(shí)施例四提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法流程圖，實(shí)施例四以上述實(shí)施例為基礎(chǔ)，將根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果，優(yōu)化為，獲取與至少兩個(gè)所述近似坐標(biāo)分別對(duì)應(yīng)隨機(jī)關(guān)節(jié)變量組，作為待選隨機(jī)關(guān)節(jié)變量組；選取距離所述末端關(guān)節(jié)最遠(yuǎn)的一個(gè)前端關(guān)節(jié)，作為當(dāng)前比對(duì)關(guān)節(jié)；分別在各所述待選隨機(jī)關(guān)節(jié)變量組中，獲取與所述當(dāng)前比對(duì)關(guān)節(jié)對(duì)應(yīng)的關(guān)節(jié)變量；根據(jù)所述關(guān)節(jié)變量，確定與各所述待選隨機(jī)關(guān)節(jié)變量組分別對(duì)應(yīng)的當(dāng)前比對(duì)關(guān)節(jié)移動(dòng)量；保留與所述關(guān)節(jié)移動(dòng)量的最小值對(duì)應(yīng)的待選隨機(jī)關(guān)節(jié)變量組，并過(guò)濾掉其余待選隨機(jī)關(guān)節(jié)變量組；如果保留的所述待選隨機(jī)關(guān)節(jié)變量組的數(shù)量值大于1，則朝著靠近所述末端關(guān)節(jié)的方向，選取新的前端關(guān)節(jié)作為所述當(dāng)前比對(duì)關(guān)節(jié)，并返回執(zhí)行分別在各所述待選隨機(jī)關(guān)節(jié)變量組中，獲取與所述當(dāng)前比對(duì)關(guān)節(jié)對(duì)應(yīng)的關(guān)節(jié)變量的操作，直至最終保留的所述待選隨機(jī)關(guān)節(jié)變量組的數(shù)量值為1；將最終保留的所述待選隨機(jī)關(guān)節(jié)變量組，作為所述目標(biāo)坐標(biāo)的反解結(jié)果。

本實(shí)施例提供的確定串聯(lián)機(jī)器人反解結(jié)果的方法包括以下步驟：S401、S402、S403、S404、S405、S406、S407、S408、S409、S410和S411。其中，S401、S402、S403和S404分別與實(shí)施例一中的S101、S102、S103及S104相同，相同的步驟不再贅述。

S401、獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)。

S402、隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量。

S403、根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)。

S404、根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)。

S405、獲取與至少兩個(gè)所述近似坐標(biāo)分別對(duì)應(yīng)隨機(jī)關(guān)節(jié)變量組，作為待選隨機(jī)關(guān)節(jié)變量組。

滿足誤差閾值的末端關(guān)節(jié)的近似坐標(biāo)有多個(gè)，與之對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組也有多個(gè)，這些隨機(jī)關(guān)節(jié)變量組可以作為目標(biāo)坐標(biāo)的反解結(jié)果，但它們未必是最優(yōu)的反解結(jié)果。可以從待選隨機(jī)關(guān)節(jié)變量組中進(jìn)一步選出最優(yōu)反解結(jié)果。

S406-S411為從待選隨機(jī)關(guān)節(jié)變量組中選出最優(yōu)反解結(jié)果的過(guò)程。關(guān)節(jié)移動(dòng)量越小時(shí)所消耗的能量越少，所以要以關(guān)節(jié)移動(dòng)量小的待選隨機(jī)關(guān)節(jié)變量組作為最優(yōu)反解結(jié)果。串聯(lián)機(jī)器人有很多個(gè)關(guān)節(jié)，在關(guān)節(jié)移動(dòng)量相同的情況下，遠(yuǎn)離末端關(guān)節(jié)的關(guān)節(jié)消耗的能量比靠近末端關(guān)節(jié)的關(guān)節(jié)消耗的能量多，所以在尋找最優(yōu)反解結(jié)果時(shí)要優(yōu)先保證遠(yuǎn)離末端關(guān)節(jié)的關(guān)節(jié)移動(dòng)量小。綜上，可從離末端關(guān)節(jié)最遠(yuǎn)的關(guān)節(jié)開(kāi)始，朝著靠近末端關(guān)節(jié)的方向，逐個(gè)關(guān)節(jié)地找最小的關(guān)節(jié)移動(dòng)量。

S406、選取距離所述末端關(guān)節(jié)最遠(yuǎn)的一個(gè)前端關(guān)節(jié)，作為當(dāng)前比對(duì)關(guān)節(jié)。

一種從待選隨機(jī)關(guān)節(jié)變量組中進(jìn)一步選出最優(yōu)反解結(jié)果的方法是，選關(guān)節(jié)移動(dòng)量最小的待選隨機(jī)關(guān)節(jié)變量組作為最優(yōu)反解結(jié)果。因?yàn)榇?lián)機(jī)器人有多個(gè)關(guān)節(jié)，可以逐個(gè)關(guān)節(jié)地找關(guān)節(jié)移動(dòng)量的最小值。當(dāng)關(guān)節(jié)移動(dòng)量相同時(shí)，遠(yuǎn)離末端的關(guān)節(jié)消耗的能量比靠近末端的關(guān)節(jié)消耗得多，所以優(yōu)先保證離末端遠(yuǎn)的關(guān)節(jié)的關(guān)節(jié)移動(dòng)量小。所以開(kāi)始時(shí)選取距離末端關(guān)節(jié)最遠(yuǎn)的前端關(guān)節(jié)作為當(dāng)前比對(duì)關(guān)節(jié)。

S407、分別在各所述待選隨機(jī)關(guān)節(jié)變量組中，獲取與所述當(dāng)前比對(duì)關(guān)節(jié)對(duì)應(yīng)的關(guān)節(jié)變量。

待選隨機(jī)關(guān)節(jié)變量組中包括各個(gè)關(guān)節(jié)的關(guān)節(jié)變量，分別對(duì)應(yīng)各個(gè)關(guān)節(jié)。每個(gè)待選隨機(jī)關(guān)節(jié)變量組對(duì)應(yīng)著一組關(guān)節(jié)關(guān)節(jié)變量。因?yàn)槭侵饌€(gè)關(guān)節(jié)進(jìn)行比較的，所以每次比較的時(shí)候需獲取當(dāng)前比對(duì)關(guān)節(jié)的關(guān)節(jié)變量。

S408、根據(jù)所述關(guān)節(jié)變量，確定與各所述待選隨機(jī)關(guān)節(jié)變量組分別對(duì)應(yīng)的當(dāng)前比對(duì)關(guān)節(jié)移動(dòng)量。

關(guān)節(jié)移動(dòng)量越小消耗的能量越少，根據(jù)關(guān)節(jié)變量確定當(dāng)前比對(duì)關(guān)節(jié)的關(guān)節(jié)移動(dòng)量。

S409、保留與關(guān)節(jié)移動(dòng)量的最小值對(duì)應(yīng)的待選隨機(jī)關(guān)節(jié)變量組，并過(guò)濾掉其余待選隨機(jī)關(guān)節(jié)變量組。

待選隨機(jī)關(guān)節(jié)變量組根據(jù)當(dāng)前比對(duì)關(guān)節(jié)的關(guān)節(jié)移動(dòng)量進(jìn)行一次優(yōu)選，找出當(dāng)前比對(duì)關(guān)節(jié)的關(guān)節(jié)移動(dòng)量的最小值，保留該最小值對(duì)應(yīng)的待選隨機(jī)關(guān)節(jié)變量組。當(dāng)保留的待選隨機(jī)關(guān)節(jié)變量組為一組，該待選隨機(jī)關(guān)節(jié)變量組即為最優(yōu)的反解結(jié)果。當(dāng)保留的待選隨機(jī)關(guān)節(jié)變量組不止一組時(shí)，還需進(jìn)行進(jìn)一步優(yōu)選。

S410、判斷保留的待選隨機(jī)關(guān)節(jié)變量組的數(shù)量值是否大于1，如果大于1則返回執(zhí)行步驟S406，如果不大于1則繼續(xù)執(zhí)行步驟S411。

如果保留的待選隨機(jī)關(guān)節(jié)變量組的數(shù)量值大于1，說(shuō)明沒(méi)有找到最優(yōu)反解結(jié)果，則返回執(zhí)行步驟S406，朝著靠近末端關(guān)節(jié)的方向，選取新的前端關(guān)節(jié)作為當(dāng)前比對(duì)關(guān)節(jié)，選取該當(dāng)前比對(duì)關(guān)節(jié)的關(guān)節(jié)移動(dòng)量最小值對(duì)應(yīng)的保留的待選隨機(jī)關(guān)節(jié)變量組。如果不大于1則說(shuō)明最終保留的那個(gè)待選隨機(jī)關(guān)節(jié)變量組是最優(yōu)的，繼續(xù)執(zhí)行步驟S411。

S411、將最終保留的所述待選隨機(jī)關(guān)節(jié)變量組，作為所述目標(biāo)坐標(biāo)的反解結(jié)果。

經(jīng)過(guò)在各個(gè)關(guān)節(jié)選取最小關(guān)節(jié)移動(dòng)量后，最終保留的待選隨機(jī)關(guān)節(jié)變量組在各個(gè)關(guān)節(jié)的關(guān)節(jié)移動(dòng)量都是最小的。最終保留的待選隨機(jī)關(guān)節(jié)變量組對(duì)應(yīng)的末端關(guān)節(jié)的隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)之間的誤差在誤差閾值范圍內(nèi)，最終保留的待選隨機(jī)關(guān)節(jié)變量組的關(guān)節(jié)移動(dòng)的能量消耗是最小的，可作為目標(biāo)坐標(biāo)的反解結(jié)果，而且是最優(yōu)的。

本實(shí)施例從距離末端關(guān)節(jié)最遠(yuǎn)的前端關(guān)節(jié)開(kāi)始，朝著靠近末端關(guān)節(jié)的方向，保留與關(guān)節(jié)移動(dòng)量的最小值對(duì)應(yīng)的待選隨機(jī)關(guān)節(jié)變量組，可找到使串聯(lián)機(jī)器人移動(dòng)關(guān)節(jié)時(shí)消耗最少能量的最優(yōu)反解結(jié)果。

實(shí)施例五

圖5是本發(fā)明實(shí)施例五提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的裝置結(jié)構(gòu)示意圖，該裝置用于執(zhí)行上述實(shí)施例中的確定串聯(lián)機(jī)器人反解結(jié)果的方法。該裝置包括：目標(biāo)坐標(biāo)獲取模塊501、隨機(jī)關(guān)節(jié)變量組產(chǎn)生模塊502、隨機(jī)坐標(biāo)計(jì)算模塊503、近似坐標(biāo)選取模塊504和反解結(jié)果確定模塊505。

目標(biāo)坐標(biāo)獲取模塊501，用于獲取末端關(guān)節(jié)的目標(biāo)坐標(biāo)；其中，在所述末端關(guān)節(jié)之上，以串聯(lián)的方式連接有至少一個(gè)前端關(guān)節(jié)。

隨機(jī)關(guān)節(jié)變量組產(chǎn)生模塊502，與所述目標(biāo)坐標(biāo)獲取模塊501相連，用于隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組；其中，每個(gè)隨機(jī)關(guān)節(jié)變量組中包括與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量。

隨機(jī)坐標(biāo)計(jì)算模塊503，與所述隨機(jī)關(guān)節(jié)變量組產(chǎn)生模塊502相連，用于根據(jù)各所述隨機(jī)關(guān)節(jié)變量組，計(jì)算所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)。

近似坐標(biāo)選取模塊504，與所述隨機(jī)坐標(biāo)計(jì)算模塊503相連，用于根據(jù)所述目標(biāo)坐標(biāo)，在各所述隨機(jī)坐標(biāo)中選取所述末端關(guān)節(jié)的近似坐標(biāo)。

反解結(jié)果確定模塊505，與所述近似坐標(biāo)選取模塊504相連，用于根據(jù)與所述近似坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組，采用優(yōu)化算法確定所述目標(biāo)坐標(biāo)的反解結(jié)果。

本實(shí)施例通過(guò)隨機(jī)產(chǎn)生多個(gè)隨機(jī)關(guān)節(jié)變量組，計(jì)算末端關(guān)節(jié)的隨機(jī)坐標(biāo)，在各隨機(jī)坐標(biāo)中選取末端關(guān)節(jié)的近似坐標(biāo)，來(lái)確定目標(biāo)坐標(biāo)的反解結(jié)果。本實(shí)施例中的方法適用于任意自由度的串聯(lián)機(jī)器人，求反解結(jié)果速度快，反解結(jié)果的精度高，計(jì)算和存儲(chǔ)上開(kāi)銷(xiāo)小，沒(méi)有累積誤差。

進(jìn)一步地，所述隨機(jī)坐標(biāo)計(jì)算模塊包括代入坐標(biāo)變換矩陣單元，用于把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到預(yù)設(shè)的坐標(biāo)變換矩陣中，得到與所述末端關(guān)節(jié)對(duì)應(yīng)的多個(gè)隨機(jī)坐標(biāo)。

進(jìn)一步地，所述隨機(jī)坐標(biāo)計(jì)算模塊還包括坐標(biāo)系建立單元、變換矩陣確定單元及坐標(biāo)變換矩陣計(jì)算單元。

坐標(biāo)系建立單元，用于建立所述串聯(lián)機(jī)器人的基坐標(biāo)系O₀(x₀,y₀,z₀)、每一個(gè)關(guān)節(jié)的關(guān)節(jié)坐標(biāo)系O_i(x_i,y_i,z_i)，其中，i∈[1,n]；n為所述串聯(lián)機(jī)器人包括的關(guān)節(jié)總數(shù)。

變換矩陣確定單元，與所述坐標(biāo)系建立單元相連，用于根據(jù)所述基坐標(biāo)系，以及所述關(guān)節(jié)坐標(biāo)系，確定各個(gè)坐標(biāo)系之間的變換矩陣

坐標(biāo)變換矩陣計(jì)算單元，分別與所述變換矩陣確定單元及所述代入坐標(biāo)變換矩陣單元相連，用于根據(jù)公式計(jì)算所述末端關(guān)節(jié)在所述基坐標(biāo)系O₀(x₀,y₀,z₀)中的變換矩陣將所述作為所述預(yù)設(shè)的坐標(biāo)變換矩陣。

其中，d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z用于表示末端關(guān)節(jié)的姿態(tài)，p_x、p_y及p_z用于表示末端關(guān)節(jié)的位置；所述d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z、p_x、p_y及p_z的取值，由與各所述前端關(guān)節(jié)分別對(duì)應(yīng)的關(guān)節(jié)變量確定。

本實(shí)施例通過(guò)把每個(gè)隨機(jī)關(guān)節(jié)變量組代入到預(yù)設(shè)的坐標(biāo)變換矩陣中，得到與所述末端關(guān)節(jié)對(duì)應(yīng)的多個(gè)隨機(jī)坐標(biāo)，通過(guò)檢驗(yàn)隨機(jī)坐標(biāo)與目標(biāo)坐標(biāo)的差異即可確定對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組是否可作為反解，簡(jiǎn)化了串聯(lián)機(jī)器人求反解的過(guò)程。

進(jìn)一步地，所述近似坐標(biāo)選取模塊包括誤差值計(jì)算單元及誤差值判斷單元。

誤差值計(jì)算單元，用于計(jì)算所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)與所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值。

誤差值判斷單元，與所述誤差值計(jì)算單元相連，用于獲取所述誤差值小于誤差閾值的末端關(guān)節(jié)的隨機(jī)坐標(biāo)，作為所述末端關(guān)節(jié)的近似坐標(biāo)。

本實(shí)施例通過(guò)計(jì)算末端關(guān)節(jié)的目標(biāo)坐標(biāo)與末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值，可知與末端關(guān)節(jié)的隨機(jī)坐標(biāo)對(duì)應(yīng)的隨機(jī)關(guān)節(jié)變量組能否作為反解，能方便且有效地篩選出反解結(jié)果。

進(jìn)一步地，反解結(jié)果確定模塊包括待選隨機(jī)關(guān)節(jié)變量組獲取單元、當(dāng)前比對(duì)關(guān)節(jié)選取單元、關(guān)節(jié)變量獲取單元、移動(dòng)量確定單元、篩選隨機(jī)關(guān)節(jié)變量組單元、縮減隨機(jī)關(guān)節(jié)變量組數(shù)量值單元及保留隨機(jī)關(guān)節(jié)變量組單元。

待選隨機(jī)關(guān)節(jié)變量組獲取單元，用于獲取與至少兩個(gè)所述近似坐標(biāo)分別對(duì)應(yīng)隨機(jī)關(guān)節(jié)變量組，作為待選隨機(jī)關(guān)節(jié)變量組。

當(dāng)前比對(duì)關(guān)節(jié)選取單元，與所述待選隨機(jī)關(guān)節(jié)變量組獲取單元相連，用于選取距離所述末端關(guān)節(jié)最遠(yuǎn)的一個(gè)前端關(guān)節(jié)，作為當(dāng)前比對(duì)關(guān)節(jié)。

關(guān)節(jié)變量獲取單元，與所述當(dāng)前比對(duì)關(guān)節(jié)選取單元相連，用于分別在各所述待選隨機(jī)關(guān)節(jié)變量組中，獲取與所述當(dāng)前比對(duì)關(guān)節(jié)對(duì)應(yīng)的關(guān)節(jié)變量。

移動(dòng)量確定單元，與所述關(guān)節(jié)變量獲取單元相連，用于根據(jù)所述關(guān)節(jié)變量，確定與各所述待選隨機(jī)關(guān)節(jié)變量組分別對(duì)應(yīng)的當(dāng)前比對(duì)關(guān)節(jié)移動(dòng)量。

篩選隨機(jī)關(guān)節(jié)變量組單元，與所述移動(dòng)量確定單元相連，用于保留與所述關(guān)節(jié)移動(dòng)量的最小值對(duì)應(yīng)的待選隨機(jī)關(guān)節(jié)變量組，并過(guò)濾掉其余待選隨機(jī)關(guān)節(jié)變量組。

縮減隨機(jī)關(guān)節(jié)變量組數(shù)量值單元，與所述篩選隨機(jī)關(guān)節(jié)變量組單元相連，用于如果保留的所述待選隨機(jī)關(guān)節(jié)變量組的數(shù)量值大于1，則朝著靠近所述末端關(guān)節(jié)的方向，選取新的前端關(guān)節(jié)作為所述當(dāng)前比對(duì)關(guān)節(jié)，并返回執(zhí)行分別在各所述待選隨機(jī)關(guān)節(jié)變量組中，獲取與所述當(dāng)前比對(duì)關(guān)節(jié)對(duì)應(yīng)的關(guān)節(jié)變量的操作，直至最終保留的所述待選隨機(jī)關(guān)節(jié)變量組的數(shù)量值為1。

保留隨機(jī)關(guān)節(jié)變量組單元，與所述縮減隨機(jī)關(guān)節(jié)變量組數(shù)量值單元相連，用于將最終保留的所述待選隨機(jī)關(guān)節(jié)變量組，作為所述目標(biāo)坐標(biāo)的反解結(jié)果。

本實(shí)施例從距離末端關(guān)節(jié)最遠(yuǎn)的前端關(guān)節(jié)開(kāi)始，朝著靠近末端關(guān)節(jié)的方向，保留與關(guān)節(jié)移動(dòng)量的最小值對(duì)應(yīng)的待選隨機(jī)關(guān)節(jié)變量組，可找到使串聯(lián)機(jī)器人移動(dòng)關(guān)節(jié)時(shí)消耗最少能量的最優(yōu)反解結(jié)果。

進(jìn)一步地，誤差值計(jì)算單元具體用于根據(jù)公式ΔE＝ΔP+ΔR以及計(jì)算所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)與所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)之間的誤差值ΔE。

其中，ΔP表示位置差異，ΔR表示姿態(tài)差異，ω_ρ和ω_γ為所述位置差異ΔP和所述姿態(tài)差異ΔR的權(quán)重，P_s為末端關(guān)節(jié)的目標(biāo)位置，p_sj為末端關(guān)節(jié)的目標(biāo)位置分量，d_sj為末端關(guān)節(jié)的目標(biāo)姿態(tài)，P_s、p_sj及d_sj可從所述末端關(guān)節(jié)的目標(biāo)坐標(biāo)獲得，P_r為末端關(guān)節(jié)的隨機(jī)位置，p_rj為末端關(guān)節(jié)的隨機(jī)位置分量，d_rj為末端關(guān)節(jié)的隨機(jī)姿態(tài)，P_r、p_rj及d_rj可從所述末端關(guān)節(jié)的隨機(jī)坐標(biāo)中獲得。

本發(fā)明實(shí)施例所提供的確定串聯(lián)機(jī)器人反解結(jié)果的裝置可用于執(zhí)行本發(fā)明任意實(shí)施例所提供的確定串聯(lián)機(jī)器人反解結(jié)果的方法，具備執(zhí)行該方法相應(yīng)的功能和有益效果。

實(shí)施例六

圖6是本發(fā)明實(shí)施例六提供的用于實(shí)施確定串聯(lián)機(jī)器人反解結(jié)果的方法的主從機(jī)器人的閉環(huán)控制系統(tǒng)示意圖，圖7是本發(fā)明實(shí)施例六提供的一種確定串聯(lián)機(jī)器人反解結(jié)果的方法中的各關(guān)節(jié)坐標(biāo)系示意圖。本實(shí)施例是上述實(shí)施例的技術(shù)方案的應(yīng)用，是一個(gè)優(yōu)選的具體實(shí)施方式。

一種確定串聯(lián)機(jī)器人反解結(jié)果的方法所在的系統(tǒng)包括：操作者601、主機(jī)器人602、PC機(jī)603、控制器604、從機(jī)器人605及光學(xué)跟蹤儀606，如圖6所示。其中，主機(jī)器人602與PC機(jī)603之間通過(guò)TCP/IP協(xié)議來(lái)通信，控制器604與PC機(jī)603之間通過(guò)以太網(wǎng)來(lái)通信。

圖7示出了從機(jī)器人的三個(gè)關(guān)節(jié)，包括第一關(guān)節(jié)701、第二關(guān)節(jié)702和末端關(guān)節(jié)703，并在第一關(guān)節(jié)上建立了基坐標(biāo)系，在第二關(guān)節(jié)702上建立了中間坐標(biāo)系，在末端關(guān)節(jié)703上建立了末端坐標(biāo)系。本領(lǐng)域技術(shù)人員可以理解的是，從機(jī)器人的關(guān)節(jié)的個(gè)數(shù)可以是其他的任意值。

操作者601操作主機(jī)器人602，把主機(jī)器人601的運(yùn)動(dòng)指令通過(guò)PC機(jī)603和控制器604傳遞給從機(jī)器人605，光學(xué)跟蹤儀606給操作者601反饋從機(jī)器人605的當(dāng)前運(yùn)動(dòng)狀態(tài)和位置狀態(tài)。

PC機(jī)603從主機(jī)器人602接收到的運(yùn)動(dòng)指令是從機(jī)器人605的末端關(guān)節(jié)703的目標(biāo)坐標(biāo)，然后PC機(jī)603應(yīng)用本發(fā)明上述實(shí)施例提供的確定串聯(lián)機(jī)器人反解結(jié)果的方法，來(lái)獲得從機(jī)器人605的第一關(guān)節(jié)701和第二關(guān)節(jié)702的關(guān)節(jié)變量，控制器604根據(jù)該關(guān)節(jié)變量控制從機(jī)器人605各關(guān)節(jié)的運(yùn)動(dòng)，以最優(yōu)的方式使末端關(guān)節(jié)處于目標(biāo)坐標(biāo)。

本實(shí)施例給出了一種應(yīng)用確定串聯(lián)機(jī)器人反解結(jié)果的方法的主從機(jī)器人系統(tǒng)和從機(jī)器人的關(guān)節(jié)結(jié)構(gòu)，使得PC機(jī)能夠根據(jù)主機(jī)器人給出的從機(jī)器人的末端關(guān)節(jié)的目標(biāo)坐標(biāo)，快速而準(zhǔn)確地求出從機(jī)器人各關(guān)節(jié)的關(guān)節(jié)變量，控制器根據(jù)該關(guān)節(jié)變量控制從機(jī)器人實(shí)時(shí)跟隨主機(jī)器人的運(yùn)動(dòng)。

注意，上述僅為本發(fā)明的較佳實(shí)施例及所運(yùn)用技術(shù)原理。本領(lǐng)域技術(shù)人員會(huì)理解，本發(fā)明不限于這里所述的特定實(shí)施例，對(duì)本領(lǐng)域技術(shù)人員來(lái)說(shuō)能夠進(jìn)行各種明顯的變化、重新調(diào)整和替代而不會(huì)脫離本發(fā)明的保護(hù)范圍。因此，雖然通過(guò)以上實(shí)施例對(duì)本發(fā)明進(jìn)行了較為詳細(xì)的說(shuō)明，但是本發(fā)明不僅僅限于以上實(shí)施例，在不脫離本發(fā)明構(gòu)思的情況下，還可以包括更多其他等效實(shí)施例，而本發(fā)明的范圍由所附的權(quán)利要求范圍決定。