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

（中國(guó)航天科技集團(tuán) 第16研究所，西安 710100）

0 引言

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

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

1 笛卡爾空間插補(bǔ)概念

1.1 位姿描述

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

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

1.2 笛卡爾空間插補(bǔ)標(biāo)準(zhǔn)算法

笛卡爾空間插補(bǔ)算法的思路較簡(jiǎn)單，文獻(xiàn)[3]給出了標(biāo)準(zhǔn)算法并將其應(yīng)用于直線插補(bǔ)。

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

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

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

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

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

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

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

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

2 ‘S’型加減速曲線

2.1 歸一化時(shí)間算子分析

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

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

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

2.2 S型加減速曲線

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

圖1 S型加減速曲線

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

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

上式中vs為起始速度，vm為勻加速階段的速度，ve為終止時(shí)的速度。對(duì)每個(gè)階段的速度積分得到位移：

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

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

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

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

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

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

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

運(yùn)行總時(shí)間為T，解得T4：

機(jī)器人末端走過(guò)的路徑長(zhǎng)度為：

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

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

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

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

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

定義歸一化時(shí)間算子：

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

3 空間直線插補(bǔ)和圓弧插補(bǔ)

3.1 空間直線插補(bǔ)

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

3.2 空間圓弧插補(bǔ)

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

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

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

圓弧位置插補(bǔ)步驟如下：

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

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

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

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

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

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

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

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

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

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

4.1 直線插補(bǔ)仿真

直線兩端點(diǎn)位姿為：

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

圖3 直線插補(bǔ)的軌跡位置及速度

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

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

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

4.2 圓弧插補(bǔ)仿真

同理進(jìn)行圓弧插補(bǔ)仿真，空間不共線的三點(diǎn)位姿為：

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

圖5 圓弧插補(bǔ)的軌跡位置及速度

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

圖6 圓弧切線方向速度

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

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

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

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

[1]蔡自興.機(jī)器人學(xué)[M].第三版.北京: 清華大學(xué)出版社,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]日本機(jī)器人學(xué)會(huì)編, 宗光華等譯.機(jī)器人技術(shù)手冊(cè)[M].北京: 科學(xué)出版社, 1996: 499-501.

[5]陳偉華, 張鐵.六自由度噴涂機(jī)器人插補(bǔ)算法研究[J].微計(jì)算機(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è)計(jì)與實(shí)現(xiàn)[J].制造技術(shù)與機(jī)床, 2005, 3: 55-59.

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

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

[11]陳國(guó)良, 黃心漢, 王敏.機(jī)械手圓周運(yùn)動(dòng)的軌跡規(guī)劃與實(shí)現(xiàn)[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 33(11): 63-66.

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

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