一種多指標綜合最優的抗沖擊軌跡規劃方法

榮譽 陳剛 豆天賜

摘要 ：為提高機械臂作業效率以及抗沖擊能力，提出了一種綜合最優軌跡規劃方法。首先通過建立3-5-3多項式曲線數學模型構造出端點運動參數可控的關節運動軌跡；然后考慮關節位置、速度、加速度等約束條件，通過加權系數法定義目標函數，使機械臂的動作時間、沖擊和靈巧度達到綜合最優，在目標函數的設計中采用動態加權方法來處理關節速度與沖擊之間的矛盾；最后，針對標準粒子群算法，利用拉丁超立方抽樣函數均勻化種群， 并提出隨機慣性權重更新策略，得到改進粒子群算法，利用該算法對目標函數進行優化，得到綜合最優運動軌跡。進行了仿真和樣機實驗，實驗結果證明所提方法具有可行性。

關鍵詞 ：工業機器人；軌跡規劃；多目標優化；粒子群算法

中圖分類號 ：TP241.2

DOI：10.3969/j.issn.1004-132X.2024.02.015

開放科學（資源服務）標識碼（OSID）：

A Multi Index Comprehensive Optimal Anti Impact Trajectory

Planning Method

RONG Yu ?1，2 ?CHEN Gang 2 DOU Tianci ?1，2

1.School of Vehicle and Energy，Yanshan University，Qinhuandao，Hebei，066004

2.Hebei Key Laboratory of Special Transport Equipment，Qinhuangdao，Hebei，066004

3.College of Mechanical and Electrical Engineering，Hebei Normal University of Science &

Technology，Qinhuangdao，Hebei，066004

Abstract ： A comprehensive optimal trajectory planning method was proposed to improve the efficiency and impact resistance of robotic arms. Firstly， by establishing a 3-5-3 polynomial curve mathematical model， a joint motion trajectory with controllable endpoint motion parameters was constructed. Secondly， considering constraints such as joint position， velocity， and acceleration， the objective function was defined using the weighted coefficient method to achieve a comprehensive optimization of the action time， impact， and dexterity of the robotic arm. The dynamic weighting method was used in the design of the objective function to address the contradiction between joint velocity and impact. Finally， for the standard particle swarm optimization algorithm， the Latin hypercube sampling function was used to homogenize the population， and a random inertia weight update strategy was proposed to obtain an improved particle swarm algorithm. This algorithm was used to optimize the objective function and obtain the comprehensive optimal motion trajectory. Simulation and prototype experiments were conducted， and the experimental results demonstrate the feasibility of the proposed method.

Key words ： industrial robot; trajectory planning; multi objective optimization; particle swarm optimization（PSO）

0 引言

軌跡規劃是工業機器人完成作業任務的關鍵技術，其主要步驟如下：首先采用多項式插值或樣條曲線插值構造關節空間運動軌跡，然后通過智能算法對構造的軌跡曲線進行優化。根據不同的工作任務，軌跡優化有不同的選擇方案，主要分為單目標優化和多目標優化。

單目標優化主要有以下幾種：①時間最優軌跡優化，可以最小化作業時間進而提高生產效率；②能量最優軌跡優化，可以在降低機器能耗的同時減少機械臂的應力作用；③沖擊最優軌跡優化，可以減小沖擊，延長機器人的使用壽命。ZHANG等 ?［1］ 用五次多項式構造軌跡曲線，應用改進的自適應遺傳算法對目標函數進行優化，從而實現時間最優軌跡規劃。HE等 ?［2］ 將機械手的時間最優路徑規劃（time-optimal trajectory planning， TOTP） 問題轉化為多約束條件下的非線性最優值搜索問題，提出了一種基于模糊控制的時間搜索算法來求解時間最優軌跡。HUANG等 ?［3］ 提出了一種粒子群優化（particle swarm optimization， PSO）算法來搜索具有動力學約束的空間機械臂全局時間最優軌跡。李小為等 ?［4］ 提出了粒子群優化速度約束下的時間最優 3-5-3 多項式插值軌跡規劃方法。王成 ?［5］ ?在考慮機器人各關節速度、加速度、加加速度和力矩約束的情況下，應用遺傳算法求解工業機器人能量最優軌跡。ZHANG等 ?［6］ 提出了一種實用的時間最優光滑軌跡規劃算法，該算法利用基于動力學模型的時間最優理論規劃機器人的運動軌跡， 在幾何路徑和關節力矩約束下構建軌跡優化模型，在求解過程中動態選擇最優軌跡參數，并利用輸入整形算法實現時間最優光滑軌跡。胡智宇 ?［7］ 提出在關節空間采用五次多項式插補算法對各關節設計點到點的運動規劃，并用區間極大熵函數算法進行沖擊最優軌跡規劃。

多目標優化是指機器人兩個及以上的運動指標達到綜合最優。HE等 ?［8］ 提出了一種約束免疫多目標優化算法（constraint immune multi-objective optimization algorithm， CIMOA），對工業機器人的總運行時間和總沖擊進行尋優計算。朱世強等 ?［9］ ?采用序列二次規劃方法求解最優運動時間節點，進而規劃出滿足非線性運動學約束的時間最優脈動連續軌跡。徐海黎等 ?［10］ 采用基因環境雙演化免疫克隆算法對時間和能量的代價函數進行優化。HUANG等 ?［11］ 通過5階B樣條構造軌跡曲線，然后采用非支配排序遺傳算法（non-dominated sorting genetic algorithm Ⅱ， NSGA-Ⅱ）對整個軌跡行程時間和平均加加速度兩個目標進行優化。施祥玲等 ?［12］ 采用多目標粒子群優化（multi-objective particle swarm optimization， MOPSO）算法，以工業機器人運行時間、能量消耗和軌跡脈動為目標對其運動軌跡進行優化。呂鯤等 ?［13］ 將運行時間和關節角加加速度的積分和作為目標函數，通過遺傳算法得到滿足運動學約束和動力學約束的時間、沖擊綜合最優軌跡曲線。湯小紅等 ?［14］ 提出一種具有混沌尋優的多目標自適應粒子群優化算法，在綜合考慮時間、能耗、沖擊的情況下求解最優軌跡。

