劉鵬飛，楊孟興，宋 科，段曉妮

（中國航天科技集團 第16研究所，西安 710100）

0 引言

工業(yè)機器人在一些應(yīng)用領(lǐng)域如焊接和噴涂等需要對末端執(zhí)行器運動的軌跡進行嚴格控制，這種控制稱CP控制(Continuous Path Control)[1]，需要在笛卡爾空間內(nèi)進行軌跡插補。用示教或離線編程的方式告訴機器人路徑中的若干點，以及各點之間所走的路徑是直線和圓弧等，控制器根據(jù)插補算法自動生成路徑上的中間點，機器人運行后自動重復(fù)上述的路徑。

軌跡插補不僅是對位置的插補，也是對速度和加速度的插補[2]。不但要求插補點的位置嚴格在規(guī)定路徑上，而且要求末端執(zhí)行器的速度連續(xù)變化，在精度速度要求更高的場合要求加速度也是連續(xù)變化的。

1 笛卡爾空間插補概念

1.1 位姿描述

通常，用姿態(tài)角（回轉(zhuǎn)、俯仰、偏轉(zhuǎn)）[2]、歐拉角、等效旋轉(zhuǎn)矢量和四元素這四種方式描述姿態(tài)，這四種表示方法是等效的，它們和姿態(tài)旋轉(zhuǎn)矩陣之間轉(zhuǎn)換方法參考文獻[2]。本文用位置矢量和姿態(tài)四元數(shù)的組合描述位姿：

為了避免與機器人機械產(chǎn)生共振，插補周期應(yīng)控制1-5ms以內(nèi)[4]。在每個插補周期內(nèi)計算機需完成一次插補運算和一次運動學逆解，計算機的計算速度有限，應(yīng)盡量減少插補運算的計算量。

1.2 笛卡爾空間插補標準算法

笛卡爾空間插補算法的思路較簡單，文獻[3]給出了標準算法并將其應(yīng)用于直線插補。

Dt是插補周期；F(t)是位姿關(guān)于時間的函數(shù)；j(t)是機器人關(guān)節(jié)坐標，由F(t)逆解得到。

計算位姿函數(shù)F(t)和逆解得到j(luò)(t)是每個插補周期內(nèi)的主要工作，插補算法討論如何獲得位姿函數(shù)F(t)。機器人末端執(zhí)行器在時間T內(nèi)從位姿F0運動到位姿F1，其中位置從P0運動到P1，姿態(tài)旋轉(zhuǎn)矩陣從R0運動到R1，選擇歸一化時間算子[1,3,5]l(t)=實現(xiàn)直線均勻插補步驟如下：

1）將姿態(tài)旋轉(zhuǎn)矩陣R0和R1轉(zhuǎn)換為四元數(shù)Q0和Q1表示姿態(tài)；

2）計算位置函數(shù)，P(t)=P0+l(t)×(P1P0)；

3）計算姿態(tài)四元數(shù)函數(shù)，采用兩四元數(shù)的‘球形線性插補法’(spherical linear interpolation)[6]:

式中 ，q=arccos(Q1×Q2)，Q1×Q2是兩個四元數(shù)的點積；

均勻插補生成的位置函數(shù)是有缺陷的：在端點P(0)處，速度由零突變?yōu)榉€(wěn)定值，P(T )處又由突變?yōu)榱悖簿褪窃趦啥它c處加速度出現(xiàn)脈沖。這樣的軌跡顯然是不可取的，一方面，在機器人的啟動和停止時刻會產(chǎn)生巨大的慣性力、向心力和哥氏力，關(guān)節(jié)上的電機無法補償如此大的力，導(dǎo)致電機電流過載；另一方面，機械上會產(chǎn)生震動和噪聲，磨損加劇影響機器人的工作壽命。

本文提出用‘S’型加減速曲線保證末端執(zhí)行器的速度光滑和加速度連續(xù)。

2 ‘S’型加減速曲線

2.1 歸一化時間算子分析

首先對歸一化時間算子l(t)進行簡要分析。l(t)有如下要求：l(0)=0，l(T )=1，t[0,T]則l(t)[0,1]且單調(diào)遞增。它在插補運算中起到了調(diào)整步長的作用：若在時間T內(nèi)從P0運動到P1，等時間插入N-1個中間位置，則步長為：

