張華文，劉子亮

（1.河南省工業(yè)機(jī)器人應(yīng)用工程技術(shù)研究中心，南陽 473000；2.河南工業(yè)職業(yè)技術(shù)學(xué)院 自動(dòng)化工程學(xué)院，南陽 473000；3.天地上海采掘裝備科技有限公司，上海 201401）

為了避免機(jī)器人運(yùn)動(dòng)過程中發(fā)生碰撞，往往需要在起始點(diǎn)和終止點(diǎn)之間插入一些中間點(diǎn)來約束機(jī)器人的運(yùn)動(dòng)。 合理的軌跡規(guī)劃可使機(jī)器人運(yùn)行平滑，減少機(jī)械沖擊，同時(shí)讓機(jī)器人在通過這些中間點(diǎn)時(shí)不用停止，達(dá)到最優(yōu)的運(yùn)行時(shí)間，提高工作效率[1]。 文獻(xiàn)[2]構(gòu)造了關(guān)節(jié)空間下經(jīng)過中間點(diǎn)的五次多項(xiàng)式軌跡規(guī)劃函數(shù)，并利用MATLAB 仿真計(jì)算出機(jī)器人運(yùn)動(dòng)過程中各關(guān)節(jié)參數(shù)-時(shí)間曲線， 但運(yùn)行過程中加速度變化并不平滑。 文獻(xiàn)[3]對(duì)五次多項(xiàng)式函數(shù)插值方法中軌跡中間點(diǎn)角速度的設(shè)置方法提出改進(jìn)， 能使關(guān)節(jié)角速度和角加速度更加平滑，但是沒有考慮運(yùn)行時(shí)間最優(yōu)的問題。 文獻(xiàn)[4]以工業(yè)機(jī)器人搬運(yùn)作業(yè)為研究對(duì)象，完成了關(guān)節(jié)空間中的軌跡規(guī)劃實(shí)施過程，并就多項(xiàng)式插值的效果進(jìn)行了對(duì)比，但在中間點(diǎn)各關(guān)節(jié)速度都降為零，降低了運(yùn)行效率。 文獻(xiàn)[5]和文獻(xiàn)[6]基于遺傳算法和高次多項(xiàng)式來實(shí)現(xiàn)時(shí)間最優(yōu)的軌跡規(guī)劃，提高了軌跡跟蹤精度，但是算法較為復(fù)雜，增加系統(tǒng)的運(yùn)算負(fù)擔(dān)。

本文采用基于加速度連續(xù)過渡的線性規(guī)劃方法，將路徑點(diǎn)用直線連接起來，每兩個(gè)路徑點(diǎn)之間的運(yùn)動(dòng)看作是勻速運(yùn)動(dòng)，每個(gè)運(yùn)動(dòng)段之間用加速度連續(xù)變化的運(yùn)動(dòng)進(jìn)行過渡。 該算法只需向前看一個(gè)路徑點(diǎn)就可以將整條運(yùn)動(dòng)軌跡計(jì)算出來， 計(jì)算量小，便于編程實(shí)現(xiàn)。

1 兩點(diǎn)運(yùn)動(dòng)的軌跡規(guī)劃

用qi和qf分別表示起始和終止時(shí)刻關(guān)節(jié)角度向量和分別表示第j 個(gè)關(guān)節(jié)起始和終止時(shí)刻的關(guān)節(jié)角，kv和ka分別表示關(guān)節(jié)最大速度向量和關(guān)節(jié)最大加速度向量，kvj和kaj分別表示第j 個(gè)關(guān)節(jié)的最大速度和最大加速度。 每個(gè)關(guān)節(jié)的最大速度和加速度通?？梢愿鶕?jù)該關(guān)節(jié)的電機(jī)參數(shù)得到。

在qi和qf之間的軌跡規(guī)劃可以用式（1）表示：

式中：D=qf-qi。 規(guī)劃算法的目標(biāo)是求出r（t）的表達(dá)式。