由前文可知，國內外研究人員采用的各種優化方法非常豐富，優化目標也都相對完善，但以靈巧度最優為目標對工業機器人的運動軌跡進行優化的報道相對較少。對于任何拓撲結構的機器人，靈巧度始終是一項衡量運動性能的重要指標。機械臂若具有良好的靈巧度，則具有更好的運動控制能力，能夠更精準地執行任務。針對機械臂靈巧度量化問題，國內外學者也開展了許多研究。于凌濤等 ?［15］ 通過分析機械臂雅可比矩陣奇異值、加權全域空間條件數均值與全域空間條件數波動值，建立了綜合靈巧度指標。菅奕穎 ?［16］ 利用服務球理論，通過計算機械臂全部可到點構成的點的集合與總點數的商來衡量機械臂靈巧度。楊玉維等 ?［17］ 采用雅可比逆矩陣的條件數來衡量并聯機器人的靈巧度。楊磊等 ?［18］ 采用速度雅可比矩陣條件數的倒數來衡量空間4SPS-PS并聯機構的靈巧性。TCHON等 ?［19］ 采用內生配置空間方法，提出了局部靈巧度和全局靈巧度兩個運動學靈巧度測量指標，描述了內生靈巧度測量相對于移動操作機器人傳統性能測量的優勢，兩種靈巧度都用于確認移動操作機器人的最優配置和最優幾何形態。

本文從提高工業機器人整體運動性能出發，以3-5-3多項式曲線模型進行關節空間的軌跡插值，通過加權系數法將總時間、總沖擊和靈巧度三個指標整合為綜合指標，以綜合指標作為目標函數。同時，為獲得全局綜合最優解，提出了一種改進的粒子群算法， 以該算法對軌跡曲線進行尋優。最后通過仿真和樣機實驗來驗證所提方法的有效性。

1 機器人運動學建模

本文以FANUC M710iC50六自由度機械臂為研究對象，采用D-H表達法建立運動學模型。

1.1 機器人D-H坐標的建立

通過D-H表示法對FANUC M710iC50六自由度機械臂建立運動學模型，明確各連桿之間的運動關系。圖1所示為機械臂各個關節坐標系定義。FANUC M710iC50六自由度機械臂的D-H 參數見表1，其中，α i為連桿扭角，a i為連桿長度，θ i為關節轉角，d i為關節偏置，i=1，2，…，6。

1.2 機器人正運動學建模

已知機械臂幾何參數和各連桿間的運動關系，采用齊次變換法求得從坐標系{0}到坐標系{i}的變換過程。按照機械臂坐標系定義，連桿變換矩陣公式為

i-1 ???i ?T =

Rot （z ?i-1 ，θ i） Trans （z ?i-1 ，d i） Trans （x i，a i） Rot （x i，α i） （1）

其中， Rot （z ?i-1 ，θ i）、 Rot （x i，α i）為旋轉矩陣，分別表示繞坐標系{i-1}的z軸旋轉θ i角度和繞坐標 系{i}的x軸旋轉α i角度； Trans （z ?i-1 ，d i）、 Trans （x i，a i）為平移矩陣， 分別表示沿坐標系{i-1}的z軸方向移動d i距離和沿坐標系{i}的x軸方向移動a i距離。

由式（1）可以得到連桿齊次變換矩陣 ?i-1 ???i ?T 的通式：

i-1 ???i ?T =

cos ?θ i - sin ?θ i cos ?α i ?sin ?θ i sin ?α i a i cos ?θ i

sin ?θ i ?cos ?θ i cos ?α i - cos ?θ i sin ?α i a i sin ?θ i 0 ?sin ?α i ?cos ?α i d i 0 0 0 1 ??（2）

由式（2）可得相鄰兩坐標系間變換矩陣 0 1 T ， 1 2 T ，…， 5 6 T ，故機器人的正運動學計算公 式為

0 6 T = 0 1 T 1 2 T 2 3 T 3 4 T 4 5 T 5 6 T = ?r ?11 ?r ?12 ?r ?13 ?p x r ?21 ?r ?22 ?r ?23 ?p y r ?31 ?r ?32 ?r ?33 ?p z 0 0 0 1 ??（3）

其中， ?i-1 ???i ?T 表示桿件i坐標系向i-1坐標系的齊次變換矩陣；r ?ij 為坐標系{6}相對于坐標系{0}的旋轉矩陣元素，描述坐標系{6}相對于坐標系{0}的姿態信息；p x、p y、p z為坐標系{6}原點相對于坐標系{0}原點的位置向量分量，描述坐標系{6}原點相對于坐標系{0}原點的位置信息。

1.3 機器人逆運動學建模

對于圖1所示的空間六自由度串聯機械臂，末端位姿 0 6 T 已知。機械臂逆運動學建模就是根據式（3）求各關節轉角θ i。

首先將式（3）兩邊同時左乘 0 1 T ??-1 （θ 1）、 1 2 T ??-1 （θ 2）， 右乘 5 6 T ??-1 （θ 6）可得

1 2 T ??-1 （θ 2） 0 1 T ??-1 （θ 1） ?0 6 T ??5 6 T ??-1 （θ 6）=

2 3 T （θ 3） ?3 4 T （θ 4） ?4 5 T （θ 5） ?（4）

