王 鍇，關勝曉

(中國科學技術大學 信息科學技術學院，安徽 合肥230026)

0 引言

隨著工業(yè)化進程的推進，采用機器代替人工已是大勢所趨。目前生產線上的機器人大多遵循固定的工作模式，對于沒有視覺信息的機械臂，其運動路徑一般由程序給定，如果要改變其路徑，則需通過手動示教或修改源程序的方式，而頻繁的手動控制無益于實現(xiàn)裝置的自動化。對于引入視覺信息的機械臂，可通過圖像或視頻算法識別環(huán)境信息，進而在線規(guī)劃運動方案，提高了系統(tǒng)的適應能力。 但在環(huán)境復雜的場合，如特殊天氣、光線昏暗、目標物特征難以提取等條件下，識別檢測算法的復雜度將大幅提升，導致系統(tǒng)成本增加，不利于實際項目的運用。 工業(yè)生產線上，SCARA 因其結構相對簡單、運行穩(wěn)定高效的優(yōu)點得到了廣泛應用。 在SCARA 的設計工作上，學者們已經取得了豐碩的成果，1984 年，Adept 公司設計制造出第一臺SCARA 機器人，從此奠定了后世SCARA 結構的雛形[1]。如今的研究工作多集中于結構的優(yōu)化，如張紅等采用有限元分析和拓撲優(yōu)化方法實現(xiàn)了小臂的輕量化[2]。 為結合視覺信息，Hill 和Park 于1979 年提出了視覺伺服的概念，經過近五十年的發(fā)展，手眼視覺伺服、場景視覺伺服系統(tǒng)等已經廣泛應用于實際生產[3]。 軌跡規(guī)劃中，多項式插值是主流方法，張秀林利用B 樣條曲線對關節(jié)軌跡進行逼近，基于非均勻變異，使用改進的遺傳算法求得最優(yōu)時間，完成了最優(yōu)軌跡規(guī)劃[4]。

為拓寬機械臂的應用場景，降低系統(tǒng)成本，在定位精度有一定忍耐限度的前提下，提出了一種基于激光引導的SCARA 方案。通過DH 法建立運動學模型，分析得到工作空間，并根據點到點和多路徑點的應用需求，分別使用五次多項式插值和三次B樣條曲線插值法進行軌跡規(guī)劃，為使首末時刻關節(jié)加速度為零，引入了修正因子，可以大幅降低機械臂受到的沖擊。 結合約束條件設計罰函數(shù)，對遺傳算法種群初始化、選擇和變異方向進行改進，可以加速種群進化，并取得時間上的近似最優(yōu)解。 結果表明，相較于傳統(tǒng)的遺傳算法，改進的遺傳算法能更快地收斂，得到的最優(yōu)解比未優(yōu)化前有明顯的改觀。

1 激光引導SCARA 的系統(tǒng)組成原理

針對無視覺系統(tǒng)適應性差和智能視覺系統(tǒng)復雜度高的問題， 引入激光作為視覺識別的目標信息，用激光投影制造的特征代替復雜環(huán)境中的不確定特征，大大降低識別和定位難度，同時結合視覺信息，可識別視場中的障礙物，估計深度信息，為定位提供視覺反饋，使系統(tǒng)同時具備無視覺系統(tǒng)和智能視覺系統(tǒng)二者的優(yōu)點。

如圖1 所示，系統(tǒng)主要由任務調度器、圖像質量優(yōu)化算法、結合激光的視覺伺服系統(tǒng)和監(jiān)控平臺組成。

為確保系統(tǒng)的穩(wěn)定運行，上位機嵌入了任務調度策略。 自動運行部分包含激光頭投射動作序列、SCARA 動作序列、故障事件的處理動作及系統(tǒng)狀態(tài)量的監(jiān)測。 手動控制部分包含控制命令解析與處理、系統(tǒng)狀態(tài)監(jiān)測。 保證裝置狀態(tài)在任何時刻都是可控和穩(wěn)定的。

圖1 激光引導SCARA 的系統(tǒng)組成圖

