Yuyang Wang
email: yuyang.wang@ensam.eu

Member IEEE Jean-Rémy Chardonnet
email: jean-remy.chardonnet@ensam.eu

Frédéric Merienne
email: frederic.merienne@ensam.eu

Speed Profile Optimization for Enhanced Passenger Comfort: An Optimal Control Approach

Autonomous vehicles are expected to start reaching the market within the next years. However in practical applications, navigation inside dynamic environments has to take many factors such as speed control, safety and comfort into consideration, which is more paramount for both passengers and pedestrians. In this paper, a novel speed profile planner based on an optimal control approach considering passenger comfort is proposed. The approach is accomplished by minimizing jerk under certain comfort constraints, which inherently gives a speed profile for the central nervous system to follow naturally. Imposed with the same conditions, the widely used Jerk Limitation method is interpreted as an equivalent of the minimum time control method, the latter being used to verify that our method can ensure better continuity and smoothness of the speed profiles. A validation test was specifically designed and performed in order to show the feasibility of our method.

I. INTRODUCTION

Research on path planning for motion systems has been primarily focusing on finding an optimal path through a cost function considering the physical road such as the length and curvature of the road, or a combination of both [START_REF] Gu | On-road motion planning for autonomous vehicles[END_REF], while the optimization of proper speed profiles with respect to passenger comfort has received less attention [START_REF] González | Speed profile generation based on quintic bezier curves for enhanced passenger comfort[END_REF].

Jerk is the temporal derivative of the acceleration during motion and thus is interpreted as a change of actuator forces. Hogan [START_REF] Hogan | Adaptive Control of Mechanical Impedance by Coactivation of Antagonist Muscles[END_REF] notes that the smoothness of speed profiles can be formulated as a function of jerk. Using splines and clothoids, Labakhua et al. [START_REF] Labakhua | Smooth Trajectory Planning for Fully Automated Passengers Vehicles: Spline and Clothoid Based Methods and Its Simulation[END_REF] propose smooth trajectories with low associated accelerations and jerk, trying to provide more comfort to the passenger: firstly, the global expressed discrete points are connected through high order polynomials, and in a second step, the speed must be computed according to comfort constraints from the ISO 2631-1 standard [START_REF] Organization | ISO 2631-1: Mechanical Vibration and shock-Evaluation of human exposure to whole-body vibration-Part 1-General Requirements[END_REF], giving rise to a smooth profile, although longitudinal and lateral accelerations may still be discontinuous at some points.

On the other hand, some authors separate the problem and consider motion variables such as the spatial position, speed and acceleration independently. For example, Liu [START_REF] Liu | An on-line reference-trajectory generator for smooth motion of impulse-controlled industrial manipulators[END_REF] and Piazzi and Visioli [START_REF] Piazzi | Global minimum-jerk trajectory planning of robot manipulators[END_REF] solve the problem by setting a constant jerk in different time phases and then integrate for the acceleration and speed. Therefore the corresponding smoothness of the acceleration and speed are C 0 and C 1 continuous respectively.

We propose a novel approach in which we generate a globally lowest jerk trajectory with an optimized speed profile following also the same comfort constraints as defined in the ISO 2631-1 standard [START_REF] Organization | ISO 2631-1: Mechanical Vibration and shock-Evaluation of human exposure to whole-body vibration-Part 1-General Requirements[END_REF]. We achieve this by introducing the optimal control theory into the problem, following a standard procedure for solving optimal problems, improving passenger comfort in a global manner. The proposed method complies with the speed, acceleration and jerk limitation and meanwhile provides higher order smoothness to the acceleration and speed.

II. PROBLEM DESCRIPTION

Given a geometric trajectory generated from a path planning algorithm for the vehicle to follow, the next problem is to design a smooth profile from the starting place to the destination in the path. The speed planner should consider the speed, acceleration and jerk limits for passenger comfort, and also be time efficient in order to keep real-time computing. The theoretical passenger comfort is linked to the smoothness of the jerk profile which can be interpreted as a minimization problem [START_REF] González | Speed profile generation based on quintic bezier curves for enhanced passenger comfort[END_REF].

