一種基于運(yùn)動(dòng)學(xué)約束的工業(yè)機(jī)器人軌跡規(guī)劃算法

孫巍偉，王晗，黃民

(北京信息科技大學(xué) 機(jī)電工程學(xué)院，北京 100192)

0 引 言

軌跡規(guī)劃在工業(yè)機(jī)器人領(lǐng)域具有十分重要的意義，如何為多自由度的機(jī)械臂生成柔性而準(zhǔn)確的軌跡，學(xué)者們進(jìn)行了廣泛的研究。在軌跡規(guī)劃過程中，限制機(jī)器人的運(yùn)動(dòng)學(xué)約束可以減少執(zhí)行機(jī)構(gòu)的磨損，并且能夠提高追蹤的速度和精度[1-5]，因此，在進(jìn)行軌跡規(guī)劃時(shí)，往往要考慮機(jī)器人的運(yùn)動(dòng)學(xué)約束，如速度、加速度和加加速度等。此外，由于機(jī)器人控制具有實(shí)時(shí)性的要求，軌跡規(guī)劃算法必須快速，以便機(jī)器人能夠?qū)χ苓叚h(huán)境的變化產(chǎn)生即時(shí)的響應(yīng)。

為實(shí)現(xiàn)機(jī)器人在關(guān)節(jié)坐標(biāo)系或者任務(wù)坐標(biāo)系中的柔性運(yùn)動(dòng)，國(guó)內(nèi)外開展了大量的研究工作。J.Lloyd等[6]通過改變相鄰路徑區(qū)間內(nèi)的傳遞曲線的形狀進(jìn)行優(yōu)化設(shè)計(jì)，實(shí)現(xiàn)了工業(yè)機(jī)器人柔順的軌跡規(guī)劃；LIU S[7]把三次多項(xiàng)式用于表達(dá)單自由度機(jī)器人的運(yùn)動(dòng)軌跡，取得了一定的成效；R.L.Andersson[8]把單獨(dú)的四次多項(xiàng)式函數(shù)用于機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃中，用于表達(dá)整條軌跡；S.Macfarlane等[9]則在其工作基礎(chǔ)上進(jìn)行延伸，成功地把7段五次多項(xiàng)式應(yīng)用于工業(yè)機(jī)器人的軌跡規(guī)劃中；周旋[10]、張平[11]、翁文武[12]將三次B樣條曲線插值算法應(yīng)用于工業(yè)機(jī)器人的軌跡規(guī)劃中。此外，還有很多學(xué)者利用各種方法進(jìn)行研究，如利用S型速度曲線[13]、雙向?qū)?yōu)插補(bǔ)算法[14]、三次非均勻B樣條[15]等。但是，目前針對(duì)工業(yè)機(jī)器人的軌跡規(guī)劃一般是基于高階的多項(xiàng)式插值以及樣條曲線插值等方法，這些方法雖然能夠產(chǎn)生相對(duì)平滑和柔性的運(yùn)動(dòng)軌跡，但其計(jì)算量比較龐大，無法滿足實(shí)時(shí)性的要求，而且在機(jī)器人的運(yùn)動(dòng)學(xué)約束上也存在一些問題。

基于上述分析，本文提出一種多自由度的快速平滑軌跡生成算法，使用三次多項(xiàng)式表達(dá)機(jī)器人的運(yùn)動(dòng)軌跡，兼顧到機(jī)器人的運(yùn)動(dòng)學(xué)約束，包括速度、加速度和加加速度。該算法不涉及任何優(yōu)化，因此，計(jì)算復(fù)雜度低，能夠在機(jī)器人的一個(gè)控制周期(1～2 ms)內(nèi)完成軌跡的計(jì)算和生成，從而滿足機(jī)器人運(yùn)動(dòng)控制的實(shí)時(shí)性要求。

1 軌跡規(guī)劃算法

1.1 問題描述

