基于混合插值的工業(yè)機器人關(guān)節(jié)軌跡規(guī)劃算法

韓 江 谷濤濤 夏 鏈 董方方

合肥工業(yè)大學機械工程學院，合肥，230009

0 引言

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

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

本文采用非均勻B樣條曲線與高次多項式混合插值的優(yōu)化方法，在不增加控制系統(tǒng)計算負擔的前提下，保證機器人關(guān)節(jié)運動參數(shù)連續(xù)無突變，從而使機器人運動更加平穩(wěn)。

1 基本非均勻B樣條插值

1.1 標準非均勻k次B樣條的構(gòu)造［10］

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

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

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

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

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

支撐區(qū)間左端節(jié)點ui與B樣條的次數(shù)無關(guān)，右端點un+k+1及區(qū)間內(nèi)的節(jié)點與次數(shù)k有關(guān)，區(qū)間以外節(jié)點處的k次B樣條均為零，因此B樣條曲線可以表示為

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

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

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

其中，α與d的第一個下標表示關(guān)鍵點序號，第二個下標表示遞推級數(shù)。

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

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

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

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

于是可以聯(lián)立方程組求出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時，表示關(guān)節(jié)曲線的起點位置，即CN中的s元素；j=n+k，表示關(guān)節(jié)曲線的終點位置，即CN中的e元素。

2 基于高次多項式二次插值的軌跡優(yōu)化

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

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

2.1 3次B樣條插值的優(yōu)化

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

設首末段區(qū)間5次多項式及其導數(shù)為

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

2.2 5次B樣條插值的優(yōu)化

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

設首末段區(qū)間7次多項式及其導數(shù)為

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

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

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

3 基于高次多項式二次插值優(yōu)化的機器人仿真

3.1 機器人仿真條件

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

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

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

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

假設機器人的某個姿態(tài)為

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

表2 機器人位置與關(guān)節(jié)角度Tab.2 The position and joint angle of the robot

3.2 仿真結(jié)果與分析

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

圖2 3次B樣條優(yōu)化前后關(guān)節(jié)1運動學參數(shù)曲線Fig.2 Kinematic parameters of 1 joint under cubic B-spline optimization and before

圖3 3次B樣條優(yōu)化前后關(guān)節(jié)5運動學參數(shù)曲線Fig.3 Kinematic parameters of 5 joint cubic under B-spline optimization and before

圖4 5次B樣條優(yōu)化前后關(guān)節(jié)1運動學參數(shù)曲線Fig.4 Kinematic parameters of 1 joint underfive B-spline optimization and before

圖5 5次B樣條優(yōu)化前后關(guān)節(jié)5運動學參數(shù)曲線Fig.5 Kinematic parameters of 5 joint underfive B-spline optimization and before

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

圖6 不同關(guān)節(jié)空間軌跡規(guī)劃下的機器人末端軌跡Fig.6 Ending trajectory of robot under different joint-space trajectory planning

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

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

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

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

從關(guān)節(jié)空間看，優(yōu)化前后各關(guān)節(jié)的角位移和角速度曲線基本吻合；角加速度和角加加速度曲線變化主要體現(xiàn)在首末區(qū)間段，優(yōu)化后的參數(shù)曲線連續(xù)無突變，角加速度或角加加速度曲線從零開始變化，終了時逐漸減小為零，保證了各個關(guān)節(jié)平滑運動特性。

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

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

4 結(jié)論

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

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

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

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

［2］ 王衛(wèi)忠，趙杰，高永生，等.機器人的平面曲線軌跡規(guī)劃方法［J］.哈爾濱工業(yè)大學學報，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，F(xiàn)rance: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］ 胡小平，彭濤.一種基于多項式和牛頓插值法的機械手軌跡規(guī)劃方法［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樣條的機械手關(guān)節(jié)空間平滑軌跡規(guī)劃［J］.組合機床與自動化加工技術(shù)，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］ 鄧乾旺，羅正平.點焊機器人軌跡能耗模型及其優(yōu)化算法研究［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］ 朱世強，劉松國.機械手時間最優(yōu)脈動連續(xù)軌跡規(guī)劃算法［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］ 高藝，馬國慶，于正林，等.一種六自由度工業(yè)機器人運動學分析及三維可視化仿真［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.*