基于區(qū)域限定的奇異位置避免規(guī)劃算法①

呂永軍, 劉 峰, 鄭飂默, 吳文江, 朱 良, 葛小川

1(中國(guó)科學(xué)院大學(xué), 北京 100049)2(中國(guó)科學(xué)院大學(xué) 沈陽(yáng)計(jì)算技術(shù)研究所, 沈陽(yáng) 110171)3(沈陽(yáng)高精數(shù)控智能技術(shù)股份有限公司, 沈陽(yáng) 110171)

基于區(qū)域限定的奇異位置避免規(guī)劃算法①

呂永軍1,2, 劉 峰2,3, 鄭飂默2,3, 吳文江2,3, 朱 良1,2, 葛小川1,2

1(中國(guó)科學(xué)院大學(xué), 北京 100049)2(中國(guó)科學(xué)院大學(xué) 沈陽(yáng)計(jì)算技術(shù)研究所, 沈陽(yáng) 110171)3(沈陽(yáng)高精數(shù)控智能技術(shù)股份有限公司, 沈陽(yáng) 110171)

目前人們關(guān)于奇異位形的處理研究主要包括兩大類, 一類是研究如何避免, 一類是研究如何通過奇異點(diǎn).在分析了國(guó)內(nèi)外關(guān)于奇異位形的研究基礎(chǔ)上, 本文提出了基于空間區(qū)域限定的奇異位置避免算法. 其核心思想就是預(yù)先對(duì)機(jī)器人運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃, 給出了奇異位置及鄰近奇異位置的區(qū)域劃分方法, 從而保證在笛卡爾空間規(guī)劃時(shí)能規(guī)劃出一合理的插值點(diǎn), 同時(shí)也基于高次多項(xiàng)式曲線理論對(duì)關(guān)節(jié)空間規(guī)劃做了優(yōu)化, 保證了關(guān)節(jié)和末端運(yùn)動(dòng)的連續(xù)性、穩(wěn)定性等. 通過6軸機(jī)械臂在四五六關(guān)節(jié)時(shí)的奇異為實(shí)例, 運(yùn)用該算法對(duì)奇異的處理做了實(shí)例分析. 然后通過給定初始點(diǎn)和目標(biāo)點(diǎn)的位姿等參數(shù), 定時(shí)采樣數(shù)據(jù), 并利用MATLAB對(duì)規(guī)劃前后獲得的數(shù)據(jù)進(jìn)行了圖形仿真, 驗(yàn)證了該算法的正確性和可行性.

機(jī)器人; 軌跡規(guī)劃; 奇異位形; 極限位置

我們知道對(duì)于工業(yè)機(jī)器人操作臂來說, 其無法避免的一個(gè)問題就是奇異位形, 由于自身結(jié)構(gòu)特點(diǎn), 無論采用何種運(yùn)動(dòng)學(xué)建模方法DH法或其他方法, 都無法避免, 至少在目前無法避免. 許多國(guó)內(nèi)外的學(xué)者也一直致力于該問題研究與實(shí)踐, 并發(fā)表了大量的理論學(xué)術(shù)成果[1-9], 在實(shí)際的工業(yè)應(yīng)用中發(fā)揮了巨大作用.目前, 不管是對(duì)串聯(lián)機(jī)器人還是并聯(lián)機(jī)器人奇異位形處理問題的研究, 雖然已經(jīng)提出了許多的奇異點(diǎn)處理方法, 但是綜合這些方法的本質(zhì)特點(diǎn), 主要可以分為兩大類:

