Design of third-order uniform acceleration and deceleration trajectory based on Simulink StateFlow

In order to improve the movement accuracy of the robot, let the movement of the robot trajectory smoother, we design a third-order uniform acceleration / deceleration trajectory based on the Jerk value. Through the establishment of the mathematical model of the trajectory, combined with the logic control algorithm, constituted the core of the trajectory control. Finally, the StateFlow toolbox in Simulink as the carrier. Using the state module in StateFlow combined with the logic control conversion module, the third-order uniform acceleration and deceleration trajectory design is realized.


Introduction
Trajectory planning is an important part of the robot motion control system. In the course of the robot's work, the motion trajectory of the robot can be divided into multiple point-to-point motion trajectories.
In order to realize the accuracy, stability and smoothness of the robot during the movement, In the reference [1], a motion profile based on exponential function is proposed. The trapezoidal velocity profile is generated by the exponential function, resulting in smooth motion of the robot. In the reference [4], the robot trajectory is taken as the research object, and two kinds of point-to-point motion trajectory planning methods are proposed. In the first method, the motion parameters Jerk are neglected, only to consider the effect of velocity and acceleration on the motion trajectory. This method will cause the actuator to generate a discontinuous driving force, robots in the process of movement will have an impact, thus affecting the control accuracy.In order to solve the above problem, the author adds the third motion parameter Jerk to the motion trajectory, the acceleration profile is trapezoidal, the velocity profile is S-shaped, the movement trajectory will be more smooth, the implementing agency will have a continuous driving force in any case, reduce the impact of movement and expand the service life of the organization [1]。 Matlab as a good calculation and simulation software, it is widely used in robot technology. StateFlow as a toolbox in MATLAB Simulink, has a strong logic control function. Using StateFlow in the state module and flow chart module to establish the control model, and ultimately complete the system logic control.This paper combines the trajectory planning of robot motion with StateFlow state control to form a new trajectory control method. Compared with the traditional C language control procedures, this method is more simple and practical.
The rest of the article will be divided into four parts: The first part will introduce the mathematical model of the third order uniform acceleration and deceleration trajectory based on the Jerk value limit. The second part will introduce the algorithm of trajectory control; The third part constructs the logic control module of StateFlow, and combines the motion trajectory mathematical model to realize the motion trajectory control of the robot. The fourth part is the conclusion.

Design of Third -order Acceleration and Deceleration Motion Trajectory
The third-order acceleration and deceleration trajectory refers to the addition of the Jerk value in addition to the speed and acceleration. The Jerk value is set to a square curve, which can form a trapezoidal acceleration curve and an S-shaped velocity curve, resulting in a smoother motion trajectory. The initial position of the motion trajectory is 0 , the initial velocity is 0 , the initial acceleration is 0 , jerk is , and the basic principle of the trajectory is as follows: On the basis of the above equation of motion, through Jerk's different changes, the whole point-topoint motion process can be divided into seven segments, The value of Jerk is from positive maximum to zero to negative maximum, and through this change produces a continuously varying acceleration and velocity, resulting in a smooth trajectory, as shown in Figure 1.

Figure 1 Jerk value change diagram
In the course of motion, set the starting point 0 coordinates is Pos_start, the starting time is _ , the coordinates of the end point 7 are Pos_target, the arrival time is t out , the system running time is _ , the time required for the running process is _ . Through the above motion parameters, we can calculate the maximum speed, maximum acceleration and Jerk value: Where n is the time scale factor. The time for setting the Jerk value is T_jm, the time for the constant acceleration is T_am, the time for the constant velocity action is T_vm,, the time of the seven nodes is T_S1 − T_S7. Using the velocity, the acceleration and the Jerk value, we can compute the displacement Pos_S1 − Pos_S6 and the velocity Vel_S1 − Vel_S6 for the motion trajectory to reach each node.
After knowing the time distribution of each node and the displacement and velocity.Using the equations (4), (5), (6), we can compute the complete uniform acceleration and deceleration trajectory, and the equations of motion for each stage are as follows: When the run time T_run <= T_S1, the operating trajectory parameters for this phase are: Pos = Pos − S1 + Vel − S1(T run − T S1 ) +

Motion control logic algorithm
In order to achieve the robot's trajectory control, we designed the trajectory control flow shown in Figure  2. Enter the start and end coordinates of the input port. The system calculates the maximum speed, the maximum acceleration, and the Jerk value by the equations (4), (5) and (6). Based on these three motion parameters, combined with our established mathematical model, a third -order uniform acceleration and deceleration trajectory curve is generated. In the process of track output, the system will always monitor whether there is a new target location command input. If a new position coordinate is entered when the track has not reached the target position, the system will stop moving to the previous target position, The system will start with the new instruction time as the starting time, the location of the track as the starting coordinates, the new target location as a new destination coordinates to plan a new movement trajectory. When the system moves to the target position, it will check whether there is a new position coordinate input. If there is a new coordinate input, the system will plan a new motion trajectory. If there is no new instruction input, the track output will remain at the last coordinate position.

Motion trajectory simulation experiment
The logic control and motion trajectory generation in the experiment will be done in the MATLAB Simulink environment. Through the Simulink Stateflow toolbox, using Stateflow state module and logic control module, combined with the mathematical model of motion trajectory and logic control algorithm, to establish a uniform acceleration and deceleration trajectory generator, as shown in Figure 3.

Figure 3 Motion trajectory generator based on Simulink State Flow
The most important part of the trajectory generator is the chart module. Through the state module and condition judging module, combined with the logic control algorithm, formed the core of the motion trajectory. As shown in Figure 4, the A module is the starting module, the initial motion parameters of the trajectory are set, and the parameters are sent to the Simulink Function function, produces the first trajectory to reach the set starting position. The system will detect whether the target command has changed. When entering a new target command, the system enters the B module, sets the new motion parameters, and then the new motion parameters through the function name to call the trajectory function, generate a new trajectory. During the motion, the B-state module will always detect whether there is a new instruction input. If there is a new instruction input, the system will return to the B module to update the motion parameters, the new motion parameters are sent to the trajectory function to update the trajectory. In the trajectory generator, set the starting coordinate to 0mm, the end point coordinate is 100mm, the trajectory planning movement time is 3s. We obtained the displacement profile, velocity profile, acceleration profile and Jerk profile, as shown in Fig.5.

Conclusion
In order to improve the smoothness of the trajectory and reduce the impact of the actuator, the thirdorder uniform acceleration / deceleration trajectory curve based on the Jerk value is designed. A thirdorder uniform acceleration and deceleration trajectory generator is designed by combining the mathematical model of motion trajectory with State Flow logic state control method in Matlab Simulink. Compared with the traditional C language programming, this method has many advantages: (1) Simulink Stateflow visualization makes the design process clearer and simpler; (2) There is no complicated program code, through a simple state module and determine the condition module settings, you can complete the complex logic control.