根據式（4）兩側矩陣第三行第四列對應元素相等可得

-p y cos ?θ 1+p x sin ?θ 1+d 6（r ?23 ?cos ?θ 1-r ?13 ?sin ?θ 1）=

d 2+d 3 ?（5）

由式（5）可求解θ 1得

θ 1= arctan （m，n）- arctan （d 2+d 3，

± m 2+n 2-（d 2+d 3） 2 ） ?（6）

m=d 6r ?23 -p y ?n=d 6r ?13 -p x

根據式（4）兩側矩陣對應元素（1，4）（表示第一行、第四行元素，下同）和（2，4）分別相等，可得

k 2 sin ?θ 2-k 1 cos ?θ 2-a 2=a 3 cos ?θ 3+d 4 sin ?θ 3 ?（7）

k 1 sin ?θ 2+k 2 cos ?θ 2=a 3 sin ?θ 3-d 4 cos ?θ 3 ?（8）

其中，k 1、k 2分別為

k 1=（d 6r ?13 -p x） cos ?θ 1+（d 6r ?23 -p y） sin ?θ 1+a 1 ?（9）

k 2=p z-d 1-d 6r ?33 ??（10）

聯立式（7）和式（8），求解θ 2得

θ 2= arctan （k 3，k 4）- arctan （k 5，± k 2 3+k 2 4-k 2 5 ） ?（11）

其中，k 3、k 4、k 5分別為

k 3=2a 2k 1 ?（12）

k 4=2a 2k 2 ?（13）

k 5=a 2 3+d 2 4-a 2 2-k 2 1-k 2 2 ?（14）

將θ 2代入式（7）整理可得

θ 3= arctan （g，± a 2 3+d 2 4-g 2 ）- arctan （a 3，d 4） ?（15）

g=k 2 sin ?θ 2-k 1 cos ?θ 2-a 2

將式（3）等號兩邊同時左乘 0 1 T ??-1 （θ 1）、 1 2 T ??-1 （θ 2）、

2 3 T ??-1 （θ 3）可得

2 3 T ??-1 （θ 3） ?1 2 T ??-1 （θ 2） ?0 1 T ??-1 （θ 1） ?0 6 T =

3 4 T （θ 4） ?4 5 T （θ 5） ?5 6 T （θ 6） ?（16）

根據式（16）兩側矩陣對應元素（3，3）相等，可得

cos ?θ 5=（r ?13 c 1+r ?23 ?sin ?θ 1） sin （θ 2+θ 3）-

r ?33 ?cos ?（θ 2+θ 3） ?（17）

其中，（3，3）表示式（16）兩側矩陣第三行第三列所對應的元素值。

由式（17）可求解得到θ 5為

θ 5= arccos （（r ?13 ?cos ?θ 1+r ?23 ?sin ?θ 1） sin （θ 2+θ 3）-

r ?33 ?cos ?（θ 2+θ 3）） ?（18）

根據式（16）兩側矩陣對應元素（2，3）相等，可得

- sin ?θ 4 sin ?θ 5=r ?13 ?sin ?θ 1-r ?23 ?cos ?θ 1 ?（19）

當 sin ?θ 5≠0時，由式（19）可求解得到θ 4為

θ 4= arcsin （（r ?23 ?cos ??θ 1-r ?13 ?sin ?θ 1）/ sin ?θ 5） ?（20）

當 sin ?θ 5=0時，機械手處于奇異形位。此時，關節4與關節6軸線重合，只能求解出θ 4與θ 6的和或差。在奇異形位時，可任意選取θ 4的值，再計算相應的θ 6值。

將式（3）等號兩邊同時左乘 0 1 T ??-1 （θ 1），右乘 5 6 T ??-1 （θ 6）可得

0 1 T ??-1 （θ 1） ?0 6 T ??5 6 T ??-1 （θ 6）= 1 2 T （θ 2） ?2 3 T （θ 3） ?3 4 T （θ 4） ?4 5 T （θ 5） ?（21）

根據式（21）兩側矩陣對應元素（2，2）相等，可得

r ?32 ?cos ?θ 6+r ?31 ?sin ?θ 6=- sin ?θ 4 sin （θ 2+θ 3） ?（22）

由式（22）可求解得到θ 6為

θ 6= arctan （- sin ?θ 4 sin （θ 2+θ 3），

± r 2 ?31 +r 2 ?32 - sin ?2θ 4 sin ?2（θ 2+θ 3） ）-

arctan （r ?32 ，r ?31 ）

（23）

綜上所述，給定機器人末端位姿矩陣，即可求得滿足機器人工作要求的關節角度。

2 構造3-5-3多項式插值函數

在機器人軌跡規劃過程中，多項式插值軌跡是一種常見的軌跡構造方法。多項式插值軌跡的 核心就是計算多項式的系數，其計算量隨多項式次數的增加而逐級遞增。本文在綜合考慮軌跡在起點、終點和中間點的位置、速度、加速度和 Jerk （位置三階導數）連續以及軌跡平滑性的情況下，選擇計算量相對較小的3-5-3多項式插值構造軌跡曲線。

對于3-5-3多項式插值構造軌跡曲線，首先根據機器人在笛卡兒坐標系下起始點以及中間點的空間坐標，通過逆運動學求解各個關節在4個插值點處的關節角度，用θ ?ij 表示關節i第j個路徑點的插值角度，其中i=1，2，…，n， n表示關節數目；j=0，1，2，3表示路徑點的序號。在路徑點間采用3-5-3多項式構造軌跡曲線，其約束條件如下：①初始點、末端點和中間點的位置；②路徑點間位置、速度和加速度連續；③始末位置速度和加速度為0。

第i關節3-5-3多項式插值函數的通式為

