基于可操作度的移動(dòng)機(jī)械臂路徑規(guī)劃研究

張宏釗，劉順桂，姜 勇 ，王天龍，景鳳仁

（1.深圳供電局有限公司，深圳 518001；2.中國科學(xué)院沈陽自動(dòng)化研究所 機(jī)器人學(xué)國家重點(diǎn)實(shí)驗(yàn)室，沈陽110016）

移動(dòng)機(jī)械臂結(jié)合了移動(dòng)機(jī)器人與機(jī)械臂兩者的優(yōu)點(diǎn)，極大地增強(qiáng)了系統(tǒng)的靈活性和工作空間，可以在復(fù)雜環(huán)境中進(jìn)行搬運(yùn)、裝配和夾取等操作。與此同時(shí)移動(dòng)機(jī)械臂系統(tǒng)也帶來了一系列的問題和挑戰(zhàn)，其中就包括機(jī)械臂和移動(dòng)平臺的耦合性引起的系統(tǒng)路徑規(guī)劃問題。針對此問題，國內(nèi)外學(xué)者進(jìn)行了大量的研究工作：文獻(xiàn)[1]提出基于人工勢場法的移動(dòng)機(jī)械臂路徑規(guī)劃，此方法雖然能夠生成一條路徑有效地避開障礙物，但是卻未曾考慮到機(jī)械臂陷入局部最優(yōu)點(diǎn)而無法到達(dá)全局最優(yōu)點(diǎn)。文獻(xiàn)[2]提出一種基于A*算法全局避障規(guī)劃方法，根據(jù)機(jī)械臂和障礙物的相對位姿關(guān)系得到碰撞條件，求解出自由工作空間，進(jìn)而使用A*算法在自由空間中規(guī)劃一條無碰撞的路徑。但A*算法卻無法解決移動(dòng)機(jī)械臂耦合的問題。文獻(xiàn)[3]針對移動(dòng)機(jī)械臂的運(yùn)動(dòng)規(guī)劃問題，提出一種基于運(yùn)動(dòng)解耦的規(guī)劃策略。移動(dòng)機(jī)械臂系統(tǒng)視為移動(dòng)平臺和機(jī)械臂兩個(gè)不同的子系統(tǒng)分別來進(jìn)行控制，同時(shí)采用基于表征空間的A*搜索策略，既解決了系統(tǒng)耦合問題，又實(shí)現(xiàn)了全局最優(yōu)點(diǎn)。但是卻在某種程度上忽略機(jī)械臂的性能對路徑規(guī)劃的影響。

本文針對移動(dòng)機(jī)械臂在復(fù)雜室外環(huán)境中作業(yè)的一系列任務(wù)需求，提出基于可操作度性能指標(biāo)的移動(dòng)機(jī)械臂路徑規(guī)劃方法，分別建立了機(jī)械臂和移動(dòng)平臺兩個(gè)子系統(tǒng)的運(yùn)動(dòng)學(xué)方程，進(jìn)而求取機(jī)械臂的雅可比矩陣，分析機(jī)械臂在其工作空間內(nèi)的可操作度性能指標(biāo)，然后依據(jù)指標(biāo)值并同時(shí)考慮機(jī)械臂與移動(dòng)平臺之間的干涉，得到機(jī)械臂的靈活工作空間。最后得到移動(dòng)機(jī)械臂路徑規(guī)劃方法。

1 系統(tǒng)運(yùn)動(dòng)學(xué)模型

1.1 移動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)模型

移動(dòng)機(jī)械臂系統(tǒng)由一個(gè)六自由度機(jī)械臂和一個(gè)四輪獨(dú)驅(qū)的移動(dòng)平臺組成，系統(tǒng)模型如圖1所示。

圖1 移動(dòng)機(jī)械臂系統(tǒng)模型Fig.1 System model of mobile manipulator

分別建立機(jī)械臂和移動(dòng)平臺的運(yùn)動(dòng)學(xué)模型。圖2為機(jī)械臂的坐標(biāo)系，依據(jù)各關(guān)節(jié)坐標(biāo)系的位姿關(guān)系，利用經(jīng)典的D-H參數(shù)法得到關(guān)節(jié)和連桿的D-H參數(shù)，并將其依次代入齊次變換矩陣通式：

