一種機(jī)械臂分段插值軌跡規(guī)劃方法

鄭 濤 ，劉滿祿 ，2

（1.西南科技大學(xué)特殊環(huán)境機(jī)器人技術(shù)四川省重點(diǎn)實(shí)驗(yàn)室，四川 綿陽(yáng) 621010；2.中國(guó)科學(xué)技術(shù)大學(xué)信息科學(xué)技術(shù)學(xué)院，安徽 合肥 230026）

1 引言

軌跡規(guī)劃是工業(yè)機(jī)械臂運(yùn)動(dòng)控制系統(tǒng)的重要組成部分，其平滑性直接關(guān)系到機(jī)械臂末端位姿的精確性。軌跡規(guī)劃算法的研究是提高工業(yè)機(jī)械臂末端的精準(zhǔn)性和柔順性的前提，文獻(xiàn)[1]中提出的三次樣條插值改進(jìn)算法解決了傳統(tǒng)三次樣條插值在起始點(diǎn)和終點(diǎn)的加速度突變的情況，文獻(xiàn)[2]中提出的采用非均勻的B樣條曲線解決了機(jī)器人準(zhǔn)確經(jīng)過(guò)多個(gè)中間插補(bǔ)位姿的難題，文獻(xiàn)[3]中提出根據(jù)加速度約束，修正多項(xiàng)式最高階次數(shù)，并利用修正后的高階多項(xiàng)式進(jìn)行軌跡規(guī)劃改善了3-3-5-3-3多項(xiàng)式的平穩(wěn)性。但是以上實(shí)驗(yàn)仍未解決中間的過(guò)渡點(diǎn)仍存在角加速度突變的問(wèn)題。文獻(xiàn)[4]中比較了三次多項(xiàng)式和五次多項(xiàng)式，證明了五次多項(xiàng)式在軌跡規(guī)劃方面的平滑。文獻(xiàn)[5]中提出采用五次多項(xiàng)式對(duì)于機(jī)械臂進(jìn)行軌跡規(guī)劃，能得到相對(duì)平滑的角位移、角速度、角加速度。但是五次多項(xiàng)式所構(gòu)成的軌跡規(guī)劃曲線與構(gòu)成曲線的每個(gè)型值點(diǎn)都相關(guān)聯(lián)，而B(niǎo)樣條曲線的局部支撐性能有效的對(duì)軌跡規(guī)劃后的曲線進(jìn)行優(yōu)化，只需改變相鄰的幾個(gè)型值點(diǎn)即可。文獻(xiàn)[6]和文獻(xiàn)[7]中分析了3-4-5多項(xiàng)式與五次非均勻曲線的優(yōu)缺點(diǎn)，表明了五次非均勻B樣條曲線在降低系統(tǒng)功耗和抑制機(jī)構(gòu)殘余振動(dòng)方面具有顯著優(yōu)勢(shì)。

針對(duì)于工業(yè)機(jī)器臂軌跡規(guī)劃中，軌跡的平滑問(wèn)題和機(jī)械系統(tǒng)的沖擊問(wèn)題，提出了一種新的軌跡規(guī)劃的插值算法即“B-5-B算法”。并通過(guò)與“3-5-3算法”的對(duì)比仿真實(shí)驗(yàn)，表明了該算法提高了工業(yè)機(jī)械臂在運(yùn)動(dòng)過(guò)程中的角速度與角加速度曲線的平滑性，降低了角加速度突變引起的機(jī)械系統(tǒng)沖擊力。

2 機(jī)械臂模型