θ ?i1 （t 1）=a ?13 t 3 1+a ?12 t 2 1+a ?11 t 1+a ?10

θ ?i2 （t 2）=a ?25 t 5 2+a ?24 t 4 2+a ?23 t 3 2+a ?22 t 2 2+a ?21 t 2+a ?20

θ ?i3 （t 3）=a ?33 t 3 3+a ?32 t 2 3+a ?31 t 3+a ?30 ???（24）

式中，θ ?i1 （t 1）、θ ?i2 （t 2）、θ ?i3 （t 3）分別為多項式的角位移函數；a ?ij 為第i（i=1，2，3）段多項式的第j個系數，j取決于多項式次數；t為插值的時間變量。

式（24）中的系數a ?ij 可根據約束條件求出，根據約束條件和約束邊界可推導出關節插值點與系數的關系矩陣 A ：

A =

t 3 1 t 2 1 t 1 0 0 0 0 0 -1 0 0 0 0 0 3t 2 1 2t 1 1 0 0 0 0 -1 0 0 0 0 0 0 6t 1 2 0 0 0 0 -2 0 0 0 0 0 0 0 0 0 0 t 5 2 t 4 2 t 3 2 t 2 2 t 2 1 0 0 0 0 -1 0 0 0 5t 4 2 4t 3 2 3t 2 2 2t 2 1 0 0 0 0 -1 0 0 0 0 20t 3 2 12t 2 2 6t 2 2 0 0 0 0 -2 0 0 0 0 0 0 0 0 0 0 0 0 t 3 3 t 2 3 t 3 1 0 0 0 0 0 0 0 0 0 0 3t 2 3 2t 3 1 0 0 0 0 0 0 0 0 0 0 0 6t 3 2 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 ???（25）

式中，t i為第i段軌跡機械臂所運行時間。

約束條件和約束邊界只與時間t有關。關系矩陣 A 與系數矩陣 a 的關系表達式如下：

Aa = θ ??（26）

其中， θ 為關節角度矩陣。系數矩陣 a 的表達式為

a =［ a ?1 ?a ?2 ?a ?3］ ?T ??（27）

a ?1=［a ?13 ?a ?12 ?a ?11 ?a ?10 ］ ?（28）

a ?2=［a ?25 ?a ?24 ?a ?23 ?a ?22 ?a ?21 ?a ?20 ］ ?（29）

a ?3=［a ?33 ?a ?32 ?a ?31 ?a ?30 ］ ?（30）

關節角度矩陣 θ 的表達式如下：

θ =［0 0 0 0 0 0 x ?i3 ?0 0 x ?i0 ?0 0

x ?i2 ?x ?i1 ］ ?T ??（31）

式中，x ?ij 為第i個關節第j個插值點的位置。

綜上所述，3-5-3多項式軌跡曲線僅與時間t有關，因此可以找到一個時間，使得機器人在這段軌跡下運行時綜合性能最優。

3 目標函數的構造

軌跡優化的核心就是目標函數的構造。目前國內外學者對目標函數的構造采用的各種方法非常豐富，但這些方法存在兩類問題：一是采用單一的運動性能指標構造目標函數，其結構過于簡單，所得解無法全面考慮機械臂運動性能和應用場合；二是選取的指標過多，導致優化問題變得更為復雜，增加了目標函數構造的難度和計算量。因此，在機械臂運動性能和目標函數構造難度的權衡下，本文選擇以下三個主要指標進行目標函數的構造：①總時間，指機器人完成任務所需的實際時間，在工業自動化中，機器人的作業時間是影響生產效率的關鍵性因素;②總沖擊，指機器人執行任務時產生的撞擊和振動，沖擊直接影響機械臂運動的穩定性和可靠性;③靈巧度，指機器人完成任務時的靈活性和適應性，靈巧度的優化可以使機器人適應更加復雜的任務以及具有更快的響應 速度。

3.1 時間優化問題描述

假設機器人執行一個動作，其末端執行器經過n個點（包括起點和終點）。通過機器人逆運動學轉換成關節空間的n個對應的位置序列，故產生n-1段軌跡，機器人完成n-1段軌跡所需時間之和即機器人運動的總時間，故時間優化問題的目標函數為

S 1=∑ n－1 i=1 t i ?（32）

式中，n-1為機械臂運動軌跡的段數，即插值點數目減1。

3.2 沖擊優化問題描述

機械臂沿軌跡運動時，若存在加速度不連續的情況，則整個機械臂系統將產生較大的沖擊。沖擊影響機械臂運動穩定性的同時還會加速機械臂疲勞破壞、縮短機械臂使用壽命。又因為機械臂的沖擊與機械臂的脈動有直接關系，脈動越小則沖擊越小。而機械臂的脈動由加速度的變化（即加加速度）來體現。本文在進行沖擊優化問題描述時采用絕對平均沖擊s來衡量關節沖擊大小，其表達式如下：

第一段軌跡沖擊

s ?i1 = ?1 t 1 ∫ ?t ??1 ?0（q … ??i1 ） 2 d t ??（33）

第二段軌跡沖擊

s ?i2 = ?1 t 2 ∫ ?t ??2 ?0（q … ??i2 ） ?2 ?d t ??（34）

第三段軌跡沖擊

s ?i3 = ?1 t 3 ∫ ?t ??3 ?0（q … ??i3 ） ?2 ?d t ??（35）

式中，t 1、t 2、t 3分別為三段軌跡運行的時間；q … ??i1 、q … ??i2 、q … ??i3 分別為第i關節第一、二、三段沖擊量，即關節加速度關于時間的變化率。