為配合視覺伺服系統(tǒng)，激光頭安裝在設計的托盤上，位于SCARA 大臂下側，可以防止機械臂運行時造成的遮擋。 其投射方向由安裝在托盤上的兩個直流電機控制，可根據要求改變激光頭水平方向的偏移角和豎直方向的俯仰角。 通過編寫預置程序的方式控制激光投射軌跡，也可用遙操作的方式，在控制端實時發(fā)送指令，控制投射點的移動。 使用激光制造特征簡易有效，系統(tǒng)能在光線不足的情況下正常運行，面對特殊天氣狀況時(雨、霧等)，加入去雨和去霧算法，提升圖像質量。 安裝在SARA 機械臂末端的單目相機以固定頻率采集平面圖像，提取投射點特征，計算靶心坐標，經過坐標系變換，得到世界坐標系下的表示，最后再通過運動學逆解計算各關節(jié)的關節(jié)角。 對于起點到目標點的路徑，系統(tǒng)提供了多種方案。 如果路徑形狀無限制，可采用常用的五次多項式插值，在關節(jié)空間完成平滑軌跡的規(guī)劃。 如果要求經過某些路徑點，可采用基于三次B 樣條的規(guī)劃方案，根據給定的型值點計算控制點，利用約束的運行時間，可得到對應的角度、角速度、角加速度方程，表達系統(tǒng)運動狀態(tài)的變化。結合各運動學變量的約束，以最短時間為目標函數(shù)，使用遺傳算法求解最短時間的近似解，可完成時間最優(yōu)軌跡的規(guī)劃。 如果對軌跡形狀有嚴格要求，則需在笛卡爾空間進行規(guī)劃。 軌跡跟蹤算法引入滑?？刂品椒?，其不需要精確的系統(tǒng)模型，具備優(yōu)良的抗干擾性能，設計的快速終端滑模面能加速被控量的收斂， 并有效降低系統(tǒng)狀態(tài)切入滑膜面產生的抖振問題。

監(jiān)控平臺作為人機交互的接口，是自動化設備中不可缺少的部分。 通過有線串口或無線通信的方式收集傳感器信息，實時顯示在軟件界面中，相機回傳的圖像可以幫助操作人員掌握機械臂的工作狀態(tài)。 采集的數(shù)據存放在本地數(shù)據庫中，利于后續(xù)查閱，也可同步至云平臺，實現(xiàn)遠程監(jiān)控。提供運行模式切換按鈕、機械臂動作按鈕、緊急動作按鈕，通過向下位機發(fā)送對應指令，進而控制電機動作。 手動模式支持路徑點記錄，激光打樣后，控制器計算和記錄這些點的世界坐標，根據用戶給定的目標點進行軌跡規(guī)劃，并檢驗軌跡合理性，如果軌跡不可達，系統(tǒng)將發(fā)出提示信息，用戶可自行切換至自動運行或重新給定路徑點。

2 建立SCARA 機械臂的運動學模型

運動學模型分為正向運動學和逆向運動學模型。 二者的求解方向完全相反。 已知各關節(jié)的關節(jié)角，求解末端的目標位姿，稱為正向運動學建模；由目標位姿反求各關節(jié)的關節(jié)角，稱為反向運動學建模[5]。 實際情況中，一般已知目標位姿，需控制電機驅動機械臂末端到達指定位置，所以需要進行逆向運動學建模。

建立運動學模型前，必須建立合適的參考系，并明確各坐標系間的轉換關系。 本文以桃子TZ-500 SCARA 機器人作為實驗平臺，使用DH 坐標系法在四個關節(jié)建立坐標系，其中，關節(jié)1、2、4 為旋轉關節(jié)，關節(jié)3 為移動關節(jié)，表1 為DH 參數(shù)表，坐標系建立如圖2 所示。 以{O0-X0Y0Z0}為世界坐標系，其位置是絕對的，不隨機械臂運動，軌跡規(guī)劃時需要將末端位姿的坐標轉為世界坐標系下的表示。

表1 SCARA 機械臂DH 參數(shù)

矩陣Ti表示坐標系{Oi-XiYiZi}到坐標系{Oi-1-Xi-1Yi-1Zi-1}的轉換矩陣，1≤i≤4。

圖2 TZ-500 SCARA 機械臂DH 坐標系

將四個矩陣相乘，可得到機械臂末端相對于世界坐標系的轉換關系：

