本發(fā)明涉及機(jī)器人
技術(shù)領(lǐng)域：
，尤其涉及一種冗余機(jī)械臂的控制方法及裝置。
背景技術(shù)：
：近年來，隨著人工智能技術(shù)和機(jī)械控制技術(shù)的提高以及機(jī)器人所執(zhí)行任務(wù)復(fù)雜程度的提高，冗余機(jī)械臂(關(guān)節(jié)數(shù)大于6個(gè)的機(jī)械臂)越來越多的被應(yīng)用到了機(jī)器人中來完成各種較為復(fù)雜任務(wù)。冗余機(jī)械臂執(zhí)行笛卡爾空間特定任務(wù)時(shí)在關(guān)節(jié)空間有無窮組解，在具體應(yīng)用時(shí)，一般可以根據(jù)關(guān)節(jié)速度、關(guān)節(jié)力矩或障礙物距離等優(yōu)化指標(biāo)選擇一組最優(yōu)解來確定冗余機(jī)械臂的移動(dòng)過程。在機(jī)器人執(zhí)行較復(fù)雜的任務(wù)時(shí)，通常需要同時(shí)對冗余機(jī)械臂的多個(gè)目標(biāo)進(jìn)行優(yōu)化，目前，常用來進(jìn)行多目標(biāo)優(yōu)化的方法可歸類為加權(quán)函數(shù)法和帕累托前沿法。但是，加權(quán)函數(shù)法雖然可以利用加權(quán)系數(shù)對優(yōu)化目標(biāo)進(jìn)行加權(quán)從而將多目標(biāo)優(yōu)化問題轉(zhuǎn)化為單目標(biāo)優(yōu)化問題，但是，此類方法難以判斷加權(quán)系數(shù)變化對優(yōu)化結(jié)果造成的影響，每次只能獲得一個(gè)解，需不斷改變加權(quán)系數(shù)的值，計(jì)算步驟繁瑣且存在一定的局限性；帕累托前沿類方法的計(jì)算復(fù)雜程度隨機(jī)械臂自由度的增加而急劇增加，當(dāng)冗余機(jī)械臂關(guān)節(jié)數(shù)較多時(shí)，該類方法也存在計(jì)算復(fù)雜、步驟繁瑣的問題。技術(shù)實(shí)現(xiàn)要素：有鑒于此，本發(fā)明實(shí)施例提供一種冗余機(jī)械臂的控制方法及裝置，以解決現(xiàn)有技術(shù)中多目標(biāo)求解方法計(jì)算復(fù)雜、步驟繁瑣的技術(shù)問題。第一方面，本發(fā)明實(shí)施例提供了一種冗余機(jī)械臂的控制方法，包括：獲取冗余機(jī)械臂所在的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息；根據(jù)所述當(dāng)前點(diǎn)信息和所述目標(biāo)點(diǎn)信息確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)；以冗余度空間向量為自變量建立所述軌跡函數(shù)對應(yīng)的方程；根據(jù)目標(biāo)接近法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。第二方面，本發(fā)明實(shí)施例還提供了一種冗余機(jī)械臂的控制裝置，包括：信息獲取單元，用于獲取冗余機(jī)械臂所在的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息；軌跡單元，與所述信息獲取單元相連，用于根據(jù)所述當(dāng)前點(diǎn)信息和所述目標(biāo)點(diǎn)信息確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)；冗余度處理單元，與所述軌跡單元相連，用于以冗余度空間向量為自變量建立所述軌跡函數(shù)對應(yīng)的方程；根據(jù)目標(biāo)接近法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本發(fā)明實(shí)施例提供的控制冗余機(jī)械臂的技術(shù)方案，獲取冗余機(jī)械臂的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息，根據(jù)所獲取的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息確定冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)，以冗余度空間向量為自變量建立該軌跡函數(shù)對應(yīng)的方程，通過目標(biāo)接近法求解該方程以得到運(yùn)動(dòng)軌跡對應(yīng)的冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本發(fā)明實(shí)施例通過采用上述技術(shù)方案，可以在得到滿足設(shè)計(jì)目標(biāo)的全局最優(yōu)解的前提下，減少冗余機(jī)械臂多目標(biāo)優(yōu)化問題的搜索空間，避免多目標(biāo)求解過程中維度爆炸問題的發(fā)生，簡化冗余機(jī)械臂控制過程中所需的計(jì)算量，提高冗余機(jī)械臂的反應(yīng)速度。附圖說明通過閱讀參照以下附圖所作的對非限制性實(shí)施例所作的詳細(xì)描述，本發(fā)明的其它特征、目的和優(yōu)點(diǎn)將會變得更明顯：圖1為本發(fā)明實(shí)施例一提供的一種冗余機(jī)械臂的控制方法的流程示意圖；圖2為本發(fā)明實(shí)施例二提供的一種冗余機(jī)械臂的控制方法的流程示意圖；圖3為本發(fā)明實(shí)施例二提供的一種九自由度冗余機(jī)械臂的坐標(biāo)系示意圖圖4為本發(fā)明實(shí)施例三提供的一種冗余機(jī)械臂的控制裝置的結(jié)構(gòu)框圖。具體實(shí)施方式下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步的詳細(xì)說明?？梢岳斫獾氖?，此處所描述的具體實(shí)施例僅僅用于解釋本發(fā)明，而非對本發(fā)明的限定。另外還需要說明的是，為了便于描述，附圖中僅示出了與本發(fā)明相關(guān)的部分而非全部內(nèi)容。實(shí)施例一本發(fā)明實(shí)施例一提供一種冗余機(jī)械臂的控制方法。該方法可由冗余機(jī)械臂的控制裝置執(zhí)行，其中，該裝置可通過硬件和/或軟件實(shí)現(xiàn)，一般可集成在用于控制機(jī)器人的控制模塊中。圖1是本發(fā)明實(shí)施例一提供的冗余機(jī)械臂的控制方法的流程示意圖，如圖1所示，該方法包括：S110、獲取冗余機(jī)械臂所在的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息。本實(shí)施例中，冗余機(jī)械臂的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息可以是冗余機(jī)械臂末端的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息，也可以是冗余機(jī)械臂各關(guān)節(jié)的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息，此處不作限制?？紤]到當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息的實(shí)用性，可選的，冗余機(jī)械臂所在的當(dāng)前點(diǎn)信息可以包括機(jī)械臂末端的當(dāng)前點(diǎn)信息和冗余機(jī)械臂各關(guān)節(jié)的當(dāng)前點(diǎn)信息，目標(biāo)點(diǎn)信息可以包括機(jī)械臂末端的目標(biāo)點(diǎn)信息。其中，當(dāng)前點(diǎn)信息(目標(biāo)點(diǎn)信息)可以包括機(jī)械臂末端或機(jī)械臂各關(guān)節(jié)在當(dāng)前(目標(biāo))位置的位置坐標(biāo)、角度坐標(biāo)、線速度、角速度、線加速度和/或角加速度等信息等。示例性的，在獲取冗余機(jī)械臂的當(dāng)前點(diǎn)信息時(shí)，可以直接通過傳感器獲取冗余機(jī)械臂的當(dāng)前點(diǎn)信息或者通過采用上一時(shí)刻的計(jì)算結(jié)果作為冗余機(jī)械臂的當(dāng)前點(diǎn)信息；在獲取冗余機(jī)械臂的目標(biāo)點(diǎn)信息時(shí)，可以根據(jù)冗余機(jī)械臂末端的當(dāng)前點(diǎn)信息和冗余機(jī)械臂末端的運(yùn)動(dòng)軌跡信息計(jì)算得到冗余機(jī)械臂末端需要移動(dòng)到的目標(biāo)點(diǎn)信息。以當(dāng)前點(diǎn)信息為角度坐標(biāo)、角速度和角加速度為例，冗余機(jī)械臂某一關(guān)節(jié)在當(dāng)前位置處的角度信息和角速度信息可以通過安裝在該關(guān)節(jié)位置的編碼器獲得，其角加速度信息可以通過差分運(yùn)算獲得；或者，也可以直接調(diào)用上一時(shí)刻計(jì)算得到的機(jī)械臂各關(guān)節(jié)的目標(biāo)點(diǎn)信息作為機(jī)械臂各關(guān)節(jié)在當(dāng)前時(shí)刻的當(dāng)前點(diǎn)信息。S120、根據(jù)所述當(dāng)前點(diǎn)信息和所述目標(biāo)點(diǎn)信息確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)。本實(shí)施例中，冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)可以包含在冗余機(jī)械臂末端從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)時(shí)冗余機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)，冗余機(jī)械臂各關(guān)節(jié)的軌跡函數(shù)可以是一個(gè)或多個(gè)，即，冗余機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)可以以一個(gè)總的軌跡函數(shù)進(jìn)行整體描述，也可以以每個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡對應(yīng)一個(gè)或幾個(gè)軌跡函數(shù)的形式進(jìn)行描述。在此，需要指出的是，冗余機(jī)械臂各關(guān)節(jié)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)可以包含等式和/或不等式，例如，冗余機(jī)械臂某一關(guān)節(jié)的軌跡函數(shù)可以包含該關(guān)節(jié)的位置坐標(biāo)、速度和/或加速度與其它變量(如機(jī)械臂末端的位置坐標(biāo)、速度和/或加速度)的對應(yīng)關(guān)系，還可以包含冗余機(jī)械臂正常移動(dòng)時(shí)該關(guān)節(jié)位置坐標(biāo)、移動(dòng)速度和/或移動(dòng)角速度的取值范圍等，此處不作限制。具體的，在確定冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)時(shí)，可以首先根據(jù)冗余機(jī)械臂末端的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息確定冗余機(jī)械臂末端的軌跡函數(shù)，然后根據(jù)冗余機(jī)械臂末端與各關(guān)節(jié)的連接關(guān)系確定各關(guān)節(jié)運(yùn)動(dòng)軌跡與冗余機(jī)械臂末端運(yùn)動(dòng)軌跡之間的關(guān)系，進(jìn)而根據(jù)冗余機(jī)械臂末端的軌跡函數(shù)確定冗余機(jī)械臂各關(guān)機(jī)的軌跡函數(shù)。S130、以冗余度空間向量為自變量建立所述軌跡函數(shù)對應(yīng)的方程。具體的，可以首先確定冗余度向量與各關(guān)節(jié)位置坐標(biāo)、速度或加速度等變量的對應(yīng)關(guān)系，然后根據(jù)該對應(yīng)關(guān)系采用冗余度空間向量替換各關(guān)節(jié)對應(yīng)的軌跡函數(shù)中的位置坐標(biāo)、速度或加速度等自變量，進(jìn)而建立冗余機(jī)械臂各關(guān)節(jié)以冗余度空間向量為自變量的軌跡函數(shù)對應(yīng)的方程。其中，冗余度空間向量可以是冗余度向量空間的任一向量，冗余度向量空間可以是已有的向量空間，也可以是自定義的空間，此處不作限制。S140、根據(jù)目標(biāo)接近法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本實(shí)施例中，在得到冗余機(jī)械臂各關(guān)節(jié)的位置和速度后，可以根據(jù)需要進(jìn)一步求得冗余機(jī)械臂各關(guān)節(jié)的加速度。示例性的，當(dāng)冗余機(jī)械臂某一關(guān)節(jié)的軌跡函數(shù)對應(yīng)的方程存在有限組解時(shí)，可以求得方程的所有解，然后根據(jù)實(shí)際需要或根據(jù)設(shè)定的選取規(guī)則選取方程的一組解得到該關(guān)節(jié)在下一時(shí)刻的目標(biāo)位置并同時(shí)確定該關(guān)節(jié)從當(dāng)前位置移動(dòng)到目標(biāo)位置時(shí)的移動(dòng)速度和加速度。當(dāng)冗余機(jī)械臂某一關(guān)節(jié)的軌跡函數(shù)對應(yīng)的方程存在無窮組解時(shí)，可以任意求得該方程的一組解并通過該組解得到該關(guān)節(jié)在下一時(shí)刻的目標(biāo)位置以及從當(dāng)前位置移動(dòng)到目標(biāo)位置的移動(dòng)速度和加速度；也可以首先設(shè)定方程解的約束條件，然后求符合該約束條件的方程的解，并以該組解指導(dǎo)該關(guān)節(jié)從當(dāng)前時(shí)刻到下一時(shí)刻的移動(dòng)過程，此處不作限制。為保證冗余機(jī)械臂各關(guān)節(jié)都可以高效地移動(dòng)，減少各關(guān)節(jié)的輸出力矩和運(yùn)動(dòng)、規(guī)避奇異與避免關(guān)節(jié)運(yùn)動(dòng)的極限，優(yōu)選的，可以根據(jù)符合約束條件的方程的解得到運(yùn)動(dòng)軌跡對應(yīng)的冗余機(jī)械臂中各關(guān)節(jié)的位置、速度和加速度。本發(fā)明實(shí)施例一提供的冗余機(jī)械臂的控制方法，獲取冗余機(jī)械臂的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息，根據(jù)所獲取的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息確定冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)，以冗余度空間向量為自變量建立該軌跡函數(shù)對應(yīng)的方程，通過目標(biāo)接近法求解該方程以得到運(yùn)動(dòng)軌跡對應(yīng)的冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本實(shí)施例通過采用上述技術(shù)方案，可以在得到滿足設(shè)計(jì)目標(biāo)的全局最優(yōu)解的前提下，減少冗余機(jī)械臂多目標(biāo)優(yōu)化問題的搜索空間，避免多目標(biāo)求解過程中維度爆炸問題的發(fā)生，簡化冗余機(jī)械臂控制過程中所需的計(jì)算量，提高冗余機(jī)械臂的反應(yīng)速度。實(shí)施例二圖2為本發(fā)明實(shí)施例二提供的一種冗余機(jī)械臂的控制方法的流程示意圖。本實(shí)施例在上述實(shí)施例的基礎(chǔ)上進(jìn)行優(yōu)化，進(jìn)一步地，所述獲取冗余機(jī)械臂所在的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息包括：獲取所述冗余機(jī)械臂末端在所述目標(biāo)點(diǎn)的速度；根據(jù)所述冗余機(jī)械臂末端在目標(biāo)當(dāng)前點(diǎn)的速度將所述冗余機(jī)械臂的關(guān)節(jié)角、關(guān)節(jié)角速度和關(guān)節(jié)角加速度映射到冗余度空間。進(jìn)一步地，所述根據(jù)所述當(dāng)前點(diǎn)信息和所述目標(biāo)點(diǎn)信息確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)包括：根據(jù)逆運(yùn)動(dòng)學(xué)方程，確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的優(yōu)化目標(biāo)函數(shù)和約束條件。進(jìn)一步地，所述根據(jù)目標(biāo)接近法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度包括：在冗余度空間中，根據(jù)優(yōu)化目標(biāo)函數(shù)、與優(yōu)化目標(biāo)對應(yīng)的最小目標(biāo)值、對應(yīng)的約束條件和預(yù)設(shè)的加權(quán)系數(shù)，通過輔助向量和單目標(biāo)優(yōu)化算法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。相應(yīng)的，如圖2所示，本實(shí)施例提供的冗余機(jī)械臂的控制方法包括：S210、獲取所述冗余機(jī)械臂末端在所述目標(biāo)點(diǎn)的速度。優(yōu)選的，所述獲取所述冗余機(jī)械臂末端在所述當(dāng)前點(diǎn)的速度包括：根據(jù)所述冗余機(jī)械臂末端在所述當(dāng)前點(diǎn)的位姿和所述冗余機(jī)械臂末端在所述運(yùn)動(dòng)軌跡中的下一采樣點(diǎn)的位姿確定所述冗余機(jī)械臂末端在所述下一采樣點(diǎn)的速度。其中，冗余機(jī)械臂末端的位姿指的是冗余機(jī)械臂的位置和姿態(tài)，位置可以用位置坐標(biāo)來表示，姿態(tài)可以用角度來表示。示例性的，可以根據(jù)安裝在冗余機(jī)械臂末端的編碼器獲取冗余機(jī)械臂在當(dāng)前點(diǎn)的角速度和/或角加速度等信息，或者直接根據(jù)冗余機(jī)械臂在上一時(shí)刻的計(jì)算結(jié)果得到冗余機(jī)械臂末端在當(dāng)前點(diǎn)的角速度和/或角加速度等信息，然后根據(jù)冗余機(jī)械臂在當(dāng)前點(diǎn)的角度和下一采樣的角度得到冗余機(jī)械臂在下一采樣點(diǎn)的角速度，進(jìn)而得到冗余機(jī)械臂在下一采樣點(diǎn)的速度。S220、根據(jù)所述冗余機(jī)械臂末端在所述目標(biāo)點(diǎn)的速度將所述冗余機(jī)械臂的關(guān)節(jié)角、關(guān)節(jié)角速度和關(guān)節(jié)角加速度映射到冗余度空間。示例性的，冗余機(jī)械臂末端速度與冗余機(jī)械臂的關(guān)節(jié)角速度的關(guān)系(即冗余機(jī)械臂各關(guān)節(jié)以角速度為自變量的軌跡函數(shù))可以表示為：其中，為機(jī)械臂末端的速度，vi(i＝x,y,z)表示線速度，ωi(i＝θ,Ψ,Φ)表示角速度；為機(jī)械臂的關(guān)節(jié)角速度，N為冗余機(jī)械臂的自由度，N＝6+r，r＞1；J∈R6×N為雅克比矩陣。假設(shè)α是雅可比矩陣J中的任意r(r＝n-6＞1)列，使得剩下的6列構(gòu)成非奇異矩陣J*，則式(1)可以改寫為：從而，任何滿足式(2)的關(guān)節(jié)角速度可以進(jìn)一步改寫為：其中，是滿足式(3)的一個(gè)特解，是式(3)的齊次解，且令其中，i＝1,2,…,r，αj是矩陣α的第j列；為列矩陣，且其前r(r＝N-6)行為0；為列矩陣，且其第i行為1，前r行中除第i行外的其他行為0。定義冗余度空間向量k＝(k1,…,kr)T為冗余度向量空間的任一向量，則有：假設(shè)角速度以S(k)進(jìn)行表示，角度θ以P(k)進(jìn)行表示，角加速度以A(k)進(jìn)行表示，則冗余機(jī)械臂關(guān)節(jié)角速度可表示為冗余度空間向量k的函數(shù)：冗余機(jī)械臂關(guān)節(jié)角度θ關(guān)于冗余度空間向量k的表達(dá)式為：冗余機(jī)械臂關(guān)節(jié)角加速度關(guān)于冗余度空間向量k的表達(dá)式為：其中，θ0為上一時(shí)刻的關(guān)節(jié)角度，為上一時(shí)刻的關(guān)節(jié)角速度，Δt為采樣步長。S230、根據(jù)逆運(yùn)動(dòng)學(xué)方程，確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的優(yōu)化目標(biāo)函數(shù)和約束條件。本實(shí)施例中，冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的優(yōu)化目標(biāo)函數(shù)和約束條件可以根據(jù)需要靈活進(jìn)行設(shè)定，例如，可以對冗余機(jī)械臂移動(dòng)的范圍進(jìn)行限制以躲避障礙物，或者，對冗余機(jī)械臂的彎曲角度進(jìn)行限制以避免各關(guān)節(jié)達(dá)到其運(yùn)動(dòng)極限等。考慮到各約束條件的實(shí)用性，可選的，所述優(yōu)化目標(biāo)函數(shù)包括所述冗余機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)幅度最小或者所述冗余機(jī)械臂受對應(yīng)的齒輪間隙影響最??；所述約束條件包括：將所述冗余機(jī)械臂的關(guān)節(jié)速度約束在預(yù)設(shè)的速度范圍內(nèi)或者將所述冗余機(jī)械臂的關(guān)節(jié)角度約束在預(yù)設(shè)的角度范圍內(nèi)。示例性的，冗余機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)幅度最小的優(yōu)化目標(biāo)函數(shù)可以為：冗余機(jī)械臂受對應(yīng)的齒輪間隙影響最小的優(yōu)化目標(biāo)函數(shù)可以為：將冗余機(jī)械臂的關(guān)節(jié)速度約束在預(yù)設(shè)的速度范圍內(nèi)的約束條件可以為：將冗余機(jī)械臂的關(guān)節(jié)角度約束在預(yù)設(shè)的角度范圍內(nèi)的約束條件可以為：θmin≤θ≤θmax。S240、以冗余度空間向量為自變量建立所述軌跡函數(shù)對應(yīng)的方程。本實(shí)施例中，可以基于冗余度空間建立冗余機(jī)械臂軌跡的優(yōu)化目標(biāo)函數(shù)和約束方程，由于冗余機(jī)械臂的優(yōu)化目標(biāo)函數(shù)和約束方程都可以表示為以關(guān)節(jié)角度、關(guān)節(jié)角速度和/或關(guān)節(jié)角加速度為自變量的表達(dá)式，因此，將上式(7)、(8)、(9)代入表達(dá)式中即可得到以冗余度空間向量k為自變量的優(yōu)化目標(biāo)函數(shù)和約束方程。例如，關(guān)于關(guān)節(jié)運(yùn)動(dòng)幅度最小的優(yōu)化目標(biāo)函數(shù)可以進(jìn)一步表示為冗余度空間向量k的方程：關(guān)于機(jī)械臂受對應(yīng)的齒輪間隙影響最小的優(yōu)化目標(biāo)函數(shù)可以進(jìn)一步表示為冗余度空間向量k的方程：關(guān)于關(guān)節(jié)角速度限制的約束條件可以進(jìn)一步表示為以冗余度空間向量k為自變量的方程：關(guān)于關(guān)節(jié)角度限制的約束條件可以進(jìn)一步表示為以冗余度空間向量k為自變量的方程：S250、在冗余度空間中，根據(jù)優(yōu)化目標(biāo)函數(shù)、與優(yōu)化目標(biāo)對應(yīng)的最小目標(biāo)值、對應(yīng)的約束條件和預(yù)設(shè)的加權(quán)系數(shù)，通過輔助向量和單目標(biāo)優(yōu)化算法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。示例性的，求解軌跡函數(shù)對應(yīng)的方程的單目標(biāo)優(yōu)化算法可以根據(jù)需要靈活確定，此處不作限制。優(yōu)選的，所述單目標(biāo)優(yōu)化算法包括：牛頓-歐拉算法、Nelder-Mead單純形算法、內(nèi)點(diǎn)算法、基因算法和/或PatternSearch算法。帕累托前沿是多目標(biāo)優(yōu)化問題的解集，其定義如下：在多目標(biāo)可行域Ω上，存在一點(diǎn)x*∈Ω，若不存在點(diǎn)x∈Ω，使得f(x)≤f(x*)成立，且至少存在一點(diǎn)x′∈Ω使嚴(yán)格不等式f(x′)＞f(x*)成立，則x*為一個(gè)帕累托最優(yōu)解。以雙目標(biāo)優(yōu)化問題為例，其軌跡函數(shù)對應(yīng)的方程如下：上式中，F(xiàn)(x)＝{f1(x)f2(x)}為優(yōu)化目標(biāo)函數(shù)，為與優(yōu)化目標(biāo)函數(shù)對應(yīng)的最小目標(biāo)值，A為矩陣約束條件，b為向量約束條件，ω為加權(quán)系數(shù)向量，γ為輔助向量。其中，加權(quán)系數(shù)向量ω可由用戶或開發(fā)商根據(jù)需要進(jìn)行設(shè)置，輔助向量γ用于將原來的多目標(biāo)優(yōu)化為題轉(zhuǎn)換為單目標(biāo)優(yōu)化問題。本實(shí)施例中，可試用內(nèi)點(diǎn)法求解式(12)得到方程的最優(yōu)解，進(jìn)而通過式(7)、式(8)和式(9)得到冗余機(jī)械臂各關(guān)節(jié)與該最優(yōu)解對應(yīng)的角度、角速度和角角速度，從而得到冗余機(jī)械臂各關(guān)節(jié)在下一時(shí)刻的位置和速度。以九自由度的冗余機(jī)械臂為例(假設(shè)該九自由度冗余機(jī)械臂的D-H坐標(biāo)如圖3所示，該九自由度冗余機(jī)械臂連桿D-H參數(shù)表如表1所示)，求解冗余機(jī)械臂各關(guān)節(jié)位置和速度的過程可以為：表1θidi(mm)αiai(mm)范圍0d190°01550≤d1≤8276θ200a2＝2200-90≤θ2≤90θ30-90°0-90≤θ3≤90θ4d4＝280090°a4＝-375-180≤θ4≤180θ50-90°a5＝-a4＝375-180≤θ5≤180θ6d4＝280090°0-180≤θ6≤180θ70-90°0-90≤θ7≤90θ8d8＝165090°0-90≤θ8≤90θ900ah＝350-90≤θ9≤90首先，把冗余機(jī)械臂末端的速度轉(zhuǎn)換為以連桿3的坐標(biāo)系為參考：然后，定義腕部坐標(biāo)系。考慮到計(jì)算的簡便性，可以根據(jù)冗余機(jī)械臂的結(jié)構(gòu)特點(diǎn)定義腕部坐標(biāo)系，其中，腕部坐標(biāo)系的具體坐標(biāo)與方向可以根據(jù)需要靈活設(shè)置，此處不作限定。示例性的，可以定義腕部坐標(biāo)系為與冗余機(jī)械臂末端坐標(biāo)系平行、原點(diǎn)與連桿8坐標(biāo)系原點(diǎn)重合的坐標(biāo)系，此時(shí)，當(dāng)腕部和冗余機(jī)械臂末端的速度以連桿3坐標(biāo)系為參考時(shí)，二者的角速度和線速度存在如下關(guān)系：3ωw＝3ωh(15)3vw＝3vh-3Rw(wωh×wPh)＝3vh-3ωh×3Ph＝3vh-3ωh×ah3xh(16)其中，3ωw是以連桿3坐標(biāo)系為參考的腕部坐標(biāo)系中的角速度；3ωh是以連桿3坐標(biāo)系為參考的冗余機(jī)械臂末端坐標(biāo)系中的角速度；3vw是以連桿3坐標(biāo)系為參考的腕部坐標(biāo)系中的線速度；3vh是以連桿3坐標(biāo)系為參考的冗余機(jī)械臂末端坐標(biāo)系中的線速度；3xh是以連桿3坐標(biāo)系為參考的冗余機(jī)械臂末端坐標(biāo)系中Z方向的單位矢量的值：上式中，ci表示cos(θi)，si表示sin(θi)，cij表示cos(θi+θj)，sij表示sin(θi+θj)。定義3Jw為聯(lián)系關(guān)節(jié)角速度與腕部速度的雅克比矩陣，則有：用矢量積法可求出該雅克比矩陣如下：3Jw＝[J1J2J3J4J5J6J7J8J9](19)其中，本實(shí)施例中，可在式(19)中取任意三列作為α，J*為除組成α的三列之外的其余列。示例性的，取J5、J6和J7作為α，其余列(J1、J2、J3、J4、J8和J9)組成J*，通過式(5)可以得到：其中，J21＝a2s3-d6c5-d4-a5s5-d7c5c7+d7s5c6s7，J23＝a2c3+a4c4+a5c4c5-d6c4s5-d7c4s5c7+d7s4s6s7-d7c4c5c6s7，J31＝-d4-d7(c5c7-s5c6s7)-d6c5-a5s5，J33＝a4c4+d7(s7(s4s6-c4c5c6)-c4s5c7)+a5c4c5-d6c4s3，J41＝d7(s7(c4s6+s4c5c6)+s4s5c7)-a4s4-a5s4c5+d6s4s5，J42＝d7(s7(s4s6-c4c5c6)-c4s5c7)+a4c4+a5c4c5-d6c4s5，J84＝s7(s4s6-c4c5c6)-c4s5c7，J85＝-s7(c4s6+s4c5c6)-s4s5c7，J86＝c5c7-s5c6s7，J94＝c8(s4c6+c4c5s6)-s8(c7(s4s6-c4c5c6))+c4s5s7，J95＝s8(c7(s4s6+s4c5c6))-s4s5s7-c8(c4c6-s4c5s6)，J96＝s8(c5s7-s5c6c7)+c8s5s6。令和由式(20)可得：其中，為矩陣的第i個(gè)元素。由式(6)可得：其中，α＝(J5J6J7)。因此，將式(21)至(26)中的(j＝1,2,3,4,8和9)用(i＝5,6和7，j＝1,2,3,4,8和9)代替，將式(21)至(26)中的(k＝1,2,3,4,5和6)用-Jik(i＝5,6和7，k＝1,2,3,4,5和6)代替即可得到和的值。以為例，將式(21)至(26)中的(j＝1,2,3,4,8和9)用(j＝1,2,3,4,8和9)代替，可以得到：將式(21)至(26)中的(k＝1,2,3,4,5和6)用-J5k(k＝1,2,3,4,5和6)代替，可得到：由式(19)可知因此可得：其中，為矩陣的第i個(gè)元素。同理可以得出：其中，將關(guān)節(jié)角速度表示為冗余度向量(k5,k6,k7)的函數(shù)：將式(28)代入式(8)中，可以得到關(guān)節(jié)角度θ以冗余度向量(k5,k6,k7)為自變量的函數(shù)：其中，θ0是當(dāng)前關(guān)節(jié)角度，Δt是采樣步長。將式(28)代入式(10-1)、將式(29)代入式(10-2)可以得到關(guān)節(jié)運(yùn)動(dòng)幅度最小以及機(jī)械臂受對應(yīng)的齒輪間隙影響最小的方程分別為：其中，Hi(θ(k5,k6,k7))是連桿變換矩陣0Ti(θ(k5,k6,k7))中第二行第四列對應(yīng)的元素：0Ti(θ(k5,k6,k7))＝0T1(d1)·1T2(θ2)Li-1Ti(θi),(i＝1,L,9)(32)其中，i-1Ti(θi)是Denavit-Hartenberg矩陣：其中，di、αi和ai是Denavit-Hartenberg參數(shù)，其具體數(shù)值如表1所示。將式(28)代入式(11-1)、將式(29)代入式(11-2)可以得到關(guān)節(jié)角速度限制以及關(guān)節(jié)角度限制的約束條件為：由式(33)可得：由式(34)可以進(jìn)一步得到式(12)中的矩陣A和向量b如下：從而，多目標(biāo)優(yōu)化問題可以轉(zhuǎn)化為：由式(30)、(31)和(35)可以求得冗余度向量(k5,k6,k7)的最優(yōu)值，將冗余度向量(k5,k6,k7)的最優(yōu)值代入式(28)和式(29)即可得到冗余機(jī)械臂各關(guān)節(jié)在下一采樣點(diǎn)的角速度和角度，從而得到冗余機(jī)械臂各關(guān)節(jié)的位置和速度。本發(fā)明實(shí)施例二提供的冗余機(jī)械臂的控制方法，獲取冗余機(jī)械臂末端在目標(biāo)點(diǎn)的速度，根據(jù)冗余機(jī)械臂末端在目標(biāo)點(diǎn)的速度將冗余機(jī)械臂的關(guān)節(jié)角、關(guān)節(jié)角速度和關(guān)節(jié)角加速度映射到冗余度空間，根據(jù)逆運(yùn)動(dòng)學(xué)方程確定冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的優(yōu)化目標(biāo)函數(shù)和約束條件，以冗余度空間向量為自變量建立運(yùn)動(dòng)軌跡對應(yīng)的方程，通過輔助向量和單目標(biāo)優(yōu)化算法求解軌跡函數(shù)對應(yīng)的方程，得到運(yùn)動(dòng)軌跡對應(yīng)的冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本實(shí)施例通過采用上述技術(shù)方案，建立多目標(biāo)約束優(yōu)化問題的數(shù)學(xué)模塊，可以為求解多目標(biāo)優(yōu)化問題提供理論基礎(chǔ)，在得到滿足設(shè)計(jì)目標(biāo)的全局最優(yōu)解的前提下，減少冗余機(jī)械臂多目標(biāo)優(yōu)化問題的搜索空間，避免多目標(biāo)求解過程中維度爆炸問題的發(fā)生，簡化冗余機(jī)械臂控制過程中所需的計(jì)算量，提高冗余機(jī)械臂的反應(yīng)速度。實(shí)施例三本發(fā)明實(shí)施例三提供一種冗余機(jī)械臂的控制裝置。該裝置可由硬件和/或軟件實(shí)現(xiàn)，一般集成在用于控制機(jī)器人的控制模塊中，可通過執(zhí)行冗余機(jī)械臂的控制方法實(shí)現(xiàn)對冗余機(jī)械臂的控制。圖4所示為本實(shí)施例提供的冗余機(jī)械臂的控制裝置的結(jié)構(gòu)框圖，如圖4所示，該裝置包括：信息獲取單元410，用于獲取冗余機(jī)械臂所在的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息；軌跡單元420，與所述信息獲取單元相連，用于根據(jù)所述當(dāng)前點(diǎn)信息和所述目標(biāo)點(diǎn)信息確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)；冗余度處理單元430，與所述軌跡單元相連，用于以冗余度空間向量為自變量建立所述軌跡函數(shù)對應(yīng)的方程；根據(jù)目標(biāo)接近法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本發(fā)明實(shí)施例三提供的冗余機(jī)械臂的控制裝置，通過信息獲取單元獲取冗余機(jī)械臂的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息，通過軌跡單元根據(jù)所獲取的當(dāng)前點(diǎn)信息和目標(biāo)點(diǎn)信息確定冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的軌跡函數(shù)，通過冗余度處理單元以冗余度空間向量為自變量建立該軌跡函數(shù)對應(yīng)的方程，通過目標(biāo)接近法求解該方程以得到運(yùn)動(dòng)軌跡對應(yīng)的冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。本發(fā)明實(shí)施例通過采用上述技術(shù)方案，可以在得到滿足設(shè)計(jì)目標(biāo)的全局最優(yōu)解的前提下，減少冗余機(jī)械臂多目標(biāo)優(yōu)化問題的搜索空間，避免多目標(biāo)求解過程中維度爆炸問題的發(fā)生，簡化冗余機(jī)械臂控制過程中所需的計(jì)算量，提高冗余機(jī)械臂的反應(yīng)速度。進(jìn)一步地，所述信息獲取單元410具體用于：獲取所述冗余機(jī)械臂末端在所述目標(biāo)點(diǎn)的速度；根據(jù)所述冗余機(jī)械臂末端在所述目標(biāo)點(diǎn)的速度將所述冗余機(jī)械臂的關(guān)節(jié)角、關(guān)節(jié)角速度和關(guān)節(jié)角加速度映射到冗余度空間。進(jìn)一步地，所述獲取所述冗余機(jī)械臂末端在所述當(dāng)前點(diǎn)的速度包括：根據(jù)所述冗余機(jī)械臂末端在所述當(dāng)前點(diǎn)的位姿和所述冗余機(jī)械臂末端在所述運(yùn)動(dòng)軌跡中的下一采樣點(diǎn)的位姿確定所述冗余機(jī)械臂末端在所述下一采樣點(diǎn)的速度。進(jìn)一步地，所述軌跡單元420具體用于根據(jù)逆運(yùn)動(dòng)學(xué)方程，確定所述冗余機(jī)械臂從當(dāng)前點(diǎn)移動(dòng)到目標(biāo)點(diǎn)的運(yùn)動(dòng)軌跡對應(yīng)的優(yōu)化目標(biāo)函數(shù)和約束條件。進(jìn)一步地，所述優(yōu)化目標(biāo)函數(shù)包括所述冗余機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)幅度最小或者所述冗余機(jī)械臂受對應(yīng)的齒輪間隙影響最?。凰黾s束條件包括：將所述冗余機(jī)械臂的關(guān)節(jié)速度約束在預(yù)設(shè)的速度范圍內(nèi)或者將所述冗余機(jī)械臂的關(guān)節(jié)角度約束在預(yù)設(shè)的角度范圍內(nèi)。進(jìn)一步地，所述根據(jù)目標(biāo)接近法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度包括：在冗余度空間中，根據(jù)優(yōu)化目標(biāo)函數(shù)、與優(yōu)化目標(biāo)對應(yīng)的最小目標(biāo)值、對應(yīng)的約束條件和預(yù)設(shè)的加權(quán)系數(shù)，通過輔助向量和單目標(biāo)優(yōu)化算法求解所述軌跡函數(shù)對應(yīng)的方程，得到所述運(yùn)動(dòng)軌跡對應(yīng)的所述冗余機(jī)械臂中各關(guān)節(jié)的位置和速度。進(jìn)一步地，所述單目標(biāo)優(yōu)化算法包括：牛頓-歐拉算法、Nelder-Mead單純形算法、內(nèi)點(diǎn)算法、基因算法和/或PatternSearch算法。本實(shí)施例提供的冗余機(jī)械臂的控制裝置可執(zhí)行本發(fā)明任意實(shí)施例提供的冗余機(jī)械臂的控制方法，具備執(zhí)行冗余機(jī)械臂的控制方法相應(yīng)的功能模塊和有益效果。未在本實(shí)施例中詳盡描述的技術(shù)細(xì)節(jié)，可參見本發(fā)明任意實(shí)施例所提供的冗余機(jī)械臂的控制方法。注意，上述僅為本發(fā)明的較佳實(shí)施例及所運(yùn)用技術(shù)原理。本領(lǐng)域技術(shù)人員會理解，本發(fā)明不限于這里所述的特定實(shí)施例，對本領(lǐng)域技術(shù)人員來說能夠進(jìn)行各種明顯的變化、重新調(diào)整和替代而不會脫離本發(fā)明的保護(hù)范圍。因此，雖然通過以上實(shí)施例對本發(fā)明進(jìn)行了較為詳細(xì)的說明，但是本發(fā)明不僅僅限于以上實(shí)施例，在不脫離本發(fā)明構(gòu)思的情況下，還可以包括更多其他等效實(shí)施例，而本發(fā)明的范圍由所附的權(quán)利要求范圍決定。當(dāng)前第1頁1 2 3