從上式可以看出l(t)的變化規(guī)律決定插補步長。位置函數(shù)的n階導(dǎo)數(shù)為：

可見l(t)決定了P(t)的變化趨勢。例如上文選擇了一次函數(shù)作為歸一化時間算子，則P(t)也以一次線性規(guī)律變化，導(dǎo)致加速度在直線端點是脈沖。于是考慮尋求更平滑的歸一化時間算子以滿足速度光滑的要求。文獻[4]中提出了用拋物線擬合方法歸一化時間算子，避免了加速度在起始和終止是脈沖的情況，但速度仍然是不平滑的，加速度仍然有突變。文獻[7]用一段一次函數(shù)、兩段一次函數(shù)與正弦函數(shù)的線性組合做歸一化時間算子，解決的速度光滑的要求。本文提出用S型加減速曲線構(gòu)造歸一化時間算子l(t)也能滿足上述要求，與文獻[7]所提方法相比，減少了兩個正弦函數(shù)的計算量。

2.2 S型加減速曲線

S型曲線加減速的稱法是由系統(tǒng)在加減速階段的速度曲線形狀呈S型而來的[8,9]，廣泛應(yīng)用于高精度、高效率的柔性加工系統(tǒng)，如CNC系統(tǒng)。S型曲線加減速控制是指在加減速時，使其加加速度J(Jerk)為常數(shù)，通過對J持續(xù)時間長短的控制來產(chǎn)生平滑變化曲線，以減小對機械的沖擊。整個運動分為七段：加加速段(T1)、勻加速段(T2)、減加速段(T3)、勻速段(T4)、加減速段(T5)、勻減速段(T6)、減減速段(T7)，如圖1所示。

圖1 S型加減速曲線

下面對其各階段解析表達式進行分析，設(shè)：ti(i=0,1,7)為各階段的局部時間坐標，Ti(i=0,1,7)為各階段的運行時間如圖1所示。對每段加加速度積分可以得到加速度：

上式中J為加加速度，其大小為常值； A為最大加速度；D為最大減速度。對每個階段的加速度積分得到速度：

上式中vs為起始速度，vm為勻加速階段的速度，ve為終止時的速度。對每個階段的速度積分得到位移：

式中，si(i=0,1,2,6)為階段i到達的總位移，s0是初始時刻的位移，通常為0。

2.3 S型加減速曲線的初始化及歸一化

在插補程序執(zhí)行前要定義初始位置P0、終止位置P1、初始速度vs、終止速度ve、中間速度vm、最大加速度A、最大減速度D、運行時間T。

應(yīng)用初始值求解各階段的運行時間Ti(i=0,1,7)和加速度J。加速度從0達到最大值和從最大值降到0時間相等則有：

第三個階段（減加速階段T3）結(jié)束時達到中間速度vm，則：

將T1和T3表達式帶入式(2)并結(jié)合上式化簡得：

同理有v7=ve，將T5和T7表達式帶入式(2)并結(jié)合前式化簡得：

運行總時間為T，解得T4：

機器人末端走過的路徑長度為：

根據(jù)位移公式(3)有：

將 L、vs、vm、ve、A、D 和 T1～T7表達式帶入式 (7)可解得加加速度J并化簡得：

將J帶入式(4)～式(7)即可求得T1～T7。

至此，將J和T1～T7帶入式(1)～(3)便得到S型速度曲線的加速度a(t)、速度v(t)和位置s(t)的解析式。

把S型加減速曲線應(yīng)用于笛卡爾空間軌跡插補前還需要進行歸一化處理，使之滿足“ l(0)=0，l(T )=1，[0,T]則l(t)[0,1]且單調(diào)遞增”這三點要求。

定義歸一化時間算子：

其中，s(t)為式(3)中的位置在全局時間坐標下的函數(shù)，L為式(8)或(9)中機器人末端經(jīng)過的距離。

3 空間直線插補和圓弧插補

3.1 空間直線插補

