基于混合插值的工業機器人關節軌跡規劃算法

韓 江 谷濤濤 夏 鏈 董方方

合肥工業大學機械工程學院，合肥，230009

0 引言

一般地，為了控制機器人的關節空間運動量，使關節運動軌跡平滑，需要對機器人的關節運動進行規劃。關節運動規劃主要包括關節運動軌跡的選擇和關節運動位置的插值［1］。通過良好的關節運動約束，可以得到平穩的運動，提高運動效率，因此，有必要尋找合適的軌跡規劃方法。

傳統的關節空間軌跡規劃方法有3次多項式插值、高階多項式插值、拋物線過渡的線性插值等。隨著計算機技術的不斷發展，計算機圖形學中的優秀理論與算法逐漸應用于機器人軌跡，如樣條曲線、B樣條曲線。文獻［2?4］在關節空間中討論了3次樣條函數插值的構造過程，從仿真結果可以看出，作者得出的曲線在整個過程中連續，滿足關節位移和速度的要求，但機器人在啟動和停止時的加速度依然有數值突變，這會產生不必要的振動。胡小平等［5］采用多項式和牛頓插值的方法進行了機械手的軌跡規劃，但牛頓迭代的過程產生了大量的截斷誤差，導致規劃的結果不夠精確。文獻［6?7］采用5次樣條曲線的研究方法，規劃出關節空間的平滑運動軌跡，但加加速度并非從零開始逐漸變化。文獻［8?9］采用的7次B樣條方法可以獲得滿足要求的算法，但是對于計算機系統而言，增加階次無疑會增加系統的運算負擔，這對實時性要求的機器人系統來說，顯然不具有合理性。

本文采用非均勻B樣條曲線與高次多項式混合插值的優化方法，在不增加控制系統計算負擔的前提下，保證機器人關節運動參數連續無突變，從而使機器人運動更加平穩。

1 基本非均勻B樣條插值

1.1 標準非均勻k次B樣條的構造［10］

關節空間軌跡規劃的步驟是：先給定任務空間下的位置 時間節點(t,)，再通過機器人運動學逆求解得出各個關節角度 時間節點（t,θ)，最后對關節進行軌跡規劃。

假設任務空間內一組位置 時間節點（ti,Ri)逆解后對應關節空間角度 時間節點為（ti,θi)，對于單個關節，采用k次非均勻B樣條對關節空間的角度進行插值，使得機器人關節能夠從角度關鍵點 θi平穩過渡到 θi+1。

非均勻k次B樣條的曲線方程為

其中，di為B樣條曲線控制頂點，i=0，1，…，n；Ni,k（u)為定義在非周期非遞減節點矢量U=（u0，u1，…，un+k+1)上的k次規范B樣條基函數，n為關鍵點個數，且規定

其中，Ni,k（u）的支撐區間為（ui,un+k+1］。

支撐區間左端節點ui與B樣條的次數無關，右端點un+k+1及區間內的節點與次數k有關，區間以外節點處的k次B樣條均為零，因此B樣條曲線可以表示為

這表明，k次B樣條曲線上的點θi（u)最多和k+1個頂點dj（j=i-k，i-k+1，…，i)相關。

由非周期非遞減時間節點矢量U的元素u∈剩下n-k個內節點采用哈特利?賈德方法計算，使時間序列規范化，即有

若給定k次B樣條曲線定義域內的參數值該B樣條曲線上的插值點可采用更快速的德布爾遞推公式計算：

其中，α與d的第一個下標表示關鍵點序號，第二個下標表示遞推級數。

k次B樣條曲線上一點對u的r階導數為

1.2 基本非均勻k次B樣條軌跡插值

為了使機器人各關節準確通過給定的關鍵點，就必須反求B樣條曲線的控制點di。控制點與時間節點矢量U的內節點元素uk+1、uk+2、…、un相對應。根據累計弦長參數化的方法對時間節點歸一化，得到各節點元素：