在Adams/View中所建立的二連桿仿真模型，如圖1所示。機(jī)械臂的模型主要包括了底座、大臂、小臂。其中大臂的長(zhǎng)度為350mm，小臂的長(zhǎng)度為295mm。在Adams中底座通過(guò)固定副與地面相連接，大臂通過(guò)旋轉(zhuǎn)副與底座相連接，小臂通過(guò)旋轉(zhuǎn)副與大臂相連接，并且定義底座與大臂之間的關(guān)節(jié)為關(guān)節(jié)1，大臂與小臂之間的關(guān)節(jié)為關(guān)節(jié)2，并在相應(yīng)關(guān)節(jié)上建立的坐標(biāo)系，如圖1所示。底座的坐標(biāo)系相對(duì)于全局坐標(biāo)系繞Z軸逆時(shí)針旋轉(zhuǎn)90°，關(guān)節(jié)1的坐標(biāo)系與全局坐標(biāo)系方向相同，關(guān)節(jié)2的坐標(biāo)系相對(duì)于全局坐標(biāo)系繞Z軸逆時(shí)針轉(zhuǎn)動(dòng)45°，在機(jī)械臂末端建立一個(gè)坐標(biāo)系用于繪制運(yùn)動(dòng)軌跡，最后在底座和大臂的旋轉(zhuǎn)副以及大臂和小臂的旋轉(zhuǎn)副上設(shè)置驅(qū)動(dòng)函數(shù)。同時(shí)設(shè)置測(cè)量函數(shù)，通過(guò)AZ、WZ、WDTZ函數(shù)分別設(shè)置底座坐標(biāo)系與關(guān)節(jié)1坐標(biāo)系繞Z軸方向的旋轉(zhuǎn)角度、旋轉(zhuǎn)角速度、旋轉(zhuǎn)角加速度，作為為大臂的角位移、角速度、角加速度。設(shè)置關(guān)節(jié)1坐標(biāo)系與關(guān)節(jié)2坐標(biāo)系繞Z軸的旋轉(zhuǎn)角度、旋轉(zhuǎn)角速度、旋轉(zhuǎn)角加速度，作為為小臂的角位移、角速度、角加速度。由于在關(guān)節(jié)1和關(guān)節(jié)2處添加有驅(qū)動(dòng)函數(shù)，所以不需要再添加測(cè)量關(guān)節(jié)1和關(guān)節(jié)2處的力矩函數(shù)。

圖1 樣機(jī)模型Fig.1 Prototype Model

3 B-5-B軌跡規(guī)劃

3.1 B樣條曲線概述

假設(shè)有一系列的控制頂點(diǎn) P1、P2、P3、…、Pn+1，則 K 階（K-1 次）的B樣條的函數(shù)表達(dá)式為：

式中：θj（t）—第j-1段的角位移；Pi—第i-1個(gè)控制頂點(diǎn)；Ni，k（t）—第i-1個(gè)K階B樣條的基函數(shù)；n+1—控制頂點(diǎn)數(shù)。

B樣條基函數(shù)是一個(gè)稱(chēng)為節(jié)點(diǎn)矢量的非遞減的參數(shù)t的序列所決定的K階分段多項(xiàng)式，其表達(dá)式為：

由式（3）表明，計(jì)算第i個(gè)（K-1）次B樣條基函數(shù)Ni，k（t）時(shí)，需要 t1、t2、t3…t1+k，共 K+1 個(gè)控制頂點(diǎn)。

3.2 五次B樣條曲線

對(duì)于五次均勻B樣條曲線，取K=6得到五次樣條曲線關(guān)于時(shí)間t的函數(shù)式為：

由于在關(guān)節(jié)空間的軌跡規(guī)劃中，已知的是實(shí)際軌跡上的型值點(diǎn)，而非控制頂點(diǎn)，所以需要在給定型值點(diǎn)的情況下求控制頂點(diǎn)，假設(shè)有一系列的型值點(diǎn)θ1、θ2、θ3…θm存在，則需要構(gòu)造m-1條B樣條曲線，需要m+4個(gè)基函數(shù)同時(shí)需要求解m+4個(gè)控制頂點(diǎn)。當(dāng)型值點(diǎn)按實(shí)際軌跡進(jìn)行排列時(shí)，對(duì)于第i條的B樣條曲線，其起點(diǎn)都是第i個(gè)型值點(diǎn)，其終點(diǎn)都是第i+1個(gè)型值點(diǎn)，由此能得到m個(gè)已知方程，所以需要再補(bǔ)充4個(gè)約束條件，在五次B樣條曲線中可以對(duì)起點(diǎn)的型值點(diǎn)和終點(diǎn)的型值點(diǎn)進(jìn)行一階導(dǎo)數(shù)、二階導(dǎo)數(shù)、三階導(dǎo)數(shù)的條件約束，而在這里設(shè)定初始點(diǎn)和結(jié)束點(diǎn)的角速度、角加速度均為0，可以得到：