直線插補是已知直線始末兩點的位置和姿態(tài)，求軌跡中間點（插補點）的位置和姿態(tài)[5]。機器人末端執(zhí)行器在時間T內(nèi)從位姿F0運動到位姿F1，其中位置從P0運動到P1，姿態(tài)旋轉(zhuǎn)矩陣從R0運動到R1。采用等時插補[10]，即在插補時間T內(nèi)均勻插入N-1個點將T均勻分為N段，插補周期用式(8)計算直線兩點間距離L并帶入式(11)中的S型加減速曲線構(gòu)造的歸一化時間算子，應(yīng)用1.2節(jié)所述插補算法實現(xiàn)空間直線插補，不在贅述。

3.2 空間圓弧插補

已知空間三點位姿F1、F2、F3，位置分別為P1、P2、P3，姿態(tài)分別為 R1、R2、R3。若 P1、P2、P3這三點不共線，則存在經(jīng)過這三點的圓弧。

圓弧插補的位置插補和姿態(tài)插補需分開討論，首先討論位置插補。根據(jù)文獻[11,12]中算法求得圓心P0的坐標、圓弧半徑r和圓心角q1；在P1、P2、P3所在平面M上建立以P0為原點的新坐標系{O0}如圖2所示，方向為X0軸，平面法線方向為Z0軸方向，右手定則確定Y0軸方向，文獻給出了由坐標系{O0}到基坐標系{O}的齊次變換矩陣T。

圖2 圓弧插補空間坐標關(guān)系示意圖

圓弧位置插補步驟如下：

1）根據(jù)文獻計算圓心P0、半徑r、齊次變換陣T、圓心角q1；

2）以弧長rq1作為式(10)中的位移L，計算歸一化函數(shù)l(t)；

4）將Pt(t)從坐標系{O0}映射到基坐標系{O}內(nèi)：

其中，T是坐標系{O0}到基坐標系{O}的齊次變換矩陣。

下面討論姿態(tài)插補。圓弧插補對姿態(tài)的要求較弱，一般采用線性插值方式，即把末端執(zhí)行器在曲線上的終點和起點的之間的姿態(tài)均勻的分配到每個插補點上[13]。為了避免在端點處仍存在的加速度突變的問題，本文仍用S型加減速曲線構(gòu)造歸一化時間算子l(t)將姿態(tài)平滑插補。

已知F0、F2的姿態(tài)分別為R0、R2，則姿態(tài)插補步驟如下：

1）將姿態(tài)旋轉(zhuǎn)矩陣R0、R2轉(zhuǎn)換為四元數(shù)Q0、Q2描述姿態(tài)；

式中，q=arccos(Q0×Q2)，Q0×Q2是兩個四元數(shù)的點積；

3）將四元數(shù)Q(t)變換為姿態(tài)旋轉(zhuǎn)矩陣R(t)；

4 仿真結(jié)果與分析

4.1 直線插補仿真

直線兩端點位姿為：

初始化參數(shù)如下：起點速度和終點速度都為0，中間速度為160mm/s，最大加速度和最大減速度都為58mm/s2，插補時間12s，插補周期2ms。仿真得到軌跡如圖3所示，第一行為機器人末端在笛卡爾空間內(nèi)經(jīng)過的路徑，第二行為機器人末端的位置坐標隨時間的變化規(guī)律，第三行為機器人末端的速度坐標隨時間的變化規(guī)律。從圖中看出三個速度分量都以S型加減速規(guī)律變化，證明了算法的可行性。這個速度向量的模具有物理含義——沿著示教直線方向速度的大小，該速度也必然呈現(xiàn)‘S’型加減速變化規(guī)律。

圖3 直線插補的軌跡位置及速度

F0和F1兩點的姿態(tài)用四元數(shù)表示分別為：

Q0=(0 i 0j 0k)、Q1=(0.46837-0.85542i-0.21829j-0.3524k)為了更直觀的顯示由Q0到Q1的姿態(tài)插補軌跡，將四元數(shù)轉(zhuǎn)換為方位角（繞X-Y-Z固定角坐標系的旋轉(zhuǎn)角）表示如圖4所示，圖中g(shù)、b、a分別是繞X、Y、Z是轉(zhuǎn)角，從圖中可以看出姿態(tài)的變化也是連續(xù)平滑的。

圖4 直線插補的姿態(tài)

