工業(yè)機(jī)器人笛卡爾空間軌跡規(guī)劃

林 威，江五講

（華中科技大學(xué) 機(jī)械科學(xué)與工程學(xué)院，湖北 武漢 430074）

0 引言

在機(jī)器人笛卡爾空間規(guī)劃系統(tǒng)中，作業(yè)是用末端位姿的齊次變換矩陣序列規(guī)定的，軌跡規(guī)劃就是根據(jù)描述的作業(yè)計(jì)算出運(yùn)動(dòng)的位移、速度和加速度，生成運(yùn)動(dòng)軌跡［1］。工業(yè)機(jī)器人的軌跡規(guī)劃既可以在關(guān)節(jié)空間也可以在笛卡爾空間中進(jìn)行，但是軌跡曲線都必須平滑，以保證機(jī)器人運(yùn)行平穩(wěn)。關(guān)節(jié)空間的軌跡規(guī)劃相對(duì)簡(jiǎn)單，但是其末端的運(yùn)動(dòng)軌跡不直觀；笛卡爾空間的軌跡規(guī)劃不僅概念上直觀，而且規(guī)劃的路徑準(zhǔn)確，可以清晰地觀測(cè)到末端執(zhí)行器的運(yùn)動(dòng)軌跡［2，3］。目前工業(yè)機(jī)器人使用的最廣泛的方法是示教再現(xiàn)，也就是根據(jù)任務(wù)的需要，記錄軌跡上一些關(guān)鍵點(diǎn)，而要想使末端執(zhí)行器嚴(yán)格地按照所要求的軌跡運(yùn)動(dòng)，則必須取得足夠多的點(diǎn)。機(jī)器人的運(yùn)動(dòng)軌跡一般都是圓弧或者直線，復(fù)雜的運(yùn)動(dòng)軌跡一般用直線和圓弧對(duì)其擬合而得到，這是機(jī)器人實(shí)現(xiàn)復(fù)雜運(yùn)動(dòng)軌跡的關(guān)鍵［4，5］。本文提出了利用拋物線過(guò)渡的空間直線插補(bǔ)算法和基于局部坐標(biāo)系的空間圓弧插補(bǔ)算法，保證了實(shí)驗(yàn)室自行設(shè)計(jì)的五自由度噴涂機(jī)器人運(yùn)動(dòng)的平穩(wěn)性。

1 基于拋物線過(guò)渡的空間直線插補(bǔ)

直線的軌跡規(guī)劃是已知直線的起點(diǎn)和終點(diǎn)的位置和姿態(tài)，求解直線軌跡上插補(bǔ)點(diǎn)的位置和姿態(tài)，并保證機(jī)器人位置及姿態(tài)平穩(wěn)過(guò)渡。

假設(shè)機(jī)器人末端由P1點(diǎn)沿直線運(yùn)動(dòng)到P2點(diǎn)，（x1，y1，z1）和（α1，β1，γ1）分別為起點(diǎn)P1的位置和RPY角，（x2，y2，z2）和（α2，β2，γ2）分別為終點(diǎn)P2的位置和 RPY 角，（xi，yi，zi）和（αi，βi，γi）分別為中間插補(bǔ)點(diǎn)Pi的位置和RPY角，如圖1所示。

各插補(bǔ)點(diǎn)的位置和姿態(tài)可由公式（1）求出：

其中：λ為歸一化因子；Δx，Δy，Δz，Δα，Δβ，Δγ為位置和RPY角的增量，其求解如下：

λ歸一化因子采用拋物線過(guò)渡的線性函數(shù)，以保證整段軌跡上的位移和速度都連續(xù)。拋物線過(guò)渡的線性函數(shù)是對(duì)兩點(diǎn)的位姿使用線性插值時(shí)，在兩點(diǎn)的鄰域內(nèi)增加一段拋物線的緩沖區(qū)段。假設(shè)兩端的拋物線運(yùn)動(dòng)時(shí)間相等，即在兩個(gè)過(guò)渡區(qū)域中的加速度值相同、符號(hào)相反。設(shè)λ′為λ對(duì)時(shí)間的導(dǎo)數(shù)，λ′隨時(shí)間t變化的曲線如圖2所示。

圖1 直線運(yùn)動(dòng)示意圖

設(shè)直線段的速度為v，拋物線段的加速度為a，那么拋物線段的運(yùn)動(dòng)時(shí)間Tp和位移Lp分別為：

