TY - JOUR
AU - Li, Jin
AB - 1 Introduction Trajectory planning is moving from point A to point B while avoiding collisions over time. This can be computed in both discrete and continuous methods. Trajectory planning is a major area in robotics as it gives way to autonomous vehicles [1], gives motion trajectory to manipulators [2] or robots [3]. The goal of trajectory planning is to generate the reference inputs to the motion control system which ensures that the planned trajectories can be executed [4]. Generally, motion planning [5] can be divided into path planning [6] and trajectory planning [7], as shown in Fig 1. The type of trajectory planning contains point-to-point path [8] and continuous path [9, 10], and the trajectory planning can be implemented in Cartesian space and joint space. More specifically, trajectory planning is sometimes referred to as motion planning and erroneously as path planning. Trajectory planning is distinct from path planning in that it is parametrized by time. Essentially trajectory planning encompasses path planning in addition to planning how to move based on velocity, time, and kinematics, etc. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 1. The relationships of trajectory planning. https://doi.org/10.1371/journal.pone.0273640.g001 There are many methods can be utilized to do the trajectory planning. Volkov YS studied the general problem of interpolation by polynomial splines and considered the construction of such splines using the coefficients of expansion of a certain derivative in B-splines. Then, the properties of the obtained systems of equations are analyzed and the interpolation error is estimated [11]. Farid G et al discussed the waypoints-based trajectory generation algorithm specifically for a quadrotor UAV and considers linear interpolation along with parabolic blends to achieve high computational efficiency and continue waypoints [12]. Based on the cubic trigonometric B-spline interpolation algorithm (CTB), Li H et al proposed a new improved method: CTB-EMD, which combines the cubic trigonometric B-spline interpolation algorithm (CTB) and empirical mode decomposition (EMD). In this method, the interpolation curve is more flexible because of the adjustability of shape of the cubic trigonometric B-splines curve. Thus, the overshoot and undershoot problems in the cubic spline interpolation curve can be avoided, and then the decomposition of the signal is more accurate and effect [13]. Fang Y et al proposed a smooth and time-optimal S-curve trajectory planning method to meet the requirements of high-speed and ultra-precision operation for robotic manipulators in modern industrial applications. This method utilizes a piecewise sigmoid function to establish a jerk profile with suitably chosen phase durations such that the generated trajectories are infinitely continuously differentiable under the given constraints on velocity, acceleration and jerk [14]. Among the above methods, the most commonly used is polynomial spline interpolation. However, the generated trajectory planning will be various if different order of polynomial in the spline interpolation is used. To solve this problem, many kinds of optimization methods of trajectory planning are investigated. Ju H et al proposed a time-optimal 3-5-3 polynomial interpolation trajectory planning algorithm based on Genetic Algorithm(GA) according to the velocity limitation of manipulator [15]. Liu Y et al presented a new method of online planning high smooth and time-optimal trajectory for robotic manipulators that applies an adaptive elite genetic algorithm with singularity avoidance (AEGA-SA) [16]. Fu R et al proposed a time-optimal 3-5-3 polynomial interpolation trajectory planning algorithm based on particle swarm optimization(PSO) according to the kinematic constraints of manipulator. The algorithm solves the problem that polynomial interpolation based trajectory planning is hard to be optimized by traditional optimization methods for its shortcomings of high order and lack of convex hull property, etc [17]. Yu L et al presented a general method for the trajectory planning of the redundant planar manipulator. The new application of knot points in the quintic B-spline curve is introduced to generate inverse solutions of class I redundant joints and the particle swarm optimization algorithm is extended to generate solutions of class II redundant joints [18]. Barnett E et al introduced a novel technique, called the bisection algorithm (BA), which is fully implemented in C++ and extends dynamic programming approaches to the problem. This approach is made feasible through careful control of the numerical integration process and the use of a bisection algorithm to resolve constraint violations during integration [19]. Guo H et al presented a simultaneous trajectory planning and tracking controller for use under cruise conditions based on a model predictive control (MPC) approach to address obstacle avoidance for an intelligent vehicle [20]. To get a optimal trajectory, many constraints including actuators specifications (kinematics and dynamics) [21, 22], motion range of joints, workspace limitations, etc and many objectives including the shortest time, the shortest distance, the lowest energy consumption, the minimum oscillations [23], obstacles-avoiding [24], etc, should be considered both. Ma J et al reported on a bounded control law for nonholonomic systems of unicycle-type that satisfactorily drive a vehicle along a desired trajectory while guaranteeing a minimum safe distance from another vehicle or obstacle at all times. The control law is comprised of two parts. The first is a trajectory tracking and set-point stabilization control law that accounts for the vehicle’s kinematic and dynamic constraints (i.e. restrictions on velocity and acceleration), and the second part is a real-time avoidance control law that guarantees collision-free transit for the vehicle in noncooperative and cooperative scenarios independently of bounded uncertainties and errors in the obstacles’ detection process [25]. Wang H et al proposed a smooth point-to-point trajectory planning method for industrial robots. In the accelerated part and decelerated part, the acceleration is planned with fourth-order polynomial formed with the property of the root multiplicity. Then near time-optimal trajectory can be obtained by maximizing the constant velocity part under kinematical constraints [26]. Rodríguez-Seda EJ et al described a new convex optimization (CO) approach to time-optimal trajectory planning (TOTP), which considers both torque and jerk limits. The key insight of the approach is that the non-convex jerk limits are transformed to linear acceleration constraints and indirectly introduced into CO as the linear acceleration constraints [22]. Hsu Y et al proposed a reinforcement learning approach of collision avoidance and investigate optimal trajectory planning for unmanned aerial vehicle (UAV) communication networks [27]. Wang W et al presented an improved artificial potential field method of trajectory planning and obstacle avoidance for redundant manipulators. The method not only focused on the position for the manipulator end-effectors but also considered their posture in the course of trajectory planning and obstacle avoidance [28]. Zhang Z et al presented a hierarchical three-layer trajectory planning framework to realize real-time collision avoidance under complex driving conditions. This is mainly ascribed to the generation of a collision-free trajectory cluster based on the speed and the path re-planning [29]. Wang C et al proposed an enabling motion planning method for post-impact situations by combining the polynomial curve and artificial potential field while considering obstacle avoidance. Then, a hierarchical controller that consists of an upper (a time-varying linear quadratic regulator) and a lower (a nonlinear-optimization-based torque allocation algorithm) controller is developed to track the planned motion [26]. Specially, time-optimal trajectory planning is a more common optimization type. Reiter A et al addressed a time-optimal path following along a predefined end-effector path for kinematically redundant robots, where nonredundant robots are included as special cases [30]. Lu L et al reported a time-optimal motion planning method for robotic machining of sculptured surfaces. As there are high requirements for tool path following accuracy, an efficient numerical integration method based on the Pontryagin maximum principle is adopted as the solver for the time-optimal tool motion planning problem in robotic machining [31]. Xidias EK et al proposed a novel approach based on Genetic Algorithm for time-optimal trajectory planning of a hyper-redundant manipulator which is requested to move from an initial configuration to a final configuration in 3D workspaces considering simultaneously the kinematical constraints of the manipulator (specifically velocity and acceleration) and the presence of obstacles [32]. And, real-time trajectory planning has been a research hotspot so far due to its superior dynamic adaptability [33]. Kim J et al presented a novel trajectory planning approach suitable for online industrial robot applications along short path segments such as spot-welding. The proposed method generates trajectories that are computationally efficient, dynamically near time-optimal, and maintain path-following integrity in high-frequency planning-and-control cycles [9]. Chai R et al developed a two-step strategy for real-time trajectory planning of a hypersonic vehicle (HV) in the reentry phase. The first step generates the optimal trajectory for the HV using a recently proposed fuzzy multiobjective transcription method. In the second step, the optimally generated trajectories are utilized to train a deep neural network (DNN), which is then acted as the optimal command generator in real time [33]. However, most of optimization methods of trajectory planning are based on iterative principle. That means it will cost many time in the optimization process and the optimization will cost lots of capacity of calculation of controller as well. The problem limits the application of these iterative optimization methods on some controller with low computing capability or some scenarios with real-time requirements [16]. Therefore, an easy-implemented optimization method of trajectory planning based on event-trigger and conditional P control is proposed to address the issue, where P control is short for proportional control, which comes from PID control (Proportional-Integral-Derivative control) [34] and conditional means that P control works when some conditions are satisfied. As for event-trigger, Liu J et al proposed an event-triggered vehicle-following control scheme for connected and automated vehicles (CAVs) considering nonideal Vehicle-to-Vehicle communications such as communication delays and packet dropouts. An output-based event-triggered mechanism is employed for reducing computational burden. An Event-Triggered Model Predictive Control (ETMPC) is proposed by combining with a multi-target controller for the lateral and longitudinal vehicle-following control of CAVs [35]. Ding X et al proposed an enabling event-triggered sideslip angle estimator by using the kinematic information from a low-cost global positioning system (GPS) and an on-board inertial measurement unit (IMU) [36]. The paper is organized as follows. In Section 2, the system model of a five axis manipulator are analyzed. In Section 3, an easy-implemented optimization method of trajectory planning is proposed based on seventh order polynomial interpolation, event-trigger mechanism and conditional P control. In Section 4, comparative simulations and experiments are implemented to validate the effectiveness and efficiency of proposed optimization method. In Section 5, conclusions are drawn and future works are issued. In Appendix A and Appendix B, the forward kinematics and inverse kinematics of a five axis manipulator are deduced, respectively. In Appendix C gives out why the planning time can be used to adjust the maximum values of planning velocity and acceleration. In Appendix D gives out why the P control could guarantee the stability of time optimal controller and the converge of optimization. 2 Model The research subject: a 6 degrees of freedom (DoFs) manipulator with 5 axis and its corresponding system model are shown in Fig 2 and 2(b), respectively, where coordinate systems {0}, {1}, {2}, {3}, {4} are established on the 5 actuators of 5 DoFs, and coordinate systems {5} is established on the grasper at the end effector. Actually, there exists extra 1 DoF in the grasper at the end effector. Since the DoF is for grasping task and it does not affect the trajectory planning of manipulator, only 5 DoFs of manipulator are considered in the following. The link relationship parameters of the manipulator mainly contain: the length of the link ai, the joint angle θi, the offset of the link di and the torsion angle of the link αi, where i = 1, 2, 3, 4, 5. The four parameters are the DH parameters of the manipulator. According to the geometric model of the manipulator, its D-H parameters can be determined as shown in Table 1. The detailed forward kinematics and inverse kinematics are given out in the Appendices A and B, respectively. Specially, the forward kinematics is unique, while there are eight solutions of inverse kinematics. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 2. System model. (a) The research subject: 5 DoFs manipulator. (b) The system model of manipulator. https://doi.org/10.1371/journal.pone.0273640.g002 Download: PPT PowerPoint slide PNG larger image TIFF original image Table 1. DH parameters of manipulator. https://doi.org/10.1371/journal.pone.0273640.t001 3 Trajectory planning and optimization 3.1 Trajectory planning based on polynomial interpolation Take the trajectory planning of point-to-point path in Cartesian space as an example, the trajectory planning based on fifth order polynomial interpolation can be employed to satisfy the six constrains, that is, the position, velocity and acceleration of end effector of manipulator at the start knot point and final knot point are all should be zero and differentiable (they can be non-zero in continuous path). The trajectory planning based on fifth order polynomial interpolation only can get continuous position and velocity from start knot point to final knot point. In order to make the acceleration continuous and differentiable as well to avoid the oscillations caused by sudden accelerations when the manipulator starts and stops, the trajectory planning based on seventh order polynomial interpolation can be utilized to address the issue if the jerk at the start knot point and final knot point are both constrained to be zero. The trajectory planning based on seventh order polynomial interpolation can be written as (1) where are the position, velocity, acceleration and jerk of end effector of manipulator, respectively. ci, i = 0, …, 7 are the coefficients of the seventh order polynomial. Then the constraint conditions, i.e. the position, velocity, acceleration and jerk of end effector of manipulator at the start knot point p0 and final knot point p0 are respectively (2) where t0, tf are the known time passing through the start knot point and final knot point, respectively. The subscript {0, f} represent the known variables at the start knot point and final knot point, respectively. Specially, in the trajectory planning of point-to-point path. Substituting (2) into (1), the coefficients of the seventh order polynomial can be obtained as (3) As thus, the trajectory planning of point-to-point path in Cartesian space with constrains, specifying that the position, velocity, acceleration and jerk of end effector of manipulator at the start knot point and final knot point are continuous from zero, is handled. However, the obtained trajectory is not optimal since the planning time is pre-set and it can be optimized. 3.2 Trajectory optimization After trajectory planning, there are two steps to achieve the trajectory Optimization. The first step is to obtain one appropriate solution of inverse kinematics from the eight calculating solutions Eqs (19), (20) and (23)–(25) based on the current configuration/pose of manipulator. The second step is to seek for the optimal planning time satisfying the actuators specifications including velocity, acceleration of motor. 3.2.1 Appropriate solution of inverse kinematics. In the first step, the real-time joint angles can be measured by sensors firstly. Then the position and attitude parameters of end effector of manipulator can be obtained by forward kinematics Eqs (8)–(11). By using i-th i = 1, (i ∈ [1, 8]) solution of inverse kinematics, the corresponding joint angles are . If the difference between the actual angles θ and the calculating angles θi through forward and inverse kinematics satisfies (4) where εθ is the self-determined angle margin to distinguish the appropriate solution of inverse kinematics from the eight calculating solutions Eqs (19), (20) and (23)–(25), then the i-th solution of inverse kinematics is the appropriate solution. 3.2.2 Event-trigger mechanism. The trajectory planning Eq (1) should guarantee the planning velocity and acceleration are respectively within the maximum velocity and acceleration of actuators specifications. Compare the maximum values of planning velocity and acceleration with the maximum velocity and acceleration of actuators specifications, respectively, 5 events can be obtained as follows. Event 1: The maximum value of planning velocity is smaller than the maximum velocity of actuators specifications and the maximum value of planning acceleration is smaller than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to increase the maximum value of planning velocity until it reaches to its corresponding maximum velocity of actuators specifications, or to increase the maximum value of planning acceleration until it reaches to its corresponding maximum acceleration of actuators specifications. Noting that the maximum values of planning velocity and acceleration cannot reach to their corresponding maximum velocity and acceleration of actuators specifications simultaneously. Event 2: The maximum value of planning velocity is larger than the maximum velocity of actuators specifications, while the maximum value of planning acceleration is smaller than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum value of planning velocity until it reaches to its maximum value of actuators specifications. The maximum value of planning acceleration can not be taken into consideration since it is in its maximum values of actuators specifications. Event 3: The maximum value of planning velocity is smaller than the maximum velocity of actuators specifications, while the maximum value of planning acceleration is larger than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum value of planning acceleration until it reaches to its maximum value of actuators specifications. The maximum value of planning velocity can not be taken into consideration since it is in its maximum values of actuators specifications. Event 4: The maximum values of planning velocity and acceleration are both larger than the maximum velocity and acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum values of planning velocity and acceleration until one of them reaches to its maximum value of actuators specifications and the other is smaller than its maximum value of actuators specifications, or both of them reaches to their related maximum values of actuators specifications at the same time. Event 5: The maximum values of planning velocity and acceleration are both at the maximum velocity and acceleration of actuators specifications, or one of them at its maximum value of actuators specifications. In this event, nothing should be done since the adjustment of trajectory planning Eq (1) will affect the maximum values of planning velocity and acceleration simultaneously. It is easy to find that the adjustment of trajectory planning Eq (1) is event-based, and the event-trigger mechanism is clear. 3.2.3 Time optimal controller based on conditional P control. Before designing the time optimal controller, two problems should be clear: 1) which element can be used to adjust the maximum values of planning velocity and acceleration efficiently (what is the control input); and 2) how to use the element to do the adjustment (how to set the control law). To handle these two issues, two theorems are given out as follows. Theorem 1: In the point to point trajectory planing based on polynomial interpolation, the planning time is in inverse proportion to the maximum velocity and acceleration of planning trajectory. Theorem 2: Taking the zero errors between the maximum values of planning velocity and acceleration and the maximum velocity and acceleration of actuators specifications as the control and optimization objective, the P control could guarantee the stability of time optimal controller and the converge of optimization. The proofs of Theorem 1 and Theorem 1 are given out in the Appendices C and D, respectively. Based on the event-trigger mechanism, the detailed time optimal controller for every event can be rewritten as (5) where TOC1, TOC2, TOC3, TOC4, OPT are the corresponding control laws of Event1, Event2, Event3, Event4, Event5, respectively. Topt is the optimal planning time. are the tuning parameters of errors of velocity and acceleration in the time optimal controller, respectively. are the maximum velocity and acceleration of actuators specifications, respectively. Specially, can be individual maximum velocity and acceleration of actuators specifications for individual axis/joint. Actually, there exist two cases in the second step. They can be concluded as follows. Case 1: The planning velocity and acceleration are both in the actuators specifications. In this case, however, the planning time may be not optimal, it could be too long. As thus, the planning time T = tf − t0 can be decreased to close to the optimal planning time and the actuators specifications are satisfied simultaneously. This case only contains Event 1. Case 2: The planning velocity or acceleration is larger than that the actuators can provide. In this case, the simplest way to solve the problem is increasing the planning time T = tf − t0. This case contains Event 2, 3, 4. Event 5 is the end of time optimal controller and the optimal planning time Topt will be obtained. To avoid the overshoot of the adjustment of trajectory planning Eq (1) based on P control, Case 1 should be set/judged before Case 2. Based on the five events and two cases, in order to achieve the whole time optimal controller without using iterative algorithms, an easy-implemented optimization method for planning time T = tf − t0 is proposed as (6) where if the inequality in brackets holds, it is 1, otherwise it is 0. Remark: Under ideal conditions, the planning velocity and acceleration equal to the maximum velocity and acceleration of actuators specifications, respectively, that is, and with the optimal planning time T = tf − t0. It can be found that the time optimal controller Eq (6) is similar to P control in PID controller [34] and the P control is conditional. Whether P control is activated depends on whether the corresponding actuators specification is satisfied. That is why it is called conditional P control. Here, are similar to the proportional coefficient in PID control. Besides, other constrains contain: maximum jerk [37], kinematic and dynamic requirements, also can be considered into the right side of Eq (6). And, the optimal planning time in Eq (6) can be discretized to make it available to be used directly in practical applications. The resolution of the optimal planning time T can be the same as the servo control cycle ΔT = 0.001s. When the optimal planning time T = tf − t0 is determined, the optimized trajectory for end effector of manipulator and joint angles can be output both for control. 3.3 Control scheme The whole control scheme of trajectory optimization is shown in Fig 3. The control scheme contains three parts. The first part is a trajectory planning based on polynomial interpolation and it is similar to the research subject. The second part is an event-trigger module and it is similar to the feedback loop. The third part is a time optimal controller based on P control. The three parts form a closed loop to search for the optimal planning time and there is no input essentially. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 3. The whole control scheme of trajectory optimization. https://doi.org/10.1371/journal.pone.0273640.g003 3.4 Flow chart Based on the proposed optimization method of trajectory planning, the program flow chart is shown in Fig 4. The program flow chart contains two blocks that are consistent with the two steps in trajectory optimization. The first block is to choose the appropriate solution for inverse kinematics, and the second block considering two cases is to search for the optimal planning time within actuators specifications. The P-like control algorithm has more concise and efficient structure without considering complicated iterative process. Therefore, it can be easily implemented on micro controller unit with low computing capability. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 4. The program flow chart of trajectory optimization. https://doi.org/10.1371/journal.pone.0273640.g004 3.1 Trajectory planning based on polynomial interpolation Take the trajectory planning of point-to-point path in Cartesian space as an example, the trajectory planning based on fifth order polynomial interpolation can be employed to satisfy the six constrains, that is, the position, velocity and acceleration of end effector of manipulator at the start knot point and final knot point are all should be zero and differentiable (they can be non-zero in continuous path). The trajectory planning based on fifth order polynomial interpolation only can get continuous position and velocity from start knot point to final knot point. In order to make the acceleration continuous and differentiable as well to avoid the oscillations caused by sudden accelerations when the manipulator starts and stops, the trajectory planning based on seventh order polynomial interpolation can be utilized to address the issue if the jerk at the start knot point and final knot point are both constrained to be zero. The trajectory planning based on seventh order polynomial interpolation can be written as (1) where are the position, velocity, acceleration and jerk of end effector of manipulator, respectively. ci, i = 0, …, 7 are the coefficients of the seventh order polynomial. Then the constraint conditions, i.e. the position, velocity, acceleration and jerk of end effector of manipulator at the start knot point p0 and final knot point p0 are respectively (2) where t0, tf are the known time passing through the start knot point and final knot point, respectively. The subscript {0, f} represent the known variables at the start knot point and final knot point, respectively. Specially, in the trajectory planning of point-to-point path. Substituting (2) into (1), the coefficients of the seventh order polynomial can be obtained as (3) As thus, the trajectory planning of point-to-point path in Cartesian space with constrains, specifying that the position, velocity, acceleration and jerk of end effector of manipulator at the start knot point and final knot point are continuous from zero, is handled. However, the obtained trajectory is not optimal since the planning time is pre-set and it can be optimized. 3.2 Trajectory optimization After trajectory planning, there are two steps to achieve the trajectory Optimization. The first step is to obtain one appropriate solution of inverse kinematics from the eight calculating solutions Eqs (19), (20) and (23)–(25) based on the current configuration/pose of manipulator. The second step is to seek for the optimal planning time satisfying the actuators specifications including velocity, acceleration of motor. 3.2.1 Appropriate solution of inverse kinematics. In the first step, the real-time joint angles can be measured by sensors firstly. Then the position and attitude parameters of end effector of manipulator can be obtained by forward kinematics Eqs (8)–(11). By using i-th i = 1, (i ∈ [1, 8]) solution of inverse kinematics, the corresponding joint angles are . If the difference between the actual angles θ and the calculating angles θi through forward and inverse kinematics satisfies (4) where εθ is the self-determined angle margin to distinguish the appropriate solution of inverse kinematics from the eight calculating solutions Eqs (19), (20) and (23)–(25), then the i-th solution of inverse kinematics is the appropriate solution. 3.2.2 Event-trigger mechanism. The trajectory planning Eq (1) should guarantee the planning velocity and acceleration are respectively within the maximum velocity and acceleration of actuators specifications. Compare the maximum values of planning velocity and acceleration with the maximum velocity and acceleration of actuators specifications, respectively, 5 events can be obtained as follows. Event 1: The maximum value of planning velocity is smaller than the maximum velocity of actuators specifications and the maximum value of planning acceleration is smaller than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to increase the maximum value of planning velocity until it reaches to its corresponding maximum velocity of actuators specifications, or to increase the maximum value of planning acceleration until it reaches to its corresponding maximum acceleration of actuators specifications. Noting that the maximum values of planning velocity and acceleration cannot reach to their corresponding maximum velocity and acceleration of actuators specifications simultaneously. Event 2: The maximum value of planning velocity is larger than the maximum velocity of actuators specifications, while the maximum value of planning acceleration is smaller than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum value of planning velocity until it reaches to its maximum value of actuators specifications. The maximum value of planning acceleration can not be taken into consideration since it is in its maximum values of actuators specifications. Event 3: The maximum value of planning velocity is smaller than the maximum velocity of actuators specifications, while the maximum value of planning acceleration is larger than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum value of planning acceleration until it reaches to its maximum value of actuators specifications. The maximum value of planning velocity can not be taken into consideration since it is in its maximum values of actuators specifications. Event 4: The maximum values of planning velocity and acceleration are both larger than the maximum velocity and acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum values of planning velocity and acceleration until one of them reaches to its maximum value of actuators specifications and the other is smaller than its maximum value of actuators specifications, or both of them reaches to their related maximum values of actuators specifications at the same time. Event 5: The maximum values of planning velocity and acceleration are both at the maximum velocity and acceleration of actuators specifications, or one of them at its maximum value of actuators specifications. In this event, nothing should be done since the adjustment of trajectory planning Eq (1) will affect the maximum values of planning velocity and acceleration simultaneously. It is easy to find that the adjustment of trajectory planning Eq (1) is event-based, and the event-trigger mechanism is clear. 3.2.3 Time optimal controller based on conditional P control. Before designing the time optimal controller, two problems should be clear: 1) which element can be used to adjust the maximum values of planning velocity and acceleration efficiently (what is the control input); and 2) how to use the element to do the adjustment (how to set the control law). To handle these two issues, two theorems are given out as follows. Theorem 1: In the point to point trajectory planing based on polynomial interpolation, the planning time is in inverse proportion to the maximum velocity and acceleration of planning trajectory. Theorem 2: Taking the zero errors between the maximum values of planning velocity and acceleration and the maximum velocity and acceleration of actuators specifications as the control and optimization objective, the P control could guarantee the stability of time optimal controller and the converge of optimization. The proofs of Theorem 1 and Theorem 1 are given out in the Appendices C and D, respectively. Based on the event-trigger mechanism, the detailed time optimal controller for every event can be rewritten as (5) where TOC1, TOC2, TOC3, TOC4, OPT are the corresponding control laws of Event1, Event2, Event3, Event4, Event5, respectively. Topt is the optimal planning time. are the tuning parameters of errors of velocity and acceleration in the time optimal controller, respectively. are the maximum velocity and acceleration of actuators specifications, respectively. Specially, can be individual maximum velocity and acceleration of actuators specifications for individual axis/joint. Actually, there exist two cases in the second step. They can be concluded as follows. Case 1: The planning velocity and acceleration are both in the actuators specifications. In this case, however, the planning time may be not optimal, it could be too long. As thus, the planning time T = tf − t0 can be decreased to close to the optimal planning time and the actuators specifications are satisfied simultaneously. This case only contains Event 1. Case 2: The planning velocity or acceleration is larger than that the actuators can provide. In this case, the simplest way to solve the problem is increasing the planning time T = tf − t0. This case contains Event 2, 3, 4. Event 5 is the end of time optimal controller and the optimal planning time Topt will be obtained. To avoid the overshoot of the adjustment of trajectory planning Eq (1) based on P control, Case 1 should be set/judged before Case 2. Based on the five events and two cases, in order to achieve the whole time optimal controller without using iterative algorithms, an easy-implemented optimization method for planning time T = tf − t0 is proposed as (6) where if the inequality in brackets holds, it is 1, otherwise it is 0. Remark: Under ideal conditions, the planning velocity and acceleration equal to the maximum velocity and acceleration of actuators specifications, respectively, that is, and with the optimal planning time T = tf − t0. It can be found that the time optimal controller Eq (6) is similar to P control in PID controller [34] and the P control is conditional. Whether P control is activated depends on whether the corresponding actuators specification is satisfied. That is why it is called conditional P control. Here, are similar to the proportional coefficient in PID control. Besides, other constrains contain: maximum jerk [37], kinematic and dynamic requirements, also can be considered into the right side of Eq (6). And, the optimal planning time in Eq (6) can be discretized to make it available to be used directly in practical applications. The resolution of the optimal planning time T can be the same as the servo control cycle ΔT = 0.001s. When the optimal planning time T = tf − t0 is determined, the optimized trajectory for end effector of manipulator and joint angles can be output both for control. 3.2.1 Appropriate solution of inverse kinematics. In the first step, the real-time joint angles can be measured by sensors firstly. Then the position and attitude parameters of end effector of manipulator can be obtained by forward kinematics Eqs (8)–(11). By using i-th i = 1, (i ∈ [1, 8]) solution of inverse kinematics, the corresponding joint angles are . If the difference between the actual angles θ and the calculating angles θi through forward and inverse kinematics satisfies (4) where εθ is the self-determined angle margin to distinguish the appropriate solution of inverse kinematics from the eight calculating solutions Eqs (19), (20) and (23)–(25), then the i-th solution of inverse kinematics is the appropriate solution. 3.2.2 Event-trigger mechanism. The trajectory planning Eq (1) should guarantee the planning velocity and acceleration are respectively within the maximum velocity and acceleration of actuators specifications. Compare the maximum values of planning velocity and acceleration with the maximum velocity and acceleration of actuators specifications, respectively, 5 events can be obtained as follows. Event 1: The maximum value of planning velocity is smaller than the maximum velocity of actuators specifications and the maximum value of planning acceleration is smaller than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to increase the maximum value of planning velocity until it reaches to its corresponding maximum velocity of actuators specifications, or to increase the maximum value of planning acceleration until it reaches to its corresponding maximum acceleration of actuators specifications. Noting that the maximum values of planning velocity and acceleration cannot reach to their corresponding maximum velocity and acceleration of actuators specifications simultaneously. Event 2: The maximum value of planning velocity is larger than the maximum velocity of actuators specifications, while the maximum value of planning acceleration is smaller than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum value of planning velocity until it reaches to its maximum value of actuators specifications. The maximum value of planning acceleration can not be taken into consideration since it is in its maximum values of actuators specifications. Event 3: The maximum value of planning velocity is smaller than the maximum velocity of actuators specifications, while the maximum value of planning acceleration is larger than the maximum acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum value of planning acceleration until it reaches to its maximum value of actuators specifications. The maximum value of planning velocity can not be taken into consideration since it is in its maximum values of actuators specifications. Event 4: The maximum values of planning velocity and acceleration are both larger than the maximum velocity and acceleration of actuators specifications. In this event, the trajectory planning Eq (1) should be adjusted to decrease the maximum values of planning velocity and acceleration until one of them reaches to its maximum value of actuators specifications and the other is smaller than its maximum value of actuators specifications, or both of them reaches to their related maximum values of actuators specifications at the same time. Event 5: The maximum values of planning velocity and acceleration are both at the maximum velocity and acceleration of actuators specifications, or one of them at its maximum value of actuators specifications. In this event, nothing should be done since the adjustment of trajectory planning Eq (1) will affect the maximum values of planning velocity and acceleration simultaneously. It is easy to find that the adjustment of trajectory planning Eq (1) is event-based, and the event-trigger mechanism is clear. 3.2.3 Time optimal controller based on conditional P control. Before designing the time optimal controller, two problems should be clear: 1) which element can be used to adjust the maximum values of planning velocity and acceleration efficiently (what is the control input); and 2) how to use the element to do the adjustment (how to set the control law). To handle these two issues, two theorems are given out as follows. Theorem 1: In the point to point trajectory planing based on polynomial interpolation, the planning time is in inverse proportion to the maximum velocity and acceleration of planning trajectory. Theorem 2: Taking the zero errors between the maximum values of planning velocity and acceleration and the maximum velocity and acceleration of actuators specifications as the control and optimization objective, the P control could guarantee the stability of time optimal controller and the converge of optimization. The proofs of Theorem 1 and Theorem 1 are given out in the Appendices C and D, respectively. Based on the event-trigger mechanism, the detailed time optimal controller for every event can be rewritten as (5) where TOC1, TOC2, TOC3, TOC4, OPT are the corresponding control laws of Event1, Event2, Event3, Event4, Event5, respectively. Topt is the optimal planning time. are the tuning parameters of errors of velocity and acceleration in the time optimal controller, respectively. are the maximum velocity and acceleration of actuators specifications, respectively. Specially, can be individual maximum velocity and acceleration of actuators specifications for individual axis/joint. Actually, there exist two cases in the second step. They can be concluded as follows. Case 1: The planning velocity and acceleration are both in the actuators specifications. In this case, however, the planning time may be not optimal, it could be too long. As thus, the planning time T = tf − t0 can be decreased to close to the optimal planning time and the actuators specifications are satisfied simultaneously. This case only contains Event 1. Case 2: The planning velocity or acceleration is larger than that the actuators can provide. In this case, the simplest way to solve the problem is increasing the planning time T = tf − t0. This case contains Event 2, 3, 4. Event 5 is the end of time optimal controller and the optimal planning time Topt will be obtained. To avoid the overshoot of the adjustment of trajectory planning Eq (1) based on P control, Case 1 should be set/judged before Case 2. Based on the five events and two cases, in order to achieve the whole time optimal controller without using iterative algorithms, an easy-implemented optimization method for planning time T = tf − t0 is proposed as (6) where if the inequality in brackets holds, it is 1, otherwise it is 0. Remark: Under ideal conditions, the planning velocity and acceleration equal to the maximum velocity and acceleration of actuators specifications, respectively, that is, and with the optimal planning time T = tf − t0. It can be found that the time optimal controller Eq (6) is similar to P control in PID controller [34] and the P control is conditional. Whether P control is activated depends on whether the corresponding actuators specification is satisfied. That is why it is called conditional P control. Here, are similar to the proportional coefficient in PID control. Besides, other constrains contain: maximum jerk [37], kinematic and dynamic requirements, also can be considered into the right side of Eq (6). And, the optimal planning time in Eq (6) can be discretized to make it available to be used directly in practical applications. The resolution of the optimal planning time T can be the same as the servo control cycle ΔT = 0.001s. When the optimal planning time T = tf − t0 is determined, the optimized trajectory for end effector of manipulator and joint angles can be output both for control. 3.3 Control scheme The whole control scheme of trajectory optimization is shown in Fig 3. The control scheme contains three parts. The first part is a trajectory planning based on polynomial interpolation and it is similar to the research subject. The second part is an event-trigger module and it is similar to the feedback loop. The third part is a time optimal controller based on P control. The three parts form a closed loop to search for the optimal planning time and there is no input essentially. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 3. The whole control scheme of trajectory optimization. https://doi.org/10.1371/journal.pone.0273640.g003 3.4 Flow chart Based on the proposed optimization method of trajectory planning, the program flow chart is shown in Fig 4. The program flow chart contains two blocks that are consistent with the two steps in trajectory optimization. The first block is to choose the appropriate solution for inverse kinematics, and the second block considering two cases is to search for the optimal planning time within actuators specifications. The P-like control algorithm has more concise and efficient structure without considering complicated iterative process. Therefore, it can be easily implemented on micro controller unit with low computing capability. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 4. The program flow chart of trajectory optimization. https://doi.org/10.1371/journal.pone.0273640.g004 4 Validations 4.1 Simulations To validate the effectiveness and efficiency of proposed optimization method, comparative simulations are implemented. The simulation parameters are shown in Table 2. The simulation sets of point to point trajectory planning [21] are shown in Table 3. Specially, the actuators specifications including the limitations of velocity and acceleration of motor and they are and , respectively. And the limitations are for inputs of joints so that the actuator may contain a reducer. Download: PPT PowerPoint slide PNG larger image TIFF original image Table 2. Simulation parameters. https://doi.org/10.1371/journal.pone.0273640.t002 Download: PPT PowerPoint slide PNG larger image TIFF original image Table 3. Simulation sets. https://doi.org/10.1371/journal.pone.0273640.t003 The trajectory planning results of Simulation 1∼6 are shown in Figs 5∼10, respectively. The comparisons of simulations in Figs 5∼10 are shown in Table 4. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 5. The trajectory planning results of Simulation 1 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g005 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 6. The trajectory planning results of Simulation 2 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g006 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 7. The trajectory planning results of Simulation 3 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g007 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 8. The trajectory planning results of Simulation 4 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g008 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 9. The trajectory planning results of Simulation 5 based on the proposed optimization method. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g009 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 10. The trajectory planning results of Simulation 6 based on the proposed optimization method. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g010 Download: PPT PowerPoint slide PNG larger image TIFF original image Table 4. Comparisons of simulations. https://doi.org/10.1371/journal.pone.0273640.t004 Comparing Simulations 1, 2 & 5, it can be found that the maximums of joint angular accelerations are all inside the required angular acceleration of actuators specifications . It means the P control of in Eq (6) does not work in the three simulations. In Simulation 1, the maximum of joint angular velocity is larger than the required angular velocity of actuators specifications . In Simulation 2, the maximum of joint angular velocity is smaller than the required angular velocity of actuators specifications . In Simulation 5 based on the proposed optimization method, the maximum of joint angular velocity just equal to the required angular velocity of actuators specifications . That means the planning time should be increased in Simulation 1 and decreased in Simulation 2 to make time optimal and make actuators specifications satisfied, which is achieved by the time optimal controller Eq (6). Noting that the main effector is the maximum of joint angular velocity . Comparing Simulations 3, 4 & 6, it can be found that the maximum of joint angular velocity in Simulation 4 is inside the required angular velocity of actuators specifications , while the maximum of joint angular velocity in Simulation 3 is not. It means the P control of in Eq (6) does not work in Simulations 4, and the P control of in Eq (6) does work at the start of optimizations and does not work at the end of optimizations in Simulation 3, and only the P control of in Eq (6) does work at the end of optimizations in Simulation 3. In Simulation 3, the maximum of joint angular acceleration is larger than the required angular acceleration of actuators specifications . In Simulation 4, the maximum of joint angular acceleration is smaller than the required angular acceleration of actuators specifications . In Simulation 6 based on the proposed optimization method, the maximum of joint angular acceleration just equal to the required angular acceleration of actuators specifications . That means the planning time should be increased in Simulation 3 and decreased in Simulation 4 to make time optimal and make actuators specifications satisfied, which is achieved by the time optimal controller Eq (6) as well. Noting that the main effector is the maximum of joint angular acceleration . Comparing Simulations 1 & 2 with 3, 4, 5 & 6, it can be found that the trajectory planning based on seventh order polynomial interpolation can guarantee the planning jerk smooth and stable [37], but the trajectory planning based on fifth order polynomial interpolation cannot. Both of them can generate the continuous acceleration profile. Comparing Simulations 1 & 3 and Comparing Simulations 2 & 4, it can be found that the trajectory planning based on seventh order polynomial interpolation has higher maximum of joint angular velocity and higher maximum of joint angular acceleration than that of the trajectory planning based on fifth order polynomial interpolation. That’s the trade-off. Simulation 5 is the optimization results of Simulation 1 & 2, and Simulation 6 is the optimization results of Simulation 3 & 4. In Simulation 5, the optimization process of planning time T = tf − t0 with initial value in Simulation 1 (T = tf − t0 = 10s) and Simulation 2 (T = tf − t0 = 20s) are shown in Fig 11. In Simulation 6, the optimization process of planning time T = tf − t0 with initial value in Simulation 3 (T = tf − t0 = 10s) and Simulation 4 (T = tf − t0 = 20s) are shown in Fig 12. Actually, in Figs 11(b) and 12(b), the decreasing curve represents Case 1 and the planning time should be decreased to close to the optimal planning time, while the increasing curve in Figs 11 and 12 represents Case 2 and the planning time should be increased to close to the optimal planning time. Finally, the planning time is optimized as 15.908s in Simulation 5 and 18.106s in Simulation 6. Meanwhile, the actuators specifications are satisfied. It can be shown that, the optimizing loops are less than 10, which shows the trajectory optimization can be finished quickly. Noting that there exists an overshoot in the decreasing curve. The phenomenon is similar to P control in PID control. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 11. The optimization process of planning time T = tf − t0 in Simulation 5. (a) With initial value T = tf − t0 = 10s in Simulation 1, (b) With initial value T = tf − t0 = 20s in Simulation 2. https://doi.org/10.1371/journal.pone.0273640.g011 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 12. The optimization process of planning time T = tf − t0 in Simulation 6. (a) With initial value T = tf − t0 = 10s in Simulation 3, (b) With initial value T = tf − t0 = 20s in Simulation 4. https://doi.org/10.1371/journal.pone.0273640.g012 To evaluate the performance of calculation efficiency of proposed optimization method, comparative simulations based on Simulation 5 are implemented with two iterative algorithms: GA [15] and PSO [17], and one hierarchical three-layer trajectory planning framework [29]. Compared with these two algorithms, the required time for trajectory optimization based on the proposed optimization method is much lower, which is about 19ms, while the GA and PSO algorithms would cost about 1326ms and 1764ms, respectively, and the hierarchical three-layer trajectory planning framework would cost about 42ms. Noting that the required time is the calculated time of PC to get the optimized trajectory, instead of the planning time. And, the proposed method combining event-trigger and conditional P control, GA, PSO and the hierarchical three-layer trajectory planning framework are employed to achieve the time-optimal trajectory planning individually. Therefore, the proposed optimization method owns higher efficiency of trajectory optimization. The planning trajectory based on the proposed optimization method in Simulation 5 can be drawn by Robotics Toolbox in Matlab, as shown in Fig 13, where the blue curve is the planning trajectory. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 13. The planning trajectory based on the proposed optimization method in Simulation 5. https://doi.org/10.1371/journal.pone.0273640.g013 4.2 Experiments The experimental platform, a manipulator, is shown in Fig 2a, and its system parameters are the same as simulation parameters Table 2. The maximum velocity and acceleration of actuators specifications of each joint of manipulator are shown in Table 5. The proposed optimization method is employed to do the trajectory planning: repeating the point to point trajectory pA → pB → pC → pD → pC → pB → pA. The trajectory planning can be divided into three point to point movements: pA ↔ pB, pB ↔ pC, pC ↔ pD. Download: PPT PowerPoint slide PNG larger image TIFF original image Table 5. Simulation parameters. https://doi.org/10.1371/journal.pone.0273640.t005 The trajectory planning results of experiment based on the proposed optimization method is shown in Fig 14. Some of experimental results are also shown in Table 5. In the initial panning time T = tf = 3s of every point to point movement without optimization, the maximum values of planning velocity and acceleration should be both smaller than the maximum velocity and acceleration of actuators specifications in each joints (Event 1), which means the panning time T = tf = 3s can be smaller to make the manipulator move faster and the actuators specifications can be satisfied simultaneously. In the optimal panning time with the proposed optimization method, the optimized planning time of pA → pB, pB → pC, pC → pD are 1.537s, 1.494s, 1.537s, respectively. And the maximums of joint angular velocities are all inside the required angular velocities of actuators specifications (see Fig 14(d)), while the maximums of joint angular accelerations just equal to the required angular accelerations of actuators specifications (see Fig 14(e)). As thus, the planning times of three point to point movements all reach their optimal values. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 14. The trajectory planning results of experiment based on the proposed optimization method. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g014 The optimization process of planning time T = tf − t0 with initial value 3s of three point to point movements are shown in Fig 15. The decreasing curve represents Case 1 and the planning time should be decreased to close to the optimal planning time, while the increasing curve represents Case 2 and the planning time should be increased to close to the optimal planning time. Finally, the planning times of three movements of point to point are respectively optimized as 1.537s, 1.494s, 1.537s. Meanwhile, the actuators specifications are satisfied. It can be shown that, the optimizing loops are less than 10, which shows the trajectory optimization can be finished quickly. It makes room for the applications of proposed trajectory optimization on the micro controller with low computing capability and high real-time performance requirement. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 15. The optimization process of planning time T = tf − t0 in experiment. https://doi.org/10.1371/journal.pone.0273640.g015 4.1 Simulations To validate the effectiveness and efficiency of proposed optimization method, comparative simulations are implemented. The simulation parameters are shown in Table 2. The simulation sets of point to point trajectory planning [21] are shown in Table 3. Specially, the actuators specifications including the limitations of velocity and acceleration of motor and they are and , respectively. And the limitations are for inputs of joints so that the actuator may contain a reducer. Download: PPT PowerPoint slide PNG larger image TIFF original image Table 2. Simulation parameters. https://doi.org/10.1371/journal.pone.0273640.t002 Download: PPT PowerPoint slide PNG larger image TIFF original image Table 3. Simulation sets. https://doi.org/10.1371/journal.pone.0273640.t003 The trajectory planning results of Simulation 1∼6 are shown in Figs 5∼10, respectively. The comparisons of simulations in Figs 5∼10 are shown in Table 4. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 5. The trajectory planning results of Simulation 1 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g005 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 6. The trajectory planning results of Simulation 2 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g006 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 7. The trajectory planning results of Simulation 3 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g007 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 8. The trajectory planning results of Simulation 4 based on fifth order polynomial interpolation. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g008 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 9. The trajectory planning results of Simulation 5 based on the proposed optimization method. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g009 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 10. The trajectory planning results of Simulation 6 based on the proposed optimization method. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g010 Download: PPT PowerPoint slide PNG larger image TIFF original image Table 4. Comparisons of simulations. https://doi.org/10.1371/journal.pone.0273640.t004 Comparing Simulations 1, 2 & 5, it can be found that the maximums of joint angular accelerations are all inside the required angular acceleration of actuators specifications . It means the P control of in Eq (6) does not work in the three simulations. In Simulation 1, the maximum of joint angular velocity is larger than the required angular velocity of actuators specifications . In Simulation 2, the maximum of joint angular velocity is smaller than the required angular velocity of actuators specifications . In Simulation 5 based on the proposed optimization method, the maximum of joint angular velocity just equal to the required angular velocity of actuators specifications . That means the planning time should be increased in Simulation 1 and decreased in Simulation 2 to make time optimal and make actuators specifications satisfied, which is achieved by the time optimal controller Eq (6). Noting that the main effector is the maximum of joint angular velocity . Comparing Simulations 3, 4 & 6, it can be found that the maximum of joint angular velocity in Simulation 4 is inside the required angular velocity of actuators specifications , while the maximum of joint angular velocity in Simulation 3 is not. It means the P control of in Eq (6) does not work in Simulations 4, and the P control of in Eq (6) does work at the start of optimizations and does not work at the end of optimizations in Simulation 3, and only the P control of in Eq (6) does work at the end of optimizations in Simulation 3. In Simulation 3, the maximum of joint angular acceleration is larger than the required angular acceleration of actuators specifications . In Simulation 4, the maximum of joint angular acceleration is smaller than the required angular acceleration of actuators specifications . In Simulation 6 based on the proposed optimization method, the maximum of joint angular acceleration just equal to the required angular acceleration of actuators specifications . That means the planning time should be increased in Simulation 3 and decreased in Simulation 4 to make time optimal and make actuators specifications satisfied, which is achieved by the time optimal controller Eq (6) as well. Noting that the main effector is the maximum of joint angular acceleration . Comparing Simulations 1 & 2 with 3, 4, 5 & 6, it can be found that the trajectory planning based on seventh order polynomial interpolation can guarantee the planning jerk smooth and stable [37], but the trajectory planning based on fifth order polynomial interpolation cannot. Both of them can generate the continuous acceleration profile. Comparing Simulations 1 & 3 and Comparing Simulations 2 & 4, it can be found that the trajectory planning based on seventh order polynomial interpolation has higher maximum of joint angular velocity and higher maximum of joint angular acceleration than that of the trajectory planning based on fifth order polynomial interpolation. That’s the trade-off. Simulation 5 is the optimization results of Simulation 1 & 2, and Simulation 6 is the optimization results of Simulation 3 & 4. In Simulation 5, the optimization process of planning time T = tf − t0 with initial value in Simulation 1 (T = tf − t0 = 10s) and Simulation 2 (T = tf − t0 = 20s) are shown in Fig 11. In Simulation 6, the optimization process of planning time T = tf − t0 with initial value in Simulation 3 (T = tf − t0 = 10s) and Simulation 4 (T = tf − t0 = 20s) are shown in Fig 12. Actually, in Figs 11(b) and 12(b), the decreasing curve represents Case 1 and the planning time should be decreased to close to the optimal planning time, while the increasing curve in Figs 11 and 12 represents Case 2 and the planning time should be increased to close to the optimal planning time. Finally, the planning time is optimized as 15.908s in Simulation 5 and 18.106s in Simulation 6. Meanwhile, the actuators specifications are satisfied. It can be shown that, the optimizing loops are less than 10, which shows the trajectory optimization can be finished quickly. Noting that there exists an overshoot in the decreasing curve. The phenomenon is similar to P control in PID control. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 11. The optimization process of planning time T = tf − t0 in Simulation 5. (a) With initial value T = tf − t0 = 10s in Simulation 1, (b) With initial value T = tf − t0 = 20s in Simulation 2. https://doi.org/10.1371/journal.pone.0273640.g011 Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 12. The optimization process of planning time T = tf − t0 in Simulation 6. (a) With initial value T = tf − t0 = 10s in Simulation 3, (b) With initial value T = tf − t0 = 20s in Simulation 4. https://doi.org/10.1371/journal.pone.0273640.g012 To evaluate the performance of calculation efficiency of proposed optimization method, comparative simulations based on Simulation 5 are implemented with two iterative algorithms: GA [15] and PSO [17], and one hierarchical three-layer trajectory planning framework [29]. Compared with these two algorithms, the required time for trajectory optimization based on the proposed optimization method is much lower, which is about 19ms, while the GA and PSO algorithms would cost about 1326ms and 1764ms, respectively, and the hierarchical three-layer trajectory planning framework would cost about 42ms. Noting that the required time is the calculated time of PC to get the optimized trajectory, instead of the planning time. And, the proposed method combining event-trigger and conditional P control, GA, PSO and the hierarchical three-layer trajectory planning framework are employed to achieve the time-optimal trajectory planning individually. Therefore, the proposed optimization method owns higher efficiency of trajectory optimization. The planning trajectory based on the proposed optimization method in Simulation 5 can be drawn by Robotics Toolbox in Matlab, as shown in Fig 13, where the blue curve is the planning trajectory. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 13. The planning trajectory based on the proposed optimization method in Simulation 5. https://doi.org/10.1371/journal.pone.0273640.g013 4.2 Experiments The experimental platform, a manipulator, is shown in Fig 2a, and its system parameters are the same as simulation parameters Table 2. The maximum velocity and acceleration of actuators specifications of each joint of manipulator are shown in Table 5. The proposed optimization method is employed to do the trajectory planning: repeating the point to point trajectory pA → pB → pC → pD → pC → pB → pA. The trajectory planning can be divided into three point to point movements: pA ↔ pB, pB ↔ pC, pC ↔ pD. Download: PPT PowerPoint slide PNG larger image TIFF original image Table 5. Simulation parameters. https://doi.org/10.1371/journal.pone.0273640.t005 The trajectory planning results of experiment based on the proposed optimization method is shown in Fig 14. Some of experimental results are also shown in Table 5. In the initial panning time T = tf = 3s of every point to point movement without optimization, the maximum values of planning velocity and acceleration should be both smaller than the maximum velocity and acceleration of actuators specifications in each joints (Event 1), which means the panning time T = tf = 3s can be smaller to make the manipulator move faster and the actuators specifications can be satisfied simultaneously. In the optimal panning time with the proposed optimization method, the optimized planning time of pA → pB, pB → pC, pC → pD are 1.537s, 1.494s, 1.537s, respectively. And the maximums of joint angular velocities are all inside the required angular velocities of actuators specifications (see Fig 14(d)), while the maximums of joint angular accelerations just equal to the required angular accelerations of actuators specifications (see Fig 14(e)). As thus, the planning times of three point to point movements all reach their optimal values. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 14. The trajectory planning results of experiment based on the proposed optimization method. (a) Planning position, (b) Planning attitude, (c) Joint angle, (d) Joint angular velocity, (e) Joint angular acceleration, (f) Joint angular jerk. https://doi.org/10.1371/journal.pone.0273640.g014 The optimization process of planning time T = tf − t0 with initial value 3s of three point to point movements are shown in Fig 15. The decreasing curve represents Case 1 and the planning time should be decreased to close to the optimal planning time, while the increasing curve represents Case 2 and the planning time should be increased to close to the optimal planning time. Finally, the planning times of three movements of point to point are respectively optimized as 1.537s, 1.494s, 1.537s. Meanwhile, the actuators specifications are satisfied. It can be shown that, the optimizing loops are less than 10, which shows the trajectory optimization can be finished quickly. It makes room for the applications of proposed trajectory optimization on the micro controller with low computing capability and high real-time performance requirement. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 15. The optimization process of planning time T = tf − t0 in experiment. https://doi.org/10.1371/journal.pone.0273640.g015 5 Conclusions A method of time-optimal trajectory planning based on polynomial interpolation, event-trigger mechanism and conditional P control is proposed in this paper. The main contributions can be concluded as follows: The forward kinematics and inverse kinematics of a five axis manipulator are deduced. And, a simple method to choose one appropriate solution from multi solutions of inverse kinematics is proposed. Based on seventh order polynomial interpolation, event-trigger mechanism and conditional P control, an easy-implemented optimization method of trajectory planning is proposed to guarantee that the obtained trajectory is time optimal and satisfies actuators specifications. The effectiveness and efficiency of proposed optimization method are validated by comparative simulations and experiments. Future work will focus on easy-implemented optimization method of trajectory planning with more constraints and objectives. 6 Appendix A Forward kinematics The transformation matrix from coordinate system {i-1} (oi−1xi−1yi−1zi−1) to coordinate system {i} (oixiyizi) can be written as (7) Substituting the DH parameters into the transformation matrix Eq (7), the homogeneous transformation matrix between each coordinate system can be obtained as (8) After obtaining the homogeneous transformation matrix between coordinate system, the homogeneous transformation matrix between the coordinate system of end effector {5} and the base coordinate system {0} can be obtained through the chain rule as (9) where, Set the coordinate system {0} in the first axis as the base/reference coordinate system of manipulator, then the pose transformation matrix from the end effector to the base is shown in (9). In the forward kinematics, the position parameters x, y, z and attitude parameters α, β, γ of the end effector are dependent variables, while the joint angles θi(i = 1, 2, 3, 4, 5) are independent variables. According to the chain rule, the forward kinematics and its analytical solutions of the manipulator can be directly obtained as shown in (9), where are the parameters x, y, z of end effector of manipulator on the base coordinate system {0}, that is (10) are the attitude related parameters of end effector of manipulator on the base coordinate system {0}. Based on the rotation order: Z-Y-X, the attitude parameters (Euler angles) α, β, γ can be obtained as (11) where arctan2(Y, X) is a C function, which returns the arctangent of Y/X in radians. The sign of the values of Y and X determines the correct quadrant. B Inverse kinematics In the inverse kinematics, the joint angles θi(i = 1, 2, 3, 4, 5) are dependent variables, while the position parameters x, y, z and attitude parameters α, β, γ of end effector of manipulator are independent variables. Because the end effector of 5 axis manipulator has only 5 DoFs in Cartesian space, only 5 parameters of manipulator (3 position parameters and 2 attitude parameters) can be determined by given 5 joint angles. And the last one attitude parameter is not an independent variable, which can be calculated via the forward kinematics Eqs (8)–(11). Set the five parameters of manipulator: position parameters px, py, pz, the rotation angle α around x0 axis and β around y0 axis in the base coordinate system {0} can be determined by joint angles θi(i = 1, 2, 3, 4, 5), then the rotation angle γ around z0 axis in the base coordinate system {0} is the non-independent variable, which can be calculated via the forward kinematics Eqs (8)–(11). Thus, given five parameters of manipulator, the five joint angles θi(i = 1, 2, 3, 4, 5) can be deduced as follows. Rewriting Eq (9) yields (12) If the first column of matrix Eq (12) is equal, the following equations can be obtained (13) (14) (15) where θ234 = θ2 + θ3 + θ4. If the fourth column of matrix Eq (12) is equal, the following equations can be obtained (16) (17) (18) where θ23 = θ2 + θ3. The first joint angle θ1 can be obtained via Eq (18). And there exists two solutions, which are respectively (19) Combining Eq (19) with Eq (15), the joint angle θ5 can be obtained. And there also exists two solutions, which are respectively (20) Combining the calculated joint angle θ1 in Eq (19), θ5 in Eq (20) with Eqs (13) and (14), the unique solution of θ234 can be obtained as (21) Based on Eqs (19)–(21), the joint angles θ2, θ3 can be obtained via Eqs (16) and (17). Eqs (16) and (17) can be rewritten as (22) where k1 = px cos θ1 + py sin θ1 + d5 sin θ234, k2 = pz − d1 − d5 cos θ234, and k1, k2 are known. Then the joint angle θ3 can be obtained as (23) By solving the sum of squares of Eq (22), the joint angle θ2 can be obtained as (24) Finally, the last joint angle θ4 can be obtained as (25) C Proof of Theorem 1 In the point to point trajectory planing based on polynomial interpolation, the planning time is in inverse proportion to the maximum velocity of planning trajectory. From common sense, since the distance between start knot and final knot is constant, if the planning time increases, then the average velocity and maximum velocity will decrease generally. In theory, take a point to point trajectory planing based on third order polynomial interpolation as an example, and assume that there are two planning time (t1, t2 and t1 < t2) for the given trajectory, then the maximum velocity with planning time t1 is larger than that with planning time t2. To prove it, the detailed geometric description can be shown in Fig 16, where, v1 = b10 + b11t + b12t2 and v2 = b20 + b21t + b22t2 are the velocities of trajectory planing based on third order polynomial interpolation with planning time t1 and t2, respectively, v1max, v2max are the maximum velocities of v1, v2, respectively, S1, S2 are the enclosure areas between the horizontal axis T and curves v1, v2, respectively, which both represent the constant distance between start knot and final knot. Download: PPT PowerPoint slide PNG larger image TIFF original image Fig 16. The velocity curves of trajectory planing based on third order polynomial interpolation with planning time t1 and t2. https://doi.org/10.1371/journal.pone.0273640.g016 As thus, the proposition can be set as follow. In the point to point trajectory planing based on third order polynomial interpolation, if t1 < t2, then v1 max > v2 max. Proof: Assume that t2 = λt1, λ > 1. Since S1 = S2, then it yields (26) From the 16, the maximum values of v1, v2 can be respectively written as (27) Combining (26) and (27), it yields (28) Since t2 = λt1, λ > 1, then v1 max > v2 max. The trajectory planing based on other n-th order, including the seventh order polynomial interpolation, can be analyzed in the same way and the same conclusion can be obtained. D Proof of Theorem 2 Assume that the system state equation can be written as (29) where x1 donates the maximum velocity of trajectory planning. For the system, the desired output is x1d, which is the same as maximum velocity of actuator specifications, and u(T) is the control input with respect to planning time T. The control objective is to obtain an optimal planning time Topt to make the maximum velocity of trajectory planning equal to the maximum velocity of actuator specifications. Set the error between the maximum velocity of trajectory planning and the maximum velocity of actuator specifications as (30) then it yields (31) Let the nonnegative Lyapunov function as (32) then combining Eqs (29) and (31) yields (33) Since , then the control input can be written in P control form as (34) where kp is the coefficient of P control. And then, (35) Therefore, the P control could guarantee the stability of time optimal controller and the converge of optimization. The maximum acceleration term in the conditional P control Eq (6) can be deduced in the same way. By adding the event-trigger, this is the reason why condition P control is employed.
TI - Time-optimal trajectory planning based on event-trigger and conditional proportional control
JF - PLoS ONE
DO - 10.1371/journal.pone.0273640
DA - 2023-01-30
UR - https://www.deepdyve.com/lp/public-library-of-science-plos-journal/time-optimal-trajectory-planning-based-on-event-trigger-and-rZMWkPdfCd
SP - e0273640
VL - 18
IS - 1
DP - DeepDyve
ER -