在3-5-3多項式軌跡規劃中，具體可以將軌跡分為三個階段，即初始階段、中間階段和末尾階段。對于每個階段，根據實際情況分別賦予其不同的沖擊權重，進而實現全局的動態權重，使軌跡設計更加合理。具體可以將三段多項式軌跡沖擊所占權重表示為

S 2=w 1s ?i1 +w 2s ?i2 +w 3s ?i3 ??（36）

式中，w 1、w 2、w 3為動態權重系數，分別表示三段軌跡的沖擊權重。

對于動態權重的設定，本文根據各關節沖擊與各段軌跡運行時間的變化關系來構造動態權重函數，其變換關系如圖2所示。其中，t 1、t 2、t 3為各段軌跡運行時間；圖2 a、圖2b、圖2c 分別以t 1、t 2、t 3為自變量，其余兩段軌跡時間不變；圖2 d 以t 1、t 2、t 3為自變量。

由圖2可知，關節沖擊量隨時間的減少呈遞增的趨勢。由于圖2中關節沖擊與各段軌跡運行時間的變化情況相似，故本文選取圖2 d 中關節1絕對平均沖擊量與軌跡運行時間的離散點分別進行反比例函數、指數函數、對數函數、多項式函數曲線擬合，擬合結果如圖3所示。

根據圖3擬合曲線分別計算各條曲線與離散點的均方根誤差（ root mean squared error，RMSE ），數據見表2。

由表2可知多項式函數均方根誤差最小，但多項式函數擬合的曲線不符合關節絕對平均沖擊量與各段軌跡運行時間的變化關系。反比例函數、指數函數、對數函數三種函數中，以反比例函數擬合的曲線均方根誤差最小，即擬合程度最高。故本文在構造動態權重函數時采用反比例函數，其表達式為

T =［t 1 t 2 t 3］ ?（37）

（1）當T i= max （ T ）時，有

w i= ?min （ T ） t 1+t 2+t 3 ??（38）

（2）當T i= median （ T ） 時，有

w i= ?median （ T ） t 1+t 2+t 3 ??（39）

（3）當T i= min （ T ），有

w i= ?max （ T ） t 1+t 2+t 3 ??（40）

式中， T 為時間向量，其元素t i為第i段軌跡所運行的時間；i取1、2、3；w i為第i段軌跡沖擊的權重系數。

3.3 靈巧度優化問題描述

機器人在靠近奇異位形區域時，其輸入運動與輸出運動之間的傳遞關系逐漸失真，而靈巧度就是衡量這種失真程度的指標。雅可比矩陣的條件數作為機器人靈巧度的度量指標，其值越接近1，機器人靈巧性越好，機器人在該位姿下的運動性能越優。雅可比矩陣條件數定義如下：

K J=‖ J ‖·‖ J ??-1 ‖ ?（41）

式中， J 為雅可比矩陣，‖·‖為2- 范數。

機器人末端同時具有移動和轉動時其雅可比矩陣存在量綱不統一問題，而條件數在計算時要求雅可比矩陣所有元素的量綱統一，因此需要對雅可比矩陣進行預處理，使其元素量綱統一。本文參照文獻［20］提出的可變加權矩陣對雅可比矩陣進行規范化處理?？勺兗訖嗑仃嚤磉_式 如下：

W ?V= ??1 L v ?I ?t 0 0 ?1 L ω ?I ?r ???（42）

式中， I ?t、 I ?r為單位矩陣，下標t、r為單位矩陣的階數；L v為對應末端平移速度各行矢量模的均方根；L ω為對應轉動自由度各行矢量模的均方根。

綜上，雅可比矩陣規范量綱后的表達式為

= W ?V J ??（43）

式中， ??為雅可比量綱規范化矩陣。

機械臂運動過程中可能存在某些位姿使得機械臂關節速度趨于無窮大，導致機械臂失去控制，無法繼續運動，即奇異點。在奇異點處，機械臂的雅可比矩陣逆矩陣不存在。所以引入雅可比矩陣的偽逆矩陣來代替逆矩陣的求解。偽逆矩陣 J ?+的具體形式為

J ?+= VΣ ?+ U ??T ??（44）

其中， V 、 Σ 、 U 為雅可比矩陣的奇異值分解，其表達式為

= UΣV ??T ??（45）

其中， U 為左奇異向量矩陣； V 為右奇異向量矩陣； Σ 為奇異值矩陣，也是對角矩陣，對角線上的元素是從大到小排列的奇異值； Σ ?+為 Σ 的偽逆矩陣，也是對角矩陣，對角線上的元素是 Σ 中奇異值的倒數，若奇異值為0，則對應的元素為0，其表達式如下：

Σ ?+= diag （ 1 σ 1 ， 1 σ 2 ，…， 1 σ n ） ?（46）

式中，σ i為雅可比矩陣奇異值。

在實際計算中，通常會對奇異值進行修正，以避免除以一個接近于0的數。通常設置一個閾值，若小于閾值則將其設置為0。根據總時間、總沖擊和條件數的數量級關系，本文將閾值設置為0.01。

雖然條件數可以反映機械臂在指定位姿下的運動能力，但不能反映在整個軌跡內的運動能力，故構造整個軌跡靈巧度衡量指標，即對整個軌跡條件數取和求平均值，這一指標能夠全面反映機械臂在整個運動過程的運動學靈巧度，其表達 式為

S 3= 1 N ∑ N i=1 K i ?（47）

式中，S 3為整段軌跡條件數均值；N為軌跡插值點數目；K i為時間變量為t 1、t 2、t 3時所生成軌跡的第i個插值點處條件數的值。

3.4 機器人優化數學模型