第一類方法是在整個(gè)工作區(qū)中應(yīng)用統(tǒng)一的控制策略, 通常包括一個(gè)連續(xù)的函數(shù), 引入了對(duì)任務(wù)空間或機(jī)械臂關(guān)節(jié)空間的輕微變化, 使末端執(zhí)行器避免了奇異區(qū)域[10-11]. 這一類方法通常會(huì)導(dǎo)致一個(gè)連續(xù)修改的雅可比矩陣或操作臂末端運(yùn)動(dòng)軌跡. 且對(duì)于這個(gè)連續(xù)性的函數(shù), 在操作臂末端執(zhí)行器遠(yuǎn)離奇異位置時(shí)是趨向于零的. 在接近奇異位置時(shí), 引入一個(gè)對(duì)雅克比矩陣或任務(wù)空間的輕微調(diào)整, 使得避免雅可比矩陣病態(tài)即不可逆.

第二類方法是是對(duì)奇異區(qū)域進(jìn)行分割, 從而對(duì)周圍的奇異區(qū)域應(yīng)用不同的控制算法. 這類方法的研究成果相比第一類較多[3-7,9], 這里不再贅述.

本文在結(jié)合上述的兩類方法并針對(duì)不同類型奇異位形采用不同處理方法, 并主要針對(duì)內(nèi)部奇異問題,從空間規(guī)劃角度, 提出了一種基于機(jī)器人操作臂工作區(qū)域限定鄰近奇異位置的控制處理算法, 如表1所示.

表1 奇異位形處理算法

1 奇異位置識(shí)別

操作臂奇異直觀的物理表現(xiàn)就是在奇異處運(yùn)動(dòng)退化, 操作臂可操作的自由度減少, 而數(shù)學(xué)上則表示雅克比矩陣的行列式det(J(θ))=0, 即可得到該操作臂的奇異位置點(diǎn). 完成奇異位置的識(shí)別后就可以對(duì)該點(diǎn)進(jìn)行針對(duì)處理, 或通過一種路徑規(guī)劃算法避免該點(diǎn),或通過一種算法能夠使得操作臂末端執(zhí)行器能夠平滑穩(wěn)定的通過該奇異點(diǎn). 實(shí)際上, 事實(shí)并非如此, 關(guān)于操作空間中的奇異點(diǎn)并不具有嚴(yán)格的界限, 所以會(huì)產(chǎn)生操作臂不僅在奇異點(diǎn)而且在鄰近奇異點(diǎn)處也會(huì)發(fā)生運(yùn)動(dòng)退化的情況. 所以可以定義一個(gè)奇異區(qū)閾值det0,若在該閾值內(nèi)則稱為奇異區(qū), 大于該閾值的區(qū)則為非奇異區(qū). 有了奇異區(qū)判斷方法, 就可以在末端執(zhí)行器軌跡規(guī)劃時(shí)有效避開該奇異區(qū), 第3節(jié)將對(duì)此做詳細(xì)介紹.

2 極限位置奇異處理

首先極限位置奇異的產(chǎn)生是由于機(jī)器人操作臂自身結(jié)構(gòu)限制導(dǎo)致的, 故對(duì)于該奇異的處理通常并不是人們考慮重點(diǎn), 最有效的且在實(shí)際中的處理就是避免該奇異位置.

圖1 平面2連桿機(jī)器人操作臂

不過, 也有相關(guān)學(xué)者利用了Null Space Motion(空空間運(yùn)動(dòng))理論對(duì)極限位置奇異進(jìn)行了研究[12], 如圖1,顯示了在極限位置的平面2連桿操作臂, 此時(shí)其操作臂末端執(zhí)行器在奇異方向上將失去運(yùn)動(dòng)能力, 因?yàn)槠鋬蓚€(gè)節(jié)點(diǎn)速度產(chǎn)生的任務(wù)空間速度矢量垂直于奇異方向. 若給予該關(guān)節(jié)一個(gè)空空間的運(yùn)動(dòng), 將產(chǎn)生末端執(zhí)行器整體移動(dòng)效果, 從而避免了奇異位置. 關(guān)于空空間理論在極限位置的研究請(qǐng)參考文獻(xiàn)[9,12].

3 內(nèi)部奇異位置處理