工業(yè)機(jī)器人是由一系列通過關(guān)節(jié)相連的連桿組成的一個(gè)運(yùn)動(dòng)鏈，其中，每個(gè)空間關(guān)節(jié)可表示為i=1，2，…，n，i為機(jī)器人運(yùn)動(dòng)鏈的序列位置[16]。圖1所示為某7自由度工業(yè)機(jī)械臂及其拓?fù)浣Y(jié)構(gòu)，該機(jī)械臂具有7個(gè)球形關(guān)節(jié)，每個(gè)關(guān)節(jié)都具有不同的運(yùn)動(dòng)速度、加速度和加加速度的最大約束，每個(gè)關(guān)節(jié)的運(yùn)動(dòng)學(xué)約束也會(huì)隨著機(jī)器人所執(zhí)行任務(wù)的不同而有所差異。比如，機(jī)器人在末段執(zhí)行器空載時(shí)或者執(zhí)行較長(zhǎng)的運(yùn)動(dòng)路線時(shí)，為了降低機(jī)器人所受的應(yīng)力，軌跡規(guī)劃時(shí)應(yīng)適當(dāng)限制其加速度和加加速度的值，而在機(jī)器人執(zhí)行夾持或抓取任務(wù)時(shí)，相應(yīng)的關(guān)節(jié)可以具有較大的速度和加速度。另外，由于機(jī)器人具有多個(gè)獨(dú)立的運(yùn)動(dòng)關(guān)節(jié)，如何保證所有自由度機(jī)構(gòu)的運(yùn)動(dòng)同步，對(duì)于提高機(jī)器人運(yùn)動(dòng)精度有著重要的意義。

圖1 7自由度工業(yè)機(jī)器人及其拓?fù)浣Y(jié)構(gòu)Fig.1 7-DOF industrial robot and its topology

本文針對(duì)多自由度的工業(yè)機(jī)器人軌跡規(guī)劃問題，考慮將一條路徑轉(zhuǎn)換為機(jī)器人控制器能夠直接執(zhí)行的軌跡，即時(shí)間的函數(shù)。該路徑由一系列的路徑點(diǎn)P組成。表1所示是在軌跡規(guī)劃中所用到的符號(hào)。

表1 軌跡規(guī)劃中所用到的符號(hào)Tab.1 Symbols used in trajectory planning

因此，軌跡規(guī)劃的目的是根據(jù)每個(gè)路徑點(diǎn)P的機(jī)器人的運(yùn)動(dòng)狀態(tài)要求，生成一條機(jī)器人軌跡,滿足

(3)

其中，?i∈[1，n]。

1.2 單自由度軌跡規(guī)劃

從最簡(jiǎn)單的單自由度軌跡規(guī)劃開始，假定機(jī)器人只通過兩個(gè)路徑點(diǎn)，分別為起始點(diǎn)Pt0和終止點(diǎn)Ptf，機(jī)器人在這兩個(gè)點(diǎn)的速度和加速度都為0，因此，機(jī)器人在這兩個(gè)點(diǎn)的運(yùn)動(dòng)狀態(tài)分別為

(x(t0)，0，0)，

(4)

(x(tf)，0，0)。

(5)

為了達(dá)到運(yùn)動(dòng)時(shí)間最優(yōu)，在運(yùn)動(dòng)過程中，機(jī)器人的速度、加速度和加加速度中的某個(gè)或某幾個(gè)參數(shù)要達(dá)到其最大值，因此，采用7段多項(xiàng)式疊加的形式表示一條單自由度的運(yùn)動(dòng)軌跡，其運(yùn)動(dòng)學(xué)各項(xiàng)參數(shù)如圖2所示。

圖2 單自由度7段式機(jī)器人的運(yùn)動(dòng)軌跡Fig.2 Single-DOF seven-stage robot motion trajectory

由圖2可以清晰看到，該軌跡被分成了7段，每一段都有一個(gè)運(yùn)動(dòng)學(xué)變量達(dá)到了最大值。在T1段機(jī)器人的加加速度達(dá)到了其最大限定值，此時(shí)機(jī)器人處于變加速運(yùn)動(dòng)狀態(tài)，各個(gè)變量表達(dá)為