本文以六自由度機械臂為研究對象，采用3-5-3多項式構造軌跡曲線。若直接以多項式軌跡的系數a ?ij 作為待尋優量，則通過式（25）可以得到時間變量t 1、t 2、t 3，這時變量維度為14。由上文多項式軌跡的約束條件可知系數矩陣 A 僅與結束時間t有關，若直接以時間變量t 1、t 2、t 3作為待尋優量，這時變量的維度為3，搜索維度大幅度降低，可以有效降低尋優的復雜性和困難性。故本文對目標函數求解時以時間變量作為待尋 優量。

綜上對優化問題的描述，可以定義以下三個優化目標：

f 1=∑ 3 i=1 t i ?（48）

f 2=∑ 6 i=1 w 1s ?i1 +w 2s ?i2 +w 3s ?i3 ??（49）

f 3= 1 N ∑ N i=1 K i ?（50）

式中，t i為第i段軌跡運行時長，i=1，2，3；N為插值點數目；f 1為機械臂運行總時間；f 2為機械臂總沖擊；f 3為機械臂全局靈巧度均值。

以上３個優化目標中，f 1可以作為衡量機械臂運動效率的指標；f 2可以作為衡量關節磨損和機械臂穩定性的指標；f 3可以作為衡量機械臂整個軌跡靈活性的指標。

設定運動學和時間約束條件，將機器人多目標綜合最優問題表達如下：

f= min （ξ 1f 1+ξ 2f 2+ξ 3f 3） ?（51）

其中，ξ 1、ξ 2、ξ 3為加權參數，其作用是平衡總時間、總沖擊和全局靈巧度均值在數量級上存在的差別，并根據機器人不同的實際需求，對機器人總時間、總沖擊、全局靈巧度均值進行加權，同時強調某項指標在整體中的重要程度。

3.5 最優問題約束條件

約束條件表示為作用于機器人上的運動學性能的物理極限。由機器人結構尺寸和運動學模型可得約束條件如下：

（1）關節位置

q ??min ?≤q≤q ??max ???（52）

（2）關節速度

q · ???min ?≤q · ≤q · ???max ???（53）

（3）關節加速度

q ?¨ ???min ?≤q ?¨ ≤q ?¨ ???max ???（54）

（4）軌跡運行時間

1≤t i≤5 ?（55）

4 軌跡優化算法——改進粒子群算法

針對標準粒子群算法前期局部搜索能力弱、后期全局搜索能力弱的問題，本文在粒子群算法中引入人工蜂群 （artificial bee colony， ABC）算法 的采蜜蜂蜜源更新策略，同時提出基于隨機慣性權重的粒子速度更新公式。最后以綜合指標作為目標函數，采用改進粒子群算法對目標函數進行優化。

4.1 改進粒子群算法

粒子速度更新改進策略如下。為改善粒子群算法在迭代前期局部搜索能力的不足和迭代后期全局搜索能力的不足，并提高收斂速度和全局收斂性，本文采用隨機慣性權重策略實現慣性權重系數w的動態改變。粒子速度更新公式如下：

v ?id =wv ?id +c 1r 1（p ?id -x ?id ）+c 2r 2（p ?gd -x ?id ）

（56）

式中，w為慣性權重系數，其值越大，全局尋優能力強，局部尋優能力弱， 其值越小，全局尋優能力弱，局部尋優能力強;c 1、c 2分別為個體學習因子和社會學習因子，表示粒子向個體歷史最優位置和種群歷史最優位置移動的權重；v ?id 為粒子當前速度；r 1、r 2為［0，1］區間內的隨機數；p ?id 為當前粒子已知最優解；p ?gd 為種群已知最優解；x ?id 為粒子當前位置。

隨機慣性權重系數表達式為

w=w ??min ?+（w ??max ?-w ??min ?）rand（1）+

σ×randn（1） ?（57）

式中，w ??max ?為隨機慣性權重最大值；w ??min ?為隨機慣性權重初始值；rand（1）為［0，1］區間內均勻分布的隨機數; randn（1）為服從標準正態分布（均值為0，標準差為1）的隨機數的函數;σ為權重誤差系數，控制取值中的權重誤差，使權重w有利于向期望權重方向進化，一般取值范圍為0.2～0.5。

通過設定不同慣性權重系數和權重誤差系數，對Ackley、Alpine、Rastrigin、Rosenbrock四種測試函數進行仿真實驗，優化結果保留2位小數，具體優化結果見表3。

通過對優化難易程度不同的四種測試函數在不同慣性權重和權重誤差下進行仿真實驗，在表3中發現［0.5 0.4 0.2］這一組慣性權重和權重誤差的取值更加合適，四種函數在這一取值下的平均迭代次數較小，即平均收斂速度較快，故選取該這一組策略作為慣性權重和權重誤差的 取值。

4.2 改進粒子群優化算法的計算步驟

（1）使用基本的拉丁超立方抽樣函數 （Latin hypercube sampling， LHS） 初始化粒子的位置、隨機初始化速度。計算每個粒子對應的適應度，確定個體最優值p_best和種群最優值g_best。

（2）采用 ABC 算法采蜜蜂蜜源更新策略對粒子的位置、個體最優適應值p_best和種群最優值g_best進行更新。

（3）用改進粒子群算法速度更新公式對所有粒子的位置及速度進行更新，并計算各粒子的適應度，更新p_best、g_best。

（4）判斷收斂條件是否滿足，若滿足， 則停止迭代，輸出最優結果g_best；否則，轉到步驟（２），直至收斂。

4.3 改進粒子群優化算法與其他算法的比較

對4種不同類型的測試函數分別用粒子群算法、人工蜂群算法和改進粒子群算法進行測試，測試結果如圖4所示?？梢钥闯?，人工蜂群算法搜索能力最弱，標準粒子群算法次之，改進粒子群算法搜索能力最強。還可以明顯發現，三種算法針對不同函數時，其穩定性也不盡相同，但改進粒子群優化算法的迭代次數始終保持在較低水平。上述分析可證明，改進粒子群算法更加有效，迭代次數更少，穩定性更好。故本文采用改進粒子群優化方法對函數進行優化。

