一種基于構(gòu)形平面的冗余機械臂運動控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機器人技術(shù)領(lǐng)域，具體涉及一種能夠保證冗余機械臂在復(fù)雜的工作空 間中實現(xiàn)空間避障等多目標(biāo)條件下的運動控制的基于構(gòu)形平面的冗余機械臂運動控制方 法。
【背景技術(shù)】
[0002] 對于空間任務(wù)，考慮末端的位置和姿態(tài)，工作空間的維數(shù)是六。冗余機械臂冗余度 大于1。冗余機械臂的研究已經(jīng)成為機器人領(lǐng)域的研究熱點。
[0003] 許多專家在冗余機械臂的運動控制方面提出了很多方法。基于C空間的自由空間 法以機械臂的關(guān)節(jié)坐標(biāo)系建立C空間，將障礙物映射到C空間，形成空間構(gòu)型障礙，從而求 得C空間的補集，即自由空間。在此基礎(chǔ)上，利用啟發(fā)式搜索算法在機械臂的自由空間內(nèi)尋 找機械臂的運動路徑。該方法雖然能夠?qū)崿F(xiàn)機械臂無碰撞路徑規(guī)劃，但是由于將障礙物映 射到C空間方法較為復(fù)雜，對于復(fù)雜環(huán)境難以滿足實時性的要求。為了實現(xiàn)空間避障，對障 礙定義一個排斥勢場，目標(biāo)點處定義一個吸引勢場，機械臂的運動由兩個勢場共同作用力 來決定，由此來保證機械臂在避障的同時順利到達最終目標(biāo)點。該方法對于處理全局路徑 規(guī)劃中的動態(tài)避障非常有效，但容易陷入局部最小點處。
[0004] 在對機器人進行運動控制時，各個關(guān)節(jié)的角加速度約束、角速度約束和角度約束 是涉及的主要約束問題。在一般低速運動情況下，只要保證關(guān)節(jié)角度不超限即可，這對運動 軌跡規(guī)劃影響很小。但是，當(dāng)機器人運動速度較快時，關(guān)節(jié)角加速度和角速度極易超出約束 范圍，導(dǎo)致驅(qū)動電流過大或者超出限位的事故發(fā)生。輕則機器人運動出錯，重則損壞硬件。 此時，必須在機器人運動軌跡規(guī)劃時綜合考慮各種約束條件，最常用的方法是對運動時間 進行優(yōu)化。例如，有研究人員在用機械臂攔截快速飛行的物體時，對機械臂的運動軌跡施加 關(guān)節(jié)速度約束和力矩約束。還有學(xué)者推導(dǎo)出了具有特殊關(guān)節(jié)配置的7R機械臂的解析逆解， 并且使所有的關(guān)節(jié)滿足其關(guān)節(jié)角度限制。工作主要是服務(wù)于擬人機械臂工作空間的求解。 眾多學(xué)者對7R機械臂的仿人達點運動規(guī)劃展開了研究。也可將工作空間劃分成一個個小 格，得到每個小格內(nèi)擬人的關(guān)節(jié)角度，建立查找表，根據(jù)要求的末端位置，在查找表中找到 冗余角q3,根據(jù)解析逆解求出其他關(guān)節(jié)角。MINH利用運動元的方式實現(xiàn)擬人運動。但這些 研究工作的重點是在仿人達點運動，并沒有考慮避障。為了提高其搜索效率，

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

