王 成，王朝立，趙憶文

（1上海理工大學 光電信息與計算機工程學院，上海 200093；2中國科學院 沈陽自動化研究所，沈陽 110016）

0 引 言

目前，國內(nèi)外研發(fā)出許多不同結構的乒乓球發(fā)球機械手，通過分析發(fā)現(xiàn)，絕大多數(shù)的發(fā)球機械手存在發(fā)球不精準、發(fā)球穩(wěn)定性差、發(fā)球速度慢的缺點，使得其無法完美代替真人發(fā)球來幫助乒乓球愛好者開展訓練。為了解決這些問題，需要采用更有效的軌跡規(guī)劃方法對機械手的運動軌跡進行規(guī)劃，使得機械手運動時位置、速度、加速度連續(xù)，進而讓乒乓球發(fā)球機械手能夠快速、平滑且準確地發(fā)球；否則，不充分平滑的軌跡規(guī)劃容易導致機械系統(tǒng)產(chǎn)生諧振，這不僅會讓軌跡誤差大幅度增加，也會縮短機械手驅(qū)動部件的使用壽命。

針對這一問題，Piazza等學者提出基于三次樣條插值的軌跡規(guī)劃方法，這是屬于位置、速度、加速度連續(xù)且階次最低的多項式軌跡方法，雖然計算簡單，但是規(guī)劃出的速度曲線波動太大，并且對多個插值點進行軌跡規(guī)劃時，機械手關節(jié)的加速度容易出現(xiàn)突變現(xiàn)象；朱世強等學者采用七次B樣條曲線插值法規(guī)劃機械手軌跡以及孫亮等學者引入五次多項式插值法對機械手進行軌跡規(guī)劃，雖然這些方法可以避免機械手振動，但是卻都提高了規(guī)劃函數(shù)的階次或者復雜度，增加軌跡規(guī)劃時計算量。為解決這一問題，李白雅等學者根據(jù)分段多項式計算量小、易于理解且插值出的軌跡更加平滑等優(yōu)點，提出了4-3-4多項式插值法來規(guī)劃軌跡，不僅防止了速度和加速度突變，也讓機械手的運動軌跡更加連續(xù)、平滑、精確。雖然該方法很好地保證機械手運動平滑度、準確度，但是仍不能解決機械手工作效率低這一關鍵性問題。王學琨等學者將差分進化（DE）算法和3-5-3多項式插值法相結合，用于優(yōu)化機械手軌跡運行時間，但因?qū)C械手各個關節(jié)的約束條件考慮得不全面，過于優(yōu)化了機械手軌跡運行時間。曾關平采用三次B樣條方法對七自由度乒乓球機械手進行軌跡規(guī)劃，同時在充分考慮到運動學約束下，利用改進的遺傳算法優(yōu)化出了理想的機械手運動軌跡，雖然提高了乒乓球機械手的運動效率，也保證了其運動軌跡的平滑性，但改進的遺傳算法需進行浮點編碼、解碼操作，導致優(yōu)化過程復雜且優(yōu)化時間過長，所以在求解復雜的約束問題上有一定的困難。而文獻［8-10］中提到粒子群算法具有結構簡單、參數(shù)設置少的優(yōu)點，相比其他的解決約束優(yōu)化問題的方法具有一定的優(yōu)勢。

針對以上軌跡規(guī)劃方法的優(yōu)缺點，并在考慮機械手各個關節(jié)運動約束的同時，本文提出基于改進粒子群算法的4-3-4多項式插值法對乒乓球發(fā)球機械手進行時間最優(yōu)軌跡規(guī)劃。該方法利用改進的粒子群算法不僅解決了傳統(tǒng)多項式插值軌跡規(guī)劃方法效率低的問題，還保證了在時間達到最優(yōu)的同時使得機械手運動軌跡更加連續(xù)、平滑、準確，并且通過在優(yōu)化目標空間直接搜索最值，省略了構造粒子群算法計算自變量和因變量映射的步驟，減小粒子群尋優(yōu)的復雜性和困難性。最后，通過Matlab的仿真，證明了該方法的可行性和有效性。