4.4 改進粒子群優化算法的應用

本文對總時間、總沖擊、靈巧度三個指標進行綜合優化，將每個指標與權重系數的乘積作為目標函數，以軌跡運行時間為變量，通過改進粒子群算法尋優。

本文以機器人工作效率和穩定性最優為前提，通過設定不同的權重系數值進行仿真實驗，優化結果保留４位小數，具體優化結果見表4。

由表4可知，隨著ξ 1、ξ 2、ξ 3三個參數的調整，總時間、總沖擊、靈巧度三個指標有著不同程度的改變。比較表4中的結果后發現，［8 10 15］這一組慣性權重的取值更加合理，沖擊指標f 2的降幅較時間指標f 1的增幅大，且靈巧度指標f 3也小幅度下降。綜合考慮后選?。? 10 15］這一組數據作為目標函數的權重系數。

5 仿真與實驗

為驗證所提軌跡優化方法的有效性，以FANUC M710iC50六自由度機械臂為仿真對象，對指定關節位置序列的時間、沖擊、靈巧性綜合最優連續軌跡進行仿真驗證。仿真環境說明如下：①計算機軟硬件配置。操作系統Windows 10；處理器Intel Core i5-4200H;顯卡NVIDIA GeForce GTX 950M。②仿真軟件。MATLAB 2022b。機械臂經過的關節位置序列見表5。

按照本文所提改進粒子群算法，在考慮關節位置、速度、加速度等約束條件下，求解機械臂時間、沖擊、靈巧性綜合最優軌跡。通過記錄種群最優位置在每次迭代過程中的位置變化，得到種群最優位置進化圖，如圖5所示。

設置的粒子種群數目為40；最大迭代次數為100的前提條件下，仿真時長約 3 h 40 min 。由圖5可見，改進粒子群算法的全局搜索能力和局部搜索能力都較為突出，在經歷不超過10次迭代后就快速收斂。機械臂綜合最優軌跡仿真位姿變化如圖6所示。

為進一步驗證提出軌跡規劃方法的有效性，搭建了實驗樣機，如圖7所示?？刂葡到y總體結構簡圖見圖8。

上位機通過RS-232串口/以太網與M710iC50六自由度機械臂進行通信。上位機與機械臂建立連接后，在上位機程序中通過串口向機械臂發送控制指令和任務，控制柜將接受的指令進行解析，通過內部的處理器進行運算，確定每個關節電機的控制信號并輸出給相應的關節電機。這些信號控制電機的轉速和轉向，使機械臂關節按照預定的軌跡和運動規劃進行運動。

通過離線優化得到時間、沖擊、靈巧性綜合最優解，在機器人控制平臺上編程實現 3-5-3 多項式綜合最優軌跡。機械臂工作位姿變化圖見 圖9。

各關節優化后各角度、速度、加速度與時間關系變化如圖10所示。由圖10可見，規劃出的綜合最優軌跡經過指定的關節位置序列，關節位置、速度均連續，且滿足運動學約束，其中僅第5關節的關節角速度和角加速度波動略大?；诳箾_擊的目的，優化時盡可能減小前三個關節轉動幅度，以減小機械臂整體晃動幅度，因此增大了關節5的轉動幅度，導致關節5的關節角速度和角加速度出現較大幅度的波動。

6 結論

（1）本文以FANUC M710iC50六自由度串聯機械臂為研究對象，將關節空間中的路徑點兩兩之間通過多項式曲線連接產生機器人的運動軌跡，該軌跡規劃方法保證了運動的連續性，且關節啟停速度、加速度可以根據實際需要自由設定。

（2）對運動軌跡進行優化時，綜合考慮整個運動過程中的動作時間、沖擊和靈巧度，并在沖擊指標量化時引入動態權重系數以及在靈巧度指標中引入偽逆矩陣概念。

（3）通過加權系數法定義目標函數，以運動學邊界為約束條件，提出基于隨機慣性權重的粒子群算法，有利于改善算法的局部搜索和全局搜索能力，提高搜索效率，尋找全局綜合最 優解。

（4）對FANUC M710iC50六自由度機械臂的實驗和仿真結果表明，所提的軌跡構造方法和優化策略是真實可行的。

參考文獻 ：

［1］ ?ZHANG ?J， MENG Q， FENG X， et al. A 6-DOF Robot-time Optimal Trajectory Planning Based on an Improved Genetic Algorithm［J］. Robotics and Biomimetics， 2018， 5（1）： 1-7.

［2］ ?HE ?F， HUANG Q. Time-optimal Trajectory Planning of 6-DOF Manipulator Based on Fuzzy Control［C］∥Actuators. MDPI， 2022， 11（11）： 332.

［3］ ?HUANG ?Panfeng， XU Yangsheng. PSO-based Time-optimal Trajectory Planning for Space Robot with Dynamic Constraints［C］∥IEEE International Conference on Robotics and Biomimetics. Kunming， 2006： 1402-1407.

［4］ ?李小為，胡立坤，王琥.速度約束下PSO的六自由度機械臂時間最優軌跡規劃［J］.智能系統學報，2015，10（3）：393-398.

LI Xiaowei， HU Likun， WANG Hu. Time Optimal Trajectory Planning of PSO Six Degrees of Freedom Manipulator under Speed Constraint［J］. Journal of Intelligent Systems， 2015，10（3）： 393-398.

［5］ ?王成. 基于能量最優六自由度串聯機器人軌跡規劃研究［D］. 長春：長春工業大學，2014.