1.1 關(guān)節(jié)同步系數(shù)的計(jì)算

首先對(duì)梯形速度運(yùn)動(dòng)進(jìn)行分析，位移表達(dá)式如式（2）所示，其中τj表示加速段時(shí)間。

對(duì)速度進(jìn)行時(shí)間的積分，得到梯形的面積，即為運(yùn)動(dòng)的距離，如式（3）所示：

求得關(guān)節(jié)j 最小運(yùn)行時(shí)間為

所有關(guān)節(jié)需要在相同的時(shí)間內(nèi)完成各自的運(yùn)動(dòng)，為了達(dá)到同步的效果，每個(gè)關(guān)節(jié)需要根據(jù)運(yùn)動(dòng)距離、關(guān)節(jié)能夠達(dá)到的最大速度和加速度來調(diào)整運(yùn)行時(shí)的最大速度和加速度[7]。 設(shè)定任意一個(gè)關(guān)節(jié)為參考關(guān)節(jié)，然后為每個(gè)關(guān)節(jié)設(shè)定最大速度同步系數(shù)λj和最大加速度同步系數(shù)υj。 關(guān)節(jié)j 的最大同步速度為λjKvj，最大同步加速度為υjKaj。 下文先以兩個(gè)關(guān)節(jié)為例說明同步系數(shù)的求解。

兩個(gè)關(guān)節(jié)的最小運(yùn)行時(shí)間為

同步后的運(yùn)行時(shí)間為

根據(jù)式（6）可得：

最大同步速度不可能超過最大運(yùn)行速度，因此最大速度同步系數(shù)需滿足：

根據(jù)式（7）可得：

與之對(duì)應(yīng)的加速時(shí)間為

推廣到n 個(gè)關(guān)節(jié)可得：

根據(jù)式（7）可以得到其他所有關(guān)節(jié)的同步系數(shù)。

1.2 二次拋物線加速度的軌跡規(guī)劃

與梯形速度運(yùn)動(dòng)相似，二次拋物線加速度運(yùn)動(dòng)也分為加速、勻速和減速3 個(gè)階段。 但是其加速度不是恒定的， 而是關(guān)于時(shí)間的二次拋物線函數(shù)，如圖1 所示。 這樣，保證了加速度、速度和位移的連續(xù)，有效地減少?zèng)_擊，確保機(jī)器人的平滑運(yùn)動(dòng)。

圖1 二次拋物線加速度運(yùn)動(dòng)模型Fig.1 Quadratic parabola acceleration motion model

根據(jù)條件可知，在加速運(yùn)動(dòng)段，關(guān)節(jié)j 的位移、速度和加速度應(yīng)滿足以下5 個(gè)條件：

由這5 個(gè)限制條件，可以推出位移、速度和加速度關(guān)于時(shí)間的表達(dá)式：

式中：1≤j≤n，0≤t<τ′。 加速度在t=τ′/2 處取得最大值：

由式（15）可知，二次拋物線加速度運(yùn)動(dòng)的加速時(shí)間是梯形速度運(yùn)動(dòng)加速時(shí)間的1.5 倍。 在勻速時(shí)間段h′中，位移關(guān)于時(shí)間的表達(dá)式如下：

式中：τ′≤t<τ′+h′。

比如，某學(xué)生屢屢遲到，傳統(tǒng)做法可能是，了解學(xué)生遲到原因，幫助學(xué)生找到避免遲到的方法，同時(shí)輔以必要的懲戒。而設(shè)計(jì)思維的做法是引導(dǎo)學(xué)生對(duì)人生有目標(biāo)，對(duì)學(xué)習(xí)感興趣，對(duì)班級(jí)產(chǎn)生歸屬感，對(duì)交往有渴望等，然后再根據(jù)這些更高層次的目標(biāo)設(shè)計(jì)解決方案。

減速段和加速段是對(duì)稱的，即：tf′=2τ′+h′。

根據(jù)式（13）和式（15）可得加速段運(yùn)動(dòng)的距離為

