鄭 濤 ，劉滿祿 ，2

（1.西南科技大學特殊環(huán)境機器人技術四川省重點實驗室，四川 綿陽 621010；2.中國科學技術大學信息科學技術學院，安徽 合肥 230026）

1 引言

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

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

2 機械臂模型

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

圖1 樣機模型Fig.1 Prototype Model

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

3.1 B樣條曲線概述

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

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

B樣條基函數(shù)是一個稱為節(jié)點矢量的非遞減的參數(shù)t的序列所決定的K階分段多項式，其表達式為：

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

3.2 五次B樣條曲線

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

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

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

3.3 五次多項式

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

將實際的角度、角速度、角加速度代入上式（6）中可以得到用于B-5-B算法的過渡五次多項式的通用表達式。

4 ADAMS仿真對比試驗

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

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

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

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

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

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

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

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

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

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

5 結論

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