操作臂奇異直觀的物理表現(xiàn)就是在奇異處運(yùn)動(dòng)退化, 操作臂可操作的自由度減少, 而數(shù)學(xué)上則表示雅克比矩陣的行列式 , 即可得到該操作臂的奇異位置點(diǎn). 然而實(shí)際中的奇異點(diǎn)并不具有嚴(yán)格的界限, 所以會(huì)產(chǎn)生操作臂不僅在奇異點(diǎn)而且在鄰近奇異點(diǎn)處也會(huì)發(fā)生運(yùn)動(dòng)退化的情況. 本文將該位置和其附近的區(qū)域稱為奇異區(qū). 進(jìn)行奇異區(qū)定義在意義在于, 根據(jù)該定義可在進(jìn)行末端笛卡爾規(guī)劃時(shí)能規(guī)劃出一個(gè)合適的位置, 從而繞過奇異區(qū). 同時(shí)為保證末端插值點(diǎn)間軌跡符合期望軌跡, 具有連續(xù)和穩(wěn)定性, 還需要對(duì)關(guān)節(jié)進(jìn)行聯(lián)動(dòng)控制即進(jìn)行關(guān)節(jié)空間規(guī)劃. 整個(gè)算法的流程如圖2所示.

圖2 算法流程圖

3.1 基于奇異區(qū)識(shí)別的末端軌跡規(guī)劃

由于操作空間中的奇異點(diǎn)并不具有嚴(yán)格的界限, 不僅在奇異點(diǎn)而且在奇異點(diǎn)附近處也會(huì)發(fā)生運(yùn)動(dòng)退化的情況. 所以可以定義一個(gè)奇異區(qū)閾值 , 在閾值范圍內(nèi)則定義為奇異區(qū). 當(dāng)進(jìn)行末端笛卡爾空間規(guī)劃時(shí)對(duì)每個(gè)規(guī)劃點(diǎn)進(jìn)行判斷, 選擇符合的點(diǎn), 從而避開奇異位置區(qū).

設(shè)末端初始位姿(R1, P1)點(diǎn)a到目標(biāo)位姿(R2, P2)點(diǎn)b. 根據(jù)一定的插補(bǔ)周期T=1/N(自適應(yīng)插補(bǔ)周期算法[13])對(duì)運(yùn)動(dòng)軌跡進(jìn)行插補(bǔ), 并求出每一步的位姿. 則設(shè)第i步位置規(guī)劃為P(i), 第i步姿態(tài)規(guī)劃為R(i), 當(dāng)末端沿直線運(yùn)動(dòng)時(shí)有:

當(dāng)末端沿圓弧運(yùn)動(dòng)時(shí)如圖3, 設(shè)圓心O為(x0,y0,z0), P1為(x1,y1,z1), P2為(x2,y2,z2), Pi為(xi,yi,zi), 則有:

第i步的姿態(tài)R(i)相對(duì)初始姿態(tài)R1的旋轉(zhuǎn)量設(shè)為Rot(f,θi), 則末端規(guī)劃的第i位置的位姿量為:

圖3 末端沿圓弧運(yùn)動(dòng)

根據(jù)解析法[6]求得第i位姿所在的雅可比矩陣J|T(i). 又J|T(i)為n階方陣, 線性代數(shù)性質(zhì)有:

J|T(i)·JT|T(i)為一對(duì)稱矩陣, 故對(duì)奇異區(qū)域Ω定義如下:

由于不同情況的奇異位置其包括的的區(qū)域定義不同, 其中系數(shù)κ就是為了獲得誤差更小的閾值值而引入的一個(gè)調(diào)節(jié)因子(試驗(yàn)中該閾值的經(jīng)驗(yàn)值約為從而若在該閾值內(nèi)則稱為奇異區(qū), 大于該閾值的區(qū)則為非奇異區(qū). 這樣通過式(6), 即可準(zhǔn)確的對(duì)操作臂執(zhí)行器的工作空間進(jìn)行比較準(zhǔn)確的劃分有了奇異區(qū)判斷方法, 就可以在末端執(zhí)行器軌跡規(guī)劃時(shí)有效避開該奇異區(qū).

因?yàn)閮蓚€(gè)插值點(diǎn)之間的運(yùn)動(dòng)路徑存在多種可能選擇,僅僅完成末端的規(guī)劃避開奇異位置可能導(dǎo)致與期望軌跡有所偏差, 同時(shí)也難保證運(yùn)動(dòng)的連續(xù)平滑性和穩(wěn)定性.故還需要對(duì)關(guān)節(jié)進(jìn)行運(yùn)動(dòng)控制, 即關(guān)節(jié)空間規(guī)劃.