1 五自由度乒乓球發(fā)球機械手的運動學分析

1.1 正運動學

本文所研究的乒乓球發(fā)球機械手是由5個旋轉關節(jié)串聯(lián)連接的模塊化機械臂，如圖1所示。根據(jù)此特點，使用D-H表示法確定各個旋轉關節(jié)坐標系，如圖2所示。D-H參數(shù)見表1。

圖1 五自由度乒乓球發(fā)球機械手Fig.1 5-DOF table tennis serving manipulator

圖2 五自由度乒乓球發(fā)球機械手D-H坐標系Fig.2 D-H coordinate system of 5-DOF table tennis serving manipulator

在表1中，a表示從z軸到z軸的距離，α表示從z軸到z軸的扭轉角，d表示從x軸到x軸的距離，θ表示從x軸到x軸的轉角。

表1 五自由度乒乓球發(fā)球機械手的D-H參數(shù)表Tab.1 D-H parameter table of 5-DOF table tennis serving manipulator

其中，s＝sinθ，c＝cosθ，sα＝sinα，cα＝cosα。

將表1中的D-H參數(shù)代入公式（1）中，可以推導出5個關節(jié)之間的變換矩陣：

由式（2）中變換矩陣的連乘，可得機械手末端執(zhí)行器中心點的位姿相對于基坐標系的變換矩陣為：

1.2 逆運動學求解

針對本文所研究五自由度乒乓球機械手，當機械手末端執(zhí)行器中心點位置與姿態(tài)已知時，即已知，根據(jù)式（4）求解關節(jié)角度θ：

利用矩陣變換與反變換方法可以得出：

其中，N＝c a+s a，N＝s a-c a，M＝c o+s o，L＝c n+s n。

通過上面求出的逆運動學解，可以發(fā)現(xiàn)解并不是唯一的。所以，本文將根據(jù)規(guī)劃任務以及機械手關節(jié)約束來選擇合適的機械手運動姿態(tài)。

本文為了充分模擬機械手完整的發(fā)球過程，在初始位置和終止位置之間選擇了2個路徑點、來完成進行發(fā)球時的準備過程、抬拍過程、引拍過程，從而使得發(fā)球過程更加順暢。4個路徑點信息見表2。

表2 4個路徑點的位置與姿態(tài)信息Tab.2 Position and attitude information of four waypoints

根據(jù)表2中4個點在笛卡爾空間的信息，利用推導的逆運動學解析法將其轉換到關節(jié)空間中每個關節(jié)所對應的角度θ（表示路徑點的序號，0，1，2，3），并選取出合適的關節(jié)角度，見表3。

表3 4個路徑點對應的各個關節(jié)角度 Tab.3 Joint angles corresponding to the four path points（°）

在已知機械手在4個路徑點的各個關節(jié)角度后，下面將根據(jù)起始、終止條件利用多項式插值法規(guī)劃出每個關節(jié)理想曲線。

2 分段多項式插值軌跡規(guī)劃方法

根據(jù)分段多項式計算量小、插值出的軌跡在速度和加速度上無突變的優(yōu)點，同時為完成使得發(fā)球更加流暢而進行的準備過程、抬拍過程、引拍過程，本文利用4-3-4多項式插值法來規(guī)劃發(fā)球機械手的運動軌跡。

4-3-4多項式的通式如下：

其中，L（t）、L（t）、L（t）是關節(jié)（1，2，3，4，5）在第一、二、三段軌跡的表達式，a是第個關節(jié)軌跡在第（1，2，3）段插值多項式中的第個未知系數(shù)，t、t、t分別表示關節(jié)在第一、二、三段軌跡的插值時間。

為了使得軌跡更加連續(xù)、平滑，發(fā)球機械手的運動位置、速度和加速度約束條件為：

