Keywords

1 Introduction

To determine lateral motion, both “path planning” and “path following” are necessary. The most fundamental model for human “path following” is Kondo’s forward-looking model (Preview path-following model) [1]. Defining a fundamental model for human “path planning” remains a challenge, with existing methods lacking intuitive clarity.

The paper introduces a foundational model aligned with human-like “path planning” the Preview path-planning model. It aims to provide insights for those modeling human-like path planning. Additionally, the paper reports applying GVC technology [2] in scenarios where driving along the proposed path is achieved, showcasing the integration of lateral motion.

2 Information and Planning Guidelines for Path Planning

This chapter explores drivers’ considerations in path planning, assuming a target course centerline in the middle of the drivable area. Drivers aim to optimize their route, considering factors like shortcuts and avoiding abrupt curvature changes. The use of Tangent Points aids in understanding the target course centerline’s curvature. To control lateral acceleration, the chapter proposes a path planning guideline: derive a smoothed curvature (κp) from the original curvature (κo) by reducing its maximum value and minimizing the time rate of change, ensuring it fits within the lane width.

3 Implementation of Path Planning Using Forward Info.

3.1 Reduction and Smoothing of Curvature Maximum

Considering the application of path planning guidelines from the previous chapter, Waypoint information for the centerline is crucial. If this data list, containing coordinates, curvature, and orientation information, is available, the vehicle’s current position (ve-th) and forward-looking point (pv-th) data can be referenced. An example Waypoint data table (Table 1) is introduced. The blue line in Fig. 1 represents the original curvature (κo) of the centerline. Applying the path planning guideline involves deriving a smoothed path (κp, orange line) to reduce the maximum curvature and its time variation, realizing a smoother path planning. The orange line (κp) is a result of applying a first-order low-pass filter to the original curvature (κo).

Table 1. Example of Waypoint data
Full size table
Fig. 1.
figure 1

Image of Curvature Smoothing

Full size image

3.2 Phase Lag Compensation