由方程組（5）能確定唯一的一組控制頂點(diǎn) P1、P2、P3…Pm+3、Pm+4。將求出的控制頂點(diǎn)代入式（4）中就能求出介于兩型值點(diǎn)之間的五次B樣條函數(shù)。

3.3 五次多項(xiàng)式

根據(jù)五次多項(xiàng)式的特點(diǎn)，需要確定6個(gè)約束條件，但是由于五次多項(xiàng)式是位于兩條B樣條曲線之間，為了保證角位移、角速度、角加速度在連接點(diǎn)的平滑和連續(xù)，所以五次多項(xiàng)式起點(diǎn)和終點(diǎn)的角位移、角速度、角加速度由B樣條曲線以及連接點(diǎn)的位置決定。假設(shè)五次多項(xiàng)式運(yùn)動(dòng)的時(shí)間為t0到te，約束條件為：當(dāng)位于時(shí)間t0時(shí)，五次多項(xiàng)式的角度、角速度以及角加速度的值取上一條B樣條曲線的終點(diǎn)對(duì)應(yīng)的值，取角度為θ0、角速度為θ˙0、角加速度為θ¨0，當(dāng)位于時(shí)間te時(shí)，五次多項(xiàng)式的角度、角速度以及角加速度的值取下一條B樣條曲線的起點(diǎn)對(duì)應(yīng)的值，取角度為θe、角速度為θ˙e、角加速度為θ¨e。得到，如式（6）所示。

將實(shí)際的角度、角速度、角加速度代入上式（6）中可以得到用于B-5-B算法的過(guò)渡五次多項(xiàng)式的通用表達(dá)式。

4 ADAMS仿真對(duì)比試驗(yàn)

在Adams中采用二連桿模型進(jìn)行軌跡規(guī)劃，利用SOLIDWORKS建立模型，并給定關(guān)節(jié)空間中的對(duì)應(yīng)時(shí)間和角度序列，如表1所示。

表1 時(shí)間和角度序列表Tab.1 Time and Angle Sequence Table

通過(guò)式（6）和式（16）可以求得五次B樣條曲線的函數(shù)表達(dá)式以及過(guò)渡區(qū)域的五次多項(xiàng)式曲線的函數(shù)表達(dá)式，由于在五次B樣條曲線中自變量t的值為（0，1），而對(duì)應(yīng)的時(shí)間為0s到3s，為了使數(shù)據(jù)對(duì)應(yīng)所以對(duì)數(shù)據(jù)進(jìn)行處理，使得（0，3）能映射到（0，1）中，當(dāng)進(jìn)行了映射處理后，如果對(duì)角度函數(shù)式求導(dǎo)，將會(huì)發(fā)現(xiàn)角速度縮小了3倍，角角加速度縮小了9倍，同樣的對(duì)3s到6s、9s到12s、12s到15s的B樣條曲線和6s到9s的五次多項(xiàng)式曲線進(jìn)行映射處理，將得到的關(guān)于時(shí)間的分段函數(shù)，通過(guò)IF語(yǔ)句分別輸入到Adams中關(guān)節(jié)1和關(guān)節(jié)2中的MOTION驅(qū)動(dòng)函數(shù)里，最后將Adams測(cè)量得到的角速度值擴(kuò)大3倍，角加速度值擴(kuò)大9倍，就能得到正確的數(shù)據(jù)。在二連桿末端設(shè)置一個(gè)MAKER點(diǎn)用于觀察末端軌跡，如圖2所示。從圖中可以看出規(guī)劃的曲線軌跡是平滑、無(wú)突變的，最后通過(guò)Adams/PostProcessor模塊得到關(guān)節(jié)1和關(guān)節(jié)2的角位移、角速度、角加速度的曲線圖，由于圖幅限制，僅提供了關(guān)節(jié)1的B-5-B算法和3-5-3算法的角位移、角速度、角加速度以及力矩圖的對(duì)比，如圖3～圖5所示。從關(guān)節(jié)1的角位移圖中可以看出B-5-B算法和3-5-3算法，角度的起始點(diǎn)和終點(diǎn)都是一樣，但是3-5-3算法中角度的起伏不大，并且一次下降或上升的幅度較大，將會(huì)由于慣性的緣故產(chǎn)生機(jī)械沖擊問(wèn)題，而B(niǎo)-5-B算法曲線的起伏較大，每次下降一定的幅度后，就會(huì)有上升的動(dòng)作，從（6～9）s之間雖然兩種算法都采用了五次多項(xiàng)式，但是明顯的可以看出B-5-B算法中的五次多項(xiàng)式，更為平滑。曲線的變化更加小更平穩(wěn)。