由式（6）可知，共有n+k個未知頂點和n+1個方程，還需要增加k-1個附加方程。一般取切矢邊界條件。不同的工作場合對機器人的要求也不同，搬運、噴涂等工作對工作精度要求不高，此時可以采用3次B樣條來進行軌跡規劃，切矢邊界條件一般為啟停速度vs、ve；裝配、雕刻等高精度的工作可以采用5次B樣條甚至7次B樣條來進行軌跡規劃，切矢邊界條件一般為啟停速度vs、ve，啟停加速度 as、ae，甚至啟停加加速度 js、je。上述切矢邊界條件對應k次B樣條曲線上的端點處的1～3階導數如下：

于是可以聯立方程組求出B樣條曲線的控制頂點：

3次B樣條插值時，k=3，此時應考慮ω1、ω2；5次B樣條插值時，k=5，應考慮ω1、ω2、a1～a3；7次B樣條插值時，k=7，應考慮ω1、ω2、a1～a3、j1～j4。j=1時，表示關節曲線的起點位置，即CN中的s元素；j=n+k，表示關節曲線的終點位置，即CN中的e元素。

2 基于高次多項式二次插值的軌跡優化

求解的標準B樣條曲線無法保證機器人關節在開始和終了時位置的角加速度或角加加速度為零，這會引起機器人的振動。如果將曲線的階次提高，則可以使角加速度或角加加速度為零，但是隨著階次的升高，計算量隨之增大。因此有必要對B樣條曲線進行改進優化，以使其滿足首末端點的角加速度為零的要求。

在不增大計算量的前提下，本文提出了一種改進方法，即將B樣條曲線首末一個區間或幾個區間采用更高次多項式插值，其余區間仍采用原始B樣條插值，來解決原始B樣條在首末位置加速度、加加速度不為零的問題。

2.1 3次B樣條插值的優化

對于一般的場合，由于原始B樣條曲線已經得出，故在首末段區間有已知條件（tn,θn）、

設首末段區間5次多項式及其導數為

式（9）有6個未知數和6個方程，有唯一解，于是可以得出首末段區間的5次多項式曲線。

2.2 5次B樣條插值的優化

對于高精度要求的場合，有必要提高B樣條的階次。同3次B樣條優化原理一樣，由于原始5次B樣條曲線已經得出，故在首末段區間有已知條件（tn,θn）、（tn,θ?n）、（tn,θ?n）、（tn,θ…n）、（tn+1,θn+1）、（tn+1, θ?n+1）、（tn+1, θ?n+1）、（tn+1, θ…n+1）。

設首末段區間7次多項式及其導數為

于是可以得到方程組并寫成矩陣的形式：

式（11）有8個未知數和8個方程，有唯一解，于是可以得出首末段區間的7次多項式曲線。

值得注意的是，在上述求解末段多項式曲線時，可以采取函數圖像平移和對稱變換，使末段區間的求解與首段區間的求解方法保持一致，以簡化求解過程。求解線性方程組時，可采用LU分解的方法，能大大節約計算時間。

3 基于高次多項式二次插值優化的機器人仿真

3.1 機器人仿真條件

機器人工作過程中，有時需要通過空間中的關鍵點。關鍵點的信息包括機器人執行末端在笛卡兒坐標系下的姿態矩陣R和位置矩陣P［11］：

仿真實驗以廣州數控的RB08型工業機器人為研究對象，進行運動學的正解和逆解分析。RB08型工業機器人如圖1所示，桿件參數如表1所示。已知示教點的位姿信息后，對示教點進行運動學逆求解并確定各關節的最優解。本次仿真主要對機器人位置進行分析，即末端姿態不變。

圖1 RB08型6自由度工業機器人Fig.1 6-DOF robot RB08

表1 RB08型6自由度工業機器人桿件參數Tab.1 Connecting rod parameters of RB08 robot

假設機器人的某個姿態為

給定任務空間下的位置 時間關鍵點(t,),通過機器人運動學逆求解得出的各個關節角度時間關鍵點數據見表2。

表2 機器人位置與關節角度Tab.2 The position and joint angle of the robot

3.2 仿真結果與分析