Gonzalez et al. propose a method to address such problem using a quintic Bézier curve to approximate a smooth change in the speed under given speed and acceleration limits, leading to a G 2 continuous of the speed profile [START_REF] González | Speed profile generation based on quintic bezier curves for enhanced passenger comfort[END_REF]. This method can efficiently compute an optimized speed profile, but the physical meaning of its minimization process is unclear. Also, since the jerk limit also holds the key to speed profile generation [START_REF] Sharma | Jerk analysis in rail vehicle dynamics[END_REF], [START_REF] Powell | Passenger stability within moving railway vehicles: Limits on maximum longitudinal acceleration[END_REF], the lack of such consideration about jerk may reduce comfort and safety [START_REF] Forstberg | Ride comfort and motion sickness in tilting trains[END_REF]. Another method is the so-called Jerk Limitation method as implemented in [START_REF] Liu | An on-line reference-trajectory generator for smooth motion of impulse-controlled industrial manipulators[END_REF], [START_REF] Haschke | On-line planning of timeoptimal, jerk-limited trajectories[END_REF], in which the jerk can take three different values depending on the status of the vehicle: acceleration, cruise or deceleration. Once the vehicle decides to accelerate, it will choose the maximum jerk in order to reach the acceleration and speed limits in a minimum time, and vice versa, therefore leaving discontinuous jerk profiles. Based on this idea, we interpret this method as a minimum-time problem using the optimal control theory as a baseline to verify our new proposition.

In order to address these issues, a new speed planner based on the optimal control theory is proposed and validated. The resulting profiles are continuous and provide globally minimum jerk. Our contribution are described as follows:

• Propose a general speed planning method based on the universality and generality of the optimal control theory. Depending on different metrics to be optimized, categorize the speed optimization method with three different metrics: minimum time, minimum norm of jerk, minimum square of jerk. This will be presented in Sec. IV. • Compare these different speed planning methods. In either case, the algorithm is able to compute a desired speed profile, considering passenger comfort, but only the last metric gives the speed profile with higher continuity, as we will show in Sec. V. • Finally, apply the minimum square of jerk to solve a real case considering constraints like acceleration limits according to the ISO 2631-1 standard. Besides, we impose other constraints to the control method in order to enhance passenger comfort. This will be addressed in Sec. VI. To better show the effectiveness of our approach, the same conditions are imposed to all of these methods.

III. OPTIMAL CONTROL BASICS

A. General Formulation of an Optimal Control Problem

The optimal control problem can be posed in the following Bolza form [START_REF] Liberzon | Calculus of variations and optimal control theory: A concise introduction[END_REF]: Find the motion states x ∈ R n , the vector of static parameters p ∈ R q , the control u ∈ R m , the initial time t 0 ∈ R, and the final time t f ∈ R, that optimize

J = Φ (x, t 0 , x, t f ; p) + t f t0 L ([x, u, t; p]) dt (1)
subject to the dynamic constraints (representing the system dynamics)

ẋ = f [x, u, t; p] (2)
the state constraints

C min ≤ C[x, u, t; p] ≤ C max (3)
and the boundary conditions

φ min ≤ φ [x, t 0 , x, t f ; p] ≤ φ max (4)
J represents a performance index that measures the quality of the path. The optimal control problem can be decomposed in phases, in this case the performance index J is the sum of each performance index over the number of phases p.

B. Numerical Solutions to Optimal Control

Analytical solutions to optimal problems are almost infeasible; on the contrary, numerical methods can be of great significance and many numerical solvers are developed for such problems. We here briefly explain some basics to numerically solve optimal control problems. Several families of numerical methods can be implemented to solve (1): direct methods, indirect methods or dynamic programming (e.g., the Hamilton-Jacobi-Bellman (HJB) equation). Direct methods transform the original infinite optimal control problem into a finite dimensional nonlinear optimization problem, while indirect methods derive a boundary value problem (BVP) from the original problem. The HJB equation is based on the principle of optimality which says that all subarcs of an optimal trajectory are also optimal, but the corresponding solution is normally restricted to small state dimensions. Three major components of optimal control exist to solve optimal control problems [START_REF] Rao | A survey of numerical methods for optimal control[END_REF]: differential equations and integration of equations, systems of nonlinear algebraic equations and nonlinear optimization problems.

