，

(上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院,上海200240)

關(guān)節(jié)空間樣條的連續(xù)點(diǎn)位運(yùn)動(dòng)規(guī)劃算法

劉承立，欒楠

(上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院,上海200240)

0 引言

工業(yè)機(jī)器人的運(yùn)動(dòng)過程需要在滿足各種工作條件下給出一個(gè)高效合理的規(guī)劃。傳統(tǒng)上工業(yè)機(jī)器人的運(yùn)動(dòng)規(guī)劃分為PTP運(yùn)動(dòng)(point-to-point)和CP運(yùn)動(dòng)(continuous path)，其中PTP控制最為簡單，沒有插補(bǔ)運(yùn)算，各軸同時(shí)獨(dú)立地走完自己的規(guī)劃即可，其常用于快速定位和搬運(yùn)作業(yè)[1]。

基本的PTP運(yùn)動(dòng)從靜止開始，到達(dá)目標(biāo)點(diǎn)位置時(shí)速度減為零，相應(yīng)加速度也為零。在實(shí)際作業(yè)中，為了避開可能的障礙，或者在不同的階段采用不同的運(yùn)動(dòng)速度，往往需要用若干中間點(diǎn)來對(duì)整個(gè)PTP運(yùn)動(dòng)軌跡進(jìn)行約束，即此時(shí)機(jī)器人需要連續(xù)進(jìn)行多點(diǎn)的PTP運(yùn)動(dòng)規(guī)劃。如果簡單地逐段應(yīng)用基本的PTP規(guī)劃，那么在每一個(gè)中間點(diǎn)都有一個(gè)減速、停頓、加速的過程，這對(duì)于搬運(yùn)等實(shí)際作業(yè)時(shí)是不合理的，不僅大大降低了工作效率，而且還增加了能耗[2]。

一般的工業(yè)機(jī)器人產(chǎn)品都提供了連續(xù)點(diǎn)位運(yùn)動(dòng)規(guī)劃，典型的做法是當(dāng)機(jī)器人以給定的范圍“接近”目標(biāo)點(diǎn)時(shí)，立即以此當(dāng)前位姿作為起點(diǎn)，下一點(diǎn)作為目標(biāo)點(diǎn)，當(dāng)前速度和加速度作為初始條件，進(jìn)行下一段的規(guī)劃。在該做法中，“接近”范圍一般是以總行程的百分比表示，例如一旦完成該段行程的90%，即距離目標(biāo)點(diǎn)不足10%的時(shí)候，立即規(guī)劃并向下一個(gè)目標(biāo)點(diǎn)進(jìn)發(fā)。其優(yōu)點(diǎn)是，計(jì)算簡單，能夠?qū)崿F(xiàn)連續(xù)的點(diǎn)位運(yùn)動(dòng)。缺點(diǎn)是，示教或計(jì)算得到的中間點(diǎn)不會(huì)準(zhǔn)確到達(dá)，只是“接近”該點(diǎn)；整個(gè)軌跡的速度可能仍然不連續(xù)，比如，當(dāng)“接近”的范圍過小時(shí)，其速度連續(xù)的效果不明顯，仍然會(huì)有明顯的減速過程，只是不會(huì)減速至零；而“接近”范圍過大則距離中間目標(biāo)點(diǎn)的誤差較大時(shí)就會(huì)開始下一目標(biāo)點(diǎn)的運(yùn)動(dòng)。

因此，一種簡單高效的PTP運(yùn)動(dòng)規(guī)劃方法對(duì)于提升機(jī)器人的運(yùn)動(dòng)性能是有很大意義的。

1 關(guān)節(jié)空間樣條曲線算法原理

由于機(jī)器人的運(yùn)動(dòng)是多關(guān)節(jié)聯(lián)動(dòng)，即使不需要插補(bǔ)的PTP運(yùn)動(dòng)，至少也要保證各軸準(zhǔn)確到達(dá)目標(biāo)點(diǎn)或者中間點(diǎn)的同時(shí)性。因此,每一個(gè)軸在每一段運(yùn)動(dòng)的速度不僅取決于指令本身，也取決與其他軸的路徑及速度等運(yùn)動(dòng)情況。要針對(duì)所有可能出現(xiàn)的運(yùn)動(dòng)狀況作出一個(gè)判斷，從而給出一個(gè)全局最優(yōu)的規(guī)劃，即保證機(jī)器人運(yùn)動(dòng)軌跡準(zhǔn)確地經(jīng)過所有中間點(diǎn)，各軸運(yùn)動(dòng)速度光滑，加速度連續(xù)，且符合指令的要求。事實(shí)證明，這是非常困難的，難以做到用簡單算法在機(jī)器人控制器中實(shí)現(xiàn)實(shí)時(shí)控制，即便能實(shí)現(xiàn),也不符合PTP算法簡單快速、不需要進(jìn)行插補(bǔ)的初衷[3]。