圖2 末端軌跡圖Fig.2 The End Trajectory

圖3 關(guān)節(jié)1的角位移圖Fig.3 The Angleof Joint One

圖4 關(guān)節(jié)1的角速度圖Fig.4 The Angular Velocityof Joint One

從關(guān)節(jié)1的角速度圖中可以看出兩種軌跡規(guī)劃算法，在（0～15）s之間，B-5-B算法一共有8個(gè)峰值，表明了曲線變化得較快，3-5-3算法有5個(gè)峰值，變化較慢，但是可以明顯的看出3-5-3算法的角速度曲線，在其中兩個(gè)峰值處變化很快，容易導(dǎo)致角加速度突變產(chǎn)生機(jī)械沖擊問(wèn)題，而B(niǎo)-5-B算法雖然有8個(gè)峰值但是每個(gè)峰值處變化不大，不存在機(jī)械沖擊的問(wèn)題。

圖5 關(guān)節(jié)1的角加速度圖Fig.5 The Angular Acceleration of Joint One

從關(guān)節(jié)1的角加速度圖可以看出，B-5-B算法的曲線起點(diǎn)是從0開(kāi)始的，中間段拐角處沒(méi)有突變，終點(diǎn)也是在0處結(jié)束的，整個(gè)曲線是平滑連續(xù)的，但是3-5-3算法的曲線起點(diǎn)是從-100開(kāi)始的，表明了開(kāi)始會(huì)有加速度的突變，并且中間段拐角處存在兩個(gè)尖角，表明加速度變化很快，終點(diǎn)可以很明顯看出3-5-3算法的曲線距角加速度為0，還存在一定的距離，所以終點(diǎn)也會(huì)產(chǎn)生角加速度的突變，以上三點(diǎn)都表明了3-5-3算法存在機(jī)械沖擊。從關(guān)節(jié)1的力矩圖，如圖6所示。從圖6中可以看出兩種軌跡規(guī)劃算法的曲線起點(diǎn)和終點(diǎn)都是一樣的，起點(diǎn)的力不為0，是因?yàn)樵贏dams進(jìn)行仿真實(shí)驗(yàn)中考慮了重力的緣故。從圖中能夠看出B-5-B算法是呈階梯狀下降，相對(duì)于3-5-3算法的曲線不易產(chǎn)生力矩沖擊，并且在后半段B-5-B算法的曲線的起伏明顯比3-5-3算法的曲線的起伏小，表明B-5-B算法比3-5-3算法在運(yùn)動(dòng)規(guī)劃方面，更加的平穩(wěn)，不易產(chǎn)生沖擊。

圖6 關(guān)節(jié)1的力矩圖Fig.6 The Momentof Joint One

5 結(jié)論

基于高次B樣條曲線和高次多項(xiàng)式曲線，提出一種“B-5-B”軌跡規(guī)劃算法。通過(guò)Adams的對(duì)比仿真實(shí)驗(yàn)驗(yàn)證了該算法在機(jī)械臂運(yùn)動(dòng)過(guò)程中的角位移圖、角速度圖、角加速度圖、力矩圖均光滑且連續(xù)，有效的避免了機(jī)械沖擊，降低了虛擬樣機(jī)所受的機(jī)械系統(tǒng)沖擊力，由于采用五次B樣條曲線所以在起始位置和終點(diǎn)位置能保證角速度和角加速度為零，也解決了“3-5-3算法”在起點(diǎn)和終點(diǎn)的角加速度存在突變的問(wèn)題，在中間過(guò)渡曲線雖然和“3-5-3算法”都采用五次多項(xiàng)式，但是明顯的“B-5-B算法”的曲線更為平穩(wěn)。