Algorithm 1 shows how the direct method is used to solve optimal control problems. First, the solver makes an initial guess and solves the performance index and the dynamic constraints; then the solver converts the problem into a nonlinear optimization problem to be solved with gradientbased methods; the procedure ends once the desired accuracy is achieved. Compute the error and update the solution; 8 end 1) Numerical Solution of Differential Equations: Several methods can be used to solve differential equations, among which are time-marching methods. Time-marching methods are designed to solve differential equations as (2) at each time step t k based on the current and/or previous condition for the solution [START_REF] Esfandiari | Numerical methods for engineers and scientists using matlab[END_REF]. The most common methods are usually θfamily methods (the forward/backward Euler method and the Crank-Nicolson method) and Runge-Kutta methods. Recall that the objective is to numerically solve an optimal control problem, more specifically the one in [START_REF] Gu | On-road motion planning for autonomous vehicles[END_REF]. Integration can be addressed by many numerical schemes [START_REF] Esfandiari | Numerical methods for engineers and scientists using matlab[END_REF].

2) Nonlinear Optimization: Algorithms for nonlinear optimization or nonlinear programming problems (NLPs) are one crucial component of direct methods to solve optimal control problems. The problem is to find the vector of decision variables z ∈ R n that verifies

arg min z∈R n f (z) (5)
subject to the following constraints

g(z) = 0 h(z) ≤ 0 (6)
where g(z) ∈ R m and h(z) ∈ R p are a general form of (3). Numerical methods for solving NLPs are categorized into two groups: gradient-based methods and heuristic methods.

In gradient-based methods, the solver iterates to find an optimal solution with an initial guess of the unknown z.

At the k th iteration, a search direction and a step length are determined then the solution is updated from z k to z k+1 . Gradient-based methods will generally converge to an optimal solution that will however be local. Conversely, heuristic methods, such as genetic algorithms, allow finding a global minimum [START_REF] Horst | Introduction to Global Optimization[END_REF].

IV. SPEED PROFILE DESIGN WITH DIFFERENT METRICS

The computation of a desired speed profile can be achieved with different metrics. As an example, the vehicle may want to reach the target states in the shortest time, or to reach the target states with continuous and smooth variations of state quantities for comfort.

A. Minimum Time

As mentioned in the section above, the minimum time method de facto is the Jerk Limitation method. Once the limit of speed, acceleration and jerk are given, the Jerk Limitation method computes the accelerating and decelerating processes with the maximal or minimal quantity, resulting in the least time to reach the target states

                             min arg J = T 0 1 dt subject to ẋ = v, x(0) = x 0 , x(T ) = L, v = a, v(0) = 0, v(T ) = 0, ȧ = j,

B. Minimum Jerk 1) Why Minimum Jerk:

The third-order temporal derivative of the position is called jerk, while the fourth-order, fifthorder, and sixth-order derivatives also exist and are named snap, crackle, and pop, respectively. Accordingly, there is still the possibility to minimize snap, crackle, or pop instead of only jerk. However, in [START_REF] Richardson | Comparing smooth arm movements with the two-thirds power law and the related segmented-control hypothesis[END_REF], the ending position and moving time are fixed to observe how the trajectory x(t) changes as a function of the n th -derivative in [START_REF] Sharma | Jerk analysis in rail vehicle dynamics[END_REF], concluding that only the minimum jerk makes difference.

