自主操作機器人末端執(zhí)行器的行為動力學運動規(guī)劃

王文運， 傅衛(wèi)平， 魏明明， 王 雯， 楊世強， 韓改寧

(西安理工大學 機械與精密儀器工程學院，陜西 西安 710048)

?

自主操作機器人末端執(zhí)行器的行為動力學運動規(guī)劃

王文運， 傅衛(wèi)平， 魏明明， 王 雯， 楊世強， 韓改寧

(西安理工大學 機械與精密儀器工程學院，陜西 西安 710048)

在動態(tài)或人機合作裝配場合中，基于規(guī)則而不是生物行為啟示的現(xiàn)有操作機器人的運動規(guī)劃方法，難于以自然的方式對工作空間內(nèi)物體的運動進行描述，而行為動力學方法則能彌補這一缺點。本文在工作空間內(nèi)，建立不考慮關(guān)節(jié)約束時的操作機器人末端執(zhí)行器的行為動力學運動規(guī)劃模型。首先,定義了操作機器人末端執(zhí)行器位置運動的基本行為，通過競爭動力學協(xié)調(diào)得到位置運動的整體行為模型。然后，利用行為動力學方法設(shè)計了末端執(zhí)行器姿態(tài)運動的行為。仿真實驗結(jié)果表明：利用行為動力學方法可以實現(xiàn)操作機器人的靜態(tài)及動態(tài)場景下的自主運動規(guī)劃。

自主操作機器人； 行為動力學； 運動規(guī)劃； 位置姿態(tài)； 行為協(xié)調(diào)

自主操作機器人是指在無人員干預(yù)或人機共用工作空間的情況下自主完成作業(yè)任務(wù)的機器人。通常情況下，這類機器人的作業(yè)任務(wù)只對中間運行軌跡做安全性要求，譬如，自主裝配機器人抓取工件或者到達預(yù)裝位置的過程。如何按一定評價標準設(shè)計安全、平穩(wěn)的機器人中間運行軌跡，構(gòu)成了操作機器人的運動規(guī)劃任務(wù)。

操作機器人運動規(guī)劃方法主要有可視圖法[1]、構(gòu)型空間法[2]、快速隨機樹[3]、人工勢場法[4]等，但這些方法存在建模困難或者適合靜態(tài)場景[5-7]，不適用于與動態(tài)環(huán)境構(gòu)成復(fù)雜動力系統(tǒng)的自主操作機器人的運動規(guī)劃問題。 將環(huán)境與機器人統(tǒng)一考慮的行為動力學[8]運動規(guī)劃方法是解決該問題的可能途徑。

本文針對自主操作機器人對運動規(guī)劃的智能、安全等要求，以及國外采用行為動力學進行運動規(guī)劃時可能出現(xiàn)的突變問題，利用基于競爭動力學協(xié)調(diào)的行為動力學方法對其工作空間的軌跡進行規(guī)劃。在不考慮關(guān)節(jié)避障約束的假設(shè)下，建立自主操作機器人末端執(zhí)行器的位置運動和姿態(tài)運動的行為動力學模型，并利用競爭動力學對其行為權(quán)重進行動態(tài)協(xié)調(diào)，避免了運動規(guī)劃時可能出現(xiàn)的突變。通過在靜態(tài)和動態(tài)場景下的仿真實驗，驗證了該方法規(guī)劃操作機器人末端執(zhí)行器運動的可行性。

1 行為動力學運動規(guī)劃模型