(9)

機(jī)器人在T2段的運(yùn)動(dòng)學(xué)方程為

(13)

在第2段運(yùn)動(dòng)中，機(jī)器人的加速度達(dá)到最大值，因此，加加速度被設(shè)定為0，以防止加速度超過其最大限制，該時(shí)間段內(nèi)機(jī)器人以最大的加速度執(zhí)行勻加速運(yùn)動(dòng)。

機(jī)器人在T3段的運(yùn)動(dòng)學(xué)方程為

(17)

該時(shí)間段內(nèi)，機(jī)器人處于變加速運(yùn)動(dòng)，加速度值隨時(shí)間減小但依然為正值，因此,機(jī)器人仍然加速直到速度達(dá)到其最大約束值Vmax。然后機(jī)器人運(yùn)動(dòng)達(dá)到T4階段，該區(qū)間內(nèi)機(jī)器人以最大速度執(zhí)行勻速運(yùn)動(dòng)，由圖2可以看出，機(jī)器人的運(yùn)動(dòng)軌跡關(guān)于T4段是對(duì)稱的，T5～T7段內(nèi)，機(jī)器人執(zhí)行減速運(yùn)動(dòng)，從而在終點(diǎn)實(shí)現(xiàn)預(yù)期的運(yùn)動(dòng)狀態(tài)。T4段機(jī)器人的運(yùn)動(dòng)學(xué)方程為

(20)

x(t)=Vmaxt+x(t3)。

(21)

在7段式的軌跡生成算法中，采用Bang-Bang加加速度曲線，保持了機(jī)器人速度和加速度的連續(xù)性，計(jì)算過程未涉及任何優(yōu)化算法卻達(dá)到了條件約束范圍內(nèi)的最優(yōu)時(shí)間，原因是確保了在每個(gè)運(yùn)動(dòng)階段，系統(tǒng)都會(huì)有一個(gè)變量達(dá)到最大值，這就保證了機(jī)器人在最短的時(shí)間內(nèi)達(dá)到最大的加速度和速度。

1.3 多自由度軌跡規(guī)劃

工業(yè)機(jī)器人往往由多個(gè)串聯(lián)或者并聯(lián)的關(guān)節(jié)組合而成，是一個(gè)多自由度的系統(tǒng)。多自由度系統(tǒng)的軌跡規(guī)劃可以認(rèn)為是多個(gè)獨(dú)立單自由度規(guī)劃的疊加，但是實(shí)際的工業(yè)應(yīng)用中，簡(jiǎn)單的疊加可以實(shí)現(xiàn)多自由度系統(tǒng)的運(yùn)動(dòng)，卻無法滿足精度上的要求，因?yàn)橥ㄟ^單自由度算法規(guī)劃出的n條軌跡，雖然每條軌跡都達(dá)到了時(shí)間最優(yōu)，但每條軌跡的執(zhí)行時(shí)間都不相同，這會(huì)導(dǎo)致機(jī)器人的某個(gè)關(guān)節(jié)已經(jīng)停止運(yùn)動(dòng)的時(shí)候，其他關(guān)節(jié)仍在運(yùn)動(dòng)，這樣的運(yùn)動(dòng)形式會(huì)大大影響機(jī)器人在軌跡追蹤時(shí)的精度。因此，在對(duì)多組自由度系統(tǒng)進(jìn)行軌跡規(guī)劃時(shí)，要考慮不同自由度之間的時(shí)間同步性。時(shí)間同步是指機(jī)器人在執(zhí)行運(yùn)動(dòng)時(shí)，所有自由度在同一時(shí)刻完成該運(yùn)動(dòng)，時(shí)間同步可以減少機(jī)器人運(yùn)動(dòng)時(shí)的誤差，從而保證機(jī)器人在軌跡追蹤時(shí)的精度。算法1說明了如何將多自由度的軌跡進(jìn)行時(shí)間同步。

算法1：多自由度軌跡同步