min arg J (x(t)) = 1 2 t f t0 d n x dt n 2 dt (8)
Fig. 1a indicates that the solution to (8) requires x(t) to become a step function as the derivative order, n, increases. Consequently, the first derivative (speed) of the trajectory becomes narrower and taller along with a minimization of jerk, snap, crackle and pop (Fig. 1b). Consequently, if we choose the minimum snap method, the trajectory we obtained will have slightly higher peak speed compared to that of the method by minimizing jerk. This indicates that as n increases in [START_REF] Sharma | Jerk analysis in rail vehicle dynamics[END_REF], the performance index also yields a trajectory whose peak speed is relatively larger than the average speed. Moreover, let r denote the ratio between the peak speed and the average speed. Table I gives the values of the ratio corresponding to each derivative order. Psychophysical experiments reveal that the optimal value of r is about 1.75, and thus most resemble to minimum-jerk trajectories [START_REF] Flash | The coordination of arm movements: an experimentally confirmed mathematical model[END_REF].

2) Minimum Square of Jerk: According to the explanation of the minimum jerk phenomenon, we hereby propose the optimal control problem, given by

                             min arg J = 1 2 T 0 j 2 dt subject to ẋ = v, x(0) = x 0 , x(T ) = L, v = a, v(0) = 0, v(T ) = 0, ȧ = j, a(0) = 0, a(T ) = 0, the speed constraint: v ∈ Ω = [v min , v max ]
the acceleration constraint: a ∈ Ω = [a min , a max ] and the control constraint:

j ∈ Ω = [j min , j max ] (9)
where the difference is the performance index: instead of the minimum time, the minimum square of jerk is of more interest. The square of jerk appears here to avoid the effect of the negative sign on the minimization of the quality of the measure.

3) Minimum Norm of Jerk: Following a similar philosophy of the minimum square of jerk, another control method, the minimum norm of jerk, is also proposed here to be compared with the result from the minimum time and square of jerk methods. The problem is to find the optimal solution that transfers the system from the initial state to a given final state and minimizes the performance metric

                             min arg J = T 0 |j| dt subject to ẋ = v, x(0) = x 0 , x(T ) = L, v = a, v(0) = 0, v(T ) = 0, ȧ = j, a(0) = 0, a(T ) = 0, the speed constraint: v ∈ Ω = [v min , v max ]
the acceleration constraint: a ∈ Ω = [a min , a max ] and the control constraint: j ∈ Ω = [j min , j max ] (10) The absolute term in the performance index indicates that all the non-zero component jerk will make a contribution to the final metric, and therefore, the optimized profile should have less jerk, providing more comfort.

C. Limit of Motion States

The limit of motion states is necessary to apply constraints to the problem. For the allowed maximal speed (V max ) along a straight path, we consider only the limits required by the traffic rules. According to ISO 2631-1 [START_REF] Organization | ISO 2631-1: Mechanical Vibration and shock-Evaluation of human exposure to whole-body vibration-Part 1-General Requirements[END_REF], a passenger can choose a preferred comfort level and the corresponding acceleration as given in Table II. To compute the speed limit along a curved path, the maximal speed in the pre-planned geometrical path has to be computed considering the lateral acceleration, in a way that the norm of the whole acceleration should be less than the limits given by:

V max ≤ a w kC (11)
a w = k 2 x a 2 wx + k 2 y a 2 wy + k 2 z a 2 wz ( 12
)
where C is the maximal curvature of turn; k x , k y = k = 1.4, k z = 1 are relaxation factors suggested in [START_REF] Gonzalez | Continuous curvature planning with obstacle avoidance capabilities in urban scenarios[END_REF]; a w is the Euclidean sum of a wx , a wy and a wz which are the root mean squared accelerations along the different axes.

Considering the fact that most vehicles generally move inside a two dimensional space, we neglect the vertical component a wz in [START_REF] Liberzon | Calculus of variations and optimal control theory: A concise introduction[END_REF], i.e., a wz = 0.

V. COMPARISON OF RESULTS BETWEEN THE DIFFERENT METHODS