（1）起始點的速度和加速度為0。

（2）終止點的速度為，加速度為0。

（3）各個路徑點之間的位置、速度、加速度連續(xù)。即：

根據(jù)以上條件，可以推導出多項式未知系數(shù)a和4個路徑點之間的關系式，由此推得式（12）～（14）。

當插值時間t、t、t都不為0時，式（12）中的矩陣是可逆的。所以當插值時間已知時，通過式（12）～（14）可以求出各段多項式軌跡函數(shù)，即可知乒乓球發(fā)球機械手各個關節(jié)的運動曲線。式（12）～（14）的數(shù)學表達式可寫為：

3 改進粒子群算法

3.1 粒子群算法基本原理

粒子群算法（PSO）的主要思想就是把待求解問題（目標函數(shù)）的解看作搜索空間中的粒子，每個粒子都具有位置（x）和速度（v）兩個屬性，位置屬性決定粒子所處的位置，速度屬性決定粒子移動的方向和距離。在解空間里初始化一組隨機解（粒子），并且每個粒子都有一個由目標函數(shù)所決定的適應度值，根據(jù)式（15）和式（16）不斷迭代更新粒子，通過粒子適應度值的變化來搜索目標函數(shù)的最優(yōu)值。具體數(shù)學公式如下：

3.2 粒子群算法的改進

在PSO中的大小主要是用來調(diào)節(jié)當前粒子的速度，較大時，則算法全局搜索的能力較強；較小時，則局部收斂能力較強。、的大小分別決定了粒子本身的認知能力和粒子相互之間的信息共享能力，較大時，粒子自我認知能力強；較大時，粒子全局信息共享能力強。但標準PSO中慣性權重和學習因子都是固定值，其缺乏有效的機制調(diào)整算法前、后期、、的大小來平衡算法前、后期搜索范圍的著重程度，從而導致算法存在收斂速度慢、不易獲得全局最優(yōu)的缺點。

線性PSO通過調(diào)整慣性權重線性變換來改善傳統(tǒng)PSO的缺點。線性慣性權重曲線路如圖3所示。從圖3可以看出，線性變化的在算法初期時，較大的時間短，易于導致算法前期全局搜索能力弱；由于變化幅度不變，導致算法后期不能迅速變小，使得算法后期局部搜索能力弱，這樣一來算法也仍然存在前后期搜索范圍不平衡，從而影響算法收斂速度和優(yōu)化能力。并且線性PSO沒有考慮學習因子對算法尋優(yōu)能力的影響。

圖3 線性慣性權重曲線Fig.3 Linear inertia weight curve

所以，本文將利用非線性函數(shù)對慣性權重和學習因子進行改進，并通過3個參數(shù)之間相互配合來進一步提高算法收斂速度以及優(yōu)化能力。

為調(diào)整PSO算法前、后期全局和局部搜索能力的著重程度，本文將通過指數(shù)函數(shù)來調(diào)節(jié)隨迭代次數(shù)的增加而非線性地減小，可見式（17）：

其中，和分別表示慣性權重的最大值和最小值；表示最大迭代次數(shù)；表示當前迭代次數(shù)。

為了提高算法的收斂速度與優(yōu)化能力，即：使得算法前期注重粒子自我認知能力，算法后期注重粒子全局信息共享能力。本文采用余弦、正弦函數(shù)分別對、進行異步非線性調(diào)整，可見（18）：

其中，、、、分別表示學習因子、的最大值和最小值。

圖4、圖5分別是慣性權重、學習因子曲線對比圖。從對比圖中可以看出：利用非線性函數(shù)對、、改進后，在算法初期時，較大、較大、較小的時間更長，會使得算法全局搜索能力增強。并隨著迭代次數(shù)的變化，對、、的變化幅度也不一樣，在算法后期時，、能迅速變小、能迅速變大，使得算法局部搜索能力增強，從而讓算法的全局和局部搜索能力更加平衡，進而算法的收斂速度得到了提高，也更易于獲得全局最優(yōu)值。