其 中，θ12-4表 示θ1+θ2-θ4，θ12表 示θ1+θ2。 式(5)為末端位姿矩陣，(px，py，pz，1)為終點的齊次坐標，一般情況下，這一信息是已知量。 結合式(5)、式(6) ， 進 行 逆 運 動 學 求 解， 得 到 各 關 節(jié) 的 關節(jié)角：

根據機械臂產品手冊， 獲得TZ-500 SCARA 的第一段臂長L1=250 mm，第二段臂長L2=257 mm，基座高度d1=200 mm，末端安裝長度d3=74.5 mm，前兩個關節(jié)的限位角為[-140°，140°]。 利用已知參數(shù)，編寫MATLAB 程序，建立TZ500 的仿真模型，繪出其工作平面，如圖3 所示。

圖3 TZ500 SCARA 機械臂的工作空間俯視圖

3 SCARA 機械臂的軌跡規(guī)劃策略

合理的軌跡規(guī)劃方案可以保證機械臂能快速、穩(wěn)定、安全地抵達目標位置，為后續(xù)的軌跡跟蹤提供路徑指導。 軌跡規(guī)劃分為笛卡爾空間的規(guī)劃和關節(jié)空間的規(guī)劃。 由于前者需要通過運動學逆解在笛卡爾坐標和關節(jié)坐標之間頻繁轉換，因此計算量很大。關節(jié)空間的規(guī)劃容易滿足運動學和動力學約束[6]，因而是目前廣泛采用的方式。

3.1 點到點的五次多項式插值規(guī)劃

多項式插值函數(shù)廣泛應用于軌跡規(guī)劃中，但函數(shù)階次越高，其凸包性越差，不利于軌跡的優(yōu)化[7]，為了保證加速度的連續(xù)平滑，階次必須大于4，故兩點間的軌跡可采用五次多項式進行規(guī)劃。 由運動學模型可知， 前兩個關節(jié)決定了機械臂末端在平面上的位置，假設在世界坐標系中，末端起始點的XOY 平面坐標為(507，0)，激光投射點的平面坐標為(300，200)，關節(jié)角度q 是時間的五次函數(shù)，可寫為如下形式：

兩邊對時間求解一階導數(shù)和二階導數(shù)，得到：

其 中，q0、qd分別為起始點和終點的關節(jié)角，v0、vd分別為起始點和終點的關節(jié)角速度，α0、αd分別為起始點和終點的關節(jié)角加速度，t0、td分別為運動的初始時刻和到達終點的時刻。 通過逆運動學求解，可得關節(jié)1 的qd=-0.205 4 rad， 關節(jié)2 的qd=1.559 5 rad，在電機提供的力矩足夠的前提下(后文同)，規(guī)定起點和終點的速度，加速度為0，運行時間為3 s，帶入數(shù)據計算，可得五次多項式的系數(shù)，整理得到兩個關節(jié)軌跡的五次方程：

其中，qi(t)代表第i 個關節(jié)角度隨時間的變化關系。 對式(10)做微分運算，可得到角速度、角加速度的數(shù)學描述，兩個關節(jié)的角度、角速度、角加速度的曲線如圖4、圖5 所示。

可以看到，五次多項式的規(guī)劃方案，可以保證角度、角速度、角加速度變化的連續(xù)和平滑，關節(jié)所受沖擊也是連續(xù)的。 在軌跡形狀沒有嚴格要求時，采用五次多項式插值，可以獲得良好的運動學特性。

3.2 多路徑點的三次B 樣條曲線規(guī)劃

實際工作中，受到環(huán)境的約束，用戶可能會要求SCARA 機械臂必須經過指定的路徑點。 此時點到點的插值無法滿足控制要求。 由于笛卡爾空間規(guī)劃的計算量偏大，在路徑點之間軌跡形狀沒有嚴格要求的情況下，可采用B 樣條曲線規(guī)劃。 B 樣條曲線可以擁有任意的形狀，其具備連續(xù)性和局部支撐性[8]，各段軌跡能平滑連接，每段軌跡的形狀由若干個控制點控制，如果該段軌跡違反了約束條件，通過調整控制點，便可以對軌跡進行修正，同時不影響其他符合要求的軌跡，從而簡化了設計。 手動控制激光投射，通過視覺檢測點位置，控制器保存目標點作為型值點，利用目標點信息計算出B 樣條曲線控制點，再結合約束條件得到對應的軌跡，可保證工作空間的路徑經過指定的點。 考慮到計算復雜度，裝置采用了基于三次多項式的B 樣條曲線插值。 均勻B 樣條基函數(shù)的定義為：