Several comparisons are conducted in order to compare the smoothness of the curve, the final performance index of each method. The problem is to design a speed profile with the three different metrics described above, moving the vehicle on a distance of 20m. The limit of the speed, acceleration, and jerk magnitude are set to respectively 3m/s, 1.0m/s 2 and 1.0m/s 3 . This section first compares the speed profiles as well as the corresponding changes in acceleration and jerk; then, the performance index is compared between the different metrics. Fig. 2: Speed profiles from different methods Fig. 2 presents the speed profile using the different methods. The speed increases from 0m/s to the maximum 3m/s then remains constant. The blue line, corresponding to the minimum square of jerk, increases to the maximum in a smoother behavior, compared to those of the other two methods. Accordingly, Fig. 3 verifies this smoothness as we can see from the blue line, it changes without any sharp change, which indicates that the acceleration is continuous and differentiable.

Fig. 4 demonstrates the behavior of jerk in the whole process corresponding to each method. The peak of jerk from the minimum square metric is lower than the other two, one Fig. 3: Acceleration profiles from different methods Fig. 4: Jerk profiles from different methods property that we desire to have. Besides, the solution from the minimum time method and the minimum norm of jerk method present some nodal oscillations in the domain where the gradient is relatively large; this is not a problem of our formulation but the numerical method fails to capture the property in such domain; increasing the number of steps will help capturing the sharp gradient. Because the minimum of square method is a high order method, the jerk profile still keeps better smoothness with the same number of steps. Table III presents the final value of the performance index with respect to each method. In terms of the total time metric of the process, the minimum time method can transfer the vehicle from the initial state to the final state within the shortest time, slightly less than with the minimum norm of jerk method, while the minimum square of jerk requires the longest time. Concerning the norm metric, the minimum norm of jerk method gives the best performance index, followed by the other two methods. However, it is worth noting that the performance index obtained from the total time metric and the norm metric of jerk does not show any significant difference, while the square metric provides a significant improvement to the defect, with a value around 2.8 in the minimum square of jerk method, sharply better than 3.7 from the other two methods.

VI. VALIDATION TEST

We carried out a test on an in-home code that was developed to concatenate and post-process numerical results based on OpenGoddard1 , a Python-based open source library designed for solving optimal control problems. The chosen scenario tries to reproduce a comfortable speed profile on a geometrical path including two turns which simulates a real environment. The vehicle is designed to move totally 1.2km, including acceleration, cruise and deceleration in order to keep the comfort limits with an acceleration magnitude less than 1.5m/s 2 and a jerk magnitude less than 1.0m/s 3 . The vehicle needs to pass two consecutive turns with radius R 1 = 14.9m and R 2 = 26.38m. The maximum speed is 15m/s. The maximum speed while turning is decided according to [START_REF] Haschke | On-line planning of timeoptimal, jerk-limited trajectories[END_REF]. Fig. 5: Speed profile in a simulated real test Fig. 5 presents the speed profile starting from 0m/s and then reaching its maximum 15m/s at about 11.5s. The vehicle keeps a steady state and decelerates to 4m/s at about 24.6s in order to make the first turn with a lower speed, from 24.6s to 28.8s. Then, the vehicle repeats such procedure to make the second turn which is less curvier, and finally stops. The blue line represents the maximal speed which shall be smaller in the turning area. The speed profile strictly follows the limit conditions. Fig. 6: Acceleration profile in a simulated real test Fig. 6 depicts the corresponding acceleration profile. During the turns, there is no longitudinal acceleration but a constant lateral acceleration (1.5m/s 2 ) appears due to inertia then vanishes when back on a straight trajectory. According to [START_REF] Haschke | On-line planning of timeoptimal, jerk-limited trajectories[END_REF], we can know that the lateral acceleration will not overpass the acceleration limit since the maximum speed is set in a way to keep this property. In any case, the acceleration limit is imposed to maintain the comfort condition. Fig. 7: Jerk profile in a simulated real test Fig. 7 demonstrates the jerk response in the whole process. At a first glance, the jerk profile does not seem smooth enough due to the concatenation. However, as we have seen from Fig. 4, the minimum square of jerk metric can provide smooth changes of jerk whenever acceleration or deceleration is required. Consequently, our method assures a continuous and smooth jerk under a comfort limit.