3.2 基于高次多項(xiàng)式曲線的關(guān)節(jié)運(yùn)動(dòng)規(guī)劃

由于操作臂的運(yùn)動(dòng)是具有連續(xù)性、平滑和非線性的特點(diǎn), 但是操作臂的每個(gè)關(guān)節(jié)運(yùn)動(dòng)卻是線性的. 且末端位置和方向要隨著時(shí)間推移而表現(xiàn)平穩(wěn), 機(jī)器人關(guān)節(jié)實(shí)際都有最大速度限制, 并且在這個(gè)限制下能在最小運(yùn)動(dòng)時(shí)間內(nèi)完成盡可能多的操作, 速度不可任意選擇, 太大和太小都會(huì)導(dǎo)致不可行軌跡(即奇異導(dǎo)致的情況). 并且通常速度和加速度以及加速度的導(dǎo)數(shù)都要求保證連續(xù)性, 要滿足此條件可以用分段的高次(五次多項(xiàng)式)曲線實(shí)現(xiàn)[14], 這也是工業(yè)電機(jī)驅(qū)動(dòng)常用的速度控制方式. 因此軌跡規(guī)劃的實(shí)質(zhì)就是獲得滿足運(yùn)動(dòng)控制的高次曲線算法.

設(shè)有函數(shù)及其一階二階導(dǎo)數(shù)分別表示關(guān)節(jié)位置、速度、加速度:

設(shè)初始時(shí)t=0, 下一時(shí)刻t=T, 相關(guān)函數(shù)表值如下表2.

表2 t=0、T時(shí)刻位置、速度、加速度值

上表寫成矩陣形式如下:

將式(8)代入式(7)可得系數(shù)向量各項(xiàng)表達(dá)式:

將式(9)代入式(7)即可求得符合條件的關(guān)節(jié)位置、關(guān)節(jié)速度和關(guān)節(jié)加速度. 將該參數(shù)經(jīng)過轉(zhuǎn)換作為輸入送往PMAC控制器, 由PMAC實(shí)現(xiàn)關(guān)節(jié)的實(shí)時(shí)運(yùn)動(dòng)控制.

4 算法驗(yàn)證與分析

首先六軸串聯(lián)機(jī)械臂在4、6關(guān)節(jié)軸共線且關(guān)節(jié)角5等于零時(shí)發(fā)生奇異. 如圖4左示意圖, 要由a運(yùn)動(dòng)到b點(diǎn)位置, 若不通過式(6)繞過奇異區(qū), 那么由a到b時(shí),第四關(guān)節(jié)角度就會(huì)由θa突變到θb從而發(fā)生關(guān)節(jié)速度的瞬時(shí)改變, 導(dǎo)致運(yùn)動(dòng)失控并導(dǎo)致機(jī)構(gòu)收到損壞.

圖4 四五六關(guān)節(jié)奇異避免示意圖

要避免關(guān)節(jié)4的瞬時(shí)改變, 在末端執(zhí)行器接近到達(dá)奇異區(qū)時(shí), 根據(jù)式(6)有a沿圖4右示意圖的藍(lán)色弧線運(yùn)動(dòng), 即保持5關(guān)節(jié)角不變, 使得運(yùn)動(dòng)路徑能繞Z做圓弧運(yùn)動(dòng), 避免關(guān)節(jié)速度瞬時(shí)突變從而避免了奇異問題.