串聯(lián)型操作機器人的驅(qū)動方式與移動機器人不同，除了末端執(zhí)行器外，還有與之相聯(lián)接的各關(guān)節(jié)連桿，它們隨著末端執(zhí)行器一起運動。一般情況下，自主操作機器人運動軌跡通過規(guī)劃方法得到其工作空間的描述，再通過求逆解獲得關(guān)節(jié)空間的描述。此外，在操作機器人實際工作空間中，可能存在任意復(fù)雜輪廓的靜態(tài)和動態(tài)障礙物，此時，環(huán)境障礙物對關(guān)節(jié)連桿運動形成約束。由于考慮關(guān)節(jié)約束和復(fù)雜輪廓障礙物的操作機器人末端執(zhí)行器運動規(guī)劃問題涉及到更為復(fù)雜的理論和方法，需要專門的研究工作方能完成，因此，本文為了將行為動力學應(yīng)用到操作機器人末端執(zhí)行器的三維空間運動規(guī)劃，暫時不考慮關(guān)節(jié)連桿的約束，即末端執(zhí)行器獨立運動，并對障礙物進行了簡化處理，將其簡化為障礙球模型(以障礙物最大輪廓包絡(luò)面直徑作為障礙球的直徑)。由于操作機器人末端執(zhí)行器位置和姿態(tài)運動不耦合，本節(jié)將分別建立其位置運動和姿態(tài)運動的行為動力學模型。

1.1 行為動力學方法簡介

行為動力學方法本質(zhì)上適合解決未知復(fù)雜環(huán)境中的機器人運動規(guī)劃問題，其優(yōu)勢在本文給出的相關(guān)參考文獻中已得到證明。該方法利用非線性動力學基本理論，將機器人與其所處動態(tài)環(huán)境看成一個復(fù)雜動力系統(tǒng)，將環(huán)境中的目標和障礙物作為動力系統(tǒng)的平衡點。如果動力系統(tǒng)的特征值均具有負實部，則平衡點是穩(wěn)定的，在該平衡點構(gòu)成了一個吸引子(作為目標)，吸引子附近形成了吸引域，在該區(qū)域中行為變量逐漸趨向于該平衡點；如果動力系統(tǒng)所有特征值的實部中至少有一個正數(shù)，則平衡點不穩(wěn)定，在該平衡點形成了一個排斥子(作為障礙物)，排斥子附近形成了排斥域，該區(qū)域中行為狀態(tài)變量逐漸遠離該平衡點。

行為動力學方法的基本原理是，首先，通過一定的規(guī)則[22]設(shè)計描述機器人運動的行為變量，得到包含目標吸引子、障礙物排斥子以及機器人當前位置的描述；其次，設(shè)計機器人的奔向目標、避障等一系列的基本行為；最終，通過行為的競爭得到整體行為模型，由此產(chǎn)生機器人的規(guī)劃路徑。

假設(shè)機器人的行為變量為x(如航向角，速度等)，則其基本行為模型由非線性動力學方程(1)表示。

(1)

其中：t表示時間，env表示機器人所處位置環(huán)境，f是當前變量狀態(tài)x和機器人所處環(huán)境env的函數(shù)，表示吸引力或排斥力，即描述奔向目標或避障行為。

奔向目標行為模型以吸引子為穩(wěn)定平衡點，其穩(wěn)定性由狀態(tài)變量前的系數(shù)為負(負實部特征值)來構(gòu)造；避障行為以排斥子為不穩(wěn)定平衡點，其穩(wěn)定性由狀態(tài)變量前的系數(shù)為正(正實部特征值)來構(gòu)造。關(guān)于行為動力學穩(wěn)定性證明可參見文獻[9-12]。

根據(jù)上述方法構(gòu)造的基本行為，對其通過合適的權(quán)重進行線性疊加，得到自主機器人整體行為的動力學模型，即式(2)。

(2)

其中權(quán)值ωb∈[-1,1]表示由機器人和環(huán)境通過競爭得到基本行為活躍程度(見1.2.3節(jié))。

1.2 自主操作機器人末端執(zhí)行器位置運動行為動力學建模

選取末端執(zhí)行器運動參考點作為操作機器人當前位置運動描述點(簡稱機器人當前點，不特別指出，均為位置運動描述)。選取操作對象的運動參考(如裝配孔心)作為目標點集(靜止目標為單點)，并將場景內(nèi)與其無關(guān)的物體(其余的操作對象、非執(zhí)行該任務(wù)機器人、操作者等)作為障礙球，對機器人從初始點到達目標點的路徑進行規(guī)劃。

根據(jù)行為變量設(shè)計準則[22]，如圖1所示，在世界坐標系o-xyz內(nèi)，選取機器人當前點速度v、速度v在局部坐標系x″o″y″平面的投影v″與x″軸的夾角φ以及速度v與局部坐標系x″o″y″平面的夾角θ作為行為變量，基于動力系統(tǒng)理論有相應(yīng)的行為動力學模型即式(3)。