圖4 關節(jié)1 角度、角速度、角加速度時間曲線

圖5 關節(jié)2 角度、角速度、角加速度時間曲線

這是著名的Cox-deBoor 遞推公式。 其中Ni，k表示第i 段k 階B 樣條基函數(shù)，集合{u0，u1，…，um}是m+1 個非遞減數(shù)組成的節(jié)點向量，0≤i≤m-1，k≥1，m+1 為節(jié)點數(shù)量，[ui，ui+1]為第i 個節(jié) 點 閉 區(qū) 間，一 般 情 況 下，0 ≤u ≤1。 則B 樣 條曲線方程可表示為：

其中，p 為實際曲線上對應的點，d 為控制點，N為基函數(shù)，n+1 為控制點數(shù)量。對于有n+1 個控制點的k 次B 樣條曲線，其節(jié)點數(shù)為n+k+2。當k=3，即采用基于三次多項式的基函數(shù)時，每段軌跡的控制點有4 個，利用式(11)，可求出各階B 樣條的基函數(shù)，解得三次B 樣條基函數(shù)：

只要確定4 個控制點，就能計算出每段的實際軌跡：

式中，i=1，2，…，m-1，m 為關節(jié)空間的節(jié)點數(shù)。{Vi-1，Vi，Vi+1，Vi+2}是 一組控 制 點，θi(u)為 第i 段u 處對應的關節(jié)角。 利用關節(jié)角變化連續(xù)的條件，可得：

其中，Pi為 型值 點，i=1，2， …，m，θ0和θm分別 代 表起點和終點。 可以發(fā)現(xiàn)，式(15)一共有m 個方程，但控制點數(shù)量有m+2 個，如果要求出所有控制點，需要再補充兩個條件[9]。 對式(14)求解一階導數(shù)和二階導數(shù)，得出角速度和角加速度的表達式：

規(guī)定初始速度和終止速度為0，則ω1(0)=0，ωm-1(1)=0，代入式(16)，得到V0=V2，Vm+1=Vm-1。如果令初始加速度和終止加速度為0，則可求得V0=2V1-V2，Vm+1=2Vm-Vm-1，在世界坐標系下，取四個XOY平面的點p1(507，0)，p2(400，100)，p3(300，200)，p4(150，300)，單位為mm。 經運動學逆解，轉化為關節(jié)空間的型值點P1，P2，P3，P4，以弧度表示，由式(17)反求出控制點：

6 個控制點可形成三段軌跡，由式(14)，規(guī)定每段軌跡的運行時間為3 s，可得關節(jié)1 三段軌跡的表達式如式(18)所示：

同樣地，可以得到關節(jié)2 的三段軌跡表達式。限于篇幅，本文僅以關節(jié)1 為例，繪出各運動量隨時間變化的曲線，如圖6～圖8 所示。

圖6 關節(jié)1 的關節(jié)角曲線

圖7 關節(jié)1 的關節(jié)角速度曲線

圖8 關節(jié)1 的關節(jié)角加速度曲線

圖中，AB、BC、CD 分別代表了三段軌跡對應的運動學特征。 可以發(fā)現(xiàn)，角度和角速度在全過程都是平滑連續(xù)的，但角加速度在初始時刻和終止時刻發(fā)生突變，因而此處機械臂所受沖擊較大，表現(xiàn)為明顯的抖振，這對機械臂控制精度的提高和使用壽命都是不利的。

為了解決初始時刻加速度不為0 的問題，引入了修正因子。 由式(16)注意到，a1(0)=V0-2V1+V2，現(xiàn)在在式(14)的基礎上添加修正因子M(u)，它是一個五次函數(shù)，以0，1 兩個邊界點作為零點，第一段軌跡對應修正因子M1(u)，第三段軌跡對應修正因子M3(u)：

將u 轉化為時間t 表示的形式，此時角加速度在起始時刻和終止時刻的值均為0。 角速度和角加速度曲線如圖9、圖10 所示。