[0005] 本發(fā)明目的在于提供一種冗余機械臂逆運動控制方法。該方法不依賴機器人構(gòu) 形，形式簡單，減少計算量，能夠按需要得到優(yōu)化解，具有通用性和快速性。
[0006] 本發(fā)明的目的是這樣實現(xiàn)的：
[0007] -種運用構(gòu)形平面進行冗余機械臂運動控制的方法，包括如下步驟：
[0008] 步驟1 :給定冗余機械臂結(jié)構(gòu)參數(shù)，運用動力學(xué)對冗余機械臂進行構(gòu)形平面劃分；
[0009] 步驟2 :按照構(gòu)形平面匹配的方式進行冗余機械臂初規(guī)劃，構(gòu)形平面匹配以加權(quán) 的空間矢量法進行；
[0010] 步驟3 :對規(guī)劃的冗余機械臂的每個構(gòu)形平面進行空間障礙物干涉檢測，對產(chǎn)生 干涉的構(gòu)形平面進行重新調(diào)整和規(guī)劃；
[0011] 步驟4:對調(diào)整后的冗余機械臂規(guī)劃進行關(guān)節(jié)的速度、加速度的樣條曲線平滑處 理；
[0012] 步驟5 :對整個運動過程進行動力學(xué)校核。
[0013] 所述運用動力學(xué)的冗余機械臂的構(gòu)形平面劃分中構(gòu)形平面內(nèi)的第一個關(guān)節(jié)是回 轉(zhuǎn)關(guān)節(jié)，最后一個關(guān)節(jié)是擺動關(guān)節(jié)或者移動關(guān)節(jié)，搖擺關(guān)節(jié)和移動關(guān)節(jié)的數(shù)量是構(gòu)形平面 中的自由度數(shù)目。
[0014] 所述構(gòu)形平面所在二維平面將三維空間障礙物橫切成截圖圖形，在障礙物區(qū)域1 外擴距離k，安全避障區(qū)域由m線段組成，則第i段線段為：
[0016] 機器人構(gòu)形由η段線段組成，則第j段線段為：
Λ
[0018] 比較兩個線段的斜率是否相等，不等則求解兩條線段所在直線的交點，
[0019] 在該構(gòu)形平面中的干涉關(guān)節(jié)進行重新調(diào)整，使干涉關(guān)節(jié)與安全障礙區(qū)域靠近機器 人基座一側(cè)的邊界線段之間進行判斷。
[0020] 在已知的機械臂關(guān)節(jié)約束、機械臂末端軌跡、空間障礙物位置的條件下，所述構(gòu)形 平面通過組成冗余機械臂規(guī)劃機械臂的空間軌跡：
[0021] 空間關(guān)節(jié)約束為：
[0023] 所述構(gòu)形平面的空間位置確定采用加權(quán)空間矢量法，P1為機械臂末端軌跡規(guī)劃中 的第i點，連接Pi〇。，kj(j = 1，…η)為構(gòu)形平面i的起點到末端點的連線；
[0024] 以PA向量為導(dǎo)向，規(guī)劃構(gòu)形平面的k ,(j = 1，…η);在保證機械臂工作空間的條 件下，規(guī)劃k,(j = 1，···η-1);對構(gòu)形平面與障礙物發(fā)生重疊干涉的部分，運用介紹的方法 對該構(gòu)形平面中機械臂關(guān)節(jié)與障礙物進行干涉判斷檢查。
[0025] 所述構(gòu)形平面的自由度是η，關(guān)節(jié)a為組成第k個構(gòu)形平面的第一個關(guān)節(jié)，關(guān)節(jié)b 是回轉(zhuǎn)關(guān)節(jié)，把關(guān)節(jié)b劃分到構(gòu)形平面k+Ι中
[0026] acoa為關(guān)節(jié)a的角速度，構(gòu)形平面的自由度是n，已知構(gòu)形平面的起點的速度 acoa， "4, :β：+>,+η> -va+n, 〇

[0029] 在構(gòu)形平面k的質(zhì)心k。出建立坐標(biāo)，此坐標(biāo)平行于此構(gòu)形平面起點處的坐標(biāo)， 表示質(zhì)心k。在坐標(biāo)系a+n下的坐標(biāo)，則
[0035] 構(gòu)形平面質(zhì)心k。到各個關(guān)節(jié)的質(zhì)心的坐標(biāo)為
J 各個關(guān)節(jié)的質(zhì)心的坐標(biāo)到構(gòu)形平面質(zhì)心k。的坐標(biāo)為