4.2 圓弧插補仿真

同理進行圓弧插補仿真，空間不共線的三點位姿為：

初始化參數(shù)如下：起點速度和終點速度都為0，中間速度為220mm/s，最大加速度和最大減速度都為58mm/s2，插補時間12s，插補周期2ms。

圖5 圓弧插補的軌跡位置及速度

仿真得到軌跡如圖5所示，第一行為機器人末端在笛卡爾空間內(nèi)經(jīng)過的路徑，第二行為機器人末端的位置坐標隨時間的變化規(guī)律，第三行為機器人末端的速度坐標隨時間的變化規(guī)律。速度向量的模是沿著圓弧切線方向速度的大小，其軌跡如圖6所示。從圖中可以看出切線方向速度也是呈‘S’型加減速規(guī)律變化，起始速度和終點速度都為0，中間速度為220mm/s，與初始化的速度值一致，驗證了算法的正確性。圖7為圓弧插補的姿態(tài)曲線，圖中g(shù)、b、a分別是繞X、Y、Z是轉(zhuǎn)角。從圖5和圖7中可以看出位置及姿態(tài)的變化是連續(xù)平滑。

圖6 圓弧切線方向速度

圖7 圓弧插補的姿態(tài)

5 結(jié)束語

本文創(chuàng)新點在于將高速、高精度柔性加工系統(tǒng)中常用的‘S’型加減速曲線應(yīng)用在了機器人笛卡爾空間插補算法中，對位置和姿態(tài)都進行了插補，使機器人的運動連續(xù)平滑，該方法能有效的減少系統(tǒng)在啟動及停止時對電機和機械結(jié)構(gòu)的沖擊，能提高伺服精度，具有一定的實用價值。

不足之處：1）圓弧的姿態(tài)插補只通過了起始姿態(tài)R0和終止姿態(tài)R2，沒有通過中間姿態(tài)R1；2）兩段示教軌跡的平滑銜接需要做進一步深入研究。

[1]蔡自興.機器人學[M].第三版.北京: 清華大學出版社,2009.

[2]John J.Craig.Introduction to Robotics-Mechanics and Control (Third Edition) [M].Upper Saddle River, New Jersey: Pearson Education, Inc., 2005.

[3]R.Taylor.Planning and Execution of Straight Line Manipulator Trajectories [J].Robot Motion, Brady et al.Editors, MIT Press, Cambridge, MA, 1983: 424-436.

[4]日本機器人學會編, 宗光華等譯.機器人技術(shù)手冊[M].北京: 科學出版社, 1996: 499-501.

[5]陳偉華, 張鐵.六自由度噴涂機器人插補算法研究[J].微計算機信息, 2009年第25卷3-2期: 251-252

[6]K.Shoemake.Animating rotation with quaternion curves[J].Process dings of ACM SIGGRAPH, (San Francisco),the Singer Company, Link Flight Simulator Division,1985: 245-254.

[7]TAN Guanzheng, WANG Yuechao.Theoretical and experiment and research on time-optional trajectory Planning and control of industrial robots [J].Chinese Journals of Control & Application.2003.20(2): 1138-1142.

[8]黃艷, 李家雯, 于東, 等.CNC系統(tǒng)S型曲線加減速算法設(shè)計與實現(xiàn)[J].制造技術(shù)與機床, 2005, 3: 55-59.

[9]石川, 趙彤, 葉佩青, 等.數(shù)控系統(tǒng)S曲線加減速規(guī)劃研究[J].中國機械工程, 2007, 18(12): 1421-1425.

[10]劉極峰, 易際明.機器人技術(shù)基礎(chǔ)[M].北京: 高等教育出版社, 2006.

[11]陳國良, 黃心漢, 王敏.機械手圓周運動的軌跡規(guī)劃與實現(xiàn)[J].華中科技大學學報(自然科學版), 33(11): 63-66.

[12]吳劍波.多軸運動控制器的PCI驅(qū)動研究及其在工業(yè)機器人中的應(yīng)用[D].杭州: 浙江大學, 2010.

[13]卓揚娃, 白曉燦, 陳永明.機器人的三種規(guī)則曲線插補算法[J].裝備制造技術(shù), 2009, 11: 27-29.