VII. CONCLUSION AND FUTURE WORK

In this paper a speed planner developed with the optimal control theory was presented to obtain smooth changes in acceleration and jerk. The minimum square of jerk method with constrained motion states, especially acceleration and jerk, during planning processes is an efficient metric to achieve better passenger comfort. Numerical simulation of three different methods were compared, evidencing a clear improvement in acceleration and jerk smoothness. Although the minimum time method, as an interpretation of the Jerk Limitation method, uses the shortest time to complete the task, acceleration and jerk lack enough smoothness. The minimum square of jerk method provides a more pleasant travel, resulting in smoothness and continuity without exceeding acceleration limits as recommended in the ISO 2631-1 standard. Finally, the minimum square of jerk was applied to emulate a real geometrical path including two turns, showing the robustness of the algorithm.

In future work, improvements in the computational efficiency will be taken into consideration. The current optimization algorithm inevitably requires very high computational resources particularly in modern dynamic urban scenarios due to frequent accelerations or decelerations. One way is to consider more complex model such as dynamic programming and especially nonlinear model predictive control (NMPC), in order to achieve online performance during runtime [START_REF] Diehl | Numerical optimal control[END_REF]. Another option to boost the efficiency of the speed planner is to compute the speed profiles in a discretized form with respect to different target speeds v n (n is the sample size); then unknown desired speed profile can be found based on existing profiles using numerical methods or neural networks. The computational efficiency will be compared from different methods. Then, we intend to implement our method on a driving simulator to make experimental tests on subjects and measure the level of comfort to totally validate our approach.

  a(0) = 0, a(T ) = 0, the speed constraint: v ∈ Ω = [v min , v max ] the acceleration constraint: a ∈ Ω = [a min , a max ] and the control constraint: j ∈ Ω = [j min , j max ] (7) Equation 7 gives the definition of the problem to be solved concerning the shortest time to reach the target at distance L, at the same time following constraints regarding the real application. The solution of this equation is the desired profile of the motion system.

Fig. 1 :

 1 Fig. 1: (a) Motion with a minimum jerk (n = 3), a minimum snap (n = 4), a minimum crackle (n = 5) and a minimum pop (n = 6). (b) Motion speed for each trajectory. Note that the ratio of the peak speed to the average speed increases as n increases.

Algorithm 1 :

 1 Solving optimal control problems with direct methods Input: Initial guess with zero for all unknown nodes Output: State and control profiles w.r.t time: x, u 1 Initialize the solver; 2 while Error is larger than the accepted tolerance do

	7

3

Read the current solution of x and u; 4

Convert the problem to a constrained nonlinear optimization problem;

5

Calcuate the numerical solution in a differential form

[START_REF] González | Speed profile generation based on quintic bezier curves for enhanced passenger comfort[END_REF]

and in an integral form (1);

6

Optimize and update the solution with the gradient-based method;

TABLE I :

 I Relationship between the derivative order and the ratio of speed

	Methods	Derivative order n	Ratio r
	Minimum acceleration	2	1.5
	Minimum jerk	3	1.875
	Minimum snap	4	2.186

TABLE II :

 II Acceleration limits and corresponding comfort levels

	Comfort levels	Acceleration limits (Amax)
	Not uncomfortable	0.315m/s 2
	A little uncomfortable	0.63m/s 2
	Fairly uncomfortable	1.0m/s 2
	Uncomfortable	1.6m/s 2
	Very uncomfortable	2.5m/s 2

TABLE III :

 III Comparison between the different methods

	Method	Total time [s] Norm metric Square metric
	Minimum time	10.671	4.146	3.754
	Minimum norm	10.677	3.993	3.791
	Minimum square	10.751	4.034	2.842

https://istellartech.github.io/OpenGoddard/

*This work was supported by the China Scholarship Council(No.201708390014)