基于Jerk值限制的三階勻加減速運動軌跡設計

楊旭東 王淞

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

關鍵詞： Jerk; Stateflow; 勻加減速運動; 軌跡規劃; 機器人控制; 軌跡算法

中圖分類號： TN99?34 ? ? ? ? ? ? ? ? ? ? ? ? ?文獻標識碼： 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 ?引 ?言

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

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

1 ?三階勻加減速運動軌跡數學模型

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

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

圖1 ?Jerk值變化示意圖

在運動過程中，設定起始點[S0]的坐標為Pos_start，起始時間為[T_input]，終點[S7]的坐標為Pos_target，到達時間為[Tout]，系統運行時間為[T_run]，整個運行過程需要的時間為[T_exe]。通過以上運動參數，求出運動過程中的最大速度、最大加速度以及Jerk值。

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

2 ?以Simulink Stateflow為基礎的軌跡控制算法設計

2.1 ?運動軌跡算法設計

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

2.2 ?Simulink Stateflow控制流程設計

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

圖2 ?邏輯算法流程圖

圖3 ?Stateflow邏輯控制結構

3 ?運動軌跡仿真實驗

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

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

圖4 ?Stateflow軌跡產生器

圖5 ?勻加減速運動軌跡

4 ?結 ?論

為了提高整個運動軌跡的平滑性，減少執行機構的運動沖擊，本文設計了一種基于Jerk值限制的三階勻加減速運動曲線。將兩點之間的運動軌跡分成七段，建立相應的數學模型。使用Matlab Simulink中的Stateflow邏輯狀態控制方法，將運動軌跡的數學模型與設計的控制算法進行結合，設計了一個三階勻加減速運動軌跡產生器。

實驗中使用的軌跡設計方法與傳統機器人的運動軌跡設計相比，具有如下優點：

1） 基于Jerk值的三階運動軌跡與二階運動軌跡相比，加速度變化更加平滑，執行機構在運動過程中不會受到沖擊，使機器人的控制更加平穩，延長了機構的使用壽命。

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

參考文獻

[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.