楊旭東 王淞

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

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

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

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

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

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

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

圖1 ?Jerk值變化示意圖

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

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

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

2.1 ?運(yùn)動(dòng)軌跡算法設(shè)計(jì)

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

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

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

圖2 ?邏輯算法流程圖

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

3 ?運(yùn)動(dòng)軌跡仿真實(shí)驗(yàn)

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

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

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

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

4 ?結(jié) ?論

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

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

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

2） 基于Stateflow的邏輯算法控制與通過(guò)C語(yǔ)言來(lái)編寫算法相比，擁有可視化的操作界面，整個(gè)設(shè)計(jì)流程變得更加清晰簡(jiǎn)單，沒(méi)有復(fù)雜的程序代碼，通過(guò)簡(jiǎn)單的狀態(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.