楊旭東 王淞

摘 ?要： 為了提高機(jī)器人的運(yùn)動精度，讓機(jī)器人的運(yùn)動更加平滑，在二階運(yùn)動軌跡的基礎(chǔ)上加入Jerk值這一限制條件。實驗中根據(jù)Jerk值的變化將一個兩點(diǎn)之間的運(yùn)動軌跡分成七段，建立各個階段的數(shù)學(xué)模型。設(shè)計運(yùn)動軌跡的邏輯控制算法，以Simulink中Stateflow工具箱為載體，利用Stateflow中的狀態(tài)模塊以及邏輯控制轉(zhuǎn)換模塊，將建立的運(yùn)動軌跡數(shù)學(xué)模型以及邏輯控制演算法結(jié)合在一起，設(shè)計成一個三階勻加減速運(yùn)動軌跡產(chǎn)生器。通過三階運(yùn)動曲線生成的梯形加速度軌跡可以使機(jī)器人在運(yùn)動過程中減少運(yùn)動沖擊，延長使用壽命，在實際工業(yè)應(yīng)用中具有十分重要的意義。

關(guān)鍵詞： Jerk; Stateflow; 勻加減速運(yùn)動; 軌跡規(guī)劃; 機(jī)器人控制; 軌跡算法

中圖分類號： TN99?34 ? ? ? ? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識碼： A ? ? ? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2019）09?0103?04

Design of third?order uniform acceleration and deceleration motion trajectory

based on Jerk value limitation

YANG Xudong， WANG Song

（School of Mechanical Engineering， Guizhou University， Guiyang 550025， China）

Abstract： In order to improve the motion accuracy of the robot and make the robot′s motion more smooth， the limitation of Jerk value is added to the second?order motion trajectory. According to the change of Jerk values in the experiment， the motion trajectory between two points is divided into seven sections， and the mathematical model of each section is established. The logic control algorithm of the motion trajectory is designed. The Stateflow toolbox in Simulink is taken as the carrier， and the state module in Stateflow and logic control conversion module are used to combine the logic control algorithm with established mathematical model of motion trajectory to form a third?order uniform acceleration and deceleration motion trajectory generator. The trapezoidal acceleration trajectory generated by the third?order kinematic curve can reduce the motion impact and prolong the service life of the robot during the movement process of the robot， which is of great significance in practical industrial applications.

Keywords： Jerk; Stateflow; uniform acceleration and deceleration motion; trajectory planning; robot control; trajectory algorithm

0 ?引 ?言

在機(jī)器人的工作過程中，機(jī)器人的運(yùn)動軌跡可以劃分成多個點(diǎn)到點(diǎn)的運(yùn)動軌跡疊加而成。為了實現(xiàn)機(jī)器人在運(yùn)動過程中的精確性、穩(wěn)定性與平滑性，文獻(xiàn)[1]提出一種點(diǎn)到點(diǎn)的運(yùn)動軌跡規(guī)劃方法：在運(yùn)動軌跡設(shè)計過程中，通過運(yùn)動速度與加速度來規(guī)劃運(yùn)動軌跡的產(chǎn)生。由于在運(yùn)動過程中加速度為階躍信號，加速度軌跡從零瞬間變化到最大值，在實際應(yīng)用過程中會讓執(zhí)行機(jī)構(gòu)產(chǎn)生不連續(xù)的驅(qū)動力，機(jī)器人在運(yùn)動過程中會產(chǎn)生運(yùn)動沖擊，影響控制精度。針對以上問題，作者在運(yùn)動軌跡中加入第三個運(yùn)動參數(shù)Jerk值，通過矩形曲線Jerk值作用，加速度曲線變?yōu)樘菪?，速度曲線為S形，通過該方法生成的運(yùn)動軌跡將更加平滑，執(zhí)行機(jī)構(gòu)在任何情況下都將產(chǎn)生連續(xù)的驅(qū)動力，減少運(yùn)動沖擊，延長機(jī)構(gòu)的使用壽命。