式中：ai為桿長，m；θi為關(guān)節(jié)角度；αi-1為扭轉(zhuǎn)角，°；di為偏置，m。

圖2 機(jī)械臂關(guān)節(jié)坐標(biāo)系Fig.2 Joint coordinate system of manipulator

可以得到各關(guān)節(jié)的齊次變換矩陣，根據(jù)串聯(lián)機(jī)器人的特點(diǎn)，將各關(guān)節(jié)的齊次變換方程按順序依次相乘，可以得到機(jī)械臂的正向運(yùn)動(dòng)學(xué)方程如式（2）所示：

式中：n為法向矢量；o為方位矢量；a為接近矢量；p為位置矢量。

由移動(dòng)平臺的尺寸等相關(guān)設(shè)計(jì)參數(shù)可以得到其質(zhì)心 Op的位置坐標(biāo)為（xp，yp，zp），相對于世界坐標(biāo)系的平移變換為

式中：zp為Lh+r；Lh為移動(dòng)平臺的高度。

移動(dòng)平臺是繞世界坐標(biāo)系的z軸旋轉(zhuǎn)的，故其相對于世界坐標(biāo)系的旋轉(zhuǎn)變換矩陣為

結(jié)合以上平移變換和旋轉(zhuǎn)變換，移動(dòng)平臺質(zhì)心，即機(jī)械臂基坐標(biāo)原點(diǎn)相對于世界坐標(biāo)系的齊次變換矩陣為

（2）“構(gòu)成境域”。在繪畫現(xiàn)象中，對象是人的意識所面對的對象，而人的意識，是一具有時(shí)間性的，不斷生成、不斷流變的意識流。事物對象就是在此不斷生成，不斷流變的方式中被意識著的，于是，同一對象在意識流之中就成為“無限多的意象群”的結(jié)合物。例如賈克梅蒂為美國作家詹姆斯·洛德畫肖像時(shí)，為了要抓住自己的心理過程，力圖接近繪畫的真實(shí)本質(zhì)，賈科梅蒂在寫生過程前后總共畫了十八次，線條反復(fù)涂抹的方法，刻畫了表現(xiàn)對象在“不斷流變，不斷生成”的過程中的純視覺體驗(yàn)，以期用這種純視覺的不斷追問，來感知事物存在的本質(zhì)狀態(tài)和畫家本人的自然心相。

式（5）即為移動(dòng)平臺質(zhì)和機(jī)械臂基坐標(biāo)原點(diǎn)相對于世界坐標(biāo)系的齊次變換矩陣。在已經(jīng)得到移動(dòng)平臺相對于世界坐標(biāo)系的齊次變換的基礎(chǔ)上，只要求得機(jī)械臂相對于移動(dòng)平臺的齊次變換就可以得到移動(dòng)機(jī)械臂的運(yùn)動(dòng)學(xué)模型。

建立移動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)模型的目的是為了得到末端工具坐標(biāo)系的位姿與移動(dòng)平臺基坐標(biāo)系之間關(guān)系。如下式：

1.2 機(jī)械臂雅可比矩陣

雅可比矩陣表示機(jī)械臂關(guān)節(jié)空間速度到笛卡爾空間速度的映射關(guān)系。利用雅可比矩陣，可以建立笛卡爾空間機(jī)械臂末端執(zhí)行器的速度與各關(guān)節(jié)速度間的關(guān)系，以及末端執(zhí)行器與外界接觸力對應(yīng)各關(guān)節(jié)力之間的聯(lián)系[4]。雅可比矩陣還用于描述機(jī)械臂的某些性能指標(biāo)，在運(yùn)動(dòng)學(xué)層面，如可操作度性能指標(biāo)、速度和加速度性能指標(biāo)等；在動(dòng)力學(xué)層面，雅可比矩陣可以用于求解力性能指標(biāo)[5]等。因此，無論是運(yùn)動(dòng)學(xué)還是動(dòng)力學(xué)，雅可比矩陣都占據(jù)重要的地位。