To address phase lag introduced by the low-pass filter, the chapter incorporates forward-looking information and Taylor expansion. Anticipating future curvature based on current and forward-looking data compensates for phase lag, resulting in a simplified expression (Eq. (1) for the smoothed path planning (κp).

$$ \kappa_{p} (s_{ve} ) = \kappa_{o} (s_{ve} ) - C_{\kappa } \left( {\kappa_{o} (s_{pv} ) - \kappa_{o} (s_{ve} )} \right)V $$
(1)

3.3 Waypoint Offset Strategy for Path Planning

This section describes a specific method for path planning, which involves offsetting the X-Y coordinate information of the waypoints on the original centerline to create new path planning waypoints. The waypoints are recorded for each unit arc length parameter s (Eq. (2)). The waypoint L meters ahead of the i-th waypoint is the i + L -th waypoint.

$$ \sqrt {\left( {X_{o} \left( i \right) - X_{o} \left( {i - 1} \right)} \right)^{2} + \left( {Y_{o} \left( i \right) - Y_{o} \left( {i - 1} \right)} \right)^{2} } = 1 $$
(2)

For the i-th data point, the process is simplified as shown in Eq. (3).

$$ \kappa_{p} (i) = \kappa_{o} (i) - \kappa_{d} (i) $$
(3)

Here, κd(i) is redefined as follows:

$$ \kappa_{d} (i) = C_{\kappa } \left( {\kappa_{o} (i + L) - \kappa_{o} (i)} \right)V $$
(4)

Using Fig. 2, which shows continuous waypoints representing the centerline in the Cartesian coordinate system, the method for setting new path planning waypoints is illustrated. Let the coordinates of the (i − 1)-th waypoint on the centerline be [Xo(i − 1), Yo(i − 1)] with an orientation angle θo(i − 1). Since the arc length parameter s is 1, the angle changes when advancing by an arc length of 1 is κo(i − 1) (the lengths of the red and blue lines in the figure are 1).

$$ \theta_{o} (i) = \theta_{o} (i - 1) + \int_{0}^{1} {\kappa_{0} (i - 1)ds} = \theta_{o} (i - 1) + \kappa_{o} (i - 1) $$
(5)

Therefore, the orientation angle θp(i) of the new path planning waypoint () is:

$$ \theta_{p} (i) = \theta_{o} (i) - \kappa_{d} (i - 1) $$
(6)

The XY coordinates can be geometrically determined. Since the arc length parameter is 1, the distance from the coordinates of the (i)-th waypoint on the centerline (red ) [Xo(i),Yo(i)] to the new path planning waypoint (blue ) [Xp(i),Yp(i)] is approximately κd(i − 1) (green line ). Using this relationship and the relationship with θo(i):

$$ \left[ {\begin{array}{*{20}c} {X_{p} \left( i \right)} \\ {Y_{p} \left( i \right)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {X_{o} \left( i \right)} \\ {Y_{o} \left( i \right)} \\ \end{array} } \right] + \left[ {\begin{array}{*{20}c} {\sin \theta_{o} \left( i \right)} \\ { - \cos \theta_{o} \left( i \right)} \\ \end{array} } \right]\kappa_{d} \left( {i - 1} \right) $$
(7)

This means the new path planning waypoint is offset from the centerline by a distance κd(i − 1) to the left or right, rather than altering the curvature. Reformulating it this way, the path planning strategy can be described as: “Path planning offsets the centerline to the left or right (outside or inside) by a value proportional to the gain Cκ, the current speed V, and the difference between the current curvature and the forward-looking curvature.” This simple strategy allows easy management of how far the vehicle can move relative to the lane width using the gain Cκ in Eq. (4). Qualitatively, this strategy can be expressed as a straightforward driver behavior: “Path planning involves anticipating the amount of curvature ahead and adjusting the line to swell slightly to the opposite side. “This scenario is illustrated in Fig. 3. This model is referred to as the Preview Path-Planning Model (PPPM).

However, even if a path planning instructs the vehicle to move laterally by κd, the vehicle cannot move sideways. Therefore, when adopting the online PPPM for motion control, it is necessary to calculate κ′d for a forward distance of L′ [m], generate a target path L′ [m] ahead, and detect the lateral displacement of the vehicle and the deviation ε’ from the target path after a time of L′/V, similar to Kondo’s forward-looking path-following model [1]. Feedback control can then be implemented based on this deviation.

Fig. 2.
figure 2

Continuous Waypoints

Full size image
Fig. 3.
figure 3

Preview Path-Planning Model

Full size image

4 Verification by Numerical Calculation

To purely verify the “validity of the target path” generated by the Preview Path-Planning Model (PPPM), any tracking error caused by the vehicle model or the path-following model may introduce noise. Therefore, we generated a path using the PPPM and evaluated the path followed perfectly by the vehicle’s center of gravity without any lateral slip (the velocity vector of the center of gravity always aligns with the tangent direction of the path). Specifically, we assessed the path, yaw rate (calculated from the velocity and curvature), lateral acceleration, jerk, and the integral of the squared jerk.

4.1 Verification by Pseudo Lane Change (Constant Speed)

As the simplest example, we created a crank (pseudo lane change) course as shown in Fig. 4. The vehicle’s center of gravity moved from left to right at a constant speed of 40 km/h. The approximate lane change width is 5.2 m, and the lane transition distance is about 40 m. For the PPPM, the forward-looking time was set to 1.8 s (L = 20 m) with a gain of Cκ = 20.

As seen in Fig. 4, the PPPM generates a path offset from the center line (Normal) of the crank course. Moreover, before the Normal path curves to the right, it initially swells to the left (outside) and then exits the course in a straight line.

Fig. 4.
figure 4

Result of applying PPPM to a Lane-change-like course

Full size image

The initial outward swell in the opposite direction resembles a “counter-steering” maneuver. Assuming the vehicle follows such a path and aligns with the direction of travel, at the intersection point [X, Y] = [25 m, 0 m] where the Normal and PPPM lines cross, the vehicle positions for both paths are identical. However, it should be noted that while the yaw angle is zero for the Normal line path, a yaw angle towards the direction of travel has already developed for the PPPM line. Figures 5 and 6 compare the yaw rate and lateral acceleration.

Fig. 5.
figure 5

Comparison of yaw rate

Full size image
Fig. 6.
figure 6

Comparison of lateral acc.

Full size image

4.2 Verification on a Complex Course (Speed Planning with GVC)

In addition to the path planning by PPPM, we applied GVC and demonstrated the results of moving the vehicle’s center of gravity based on the speed plan. When applying the G-Vectoring Control (GVC) associated with lateral motion, the longitudinal acceleration Gx of the vehicle can be expressed using the lateral acceleration Gy and gain Cxy, as shown in Eq. (8) (the sign function and first-order lag element [2] are omitted).

$$ G_{x} = - C_{xy} \dot{G}_{y} $$
(8)

The vehicle’s center of gravity accelerates or decelerates solely based on the command of Eq. (8). Now, if the initial speed is V, and the speed changes by ΔV as the vehicle’s center of gravity moves along the path, this change integrates to the acceleration and deceleration commands, resulting in

$$ \Delta V = \int_{0}^{t} {G_{x} dt} = - C_{xy} \int_{0}^{t} {\dot{G}_{y} dt} = - C_{xy} G_{y} $$
(9)

Therefore, by determining the initial speed and calculating ΔV at each computational step, we can determine the speed profile (speed design) [3]. Additionally, the longitudinal and lateral accelerations at that time will form an arc in the “g-g” diagram, exhibiting the unique longitudinal and lateral motion coupling characteristics of GVC.

Figure 7 depicts the paths of the set course’s center (Centerline: Normal) and PPPM, comparing scenarios with and without GVC application, each with deceleration gain of 0.03 and acceleration gain of 0.015, as in the previous section (initial speed 40 km/h, counterclockwise, preview time 1.8 s (L = 20 m), PPPM gain Cκ = 50). Figures 8 and 9 compare longitudinal and lateral accelerations, while Figs. 10 and 11 illustrate the “g-g” diagram and the velocity of the center of gravity’s movement.

In Fig. 10, for both Normal and PPPM without GVC (blue and red lines), the longitudinal accelerations remain constant with speed, overlapping on the lateral acceleration axis. In contrast, with GVC applied (yellow and purple lines), longitudinal accelerations vary smoothly in conjunction with lateral acceleration, depicting a smooth arc-like change.

Observing Fig. 11, while the addition of GVC to the Normal line results in a slight reduction in speed compared to PPPM with GVC, the latter exhibits less speed reduction (moves faster). Nonetheless, despite this, the integration of the squared values of longitudinal and lateral jerk over one lap (cost function) as depicted in Fig. 12 and Fig. 13 shows that PPPM with GVC yields the smallest value. Thus, the effectiveness of the combined PPPM and GVC for autonomous driving route and speed planning is confirmed.

Fig. 7.
figure 7

Result of applying PPPM to a Combined course

Full size image
Fig. 8.
figure 8

Long. acc.

Full size image
Fig. 9.
figure 9

Lateral acc.

Full size image
Fig. 10.
figure 10

“g-g” diagram

Full size image
Fig. 11.
figure 11

Moving velocity

Full size image
Fig. 12.
figure 12

0.5 × (Jx2 + Jy2)

Full size image
Fig. 13.
figure 13

0.5 × ∫ (Jx2 + Jy2)dt

Full size image

5 Conclusion

In this paper, a forward-looking model termed the Preview Path-Planning Model (PPPM) is formulated, capable of computing a route (Xp, Yp) similar to that performed by humans based on the target course centerline (Xo, Yo, θo) (where the forward-looking distance is denoted as L [m]).

$$ \left[ {\begin{array}{*{20}c} {X_{p} \left( i \right)} \\ {Y_{p} \left( i \right)} \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {X_{o} \left( i \right)} \\ {Y_{o} \left( i \right)} \\ \end{array} } \right] + \left[ {\begin{array}{*{20}c} {\sin \theta_{o} \left( i \right)} \\ { - \cos \theta_{o} \left( i \right)} \\ \end{array} } \right]C_{\kappa } \left( {\kappa_{o} (i + L) - \kappa_{o} (i)} \right)V $$
(10)

Subsequently, the results of driving a lane-change course at a constant speed using PPPM and determining the speed with GVC applied to the planned route by integrating the squared values of jerk (cost function) are evaluated. The findings confirm that improved ride comfort can be achieved compared to driving along the target course centerline at a constant speed.