(3)

圖1 機器人當前點位置運動行為變量Fig.1 The behavior variables of current point location of manipulator

1.2.1 奔向目標行為

當工作空間中沒有障礙物時，機器人從當前點起始,沿直線趨向目標點的運動稱為奔向目標行為。

圖2 操作機器人末端執(zhí)行器xoy平面投影Fig.2 The projection of xoy plane of end executor of manipulator

根據(jù)奔向目標行為描述，建立目標點為穩(wěn)定平衡點的動力學模型如式(4)所示。行為變量v不能在奔向目標時無限制增大，因此，將奔向目標的速度平衡點設(shè)定為合適的有限值，當奔向目標行為權(quán)值激活后，速度v隨著動力系統(tǒng)的漸近穩(wěn)定逐漸趨向期望速度。

(4)

其中：φtar、θtar分別為目標點相對于機器人當前點的x″軸方向、x″o″y″平面的方位角；vmax為機器人運動最大線速度；φ、θ、v均為操作機器人行為變量；λtar、λv為吸引強度；dtar為目標點與機器人當前點之間的距離(見式(5))；c1為吸引強度衰減系數(shù)；c2為最小吸引強度；cv為速度變化影響系數(shù)。

(5)

趨向目標行為動力學模型的行為變量φ、θ、v與φtar、θtar、dtar、vmax有關(guān)，與其余環(huán)境信息無關(guān)。

1.2.2 避障行為

機器人當前點的避障行為是指其在奔向目標點過程中遇到障礙時，能夠自主安全地繞過障礙的行為。本節(jié)設(shè)計了場景內(nèi)為單個障礙物的情況下的基于不穩(wěn)定平衡點的避障行為動力學模型，即式(6)。當機器人工作空間存在障礙物時，首先獲得每個障礙物的避障行為模型，然后將其線性疊加，最終得到考慮所有障礙物影響的避障行為模型。

(6)

其中：φobs、θobs分別為障礙物簡化球心相對于機器人當前點的x″軸方向、x″o″y″平面方位角；vmin為避障期望速度；λobs為排斥強度；λv為避障期望速度的吸引強度；dobs為機器人當前點與障礙球心之間的距離(見式(7))；c3、c4分別為排斥力的距離和速度衰減系數(shù)；R′為規(guī)劃路徑的曲率半徑；σ為機器人避障排斥的角度范圍，(見式(8))。

(7)

(8)

式中r是機器人當前點簡化成球體后的半徑，R是障礙物最大半徑。

1.2.3 操作機器人當前點的整體行為模型

操作機器人當前點的整體行為是外部環(huán)境與操作機器人內(nèi)部固有性質(zhì)共同決定的行為。通過設(shè)計隨整個系統(tǒng)內(nèi)部相對位置及速度因素變化而動態(tài)變化的權(quán)重系數(shù)，得到線性疊加后的整體行為模型，即式(9)。

(9)

式中，ωtar為趨向目標行為權(quán)重，ωobs為避障行為權(quán)重，b為動力系統(tǒng)阻尼系數(shù)(防止運動軌跡發(fā)散)。

式(9)中，ωtar∈[0,1]和ωobs∈[0,1]作為操作機器人當前點的行為權(quán)重，反映了奔向目標行為與避障行為的相互抑制程度?；诜蔷€性動力學理論，通過設(shè)計合適參數(shù)，得到奔向目標行為與避障行為的競爭動力學模型如式(10)[23]。

(10)

式中，α1、α2分別為奔向目標行為、避障行為的競爭優(yōu)勢；γ12為奔向目標與避障行為共存時避障行為對奔向目標行為的制約程度；γ21為奔向目標與避障行為共存時奔向目標行為對避障行為的制約程度。

α1、α2、γ12、γ21均為與環(huán)境相關(guān)參數(shù)， 考慮到取值范圍為[0,1]以及速度和障礙物距離對其產(chǎn)生的影響，設(shè)計競爭參數(shù)式(11)。