輸入：機(jī)器人的起點(diǎn)運(yùn)動(dòng)狀態(tài)M(ti)和終點(diǎn)運(yùn)動(dòng)狀態(tài)M(tf)，運(yùn)動(dòng)學(xué)約束K

輸出：時(shí)間同步的運(yùn)動(dòng)軌跡

1：fork=1 到ndo

2：計(jì)算第k個(gè)自由度的軌跡，并計(jì)算該自由度上軌跡的執(zhí)行時(shí)間Tk

3：通過比較得到最大執(zhí)行時(shí)間Tmax

4：計(jì)算時(shí)間調(diào)整系數(shù)Ci=Tk/Tmax(Ci≤1)

5：改變每個(gè)自由度上運(yùn)動(dòng)學(xué)約束Ki′=KiCi

6：根據(jù)新運(yùn)動(dòng)學(xué)約束Ki′計(jì)算新的運(yùn)動(dòng)軌跡

7：end for

算法首先按照舊的運(yùn)動(dòng)學(xué)約束計(jì)算每個(gè)自由度上的運(yùn)動(dòng)軌跡與運(yùn)行時(shí)間，然后根據(jù)最大的運(yùn)行時(shí)間對(duì)其他自由度上的運(yùn)動(dòng)進(jìn)行調(diào)整，方法是通過簡(jiǎn)單的線性運(yùn)算減小運(yùn)動(dòng)學(xué)約束，通過這種方法，機(jī)器人能夠在其中一個(gè)自由度上實(shí)現(xiàn)給定的最大速度、加速度和加加速度，而將其余n-1個(gè)自由度運(yùn)動(dòng)時(shí)間延長(zhǎng)至與該自由度相同。

2 仿真驗(yàn)證

為了驗(yàn)證本文提出的算法，分別使用時(shí)間未同步和時(shí)間同步算法生成一條二自由度的軌跡。該軌跡連接3個(gè)路徑點(diǎn)，其位置為P1(0，0)，P2(0.2，0.3)和P3(0.1，0.5)。假定機(jī)器人在這3個(gè)路徑點(diǎn)的運(yùn)動(dòng)速度和加速度都為0，該例中采用的運(yùn)動(dòng)學(xué)約束為

|Jmax|=0.9 m/s3，

|Amax|=0.3 m/s2，

|Vmax|=0.15 m/s。

(22)

圖3和圖4分別是未使用同步算法和使用同步算法生成的運(yùn)動(dòng)軌跡。

圖3 時(shí)間未同步的二自由度運(yùn)動(dòng)軌跡Fig.3 Two-DOF trajectories of time unsynchronized

從圖3可以看出，未同步的軌跡中，x軸的運(yùn)動(dòng)完成的時(shí)間早于y軸上的運(yùn)動(dòng)，但是兩個(gè)軸上的運(yùn)動(dòng)學(xué)參數(shù)都到達(dá)了其最大的約束值。

從圖4可以看出，時(shí)間同步的軌跡則在兩個(gè)自由度上保持了相同的運(yùn)動(dòng)時(shí)間，為了保證兩個(gè)軸的運(yùn)動(dòng)時(shí)間相同，算法對(duì)x軸原本的運(yùn)動(dòng)學(xué)約束進(jìn)行了調(diào)整。計(jì)算效率上，通過100次的軌跡計(jì)算，該二自由度軌跡在Core(TM)2 Quad CPU 2.66 GHz的計(jì)算機(jī)上平均計(jì)算時(shí)間為0.8 ms。因此，該算法可以滿足實(shí)時(shí)性的需求，比如視覺伺服等。

3 試驗(yàn)結(jié)果