得到整段的運(yùn)行時(shí)間為

為了能達(dá)到最大的同步速度，運(yùn)行距離必須滿足：

如果該條件不能滿足，必須降低最大運(yùn)行速度，新的最大運(yùn)行速度為

2 多路徑點(diǎn)的軌跡規(guī)劃

為了避免機(jī)器人運(yùn)動(dòng)過程中發(fā)生碰撞，往往需要在起始點(diǎn)和終止點(diǎn)之間插入一些中間點(diǎn)來約束機(jī)器人的運(yùn)動(dòng)。 使用q1，q2，...，qm表示一段軌跡中的所有路徑點(diǎn)。 根據(jù)第1 節(jié)中的方法，假設(shè)過每個(gè)路徑點(diǎn)的速度都為0，可以計(jì)算得到同步系數(shù)和加速段時(shí)間τk′，其中k 代表由qk和qk+1所組成的段。

假設(shè)第k 段之間是勻速運(yùn)動(dòng)，則第k 段的運(yùn)行時(shí)間hk為

為了生成一條在中間路徑點(diǎn)不停止的平滑軌跡，需要在k-1 段和k 段之間進(jìn)行平滑過渡，如圖2所示。 在第k 個(gè)路徑點(diǎn)的過渡段的時(shí)間為2Tk。 為了保證加速度的連續(xù)，需要用一個(gè)二次函數(shù)來表示過渡段加速度的變化。 在過渡段，每個(gè)關(guān)節(jié)都以能夠達(dá)到的最大加速度運(yùn)行，以保證軌跡能夠盡可能地逼近中間點(diǎn)。 過渡時(shí)間的表達(dá)式如下：

圖2 關(guān)節(jié)空間加速度連續(xù)過渡的線性規(guī)劃Fig.2 Interpolations with continuous acceleration blends in joint space

式中：k=2，3，…，m-1；j=1，2，…，n。

所有關(guān)節(jié)只在整個(gè)軌跡的起始點(diǎn)和終止點(diǎn)的過渡是同步的，根據(jù)式（15）可計(jì)算得到起始點(diǎn)和終止點(diǎn)的過渡時(shí)間Tkj=τ′/2。

為了方便表示，用Tk代表Tkj。如圖2 所示，關(guān)節(jié)j 在第k 段中勻速段的位移關(guān)于時(shí)間的表達(dá)如下：

考慮第k 個(gè)點(diǎn)的過渡段，有以下幾個(gè)限制條件：

根據(jù)上述條件，可以得到關(guān)節(jié)j 在第k 段中過渡段的位移關(guān)于時(shí)間的表達(dá)式是一個(gè)四次多項(xiàng)式，表達(dá)式如下：

式中：tk≤t

速度和加速度關(guān)于時(shí)間的表達(dá)式如下：

加速度在t=tk+Tk處取得最大值：

根據(jù)最大加速度kaj，該表達(dá)式可用來計(jì)算Tk。需要注意的是，必須滿足hk≥Tk+Tk+1。如果該條件不滿足，需要降低該段的運(yùn)行速度，重新進(jìn)行規(guī)劃。

3 算法設(shè)計(jì)

過路徑點(diǎn)的關(guān)節(jié)空間的軌跡規(guī)劃算法流程如圖3 所示。

圖3 多路徑點(diǎn)的關(guān)節(jié)空間的軌跡規(guī)劃算法流程Fig.3 Flow chart of trajectory planning with via points in joint space

算法在實(shí)現(xiàn)時(shí)有以下幾個(gè)關(guān)鍵點(diǎn)：

（1）必須保證每個(gè)關(guān)節(jié)運(yùn)動(dòng)的同步。

（2）需要判斷每段的距離是否滿足hk≥Tk+Tk+1，如果不滿足，則重新計(jì)算該段的運(yùn)行速度，重新進(jìn)行規(guī)劃。 可能會(huì)出現(xiàn)不論怎樣調(diào)整速度，都無法完成規(guī)劃的情況，此時(shí)程序便陷入死循環(huán)。 因此，需要設(shè)定循環(huán)次數(shù)上限，如果循環(huán)達(dá)到上限后，仍然無法完成規(guī)劃，則程序給出相應(yīng)提示。