求解雅可比矩陣通式為

線速度采用時(shí)間求導(dǎo)法，角速度采用構(gòu)造法，綜合求解機(jī)械臂的雅可比矩陣。速度的雅克比矩陣可以用一個(gè)速度列向量來表示：

式中：vx，vy，vz為 x，y，z軸上的線速度；ωx，ωy，ωz為 x，y，z軸上的角速度。

將速度雅克比分解兩個(gè)部分矢量，線速度矢量和角速度量。機(jī)械臂速度雅克比矩陣為

可得

通過雅可比矩陣可以求解機(jī)械臂在其工作空間內(nèi)的可操作度性能指標(biāo)，進(jìn)而來判斷其靈活性。

2 機(jī)械臂工作空間分析

根據(jù)雅可比矩陣求解可操作度性能指標(biāo)，由于可操作性能是機(jī)械臂固有屬性，所以機(jī)械臂的可操作度性能指標(biāo)在一定程度上影響機(jī)械臂的工作性能，限制了機(jī)械臂的靈活工作空間，所以基于機(jī)械臂可操作度提出了移動(dòng)機(jī)械臂分層軌跡規(guī)劃[6]。

機(jī)械臂的性能評價(jià)主要有速度性能指標(biāo)、加速度性能指標(biāo)、可操作度性能指標(biāo)、工作空間性能指標(biāo)和力性能指標(biāo)等。其中衡量一個(gè)機(jī)器人在工作空間的整體靈活性的性能指標(biāo)是可操作度。

可操作度的物理意義是各個(gè)方向上運(yùn)動(dòng)能力的綜合度量，Yoshikawa將雅可比與其轉(zhuǎn)置之積的行列式定義為可操作度的度量指標(biāo)，即：

式中：J（q）為雅可比矩陣。

顯然，利用操作度指標(biāo)可以直接判別機(jī)器人的奇異位形?？刹僮鞫戎笜?biāo)w在［0，1］之間，當(dāng)w=0時(shí)，機(jī)器人處于奇異位形，此時(shí)機(jī)器人的可操作度為0。由此可知，可操作度值越大，該位形越遠(yuǎn)離奇異狀態(tài)。

根據(jù)機(jī)械臂可操作度指標(biāo)w的取值范圍不同，將機(jī)械臂的工作空間進(jìn)一步細(xì)化。在前文中已經(jīng)求解出機(jī)械臂的理論工作空間，這里不再贅述，根據(jù)機(jī)械臂可操作度求解靈活工作空間流程如圖3如下。

圖3 機(jī)械臂可操作度求解靈活工作空間流程Fig.3 Manipulator operability to solve flexible workspace flow chart

首先，根據(jù)機(jī)械臂的正向運(yùn)動(dòng)學(xué)方程，確定機(jī)械臂末端位姿的矢量表達(dá)式；然后，在關(guān)節(jié)約束范圍內(nèi)，隨機(jī)生成一組關(guān)節(jié)變量；最后，將生成的關(guān)節(jié)變量代入正向運(yùn)動(dòng)學(xué)表達(dá)式，生成工作空間三維點(diǎn)云圖。利用隨機(jī)函數(shù)產(chǎn)生關(guān)節(jié)變量：

將產(chǎn)生的一組關(guān)節(jié)變量值代入下式中：

式中：θmaxi為關(guān)節(jié)變量上限，i=1，2，…，6；θmini為關(guān)節(jié)變量下限，i=1，2，…，6。

傳統(tǒng)方法求解得到的是理論上的工作空間，并不能反映出機(jī)械臂實(shí)際工作的真實(shí)情況。由于機(jī)械臂是安裝在移動(dòng)平臺上的，這就導(dǎo)致了機(jī)械臂在運(yùn)動(dòng)過程中與移動(dòng)平臺發(fā)生干涉的可能，同時(shí)也在一定程度上限制了機(jī)械臂的實(shí)際工作空間。又因?yàn)闄C(jī)械臂本身在某些位置也存在奇異位形，又在一定程度上制約了機(jī)械臂的靈活性。為使機(jī)械臂能在其實(shí)際工作空間范圍內(nèi)高效地完成任務(wù)，這里引入機(jī)械臂的可操作度性能指標(biāo)概念。