圖9 加入修正因子的關節(jié)1 角速度曲線

圖10 加入修正因子的關節(jié)1 角加速度曲線

可見，初始時刻和終止時刻的加速度已經為0，意味著沖擊大大減小。 在軌跡過渡段雖然連續(xù)，但不平滑，這是采用三次多項式的缺陷，本文采用基于三次多項式B 樣條曲線規(guī)劃，是對降低計算復雜度和提升運動性能所做的平衡。

3.3 基于改進遺傳算法的最優(yōu)時間軌跡規(guī)劃

最優(yōu)時間的軌跡規(guī)劃，對于提高機械臂的運行效率至關重要。 研制自動化基坑開挖系統(tǒng)的初衷，就是在縮減人力成本的前提下，進一步提高施工效率。旋挖土方時，需要將挖出的土方轉移至堆積區(qū)，再返回繼續(xù)作業(yè)。 這段軌跡一般有較遠的距離，且轉移土方的動作序列重復度高，如果機械臂的軌跡是時間最優(yōu)的，將會顯著地提高開挖效率。 遺傳算法是解決優(yōu)化問題的優(yōu)秀方案，其原理是模擬自然界的種群繁衍，通過選擇初始種群，不斷地進行交叉、選擇、變異，最終獲得性能優(yōu)良的個體[10]。 而選擇的標準，則是定義的適應度函數(shù)。 算法的主體包括初始種群的選取和初始化，種群編碼，適應度計算，交叉，選擇，變異，不斷迭代，直到滿足算法終止條件。本文基于前文三次B 樣條曲線的結果進行優(yōu)化。

(1)種群選擇與初始化

算法的優(yōu)化目標是時間最優(yōu)解，這里設置種群區(qū)間為[0.1，3](取0 會導致后續(xù)計算中表達式分母為0)，設種群數(shù)量為100，將此區(qū)間均分為100 份，在每個區(qū)間內產生一個個體，這樣可以保證個體均勻分布在解空間內，同時保證了隨機性。

(2)編碼

采用二進制編碼，方便后續(xù)的交叉與變異操作。規(guī)定可行解的精度為0.001，則二進制編碼長度為12 位。

(3)定義適應度函數(shù)

結合角速度大小約束、角加速度大小約束以及不同段軌跡切換時的連續(xù)性，得到式(21)的限定條件：

其中，0≤u≤1；i 為關節(jié) 編號，i=1，2；j 為第j 段軌跡，j=1，2，3。 式(21)從上到下代表的含義分別為：所有關節(jié)在任意段軌跡的最大角速度不超過60°/s，關節(jié)1 在任意段軌跡的最大角加速度不超過45°/s2，關節(jié)2 在任意段軌跡的最大角加速度不超過60°/s2，所有關節(jié)在軌跡切換時的角速度、角加速度是連續(xù)的。 基于罰函數(shù)設計適應度函數(shù)，當所有變量滿足約束條件時，懲罰項很小，反之，懲罰項越大，適應度函數(shù)的值越小，達到篩選子代個體的作用：

其中，T為軌跡運行總時間，P1、P2、P3為懲罰項，τi為第i個關節(jié)的力矩，τi，max為第i個關節(jié)可提供的最大力矩，fitness 為適應度函數(shù)。

(4)選擇與交叉

運用輪盤賭算法，根據個體的適應度值計算個體在子代中出現(xiàn)的概率， 適應度值越高的個體，其被選擇的概率越大。 按照適應度從大到小排序，保留較大的一半作為下一代的一部分，并記錄每代的最優(yōu)個體。 選擇兩個個體作為父親和母親，并進行單點交叉。原則上，交叉操作不能破壞優(yōu)良的個體，因而，對適應度高的個體，交叉概率應較小，對于適應度適中的個體， 其產生優(yōu)良個體的概率較大，應提高其交叉變異的概率。 另外，后期交叉變異的概率相對于前期應該減小，因為后期更注重于局部搜索，過大的概率參數(shù)將使收斂時間增加。 結合以上分析，設計自適應的交叉率pc：

其中，f 為所選個體的適應度，sumf為種群適應度總和，p 即為f 所占比例，G 為總迭代次數(shù)，g 為當前迭代次數(shù)，size 為種群大小。 p 越接近平均適應度，交叉概率越大。