直線運(yùn)動(dòng)總位移和時(shí)間分別為：

對(duì)拋物線段的位移、時(shí)間、加速度歸一化處理得：

則λ的計(jì)算公式為：

其中：t＝i／N，i＝0，1，2，…，N，N為插補(bǔ)點(diǎn)個(gè)數(shù)，i＝0代表起點(diǎn)，i＝N代表終點(diǎn)。將所求λ代入式（2）即可求得各插補(bǔ)點(diǎn)的位置和姿態(tài)。求得各插補(bǔ)點(diǎn)的位置和姿態(tài)后，再通過(guò)運(yùn)動(dòng)學(xué)反解，可以得到各插補(bǔ)點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度值。

圖2 λ′隨時(shí)間t變化的曲線

2 基于局部坐標(biāo)系的空間圓弧插補(bǔ)

假設(shè)機(jī)器人末端執(zhí)行器從起始位置P1經(jīng)過(guò)中間點(diǎn)P2到達(dá)終點(diǎn)P3，若這3點(diǎn)不共線，則一定存在從P1經(jīng)P2到P3的圓弧。按照以下步驟進(jìn)行空間圓弧的軌跡點(diǎn)位置的插補(bǔ)，如圖3所示。

圖3 坐標(biāo)系與局部坐標(biāo)系

（1）求圓心坐標(biāo)P0（x0，y0，z0）及半徑R。由P1（x1，y1，z1），P2（x2，y2，z2），P3（x3，y3，z3）三點(diǎn)可確定平面，得方程：

根據(jù)圓心到三點(diǎn)的距離都為半徑可列出下列方程：

（2）建立圓弧所在平面的新坐標(biāo)系。取圓心P0為新坐標(biāo)系原點(diǎn)O′，P0P1方向?yàn)閤′軸方向，其單位方向向量為：

P1P2和P2P3決定圓弧所在平面，取垂直于此平面的方向?yàn)閦′軸，其單位方向向量為：

根據(jù)右手法則，y′軸的單位方向向量為：

用齊次變換矩陣TR描述新坐標(biāo)系O′－x′y′z′相對(duì)于原坐標(biāo)系O－xyz的位姿，則：

（3）將P1，P2，P3，P0從原坐標(biāo)系O－xyz變換到新坐標(biāo)系O′－x′y′z′。設(shè)P1，P2，P3，P0在原坐標(biāo)系O－xyz中的坐標(biāo)值分別為（x1，y1，z1），（x2，y2，z2），（x3，y3，z3），（x0，y0，z0），在新坐標(biāo)系O′－x′y′z′中的坐標(biāo)值分別為（x′1，y′1，z′1），（x′2，y′2，z′2），（x′3，y′3，z′3），（x′0，y′0，z′0），則：

由建立的新坐標(biāo)系O′－x′y′z′可知：x′0＝y(tǒng)′0＝z′0＝0，z′1＝z′2＝z′3＝0，x′1＝R。

（4）在新坐標(biāo)系O′－x′y′z′上進(jìn)行圓弧插補(bǔ)。設(shè)圓弧上一點(diǎn)p在O′－x′y′z′中的坐標(biāo)為（x′，y′，z′），對(duì)應(yīng)的歸一化因子為λ，從P1到p的角度為θ，從P1到P3的角度為θ′，則：

（5）將新坐標(biāo)系上的插補(bǔ)點(diǎn)的值轉(zhuǎn)換到原坐標(biāo)系中。設(shè)點(diǎn)p在原坐標(biāo)系O－xyz中的坐標(biāo)值為（x，y，z），對(duì)應(yīng)的p′在新坐標(biāo)系O′－x′y′z′中的坐標(biāo)值為（x′，y′，z′），則：

根據(jù)以上步驟，可以得到圓弧上各插補(bǔ)點(diǎn)的位置。各插補(bǔ)點(diǎn)的姿態(tài)可由下面方程求出：

求得各插補(bǔ)點(diǎn)的位置和姿態(tài)后，再通過(guò)運(yùn)動(dòng)學(xué)反解，可以得到各插補(bǔ)點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度值。

3 實(shí)驗(yàn)分析

以實(shí)驗(yàn)室自行設(shè)計(jì)的五自由度噴涂機(jī)器人為實(shí)驗(yàn)平臺(tái)，驗(yàn)證以上算法。該機(jī)器人結(jié)構(gòu)示意圖如圖4所示。