考慮到機(jī)械臂本身的結(jié)構(gòu)特點(diǎn)及與移動(dòng)平臺發(fā)生干涉的可能性，機(jī)械臂的實(shí)際工作空間并非是理論工作空間。在實(shí)際工作空間中機(jī)械臂的可操作度性能是有差異的，在大部分區(qū)域機(jī)械臂都具有良好的可操作性能；在一些區(qū)域機(jī)械臂的可操作性一般，可以勉強(qiáng)完成作業(yè)任務(wù)；在另外一些區(qū)域機(jī)械臂的操作度極差甚至幾乎處于奇異位形，是在任務(wù)規(guī)劃或控制中絕對要避免的。為使機(jī)械臂在執(zhí)行作業(yè)任務(wù)時(shí)具有良好的可操作性能，提出了基于可操作度性能指標(biāo)的規(guī)劃策略。

3 路徑規(guī)劃方法

將基于可操作度性能指標(biāo)劃分的工作空間分別投影到XZ平面和YZ平面，可以得到機(jī)械臂的靈活工作空間[7]、欠靈活工作空間及奇異工作空間在機(jī)械臂實(shí)際工作空間內(nèi)的分層分布情況。

對于點(diǎn)到點(diǎn)的工作任務(wù)來說，由于移動(dòng)機(jī)械臂具有移動(dòng)性能，所以一般情況下可以保證移機(jī)械臂末端執(zhí)行器的初始位置在靈活工作空間內(nèi)，故移動(dòng)機(jī)械臂的初始位置和目標(biāo)位置的狀態(tài)只有4種情況：①初始點(diǎn)和目標(biāo)點(diǎn)在同一層工作空間且都在靈活工作空間內(nèi)；②初始點(diǎn)和目標(biāo)點(diǎn)不在同一層工作空間，初始點(diǎn)在靈活工作空間內(nèi)，而目標(biāo)點(diǎn)在欠靈活工作空間內(nèi)；③初始點(diǎn)和目標(biāo)點(diǎn)不在同一操作空間，且初始點(diǎn)在靈活工作空間而目標(biāo)點(diǎn)則在欠靈活工作空間之中；④初始點(diǎn)和目標(biāo)點(diǎn)不在同一操作空間中，且初始點(diǎn)在靈活工作空間而目標(biāo)點(diǎn)不在操作空間之中，如圖4所示。

圖4 移動(dòng)機(jī)械臂操作空間起始點(diǎn)和目標(biāo)點(diǎn)分布情況Fig.4 Mobile manipulator workspace starting point and target point distribution

根據(jù)圖4可知，當(dāng)起始點(diǎn)和目標(biāo)點(diǎn)在靈活工作空間內(nèi)時(shí)，機(jī)械臂執(zhí)行任務(wù)時(shí)其操作性能最好，而此時(shí)移動(dòng)平臺是不需要調(diào)整位姿就可以滿足作業(yè)任務(wù)需求；但是當(dāng)目標(biāo)點(diǎn)位于操作性能較差甚至是工作空間之外時(shí)，就需要通過協(xié)調(diào)移動(dòng)平臺和機(jī)械臂來進(jìn)一步滿足任務(wù)需求，這也就是移動(dòng)平臺和機(jī)械臂之間協(xié)調(diào)的必要性。