(5)變異

合理的變異操作可以減小后期陷入局部極值的可能，增加局部搜索能力[11]。前期變異概率太小，將無法發(fā)揮變異增加種群多樣性的效果，后期變異概率太大，產生不良個體的概率也會變大，延長優(yōu)化時間，因而采用固定的變異概率是不合適的。 這里根據迭代的代數(shù)，逐漸減小變異概率：

目標函數(shù)可定義為：

前文的B 樣條曲線軌跡，自定義運行時間為每段3 s，取T1=T2=T3，繪出[0.1，3]定義域內的目標函數(shù)曲線，并生成包含300 個個體的種群，觀察它們的分布情況。 具體分布見圖11。

圖11 種群個體分布圖

可以發(fā)現(xiàn)，最優(yōu)解很靠近圖中的曲線，說明最優(yōu)解中三段軌跡的運行時間應當是接近的。 初始交叉率為0.8，變異率為0.2，迭代300 次，可獲得最優(yōu)解的近似，分別采用未改進的遺傳算法和改進后的遺傳算法展開計算，獲得迭代次數(shù)和最優(yōu)解(軌跡總運行時間)的趨勢圖，如圖12、圖13 所示。圖中每個“*”號代表著每一代的最優(yōu)個體，在傳統(tǒng)遺傳算法中，大約在60 代后趨于穩(wěn)定，但能發(fā)現(xiàn)，所求的值明顯還可以繼續(xù)優(yōu)化，屬于局部最優(yōu)。 采用固定的交叉變異率，可能破壞了一些已有的優(yōu)良個體，使得個體遠離最優(yōu)解,且存在最優(yōu)解求解穩(wěn)定性差的問題。 改進后的遺傳算法迭代了30 代后就開始趨于穩(wěn)定了，且后期沒有產生差異巨大的不良個體。求得的最優(yōu)解由圓圈標出，見圖12(b)、圖13(b)。 最優(yōu)解為T1=2.230 4 s，T2=2.230 4 s，T3=2.230 4 s。

圖12 傳統(tǒng)遺傳算法的優(yōu)化過程

圖13 改進遺傳算法的優(yōu)化過程

根據求得的最優(yōu)解，與前文定義的3 s 運行軌跡的方案做對比，以關節(jié)1 為例，得到角度、角速度、角加速度曲線，如圖14 所示。

可以看到，在各參數(shù)滿足約束條件的前提下，優(yōu)化時間達到了6.7 s 左右，已經有了較為明顯的提升。

4 結論

作為智能開孔機器人項目的重要部分，基于激光引導的SCARA 裝置旨在以低成本且有效的方式利用視覺信息，提升系統(tǒng)對不同環(huán)境的適應性。 裝置設計并涵蓋了最優(yōu)軌跡規(guī)劃在內的多種規(guī)劃方案，并配備成熟的滑模變結構控制方法。 系統(tǒng)任務調度器指導執(zhí)行機構的動作，保證裝置穩(wěn)定運行。支持遙操作等多種方式的人機交互，提高管理簡便性的同時，降低了工人的勞動強度。 通過DH 坐標系建立了TZ500 機械臂的正逆運動學模型，并根據實際參數(shù)繪制了工作空間。 針對點到點簡單軌跡和多限定點的復雜路徑，分別采用五次多項式插值和三次B 樣條插值進行規(guī)劃，并獲得了符合要求的軌跡。 對初始時刻和終止時刻加速度不為0 的問題，引入了修正因子，有效地降低了首末兩個時刻機械臂受到的沖擊。 基于三次B 樣條曲線規(guī)劃，利用改進的遺傳算法，再結合約束條件，設計了帶懲罰項的適應度函數(shù)，求解時間最優(yōu)解，實驗證明，改進的遺傳算法收斂速度更快，優(yōu)化后的運行時間比優(yōu)化前有較為明顯的縮短，且求解精度更高。 針對加速度不平滑的問題，后續(xù)可考慮使用基于五次多項式的B 樣條曲線規(guī)劃，為了提高運行速度，可做多線程程序編寫的嘗試，通過提高并發(fā)度充分利用計算機資源，從而削減計算復雜度提升帶來的負面影響。

圖14 優(yōu)化前后的角度、角速度、角加速度對比