Matlab軟件因為其強(qiáng)大的功能，在機(jī)械自動化領(lǐng)域的應(yīng)用越來越廣。Stateflow是Matlab Simulink中一個具有邏輯控制功能的工具箱，可以通過Stateflow中的狀態(tài)模塊和流程圖模塊，建立需要的控制模型，完成想要的邏輯控制。將機(jī)器人運(yùn)動的軌跡規(guī)劃與Stateflow結(jié)合起來，形成一種全新的軌跡控制方法，與傳統(tǒng)的通過C語言編寫控制程序相比，更加簡單實用。

1 ?三階勻加減速運(yùn)動軌跡數(shù)學(xué)模型

三階勻加減速運(yùn)動軌跡是指：在運(yùn)動軌跡的運(yùn)動參數(shù)中，加入除速度與加速度之外的第三個運(yùn)動參數(shù)Jerk值。把Jerk值設(shè)置成階躍信號曲線，可以得到梯形的加速度曲線與S形的速度曲線，最終獲得更加平滑的運(yùn)動軌跡曲線。設(shè)定運(yùn)動軌跡的初始位置為[x0]，初始速度為[v0]，初始加速度為[a0]，Jerk為[j]，運(yùn)動軌跡的基本原理如下：

在上述運(yùn)動方程的基礎(chǔ)上，通過Jerk的不同變化，可以將整個點(diǎn)到點(diǎn)的運(yùn)動過程分成七段，Jerk值從正最大值到零再到負(fù)最大值，通過這種變化產(chǎn)生連續(xù)變化的加速度與速度，得到平滑的運(yùn)動軌跡，如圖1所示。

圖1 ?Jerk值變化示意圖

在運(yùn)動過程中，設(shè)定起始點(diǎn)[S0]的坐標(biāo)為Pos_start，起始時間為[T_input]，終點(diǎn)[S7]的坐標(biāo)為Pos_target，到達(dá)時間為[Tout]，系統(tǒng)運(yùn)行時間為[T_run]，整個運(yùn)行過程需要的時間為[T_exe]。通過以上運(yùn)動參數(shù)，求出運(yùn)動過程中的最大速度、最大加速度以及Jerk值。

在求解整個勻加減速運(yùn)動軌跡之前還需要知道到達(dá)每個節(jié)點(diǎn)的時間。設(shè)置Jerk值作用的時間為[T]_jm，恒定加速度作用的時間為[T]_am，恒定速度作用的時間為[T]_vm，七個節(jié)點(diǎn)的時間分別為[T]_[S1]～[T]_[S7]。利用運(yùn)動軌跡的最大速度、加速度與Jerk值，就可以求出運(yùn)動軌跡到達(dá)每個節(jié)點(diǎn)的位移[Pos_S1～Pos_S6]與速度[Vel_S1～Vel_S6]。在已知各個節(jié)點(diǎn)的時間分布以及位移和速度以后，利用式（4）～式（6）就可以求出整個點(diǎn)到點(diǎn)的勻加減速運(yùn)動軌跡，每個階段的運(yùn)動方程式如下：

2 ?以Simulink Stateflow為基礎(chǔ)的軌跡控制算法設(shè)計

2.1 ?運(yùn)動軌跡算法設(shè)計