(11)

式中，do為機器人與障礙物之間的距離；de為相互作用區(qū)中心與障礙物之間的距離；dc為相互作用區(qū)半徑；co1～co4為競爭參數(shù)的衰減系數(shù)。

1.3 自主操作機器人姿態(tài)的行為動力學建模

將機器人當前姿態(tài)假想成具有方向的球體，如圖3所示姿態(tài)方向線ori在局部坐標系x″o″y″平面的投影ori″與x″軸夾角φ以及姿態(tài)方向線ori與局部坐標系x″o″y″平面的夾角ψ作為姿態(tài)運動的行為變量；選取操作機器人工作空間中的被抓物體形心所在中心線姿態(tài)作為目標姿態(tài)吸引子，如圖4所示目標中心線ori在局部坐標系x″o″y″平面投影ori″與x″軸夾角為φtar以及目標中心線ori與局部坐標系x″o″y″平面夾角ψtar，建立操作機器人末端執(zhí)行器的姿態(tài)行為動力學模型，即式(12)。

(12)

其中,λori為姿態(tài)吸引強度。

圖3 機器人當前點姿態(tài)行為變量Fig.3 The behavior variables of current posture of manipulator

(o-xyz為世界坐標系，x″y″z″為隨操作對象平移的坐標系，其坐標原點為操作對象的形心)圖4 目標姿態(tài)Fig.4 The posture of target

2 仿真實驗

2.1 靜態(tài)場景運動規(guī)劃仿真實驗

本次仿真實驗所設(shè)計的靜態(tài)場景是指操作機器人的工作空間內(nèi)的目標的位置及姿態(tài)和障礙簡化球的位置及尺寸不發(fā)生變化。實驗的模型參數(shù)設(shè)置如下：

行為動力學模型初始參數(shù)設(shè)置如下：

從圖5可以看出，操作機器人位置運動軌跡實現(xiàn)了三維避障，并且隨著位置趨近目標點的同時，姿態(tài)也在趨近目標姿態(tài)。仿真實驗中奔向目標和避障行為的權(quán)重變化如圖6所示，在接近障礙物時避障行為權(quán)重急劇增加，奔向目標行為權(quán)重相應(yīng)降低；當遠離障礙物時，避障行為權(quán)重迅速下降，奔向目標行為權(quán)重相應(yīng)上升，實現(xiàn)了基本行為通過競爭動力學得到激活或者抑制的目的，且不出現(xiàn)突變，但速度有變化。操作機器人位置運動當前點的線速度變化如圖7所示，奔向目標時線速度增加到限定值，奔向目標行為和避障行為共存時速度降低到安全速度。

(紅色箭頭代表起點及其姿態(tài)，黑色箭頭代表目標點及其姿態(tài)，藍色箭頭代表機器人姿態(tài))圖5 末端執(zhí)行器位姿運動參考點三維軌跡Fig.5 The 3D posture movement trajectory of end executor point of manipulator

(藍色圖線代表奔向目標行為，紅色圖線代表避障行為)圖6 奔向目標行為和避障行為權(quán)重變化圖Fig.6 The weight variable figure of behavior towards the target and avoidance obstacle

圖7 末端執(zhí)行器運動參考點速度變化圖Fig.7 The velocity variable figure of end executor of manipulator

從靜態(tài)場景仿真實驗可以看出，采用行為動力學方法可以實現(xiàn)靜態(tài)場景下的自主操作機器人末端執(zhí)行器的運動規(guī)劃任務(wù)。采用酷睿i5處理器、主頻為2.5GHz的筆記本電腦，在MATLAB平臺R2010中運行仿真程序，平均每步規(guī)劃計算時間136ms，可以滿足實時性要求。

2.2 動態(tài)場景運動規(guī)劃仿真實驗