圖4 噴涂機(jī)器人結(jié)構(gòu)示意圖

對(duì)于直線運(yùn)動(dòng)，給定機(jī)器人末端起點(diǎn)、終點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度；對(duì)于空間圓弧運(yùn)動(dòng)，給定機(jī)器人末端起點(diǎn)、中間點(diǎn)、終點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度，通過(guò)運(yùn)動(dòng)學(xué)正解求出兩點(diǎn)對(duì)應(yīng)的位姿矩陣，然后采用前面提出的軌跡規(guī)劃算法進(jìn)行插補(bǔ)運(yùn)算，得到各插補(bǔ)點(diǎn)的位姿，最后通過(guò)運(yùn)動(dòng)學(xué)反解，求得各插補(bǔ)點(diǎn)對(duì)應(yīng)的各關(guān)節(jié)角度。通過(guò)示教的方式進(jìn)行實(shí)驗(yàn)，由軌跡規(guī)劃得到的結(jié)果畫出各關(guān)節(jié)角度變化圖，設(shè)定運(yùn)動(dòng)為一段空間直線P1P2、一段平面圓弧P2P3P4、一段空間圓弧P4P5P6，如圖5所示，得到的各關(guān)節(jié)角度隨時(shí)間的變化如圖6所示。圖6中，θ1，θ2，θ3，θ4和θ5分別對(duì)應(yīng)圖4中關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4和關(guān)節(jié)5的角度。

根據(jù)以上實(shí)驗(yàn)分析可知，按照示教的軌跡，采用基于拋物線過(guò)渡的空間直線插補(bǔ)算法和基于局部坐標(biāo)系的空間圓弧插補(bǔ)算法，使實(shí)驗(yàn)五自由度噴涂機(jī)器人成功按照要求的軌跡進(jìn)行運(yùn)動(dòng)，且運(yùn)行平穩(wěn)，軌跡銜接平滑。

圖5 設(shè)定的運(yùn)動(dòng)軌跡圖

圖6 各關(guān)節(jié)角度隨時(shí)間變化圖

4 結(jié)束語(yǔ)

空間直線和圓弧的軌跡規(guī)劃算法是最基礎(chǔ)的算法，已經(jīng)廣泛應(yīng)用于工業(yè)機(jī)器人中。以自行設(shè)計(jì)的五自由度噴涂機(jī)器人為實(shí)驗(yàn)平臺(tái)，通過(guò)示教的方式，記錄一系列離散的關(guān)節(jié)角度，再通過(guò)機(jī)器人運(yùn)動(dòng)學(xué)正解，求出這些點(diǎn)對(duì)應(yīng)的末端執(zhí)行器的位姿，在笛卡爾空間中，根據(jù)設(shè)定的運(yùn)動(dòng)進(jìn)行軌跡規(guī)劃，計(jì)算各插補(bǔ)點(diǎn)的位姿，再通過(guò)機(jī)器人運(yùn)動(dòng)學(xué)反解，將這些插補(bǔ)點(diǎn)的位姿反解為關(guān)節(jié)角度，最后由這些關(guān)節(jié)角控制機(jī)器人末端執(zhí)行器沿著規(guī)劃的軌跡運(yùn)動(dòng)。由于采用了基于拋物線過(guò)渡的空間直線插補(bǔ)算法和基于局部坐標(biāo)系的空間圓弧插補(bǔ)算法，保證了機(jī)器人的運(yùn)動(dòng)平穩(wěn)、平滑。

［1］ 熊有倫.機(jī)器人技術(shù)基礎(chǔ)［M］.武漢：華中理工大學(xué)出版社，1996.

［2］ 盧君宜，楊慶華，高峰，等.五自由度農(nóng)業(yè)采摘機(jī)器人軌跡規(guī)劃［J］.機(jī)電工程，2010（12）：1－6.

［3］ 張凱，劉成良，付莊，等.6R機(jī)器人軌跡規(guī)劃及其在焊接中的應(yīng)用［J］.機(jī)械設(shè)計(jì)，2002（10）：20－23.

［4］ 王悅善，陳曾漢.嵌入式數(shù)控系統(tǒng)實(shí)時(shí)插補(bǔ)加減速控制算法研究［J］.煤炭技術(shù)，2012（2）：25－26.

［5］ 王衛(wèi)忠，趙杰，高永生，等.機(jī)器人的平面曲線軌跡規(guī)劃方法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2008（3）：389－392.