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

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

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

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

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

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

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

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

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

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

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

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

表1 奇異位形處理算法

1 奇異位置識別

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

2 極限位置奇異處理

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

圖1 平面2連桿機器人操作臂

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

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

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

圖2 算法流程圖

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

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

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

當末端沿圓弧運動時如圖3, 設圓心O為(x0,y0,z0), P1為(x1,y1,z1), P2為(x2,y2,z2), Pi為(xi,yi,zi), 則有:

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

圖3 末端沿圓弧運動

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

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

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

因為兩個插值點之間的運動路徑存在多種可能選擇,僅僅完成末端的規(guī)劃避開奇異位置可能導致與期望軌跡有所偏差, 同時也難保證運動的連續(xù)平滑性和穩(wěn)定性.故還需要對關節(jié)進行運動控制, 即關節(jié)空間規(guī)劃.

3.2 基于高次多項式曲線的關節(jié)運動規(guī)劃

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

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

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

表2 t=0、T時刻位置、速度、加速度值

上表寫成矩陣形式如下:

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

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

4 算法驗證與分析

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

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

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

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

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

圖6 未規(guī)劃的末端運動相對X、Y、Z軸變化曲線

圖7 規(guī)劃后末端運動空間軌跡曲線

圖8 規(guī)劃后末端運動相對X、Y、Z軸變化曲線

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

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

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

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

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

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

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

6 結(jié)語

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

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 方躍法,陳集.機器人通過奇異位形的控制方法.齊齊哈爾大學學報:自然科學版,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 于靖軍,劉辛軍,丁希侖,戴建生.機器人機構(gòu)學的數(shù)學基礎.北京:機械工業(yè)出版社,2008.

7 譚民,等.先進機器人控制.北京:高等教育出版社,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方法的多機器人編隊控制[學位論文].長沙:國防科學技術大學,2010.

13 馮蕊,劉鴻飛,高俊斌,等.工業(yè)機器人自適應生成插補周期的軌跡規(guī)劃算法.機床與液壓,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

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

2016-03-24;收到修改稿時間:2016-05-05

10.15888/j.cnki.csa.005505