arXiv:1610.02881v2 [cs.RO] 9 Dec 2016 Essential Properties of Numerical Integration for Time-optimal Trajectory Planning Along a Specified Path Peiyao Shen, Xuebo Zhang and Yongchun Fang Abstract — This letter summarizes some known properties and also presents several new properties of the Numerical Integration ( NI ) method for time-optimal trajectory planning along a speci- fied path. The contribution is that rigorous mathematical proofs of these properties are presented, most of which have not been reported in existing literatures. We first give some properties regarding switch points and accelerating/decelerating curves of the NI method. Then, for the fact that when kinematic constraints are considered, the original version of NI which only considers torque constraints may result in failure of trajectory planning, we give the concrete failure conditions with rigorous mathematical proof. Accordingly, a failure detection algorithm is given in a ‘run-and-test’ manner. Some simulation results on a unicycle vehicle are provided to verify those presented properties. Note that though those known properties are not discovered first, their mathematical proofs are given first in this letter. The detailed proofs make the theory of NI more complete and help interested readers to gain a thorough understanding of the method. Index Terms — Time-optimal trajectory planning, Numerical Integration, Properties with rigorous proofs. I. I NTRODUCE Due to low computational complexity, decoupled planning [1]–[3] becomes a popular motion planning method, which consists of two stages. In the first stage, path planning methods are used to generate a geometric path with high level constraints including obstacle avoidance, curvature, and so on. In the second stage, trajectory planning, which aims to assign a motion time profile to the specified path, could be then simplified as a planning problem in two-dimensional path parametrization space ( s , ̇ s ), with s and ̇ s being the path coordinate and path velocity respectively. To improve working efficiency, several time-optimal methods have been reported for the trajectory planning stage along a specified path, including Dynamic Programming [4]–[6], Convex Op- timization [7]–[9] and NI [10]–[22]. In addition, the work in [23] proposes a real-time trajectory generation approach for omni-directional vehicles by constrained dynamic inversion. In this letter, we will focus on the NI method, since its computational efficiency is shown in [13] to be better than other methods. The original version of NI is proposed at almost the same time in the works [10], [11], which aims to give a time-optimal trajectory along a specified path in the presence of only torque constraints for manipulators. This work is supported in part by National Natural Science Foundation of China under Grant 61573195 and Grant 613205017. The authors are with the Institute of Robotics and Automatic Information System, Nankai University, Tianjin 300071, China, and also with the Tianjin Key Laboratory of Intelligent Robotics, Nankai University, Tianjin 300071, China (e-mail: zhangxuebo@nankai.edu.cn) Based on Pontryagin Maximum Principle, NI possesses a bang-bang structure of torque inputs, thus, the core of NI is the computation of switch points. The accelerating and decelerating curves, integrated from those switch points, constitute the time-optimal trajectory. The work in [12] presents three types of switch points: tangent, discontinuity and zero-inertia points. The detection methods for zero- inertia switch points are presented in [13]–[15]. Based on previous works, Pham [13] provides a fast, robust and open- source implementation for NI in C++/Python, which is subse- quently extended to the case of redundantly-actuated systems in the work [16]. In addition to manipulators [17], [18], NI is also applied to spacecrafts [19] and humanoid robots [20]. In real applications, in addition to torque constraints, velocity constraints (bounded actuator velocity and path velocity) should also be considered. Under these constraints, the works in [21], [22] indicate that the original NI method with only torque constraints [10] can not be directly applied. Considering velocity and acceleration constraints, the work [18] focuses on detecting tangent, discontinuity and zero- inertia switch points on the speed limit curve decided by velocity constraints. However, rigorous proofs have not been reported to expose the conditions and underlying reasons of failure cases in aforementioned literatures. In this brief, we summarize some known properties and amend corresponding mathematical proofs which have not been reported in existing literatures; in addition, some new properties are excavated and proven rigorously. Some proper- ties indicate the evolution of accelerating/decelerating curves and their intersection points. And another important property indicates that, when kinematic constraints are considered, the original version of NI which only considers torque con- straints, may result in failure of trajectory planning tasks. For this property, we first give the concrete failure conditions and detailed proofs. Accordingly, the failure detection algorithm is given in a ‘run-and-test’ manner. Simulation results on a unicycle vehicle are provided to verify these properties. The main contribution of this letter is the rigorous and detailed proofs for all presented properties: 1) For Properties 2-3 reported in [24], we first give their mathematical proofs in Section III-A. 2) Some new properties are presented and proven, includ- ing Property 4 in Section III-B and Properties 5-6 in Section III-C. These properties and proofs make the theory of NI more complete and help interested readers to gain an thorough understanding of the NI method. The remainder of this letter is divided into four sections.
Section II introduces notations and procedures of the NI method. Section III presents essential properties of the NI method, and provides rigorous mathematical proofs. In Section IV, simulation results are provided to verify the properties. Finally, Section V gives some conclusions. II. N UMERICAL I NTEGRATION In this section, we briefly introduce the notations and procedures of the NI method in [10], [11]. For the time- optimal path-constrained trajectory planning problem, based on Pontryagin Maximum Principle, the original NI method [10], [11] uses a bang-bang structure of torque input to generate a time-optimal trajectory. First, torque constraints are converted to path acceleration constraints along the given path. Then, switch points of path acceleration are found. Finally, a time-optimal trajectory is integrated numerically with maximum and minimum path acceleration. A stable and open-source implementation of NI method can be found in [13], and kinematic constraints are handled in the im- plementation with the method proposed in [21]. Yet, in the presence of kinematic constraints, the rigorous proof showing conditions of failure of the original NI with only torque constraints, has not been reported. A. Time-optimal Path-constrained Trajectory Planning The time-optimal path-constrained trajectory planning problem is to find a time-optimal velocity profile along the given path for a robot under various constraints such as torque constraints, velocity constraints, and so on. Note that the direction of the path velocity is supposed to be tangent to the given path. B. Notations In order to explain various notations clearer, we will refer to Fig. 1 in the following. s 0 e s 0 s e 0 1 1 ! " M VC s sp ! " sp ! " s e s Fig. 1. Red solid curves β 0 , β 1 represent accelerating β - pro f iles . Green solid curves α 1 , α e represent decelerating α - pro f iles . Red ⊲ and green ⊳ denote the points sp β → α and sp α → β , respectively. The scalars ̇ s 0 , ̇ s e are respectively the starting and terminal path velocity at the endpoints of the given path. The s e is the total length of the given path. s : Path coordinate along a specified path. ̇ s : Path velocity along a specified path. ̈ s : Path acceleration along a specified path. A A A ( s ) ̈ s + B B B ( s ) ̇ s 2 + C C C ( s ) ≤ 0 0 0 [10]: The inequality constraint along a specified path for s , ̇ s , ̈ s , which is derived from torque constraints. As an example, for an n -dof manipulator, in order to guarantee torque constraints in a path-constrained trajectory planning task, the inequality constraint [10] M ( ξ ξ ξ ) ̈ ξ ξ ξ + ̇ ξ ξ ξ T P ( ξ ξ ξ ) ̇ ξ ξ ξ + Q Q Q ( ξ ξ ξ ) ≤ 0 0 0 (1) should hold, where the state ξ ξ ξ is an n -dimensional vector, M is an m × n matrix, P is an n × m × n tensor and Q Q Q is an m -dimensional vector. Along the specified path, the robot state and its differentials are ξ ξ ξ = ξ ξ ξ ( s ) , ̇ ξ ξ ξ = ξ ξ ξ s ̇ s , ̈ ξ ξ ξ = ξ ξ ξ ss ̇ s 2 + ξ ξ ξ s ̈ s (2) where ξ ξ ξ s = d ξ ξ ξ / ds , ξ ξ ξ ss = d ξ ξ ξ s / ds . After substituting (2) into (1), we obtain that A A A ( s ) ̈ s + B B B ( s ) ̇ s 2 + C C C ( s ) ≤ 0 0 0 , (3) with A A A ( s ) = M ( ξ ξ ξ ( s )) ξ ξ ξ s ( s ) , B B B ( s ) = M ( ξ ξ ξ ( s )) ξ ξ ξ ss ( s ) + ξ ξ ξ s ( s ) T P ( ξ ξ ξ ( s )) ξ ξ ξ s ( s ) , C C C ( s ) = Q Q Q ( ξ ξ ξ ( s )) . α ( s , ̇ s ) , β ( s , ̇ s ) : In order to guarantee torque constraints, the scalars s , ̇ s , ̈ s should satisfy the inequality (3). Therefore, given the path coordinate s and path velocity ̇ s , the path acceleration ̈ s satisfies the following inequality α ( s , ̇ s ) ≤ ̈ s ≤ β ( s , ̇ s ) , (4) where the minimum path acceleration α ( s , ̇ s ) and maximum path acceleration β ( s , ̇ s ) are computed as α ( s , ̇ s ) = max { α i | α i = − B i ( s ) ̇ s 2 − C i ( s ) A i ( s ) , A i ( s ) < 0 } , (5) β ( s , ̇ s ) = min { β i | β i = − B i ( s ) ̇ s 2 − C i ( s ) A i ( s ) , A i ( s ) > 0 } , (6) wherein the integer i ∈ [ 1 , m ] , with m being the dimension of the vector A A A . Please see the detailed description in [11] . MVC : The maximum velocity curve in the plane ( s , ̇ s ) is represented as MVC ( s ) = min { ̇ s ≥ 0 | α ( s , ̇ s ) = β ( s , ̇ s ) } , s ∈ [ 0 , s e ] . (7) For instance, the cyan dash curve in Fig. 1 is MVC . If the robot state is on the MVC , there exists at least one saturated actuator torque. AR : The admissible region, in the plane ( s , ̇ s ) , is enclosed by the curve MVC and the lines ̇ s = 0 , s = 0 , s = s e . Within the AR except for the boundary MVC , all actuator torques are between lower and upper bounds ( α ( s , ̇ s ) < β ( s , ̇ s ) ). α - pro f ile : The decelerating curve, in the plane ( s , ̇ s ) , is integrated backward with minimum acceleration α ( s , ̇ s ) in (5). The slope k α is computed as k α = d ̇ s / ds = α ( s , ̇ s ) / ̇ s . β - pro f ile : The accelerating curve, in the plane ( s , ̇ s ) , is integrated forward with maximum acceleration β ( s , ̇ s ) in (6). The slope k β is computed as k β = d ̇ s / ds = β ( s , ̇ s ) / ̇ s . sp α → β [12]: Switch points from decelerating to acceler- ating curves, such as the green ⊳ in Fig. 1. They are on the MVC curve, and there are three different types: tangent, discontinuity or zero-inertia points. At tangent points, the slope k mvc = d ̇ s / ds of MVC is equal to k α (= k β ) . At
discontinuity points, MVC is discontinuous. At zero-inertia points, at least one A i ( s ) = 0 holds for the corresponding path coordinate s . sp β → α : Switch points from accelerating to decelerating curves, such as the red ⊲ in Fig. 1. C. Numerical Integration Algorithm Procedures of the original version of NI [10] which only considers torque constraints are given as follows: NI-1. In the plane ( s , ̇ s ) , starting from ( s = 0 , ̇ s = ̇ s 0 ) , the accelerating curve β - pro f ile is integrated forward with maximum path acceleration β ( s , ̇ s ) until one of the following cases occurs: • the curve MVC is hit, and go to NI-2 ; • the line ̇ s = 0 is hit, and output that this path is not traversable; • the line s = s e is hit, and go to NI-3 . NI-2. From the hitting point, searching forward along MVC , the first tangent, discontinuity or zero-inertia point found is sp α → β . • If sp α → β is detected, from the switch point, an α - pro f ile is integrated backward with α ( s , ̇ s ) until it intersects the generated β - pro f ile in NI-1, NI-2 at a point sp β → α , and one new β - pro f ile is integrated forward as NI-1 . • If sp α → β is not detected, go to NI-3 . NI-3. Starting from ( s = s e , ̇ s = ̇ s e ) , the decelerating curve α - pro f ile is integrated backward with minimum path acceleration α ( s , ̇ s ) until it intersects the gen- erated β - pro f ile in NI-1, NI-2 at a point sp β → α . Finally, output a time-optimal trajectory consisting of those generated accelerating and decelerating curves in NI-1, NI-2 and NI-3 . Note that chattering or vibration is usually caused by high- frequent switching of acceleration. Fortunately, the work in [10] has indicated that switch points are finite for the NI method, which generally does not cause severe chattering phenomenons. III. P ROPERTIES In this section, essential properties of NI are provided with rigorous mathematical proofs. Note that Property 1 has been presented and proven in [24]; Properties 2-3 have been re- ported in [24], and we will amend their mathematical proofs; Properties 4-5 are first exposed in this letter with rigorous proofs; in the presence of kinematic constraints, the failure of the original NI with only torque constraints is reported in [21], [22], and we will give the mathematical conditions and underlying reasons of failure cases in Property 6 . See the proofs of Properties 2-5 in Appendix A-D, respectively. A. Property of α -pro f iles and β -pro f iles In the admissible region except for the boundary MVC ( AREM ), the inequality α ( s , ̇ s ) < β ( s , ̇ s ) holds, which indi- cates k α < k β for each point. Three known properties are summarized in the following, and we will give rigorous s s profile ! profile ! 1 X m X 1 m * m Y 0 sp ! " sp ! " M VC 1 1 Y D Fig. 2. m > 1 : Intersection points X i , i ∈ [ 1 , m ] are on β ∗ , wherein X j , 1 < j < m is on X 1 X m ⌢ . Each X i has one corresponding decelerating curve α i and one switch point Y i . The region D is enclosed by α 1 , β ∗ and MVC . s s 0 ! M VC s † M VC 1 M VC 2 M VC 3 M VC ! V s p sp ! " 1 o 2 o Fig. 3. The maximum velocity curve is altered from MVC = MVC 1 + MVC 2 + MVC 3 to MVC ∗ = MVC 1 + MVC † + MVC 3 . mathematical proofs for Properties 2-3 , which have not been reported in existing literatures. Property 1: In the AREM region, any two α - pro f iles never intersect with each other. Neither do β - pro f iles . (Please see the proof in [24].) Property 2: In the AREM region, if an α - pro f ile inter- sects another β - pro f ile at a point ( s = s c , ̇ s = ̇ s c ) , in terms of path velocity, the α - pro f ile is greater than the β - pro f ile in the left neighborhood of s c , but less than the β - pro f ile in the right neighborhood of s c . Property 3: In the AREM region, an α - pro f ile is not tangent to another β - pro f ile at any point. B. Property of sp β → α The point sp β → α is the intersection point between α - pro f ile and β - pro f ile , denoted as the red ⊲ in Fig. 2. In the iterative process of NI (see Section II-C), a β - pro f ile may intersect a finite number of integrated back- ward α - pro f iles [10]. Which one of these intersection points is finally chosen as sp β → α on the β - pro f ile ? The answer is given in Property 4 , which is first presented in this letter with rigorous proof. Property 4: If one β - pro f ile intersects m ≥ 1 α - pro f iles , respectively at points X i , i ∈ [ 1 , m ] , and in terms of path coordinate, X i is less than X j , 1 ≤ i < j ≤ m , then, X 1 is finally chosen as sp β → α on the β - pro f ile after finite iterations. Remark 1: In Fig. 2, Y 1 may be the terminal point ( s e , ̇ s e ) . In this special situation, Property 4 still holds. The case m = 1 obviously holds. Meanwhile, the case m > 1 also holds since the trajectory starting from X i , i > 1 cannot arrive at
the terminal point according to Properties 1-3 . In addition, other points Y i , i > 1 cannot be the terminal point ( s e , ̇ s e ) since NI has completed all procedures at the terminal point (see the procedure NI-3 in Section II-C). The Properties 1-4 show the evolution of accelerat- ing/decelerating curves and their intersection points. C. Property of NI with kinematic constraints The original NI method [10] generates a time-optimal trajectory with bounded torque. However, when velocity constraints (bounded actuator velocity and path velocity) are taken into account, the original NI (as shown in Section II-C) may result in a failure. Several examples for this property are shown in the work in [21], [22], but the concrete failure conditions and the detailed proof have not been reported. In the subsequent Property 6 , we will elaborate the failure conditions and provide the mathematical proof. Note that Property 5 is first presented and proven to indicate the existence of tangent switch points on the limit curve considering velocity constraints, which is used in the proof of Property 6 . Actuator velocity constraints are transformed into path velocity constraints by kinematic models of robots, therefore, velocity constraints are represented as V ( s ) : s → ̇ s , s ∈ [ 0 , s e ] , (8) where the scalar V ( s ) is the maximum path velocity. Due to the velocity constraints, the maximum velocity curve is altered as MVC ∗ ( s ) = min ( MVC ( s ) , V ( s )) , s ∈ [ 0 , s e ] . (9) In the region enclosed by MVC ∗ , s = 0 , s = s e , ̇ s = 0, acceler- ating and decelerating curves satisfy all velocity and torque constraints. In addition, the part of MVC ∗ , which is different from MVC , is represented as MVC † ( s )= { MVC ∗ ( s ) | MVC ∗ ( s ) < MVC ( s ) , s ∈ [ 0 , s e ] } . (10) For instance, the dash-dot curve o 1 o 2 ⌢ is MVC † in Fig. 3. The maximum velocity curve altering from MVC to MVC ∗ results in a decreasing number of tangent switch points. For instance, the tangent switch point p disappears due to MVC † instead of MVC 2 in Fig. 3. Property 5: Tangent switch points are nonexistent on MVC † . The NI method [10] with torque constraints, generates a time-optimal trajectory T . After considering velocity con- straints V ( s ) , the maximum velocity curve is altered from MVC to MVC ∗ , which causes that tangent switch points on the part MVC † of MVC ∗ are nonexistent according to Property 5 . It has negative effects on searching switch points in the NI method, and may result in failure of trajectory planning tasks. Property 6: If the following conditions hold: C 1 : ∃ s ∗ ∈ [ 0 , s e ] , MVC ∗ ( s ∗ ) < T ( s ∗ ) , C 2 : discontinuity and zero-inertia switch points on MVC † are nonexistent, β − α − ( ) ( ) β α β α Fig. 4. Left: β - pro f ile , Right: α - pro f ile β − α − ( ) ( ) β α α β β Fig. 5. Left: β - pro f ile , Right: β - pro f ile β − α − ( ) ( ) β α α α β Fig. 6. Left: α - pro f ile , Right: α - pro f ile β − α − ( ) ( ) β α α α β β Fig. 7. Left: α - pro f ile , Right: β - pro f ile
then, the NI method using MVC ∗ fails to find out a feasible trajectory. Proof: The proof is proceeded in 4 steps. Step 1: To show four essential cases of C 1 . The time-optimal trajectory T , which is obtained from the NI method using MVC , consists of several α - pro f iles and β - pro f iles , satisfying T ( s ) ≤ MVC ( s ) , s ∈ [ 0 , s e ] . According to (10) and C 1 , there must exist one part of MVC † below T , and this part is called as MVC ‡ . On both sides of MVC ‡ , the trajectory T could be accelerating or decelerating, thus there are four essential cases for the condition C 1 as shown in Figs. 4-7. The purple dash-dot line o 3 o 4 ⌢ represents MVC ‡ . The gray dotted line o 3 o 4 and red/green solid lines constitute the trajectory T . The green ⊳ and red ⊲ are sp α → β and sp β → α points respectively. These essential cases are regarded as basic components of other complex cases, thus, for clarify, the proof of complex cases will be presented in Remark 2 . Step 2: To prove that for each essential case, the NI method using MVC ∗ must run to the procedure NI-2 in Section II- C, which is to search forward switch point along MVC † (the purple dash-dot curve o 1 o 2 ⌢ ) from the hitting point . Let p 1 be the neighborhood sp α → β of T on the left side of MVC † , which also can be p 1 = ( 0 , ̇ s 0 ) . Starting from p 1 , NI integrates β 1 forward. For essential cases as Figs. 4-5, the accelerating curve β 1 hits MVC † , then NI will search forward sp α → β along MVC † from the hitting point of o 1 o 3 ⌢ . For essential cases as Figs. 6-7, all accelerating curves including β 1 and β - pro f iles from p 1 o 1 ⌢ cannot intersect α 3 for Property 2 , thus, it must intersect p 1 o 1 ⌢ or o 1 o 3 ⌢ . If the hit curve is o 1 o 3 ⌢ , then NI will search forward sp α → β from the hitting point along MVC † . If the hit curve is p 1 o 1 ⌢ , then NI will search forward sp α → β from the hitting point along p 1 o 1 ⌢ . The number of switch points on p 1 o 1 ⌢ is finite [10], therefore switch points on p 1 o 1 ⌢ will run out, and NI will go on searching forward sp α → β along MVC † . Step 3: To prove that those α -pro f iles, starting from the right side of o 4 , cannot intersect β -pro f iles, which are on the left side of o 3 and generated in iterative process of NI. Based on Property 5 and condition C 2 of Property 6 , switch points on MVC † are nonexistent. Therefore, NI will go on searching forward switch points on the right side of o 2 . In Figs. 4-7, the integrated backward α - pro f ile , starting from the sp α → β of o 2 p 2 ⌢ , cannot intersect α 2 , β 3 according to Properties 1-2 , thus, it must hit the purple dash-dot o 4 o 2 ⌢ or cyan dash o 2 p 2 ⌢ . Moreover, in the admissible region, starting from the right side of p 2 , integrated backward α - pro f iles cannot intersect those β - pro f iles generated by NI and on the left side of o 3 , which is proven by contradiction. Assume that in the admissible region, from the right side of p 2 , an integrated backward α - pro f ile intersects one of those β - pro f iles . Then, the α - pro f ile should be part of the trajectory T based on Property 4 . However, in terms of path velocity, the α - pro f ile is less than MVC ∗ , which contradicts with C 1 . Therefore, this assumption is invalid, and α - pro f iles from the right side of p 2 cannot intersect those β - pro f iles generated on the left side of o 3 . In addition, if p 2 is the terminal point ( s e , ̇ s e ) , then starting from p 2 and switch points at the right side of o 2 , all integrated backward α - pro f iles must hit the purple dash-dot o 4 o 2 ⌢ or cyan dash o 2 p 2 ⌢ according to Properties 1-3 , which also indicates that this step holds. Step 4: To synthesize above analysis and indicate failure of trajectory planning tasks. The above Step 1-3 indicate that if conditions of Property 6 hold, then the NI method using MVC ∗ fails to output a feasible trajectory: in the iterative process, α - pro f iles and β - pro f iles on two different sides of MVC ‡ do not intersect in the admissible region, which causes that the final trajectory is incomplete. In summary, Property 6 holds. Remark 2: For other complex cases mentioned in Step 1 , there may exist many parts of MVC † below T . The trajectory T could be accelerating or decelerating at the first and last part as Step 1 . In these cases, NI still searches switch points along MVC † from the first part in the same reason as Step 2 . The condition C 2 indicates that the sp α → β on MVC † is nonexistent. Starting from switch points at the right side of the last part, all α - pro f iles cannot intersect β - pro f iles at the left side of the first part as Step 3 . Therefore, in these complex cases, α - pro f iles and β - pro f iles on two different sides of MVC † do not intersect in the admissible region. It indicates that Property 6 still holds for complex cases. In the presence of velocity and torque constraints, Property 6 actually gives sufficient conditions for failure of the NI method in [10], which is important because it is theoretically shown that the failure cases indeed exists for this method. In the following, a necessary and sufficient failure condition is given by a numerical ‘run-and-test’ algorithm ( RT ): RT-1. In the plane ( s , ̇ s ) , the curve MVC ∗ ( s ) is ob- tained with (9). It is initialized that the point p is ( 0 , ̇ s 0 ) , boolean variable isContinuous is T RUE , the longest continuous trajectory T ∗ from ( 0 , ̇ s 0 ) is null and the scalar sLast is zero. Then go to RT-2 . RT-2. Starting from the point p , a β - pro f ile is integrated forward until it hits the boundaries of the admis- sible region at ( s = s h , ̇ s = ̇ s h ) . If isContinuous is T RUE , then RT will call the subfunction addPro f ile ( T ∗ , β - pro f ile ), and sLast is updated as s h . And go to RT-3 . RT-3. From the hitting point, the first switch point found along MVC ∗ or terminal point is assigned to the point q . Starting from q , an α - pro f ile is integrated backward until it intersects one of following lines: • The trajectory T ∗ , then RT will call the sub- function addPro f ile ( T ∗ , α - pro f ile ) and set isContinuous = TRUE . Meanwhile, sLast is up- dated as the path coordinate of q . • The boundaries of the admissible region, then RT will set isContinuous = FALSE . The point p is updated as q . If p is the terminal point, then go to RT-4 , else go to RT-2 . RT-4. If sLast < s e , this method fails to generate a feasible trajectory, else output a feasible trajectory T ∗ .
Remark 3: The subfunction addPro f ile adds accelerat- ing/decelerating curves to T ∗ . The core of this algorithm is running the NI method and detecting whether those generated accelerating and decelerating curves constitute a continuous trajectory on the whole path. The scalar sLast indicates the path length of the trajectory T ∗ . Therefore, after the algorithm is completed, the inequality sLast < s e in RT-4 is a necessary and sufficient failure condition for the NI method in presence of velocity and torque constraints. IV. S IMULATION R ESULTS In order to verify Properties 1-6 , some simulation results on a unicycle vehicle are provided in this section. A. Model A unicycle vehicle moves along a specified path, with the direction of the vehicle being always tangent to this given path from a initial pose to a target pose. The angular velocity ω ∈ R , the path velocity v ∈ R , and the path curvature κ ∈ R have the following relationship [25]: ω = κ v . (11) For the specified path, the curvature could be computed as a function of the path coordinate as follows: κ : s ∈ [ 0 , s e ] → κ ( s ) ∈ R , (12) where the scalar s is the path coordinate. According to (11) and (12), the kinematic model of the vehicle is described as u u u = M M M ( s ) ̇ s , (13) where u u u = [ ω v ] T , and M M M ( s ) = [ κ ( s ) 1 ] T . Taking the time derivative of (13) yields that ̇ u u u = M M M ( s ) ̈ s + M M M s ( s ) ̇ s 2 , (14) where ̇ u u u = [ ̇ w ̇ v ] T , M M M s ( s ) = [ κ s 0 ] T , κ s = d κ / ds . The scalars ̇ v , ̇ w are the path acceleration and angular acceleration, re- spectively. Velocity and acceleration constraints are given as − v v v max ≤ u u u ≤ v v v max , (15) − a a a max ≤ ̇ u u u ≤ a a a max , (16) where v v v max ∈ R 2 and a a a max ∈ R 2 are constant vectors repre- senting the velocity boundary and the acceleration boundary, respectively. These vector inequalities (15)-(16) should be interpreted componentwise. In order to guarantee acceleration constraints, substituting (14) into (16) yields that A A A ( s ) ̈ s + B B B ( s ) ̇ s 2 + C C C ( s ) ≤ 0 0 0 , (17) where A A A ( s ) = [ M M M ( s ) T − M M M ( s ) T ] T , B B B ( s ) = [ M M M s ( s ) T − M M M s ( s ) T ] T and C C C ( s ) = − [ a a a T max a a a T max ] T , which are all 4 × 1 vectors. In order to guarantee velocity constraints, substituting (13) into (15) yields that A A A ( s ) ̇ s + D D D ( s ) ≤ 0 0 0 , (18) where A A A ( s ) = [ M M M ( s ) T − M M M ( s ) T ] T and D D D ( s ) = − [ v v v T max v v v T max ] T , which are all 4 × 1 vectors. 0 0.5 1 1.5 2 2.5 3 −0.5 0 0.5 1 1.5 2 2.5 3 x [ m ] y [ m ] path starting position terminal position Fig. 8. The specified path: cubic B ́ ezier curve The velocity limit curve MVC ( s ) considering acceleration constraints is computed with (7) and (17), and the limit curve V ( s ) considering velocity constraints is computed with (8) and (18). When both acceleration and velocity constraints are taken into account, the maximum velocity curve is obtained as MVC ∗ by fusing MVC ( s ) and V ( s ) with (9). B. Results As shown in Fig. 8, the specified blue path is the following cubic B ́ ezier curve: x =( 1 − λ ) 3 x 0 + 3 ( 1 − λ ) 2 λ x 1 + 3 ( λ 2 − λ 3 ) x 2 + λ 3 x 3 , y =( 1 − λ ) 3 y 0 + 3 ( 1 − λ ) 2 λ y 1 + 3 ( λ 2 − λ 3 ) y 2 + λ 3 y 3 , where the position of the vehicle is ( x [ m ] , y [ m ]) , the points ( x i [ m ] , y i [ m ]) , i ∈ [ 0 , 3 ] are path control points, and the scalar λ ∈ [ 0 , 1 ] is the path parameter. The λ and s obey a nonlinear scaling relation. The starting and terminal path velocity ̇ s 0 = ̇ s e = 0 [ m / s ] . This cubic B ́ ezier curve is used to find a path from a initial pose to a target pose so that the orientation of the unicycle vehicle is always tangent to the path, and thus the nonholonomic constraint is satisfied. The following cases show that the NI method assigns velocity profiles to the path. Case 1: This case shows that, when the velocity con- straints are moderate, the NI method [10] using MVC ∗ generates the time-optimal trajectory. In this simulation, the velocity and acceleration constraints are set as v v v max = [ 0 . 5 rad / s 1 . 3 m / s ] T , a a a max = [ 0 . 05 rad / s 2 0 . 1 m / s 2 ] T . In Fig. 9, the cyan dash line represents MVC ( s ) with ac- celeration constraints. When considering velocity constraints corresponding to the purple dash-dot line V ( s ) in Fig. 9, the maximum velocity curve is altered from MVC to MVC ∗ , which is represented as the boundary between the gray (inadmissible) and blank (admissible) regions. To facilitate subsequent analysis, symbols # 1 , # 2 are used to represent the two closed areas bounded by MVC ( s ) and V ( s ) . The red and green solid lines are accelerating curves ( β - pro f iles ) and decelerating curves ( α - pro f iles ), respectively, which comply with Properties 1-3 . The red ⊲ and green ⊳ denote the points sp β → α and sp α → β respectively (see Section II-B). As shown in Fig. 9, under the curve MVC ∗ , the NI method outputs the optimal trajectory as follows. The accelerating curve β 0 , starting from ( 0 , ̇ s 0 ) , hits MVC ∗ at p 1 . Searching forward along MVC ∗ from p 1 , the first sp α → β found is p 2 .
0 1 2 3 4 5 6 0 0.2 0.4 0.6 0.8 1 1.2 1.4 s [ m ] ̇ s [ m/s ] o 1 α 1 p 2 ο 3 α 2 α e β 3 inadmissible region V(s) p 1 ο 2 MVC(s) β 0 # 2 # 1 admissible region β 1 p 3 p 4 p 5 Fig. 9. Case 1: The NI method outputs the optimal trajectory with constraints v v v max = [ 0 . 5 rad / s 1 . 3 m / s ] T , a a a max = [ 0 . 05 rad / s 2 0 . 1 m / s 2 ] T 0 1 2 3 4 5 6 0 0.2 0.4 0.6 0.8 1 1.2 1.4 s [ m ] ̇ s [ m/s ] β 0 p 1 o 1 β 1 V(s) MVC(s) inadmissible region # 1 p 2 admissible region # 2 α e p 3 ο 2 p 4 p 5 p 9 p 8 α 1 p 6 p 7 Fig. 10. Case 2: The NI method fails to output a feasible trajectory with constraints v v v max = [ 0 . 2 rad / s 1 . 3 m / s ] T , a a a max = [ 0 . 05 rad / s 2 0 . 1 m / s 2 ] T Starting from p 2 , the integrated backward decelerating curve α 1 intersects β 0 at o 1 , and the integrated forward accelerating curve β 1 hits MVC ∗ at p 3 . Then, searching forward along MVC ∗ from p 3 , the first sp α → β found is p 4 . Starting from p 4 , the integrated backward decelerating curve α 2 intersects β 1 at o 2 , and the accelerating curve β 3 hits the MVC ∗ at p 5 . No sp α → β is found along MVC ∗ from p 5 when s ≤ s e . Therefore, the integrated backward α e , starting from ( s e , ̇ s e ) , intersects β 1 at o 3 . The sp β → α on β 1 is updated from o 2 to o 3 , which verifies Property 4 . Finally, the NI method outputs the feasible and optimal trajectory: β 0 − α 1 − β 1 − α e . Case 2: This case shows that, when the velocity constraints are too restrictive, the conditions in Property 6 will be satisfied, and thus the NI method [10] using MVC ∗ fails to output a feasible trajectory. In this simulation, the velocity constraint v v v max is modified as [ 0 . 2 rad / s 1 . 3 m / s ] T and the acceleration constraint a a a max remains the same as that of Case 1 . Therefore, in Fig. 10, the velocity limit curve V ( s ) due to the velocity constraint becomes lower, and the areas of two inadmissible regions # 1 , # 2 increase. Meanwhile, the trajectory T : β 0 − α 1 − β 1 − α e , obtained by the NI method using MVC , is greater than MVC ∗ at the region # 1 , and the 1.4 1.6 1.8 2 2.2 2.4 2.6 2.8 0.1 0.15 0.2 0.25 0.3 0.35 0.4 s [ m ] ̇ s [ m/s ] V(s) # 1 MVC(s) p 2 β 1 o 1 β 0 α 1 p 1 p 6 p 7 p 5 Fig. 11. The enlarged view of the region # 1 in Fig. 10 sp α → β on MVC † is nonexistent, which indicates that the conditions of Property 6 hold and also verifies Property 5 . Under the curve MVC ∗ , the NI method fails to output a feasible trajectory, which is described as follows. For clarity, the region # 1 is enlarged as shown in Fig. 11. The integrated forward curve β 0 from ( 0 , ̇ s 0 ) hits the MVC ∗ at p 1 . Then, searching forward from p 1 along MVC ∗ , the first sp α → β found is the point p 2 . However, starting from p 2 , the integrated backward α 1 hits the MVC ∗ at p 5 before intersecting β 0 (the point o 1 is in the inadmissible region). Then, the integrated forward β 1 from p 2 hits the MVC ∗ at p 3 . No sp α → β is found along MVC ∗ from p 3 when s ≤ s e . Therefore, the integrated backward α e , starting from ( s e , ̇ s e ) , intersects β 1 at o 2 . In all these procedures, the α - pro f iles on the right side of the purple dash-dot curve MVC † ( p 6 p 7 ⌢ ), cannot intersect β 0 . This MVC † ( p 6 p 7 ⌢ ) breaks the intersection between the accelerating and decelerating curves, and causes that the final trajectory is blank between p 1 and p 5 , which indicates that the NI method fails and verifies Property 6 . V. C ONCLUSION This letter revisits the original version of NI method for time-optimal trajectory planning along specified paths. On this basis, we first summarize several known and new prop- erties regarding switch points and accelerating/decelerating curves of the NI method, and give corresponding mathemati- cal proofs. Then, we provide concrete failure conditions and rigorous proofs for the property, which indicates that, in the presence of velocity constraints, the original version of NI which only considers torque constraints may result in failure of trajectory planning tasks. Accordingly, a failure detection algorithm is given in a ‘run-and-test’ manner. Simulation results on a unicycle vehicle are provided to verify these presented properties. A PPENDIX A. Proof of Property 2 Proof: In the AREM region, an α - pro f ile intersects an β - pro f ile at a point ( s = s c , ̇ s = ̇ s c ) . At the neighborhood
of s c , the slopes of the α - pro f ile and β - pro f ile satisfy the inequality k α < k β and the Lipschitz condition [11]. Therefore, based on Comparison Theorem [26] (Let y , z be solutions of the differential equations ̇ y = F ( x , y ) , ̇ z = G ( x , z ) . If F ( x , y ) < G ( x , z ) , x ∈ [ a , b ] , the function F or G satisfies a Lipschitz condition, and y ( a ) = z ( a ) , then y ( x ) < z ( x ) , x ∈ ( a , b ] ), it is proven that the α - pro f ile is greater than the β - pro f ile in the left neighborhood of s c , but less than the β - pro f ile in the right neighborhood of s c . B. Proof of Property 3 Proof: This property is proven by contradiction. As- sume that an α - pro f ile is tangent to another β - pro f ile in the AREM region. Then, on the tangent point, the slope k α of the α - pro f ile is equal to k β of the β - pro f ile , which contradicts with the inequality k α < k β in the AREM region. Thus, the assumption is invalid and the property is proven. C. Proof of Property 4 Proof: In terms of the number m of intersection points, there are totally two cases: m = 1 , m > 1. Case 1: m = 1. There is only one intersection point, so the point sp β → α on the β - pro f ile is X 1 . This property holds for m = 1. Case 2: m > 1. There are m intersection points as Fig. 2. In terms of path coordinate, the intersection point X i is less than X j , 1 ≤ i < j ≤ m . Each X i has one corresponding decelerating curve α i and one switch point Y i . According to Property 1 , Y i is at the right side of Y j , i < j . If X i , i > 1 is chosen as sp β → α on β ∗ , then, starting from X i , the trajectory consisting of α - pro f iles and β - pro f iles cannot leave the region D , which is enclosed by α 1 , β ∗ and MVC , across α 1 due to Properties 1-3 . Thus, X 1 is chosen as sp β → α on β ∗ , which can aid the trajectory to leave the region D along α 1 and go on extending to the right side of Y 1 with β 1 . This property holds for m > 1. In summary, Property 4 holds. D. Proof of Property 5 Proof: Due to (10), MVC † is less than MVC . According to the definition of AR , the inequality α ( s , ̇ s ) < β ( s , ̇ s ) holds on MVC † . Then, based on the facts k α = α ( s , ̇ s ) / ̇ s , k β = β ( s , ̇ s ) / ̇ s , the inequality k α < k β also holds on MVC † , which violates k α = k β in the definition of tangent switch points (see sp α → β in Section II-B). Therefore, tangent switch points on MVC † are nonexistent. R EFERENCES [1] E. Barnett and C. Gosselin, “Time-optimal trajectory planning of cable-driven parallel mechanisms for fully-specified paths with G1 discontinuities,” Journal of Dynamic Systems Measurement and Con- trol , vol. 137, no. 7, pp. 603-617, 2013. [2] K. Kim and B. Kim, “Minimum-time trajectory for three-wheeled omnidirectional mobile robots following a bounded-curvature path with a referenced heading profile,” IEEE Transactions on Robotics , vol. 27, no. 4, pp. 800-808, 2011. [3] S. Macfarlane and E. Croft, “Jerk-bounded manipulator trajectory planning: design for real-time applications,” IEEE Transactions on Robotics and Automation , vol. 19, no. 1, pp. 42-52, 2003. [4] K. Shin and N. McKay, “Selection of near-minimum time geometric paths for robotic manipulators,” IEEE Transactions on Automatic Control , vol. 31, no. 6, pp. 501-511, 1986. [5] K. Shin and N. McKay, “A dynamic programming approach to trajec- tory planning of robotic manipulators,” IEEE Transactions Automatic Control , vol. 31, no. 6, pp. 491-500, 1986. [6] S. Singh and M. Leu, “Optimal trajectory generation for robotic ma- nipulators using dynamic programming,” Journal of Dynamic Systems Measurement and Control , vol. 109, no. 2, pp. 88-96, 1987. [7] D. Verscheure, B. Demeulenaere, J. Swevers, J. Schutter, and M. Diehl, “Time-optimal path tracking for robots: A convex optimization approach,” IEEE Transactions on Automatic Control , vol. 54, no. 10, pp. 2318-2327, 2009. [8] K. Hauser, “Fast interpolation and time-optimization with contact,” International Journal of Robotics Research , vol. 33, no. 9, pp. 1231- 1250, 2014. [9] F. Debrouwere, W. Loock, G. Pipeleers, Q. Dinh, M. Diehl, J. Schutter and J. Swevers, “Time-optimal path following for robots with convex- concave constraints using sequential convex programming,” IEEE Transactions on Robotics , vol. 29, no. 6, pp. 1485-1495, 2013. [10] K. Shin and N. Mckay, “Minimum-time control of robotic manipula- tors with geometric path constraints,” IEEE Transactions on Automatic Control , vol. 30, no. 6, pp. 531-541, 1985. [11] J. Bobrow, S. Dubowsky and J. Gibson, “Time-optimal control of robotic manipulators along specified paths,” International Journal of Robotics Research , vol. 4, no. 3, pp. 3-17, 1985. [12] J. Slotine and H. Yang, “Improving the efficiency of time-optimal path-following algorithms,” IEEE Transactions on Robotics and Au- tomation , vol. 5, no. 1, pp. 118-124, 1989. [13] Q. Pham, “A general, fast, and robust implementation of the time- optimal path parameterization algorithm,” IEEE Transactions on Robotics , vol. 30, no. 6, pp. 1533-1540, 2014. [14] F. Pfeiffer and R. Johanni, “A concept for manipulator trajectory planning,” IEEE Journal on Robotics and Automation , vol. 3, no. 2, pp. 115-123, 1987. [15] Z. Shiller, “On singular time-optimal control along specified paths,” IEEE Transactions on Robotics and Automation , vol. 10, no. 4, pp. 561-566, 1994. [16] Q. Pham and O. Stasse, “Time-optimal path parameterization for redundantly actuated robots: A numerical integration approach,” IEEE/ASME Transactions on Mechatronics , vol. 20, no. 6, pp. 3257- 3263, 2015. [17] S. Behzadipour and A. Khajepour, “Time-optimal trajectory planning in cable-based manipulators,” IEEE Transactions on Robotics , vol. 22, no. 3, pp. 559-563, 2006. [18] T. Kunz and M. Stilman, “Time-optimal trajectory generation for path following with bounded acceleration and velocity,” Robotics: Science and Systems , vol. 8, no. 12, pp. 9-13, 2012. [19] H. Nguyen and Q. Pham, “Time-optimal path parameterization of rigid-body motions: Applications to spacecraft reorientation,” Journal of Guidance Control and Dynamics , vol. 39, no. 7, pp. 1667-1671, 2016. [20] Q. Pham and Y. Nakamura, “Time-optimal path parameterization for critically dynamic motions of humanoid robots,” Proceedings of the 2012 IEEE International Conference on Humanoid Robots , pp. 165- 170, 2012. [21] L. ˇ Zlajpah, “On time optimal path control of manipulators with bounded joint velocities and torques,” Proceedings of the 1996 IEEE International Conference on Robotics and Automation , pp. 1572-1577, 1996. [22] F. Lamiraux and J. Laumond, “From paths to trajectories for multi- body mobile robots,” Proceedings of the 5th International Symposium on Experimental Robotics , pp. 301-309, 1998. [23] T. Kalm ́ ar-Nagy, “Real-time trajectory generation for omni-directional vehicles by constrained dynamic inversion,” Mechatronics , vol. 35, pp. 44-53. [24] Q. Pham, S. Caron and Y. Nakamura, “Kinodynamic planning in the configuration space via velocity interval propagation,” Proceedings of Robotics: Science and Systems , 2013. [25] C. Bianco and M. Romano, “Optimal velocity planning for au- tonomous vehicles considering curvature constraints,” Proceedings of the 2007 IEEE International Conference on Robotics and Automation , pp. 2706-2711, 2007. [26] G. Birkhoff and G. Rota, “Ordinary differential equations,” Ussr Computational Mathematics Physics , vol. 4, no. 4, pp. 230-232, 1964.