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

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

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