圖4 慣性權重對比曲線Fig.4 Comparison curve of inertia weight

圖5 學習因子對比曲線Fig.5 Comparison curve of learning factors

4 基于改進的粒子群算法優(yōu)化軌跡規(guī)劃問題

4.1 優(yōu)化目標

在乒乓球運動中，快速、連續(xù)、準確的發(fā)球會更容易讓對手失分。所以當使用乒乓球發(fā)球機械手來模擬真人進行發(fā)球時，在保證機械手的運動連續(xù)、平滑、準確的同時，需要縮短發(fā)球機械手的運動時間。相比通過對多項式的系數(shù)進行優(yōu)化來優(yōu)化時間，本文將直接根據(jù)約束條件對每個關節(jié)軌跡的3段運行時間進行優(yōu)化，可以大大降低優(yōu)化變量的維度和算法的復雜度，從而使得算法能夠更加快速、準確地尋找到最優(yōu)值。第個關節(jié)的時間優(yōu)化目標函數(shù)為：

為保證乒乓球發(fā)球機械手運動連續(xù)以及安全使用，需要對發(fā)球機械手每個關節(jié)的位置、速度、加速度進行約束。約束條件為：

（1）已知各個關節(jié)在4個路徑點處的關節(jié)角度（θ），參見表3。

（2）各個關節(jié)的速度、加速度邊界條件，詳見式（11）。

（3）各個關節(jié)在3段軌跡中的位置、速度最大限制，詳見式（20）、式（21）。

4.2 改進的粒子群算法優(yōu)化4-3-4多項式軌跡規(guī)劃

采用改進PSO來優(yōu)化乒乓球發(fā)球機械手關節(jié)的運行時間的分步過程如下：

在關節(jié)的3段插值時間的搜索空間中為種群P、P、P分別隨機生成個粒子，然后初始化3個粒子的位置和速度。

將組插值時間變量代入式（12）（14）中計算多項式的未知系數(shù)a。

將步驟2中求解出的多項式系數(shù)a代入式（10）中，根據(jù)時間計算關節(jié)的實時位置；然后對式（10）求導，計算關節(jié)的實時速度。

計算每個粒子的適應度值。在步驟3中，若在3段軌跡中，關節(jié)的任意一段位置、速度大于位置、速度約束值，則將此粒子的適應度值設定為極大常數(shù)B（以便通過適應值的比較，使得此粒子將不會被選為最優(yōu)粒子）；反之，則根據(jù)目標函數(shù)計算粒子的適應度值。

將步驟4計算出的每個粒子的適應度值進行比較，選擇出每個粒子自身所經(jīng)歷過的最好位置（個體最優(yōu)值）以及整個種群粒子所經(jīng)歷過的最好位置（群體最優(yōu)值）。

根據(jù)式（17）、（18）更新每個粒子的位置和速度，生成新的種群。

根據(jù)終止條件判斷算法是否結束。滿足終止條件，則輸出關節(jié)的運行最優(yōu)時間；反之，則返回到步驟2。

關節(jié)的最優(yōu)運行時間優(yōu)化流程可見圖6。

圖6 第i個關節(jié)進行最優(yōu)時間軌跡規(guī)劃程序流程圖Fig.6 Flow chart of optimal time trajectory planning for the i-th joint

5 實驗與仿真結果分析

5.1 乒乓球發(fā)球機械手模型的建立

根據(jù)表1中乒乓球發(fā)球機械手的D-H參數(shù)，其中92 cm，28 cm，48 cm，在Matlab中利用Robotics Toolbox工具箱中的函數(shù)和函數(shù)構建發(fā)球機械手的三維模型，如圖7所示。

圖7 五自由度乒乓球發(fā)球機械手三維模型Fig.7 Three dimensional model of 5-DOF table tennis serving manipulator

5.2 乒乓球發(fā)球機械手工作空間仿真