綜上所述，基于可操作度性能指標(biāo)的移動(dòng)機(jī)械臂規(guī)劃策略為首先保證起始點(diǎn)相對于機(jī)械臂基座的笛卡爾空間距離小于靈活工作空間半徑r1；然后判斷目標(biāo)點(diǎn)相對機(jī)械臂基座的距離，若小于r1，則直接對機(jī)械臂進(jìn)行規(guī)劃，這屬于機(jī)械臂層面的軌跡規(guī)劃；否則，先規(guī)劃移動(dòng)平臺的軌跡，當(dāng)移動(dòng)平臺運(yùn)動(dòng)到使機(jī)械臂基座到目標(biāo)點(diǎn)的距離小于r1時(shí)，再進(jìn)行機(jī)械臂的軌跡規(guī)劃，這屬于移動(dòng)平臺層面的軌跡規(guī)劃。這種規(guī)劃策略的突出優(yōu)勢在于消除了機(jī)械臂與移動(dòng)平臺之間的相互耦合影響，同時(shí)利用了其冗余性。

4 仿真與實(shí)驗(yàn)

4.1 仿真

為了驗(yàn)證基于可操作度性能指標(biāo)的移動(dòng)機(jī)械臂軌跡規(guī)劃策略，設(shè)計(jì)如下仿真環(huán)境，如圖5所示。針對起始點(diǎn)和目標(biāo)點(diǎn)在機(jī)械臂靈活工作空間的幾種分布情況，選擇最具有代表性的任務(wù)，對移動(dòng)機(jī)械臂軌跡規(guī)劃策略進(jìn)行驗(yàn)證。

圖5 移動(dòng)機(jī)械臂軌跡規(guī)劃策略仿真驗(yàn)證Fig.5 Simulation of mobile robotic trajectory planning strategy

根據(jù)仿真結(jié)果分析，設(shè)置的目標(biāo)點(diǎn)并不在UR10機(jī)械臂的工作空間范圍內(nèi)，所以首先對移動(dòng)平臺進(jìn)行軌跡規(guī)劃，規(guī)劃的目標(biāo)是使目標(biāo)點(diǎn)在機(jī)械臂的靈活工作空間中；然后，規(guī)劃機(jī)械臂層面的軌跡，機(jī)械臂從當(dāng)前位置運(yùn)動(dòng)到目標(biāo)位置。由此仿真結(jié)果可以證實(shí)基于可操作度的移動(dòng)機(jī)械臂軌跡規(guī)劃策略有效可行。

4.2 實(shí)驗(yàn)

利用變電站巡檢機(jī)器人系統(tǒng)平臺對規(guī)劃策略進(jìn)行實(shí)驗(yàn)驗(yàn)證。如圖6所示，移動(dòng)機(jī)械臂的初始位置在A點(diǎn)，目標(biāo)位置在G點(diǎn)，顯然不能由機(jī)械臂直接規(guī)劃來完成，所以先進(jìn)行移動(dòng)平臺層面的軌跡規(guī)劃，運(yùn)動(dòng)到B點(diǎn)后，測得G點(diǎn)已經(jīng)位于機(jī)械臂的靈活工作空間，機(jī)械臂直接運(yùn)行到目標(biāo)點(diǎn)G。由實(shí)驗(yàn)結(jié)果可證實(shí)移動(dòng)機(jī)械臂軌跡規(guī)劃策略是可行的。

圖6 移動(dòng)機(jī)械臂軌跡規(guī)劃策略實(shí)驗(yàn)驗(yàn)證Fig.6 Experimental planning of mobile manipulator trajectory planning

5 結(jié)語

本文針對移動(dòng)機(jī)械臂在復(fù)雜室外環(huán)境中的作業(yè)需求，提出并實(shí)現(xiàn)基于可操作度的移動(dòng)機(jī)械臂路徑規(guī)劃方法。將非線性耦合的移動(dòng)機(jī)械臂系統(tǒng)分解成兩個(gè)子系統(tǒng)，并分別建立其運(yùn)動(dòng)學(xué)模型，進(jìn)而基于機(jī)械臂的雅可比矩陣求解機(jī)械臂的可操作度性能指標(biāo)，得到機(jī)械臂在干涉條件下的靈活工作空間。根據(jù)機(jī)械臂在靈活工作空間的分布特點(diǎn)得到路徑規(guī)劃方法，最后通過仿真和實(shí)驗(yàn)均證明了移動(dòng)機(jī)械臂路徑規(guī)劃方法的有效行和可靠性。