在MATLAB R2014a中進行仿真實驗，計算機的硬件配置為：Intel core i3-2310M CPU，2.1 GHz，4GB RAM。通過仿真實驗，得到各個關節的原始B樣條曲線插值（虛線）和改進優化后的B樣條曲線插值（實線），圖2～圖6為部分關節曲線圖，為了使曲線圖展示更清晰，其余關節曲線圖不再展示，但未展示的曲線圖優化結果與優化目標一致。

圖2 3次B樣條優化前后關節1運動學參數曲線Fig.2 Kinematic parameters of 1 joint under cubic B-spline optimization and before

圖3 3次B樣條優化前后關節5運動學參數曲線Fig.3 Kinematic parameters of 5 joint cubic under B-spline optimization and before

圖4 5次B樣條優化前后關節1運動學參數曲線Fig.4 Kinematic parameters of 1 joint underfive B-spline optimization and before

圖5 5次B樣條優化前后關節5運動學參數曲線Fig.5 Kinematic parameters of 5 joint underfive B-spline optimization and before

為驗證關節空間軌跡優化對機器人末端運動的影響，將關節角度插值離散點進行運動學正解，得到機器人末端的運動軌跡，其中，示教關鍵點以“*”的形式標出，優化插值軌跡與原始插值軌跡幾乎重合，為了清晰地展示仿真結果，僅僅給出了優化后的末端軌跡圖形和原始7次B樣條插值下的末端軌跡圖形，如圖6所示。以差分的形式近似計算得到機器人末端的切向速度、切向加速度、切向加加速度曲線，如圖7、圖8所示。

圖6 不同關節空間軌跡規劃下的機器人末端軌跡Fig.6 Ending trajectory of robot under different joint-space trajectory planning

圖7 3次B樣條優化前后機器人末端運動學參數曲線Fig.7 Comparison of kinematic parameters of the robot under cubic B-spline optimization and before

為對比不同軌跡規劃的效率，需要統計每種方法消耗的時間。按照機器人控制系統插值周期10 ms進行計算，共進行了2 800次插值，不同的規劃方式消耗的時間如表3所示。其中，原始7次B樣條的時間為關節空間下的7次B樣條插值消耗的時間。

圖8 5次B樣條優化前后機器人末端運動學參數曲線Fig.8 Comparison of kinematic parameters under five B-spline optimization and before

表3 各種軌跡規劃仿真的計算時間Tab.3 Calculation time for various trajectory planning simulations s

從關節空間看，優化前后各關節的角位移和角速度曲線基本吻合；角加速度和角加加速度曲線變化主要體現在首末區間段，優化后的參數曲線連續無突變，角加速度或角加加速度曲線從零開始變化，終了時逐漸減小為零，保證了各個關節平滑運動特性。

從笛卡兒空間看，優化后的機器人末端軌跡和原始末端軌跡均能準確通過各個關鍵點；優化后的機器人末端運動學參數，如切向加速度和切向加加速度從零開始變化，終了時逐漸減小為零。此外，從仿真的末端空間軌跡看，未經過優化的B樣條插值，由于更嚴格的邊界條件，機器人末端的空間路徑隨著B樣條階次的升高而變長，因此，混合插值優化算法能使機器人運動幅度更小。

從規劃時間來看，不同的規劃方式所消耗的時間不同，但是可以看出，原始B樣條與改進B樣條之間的規劃時長很接近，卻能達到更高階次B樣條的規劃效果。優化的3次B樣條比原始3次B樣條多耗時3.25%，比原始5次B樣條節省42.50%的時間；優化的5次B樣條比原始5次B樣條多耗時6.43%，比原始7次B樣條節省40.97%的時間。隨插補次數的增加，優化B樣條插值算法表現出的優勢將會更明顯。

4 結論

（1）針對關節空間一般B樣條插值存在的啟停特性問題，通過采用B樣條與高次多項式混合插值優化的方法，保證了機器人關節與末端執行器的運動學參數連續無突變，改善了機器人運動特性。