根據(jù)動態(tài)場景的要求，設(shè)計障礙球半徑變化規(guī)律如下：ri=ai+bi*rand(1)，其中ri為第i個障礙球半徑，ai為基本半徑(范圍為5～15)，bi為半徑變化因子(范圍為5～8)，rand(1)為0～1之間隨機數(shù)。為了方便比較，障礙球半徑變化仿真實驗進行了兩次，實驗數(shù)據(jù)在同一幅圖中顯示(其中障礙球半徑為基本尺寸)。從圖8可以看出，由于障礙球尺寸的微小變化，兩條規(guī)劃路徑差異比較大，但是末端執(zhí)行器位置運動軌跡均實現(xiàn)了平滑避障。

圖8 障礙球半徑動態(tài)變化的末端執(zhí)行器運動規(guī)劃Fig.8 The motion planning of end executor in the case of obstacle sphere dynamic change

針對動態(tài)場景中可能會有移動障礙的情況，同時考慮障礙球半徑隨機變化，設(shè)計移動障礙球的運動規(guī)律：

式中t為迭代步。

從圖9可以看出，當工作場景中存在移動障礙時，末端執(zhí)行器能夠平滑地避開移動障礙球。

圖9 存在移動障礙的末端執(zhí)行器運動規(guī)劃Fig.9 The motion planning of end executor in the case of obstacle movement

從動態(tài)場景仿真實驗可以看出，行為動力學方法在一定條件下適應(yīng)場景動態(tài)變化的運動規(guī)劃任務(wù)，而且規(guī)劃路徑比較靈活，可以實現(xiàn)自主機器人的行為演化。

3 結(jié) 語

運動規(guī)劃是操作機器人成為自主裝配機器人面臨的關(guān)鍵問題。為了解決現(xiàn)有操作機器人運動規(guī)劃方法不能很好適應(yīng)自主操作機器人與環(huán)境的耦合信息難以融合問題，本文采用行為動力學方法對操作機器人末端執(zhí)行器工作空間的位置和姿態(tài)的運動進行了規(guī)劃。研究表明，依據(jù)所建立的行為動力學運動規(guī)劃模型，可以在靜態(tài)或動態(tài)的工作場景中產(chǎn)生光滑連續(xù)的末端執(zhí)行器位姿運動軌跡，實現(xiàn)無關(guān)節(jié)約束時的操作機器人導航要求。由于行為動力學方法只需要底層工作空間中實體的距離信息，因此也適用于其它類型操作機器人的運動規(guī)劃。

下一步研究將以行為動力學為基礎(chǔ)，主要圍繞以下內(nèi)容展開：①通過基于機器視覺的末端執(zhí)行器空間運動規(guī)劃軌跡的檢測，試驗驗證本文的理論模型；②考慮機器人末端執(zhí)行器速度平滑的運動規(guī)劃；③操作機器人關(guān)節(jié)連桿約束避障的運動規(guī)劃；④復(fù)雜場景下的操作機器人運動規(guī)劃；⑤模型參數(shù)的自適應(yīng)動態(tài)調(diào)整，以提高操作機器人運動規(guī)劃的自主性。

[1]RASHIDAT,ALIAA,FRASCAM,etal.Pathplanningwithobstacleavoidancebasedonvisibilitybinarytreealgorithm[J].RoboticsandAutonomousSystems, 2013, 61(12): 1440-1449.

[2]ROYD.Algorithmicpathplanningofstaticrobotsinthreedimensionsusingconfigurationspacemetrics[J].Robotica, 2011, 29(2): 295-315.

[3]周芳，朱齊丹，張新福，等. 基于雅可比轉(zhuǎn)秩-RRT法的冗余機械手路徑規(guī)劃[J]. 控制工程，2009，16(6)：756-759.

ZHOUFang,ZHUQidan,ZHANGXinfu,etal.PathplanningapproachforredundantmanipulatorbasedonJacobiantranspose-RRTalgorithm[J].ControlEngineeringofChina, 2009, 16(6): 756-759.

[4]彭艷，國文青，劉梅，等. 基于切點優(yōu)化人工勢場法的三維避障規(guī)劃[J]. 系統(tǒng)仿真學報，2014，26(8)：1758-1762.

PENGYan,GUOWenqing,LIUMei,etal.Obstacleavoidanceplanningbasedonartificialpotentialfieldoptimizedbypointoftangencyinthree-dimensionalspace[J].JournalofSystemSimulation, 2014, 26(08):1758-1762.