為了使機(jī)器人在運(yùn)動過程中控制起來更加簡便，實驗中設(shè)計了如圖2所示的軌跡算法路程圖。首先在軌跡產(chǎn)生器的輸入端輸入起始坐標(biāo)與終點(diǎn)坐標(biāo)，系統(tǒng)根據(jù)式（4）～式（6）計算出完成本次軌跡所需要的最大速度、最大加速度以及Jerk值。以這三個運(yùn)動參數(shù)為基礎(chǔ)，結(jié)合運(yùn)動軌跡數(shù)學(xué)模型產(chǎn)生一個三階勻加減速運(yùn)動軌跡曲線。在整個軌跡輸出的過程中，系統(tǒng)會時刻監(jiān)測有沒有新的位置指令輸入。如果在軌跡還沒有到達(dá)終點(diǎn)坐標(biāo)時輸入了一個新的位置指令，則系統(tǒng)將停止向上一次輸入的終點(diǎn)坐標(biāo)運(yùn)動，以下達(dá)新指令的時間為起始時間，那一刻所處的位置坐標(biāo)為起始坐標(biāo)，以新的輸入坐標(biāo)為終點(diǎn)坐標(biāo)去規(guī)劃一個運(yùn)動軌跡。當(dāng)系統(tǒng)運(yùn)動到目標(biāo)位置以后，它會再一次檢驗有沒有新的位置坐標(biāo)輸入，如果有將向下一個坐標(biāo)運(yùn)動，如果沒有新的指令輸入，則軌跡輸出會一直停留在最后一個終點(diǎn)坐標(biāo)上。

2.2 ?Simulink Stateflow控制流程設(shè)計

在Matlab Simulink環(huán)境中，根據(jù)Stateflow工具箱中的狀態(tài)模塊與流程圖的判定條件語法，結(jié)合邏輯控制算法，組成運(yùn)動軌跡控制的核心。如圖3所示，Initialize模塊為起始模塊，設(shè)置運(yùn)動軌跡的初始化運(yùn)動參數(shù)，并且將參數(shù)送到運(yùn)動軌跡的數(shù)學(xué)模型中，產(chǎn)生第一個運(yùn)動軌跡，到達(dá)起始點(diǎn)。之后系統(tǒng)就會判斷目標(biāo)指令有沒有變化，當(dāng)輸入新的目標(biāo)指令以后，系統(tǒng)進(jìn)入Judge模塊，重新設(shè)置新的運(yùn)動參數(shù)，然后將新的運(yùn)動參數(shù)傳送到運(yùn)動軌跡的數(shù)學(xué)模型中，生成新的運(yùn)動軌跡。在運(yùn)動過程中，Judge狀態(tài)模塊也會一直檢測是否有新的指令輸入。如果有新的指令輸入，系統(tǒng)將會更新Judge模塊的軌跡參數(shù)，將重置的參數(shù)送入到運(yùn)動軌跡數(shù)學(xué)模型中，生成新的運(yùn)動軌跡。

圖2 ?邏輯算法流程圖

圖3 ?Stateflow邏輯控制結(jié)構(gòu)

3 ?運(yùn)動軌跡仿真實驗

整個運(yùn)動軌跡的控制實驗都是在Matlab Simulink環(huán)境中完成。首先，將建立的運(yùn)動軌跡數(shù)學(xué)模型編寫成Matlab控制程序，封裝成一個軌跡輸出控制模塊;然后，使用Simulink Stateflow設(shè)計的軌跡控制算法封裝成一個邏輯算法控制模塊，兩個模塊之間通過運(yùn)動軌跡參數(shù)建立聯(lián)系;最后，加入輸入端口與輸出端口，建立一個三階勻加減速運(yùn)動軌跡產(chǎn)生器，如圖4所示。通過指令輸入端口輸入目標(biāo)位置，經(jīng)過邏輯算法控制模塊將運(yùn)動軌跡參數(shù)輸送到運(yùn)動軌跡的數(shù)學(xué)模型中，通過數(shù)學(xué)模型的計算，將生成的運(yùn)動軌跡輸送到軌跡輸出端口。

為了檢驗實驗中所建立的勻加減速運(yùn)動軌跡的實際效果，設(shè)定起始坐標(biāo)為0 mm，終點(diǎn)坐標(biāo)為100 mm，整段軌跡輸出需要的總時間為3 s。通過該軌跡產(chǎn)生器生成運(yùn)動軌跡的位移軌跡、速度軌跡、加速度軌跡以及Jerk軌跡如圖5所示。

圖4 ?Stateflow軌跡產(chǎn)生器

圖5 ?勻加減速運(yùn)動軌跡

4 ?結(jié) ?論