（2）采用高次多項式與B樣條混合插值的方法，在不增加機器人控制系統運行負擔的情況下，能達到更高階次B樣條插值的效果。

（3）通過MATLAB仿真，驗證了該混合插值算法的有效性，為改進的插值算法在機器人控制系統中的應用提供了理論支撐和參考依據。

［1］ 譚民，徐德，侯增廣，等.先進機器人控制［M］.北京：高等教育出版社，2007：90?107.TAN Min，XU De，HOU Zengguang.Advanced Robot Control［M］.Beijing：Higher Education Press，2007：90?107.

［2］ 王衛忠，趙杰，高永生，等.機器人的平面曲線軌跡規劃方法［J］.哈爾濱工業大學學報，2008，40（3）：389?392.WANG Weizhong，ZHAO Jie，GAO Yongsheng,et al.An Approach for Robot Plane Curve Trajectory Plan?ning［J］.Journal of Harbin Institute of Technology，2008，40（3）：389?392.

［3］ MENASRI R，MENASRIJ R.Smooth Trajectory Planning for Robot Using Particle Swarm Optimiza?tion［C］//International Conference on Swarm Intelli?gence Based Optimization.Mulhouse，France:ICSIBO，2014：50?59.

［4］ ABU?DAKKA J，ASSAD F.Parallel?populations Ge?netic Algorithm for the Optimization of Cubic Polyno?mial Joint Trajectories for Industrial Robot［C］//In?ternational Conference on Intelligent Robotics and Ap?plications.Aachen，2011：83?92.

［5］ 胡小平，彭濤.一種基于多項式和牛頓插值法的機械手軌跡規劃方法［J］.中國機械工程，2012，23（24）：2946?2949.HU Xiaoping，PENG Tao.A Trajectory Planning Method Based on Newton Interpolation and Polyno?mial for Manipulator［J］.China Mechanical Engi?neering，2012，23（24）：2946?2949.

［6］ 李小霞，汪木蘭.基于五次B樣條的機械手關節空間平滑軌跡規劃［J］.組合機床與自動化加工技術，2012，8（8）：39?42.LI Xiaoxia，WANG Mulan.Smooth Trajectory Plan?ning Based on Five Degrees B?spline for Manipula?tors in Joint Space［J］.Modular Machine Tool&Automatic Manufacturing Technique，2012，8（8）：39?42.

［7］ 鄧乾旺，羅正平.點焊機器人軌跡能耗模型及其優化算法研究［J］.中國機械工程，2016，27（1）：14?20.DENG Qianwang，LUO Zhengping.Study on Ener?gy Consumption Model and Its Optimization Algo?rithm for Spot Welding Robot Trajectory［J］.China Mechanical Engineering，2016，27（1）:14?20.

［8］ 朱世強，劉松國.機械手時間最優脈動連續軌跡規劃算法［J］.機械工程學報，2010，46（3）：47?52.ZHU Shiqiang，LIU Songguo.Time?optimal and Jerk?continuous Trajectory Planning Algorithm for Ma?nipulators［J］.Journal of Mechanical Engineering，2010，46（3）:47?52.

［9］ LIU Liang，CHEN Chaoying.Smooth Trajectory Planning for a Parallel Manipulator with Joint Fric?tion and Jerk Constraints［J］.International Journal of Control，Automation and Systems，2016，8（14）：1022?1036.

［10］ 施法中.計算機輔助幾何設計與非均勻有理B樣條［M］.北京：高等教育出版社，2013：78?110.SHI Fazhong.Computer Aided Geometric Design and Non Uniform Rational B Spline（CAGD&NURBS）［M］.Beijing：Higher Education Press，2013:78?110.

［11］ 高藝，馬國慶，于正林，等.一種六自由度工業機器人運動學分析及三維可視化仿真［J］.中國機械工程，2016，27（13）：1726?1731.GAO Yi，MA Guoqing，YU Zhenglin，et al.Kine?matics Analysis of a 6?DOF Industrial Robot and Its 3D Visualization Simulation［J］.China Mechan?ical Engineering，2016，27（13）：1726?1731.*