機(jī)械臂末端執(zhí)行器在a的末端初始位姿為Ta=[0,0,1,883.7;0,-1,0,0;1,0,0,700;0,0,0,1], 到目標(biāo)點(diǎn)b的位姿為Tb=[0,0,1,883.7;0,-1,0,200;1,0,0,700;0,0,0,1],利用工具箱進(jìn)行驗(yàn)證仿真運(yùn)動(dòng). 軌跡規(guī)劃采樣時(shí)間為56毫秒, 并對(duì)實(shí)驗(yàn)中采集到的從初始點(diǎn)a到終點(diǎn)b輸出的各關(guān)節(jié)角度數(shù)據(jù)通過處理, 利用MATLAB對(duì)規(guī)劃前后的末端執(zhí)行器的運(yùn)動(dòng)軌跡和各關(guān)節(jié)角的曲線進(jìn)行仿真. 如圖5、6為未采用式(4.2.2.3/4)限制條件時(shí)由空間位置a點(diǎn)運(yùn)動(dòng)到空間位置b點(diǎn)的一種軌跡曲線, 可見其軌跡曲線及加速度曲線并不滿足平滑和連續(xù)性要求. 圖7、8為優(yōu)化限制條件后的軌跡曲線. 同時(shí)優(yōu)化后各個(gè)六個(gè)關(guān)節(jié)的角度值、角速度、交加速度變化曲線如圖9至14所示.

圖5 未規(guī)劃的末端軌跡曲線

圖6 未規(guī)劃的末端運(yùn)動(dòng)相對(duì)X、Y、Z軸變化曲線

圖7 規(guī)劃后末端運(yùn)動(dòng)空間軌跡曲線

圖8 規(guī)劃后末端運(yùn)動(dòng)相對(duì)X、Y、Z軸變化曲線

圖9 關(guān)節(jié)1角度值、角速度、交加速度變化曲線

圖10 關(guān)節(jié)2的角度值、角速度、交加速度變化曲線

圖11 關(guān)節(jié)3的角度值、角速度、交加速度變化曲線

圖12 關(guān)節(jié)4的角度值、角速度、交加速度變化曲線

圖13 關(guān)節(jié)5的角度值、角速度、交加速度變化曲線

圖14 關(guān)節(jié)6的角度值、角速度、交加速度變化曲線

從上述各圖中可以觀察到末端運(yùn)動(dòng)軌跡曲線變化情況, 以及每個(gè)關(guān)節(jié)有初始點(diǎn)運(yùn)動(dòng)到目標(biāo)的的速度、加速度等變化情況, 從圖中各曲線可以看到避過了奇異區(qū), 保證了運(yùn)動(dòng)連續(xù)性, 各關(guān)節(jié)運(yùn)動(dòng)并未出現(xiàn)突變情況表現(xiàn)相對(duì)平滑, 表明了算法的正確和可行性.

6 結(jié)語

在分析了國(guó)內(nèi)外關(guān)于奇異位形的研究基礎(chǔ)上, 提出了基于空間區(qū)域限定的奇異位置避免算法. 從速度控制角度, 通過預(yù)先對(duì)機(jī)器人運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃, 給出了奇異位置及鄰近奇異位置的區(qū)域劃分方法, 從而在笛卡爾空間規(guī)劃時(shí)能規(guī)劃出一合理的插值點(diǎn), 在通過在關(guān)節(jié)空間時(shí), 基于高次曲線理論對(duì)關(guān)節(jié)速度、加速度等的規(guī)劃, 保證了關(guān)節(jié)和末端運(yùn)動(dòng)的連續(xù)性、穩(wěn)定性等. 最后通過MATLAB進(jìn)行實(shí)驗(yàn)了仿真, 仿真結(jié)果驗(yàn)證了該算法的正確性和可行性.

1 Chen G, Jia Q, Sun H. The study on algorithm for avoiding dynamics singularities of space robot. International Conference on Control and Automation, 2009.