為了提高整個運(yùn)動軌跡的平滑性，減少執(zhí)行機(jī)構(gòu)的運(yùn)動沖擊，本文設(shè)計了一種基于Jerk值限制的三階勻加減速運(yùn)動曲線。將兩點(diǎn)之間的運(yùn)動軌跡分成七段，建立相應(yīng)的數(shù)學(xué)模型。使用Matlab Simulink中的Stateflow邏輯狀態(tài)控制方法，將運(yùn)動軌跡的數(shù)學(xué)模型與設(shè)計的控制算法進(jìn)行結(jié)合，設(shè)計了一個三階勻加減速運(yùn)動軌跡產(chǎn)生器。

實驗中使用的軌跡設(shè)計方法與傳統(tǒng)機(jī)器人的運(yùn)動軌跡設(shè)計相比，具有如下優(yōu)點(diǎn)：

1） 基于Jerk值的三階運(yùn)動軌跡與二階運(yùn)動軌跡相比，加速度變化更加平滑，執(zhí)行機(jī)構(gòu)在運(yùn)動過程中不會受到?jīng)_擊，使機(jī)器人的控制更加平穩(wěn)，延長了機(jī)構(gòu)的使用壽命。

2） 基于Stateflow的邏輯算法控制與通過C語言來編寫算法相比，擁有可視化的操作界面，整個設(shè)計流程變得更加清晰簡單，沒有復(fù)雜的程序代碼，通過簡單的狀態(tài)模塊搭建以及判定條件設(shè)置即可完成復(fù)雜的邏輯控制。

參考文獻(xiàn)

[1] HASCHKE R， WEITNAUER E， RITTER H. On?line planning of time?optimal， Jerk?limited trajectories [C]// 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice： IEEE， 2008： 3248?3253.

[2] XIONG J， XU X， ZHAO X， et al. Research on trajectory?planning based on genetic algorithms for SCARA robot [C]// Proceedings of 2014 IEEE Chinese Guidance， Navigation and Control Conference. Yantai， China： IEEE， 2015： 1321?1324.

[3] DIMARCO C， ZIEGERT J C， VERMILLION C. Exponential and sigmoid?interpolated machining trajectories [J]. Journal of manufacturing systems， 2015， 37： 535?541.

[4] KR?GER T， WAHL F M. Online trajectory generation： basic concepts for instantaneous reactions to unforeseen events [J]. IEEE transactions on robotics， 2010， 26（1）： 94?111.

[5] MathWorks. Model and simulate decision logic using state machines and flow charts [EB/OL]. [2018?02?13]. https：//www.mathworks.com/products/stateflow.html.

[6] RYMANSAIB Z， IRAVANI P， SAHINKAYA M N. Exponential trajectory generation for point to point motions [C]// 2013 IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Wollongong： IEEE， 2013： 906?911.

[7] GAO N N， WANG D Q， DING M W， et al. Control system design and trajectory planning for SCARA robots [J]. Applied mechanics & materials， 2014， 602： 1001?1005.

[8] BROQUE?RE， X， SIDOBRE D， HERRERA?AGUILAR I. Soft motion trajectory planner for service manipulator robot [C]// 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice： IEEE， 2008： 2808?2813.

[9] SENCER B， ISHIZAKI K. Smooth polynomial interpolation for point?to?point trajectories with vibration avoidance [C]// 2015 IEEE Conference on Industrial Electronics Society. Yokohama： IEEE， 2016： 2070?2075.

[10] KATZSCHMANN R， KROGER T， ASFOUR T， et al. Towards online trajectory generation considering robot dynamics and torque limits [C]// 2013 IEEE/RSJ International Confe?rence on Intelligent Robots and Systems. Tokyo： IEEE， 2013： 5644?5651.

[11] ARDAKANI M M G， STEMMANN M， ROBERTSSON A， et al. An analytic solution to fixed?time point?to?point trajectory planning [C]// 2015 IEEE Conference on Control Applications. Sydney： IEEE， 2015： 306?311.