考慮到乒乓球發(fā)球機械手在實際工作中進行軌跡規(guī)劃時，會存在奇異點（不可到達的區(qū)域），導致機械手無法運動。本文基于蒙特卡羅法理論基礎上，在Matlab環(huán)境下，使用（，1）隨機函數(shù)對機械手5個關節(jié)的角度隨機采樣進行工作空間仿真分析。合理選取30 000，發(fā)球機械手的工作空間仿真結果如圖8所示。

觀察圖8可知，發(fā)球機械手的工作空間存在少許空穴、空洞現(xiàn)象，因此在軌跡規(guī)劃仿真時應避免這些奇異點，以防后續(xù)無法進行實際發(fā)球操作。

圖8 五自由度乒乓球發(fā)球機械手的工作空間Fig.8 Workspace of a 5-DOF table tennis serving manipulator

5.3 笛卡爾空間速度與關節(jié)空間速度的線性映射

根據(jù)表3中終止點位置（θ），在Matlab中利用Robotics Toolbox工具箱中函數(shù)和函數(shù)求解出五自由度發(fā)球機械手的雅可比矩陣以及逆雅克比矩陣，然后根據(jù)式（22）求解出關節(jié)空間速度，于是可得：

根據(jù)發(fā)球任務（發(fā)球機械手在終止點處的笛卡爾空間速度為［100 200 200 0 0 0］，即機械手末端線速度為300 cm／s，角速度為0，利用式（22）求解出5個關節(jié)在終止點時的關節(jié)空間速度為：

5.4 軌跡規(guī)劃仿真對比實驗

通過上文仿真出的機械手工作空間、表3中各個關節(jié)的位置以及及終止點的速度，在3段軌跡的插值時間都為2 s時，單獨使用4-3-4多項式插值法規(guī)劃各個關節(jié)的位置、速度和加速度曲線。參見圖9～圖11。

圖9 優(yōu)化前5個關節(jié)的角位置變化曲線Fig.9 The angular position curves of the first five joints

圖10 優(yōu)化前5個關節(jié)的角速度變化曲線Fig.10 The angular velocity curves of the first five joints

圖11 優(yōu)化前5個關節(jié)的角加速度變化曲線Fig.11 The angular acceleration curves of the first five joints

從圖9～圖11中可以看出，單獨使用4-3-4多項式插值法規(guī)劃機械手軌跡時，雖然機械手各個關節(jié)的位置、速度和加速度曲線是連續(xù)的，且全程都無突變，但是各個關節(jié)的運行速度都沒趨向關節(jié)最大限制速度（80°／s），加速度也較小?？梢娫摲椒ú]有充分利用機械手的性能，從而導致機械手運動效率低，不能夠快速地完成發(fā)球動作。

為驗證基于改進PSO的4-3-4軌跡規(guī)劃法可行性與有效性，本文以求解關節(jié)1的軌跡最佳運行時間為例，分別使用基于標準PSO（SPSO）、線性PSO（PSO-I）、遺傳算法（GA）、差分進化算法（DE）、改進PSO（NPSO）的4-3-4軌跡規(guī)劃法對其進行優(yōu)化，可見圖12。表4為5種算法優(yōu)化效率對比結果。粒子群算法的參數(shù)設置為：種群數(shù)20，粒子取值范圍為［0，2］，、分別為0.9、0.4，學習因子、的變化范圍為［0.02，0.05］，最大迭代次數(shù)為100。

圖12 關節(jié)1的運行時間迭代收斂對比圖Fig.12 Comparison diagram of running time iteration convergence of joint 1

表4 5種算法求解關節(jié)1最優(yōu)時間的效率對比結果Tab.4 The efficiency comparison results of five algorithms to solve the optimal time of joint 1 s