本文將提出的軌跡生成算法運(yùn)用到AuBo-i5工業(yè)機(jī)械臂上，該機(jī)械臂含有6個(gè)關(guān)節(jié)自由度，試驗(yàn)設(shè)定機(jī)械臂的起始關(guān)節(jié)位置為(0，0，0，0，0，0)，終點(diǎn)關(guān)節(jié)位置為(120°，50°，20°，60°，40°，90°)，機(jī)械臂的運(yùn)動(dòng)學(xué)約束設(shè)置為(Jmax，Amax，Vmax)=(90 °/s3，30 °/s2，15 °/s)。采用提出的時(shí)間同步算法計(jì)算生成運(yùn)動(dòng)軌跡，然后發(fā)送給機(jī)械臂執(zhí)行，圖5 記錄了機(jī)械臂的實(shí)際運(yùn)動(dòng)軌跡及各項(xiàng)參數(shù)?？梢钥闯?，機(jī)械臂在所有的關(guān)節(jié)上保持了相同的運(yùn)動(dòng)時(shí)間，由于在獨(dú)立的自由度上，關(guān)節(jié)1的運(yùn)動(dòng)距離最大，運(yùn)動(dòng)時(shí)間最長(zhǎng)，因此，只有該關(guān)節(jié)的運(yùn)動(dòng)學(xué)參數(shù)達(dá)到了約束范圍內(nèi)的最大值，而其余各關(guān)節(jié)為了保證能夠達(dá)到相等的運(yùn)動(dòng)時(shí)間，按照比例減小了其運(yùn)動(dòng)學(xué)約束。該6自由度軌跡的計(jì)算時(shí)間僅為0.9 ms，因此，完全可以滿足工業(yè)機(jī)器人運(yùn)動(dòng)控制實(shí)時(shí)性的要求。

圖4 時(shí)間同步的二自由度運(yùn)動(dòng)軌跡Fig.4 Two-DoF trajectories of time synchronization

圖5 機(jī)械臂的試驗(yàn)運(yùn)動(dòng)軌跡Fig.5 Experimental trajectories for the robot manipulator

該試驗(yàn)考慮了工業(yè)機(jī)械臂關(guān)節(jié)空間的軌跡規(guī)劃，本文提出的軌跡生成算法主要應(yīng)用在路徑規(guī)劃算法的下一層，即路徑規(guī)劃給出路徑點(diǎn)后由本文的算法計(jì)算出相應(yīng)的運(yùn)動(dòng)軌跡。工業(yè)機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃包含關(guān)節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃，由于受自由度限制，笛卡爾空間光滑的軌跡可能不一定很合理，因此需要引入合理的逆運(yùn)動(dòng)學(xué)算法將空間路徑點(diǎn)轉(zhuǎn)化為合理的關(guān)節(jié)路徑點(diǎn)，再利用提出的算法進(jìn)行計(jì)算。

4 結(jié) 語

本文提出了一種基于運(yùn)動(dòng)學(xué)約束的工業(yè)機(jī)器人軌跡規(guī)劃算法，該算法使用三次多項(xiàng)式表達(dá)機(jī)器人軌跡，因此可以生成柔性的運(yùn)動(dòng)。由于該方法不涉及任何優(yōu)化算法，軌跡的產(chǎn)生由多項(xiàng)式直接計(jì)算生成，所以該算法所需要的計(jì)算時(shí)間極短，可以在機(jī)器人一個(gè)運(yùn)動(dòng)控制周期內(nèi)生成運(yùn)動(dòng)軌跡，這對(duì)將各種傳感器信號(hào)集成到機(jī)器人的反饋控制中有重要的實(shí)際意義。同時(shí)提出了一種簡(jiǎn)單有效的時(shí)間同步策略以減小機(jī)器人軌跡跟蹤時(shí)的誤差，通過試驗(yàn)驗(yàn)證，該算法符合機(jī)器人的運(yùn)動(dòng)控制實(shí)時(shí)性要求，可以作為機(jī)器人控制邏輯結(jié)構(gòu)中路徑規(guī)劃和底層控制的中間層，軌跡生成算法接收路徑規(guī)劃的結(jié)果，并快速地將其轉(zhuǎn)化為機(jī)器人底層控制器可以直接執(zhí)行的時(shí)間函數(shù)。因此,該算法在視覺伺服或者力覺阻抗控制的應(yīng)用中具備一定的使用價(jià)值，可以在未來開展相應(yīng)的試驗(yàn)驗(yàn)證。