[8]IOSSIFIDISI.Utilizingartificialskinfordirectphysicalinteraction[C]//2013IEEEInternationalConferenceonRoboticsandBiomimetics(ROBIO), 2013: 810-815.

[12]LARGE E W, CHRISTENSEN H I, BAJCSY R. Scaling the dynamic approach to path planning and control: Competition among behavioral constraints[J]. The International Journal of Robotics Research, 1999, 18(1): 37-58.

[13]XU Heting, Yang S X. Real-time collision-free motion planning of nonholonomic robots using a neural dynamics based approach[C]//2002 Proceedings ICRA’02 IEEE International Conference on Robotics and Automation, 2002: 3087-3092.

[14]ALTHAUS P, CHRISTENSEN H I. Behavior coordination in structured environments[J]. Advanced Robotics, 2003, 17(7): 657-674.

[19]IOSSIFIDIS I, MALYSIAK D, REIMANN H. Model-free local navigation for humanoid robots[C]//2011 IEEE International Conference on Robotics and Biomimetics (ROBIO), 2011: 2204-2210.

[20]雷艷敏，朱齊丹，馮志彬. 基于速度障礙和行為動力學的動態(tài)運動規(guī)劃[J]. 華中科技大學學報(自然科學版)，2011，39(4)：15-19.

LEI Yanmin, ZHU Qidan, FENG Zhibin. Dynamic path planning using velocity obstacles and behavior dynamicals[J]. Journal Huazhong Univsity of Science and Technology(Natural Science Edition), 2011, 39(4): 15-19.

[21]郝大鵬，傅衛(wèi)平，王雯. 基于接觸時間的移動機器人安全導航[J].計算機應(yīng)用，2014，34(4)：1209-1212.

HAO Dapeng, FU Weiping, WANG Wen. Mobile robot safety navigation based on time to contact[J]. Journal of Computer Applications, 2014, 34(4): 1209-1212.

[23]LARGE E W, CHRISTENSEN H I, BAJCSY R. Dynamic robot planning: cooperation through competition[C]//1997 IEEE International Conference on Robotics and Automation, 1997: 2306-2311.

(責任編輯 王衛(wèi)勛)

Behavior dynamics method for the motion planning of the end-effector of autonomous manipulator

WANG Wenyun, FU Weiping, WEI Mingming, WANG Wen, YANG Shiqiang, HAN Gaining

(School of Mechanical and Precision Instrument Engineering, Xi’an University of Technology, Xi’an 710048, China)

In the dynamic or human-robot cooperation assembly scene, it is difficult for the existing methods for path planning of manipulator based on rules rather than the enlightenment of biological behavior to describe the movement of objects in the space with the natural way, with behavior dynamics method making up for the disadvantages. In this paper, the behavior dynamics method is used in working space of the end-effector of manipulator for the motion planning. Firstly, we define the basic behavior for position movement of robot end-effector, with the position movement of the overall behavior obtained by a competitive dynamics model; secondly, we use the behavior dynamics method for designing orientation movement behavior. The results of the simulation experiment show that the use of the behavior dynamics method can realize the manipulator’s motion planning in the static and dynamical scene.

autonomous manipulator; behavior dynamical method; motion planning; position and orientation; behavior coordination

10.19322/j.cnki.issn.1006-4710.2016.04.016

2015-11-02

國家自然科學基金資助項目(51475365)；陜西省自然科學基礎(chǔ)研究計劃重點資助項目(2011JZ012)；陜西省科技統(tǒng)籌創(chuàng)新工程重點實驗室資助項目(2014SZ10-P04)；陜西省教育廳重點實驗室科研計劃資助項目(13JS070)

王文運，男，碩士生，研究方向為智能機器人技術(shù)。E-mail: wyw11_11_11@foxmail.com

傅衛(wèi)平，男，博士，教授，博導，研究方向為智能機器人、智能車輛及其控制理論與技術(shù)、機電系統(tǒng)動力學及其控制、現(xiàn)代物流系統(tǒng)工程與技術(shù)等。E-mail: weipingf@xaut.edu.cn

TP242.6

A

1006-4710(2016)04-0468-07