WANG Cheng. Research on Trajectory Planning of Six Degrees of Freedom Serial Robot Based on Energy Optimization［D］. Changchun：Changchun University of Technology， 2014.

［6］ ?ZHANG ?T， ZHANG M， ZOU Y. Time-optimal and Smooth Trajectory Planning for Robot Manipulators［J］. International Journal of Control， Automation and Systems， 2021， 19（1）： 521-531.

［7］ ?胡智宇. 基于沖擊最優的弧焊機器人軌跡規劃研究［D］.哈爾濱：哈爾濱工業大學，2019.

HU Zhiyu. Research on Trajectory Planning of Arc Welding Robot Based on Impact Optimization［D］. Harbin：Harbin Institute of Technology， 2019.

［8］ ?HE ?T， ZHANG Y， SUN F， et al. Immune Optimization Based Multi-objective Six-DOF Trajectory Planning for Industrial Robot Manipulators［C］∥12th World Congress on Intelligent Control and Automation（WCICA）. Guilin， 2016： 2945-2950.

［9］ ?朱世強，劉松國，王宣銀，等.機械手時間最優脈動連續軌跡規劃算法［J］.機械工程學報，2010，46（3）：47-52.

ZHU Shiqiang， LIU Songguo， WANG Xuanyin， et al. Time Optimal Pulsation Continuous Trajectory Planning Algorithm for Robotic Arms［J］. Journal of Mechanical Engineering， 2010，46（3）： 47-52.

［10］ ?徐海黎，解祥榮，莊健，等.工業機器人的最優時間與最優能量軌跡規劃［J］.機械工程學報，2010，46（9）：19-25.

XU Haili， XIE Xiangrong， ZHUANG Jian， et al. Optimal Time and Energy Trajectory Planning for Industrial Robots［J］. Journal of Mechanical Engineering， 2010，46（9）： 19-25.

［11］ ?HUANG ?J， HU P， WU K， et al. Optimal Time-jerk Trajectory Planning for Industrial Robots［J］. Mechanism and Machine Theory， 2018， 121： 530-544.

［12］ ?施祥玲，方紅根.工業機器人時間-能量-脈動最優軌跡規劃［J］.機械設計與制造，2018（6）：254-257.

SHI Xiangling， FANG Honggen. Optimal Trajectory Planning for Time Energy Pulsation of Industrial Robots［J］. Mechanical Design and Manufacturing， 2018（6）：254-257.

［13］ ?呂鯤，陳宗元，張業明.裝磚碼垛機器人時間-沖擊最優軌跡規劃［J］.機械工程師，2019（9）：1-4.

LYU Kun， CHEN Zongyuan， ZHANG Yeming. Time Impact Optimal Trajectory Planning for Brick Stacking Robot［J］. Mechanical Engineer， 2019（9）： 1-4.

［14］ ?湯小紅，任壘壘，張宏，等.基于CPSO優化算法的六自由度機械臂軌跡規劃［J］.組合機床與自動化加工技術，2023（3）：59-62.

TANG Xiaohong， REN Leilei， ZHANG Hong， et al. Path Planning of Six Degrees of Freedom Manipulator Based on CPSO Optimization Algorithm［J］. Modular Machine Tool and Automatic Machining Technology， 2023（3）：59-62.

［15］ ?于凌濤，楊景，王嵐，等.基于靈巧度的手術機械臂尺寸與結構優化［J］.哈爾濱工程大學學報，2017，38（12）：1943-1950.

YU Lingtao， YANG Jing， WANG Lan， et al. Optimization of Size and Structure of Surgical Robotic Arms Based on Dexterity［J］. Journal of Harbin Engineering University， 2017，38（12）： 1943-1950.

［16］ ?菅奕穎. 6-DOF工業機器人的工作空間與靈巧性分析及其應用［D］.武漢：華中科技大學，2015.

KAN Yiying. Analysis of Workspace and Dexterity of 6-DOF Industrial Robots and Their Applications［D］. Wuhan： Huazhong University of Science and Technology， 2015.

［17］ ?楊玉維，趙新華.3-RRRT并聯機器人工作空間與靈巧度的分析［J］.機械設計，2005（2）：11-13.

YANG Yuwei， ZHAO Xinhua. Analysis of Workspace and Dexterity of 3-RRRT Parallel Robots［J］. Mechanical Design， 2005（2）： 11-13.

［18］ ?楊磊，王南，顏亮，等.空間4SPS-PS并聯機構的剛度及靈巧性分析［J］.組合機床與自動化加工技術，2013（11）：23-25.

YANG Lei， WANG Nan， YAN Liang， et al. ?Analysis ?of Stiffness and Dexterity of Spatial 4SPS-PS Parallel Mechanism［J］. Modular Machine Tool and Automation Processing Technology， 2013（11）： 23-25.

［19］ ?TCHON ?K， ZADARNOWSKA K．Kinematic Dexterity of Mobile Manipulators： an Endogenous Configuration Space Approach［C］∥9th IEEE International Conference on Methods and Models in Automation and Robotics. Miedzyzdroje，2003：721-732.

［20］ ?劉志忠，柳洪義，羅忠，等.基于可變加權矩陣的機器人雅可比矩陣規范化［J］.機械工程學報，2014， 50（23）：29-35.

LIU Zhizhong， LIU Hongyi， LUO Zhong， et al. Standardization of Robot Jacobian Matrix and Determinant Based on Variable Weighting Matrix［J］. Journal of Mechanical Engineering， 2014，50（23）： 29-35.

（ 編輯 陳 勇 ）

作者簡介 ：

陳 剛 ，男，1998年生，碩士研究生。研究方向為機器人。

榮 譽 （通信作者），男，1981年生，副教授、博士。研究方向為變胞、變尺寸工業機器人等。E-mail：lixiangcg@126.com。