（3）在計(jì)算插補(bǔ)點(diǎn)時(shí)，為了確定時(shí)刻t 的位移，首先要確定t 所處的時(shí)間段。為了提高查找效率，可以使用二分法判斷t 所在的時(shí)間段。

4 實(shí)驗(yàn)結(jié)果與分析

使用MATLAB 進(jìn)行算法仿真驗(yàn)證。分別在時(shí)刻0 s，0.2 s，0.7 s 處設(shè)置3 個(gè)示教點(diǎn)，如表1 所示。

表1 示教點(diǎn)參數(shù)Tab.1 Teaching point parameters

設(shè)定插補(bǔ)周期為0.01 s，可得到各關(guān)節(jié)角度、速度和加速度的變化曲線。 由圖4 可知，各關(guān)節(jié)位移變化平滑連續(xù)且運(yùn)動(dòng)同步，終止點(diǎn)位置精確。由圖5可知，各關(guān)節(jié)以逼近最大速度運(yùn)行，變化連續(xù)且運(yùn)行同步，初始速度和終止速度均為0。 由圖6 可知，各關(guān)節(jié)加速度呈現(xiàn)二次拋物線變化方式，變化連續(xù)且運(yùn)行同步， 最大運(yùn)行加速度逼近關(guān)節(jié)最大加速度，初始加速度和終止加速度都為0。 結(jié)果表明，該算法能夠根據(jù)教點(diǎn)正確規(guī)劃各關(guān)節(jié)的運(yùn)動(dòng)軌跡；確保各關(guān)節(jié)能夠同步運(yùn)行；位移、速度和加速度都能夠平滑連續(xù)的變化，提高運(yùn)行平穩(wěn)性；速度和加速度都能夠逼近關(guān)節(jié)最大值， 有效地優(yōu)化運(yùn)行時(shí)間，提高運(yùn)行效率。

圖4 關(guān)節(jié)角度變化曲線Fig.4 Joint angle curve

圖5 關(guān)節(jié)速度變化曲線Fig.5 Joint velocity curve

圖6 關(guān)節(jié)加速度變化曲線Fig.6 Joint acceleration curve

5 結(jié)語

以六自由度工業(yè)機(jī)器人為研究對(duì)象，分析只有起始和終止兩點(diǎn)的運(yùn)動(dòng)軌跡，為實(shí)現(xiàn)各關(guān)節(jié)運(yùn)動(dòng)同步引入速度和加速度同步系數(shù)2 個(gè)參數(shù)，為保證運(yùn)行平衡采用二次拋物線加速度運(yùn)行方式推算出位移、速度和加速度關(guān)于時(shí)間的表達(dá)式，并給出運(yùn)行限制條件；分析多路徑點(diǎn)的運(yùn)動(dòng)軌跡，把每兩個(gè)路徑點(diǎn)之間看作是勻速運(yùn)動(dòng)，每個(gè)運(yùn)動(dòng)段之間用加速度連續(xù)變化的運(yùn)動(dòng)進(jìn)行過渡；分析算法設(shè)計(jì)的關(guān)鍵點(diǎn)，給出算法流程；使用MATLAB 進(jìn)行算法仿真驗(yàn)證。

該算法保證了多路徑點(diǎn)運(yùn)動(dòng)中各關(guān)節(jié)的加速度、速度和位移的變化平滑連續(xù)，實(shí)現(xiàn)各關(guān)節(jié)同步運(yùn)行和時(shí)間最優(yōu)，確保機(jī)器人能夠平滑高效地運(yùn)行。 通過將軌跡分解成多個(gè)低階多項(xiàng)式的方法有效地降低運(yùn)算復(fù)雜度，減小運(yùn)算量，易于編程實(shí)現(xiàn)。