1.1 傳統(tǒng)三次樣條曲線規(guī)劃

為了保證運(yùn)動(dòng)的平穩(wěn)性，就要保證速度光滑，加速度連續(xù)。因此,機(jī)器人的運(yùn)動(dòng)規(guī)劃至少應(yīng)該是由三次函數(shù)構(gòu)建，為此首先想到采用三次樣條曲線進(jìn)行規(guī)劃[4]。針對(duì)某一個(gè)軸，直接將所有經(jīng)過的m個(gè)點(diǎn),作為樣條點(diǎn)，如圖1所示,按照指令給定的速度計(jì)算每一段運(yùn)動(dòng)所需要的時(shí)間，實(shí)際中每段時(shí)間取所有關(guān)節(jié)軸中所需時(shí)間最大值，從而保證同時(shí)到達(dá)。此時(shí)m-1段樣條曲線共計(jì)4(m-1)個(gè)未定參數(shù)，除起點(diǎn)和終點(diǎn)以外的m-2個(gè)中間點(diǎn)滿足的條件為位置準(zhǔn)確到達(dá)且連續(xù)、速度連續(xù)、加速度連續(xù)，起點(diǎn)和終點(diǎn)處位置準(zhǔn)確到達(dá)，共計(jì)4m-6個(gè)條件，還欠缺的2個(gè)一般令起點(diǎn)和終點(diǎn)速度為零作為邊界條件，就可以構(gòu)成標(biāo)準(zhǔn)的三次樣條函數(shù)[5]，如圖1所示。圖中已對(duì)角位移、角速度、角加速度和時(shí)間量做了歸一化處理，下同。

圖1 傳統(tǒng)三次樣條曲線下角位移、角加速度曲線

圖1中，實(shí)線為以傳統(tǒng)三次樣條曲線規(guī)劃出的軌跡，虛線為理想軌跡，點(diǎn)劃線為傳統(tǒng)三次樣條下的加速度曲線。可以發(fā)現(xiàn)，簡單采用傳統(tǒng)三次樣條曲線進(jìn)行規(guī)劃具有的不足是,由于該方式下只能指定2個(gè)邊界條件，所以只能選擇指定速度，那么起點(diǎn)和終點(diǎn)加速度不為零，有一定的跳變，這會(huì)使得在實(shí)際應(yīng)用中產(chǎn)生震動(dòng)，對(duì)應(yīng)圖1中t=0和t=10時(shí)刻；不能保形，樣條的光滑性會(huì)導(dǎo)致不期望的運(yùn)動(dòng)，例如某關(guān)節(jié)在t=0-2時(shí)間段內(nèi)期望是靜止不動(dòng)的，但是由于樣條對(duì)曲線進(jìn)行光滑會(huì)使其產(chǎn)生如圖1所示的運(yùn)動(dòng)，這是不期望出現(xiàn)的[6]。

1.2 算法基本原理

為了解決上面的缺點(diǎn)，可以采取分段的方法，增加可變參數(shù)來達(dá)到調(diào)節(jié)或約束曲線形狀的目的。例如，在2個(gè)示教點(diǎn)或計(jì)算得到的點(diǎn)之間插入n個(gè)中間點(diǎn)，將一段運(yùn)動(dòng)曲線分為n+1段，這樣就可以進(jìn)一步地滿足更多的邊界條件。以插入2個(gè)中間點(diǎn)(n=2)為例，將某段運(yùn)動(dòng)曲線分為3段，比如，任意2個(gè)示教點(diǎn)之間的運(yùn)動(dòng)，可以指定如下邊界條件：兩端點(diǎn)位置(示教點(diǎn)或計(jì)算得到的點(diǎn))、兩端點(diǎn)速度、兩端點(diǎn)加速度，共計(jì)6個(gè)條件[7]。而一般三次樣條S=a0+a1t+a2t2+a3t3,只有4個(gè)未定參數(shù)，只需要4個(gè)不相關(guān)條件即可。如果按照本文思想，將該段運(yùn)動(dòng)按時(shí)間等分為3段，則

(1)

a10,a11,……,a33為12個(gè)未定參數(shù)。

(2)

聯(lián)立式(1)和式(2)方程組，可導(dǎo)出式(1)中各式的待定參數(shù)為：