2 Marani G, Kim J, Yuh J, et al. A real-time approach for singularity avoidance in resolved motion rate control of robotic manipulators. IEEE International Conference on Robotics and Automation, 2002. Proc. ICRA ’02. IEEE. 2002.

3 Merlet J. Singular configurations of parallel manipulators andGrassmann geometry. The International Journal of Robotics Research, 1989, 8(5): 45–56.

4 方躍法,陳集.機(jī)器人通過奇異位形的控制方法.齊齊哈爾大學(xué)學(xué)報(bào):自然科學(xué)版,2002,18(1):67–70.

5 Tchon K, Muszynski R. Singular inverse kinematic problem for robotic manipulators: A normal form approach. IEEE Trans. on Robotics & Automation, 1998, 14(1): 93–104.

6 于靖軍,劉辛軍,丁希侖,戴建生.機(jī)器人機(jī)構(gòu)學(xué)的數(shù)學(xué)基礎(chǔ).北京:機(jī)械工業(yè)出版社,2008.

7 譚民,等.先進(jìn)機(jī)器人控制.北京:高等教育出版社,2007.

8 Liu H, Zhang T. A new approach to avoid singularities of 6-DOF industrial robot. 2010 International Conference on Mechatronics and Automation (ICMA). 2010.

9 Oetomo D, Jr MHA. Singularity robust algorithm in serial manipulators. Robotics and Computer-Integrated Manufacturing, 2009, 25(1): 122–134.

10 Liu H, Zhang T. A new approach to avoid singularities of 6-DOF industrial robot. 2010 International Conference on Mechatronics and Automation (ICMA). 2010.

11 Kim J, Marani G, Chung W, Yuh J. Task reconstruction method for real-time singularity avoidance for robotic manipulators. Adv Robot 2006, 20(4): 453–481.

12 鄔林波.基于NSB方法的多機(jī)器人編隊(duì)控制[學(xué)位論文].長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué),2010.

13 馮蕊,劉鴻飛,高俊斌,等.工業(yè)機(jī)器人自適應(yīng)生成插補(bǔ)周期的軌跡規(guī)劃算法.機(jī)床與液壓,2009,37(9):173–175.

14 Whitty M. Robotics, vision and control. Fundamental algorithms in MATLAB. Industrial Robot, 2012, 39(6): 75-85.

Singularity Avoidance Planning Algorithm Based on the Identification of Singular Configurations

LV Yong-Jun1,2, LIU Feng2,3, ZHENG Liao-Mo2,3, WU Wen-Jiang2,3, ZHU Liang1,2, GE Xiao-Chuan1,212
(University of Chinese Academy of Sciences, Beijing 100049, China) (Shenyang Institute of Computing Technology, University of Chinese Academy of Sciences, Shenyang 100171, China)3(Shenyang Golding NC Intelligent Tech. Co. Ltd., Shenyang 100171, China)

This paper firstly summarizes the research on the singularity in the past. One is the study of how to avoid it, the other is the study of how to pass the singular point. On the basis of previous studies, a new method is proposed to avoid the singular location based on the space region limited. The main point is to pre-plan the motion trajectory of the robot, and then give a method about how to differentiate the singular position and the adjacent singular position to ensure that a reasonable interpolation point can be planned during the Cartesian space planning. Meanwhile, based on the higher order polynomial theory, joint space planning is optimized to ensure the continuity and stability of joint and terminal motion. Taking 6 axis manipulator in wrist joint singular as an example, we use the proposed algorithm to analyze the process of the singular position. Given the parameters such as the initial point and the target point, the data obtained is simulated graphically in MATLAB, and the correctness and feasibility of the algorithm are verified.

robot; trajectory planning; singular configuration; extreme position

國(guó)產(chǎn)高檔數(shù)控機(jī)床、系統(tǒng)及其技術(shù)在航空領(lǐng)域的綜合應(yīng)用驗(yàn)證及工藝研究(2014ZX04001051)

2016-03-24;收到修改稿時(shí)間:2016-05-05

10.15888/j.cnki.csa.005505