[0041] 關(guān)節(jié)a為構(gòu)形平面的最后一個關(guān)節(jié)，則關(guān)節(jié)a+Ι為與之相鄰的下一個構(gòu)形平面的 第一個關(guān)節(jié)，令3?3、3起為上一個構(gòu)形平面的最后一個關(guān)節(jié)的角速度，角加速度，i = 1，關(guān) 節(jié)a所在的構(gòu)形平面為第一個構(gòu)形平面，則acoa、d成為基座標(biāo)的角速度，角加速度，基座標(biāo) 不動，則
[0049] 最后一個關(guān)節(jié)乂+1: I 末端操作手在空間是自由的，則MOTd、FOTd等于
零。；
[0053] 令％=#，即機器人基座受到的支撐作用相當(dāng)向上的重力加速度g。
[0054] 本發(fā)明與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果在于：
[0055] (1)結(jié)合冗余機械臂的結(jié)構(gòu)特點和工作方式，將構(gòu)形平面引入到冗余機械臂的運 動控制中，該方法能夠直觀快速實現(xiàn)冗余機械臂的運動控制規(guī)劃。
[0056] (2)以構(gòu)形平面為基礎(chǔ)，通過空間矢量引導(dǎo)的方式對構(gòu)形平面進行空間位置確定， 進而確定較為合理的冗余機械臂空間位形。該方法避免了采用傳統(tǒng)解析方法依賴機器人構(gòu) 形形式和自由度，也避免數(shù)值方法的求解精度和求解速度問題，具有實用性和通用性。
[0057] (3)基于構(gòu)形平面的動力學(xué)方法。該方法以構(gòu)形平面為單位，減少推算和求解步 驟，能夠快速校核機械臂在規(guī)劃過程中的關(guān)節(jié)運動性能，為優(yōu)化和調(diào)整機械臂規(guī)劃提供依 據(jù)。
【附圖說明】
[0058] 圖1構(gòu)形平面劃分示意圖附圖；
[0059] 圖2構(gòu)形平面內(nèi)的障礙物干涉檢測圖；
[0060] 圖3空間構(gòu)形平面位置確定圖；
[0061 ] 圖4運動控制流程圖。
【具體實施方式】
[0062] 下面結(jié)合附圖對本發(fā)明做更詳細的描述。
[0063] 本發(fā)明公開了一種運用構(gòu)形平面進行冗余機械臂運動控制的方法。受機械臂關(guān)節(jié) 約束和空間障礙的限制，冗余機械臂的運動控制是一個復(fù)雜過程。本發(fā)明從組成冗余機械 臂構(gòu)形平面入手，利用空間幾何的方法，對冗余機械臂進行空間運動控制，通過空間矢量引 導(dǎo)、避障路徑的比較、動力學(xué)校核，快速找到空間優(yōu)化的路徑，實現(xiàn)多目標(biāo)軌跡規(guī)劃方法。該 方法不依賴機器人構(gòu)形，形式簡單，降低了求解的難度，減少計算量，能夠按需要得到優(yōu)化 解，具有通用性和快速性。
[0064] 本發(fā)明采用的技術(shù)方案是：
[0065] 步驟1 :給定冗余機械臂結(jié)構(gòu)參數(shù)，運用動力學(xué)對冗余機械臂進行構(gòu)形平面劃分。
[0066] 步驟2 :按照構(gòu)形平面匹配的方式進行冗余機械臂初規(guī)劃，構(gòu)形平面匹配以加權(quán) 的空間矢量法進行。
[0067] 步驟3 :對規(guī)劃的冗余機械臂的每個構(gòu)形平面進行空間障礙物干涉檢測，對產(chǎn)生 干涉的構(gòu)形平面進行重新調(diào)整和規(guī)劃。
[0068] 步驟4 :對調(diào)整后的冗余機械臂規(guī)劃進行關(guān)節(jié)的速度、加速度的樣條曲線平滑處 理