(3)

(4)

(5)

2 算例驗(yàn)證

基于關(guān)節(jié)空間樣條的連續(xù)點(diǎn)位運(yùn)動(dòng)算法已在一種四自由度碼垛機(jī)器人中得到驗(yàn)證，能夠快速實(shí)現(xiàn)軌跡規(guī)劃，并且實(shí)現(xiàn)了加速度無跳變，運(yùn)動(dòng)速度光滑且能準(zhǔn)確到達(dá)中間示教點(diǎn)。算法規(guī)劃出的2個(gè)示教點(diǎn)之間做點(diǎn)位運(yùn)動(dòng)的角位移、角速度和角加速度曲線如圖2所示。算法規(guī)劃出的3個(gè)示教點(diǎn)之間做點(diǎn)位運(yùn)動(dòng)的角位移、角速度和角加速度曲線如圖3～圖5所示。其中，圖3對(duì)應(yīng)2端點(diǎn)速度為0，中間點(diǎn)高速的運(yùn)動(dòng)情況；圖4對(duì)應(yīng)2端點(diǎn)為高速，中間點(diǎn)速度為0的運(yùn)動(dòng)情況；圖5對(duì)應(yīng)起始點(diǎn)和中間點(diǎn)高速運(yùn)動(dòng)，末端點(diǎn)降速為0的運(yùn)動(dòng)情況。由圖2～圖5可以看出，在各種工況下，該算法均能實(shí)現(xiàn)示教點(diǎn)或計(jì)算得出點(diǎn)的位置準(zhǔn)確到達(dá)，而且由于該算法可以滿足更多的邊界條件，從而徹底消除了加速度跳變，滿足中間點(diǎn)位置、速度和加速度連續(xù)的要求，實(shí)現(xiàn)了位置、速度的平滑過渡。

圖2 點(diǎn)位運(yùn)動(dòng)時(shí)關(guān)節(jié)坐標(biāo)下φ、ω、a

圖3 連續(xù)點(diǎn)位運(yùn)動(dòng)1

圖5 連續(xù)點(diǎn)位運(yùn)動(dòng)3

3 實(shí)際運(yùn)用

規(guī)劃的流程如圖6所示。在實(shí)際工程應(yīng)用中，為了簡化處理，要求每個(gè)示教點(diǎn)上加速度為零，速度則根據(jù)用戶給定參數(shù)和應(yīng)用特點(diǎn)共同指定。機(jī)器人運(yùn)動(dòng)一般不允許位置超調(diào)，而在該算法的基礎(chǔ)上還需要做出如下處理，如果下一段位置反向，則該段末速度降為零；如果后續(xù)的同向運(yùn)動(dòng)距離過短，速度根據(jù)減速所需距離下降；否則按指令給定的速度運(yùn)動(dòng)，每個(gè)軸的實(shí)際速度需要根據(jù)各自軸位移的比例調(diào)整，以保證運(yùn)動(dòng)的同時(shí)性。

圖6 規(guī)劃過程

4 結(jié)束語

機(jī)器人的運(yùn)動(dòng)規(guī)劃是其運(yùn)動(dòng)控制的核心部分之一，對(duì)于機(jī)器人的運(yùn)動(dòng)過程起著至關(guān)重要的作用。該規(guī)劃方法已在四軸碼垛機(jī)器人上得到應(yīng)用，以其為例，起初應(yīng)用傳統(tǒng)的三次樣條規(guī)劃其進(jìn)行碼垛運(yùn)動(dòng)，過程中常出現(xiàn)小幅震動(dòng)(加速度跳變引起)、超調(diào)、速度受限和中間點(diǎn)不能準(zhǔn)確到達(dá)等問題，后來采用基于關(guān)節(jié)空間樣條進(jìn)行插點(diǎn)分段規(guī)劃，并對(duì)特殊運(yùn)動(dòng)狀態(tài)做出處理，最終實(shí)現(xiàn)了快速、光順的搬運(yùn)動(dòng)作。該規(guī)劃算法具有通用性，并不局限于4軸，也不局限于搬運(yùn)機(jī)器人，可廣泛用于各類機(jī)器人空間內(nèi)的連續(xù)點(diǎn)位運(yùn)動(dòng)，尤其是要求中間示教點(diǎn)準(zhǔn)確到達(dá)的情況。當(dāng)示教點(diǎn)(或者離線計(jì)算點(diǎn))比較密，精度要求不高時(shí)，可以直接作為樣條插補(bǔ)使用。

