On the Structure of the Time-Optimal Path Parameterization Problem with Third-Order Constraints
Total Page:16
File Type:pdf, Size:1020Kb
On the Structure of the Time-Optimal Path Parameterization Problem with Third-Order Constraints Hung Pham, Quang-Cuong Pham Abstract—Finding the Time-Optimal Parameterization of a we survey some of the attempts to address TOPP with third- Path (TOPP) subject to second-order constraints (e.g. acceler- order constraints. ation, torque, contact stability, etc.) is an important and well- 1) Numerical integration methods: TOPP with second- studied problem in robotics. In comparison, TOPP subject to third-order constraints (e.g. jerk, torque rate, etc.) has received order constraints can be efficiently solved by the numerical far less attention and remains largely open. In this paper, we integration method: from Pontryagin’s Maximum Principle, investigate the structure of the TOPP problem with third-order the optimal velocity profile in the (s; s_) plane [s(t) denotes constraints. In particular, we identify two major difficulties: the position on the path at a given time instant t] is “bang- (i) how to smoothly connect optimal profiles, and (ii) how to bang” and can thus be found by integrating successively the address singularities, which stop profile integration prematurely. We propose a new algorithm, TOPP3, which addresses these maximum and minimum accelerations s¨. In [10], Tarkiainen two difficulties and thereby constitutes an important milestone and Shiller pioneered the extension this approach to third- towards an efficient computational solution to TOPP with third- order constraints. Here, one integrates profiles in the 3D order constraints. space (s; s;_ s¨), following successively maximum, minimum ... and maximum jerk s . As discussed later in Section II, there I. INTRODUCTION are two main difficulties: (i) contrary to the 2D case, minimum Given a robot and a smooth path in the robot’s configuration and maximum profiles in the (s; s;_ s¨) space do not generally space, the problem of finding the Time-Optimal Parameter- intersect each other, (ii) there are singularities that stop profile ization of that Path (TOPP) with second-order constraints integration prematurely. To address (i), the authors proposed to (e.g. bounds on acceleration, torques, contact stability) is an connect the initial maximum jerk profile to the final maximum important and well-studied in robotics. An efficient algorithm jerk profile by (a) stepping along the initial profile, (b) at to solve this problem was first proposed in the 1980’s [1], [2] each step, integrate a minimum jerk profile and check whether and has been continuously perfected since then, see [3] for that profile connects with the final profile. This procedure a historical review. The algorithm has also been extended to corresponds to a Single Shooting method, which is known handle a wide range of problems, from manipulators subject to be sensitive to initial condition [11]. Regarding (ii), the to torque bounds [1], [4] to vehicles or legged robots subject authors did not propose a method to overcome singularities; to balance constraints [5], [6], [7], [8], to kinodynamic motion instead, they suggested to modify the original path to avoid planning [9], etc. them. However, this workaround is questionable as in practice, According to Pontryagin’s Maximum Principle, time- paths usually contain a large number of singularities [3]. optimal trajectories are bang-bang, which implies instanta- In a recent work [12], Mattmuller and Gilser proposed an neous switches in the second-order quantities that are con- approximate method to compute near-optimal profiles: they strained. For instance, time-optimal trajectories with acceler- introduce “split points” artificially to “guide” the profiles away ation constraints will involve acceleration switches, which in from singularities. While their method presents the advantage turn implies infinite jerk. This is one of the drawbacks of of simplicity, it is sub-optimal and does not help understanding TOPP with second-order constraints. the structure of the true time-optimal solutions. arXiv:1609.05307v3 [cs.RO] 19 Sep 2017 To address this issue, one can consider TOPP with third- 2) Optimization-based methods: TOPP with second-order order constraints. In many industrial applications, constraining constraints can also be solved robustly (albeit at the expense third-order quantities such as jerk, torque rate or force rate of computation speed [3]) using convex optimization [13], is also part of the problem definition [10]. For instance, a [14]. While we are not aware of any result on formulating hydraulic actuator exerts forces by forcing oil to travel through TOPP with third-order constraints as a convex optimization its piston and gets compressed, which results in the actuating problem, there have been a number of works that approximate force rate being restricted. As another example, DC electric the constraints to make them convex. For instance, the authors motors have bounds on input voltages, which translate directly of [15] approximated jerk constraint by linear functions. In to torque rate constraints. [16], the authors replaced jerk constraints by squared jerk terms in the objective function. In a recent work [17], the A. Related works authors proposed to represent the profiles by a class of While TOPP with second-order constraints can essentially C1 functions. This representation ensures the continuities be considered as solved [3], the structure of TOPP with third- of higher-order derivatives such as jerk, snap, etc. while still order constraints is much less well understood. In the sequel, allowing for efficient solutions via convex optimization. In [18], [19], [20], the authors approximated the optimal The boundary conditions of a TOPP with third-order con- profiles by a piece-wise polynomial in the (s; s_) plane. The straints consist of configuration velocities and accelerations polynomials are controlled via a finite set of knot points. The at both the start and the goal. Combining with equation (1), path parameterization problem then becomes equivalent to an one can compute the corresponding initial velocities and optimization problem where the independent variables are the accelerations as coordinates of the knot points. However, this formulation is 2 kvbegk kabeg − qss(0)s _0k non-convex and there is no guarantee that it converges to true s0 = 0; s_0 = ; s¨0 = ; kqs(0)k kqs(0)k time-optimal solutions. where vbeg and abeg are the initial joint velocity and accel- B. Our contributions eration respectively. The end conditions (s1; s_1; s¨1) can be In this paper, we follow the numerical integration approach computed similarly. and investigate the structure of the time-optimal solutions. We consider third-order constraints of the form Specifically, our contribution is threefold: ... a s b c 3 d ≤ (2) 1) we propose a Multiple Shooting Method (MSM) to (s) + (s)_ss¨ + (s)_s + (s) 0; smoothly connect two maximum jerk profiles by a where a(s); b(s); c(s); d(s) are m-dimensional vectors. minimum jerk profile. This method is more efficient and As in the second-order case, the bounds (2) can represent a stable than the the one proposed in [10]; wide variety of constraints, from direct jerk bounds to bounds 2) we analyze third-order singularities, which arise very on torque rate or force rate 1. For instance, direct jerk bounds frequently and systematically stop profile integration, ... (jmin ≤ q ≤ jmax ) can be accommodated by setting a, b, c, leading to algorithm failure. We propose a method to d as follows address such singularities; q (s) 3q (s) 3) based on the above contributions, we implement TOPP3, a(s) = s ; b(s) = ss ; −q − q which solves TOPP with third-order constraints effi- s(s) 3 ss(s) q (s) −j ciently and yields true optimal solutions in a number c(s) = sss ; d(s) = max : of situations. −qsss(s) jmin For simplicity, this paper focuses on pure third-order con- Similar to the classical TOPP algorithm with second order straints. Taking into account first- and second-order constraints constraints [1], [3], one can next define, at any state (s; s;_ s¨), (e.g. bounds on velocity and acceleration) is possible but will the minimum jerk γ(s; s;_ s¨) and maximum jerk η(s; s;_ s¨) as significantly increase the complexity of the exposition. follows The remainder of the paper is organized as follows. In 3 −bi(s)_ss¨ − ci(s)_s − di(s) Section II, we introduce the basic notations and formulate the γ(s; s;_ s¨) := max j ai(s) < 0 ; problem. In Section III, we introduce our method to smoothly i ai(s) 3 connect two maximum jerk profiles. In Section IV, we propose −bi(s)_ss¨ − ci(s)_s − di(s) η(s; s;_ s¨) := min j ai(s) > 0 : a solution to the problem of singularities. In Section V, we i ai(s) present the numerical experiments. Finally, in Section VI, we (3) offer some discussions and directions for future work. Note that the above definitions neglect the case where some ai are zero. As in the case of second-order constraints, a path II. STRUCTURE OF TOPP WITH THIRD-ORDER position s where at least one of the ai(s) is zero can trigger CONSTRAINTS a singularity. A. Problem setting A profile is is a curve in the (s; s;_ s¨)-space where s is Consider a n dof robotic system whose configuration is always increasing. A time-parameterization of q corresponds n a vector q 2 R . A geometric path P is a mapping to a profile connecting (s0; s_0; s¨0) to (s1; s_1; s¨1). From the q(s) from [0; s ] to the configuration space. A time- definition of γ and η, a time-parameterization of q satisfies the s2[0;send] end ... constraints (2) if and only if the jerk s along the corresponding parameterization of the path P is an increasing scalar mapping ..