一種機(jī)器人笛卡爾空間軌跡的規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及一種機(jī)器人笛卡爾空間軌跡的規(guī)劃方法，設(shè)及逆運(yùn)動(dòng)學(xué)和姿態(tài)規(guī)劃。
【背景技術(shù)】
[0002] 工業(yè)機(jī)器人應(yīng)用的深度與廣度已成為衡量一個(gè)國家制造業(yè)水平和科技水平的重 要標(biāo)志。自從第一臺(tái)工業(yè)機(jī)器人于1962年應(yīng)用于GeneralMotor公司生產(chǎn)線，機(jī)器人技術(shù) 獲得快速的發(fā)展，尤其，目前在人工成本不斷提高和"工業(yè)4. 0"概念提出的背景下，機(jī)器人 革命有望成為"第=次工業(yè)革命"的重要切入點(diǎn)和增長點(diǎn)，而目前我國已經(jīng)成為全球最大的 機(jī)器人市場，2015年5月國務(wù)院印發(fā)《中國制造2025》，明確提出要重點(diǎn)支持機(jī)器人技術(shù)的 發(fā)展和應(yīng)用，將其列為十大重點(diǎn)領(lǐng)域之一。
[0003] 運(yùn)動(dòng)學(xué)和軌跡規(guī)劃是機(jī)器人技術(shù)實(shí)際應(yīng)用的基礎(chǔ)，其中軌跡規(guī)劃包括位置和姿 態(tài)兩方面，尤其在笛卡爾空間的軌跡規(guī)劃。工業(yè)實(shí)際應(yīng)用中，當(dāng)前在笛卡爾空間下進(jìn)行軌跡 規(guī)劃多是在關(guān)節(jié)空間中進(jìn)行，末端實(shí)際運(yùn)動(dòng)軌跡復(fù)雜，同時(shí)，實(shí)際應(yīng)用中常要求末端W不同 的姿態(tài)進(jìn)行，而當(dāng)前的規(guī)劃方法存在易導(dǎo)致奇異性等弊端。其中，運(yùn)動(dòng)學(xué)問題是機(jī)器人運(yùn)動(dòng) 控制和軌跡規(guī)劃的基礎(chǔ)，解析形式的機(jī)器人逆運(yùn)動(dòng)學(xué)問題是機(jī)器人研究中的難題，也是機(jī) 器人研究領(lǐng)域的熱點(diǎn)問題。
[0004] 位置逆解問題是機(jī)器人運(yùn)動(dòng)規(guī)劃和軌跡規(guī)劃的基礎(chǔ)，只有通過逆運(yùn)動(dòng)學(xué)把空間位 姿轉(zhuǎn)換為關(guān)節(jié)變量，才能實(shí)現(xiàn)對(duì)機(jī)械臂末端執(zhí)行器的控制；而且機(jī)器人逆運(yùn)動(dòng)學(xué)求解的效 率直接影響運(yùn)動(dòng)軌跡控制效果和作業(yè)效率。由于機(jī)器人正運(yùn)動(dòng)學(xué)是在已知各個(gè)關(guān)節(jié)角度求 解末端的位置和姿態(tài)，相對(duì)簡單且解唯一；逆運(yùn)動(dòng)學(xué)是是正運(yùn)動(dòng)學(xué)問題的逆過程，是已知機(jī) 械臂末端的位置和姿態(tài)，計(jì)算各個(gè)關(guān)節(jié)轉(zhuǎn)過的角度值，串聯(lián)機(jī)器人逆向運(yùn)動(dòng)學(xué)的求解過程 相對(duì)復(fù)雜，一般會(huì)出現(xiàn)多解或者甚至無解的情況。目前逆運(yùn)動(dòng)學(xué)求解算法分為兩大類；封閉 解/解析法和數(shù)值解法，其中，數(shù)值解法中的迭代過程會(huì)降低求解速度，不利于現(xiàn)代工業(yè)機(jī) 械臂的實(shí)時(shí)控制；目前常用的數(shù)值法有如基于雅克比矩陣的牛頓-拉普松迭代法，根據(jù)建 立的正運(yùn)動(dòng)模型進(jìn)行迭代計(jì)算，得出機(jī)器人逆運(yùn)動(dòng)學(xué)結(jié)果，但是在計(jì)算時(shí)間和結(jié)果的精確 性上很難同時(shí)保證，W及擬牛頓共輛梯度法，其存在雅克比矩陣奇異性，且算法初始狀態(tài)的 取值直接將影響算法的收斂度和求解精度，在計(jì)算時(shí)間和精確上很難同時(shí)保證；神經(jīng)網(wǎng)絡(luò) 法是通過學(xué)習(xí)網(wǎng)絡(luò)權(quán)、闊值參數(shù)建立笛卡爾空間下連桿構(gòu)型同各個(gè)關(guān)節(jié)角度映射關(guān)系，而 網(wǎng)絡(luò)學(xué)習(xí)需要大量樣本數(shù)據(jù)，在實(shí)際中卻較難獲取，同時(shí)，網(wǎng)絡(luò)學(xué)習(xí)所需要時(shí)間較長，不適 宜實(shí)時(shí)控制；另外，遺傳算法是利用全局并行捜索特性，常規(guī)遺傳算法在解決優(yōu)化問題存在 早熟即收斂速度慢等缺陷，降低優(yōu)化性能，影響求解精度；神經(jīng)網(wǎng)絡(luò)、遺傳算法和專家系統(tǒng)、 模糊邏輯聯(lián)合起來進(jìn)行混合求解，在解決多自由度機(jī)器人逆運(yùn)動(dòng)學(xué)是需要復(fù)雜的算法程序 和高性能的配置，所W，數(shù)值法計(jì)算耗時(shí)長，求解精度不高，不適宜高精度和實(shí)時(shí)性作業(yè)任 務(wù)；遺傳算法和神經(jīng)網(wǎng)絡(luò)方法可對(duì)求解進(jìn)行優(yōu)化，避免局部收斂，得出逆運(yùn)動(dòng)學(xué)結(jié)果，但與 迭代方法一樣，在計(jì)算時(shí)間和計(jì)算精度上存在制約性。
[0005] 封閉解是實(shí)際應(yīng)用中需要的，可W直接計(jì)算各關(guān)節(jié)角度理論值，不需要迭 代捜索尋優(yōu)，具有計(jì)算速度快，精度高，包含幾何法和代數(shù)法，其中幾何法被視為僅 適用于結(jié)構(gòu)簡單的少自由度機(jī)械臂；代數(shù)法包括通過D-H建模結(jié)合逆矩陣方式和 利用旋量結(jié)合指數(shù)積通過轉(zhuǎn)化為化den-K址an子問題求解等。前者是最常用的方 法，即先將機(jī)器人各關(guān)節(jié)轉(zhuǎn)動(dòng)角度設(shè)為01，0 2, 0 3... 0。，然后代入正運(yùn)動(dòng)學(xué)方程 巧=巧倘）*巧㈱*巧褲)*…*巧''''1供.)，推導(dǎo)出
[0006]
[0007] 方程左邊的數(shù)據(jù)已知，采取逆矩陣的方式分離變量，尋找其中姿態(tài)和位置間存在 的等量關(guān)系，如；（巧(0!)) -，*巧=巧(&) *巧倘）*…巧-1供,.)，之后進(jìn)行逐級(jí)分離變量， 從而求得有關(guān)角度，但任意構(gòu)造的方程可能導(dǎo)致其中的求解過程異常復(fù)雜W及后續(xù)需要進(jìn) 行有效解的尋值和匹配，需要更復(fù)雜的算法，所W要得到其中的有效解析解表達(dá)式，求解過 程復(fù)雜，解算效率不高。欠自由度機(jī)械臂不能實(shí)現(xiàn)任意姿態(tài)和位置，當(dāng)任意給定時(shí)，往往無 解；國內(nèi)外學(xué)者曾使用螺旋理論推導(dǎo)5R解析解的框架，幾何法或代數(shù)法也曾分別被用來求 解5R機(jī)械臂逆運(yùn)動(dòng)學(xué)解析解，一般無法或很難在執(zhí)行具體任務(wù)前預(yù)先判定操作點(diǎn)是否在 工作空間內(nèi)，需要采用查表法等，而且，傳統(tǒng)的求解算法通常存在解算范圍小，需要匹配或 通過其他算法選擇最優(yōu)解、特殊位置處需單獨(dú)討論、最優(yōu)解不易直觀確定等問題，嚴(yán)重影響 機(jī)器人的響應(yīng)速度。因此，位置逆解問題作為機(jī)器人運(yùn)動(dòng)學(xué)中最基礎(chǔ)、最重要的研究問題之 一，直接關(guān)系到運(yùn)動(dòng)分析、軌跡規(guī)劃和實(shí)時(shí)控制等，甚至后續(xù)的速度和加速度分析。而且，逆 運(yùn)動(dòng)學(xué)的求解速度和準(zhǔn)確度直接影響工業(yè)機(jī)器人的實(shí)時(shí)控制的難W程度，也將直接決定機(jī) 械臂執(zhí)行復(fù)雜任務(wù)的能力。
[0008] 而軌跡規(guī)劃是逆運(yùn)動(dòng)學(xué)的實(shí)際應(yīng)用，工業(yè)應(yīng)用中運(yùn)動(dòng)軌跡規(guī)劃的好壞直接影響機(jī) 器人作業(yè)質(zhì)量。機(jī)器人末端運(yùn)動(dòng)是由關(guān)節(jié)變量直接決定的，由于關(guān)節(jié)坐標(biāo)空間和直角坐標(biāo) 空間轉(zhuǎn)換關(guān)系復(fù)雜，同時(shí)在關(guān)節(jié)空間進(jìn)行路徑規(guī)劃，計(jì)算比較簡單，且不會(huì)產(chǎn)生機(jī)構(gòu)奇異問 題，即可W避免雅克比矩陣奇異時(shí)形成的速度失控現(xiàn)象，所W除了示教再現(xiàn)法W外，目前軌 跡規(guī)劃多數(shù)采取關(guān)節(jié)空間的規(guī)劃方案，但其在直角坐標(biāo)空間的最終軌跡，路徑點(diǎn)之間的軌 跡形狀往往是十分復(fù)雜的，因此只有對(duì)初始點(diǎn)和最終期望點(diǎn)的位姿有要求時(shí)而對(duì)中間路徑 無要求的任務(wù)，才可W在關(guān)節(jié)空間直接進(jìn)行路徑規(guī)劃。當(dāng)末端軌跡形狀有一定要求，在關(guān) 節(jié)空間規(guī)劃則很難或無法達(dá)到要求，比如，當(dāng)需要進(jìn)行連續(xù)軌跡的作業(yè)任務(wù)，則必須在直角 坐標(biāo)空間中規(guī)劃出需要的軌跡。目前存在的直角坐標(biāo)空間規(guī)劃方法計(jì)算工作量大，W及姿 態(tài)規(guī)劃問題復(fù)雜，導(dǎo)致所規(guī)劃的軌跡有可能接近或通過奇異點(diǎn)，造成無法解算的后果。尤其 針對(duì)少自由度機(jī)械臂，當(dāng)姿態(tài)規(guī)劃不成功時(shí)，往往通過犧牲末端位姿精度來達(dá)到回避奇異 位置的目標(biāo)，然而當(dāng)進(jìn)行目標(biāo)捕獲、精細(xì)操作等時(shí)，末端位姿精度的犧牲影響任務(wù)的成功執(zhí) 行。
[0009] 因此，在路徑要求不高的抓取操作時(shí)在關(guān)節(jié)空間中進(jìn)行規(guī)劃尚可，但當(dāng)有嚴(yán)格軌 跡要求時(shí)，如焊接等，需要復(fù)雜的算法來保證精度，而在笛卡爾空間進(jìn)行軌跡規(guī)劃的精確性 是基于關(guān)節(jié)空間規(guī)劃的該種方法所無法比擬的。但在姿態(tài)規(guī)劃方面，由于其所具有的非線 性和禪合性，目前存在的算法多數(shù)采取插補(bǔ)方案進(jìn)行，靈活性差，尤其在五自由度等少自由 度機(jī)器人中，存在諸多重大缺陷，容易造成規(guī)劃路徑的奇異性，甚至無法求解的局面。

【發(fā)明內(nèi)容】

[0010] 針對(duì)現(xiàn)有技術(shù)中存在的技術(shù)問題，本發(fā)明的目的是；提供一種機(jī)器人空間軌跡的 規(guī)劃方法。
[0011] 本發(fā)明通過W下步驟實(shí)現(xiàn)：
[0012] 步驟1 ;建立連桿坐標(biāo)系；將機(jī)器人連桿中基坐標(biāo)系固連于基座，依次建立坐標(biāo) 系，并命名各個(gè)關(guān)節(jié)軸的轉(zhuǎn)動(dòng)角度分別為0 1、0 2、0 0 1;工業(yè)中各個(gè)廠家生產(chǎn)的機(jī)器人 的初始狀態(tài)及關(guān)節(jié)運(yùn)動(dòng)范圍有所不同，但其理論模型一致；
[001引步驟2 ;由運(yùn)動(dòng)學(xué)建模分析方法得正運(yùn)動(dòng)學(xué)方程；巧=巧*巧*巧...巧^1;
[0014]W下為各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)角度的求解過程：
[0015] 步驟3;主控關(guān)節(jié)C轉(zhuǎn)動(dòng)角度日。的求解
[0016]令底部靠近基座的豎直關(guān)節(jié)為主控關(guān)節(jié)（controllingjoint)，一般為靠近基座 的第一個(gè)關(guān)節(jié)位置，距離末端執(zhí)行器最近的具有水平關(guān)節(jié)軸關(guān)節(jié)th，其位置記為Pth，由末 端執(zhí)行器（endeffector)位置通過接近矢量（approachingvector)計(jì)算得到；當(dāng)關(guān)節(jié)C 處存在連桿偏距a。時(shí)，在底部具有水平軸關(guān)節(jié)化附近建立虛關(guān)節(jié)（virtualjoint),其位 置記為巧心。.，即
[0017]
如五關(guān)節(jié)型KUKAyouBot機(jī)械臂，當(dāng)關(guān)節(jié)C處不存在連桿偏距a。時(shí)， 即連桿偏距3。= 0,則巧b=巧也如五關(guān)節(jié)型Katana機(jī)械臂；令二者中間具有水平軸關(guān) 節(jié)mh位置為IV;由P th、P_*、巧ft.,,.,.空間S點(diǎn)構(gòu)成空間S角形，其中Pth、空間兩點(diǎn)得向量 巧/^1巧M利用向量幾何投影性質(zhì)求得關(guān)節(jié)C轉(zhuǎn)動(dòng)角度0當(dāng)存在沿或繞基坐標(biāo)系X、y、Z 方向移動(dòng)或轉(zhuǎn)動(dòng)時(shí)，投影位置受到來自沿軸向移動(dòng)的影響，需結(jié)合末端姿態(tài)W求解0。。
[001引因此，采用幾何投影法和atan2函數(shù)，可求解實(shí)際機(jī)械臂在[-31，31 ]的轉(zhuǎn)動(dòng)的范 圍。
[001引 新^。！=渝?姑(巧巧h(2)，巧、巧A.(l));非高空頂部作業(yè)下，也。a;=a.ton.2仍，巧?)； [0020] 此解由向量幾何投影方式直接求得關(guān)節(jié)C的解，稱之為0。的投影解；
[00引]免['此'",二聲。a- '站促。U) *開；
[0022] 此解為由投影解通過相位轉(zhuǎn)換（二者相對(duì)坐標(biāo)系原點(diǎn)中屯、對(duì)稱）取得關(guān)節(jié)C的 解，稱之為9。的相位解；
[0023] 針對(duì)進(jìn)行高空頂部大幅度作業(yè)時(shí)，末端多次出現(xiàn)在不同象限，主控關(guān)節(jié)轉(zhuǎn)動(dòng)角度 會(huì)在不同相位跳動(dòng)，可對(duì)其進(jìn)行優(yōu)化。
[0024] 步驟4 ;中間關(guān)節(jié)血轉(zhuǎn)動(dòng)角度0 mid的求解
[00巧]求得關(guān)節(jié)C轉(zhuǎn)動(dòng)角度0。后，若關(guān)節(jié)C處存在連桿偏距a。，則由投影解0CW所得底 部具有水平軸關(guān)節(jié)化的實(shí)際所處位置Pbh為
[002引則關(guān)節(jié)化與關(guān)節(jié)th位置間的向量關(guān)系站:化二巧^巧h，取模求得長度lPl>Mh= \Pbhbh\
[0027]由相位片得關(guān)節(jié)化的實(shí)際所處位置Pbh為
[002引則關(guān)節(jié)化與關(guān)節(jié)th的向量關(guān)系巧hif。=巧h -巧，取模求得長度 IPbMhv",二\P村比h.r,_,,入;
[0029] 設(shè)關(guān)節(jié)化與關(guān)節(jié)血的連桿長度記為Ibm，關(guān)節(jié)血與關(guān)節(jié)th的連桿長度記為Imt; 當(dāng)關(guān)節(jié)恤、mh和th間存在移動(dòng)關(guān)節(jié)時(shí)，Ibm和1 mt為含移動(dòng)關(guān)節(jié)在內(nèi)的各關(guān)節(jié)間的連桿長 度。由Pbh、Pmh、Pt適間S點(diǎn)構(gòu)成空間S角形，由S角形的邊角關(guān)系余弦定理，求解出中間關(guān) 節(jié)mh轉(zhuǎn)動(dòng)角度0mw即相應(yīng)于9。的投影解情形下
[0032] 同理，可求相應(yīng)于0。相位解的0 mid其他兩個(gè)解；
[003引步驟5 ;靠近基座具有水平軸關(guān)節(jié)化轉(zhuǎn)動(dòng)角度0 bh的求解
[0034] 針對(duì)機(jī)器人的非線性強(qiáng)禪合特點(diǎn)，相對(duì)于禪合問題的解禪處理方式，留元法采取 先引入多變量，充分利用禪合關(guān)系，尋求相應(yīng)關(guān)系的函數(shù)算式，避免可能帶來的增根剔除、 真解篩選、最優(yōu)解尋值匹配等。通過Matl油等計(jì)算機(jī)代數(shù)系統(tǒng)的符號(hào)計(jì)算處理，可尋求關(guān) 于0bh的函數(shù)算式，在處理過程，將0bh描述為0C和0mJ勺函數(shù)，即0bh=f2(ec，0mJ。 令k = k(0)，0及0j表示已求解的關(guān)節(jié)轉(zhuǎn)動(dòng)角度，有ki=ki(0)，k2=k2(9)，則
[00；35] ki= b*sin(日1+日J(rèn))+a*sin日i;k2= b*cos(日1+日J(rèn))+a*cos白i
[0036] 其他算法通過消元求得0 1，割裂了中間的禪合關(guān)系，后續(xù)獲取組解需要尋值匹 配；本發(fā)明提出留元法，即保留0J，不輕易消去0J，應(yīng)用輔角公式，為解決禪合問題提供思 路。方法一應(yīng)用輔角公式，即留元S角法：
[00；37] ki= (b*cos白j+a)*sin白i+b*sin白j*cos白askg二 〇3*cos日j+a)*cos日i-b*sin日j*sin白i
[0038]令M=b*cos日j+a、N=b*sin日j，則=姑倘。(興，M)。
[0039] 因?yàn)闄C(jī)器人各連桿長度不等，則M2+N2恒大于零，所W分母恒不等于0,即 「00401
[0042]
[004引因?yàn)?郝的取值范圍，令5打=|巧-腳，則關(guān)節(jié)1取值為日1 = 白i+ (1-sign (sn)) * 31；
[0044] W上步驟中采用S角輔角公式，其求解范圍為[-31/2, 31/2]，雖然目前絕大多數(shù) 工業(yè)機(jī)械臂節(jié)活動(dòng)范圍均在上述求解范圍內(nèi)，也存在某種機(jī)械臂會(huì)略超出該范圍，方法二 通過聯(lián)立方程法求解算法，求解范圍為