從圖12中可以看出，SPSO優(yōu)化效果最差，但收斂速度比DE較快。DE、SPSO、GA、PSO-I分別在第85代、第78代、第66代和第56代收斂，而NPSO在第21代收斂。并且NPSO將關節(jié)1運行時間最終收斂到1.948 5 s，相比其他4種算法，得出了最短的運行時間。從表4中可以看出NPSO處理軌跡優(yōu)化的時間為3.886 s，可知NPSO的計算效率更加快速。因此，本文提出的NPSO相比其他4種算法具有更好的收斂速度和尋找最優(yōu)值的能力。換言之，即：基于改進PSO的4-3-4軌跡規(guī)劃法可以更好地在滿足運動約束下（最大化機械手性能）對機械手軌跡的運行時間進行優(yōu)化。

在關節(jié)速度限制為［-80，80］（°／s）的情況下，根據(jù)4.2小節(jié)中的步驟1～步驟7，利用基于改進PSO算法的4-3-4軌跡規(guī)劃法獲得5個關節(jié)在各段軌跡的最優(yōu)運行時間，見表5。

表5 各個關節(jié)在速度［-80，80］（°／s）的最優(yōu)運行時間Tab.5 Optimal running time of each joint while the velocity is between［-80，80］（°／s） s

由于機器人的關節(jié)運動具有時間同步性，因此需選擇5個關節(jié)在各段軌跡的運行時間的最大值，即：

可以得出3段軌跡運行的最優(yōu)時間分別為0.683 1 s，0.618 0 s，0.695 6 s，比未優(yōu)化時縮短了4.003 3 s。

圖13～圖15分別顯示了采用基于改進粒子群算法的4-3-4多項式插值法所規(guī)劃出的乒乓球發(fā)球機械手5個關節(jié)運動的位置、速度和加速度曲線。圖16是乒乓球發(fā)球機械手末端運動軌跡。

圖13 優(yōu)化后5個關節(jié)的角位置變化曲線Fig.13 The angular position curve of the five joints after optimization

圖14 優(yōu)化后5個關節(jié)的角速度變化曲線Fig.14 The angular velocity curves of the five joints after optimization

圖15 優(yōu)化后5個關節(jié)的角加速度變化曲線Fig.15 The angular acceleration curves of the five joints after optimization

圖16 乒乓球發(fā)球機械手的末端運動軌跡Fig.16 End motion trajectory of table tennis serving manipulator

從圖13～圖15中可以清晰看出，在滿足運動約束的情況下，優(yōu)化后的機械手各個關節(jié)的位置、速度、加速度曲線更加連續(xù)、平滑。相比未優(yōu)化前的各個關節(jié)運動曲線圖，各個關節(jié)的運動速度都得到提高，并趨向關節(jié)最大限制速度，而且各個關節(jié)的加速度也有所提高且無突變現(xiàn)象。圖16中也顯示出了乒乓球發(fā)球機械手能夠平滑地通過給定的路徑點完成發(fā)球任務。

6 結束語

本文以五自由度乒乓球發(fā)球機械手為研究對象，根據(jù)其各個關節(jié)的結構特性，利用D-H參數(shù)法建立發(fā)球機械手的三維模型。通過機械手在關節(jié)空間的位姿變換理論推導出發(fā)球機械手的運動數(shù)學模型，并針對機械手運動過程可能出現(xiàn)的奇異現(xiàn)象，利用蒙特卡羅法分析發(fā)球機械手的工作空間；然后，在滿足乒乓球發(fā)球機械手運動學約束下，為使得機械手能夠快速、連續(xù)、準確地完成發(fā)球動作，提出了一種基于改進PSO的4-3-4多項式插值軌跡規(guī)劃方法；最后，通過Matlab仿真對比實驗，不僅證明了利用非線性變換的慣性權重和學習因子能夠提高粒子群算法的收斂速度和優(yōu)化能力，還證明了基于改進PSO的4-3-4多項式插值法能夠大大縮短機械手的運行時間，同時也確保了各個關節(jié)運動的位置、速度、加速度更加連續(xù)、平滑，從而提高了乒乓球發(fā)球機械手末端運動的精度以及機械手的使用壽命。