[1] 曹波,曹其新,童上高,等.基于時(shí)間最優(yōu)的搬運(yùn)機(jī)器人運(yùn)動(dòng)規(guī)劃[J].組合機(jī)床與自動(dòng)化加工技術(shù),2012，7(7):33-38.

[2] 黃時(shí)聰,王剛,郝杰成.機(jī)器人PTP控制曲線的研究與實(shí)現(xiàn)[J].機(jī)械工業(yè)自動(dòng)化,1992,14(3):24-28.

[3] Guo Z,Hsia T.Joint trajectory generation for redundant robots in an environment with obstacles[C]//In IEEE Int.Conf.on Robotics and Automation,1990.157-162.

[4] 李純軍,尹周平,熊濤,等.一種點(diǎn)位運(yùn)動(dòng)快速規(guī)劃算法[J].組合機(jī)床與自動(dòng)化加工技術(shù),2011,3(3):62-64.

[5] Mu H H,Zhou Y F.Third-order trajectory planning for high accuracy point-to-point motion[J].Journal of Huazhong University of Science and Technology,2007(35):58-61.

[6] 秦峰,徐向榮.基于樣條函數(shù)法的機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃[J].機(jī)械科學(xué)與技術(shù),1996,15(4):561-565.

[7] 張紅強(qiáng),章兢,王耀南,等.機(jī)器人關(guān)節(jié)空間B樣條軌跡設(shè)計(jì)的混沌優(yōu)化[J].電機(jī)與控制學(xué)報(bào),2007,11(2):174-177.

[8] 葉留青,楊夢(mèng)龍,陳紹春.一類C1保形三次Spline插值函數(shù)的充要條件及其構(gòu)造方法[J].河南科學(xué),2006,24(3):316-318.

Continuous Point-to-point Motion Planning Algorithm Based on Splines in Joint Space

LIUChengli,LUANNan

(School of Mechanical Engineering,Shanghai Jiaotong University,Shanghai 200240,China)

為了提高工業(yè)機(jī)器人的運(yùn)動(dòng)平穩(wěn)性和快速性，保證機(jī)器人在走曲線時(shí)能夠準(zhǔn)確到達(dá)各示教點(diǎn)并且實(shí)現(xiàn)平滑過渡，提出一種基于關(guān)節(jié)空間樣條的連續(xù)點(diǎn)位運(yùn)動(dòng)規(guī)劃算法。以關(guān)節(jié)空間的單個(gè)軸為例，從一般三階S曲線軌跡出發(fā)，為了保證規(guī)劃得到的加速度不跳變以及樣條曲線能夠保形，在兩個(gè)相鄰示教點(diǎn)之間插入n個(gè)中間點(diǎn)，將一段軌跡分為n+1段，根據(jù)該關(guān)節(jié)起點(diǎn)和終點(diǎn)的位置、速度和加速度以及插入點(diǎn)位置、速度、加速度連續(xù)的邊界條件，可以得到新的規(guī)劃軌跡。最后通過實(shí)驗(yàn)比較驗(yàn)證，該規(guī)劃算法能夠快速實(shí)現(xiàn)過程點(diǎn)的準(zhǔn)確到達(dá)并且加速度沒有跳變、曲線能夠保形，具有實(shí)際推廣價(jià)值。

連續(xù)點(diǎn)位運(yùn)動(dòng)；規(guī)劃算法；關(guān)節(jié)空間樣條

To improve the moving stationarity and rapidity of industrial robots and ensure its reachability to each teaching point and smooth transition,this paper presents a kind of continuous point-to-point motion planning algorithm based on splines in joint space.Take a single axis in joint space as an example,we can firstly analyzed the general third-order S-curve trajectory.In order to ensure the planning acceleration have no jump and the curve is conformal,we insertnintermediate points between two adjacent teaching points and therefore a section of the trajectory is divided inton+1segments.According to these boundary conditions-the position,velocity and acceleration of the start and the end points and their continuity at the inserted points,a new planned trajectory can be obtained. Finally,by the experimental verification, this planning algorithm can make the robot quickly and accurately reach the teaching points without acceleration-jump and the curve conformal,and can be applied to practical situation.

continuous point-to-point motion;planning algorithm;splines in joint space

2014-04-14

TP242.2

A

1001-2257(2014)08-0074-04

劉承立(1990-)，男，山東聊城人，碩士研究生，研究方向?yàn)楣I(yè)機(jī)器人與智能控制;欒楠(1973-)，男，湖北武漢人，副教授，研究方向?yàn)闄C(jī)器人學(xué)、機(jī)器人智能控制。