基于區域限定的奇異位置避免規劃算法①

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

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

基于區域限定的奇異位置避免規劃算法①

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

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

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

機器人; 軌跡規劃; 奇異位形; 極限位置

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

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

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

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

表1 奇異位形處理算法

1 奇異位置識別

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

2 極限位置奇異處理

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

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

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

3 內部奇異位置處理

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

圖2 算法流程圖

3.1 基于奇異區識別的末端軌跡規劃

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

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

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

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

圖3 末端沿圓弧運動

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

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

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

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

3.2 基于高次多項式曲線的關節運動規劃

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

設有函數及其一階二階導數分別表示關節位置、速度、加速度:

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

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

上表寫成矩陣形式如下:

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

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

4 算法驗證與分析

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

圖4 四五六關節奇異避免示意圖

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

機械臂末端執行器在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],利用工具箱進行驗證仿真運動. 軌跡規劃采樣時間為56毫秒, 并對實驗中采集到的從初始點a到終點b輸出的各關節角度數據通過處理, 利用MATLAB對規劃前后的末端執行器的運動軌跡和各關節角的曲線進行仿真. 如圖5、6為未采用式(4.2.2.3/4)限制條件時由空間位置a點運動到空間位置b點的一種軌跡曲線, 可見其軌跡曲線及加速度曲線并不滿足平滑和連續性要求. 圖7、8為優化限制條件后的軌跡曲線. 同時優化后各個六個關節的角度值、角速度、交加速度變化曲線如圖9至14所示.

圖5 未規劃的末端軌跡曲線

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

圖7 規劃后末端運動空間軌跡曲線

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

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

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

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

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

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

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

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

6 結語

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

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 于靖軍,劉辛軍,丁希侖,戴建生.機器人機構學的數學基礎.北京:機械工業出版社,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 馮蕊,劉鴻飛,高俊斌,等.工業機器人自適應生成插補周期的軌跡規劃算法.機床與液壓,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

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

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

10.15888/j.cnki.csa.005505