Advanced Frenet-Based Inverse Motion Planning for Autonomous Vehicles: Unified Mathematical Framework

1. Introduction

1.1. Problem Statement and Motivation

The choice of coordinate type for determining the trajectory model and speed distribution of an autonomous vehicle (AV) is one of the key challenges in motion modeling. On the one hand, the Cartesian coordinate system appears the most attractive, as the expressions for motion models are the simplest. However, this often complicates the formulation of motion boundaries in the same coordinates. On the other hand, it is rational to evaluate AV motion relative to a reference line, which is typically the lane midline, resulting in curvilinear coordinates. Recent surveys have systematically analyzed optimization paradigms for connected and automated vehicles, identifying first-principles model-based optimization, data-driven methods, and hybrid architectures as the dominant frameworks [1,2]. Planning AV motion within the Frenet coordinate system is primarily motivated by the structured nature of road space, clear boundaries, and smoothly varying curvature.

Consequently, the resulting motion of the AV mass center is conveniently obtained by superimposing two components: sliding along the reference line and moving perpendicularly to it. The advantage of this approach is that once the reference curve is predefined as a mathematical model, the task of finding the trajectory and distributing kinematic parameters (such as speeds and accelerations) is significantly simplified. Thus, simultaneous optimization becomes feasible rather than the traditional two-stage approach of sequential path and speed optimization. Due to the curvature, the direction perpendicular to the reference rotates, with higher AV speeds accelerating this rotation. At low speeds and with minimal curvature, this effect on relative speeds and accelerations can be neglected. However, doing so yields imprecise estimates of kinematic parameters. While this may suffice for generating variables used as optimization criteria, such inaccuracies inevitably grow as vehicle speed and reference curvature increase.

Substantial work has leveraged the Frenet frame for trajectory planning. Recent extensions include the Frenet Corridor Planner (FCP), an optimization-based local path-planning strategy that ensures smooth and safe navigation around obstacles [3], and GPU-accelerated implementations for embedded real-time systems [4]. Furthermore, polynomial-based planning within the Frenet frame has been effectively applied to lane-changing maneuvers using fifth-degree polynomials to balance comfort and safety [5].

Another challenge involves ensuring the continuity and smoothness of geometric and kinematic parameters encompassing both speed and curvature functions. To maintain the continuity of parameters such as the yaw rate, angular acceleration, and longitudinal and transverse jerks, it is necessary to ensure smoothness up to the curvature's third derivative of. This imposes strict requirements on the degree of the polynomials used: they must be sufficiently high to guarantee high differentiability, yet low enough to minimize the number of optimization parameters and ensure model stability.

A significant shortcoming of many existing approaches lies in their geometric-first orientation: they focus entirely on optimizing the spatial trajectory as the primary objective, treating kinematic parameters and controls as mere derivatives. However, forcing smoothness on the trajectory itself frequently leads to severe control jerks and actuator chattering during subsequent differentiation. In contrast, the philosophy adopted in this work recognizes that the physical trajectory is merely a consequence of properly coordinated control mechanisms. Consequently, prioritizing the smoothness and continuity of actuation and velocity references - placing them as close as possible to the controlling devices - is far more effective. The trajectory is thus treated secondarily, serving primarily as a tool for positional verification and spatial monitoring, which fundamentally bypasses the control-chattering trap inherent in conventional methods.

Satisfying such a diverse and extensive set of constraints simultaneously represents one of the most formidable challenges in motion planning. These limitations span physical and hardware boundaries (such as tire-road friction limits, motor and actuator capabilities, and signal response rates), kinematic thresholds (velocities, accelerations, and longitudinal/transverse jerks), and spatial corridors (strict containment within safety boundaries narrower than the physical lane). A critical issue arises from the intrinsic coupling of these constraints: fulfilling them all concurrently places a severe burden on the optimization process. This friction is particularly pronounced in conventional two-stage frameworks - where path generation and speed profile distribution are decoupled. In such sequential approaches, optimizing the trajectory first often leads to a kinematic dead end or suboptimal regions when speed allocation is applied afterward.

Thus, the problem is to develop a more accurate model that incorporates both the geometric relationship between the trajectory and the reference, and the kinematic relationship determined by the ratio of speeds along and perpendicular to the reference line. Although this mathematical framework is more complex, it improves forecasting efficiency by reducing the degrees of freedom required to shape the trajectory. Consequently, the primary computational load of the optimization procedure shifts toward managing the speed in mutually perpendicular directions. Recent optimization-based approaches have explored various formulations to manage these constraints, including control barrier functions for safety-critical limits [6] and bi-level quadratic programming for real-time feasibility [7].

1.2. Research Gap and Contribution

Despite the substantial progress in Frenet-based motion planning, several limitations persist in existing approaches. First, many methods employ sequential path-speed decomposition, which can lead to suboptimal solutions and control chattering [8]. While recent advances in path-velocity coupled planning have attempted to mitigate this issue by using sparse, normal-plane-constrained trajectories [9], they often lack the integrated kinematic continuity required for smooth actuation. Inverse dynamics-based approaches have further advanced this paradigm by centering the optimization on an inverse approach that concurrently optimizes trajectory and speed while ensuring kinematic continuity [10], yet these methods have not been extended to higher-order curvature continuity in the Frenet frame.

Second, trajectory representations often lack the higher-order continuity required for smooth actuation, with typical approaches ensuring only $G^1$ or $G^2$ continuity [11]. Although Hermite spline-based representations have shown promise in spatiotemporal optimization, they have not been systematically formulated within a unified simultaneous optimization framework for high-order ($C^3$) curvature derivatives. Hermite curve interpolation has also been applied for obstacle avoidance [12], but generally without consideration of the full kinematic profile.

Third, the reference curve generation and speed profile optimization are often treated as independent problems, missing opportunities for synergistic improvement. Concurrently, learning-based motion planning has emerged as a promising direction, with data-driven optimal control approaches seeking to synergize theoretical guarantees of optimal control with the adaptive capabilities of machine learning [13]. However, these methods often struggle with guaranteed constraint satisfaction and interpretability, underscoring the continued need for robust optimization-based frameworks like the one proposed herein.

The proposed framework addresses these limitations through three key innovations: (1) a simultaneous trajectory and speed optimization approach that eliminates the need for sequential path-speed decomposition, (2) a high-order differentiable virtual rail representation with $C^3$ continuity of curvature using Hermite polynomials, and (3) a unified finite element formulation that enables consistent optimization of both geometric and kinematic parameters within the Frenet coordinate system. This integrated approach ensures maximum trajectory smoothness, eliminates control chattering, and provides a rigorous mathematical foundation for complex motion planning in autonomous vehicles.

The paper is structured as follows: Section 2 reviews relevant literature. Section 3 discusses the mathematical foundations of reference and trajectory representation in the Frenet coordinate system, covering concepts like the virtual rail and rail optimization. Section 4 derives vehicle kinematic parameters in the Frenet system and describes transformations to local coordinates. Section 5 presents a framework for dynamic speed profile generation and optimization. Section 6 presents simulation results for lane-change maneuvers on a curved three-lane road, examining aspects such as smoothness and constraint adherence. Section 7 concludes with a summary of findings and future research directions.

2. Literature Review

2.1. Motion Planning for Autonomous Vehicles: Frameworks and Classifications

Motion planning for AVs has emerged as one of the most critical and extensively researched areas in intelligent transportation systems. The trajectory planning problem fundamentally involves generating a time-parameterized path from a starting state to a desired goal state while satisfying vehicle dynamics, kinematic constraints, safety requirements, and ride comfort criteria. Existing approaches are commonly classified into five broad categories: graph search methods, sampling-based methods, mathematical optimization, curve interpolation, and machine learning-based approaches [14].

Among these, optimization-based trajectory planning has gained substantial traction due to its ability to handle multiple objectives and constraints simultaneously. Optimization-based approaches can be further subdivided into model predictive control (MPC), which solves a receding-horizon optimal control problem [1], and offline trajectory optimization, which generates a complete trajectory over a fixed planning horizon. Recent surveys have systematically analyzed optimization paradigms for connected and automated vehicles, identifying first-principles model-based optimization, data-driven methods, and hybrid architectures as the dominant frameworks. Learning-based motion planning has also emerged as a promising direction, with data-driven optimal control (DDOC) approaches seeking to synergize theoretical guarantees of optimal control with adaptive capabilities of machine learning.

2.2. The Frenet Coordinate System: Foundations and Advantages

The choice of coordinate system fundamentally influences the formulation and tractability of the motion planning problem. While the Cartesian coordinate system simplifies kinematic expressions, it complicates the representation of road boundaries and lane constraints [1]. The Frenet (or Frenet-Serret) frame, by contrast, provides a natural coordinate system for road-constrained vehicle motion. In the Frenet frame, the vehicle's position is decomposed into a longitudinal coordinate $s$ along a reference path (typically the lane centerline) and a lateral coordinate $L$ perpendicular to it [14].

The Frenet frame offers several inherent advantages for on-road trajectory planning. First, it decouples the two-dimensional vehicle motion into independent longitudinal and lateral components, significantly simplifying the planning problem [14]. Second, it transforms arbitrarily shaped road geometries into a straight-line representation, reducing the complexity of constraints associated with the reference line [15]. Third, the structured nature of the Frenet frame facilitates the formulation of boundary constraints and enables efficient trajectory generation through polynomial or spline-based parameterization [16].

A substantial body of research has leveraged the Frenet frame for trajectory planning. Werling et al. pioneered the Frenet-based optimal trajectory generation framework for dynamic street scenarios, which has become a cornerstone of modern AV motion planning [14]. Subsequent work has extended this foundation with various enhancements, including the Frenet Enveloping Planner, which prioritizes smoothness and low computational complexity for scalable local path planning, and the Frenet Corridor Planner, an optimization-based strategy that ensures smooth and safe navigation around obstacles. The FRENETIX framework further demonstrated a high-performance, modular motion-planning approach that leverages Frenet frame optimization with enhanced computational efficiency and adaptability [15].

2.3. Simultaneous Trajectory and Speed Optimization

A persistent challenge in motion planning is the inherent coupling between the spatial trajectory and the temporal speed profile. Traditional planning architectures adopt a two-stage sequential approach: path planning first generates a geometric path, followed by speed profile optimization along that fixed path [17]. While computationally efficient, this decoupling can lead to suboptimal solutions, as the geometric path may not accommodate feasible speed distributions, particularly under complex constraints such as high curvature, dynamic obstacles, or aggressive maneuvers.

To address this limitation, simultaneous trajectory and speed optimization has emerged as a more integrated approach. By optimizing both the spatial geometry and temporal kinematics concurrently, these methods can achieve superior performance in terms of smoothness, comfort, and dynamic feasibility [17]. Recent work has demonstrated the efficacy of simultaneous optimization using finite-element representations in Frenet coordinates, where the trajectory parameters and speed distribution are determined within a unified optimization framework [1]. Inverse dynamics-based approaches have further advanced this paradigm by centering the optimization on an inverse approach that concurrently optimizes trajectory and speed while ensuring kinematic continuity [10].

The proposed framework in this paper builds upon and extends these simultaneous optimization principles. While existing approaches typically employ a path-speed decomposition within the Frenet frame [15], our method introduces a unified optimization that simultaneously determines both the geometric rail and the speed distribution, eliminating the need for sequential processing and fundamentally bypassing the control-chattering trap inherent in conventional two-stage methods.

2.4. Trajectory Representation: Polynomials, Splines, and Hermite Interpolation

The representation of trajectories profoundly affects the smoothness, continuity, and computational tractability of the planning problem. Various parametric representations have been explored, each offering different trade-offs between flexibility, continuity, and optimization complexity. Polynomial trajectories are widely used due to their simplicity and differentiability. Quintic polynomials are commonly employed in the Frenet frame to generate spatially feasible trajectories, as they provide sufficient degrees of freedom to satisfy position, velocity, and acceleration boundary conditions [15]. Fifth-degree polynomials have also been used to generate lane-changing trajectories, enabling smooth transitions while maintaining comfort constraints [2].

Bézier curves offer another powerful representation, leveraging control points to intuitively shape trajectories while providing guaranteed properties such as convex hull containment and endpoint interpolation. Recent research has applied Bézier curves for AV trajectory planning with $G^2$ continuity, and for enforcing geometric and obstacle-avoidance constraints through linear parameterization [17]. Hermite splines provide a particularly attractive framework for trajectory planning due to their ability to enforce continuity of position, velocity, acceleration, and higher-order derivatives at segment boundaries [18]. The MIGHTY planner demonstrated that Hermite spline-based spatiotemporal optimization can significantly reduce in computation time while maintaining a 100% success rate [19]. Cubic Hermite splines have been used to describe curvature-continuous trajectories for autonomous vehicles, ensuring smooth transitions between waypoints [20]. Hermite interpolation has also been applied for estimating and updating trajectories based on error dynamics, leveraging the endpoint behavior control properties of Hermite splines [18].

The proposed framework extends Hermite-based representation to the third derivative of curvature, employing a two-degree-of-freedom finite element model with cubic Hermite polynomials. This enables $C^3$ continuity of the reference curvature and speed profiles, which is essential for eliminating discontinuous control inputs and achieving maximum trajectory smoothness - a requirement that conventional polynomial or Bézier representations cannot guarantee without higher-order formulations.

2.5. Optimization Methods and Constraint Handling

The solution of the trajectory optimization problem typically requires nonlinear programming techniques capable of handling multiple objectives and constraints. Sequential Quadratic Programming (SQP) has been widely adopted for its efficiency and robustness for medium-scale nonlinear optimization problems [16]. SQP iteratively solves a quadratic programming subproblem at each iteration, making it well-suited for trajectory-planning applications where the objective function and constraints are smooth yet nonlinear.

Constraint handling is a critical aspect of trajectory optimization. Constraints may include vehicle kinematic limits (velocity, acceleration, jerk), dynamic limits (tire-road friction, actuator saturation), spatial boundaries (lane boundaries, obstacle avoidance), and continuity requirements (smooth stitching between trajectory segments. Recent approaches have explored various constraint formulations, including control barrier functions for safety-critical constraints [15], spatiotemporal corridors for collision-free navigation, and dynamic risk fields for assessing obstacle risks [16].

The proposed framework incorporates a comprehensive set of constraints spanning kinematic limits, dynamic feasibility, spatial boundaries, and continuity requirements. The finite element formulation enables systematic enforcement of continuity conditions at element boundaries, while the integral constraint approach ensures that kinematic parameters remain within prescribed bounds throughout the entire trajectory.

2.6. The Virtual Rail Concept and Reference Path Generation

The concept of a virtual rail or virtual track has emerged as a promising paradigm for structured AV motion planning. In this framework, vehicles follow predefined paths - virtual rails - that serve as spatial guides for motion planning and control. The virtual rail concept draws inspiration from railway operations, where trains follow fixed tracks, and applies similar principles to guide autonomous vehicles along road networks. Virtual rail-based systems offer several advantages for AV motion planning. They provide a stable reference for trajectory generation, simplify the formulation of spatial constraints, and enable rapid convergence of optimization processes. The virtual rail can be continuously updated based on sensor data, road geometry, and traffic conditions, enabling dynamic adaptation while preserving the benefits of a structured reference [20].

In the context of Frenet-based planning, the virtual rail serves as the reference curve from which longitudinal and lateral displacements are measured. The quality of this reference - its smoothness, continuity, and adherence to road geometry - directly influences the quality of the resulting trajectory. The proposed framework introduces a systematic two-stage pipeline for virtual rail generation: a preliminary spline provides initial geometric information, followed by an optimization procedure that constructs a high-order differentiable reference rail satisfying $C^3$ continuity of curvature.

3. Frenet System: Mathematics for Reference and Trajectory

3.1. Description of the Modeling Approach

Consider an autonomous vehicle (AV) operating within a sensor-observable horizon, where the road curvature allows estimation of the lane centerline over a distance that significantly exceeds the vehicle's length. In this framework, the motion of the AV's center of mass is decomposed (Figure 1) into a longitudinal displacement along the reference curve ${\overrightarrow{s}}_r$ and a lateral displacement $L$ along the normal vector ${\overrightarrow{\nu }}_r$. To ensure the generated motion trajectory ${\overrightarrow{s}}_t$ maintains maximum smoothness, the yaw angle must be precisely determined relative to the reference geometry. Given a known reference curve, the optimal trajectory search involves a dynamic variation of the parameters $s_r$ and $L$. Consequently, the primary objective is to establish a rigorous mathematical relationship between the velocity distribution functions along the reference curve and its normal. Assuming the reference coordinates $x_r\left(s_r\right)$ and $y_r\left(s_r\right)$ are defined as functions of the parameter $s_r$, the trajectory points ${\overrightarrow{s}}_t$ can be derived as follows:

$$x_t=x_r-L\mathit{sin}\left({\alpha }_r\right),y_t=y_r+L\mathit{cos}\left({\alpha }_r\right)$$

(1)

To further analyze the geometric relationships, consider the formation of the trajectory arc differential (as illustrated in Figure 2). We define an infinitesimal segment of the reference arc $d{s}_r$, subtended by the central angle $d{\alpha }_r$ between two adjacent radii $R_r$. Analogously, the trajectory arc element $d{s}_t$ is formed between the radii $R_t$ within the angle $d{\alpha }_t$. Under the assumption of small angular increments, the differentials of the reference and trajectory arcs can be expressed as:

$$d{s}_r=R_r\mathit{sin}\left(d{\alpha }_r\right)\approx R_{r}d{\alpha }_r,d{s}_t=R_t\mathit{sin}\left(d{\alpha }_t\right)\approx R_{t}d{\alpha }_t$$

(2)

Considering a point characterized by the coordinate $s_r$, tangent slope angle ${\alpha }_r$, and lateral displacement $L$, a differential transition to the coordinate $s_r+d{s}_r$ results in updated parameters ${\alpha }_r+d{\alpha }_r$ and $L+dL$. By projecting an arc segment $d{p}_t$ parallel to $d{s}_r$ from the point $s_t$, a reference point for the increment $dL$ is established at the intersection with the normal $L$. Consequently, the longitudinal component of the trajectory along the reference curve is expressed as follows:

$$d{p}_t=d{s}_r-Ld{\alpha }_r=\left(1-L{\displaystyle \frac{d{\alpha }_r}{d{s}_r}}\right)d{s}_r=\left(1-L{K}_r\right)d{s}_r=k_{rL}d{s}_r$$

(3)

where the geometric scaling factor $k_{rL}$ is defined as:

$$k_{rL}=1-L{K}_r={\displaystyle \frac{d{p}_t}{d{s}_r}}$$

In the limit of infinitesimal values (Figure 2b), the segments $d{p}_t$ and $d{s}_r$ are approximated as chords, forming a right triangle with $dL$. Consequently, the elevation angle ${\alpha }_p$ is defined as:

$${\alpha }_p=arctan\left({\displaystyle \frac{dL}{d{p}_t}}\right)$$

(4)

The resulting trajectory slope angle ${\alpha }_t$, relative to the reference angle ${\alpha }_r$, is then given by:

$${\alpha }_t={\alpha }_r+{\alpha }_p$$

(5)

To describe the motion dynamics, we define the velocity components $V_r$ along the reference curve and $V_L$ transverse to it. Their relationship is established through the time derivative of the displacement:

$$V_r={\displaystyle \frac{d{s}_r}{dt}},V_L={\displaystyle \frac{dL}{dt}}={\displaystyle \frac{dL}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{dL}{d{s}_r}}{V}_r,v_{Lr}={\displaystyle \frac{V_L}{V_r}}$$

(6)

Defining the lateral velocity ratio as $v_{Lr}=V_L/V_r$, we can express the derivative of $L$ with respect to the reference coordinate $s_r$ as:

$${\displaystyle \frac{dL}{d{s}_r}}={\displaystyle \frac{V_L}{V_r}}=v_{Lr}$$

(7)

Finally, neglecting higher-order infinitesimals, the differential of the trajectory arc length $d{s}_t$ is expressed using the Pythagorean theorem:

$$d{s}_t=\sqrt{{dp}_t^2+{dL}^2}=\sqrt{{ds}_r^2{k}_r^2+{dL}^2}=d{s}_r\sqrt{k_r^2+{\left({\displaystyle \frac{dL}{d{s}_r}}\right)}^2}=d{s}_r\sqrt{k_{rL}^2+v_{Lr}^2}$$

(8)

For the specific case where $V_L=0$ (i.e., pure longitudinal motion relative to the reference curve), the differential arc length simplifies to:

$$d{s}_t=d{s}_r\sqrt{k_{rL}^2+{\left(0\right)}^2}=k_{rL}d{s}_r=d{p}_t$$

This demonstrates that the trajectory is composed of distinct geometrical and relative velocity components. The ratio between the differentials of the reference and trajectory arcs, denoted as $s_{tr}^{\text{'}}$, is defined as:

$$s_{tr}^{\text{'}}={\displaystyle \frac{d{s}_t}{d{s}_r}}=\sqrt{k_{rL}^2+v_{Lr}^2},s_{rt}^{\text{'}}={\displaystyle \frac{d{s}_r}{d{s}_t}}={\displaystyle \frac{1}{\sqrt{k_{rL}^2+v_{Lr}^2}}}={\displaystyle \frac{1}{s_{tr}^{\text{'}}}}$$

(9)

Returning to the geometric relations established in equations (4) and (5), we can derive the elevation angle ${\alpha }_p$ in terms of our velocity ratios:

$${\alpha }_p=arctan\left({\displaystyle \frac{dL}{d{p}_t}}\right)=arctan\left({\displaystyle \frac{dL}{d{s}_r}}/{\displaystyle \frac{d{p}_t}{d{s}_r}}\right)=arctan\left({\displaystyle \frac{v_{Lr}}{k_{rL}}}\right)$$

(10)

Having established the angular relationship, we now analyze the curvature transition from the reference $K_r$ to the target trajectory $K_t$.

$$K_r={\displaystyle \frac{d{\alpha }_r}{d{s}_r}},K_t={\displaystyle \frac{d{\alpha }_t}{d{s}_t}}={\displaystyle \frac{d{\alpha }_t}{d{s}_r}}{\displaystyle \frac{d{s}_r}{d{s}_t}}={\displaystyle \frac{d{\alpha }_t}{d{s}_r}}{s}_{rt}^{\text{'}}$$

(11)

To establish a comprehensive relationship between the parameters of the trajectory and the reference path, we derive the angular evolution along the reference arc $s_r$:

$${\displaystyle \frac{d{\alpha }_t}{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}\left({\alpha }_r+{\alpha }_p\right)={\displaystyle \frac{d{\alpha }_r}{d{s}_r}}+{\displaystyle \frac{d{\alpha }_p}{d{s}_r}}=K_r+{\displaystyle \frac{d{\alpha }_p}{d{s}_r}}$$

(12)

To evaluate the term $d{\alpha }_p/d{s}_r$, we apply the derivative to the previously defined expression for ${\alpha }_p$:

$${\displaystyle \frac{d{\alpha }_p}{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}arctan\left({\displaystyle \frac{v_{Lr}}{k_{rL}}}\right)={\displaystyle \frac{1}{1+{\left(\frac{v_{Lr}}{k_{rL}}\right)}^2}}{\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{v_{Lr}}{k_{rL}}}\right)$$

(13)

By substituting the relationship for $s_{rt}^{\text{'}2}$, the expression simplifies significantly. Applying the quotient rule to the derivative term, we obtain:

$$s_{rt}^{\text{'}2}={\displaystyle \frac{1}{k_{rL}^2+v_{Lr}^2}},{\displaystyle \frac{1}{1+{\left(\frac{v_{Lr}}{k_{rL}}\right)}^2}}={\displaystyle \frac{k_{rL}^2}{k_{rL}^2+v_{Lr}^2}}=s_{rt}^{\text{'}2}{k}_{rL}^2,{\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{v_{Lr}}{k_{rL}}}\right)={\displaystyle \frac{1}{k_{rL}^2}}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)$$

Then,

$${\displaystyle \frac{d{\alpha }_p}{d{s}_r}}=s_{rt}^{\text{'}2}{k}_{rL}^2{\displaystyle \frac{1}{k_{rL}^2}}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)=s_{rt}^{\text{'}2}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)$$

(14)

To finalize the expression for $K_t$, we first evaluate the derivatives involved in equation (14):

$${\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{V_L}{V_r}}\right)={\displaystyle \frac{1}{V_r^2}}\left(V_r{\displaystyle \frac{d{V}_L}{d{s}_r}}-V_L{\displaystyle \frac{d{V}_r}{d{s}_r}}\right)$$

(15)

$${\displaystyle \frac{d{k}_{rL}}{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}\left(1-L{K}_r\right)=-\left({\displaystyle \frac{dL}{d{s}_r}}{K}_r+L{\displaystyle \frac{d{K}_r}{d{s}_r}}\right)=-\left(v_{Lr}{K}_r+L{\displaystyle \frac{d{K}_r}{d{s}_r}}\right)$$

Substituting these back into our expression for angular evolution, the rate of change of the trajectory angle with respect to the reference arc becomes:

$${\displaystyle \frac{d{\alpha }_t}{d{s}_r}}=K_r+s_{rt}^{\text{'}2}{k}_{rL}^2{\displaystyle \frac{1}{k_{rL}^2}}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)=K_r+s_{rt}^{\text{'}2}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)$$

(16)

Finally, by applying the chain rule, we express the trajectory curvature $K_t$ in terms of the reference parameters:

$$K_t=s_{rt}^{\text{'}}{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}=s_{rt}^{\text{'}}\left(K_r+s_{rt}^{\text{'}2}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)\right)$$

(17)

For higher-order control tasks, the derivative of the trajectory curvature is obtained as follows:

$${\displaystyle \frac{d{K}_t}{d{s}_t}}={\displaystyle \frac{d{K}_t}{d{s}_r}}{\displaystyle \frac{d{s}_r}{d{s}_t}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}{s}_{rt}^{\text{'}}\right){\displaystyle \frac{d{s}_r}{d{s}_t}}=s_{rt}^{\text{'}}\left(s_{rt}^{\text{'}}{\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}+{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\right)$$

(18)

To determine the evolution of the trajectory curvature, we derive the second-order angular relationship by differentiating Eq. (16) with respect to $s_r$:

$${\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}\right)={\displaystyle \frac{d{K}_r}{d{s}_r}}+\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right){\displaystyle \frac{d}{d{s}_r}}\left(s_{rt}^{\text{'}2}\right)+$$

(19)

$$+s_{rt}^{\text{'}2}{\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)={\displaystyle \frac{d{K}_r}{d{s}_r}}+{\displaystyle \frac{2}{s_{rt}^{\text{'}}}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K_r\right)+s_{rt}^{\text{'}2}\left({\displaystyle \frac{d^2{v}_{Lr}}{d{s}_{rt}^2}}{k}_{rL}-{\displaystyle \frac{d^2{k}_{rL}}{s_{rt}^2}}{v}_{Lr}\right)$$

Here, based on Eq. (16), we may deduce that

$${\displaystyle \frac{1}{s_{rt}^{\text{'}2}}}\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K_r\right)=\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{k}_{rL}-{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{v}_{Lr}\right)$$

To evaluate the derivative of the scaling factor $s_{rt}^{\text{'}}$, we differentiate its expression with respect to $s_r$:

$${\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}\left({\left(\sqrt{k_{rL}^2+v_{Lr}^2}\right)}^{-1}\right)=-{\displaystyle \frac{\frac{d}{d{s}_r}\left(k_{rL}^2+v_{Lr}^2\right)}{2{\left(\sqrt{k_{rL}^2+v_{Lr}^2}\right)}^3}}=-s_{rt}^{\text{'}3}\left(k_{rL}{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}+v_{Lr}{\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}\right)$$

(20)

The second derivatives from Eq. (19), including Eq. (15)

$${\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}\right)={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{1}{V_r^2}}\left(V_r{\displaystyle \frac{d{V}_L}{d{s}_r}}-V_L{\displaystyle \frac{d{V}_r}{d{s}_r}}\right)\right)=\left(V_r{\displaystyle \frac{d{V}_L}{d{s}_r}}-V_L{\displaystyle \frac{d{V}_r}{d{s}_r}}\right){\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{1}{V_r^2}}\right)+$$

(21)

$$+{\displaystyle \frac{1}{V_r^2}}{\displaystyle \frac{d}{d{s}_r}}\left(V_r{\displaystyle \frac{d{V}_L}{d{s}_r}}-V_L{\displaystyle \frac{d{V}_r}{d{s}_r}}\right)={\displaystyle \frac{1}{V_r^2}}\left(V_r{\displaystyle \frac{d^2{V}_L}{d{s}_r^2}}-V_L{\displaystyle \frac{d^2{V}_r}{d{s}_r^2}}\right)-{\displaystyle \frac{2}{V_r}}{\displaystyle \frac{d{V}_r}{d{s}_r}}{\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}$$

And the second derivative of the relative curvature, considering Eq. (15), is given by:

$${\displaystyle \frac{d^2{k}_{rL}}{{ds}_r^2}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{k}_{rL}}{d{s}_r}}\right)=-{\displaystyle \frac{d}{d{s}_r}}\left(K_r{v}_{Lr}+L{\displaystyle \frac{d{K}_r}{d{s}_r}}\right)=-\left(K_r{\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}+2v_{Lr}{\displaystyle \frac{d{K}_r}{d{s}_r}}+L{\displaystyle \frac{d^2{K}_r}{{ds}_r^2}}\right)$$

(22)

To fully characterize trajectory curvature continuity, we extend our derivation to the second-order derivative with respect to the trajectory arc length $s_t$

$${\displaystyle \frac{d^2{K}_t}{d{s}_t^2}}={\displaystyle \frac{d}{d{s}_t}}\left({\displaystyle \frac{d{K}_t}{d{s}_t}}\right)={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{K}_t}{d{s}_r}}{s}_{rt}^{\text{'}}\right){\displaystyle \frac{d{s}_r}{d{s}_t}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{K}_t}{d{s}_r}}{s}_{rt}^{\text{'}}\right)s_{rt}^{\text{'}}=s_{rt}^{\text{'}}\left(s_{rt}^{\text{'}}{\displaystyle \frac{d^2{K}_t}{d{s}_r^2}}+{\displaystyle \frac{d{K}_t}{d{s}_r}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\right)$$

(23)

By projecting these dynamics onto the reference path arc length $s_r$, we isolate the contribution of the reference geometry:

$${\displaystyle \frac{d^2{K}_t}{d{s}_r^2}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{K}_t}{d{s}_r}}\right)={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}{s}_{rt}^{\text{'}}+{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\right)={\displaystyle \frac{d^3{\alpha }_t}{d{s}_r^3}}{s}_{rt}^{\text{'}}+2{\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}+{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}{\displaystyle \frac{d^2{s}_{rt}^{\text{'}}}{d{s}_r^2}}$$

(24)

Furthermore, to facilitate robust motion planning under varying curvature, we define the third-order derivative of the orientation angle ${\alpha }_t$, which accounts for higher-order geometric transitions:

$${\displaystyle \frac{d^3{\alpha }_t}{d{s}_r^3}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}\right)={\displaystyle \frac{d^2{K}_r}{d{s}_r^2}}+{\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{2}{s_{rt}^{\text{'}}}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K_r\right)\right)+{\displaystyle \frac{d}{d{s}_r}}\left(s_{rt}^{\text{'}2}\left({\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}{k}_{rL}-{\displaystyle \frac{d^2{k}_{rL}}{s_{rt}^2}}{v}_{Lr}\right)\right)$$

(25)

The expansion of the second term within Eq. (25) is calculated as follows:

$${\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{2}{s_{rt}^{\text{'}}}}{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K_r\right)\right)=2\left(\left({{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K}_r\right){\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{s}_{rt}^{\text{'}}/d{s}_r}{s_{rt}^{\text{'}}}}\right)+{\displaystyle \frac{d{s}_{rt}^{\text{'}}/d{s}_r}{s_{rt}^{\text{'}}}}{\displaystyle \frac{d}{d{s}_r}}\left({{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K}_r\right)\right)=$$

(26)

$$={\displaystyle \frac{2}{s_{rt}^{\text{'}}}}\left({\displaystyle \frac{1}{s_{rt}^{\text{'}}}}\left({\displaystyle \frac{d^2{s}_{rt}^{\text{'}}}{d{s}_r^2}}{s}_{rt}^{\text{'}}-{\left({\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\right)}^2\right)\left({{\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-K}_r\right)+{\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\left({\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}-{\displaystyle \frac{d{K}_r}{d{s}_r}}\right)\right)$$

The third term within Eq. (25)

$${\displaystyle \frac{d}{d{s}_r}}\left(s_{rt}^{\text{'}2}\left({\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}{k}_{rL}-{\displaystyle \frac{d^2{k}_{rL}}{s_{rt}^2}}{v}_{Lr}\right)\right)=2s_{rt}^{\text{'}}\left({\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}{k}_{rL}-{\displaystyle \frac{d^2{k}_{rL}}{s_{rt}^2}}{v}_{Lr}\right){\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}+$$

(27)

$$+s_{rt}^{\text{'}2}\left({\displaystyle \frac{d^3{v}_{Lr}}{d{s}_r^3}}{k}_{rL}-{\displaystyle \frac{d^3{k}_{rL}}{s_{rt}^3}}{v}_{Lr}+{\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}{\displaystyle \frac{d{k}_{rL}}{d{s}_r}}-{\displaystyle \frac{d^2{k}_{rL}}{s_{rt}^2}}{\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}\right)$$

The third-order derivative of the velocity scale $v_{Lr}$ with respect to the path parameter $s_r$ in Eq. (27)

$${\displaystyle \frac{d^3{v}_{Lr}}{d{s}_r^3}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}\right)=-{\displaystyle \frac{2}{V_r^3}}\left(V_r{\displaystyle \frac{d^2{V}_L}{d{s}_r^2}}-V_L{\displaystyle \frac{d^2{V}_r}{d{s}_r^2}}\right){\displaystyle \frac{d{V}_r}{d{s}_r}}+$$

(28)

$$+{\displaystyle \frac{1}{V_r^2}}\left(V_r{\displaystyle \frac{d^3{V}_L}{d{s}_r^3}}-V_L{\displaystyle \frac{d^3{V}_r}{d{s}_r^3}}+{\displaystyle \frac{d{V}_r}{d{s}_r}}{\displaystyle \frac{d^2{V}_L}{d{s}_r^2}}-{\displaystyle \frac{d{V}_L}{d{s}_r}}{\displaystyle \frac{d^2{V}_r}{d{s}_r^2}}\right)-{\displaystyle \frac{2}{V_r}}\left(\left({\displaystyle \frac{d^2{V}_L}{d{s}_r^2}}-{\displaystyle \frac{1}{V_r}}{\left({\displaystyle \frac{d{V}_r}{d{s}_r}}\right)}^2\right){\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}+{\displaystyle \frac{d{V}_r}{d{s}_r}}{\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}\right)$$

The third-order derivative of $k_{rL}$ in Eq. (27)

$${\displaystyle \frac{d^3{k}_{rL}}{s_{rt}^3}}=-\left(K_r{\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}+3\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{\displaystyle \frac{d{K}_r}{d{s}_r}}+v_{Lr}{\displaystyle \frac{d^2{K}_r}{{ds}_r^2}}\right)+L{\displaystyle \frac{d^3{K}_r}{{ds}_r^3}}\right)$$

(29)

The second-order derivative of $s_{rt}^{\text{'}}$ in Eq. (24)

$${\displaystyle \frac{d^2{s}_{rt}^{\text{'}}}{d{s}_r^2}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\right)={\displaystyle \frac{3}{s_{rt}^{\text{'}}}}{\left({\displaystyle \frac{d{s}_{rt}^{\text{'}}}{d{s}_r}}\right)}^2-s_{rt}^{\text{'}3}\left({\left({\displaystyle \frac{d{k}_{rL}}{d{s}_r}}\right)}^2+{\left({\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}\right)}^2+k_{rL}{\displaystyle \frac{d^2{k}_{rL}}{d{s}_r^2}}+v_{Lr}{\displaystyle \frac{d^2{v}_{Lr}}{d{s}_r^2}}\right)$$

(30)

Conclusion. Consequently, as evidenced by the derived components in Eqs. (28)-(30), achieving maximum trajectory smoothness is predicated on ensuring the continuity of the third-order derivatives of the reference curvature $K_r$ and the velocity profiles$V_r$, $V_L$. This analytical dependency underscores a critical design requirement: to eliminate discontinuous control inputs, the curvature and speed generation layer must enforce C³ continuity across the reference frame $s_r$.

3.2. Mathematical Approach to a Virtual Rail Modeling

To eliminate the impact of nodal discontinuities on the transition dynamics between segments, smooth functions alone are insufficient; they must also ensure high-order differentiability. Consequently, it is necessary to construct a reference curve that functions as a «virtual rail», providing stability and rapid convergence for optimization processes. Thus, the trajectory planning task must be decoupled into a purely spatial sub-problem (the rail - reference) and a spatiotemporal (velocity-constrained) sub-problem.

Representation of the Target Functions via Piecewise Polynomials. Consider a finite element of length $S$, within which a function $g\left(s\right)$ approximates the target function. Assume that this function can be represented with sufficient accuracy by a Lagrange polynomial of degree $p$. Alternatively, the same function can be expressed through a set of shape functions $\mathit{F}$ and nodal parameters $\mathit{Q}$, which represent the degrees of freedom (DOF) at the finite element's nodes. Thus, we can write:

$$\mathit{A}=\left(\begin{array}{c}{a}_0\\ ⋮\\ a_p\end{array}\right),\mathit{X}=\left(\begin{array}{c}{s}^0\\ ⋮\\ s^p\end{array}\right),\mathit{Q}=\left(\begin{array}{c}{q}_1\\ ⋮\\ q_{p+1}\end{array}\right),\mathit{F}=\left(\begin{array}{c}{f}_1\\ ⋮\\ f_{p+1}\end{array}\right),g\left(s\right)={\mathit{A}}^T\mathit{X}={\mathit{Q}}^T\mathit{F}$$

(31)

where $a_j$ = polynomial coefficient, j $\in$ [0, $p$], $f_j$ = shape function, and $q_j$ = DOF, j $\in$ [1, $p$ +1].

For a two-node finite element (FE), the polynomial degree $p$ must be odd. Based on the nodal coordinates $\left[0,S\right]$, the matrix $\mathit{B}$ is constructed to enforce continuity up to the $k$-th derivative at the nodes:

$${\mathit{B}}_0=\left(\begin{array}{c}{\left.{\mathit{X}}^T\right|}_0\\ ⋮\\ {\left.{\displaystyle \frac{d^k{\mathit{X}}^T}{d{x}^k}}\right|}_0\end{array}\right),{\mathit{B}}_S=\left(\begin{array}{c}{\left.{\mathit{X}}^T\right|}_S\\ ⋮\\ {\left.{\displaystyle \frac{d^k{\mathit{X}}^T}{d{x}^k}}\right|}_S\end{array}\right),\mathit{B}=\left(\begin{array}{c}{\mathit{B}}_0\\ {\mathit{B}}_S\end{array}\right),\mathit{F}={\left({\mathit{B}}^T\right)}^{-1}\mathit{X}$$

(32)

Assuming $s=\xi S$, where the dimensionless parameter $\xi \in \left[0,1\right]$, we transition to basis functions ${\mathit{F}}_{\xi }$ corresponding to a unit-length FE. Consequently, for the $i$-th element, the following relations hold:

$${\mathit{s}}_i=\left(\begin{array}{ccc}{S}_i^0& \dots & S_i^k\end{array}\right),{\mathit{S}}_i=diag\left(\begin{array}{cc}{\mathit{s}}_i& {\mathit{s}}_i\end{array}\right),g_i\left(s\right)=g_i\left({\mathit{Q}}_i,S_i,\xi \right)={\mathit{Q}}_i^T{\mathit{S}}_i{\mathit{F}}_{\xi }$$

(33)

Furthermore, the $k$-th derivative and the first integrals of the function $g\left(s\right)$ are defined as:

$${\displaystyle \frac{d^k{g}_i}{d{s}^k}}={\mathit{Q}}_i^T\left({\displaystyle \frac{{\mathit{S}}_i}{S_i^k}}\right){\displaystyle \frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}},\int g_{i}ds={\mathit{Q}}_i^T{\mathit{S}}_i{S}_i\int {\mathit{F}}_{\xi }d\xi ,\iint g_{i}d{s}^2={\mathit{Q}}_i^T{\mathit{S}}_i{S}_i^2\iint {\mathit{F}}_{\xi }d{\xi }^2$$

(34)

Two-DOF Model. In this formulation, the second derivative of curvature is assigned as the primary function, while a cubic Hermite spline is employed for segment coupling. Consequently, each node requires two parameters ${\mathit{q}}_{ri}$, resulting in a total of four parameters ${\mathit{Q}}_{ri}$ per segment:

$${\mathit{q}}_{ri}={\left(\begin{array}{cc}{\left.{\displaystyle \frac{d^2{K}_r}{{ds}_r^2}}\right|}_i& {\left.{\displaystyle \frac{d^3{K}_r}{{ds}_r^3}}\right|}_i\end{array}\right)}^T,{\mathit{Q}}_{ri}=\left(\begin{array}{c}{\mathit{q}}_{ri}\\ {\mathit{q}}_{r\left(i+1\right)}\end{array}\right)$$

(35)

For each $i$-th segment $S_{ri}$ within the planning horizon, the mathematical model is established using nodal parameters, segment length, and basis functions, such that:

$${\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}={\mathit{Q}}_{ri}^T{\mathit{S}}_{ri}{\mathit{F}}_{\xi },{\displaystyle \frac{d^3{K}_{ri}}{{ds}_r^3}}={\mathit{Q}}_{ri}^T\left({\displaystyle \frac{{\mathit{S}}_{ri}}{S_{ri}}}\right){\displaystyle \frac{d{\mathit{F}}_{\xi }}{d\xi }}$$

(36)

Consequently, the first curvature derivative for the $i$-th segment is obtained through the first integral:

$${\displaystyle \frac{{dK}_{ri}}{d{s}_r}}=\int {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r+{\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0$$

(37)

where the integration constant ${\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0$ is defined from initial conditions.

In turn, the first antiderivative is determined as:

$$\int {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r=\int {\mathit{Q}}_{ri}^T{\mathit{S}}_{ri}{\mathit{F}}_{\xi }d{s}_r={\mathit{Q}}_{ri}^T{\mathit{S}}_{ri}{S}_{ri}\int {\mathit{F}}_{\xi }d\xi$$

(38)

Furthermore, the curvature for the $i$-th segment is derived by successive integration of Eq. (37):

$$K_{ri}=\int {\displaystyle \frac{{dK}_{ri}}{d{s}_r}}d{s}_r+K_{ri0}=\int \left(\int {\displaystyle \frac{d^2{K}_{\mathit{ri}}}{{\mathit{ds}}_r^2}}d{s}_r+{\left.{\displaystyle \frac{{\mathit{dK}}_{\mathit{ri}}}{d{s}_r}}\right|}_0\right)d{s}_r+K_{ri0}=$$

(39)

$$=\iint {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r^2+\int {\left.{\displaystyle \frac{{\mathit{dK}}_{\mathit{ri}}}{d{s}_r}}\right|}_{0}d{s}_r+K_{ri0}=\iint {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r^2+{\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0{s}_r+K_{ri0}$$

where the integration constant $K_{ri0}$ denotes the initial curvature.

Considering Eq. (34), the components of Eq. (39) are formulated as follows:

$$\iint {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r^2={\mathit{Q}}_{ri}^T{\mathit{S}}_{ri}{S}_{ri}^2\iint {\mathit{F}}_{\xi }d{\xi }^2,{\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0{s}_r=S_{ri}{\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0\xi$$

(40)

To obtain the reference angle ${\alpha }_{ri}$, an additional integration is performed. Thus:

$${\alpha }_{ri}=\int K_{ri}\left(s_r\right)d{s}_r=\int \left(\iint {\displaystyle \frac{d^2{K}_{\mathit{ri}}}{{\mathit{ds}}_r^2}}d{s}_r^2+{\left.{\displaystyle \frac{{\mathit{dK}}_{\mathit{ri}}}{d{s}_r}}\right|}_0{s}_r+K_{i0}\right)d{s}_r+{\alpha }_{ri0}=$$

(41)

$$=\iiint {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r^3+{\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0{\displaystyle \frac{s_r^2}{2}}+K_{ri0}{s}_{ri}+{\alpha }_{ri0}$$

where ${\alpha }_{ri0}$ is the integration constant and other components can be found as follows

$$\iiint {\displaystyle \frac{d^2{K}_{ri}}{{ds}_r^2}}d{s}_r^3={\mathit{Q}}_{ri}^T{\mathit{S}}_{ri}{S}_{ri}^3\iiint {\mathit{F}}_{\xi }d{\xi }^3,{\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0{\displaystyle \frac{s_r^2}{2}}={{\displaystyle \frac{S_{ri}^2}{2}}\left.{\displaystyle \frac{{dK}_{ri}}{d{s}_r}}\right|}_0{\xi }^2,K_{ri0}{s}_{ri}=K_{ri0}{S}_{ri}\xi$$

(42)

The curvature $K_r$ and the angle ${\alpha }_r$ are related by:

$$K_r\left(s_r\right)={\displaystyle \frac{d{\alpha }_r}{d{s}_r}},K_r\left(s_r\right)d{s}_r=d{\alpha }_r$$

(43)

In turn,

$$\int K_r\left(s_r\right)d{s}_r=\int d{\alpha }_r,H_{ri}+{\alpha }_{ri0}={\alpha }_{ri},H_{ri}=\int K_r\left(s_r\right)d{s}_r$$

(44)

The matrices of curvature, its integrals, and derivatives are obtained by concatenating all segments, considering that the terminal value of the preceding segment equals the initial value of the subsequent one. The coordinates of the reference trajectory are then derived from these relations, accounting for the initial angle ${\alpha }_{r0}$ and position $\left(x_{r0},y_{r0}\right)$:

$$x_r\left(s_r\right)={\int }_0^{s_r}\mathit{cos}\left({\int }_0^{s_r}{K}_r\left(s_r\right)d{s}_r+{\alpha }_{r0}\right)d{s}_r+x_{r0}={\int }_0^{s_r}\mathit{cos}\left({\alpha }_r\left(s_r\right)\right)d{s}_r+x_{r0}$$

(45)

$$y_r\left(s_r\right)={\int }_0^{s_r}\mathit{sin}\left({\int }_0^{s_r}{K}_r\left(s_r\right)d{s}_r+{\alpha }_{r0}\right)d{s}_r+y_{r0}={\int }_0^{s_r}\mathit{sin}\left({\alpha }_r\left(s_r\right)\right)d{s}_r+y_{r0}$$

(46)

3.3. Optimization of the Rail as a Geometric Reference of the Motion Space

The reference trajectory requires optimization, which can be approached in two ways. The first method utilizes anchor points near the lane centerline in Cartesian coordinates, identified via computer vision. The second approach relies on a fast preliminary interpolation of the road centerline, which is subsequently used to minimize the integral deviation against a target reference. In this framework, solving the problem in Cartesian coordinates is unnecessary, as the formulation readily transitions to the natural (Frenet) coordinate frame.

To implement this second approach, the reference geometry is synthesized via a systematic two-stage pipeline. First, a preliminary spline is computed using three strategic points. The initial boundary conditions are rigidly tied to the starting position and heading angle inherited from the preceding segment, while the terminal conditions employ a not-a-knot constraint. Because this initial formulation resolves to a straightforward linear system of equations, it guarantees minimal curvature for the road corridor centerline with negligible computational overhead.

Building upon this initial centerline approximation, the core optimization procedure constructs the final reference rail. Rather than utilizing raw splines - which inherently lack guaranteed higher-order smoothness and introduce trajectory discontinuities at segment interfaces - the rail is optimized to tightly adhere to the preliminary curve. This optimization treats the rail as a differentiable geometric reference of the motion space, ensuring precise control over continuity and establishing a robust foundation for seamless multi-segment stitching and simultaneous path-speed optimization.

Primary Spline. If at least three points relative to the vehicle's position are defined, a spline determining the road curvature can be computed with high computational efficiency. The calculations are performed in the Cartesian coordinate system $\left(x,y\right)$, providing the foundational curvilinear parameters.

The elementary arc length is given by:

$ds=\sqrt{1+{\left(dy/dx\right)}^2}dx$     (47)

For the tangent angle, the trajectory derivative defines the tangent of the slope angle:

$dy/dx=\mathit{tan}\left({\alpha }_s\left(x\right)\right)$    (48)

Consequently, the slope angle is:

${\alpha }_s\left(x\right)=\mathit{arctan}\left(dy/dx\right)$     (49)

Then, using interpolation, the baseline geometric information is represented as a smooth angle function along the spline arc length:

${\alpha }_s\left(s\right)={\alpha }_s\left(s\left(x\right)\right)$     (50)

Cost Function Components. The problem is to find nodal parameters of the curvature derivative model to simultaneously ensure both the tightest possible fit of the rail to the spline and high smoothness of the curvature and its derivatives. Unlike a spline, which inevitably suffers from discontinuities in higher derivatives, the rail can provide a seamless connection between segments up to higher derivatives. Obviously, to enable flexible adaptation of the rail during optimization, sufficient degrees of freedom must be provided. Therefore, we use the following criteria.

Rail length. Since the spline is represented with minimal curvature, the convergence of the rail and spline lengths helps redistribute the curvature intensity along the rail. Obviously, the length of the rail itself, $S_r$, can vary slightly and deviate from the ideal final spline length value $S$ during the optimization process. Then, along the curvilinear coordinate $s$, the integral $I_s$ of the square of the length error is:

$I_s\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{S}{\int }}}{\left(s_s\left(s\right)-s_r\left({\mathit{q}}_r,s\right)\right)}^{2}ds$     (51)

Rail angle. As the main approximation parameter, it is convenient to choose the curve angle in the natural coordinate system. Firstly, this is already the integrated curvature, and secondly, it eliminates the need to return to the Cartesian system to switch to control point positions. If the rail has an angle distribution ${\alpha }_r\left(q_r,s\right)$ along the arc length, and the base spline has ${\alpha }_s\left(s\right)$, then the integral $I_{\alpha }$ of the square of the angle error is:

$I_{\alpha }\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{S}{\int }}}{\left({\alpha }_s\left(s\right)-{\alpha }_r\left({\mathit{q}}_r,s\right)\right)}^{2}ds$     (52)

Curvature. We impose a requirement on the rail to minimize curvature usage:

$I_{K_r}\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{s_r}{\int }}}{K}_r^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{K}_r^{2}d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{K_r}\left(\xi \right)d\xi$     (53)

Curvature intensity. We impose a requirement on the rail to minimize the number of inflection points:

$I_{{dK}_r}\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{s_r}{\int }}}{\left({\displaystyle \frac{d{K}_r}{d{s}_r}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d{K}_r}{d{s}_r}}\right)}^{2}d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{{dK}_r}\left(\xi \right)d\xi$      (54)

Second and third derivatives of curvature. We impose a requirement on the rail to maximize the straightening of curves directly generated by the model in Eq. (35):

$I_{{d2K}_r}\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{s_r}{\int }}}{\left({\displaystyle \frac{d^2{K}_r}{{ds}_r^2}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d^2{K}_r}{{ds}_r^2}}\right)}^{2}d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{{d2K}_r}\left(\xi \right)d\xi$      (55)

$I_{{d3K}_r}\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{s_r}{\int }}}{\left({\displaystyle \frac{d^3{K}_r}{{ds}_r^3}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d^3{K}_r}{{ds}_r^3}}\right)}^{2}d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{{d3K}_r}\left(\xi \right)d\xi$      (56)

Fourth derivative of curvature. This is a derivative where the smoothness of the junction between finite elements is no longer ensured. It is represented by a broken line where, moreover, the values at adjacent finite element nodes generally do not coincide. We impose a requirement on the rail parameters for this broken line to be as close to zero as possible.

$I_{{d4K}_r}\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{s_r}{\int }}}{\left({\displaystyle \frac{d^4{K}_r}{{ds}_r^4}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d^4{K}_r}{{ds}_r^4}}\right)}^{2}d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{{d4K}_r}\left(\xi \right)d\xi$      (57)

Fifth derivative of curvature is represented only by constants across the finite element sections; however, this derivative is most sensitive to the convergence of the solution, and our task is to ensure that adjacent steps are grouped as closely together as possible.

$I_{{d5K}_r}\left({\mathit{q}}_r\right)={\displaystyle \underset{0}{\overset{s_r}{\int }}}{\left({\displaystyle \frac{d^5{K}_r}{{ds}_r^5}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d^5{K}_r}{{ds}_r^5}}\right)}^{2}d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{{d5K}_r}\left(\xi \right)d\xi$      (58)

Cost Function C_f is formed as the sum of the weighted criteria indicated above. The condition of minimizing the objective function has the following view:

$C_f=C_f\left({\mathit{q}}_r\right)={\mathit{W}}^T\mathit{I}\left({\mathit{q}}_r\right)={\mathit{W}}^T\mathit{I}\to min$     (59)

where ${\mathit{q}}_r$ is the vector of the search-for parameters, I is the vector of the integral criteria, W is the vector of weight factors.

Thus,

$\mathit{W}=\left(\begin{array}{c}{W}_s\\ W_{\alpha }\\ \begin{array}{c}{W}_{K_r}\\ W_{{dK}_r}\\ \begin{array}{c}{W}_{{d2K}_r}\\ W_{{d3K}_r}\\ \begin{array}{c}{W}_{{d4K}_r}\\ W_{{d5K}_r}\end{array}\end{array}\end{array}\end{array}\right)$, $\mathit{I}=\mathit{I}\left({\mathit{q}}_r\right)=\left(\begin{array}{c}{I}_s\left({\mathit{q}}_r\right)\\ I_{\alpha }\left({\mathit{q}}_r\right)\\ \begin{array}{c}{I}_{K_r}\left({\mathit{q}}_r\right)\\ I_{{dK}_r}\left({\mathit{q}}_r\right)\\ \begin{array}{c}{I}_{{d2K}_r}\left({\mathit{q}}_r\right)\\ I_{{d3K}_r}\left({\mathit{q}}_r\right)\\ \begin{array}{c}{I}_{{d4K}_r}\left({\mathit{q}}_r\right)\\ I_{{d5K}_r}\left({\mathit{q}}_r\right)\end{array}\end{array}\end{array}\end{array}\right)$     (60)

where $W_s$, $W_{\alpha }$, $W_{K_r}$, $W_{{dK}_r}$, $W_{{d2K}_r}$, $W_{{d3K}_r}$, $W_{{d4K}_r}$, $W_{{d5K}_r}$ are weight coefficients for the rail length, angle, and its curvature's derivatives, respectively.

3.4. Boundary Constraints

We do not impose rigid constraints on the track curve, allowing it to adapt fully to the spline without accumulating errors, since it is impossible to simultaneously achieve high accuracy in angle deviation and satisfy strict constraints on track nodes (specifically, initial and final curvatures). Note that the spline's purpose is merely to guide the search, and strict matching of geometric parameters is not required. For a subsequent track segment, the initial data are defined by the geometric parameters of the end of the previous track rather than the preceding spline. Furthermore, the spline's initial angle itself can be set to match the final angle of the previous track segment. In short, soft constraints accelerate the optimization process, rendering it stable with minimal error accumulation. Accordingly, we divide the constraints into final and continuity constraints.

Final constraints help eliminate error accumulation and rigidly anchor the track segment to the spline. Therefore, we introduce the requirement for matching the coordinates of the endpoint $S_f$ and the corresponding angles.

${\mathit{c}}_{sf}=\left(\begin{array}{c}{\alpha }_s\left(S_f\right)\\ x_s\left(S_f\right)\\ y_s\left(S_f\right)\end{array}\right)=\left(\begin{array}{c}{\alpha }_{sf}\\ x_{sf}\\ y_{sf}\end{array}\right),{\mathit{c}}_{rf}=\left(\begin{array}{c}{\alpha }_r\left(S_f\right)\\ x_r\left(S_f\right)\\ y_r\left(S_f\right)\end{array}\right)=\left(\begin{array}{c}{\alpha }_{rf}\\ x_{rf}\\ y_{rf}\end{array}\right),{\mathit{c}}_f={\mathit{c}}_{sf}-{\mathit{c}}_{rf}$     (61)

where the index $s$ corresponds to the spline, and $r$ to the rail.

Discrete constraints compensate for the fourth-derivative curvature mismatch among the nodes of the piecewise-linear approximation. As mentioned, continuity at the finite element (FE) nodes is violated for this derivative; however, we can formulate the linear functions along the FEs such that, among all possible variations, the selected curve ensures that the values at the final nodes ($f$) of the preceding FEs match the initial nodes ($0$) of the subsequent ones. Expressing this mathematically, we obtain the following condition:

${\mathit{c}}_{ds}=\left(\begin{array}{c}{\left.{\displaystyle \frac{d^4{K}_r}{{ds}_r^4}}\right|}_2^{\left(0\right)}\\ ⋮\\ {\left.{\displaystyle \frac{d^4{K}_r}{{ds}_r^4}}\right|}_N^{\left(0\right)}\end{array}\right),{\mathit{c}}_{dp}=\left(\begin{array}{c}{\left.{\displaystyle \frac{d^4{K}_r}{{ds}_r^4}}\right|}_1^{\left(f\right)}\\ ⋮\\ {\left.{\displaystyle \frac{d^4{K}_r}{{ds}_r^4}}\right|}_{N-1}^{\left(f\right)}\end{array}\right),{\mathit{c}}_d={\mathit{c}}_{ds}-{\mathit{c}}_{dp}$      (62)

where $N$ corresponds to the number of FEs.

Note that, in the general case where data from the previous segment are available, its fourth derivative of curvature must be adapted to the generation conditions of the subsequent rail and incorporated into Eq. (62). Consequently, the final set of equality constraints is expressed as:

${\mathit{c}}_{eq}=\left(\begin{array}{c}{\mathit{c}}_f\\ {\mathit{c}}_d\end{array}\right)=0$      (63)

3.5. Numerical Integration Technique

We employ a numerical integration technique based on the $N$-point Gaussian quadrature scheme to simultaneously ensure computational efficiency and precision. If an integrand $z\left(s_r\right)$ is considered within am $i$-th segment $\left[s_{r\left(i-1\right)},s_{r\left(i\right)}\right]$, substituting $s_r=\xi s_r$ yields:

${\displaystyle \underset{0}{\overset{S_r}{\int }}}z\left(s_r\right)d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}z\left(\mathit{Q},S_r,\xi \right)d\xi \approx S_r{\displaystyle \sum _{k=1}^N}{w}_{k}z\left(\xi \left({\vartheta }_k\right)\right)det\left(J\left({\vartheta }_k\right)\right)$     (64)

where $\mathit{Q}$ is the vector of nodal parameters, $w_k$ is the integration weight at the $k$-th point, $k\in \left[1,N\right]$, ${\vartheta }_k$ is the $k$-th point in the master-element coordinate system, $J$ is the Jacobian, and $N$ is the number of integration points.

For one-dimensional finite elements, the coordinate mapping and Jacobian simplify to:

$\xi \left({\vartheta }_k\right)={\displaystyle \frac{\left({\xi }_1-{\xi }_0\right)}{2}}\left({\vartheta }_k+1\right)+{\xi }_0={\displaystyle \frac{1}{2}}\left({\vartheta }_k+1\right),det\left(J\right)={\displaystyle \frac{{\xi }_1-{\xi }_0}{2}}={\displaystyle \frac{1}{2}}$          (65)

Defining the vectors:

$\mathit{\vartheta }=\left(\begin{array}{c}{\vartheta }_1\\ ⋮\\ {\vartheta }_N\end{array}\right),\mathit{w}=\left(\begin{array}{c}{w}_1\\ ⋮\\ w_N\end{array}\right),\mathit{\xi }={\left(\begin{array}{c}{\xi }_1\\ ⋮\\ {\xi }_N\end{array}\right)}^T,\mathit{z}={\left(\begin{array}{c}{z}_1\\ ⋮\\ z_N\end{array}\right)}^T$     (66)

the compact expression for calculating the integral becomes:

$S_r{\displaystyle \underset{0}{\overset{1}{\int }}}z\left(\mathit{Q},S_r,\xi \right)d\xi \approx {\displaystyle \frac{S_r}{2}}\mathit{z}\mathit{w}$     (67)

where $s_r$ is the reference length, and $\mathit{z}$ is the vector of integrands of dimension 1×N.

In turn, each segment $\left[s_{r\left(i-1\right)},s_{r\left(i\right)}\right]$, may also be represented as a sub-finite element corresponding to a range $\left[{\xi }_{i-1},{\xi }_i\right]$ and again considered in the segment $\vartheta \in \left[0,1\right]$. Then:

${\displaystyle \underset{{\xi }_{i-1}}{\overset{{\xi }_i}{\int }}}{z}_i\left(\xi \right)d\xi ={\Delta \xi }_i{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_i\left(\mathit{Q}{,S}_r,\xi \left(\vartheta \right)\right)d\vartheta$     (68)

Summing all the sub-elements, for $i$-th FE we get:

$S_r{\displaystyle \frac{{\Delta \xi }_i}{2}}{\displaystyle \sum _{k=1}^N}{z}_i\left(\mathit{Q}{,S}_r,{\xi }_i\left({\vartheta }_k\right)\right)w_k=S_r{\displaystyle \frac{{\Delta \xi }_i}{2}}{\mathit{z}}_i\mathit{w}$    (69)

4. Vehicle Kinematic Parameters in the Frenet Coordinate System

The trajectory vector ${\overrightarrow{r}}_t$ is represented by decomposing motion along the reference ${\overrightarrow{r}}_r$ and perpendicular to it along the direction of ${\overrightarrow{r}}_L$:

${\overrightarrow{r}}_t=r_t{\overrightarrow{u}}_t={{\overrightarrow{r}}_r+{L\overrightarrow{\nu }}_r=\left(\begin{array}{c}1\\ L\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{r}}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$     (70)

where ${\overrightarrow{u}}_t$, ${\overrightarrow{\nu }}_r$ = are unitary vectors.

Speeds. The speed along the reference is derived as follows:

${\overrightarrow{V}}_r={\displaystyle \frac{d{\overrightarrow{r}}_r}{dt}}={\displaystyle \frac{d{\overrightarrow{r}}_r}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}=V_r{\overrightarrow{\tau }}_r={\left(\begin{array}{c}{V}_r\\ 0\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$     (71)

where ${\overrightarrow{\tau }}_r$ = unitary vector.

The differentials of the Frenet vectors are:

${\displaystyle \frac{d{\overrightarrow{\tau }}_r}{dt}}={\displaystyle \frac{d{\overrightarrow{\tau }}_r}{d{\alpha }_r}}{\displaystyle \frac{d{\alpha }_r}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\overrightarrow{\nu }}_r{K}_r{V}_r,{\displaystyle \frac{d{\overrightarrow{\nu }}_r}{dt}}={\displaystyle \frac{d{\overrightarrow{\nu }}_r}{d{\alpha }_r}}{\displaystyle \frac{d{\alpha }_r}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}=-{\overrightarrow{\tau }}_r{K}_r{V}_r$  (72)

${\displaystyle \frac{d}{dt}}\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)=V_r{K}_r\left(\begin{array}{cc}0& 1\\ -1& 0\end{array}\right)\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)=V_r{K}_{r}D\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)={\Omega }_{r}D\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$      (73)

where

${\Omega }_r=V_r{K}_r,D=\left(\begin{array}{cc}0& 1\\ -1& 0\end{array}\right)$The speed component perpendicular to the reference trajectory $s_r$ is derived as:

${\overrightarrow{V}}_L={\displaystyle \frac{d}{dt}}\left({\left(\begin{array}{c}0\\ L\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)\right)={\left(\begin{array}{c}0\\ {\displaystyle \frac{dL}{dt}}\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)+{\left(\begin{array}{c}0\\ L\end{array}\right)}^T{\displaystyle \frac{d}{dt}}\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)={\left(\begin{array}{c}-L{V}_r{K}_r\\ V_L\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$     (74)

The velocity vector along the trajectory $s_t$ is obtained by summing the longitudinal and lateral components:

${\overrightarrow{V}}_t={\overrightarrow{V}}_r+{\overrightarrow{V}}_L={\left(\begin{array}{c}{V}_r\left(1-L{K}_r\right)\\ V_L\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)={\left(\begin{array}{c}{V}_r{k}_{rL}\\ V_L\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$     (75)

The trajectory speed $V_t$ can be determined via two equivalent formulations. The first is the magnitude derived from the decomposition in Eq. (75):

$V_t=\sqrt{{V_r^{2}k}_{rL}^2+V_L^2}=\sqrt{V_r^2\left(k_{rL}^2+{\displaystyle \frac{V_L^2}{V_r^2}}\right)}=\sqrt{V_r^2\left(k_{rL}^2+{\left({\displaystyle \frac{V_L}{V_r}}\right)}^2\right)}=V_r\sqrt{k_{rL}^2+v_{Lr}^2}$     (76)

The second approach is based on the differentiation of the trajectory's elementary arc length:

$V_t={\displaystyle \frac{d{s}_t}{dt}}={\displaystyle \frac{d{s}_t}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}=s_{tr}^{\text{'}}{V}_r=V_r\sqrt{k_{rL}^2+v_{Lr}^2}$     (77)

The consistency between the results of Eqs. (75) and (77) confirms the validity of the derived velocity relationships.

Acceleration. By differentiating the trajectory velocity vector derived in Eq. (75) with respect to time, we obtain the expression for the trajectory acceleration ${\overrightarrow{a}}_t$:

${\overrightarrow{a}}_t={\displaystyle \frac{d{\overrightarrow{V}}_t}{dt}}={\displaystyle \frac{d}{dt}}{\left(\begin{array}{c}{V}_r{k}_{rL}\\ V_L\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)+{\left(\begin{array}{c}{V}_r{k}_{rL}\\ V_L\end{array}\right)}^T{\displaystyle \frac{d}{dt}}\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)=$     (78)

$$={\left(\begin{array}{c}{\displaystyle \frac{d{V}_r}{dt}}{k}_{rL}+V_r{\displaystyle \frac{d{k}_{rL}}{dt}}\\ {\displaystyle \frac{d{V}_L}{dt}}\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)+{\Omega }_r{\left(\begin{array}{c}{V}_r{k}_{rL}\\ V_L\end{array}\right)}^{T}D\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)={\left(\begin{array}{c}{k}_{rL}{\displaystyle \frac{d{V}_r}{dt}}+V_r{\displaystyle \frac{d{k}_{rL}}{dt}}-{\Omega }_r{V}_L\\ {\displaystyle \frac{d{V}_L}{dt}}+{\Omega }_r{V}_r{k}_{rL}\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$$

Here, considering Eq. (15), are denoted

${\displaystyle \frac{d{V}_r}{dt}}={\displaystyle \frac{d{V}_r}{{ds}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d{V}_r}{{ds}_r}}{V}_r,{\displaystyle \frac{d{V}_L}{dt}}={\displaystyle \frac{d{V}_L}{{ds}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d{V}_L}{{ds}_r}}{V}_r,{\displaystyle \frac{d{k}_{rL}}{dt}}={\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{V}_r$     (79)

Jerk. By differentiating the acceleration vector Eq. (78) with respect to time and utilizing the Frenet frame rotation defined in Eq. (73), we obtain the jerk ${\overrightarrow{j}}_t$ decomposition:

${\overrightarrow{j}}_t={\displaystyle \frac{d{\overrightarrow{a}}_t}{dt}}={\left(\begin{array}{c}\left({\displaystyle \frac{d^2{V}_r}{d{t}^2}}-{\Omega }_r^2{V}_r\right)k_{rL}+2\left({\displaystyle \frac{d{k}_{rL}}{dt}}{\displaystyle \frac{d{V}_r}{dt}}-{\Omega }_r{\displaystyle \frac{d{V}_L}{dt}}\right)+V_r{\displaystyle \frac{d^2{k}_{rL}}{d{t}^2}}-{E_{r}V}_L\\ {\displaystyle \frac{d^2{V}_L}{d{t}^2}}+{2\Omega }_r\left(k_{rL}{\displaystyle \frac{d{V}_r}{dt}}+V_r{\displaystyle \frac{d{k}_{rL}}{dt}}\right)-{\Omega }_r^2{V}_L+E_r{V}_r{k}_{rL}\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)$     (80)

Here, the components of Eq. (80) are defined as follows

$E_r={\displaystyle \frac{d{\Omega }_r}{dt}}={\displaystyle \frac{d{\Omega }_r}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d\left(K_r{V}_r\right)}{d{s}_r}}{V}_r=\left(V_r{\displaystyle \frac{d{K}_r}{d{s}_r}}+K_r{\displaystyle \frac{d{V}_r}{d{s}_r}}\right)V_r={\Omega }_r{\displaystyle \frac{d{V}_r}{d{s}_r}}+V_r^2{\displaystyle \frac{d{K}_r}{d{s}_r}}$     (81)

${\displaystyle \frac{d^2{V}_r}{d{t}^2}}={\displaystyle \frac{d}{dt}}\left({\displaystyle \frac{d{V}_r}{dt}}\right)={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{V}_r}{d{s}_r}}{V}_r\right){\displaystyle \frac{d{s}_r}{dt}}=\left(V_r{\displaystyle \frac{d^2{V}_r}{d{s}_r^2}}+{\left({\displaystyle \frac{d{V}_r}{d{s}_r}}\right)}^2\right)V_r$     (82)

${\displaystyle \frac{d^2{V}_L}{d{t}^2}}={\displaystyle \frac{d}{dt}}\left({\displaystyle \frac{d{V}_L}{dt}}\right)={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d{V}_L}{d{s}_r}}{V}_r\right){\displaystyle \frac{d{s}_r}{dt}}=\left(V_r{\displaystyle \frac{d^2{V}_L}{d{s}_r^2}}+{\displaystyle \frac{d{V}_L}{d{s}_r}}{\displaystyle \frac{d{V}_r}{d{s}_r}}\right)V_r$     (83)

${\displaystyle \frac{d^2{k}_{rL}}{d{t}^2}}={\displaystyle \frac{d}{dt}}\left({\displaystyle \frac{d{k}_{rL}}{d{s}_r}}{V}_r\right)=-\left({E_{r}v}_{Lr}+{\Omega }_r{\displaystyle \frac{d{v}_{Lr}}{dt}}+{V_{L}V}_r{\displaystyle \frac{d{K}_r}{d{s}_r}}+L\left({\displaystyle \frac{d{V}_r}{dt}}{\displaystyle \frac{d{K}_r}{d{s}_r}}+V_r^2{\displaystyle \frac{d^2{K}_r}{d{s}_r^2}}\right)\right)$    (84)

where

${\displaystyle \frac{d{v}_{Lr}}{dt}}={\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d{v}_{Lr}}{d{s}_r}}{V}_r$

Transition to the Vehicle Coordinate System. As illustrated in Figure 3b, the velocity vectors ${\overrightarrow{V}}_r$ and ${\overrightarrow{V}}_t$ differ by the angle ${\alpha }_p$. Considering the ideal vehicle kinematics model, the local coordinate system of the autonomous vehicle (AV), shown in Figure 2, is rotated by an angle $-{\beta }_t$ relative to the unit vector ${\overrightarrow{\tau }}_t$. The central slip angle ${\beta }_t$, characterizing the relationship between lateral and longitudinal speed components, is defined as:

${\beta }_t=\mathit{arcsin}\left(b/R_t\right)=\mathit{arcsin}\left(b{K}_t\right)$, $\left|b{K}_t\right|<1$     (85)

where $b$ denotes the distance between the vehicle mass center and the rear axle.

Transitions between the reference Frenet coordinate system and the AV's local Cartesian coordinate system are performed using the direction cosine matrix $C\left(\cdot \right)$:

$\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)=C\left(-{\alpha }_p\right)\left(\begin{array}{c}{\overrightarrow{\tau }}_t\\ {\overrightarrow{\nu }}_t\end{array}\right),\left(\begin{array}{c}{\overrightarrow{\tau }}_t\\ {\overrightarrow{\nu }}_t\end{array}\right)=C\left({\beta }_t\right)\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)$,    (86)

$\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)=C\left(-{\alpha }_p\right)C\left({\beta }_t\right)\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)=C\left({\beta }_t-{\alpha }_p\right)\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right),C\left(\bullet \right)=\left(\begin{array}{cc}\mathit{cos}\left(\bullet \right)& \mathit{sin}\left(\bullet \right)\\ -\mathit{sin}\left(\bullet \right)& \mathit{cos}\left(\bullet \right)\end{array}\right)$where ${\overrightarrow{\tau }}_t$, ${\overrightarrow{\nu }}_t$ are the unitary Frenet vectors in the trajectory coordinate system; ${\overrightarrow{u}}_{\zeta }$, ${\overrightarrow{u}}_{\mu }$ are the unitary vectors of the AV's local coordinate system.

Coordinate Transformation. By applying Eq. (86), any kinematic parameter $\overrightarrow{p}$ (such as velocity, acceleration, or jerk) defined in the reference system can be decomposed into the AV's local coordinate system $\zeta \mu$. The transformation relationship is given by:

${\left(\begin{array}{c}{p}_{\tau r}\\ p_{\nu r}\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{\tau }}_r\\ {\overrightarrow{\nu }}_r\end{array}\right)={\left(\begin{array}{c}{p}_{\zeta }\\ p_{\mu }\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right),{\left(\begin{array}{c}{p}_{\tau r}\\ p_{\nu r}\end{array}\right)}^{T}C\left({\beta }_t-{\alpha }_p\right)\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)={\left(\begin{array}{c}{p}_{\zeta }\\ p_{\mu }\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)$,   (87)

$\left(\begin{array}{c}{p}_{\zeta }\\ p_{\mu }\end{array}\right)=C\left({\alpha }_p-{\beta }_t\right)\left(\begin{array}{c}{p}_{\tau r}\\ p_{\nu r}\end{array}\right),\left(\begin{array}{c}{p}_{\tau r}\\ p_{\nu r}\end{array}\right)=C^T\left({\alpha }_p-{\beta }_t\right)\left(\begin{array}{c}{p}_{\zeta }\\ p_{\mu }\end{array}\right)$

Yaw Rate and Angular Acceleration. While these parameters are not explicitly required in the primary kinematic expressions, they are essential for assessing vehicle dynamics and stability, and for formulating kinematic constraints. We now proceed to derive these parameters relative to the reference Frenet coordinate system.

The vehicle yaw angle $\varphi$ is determined by the trajectory tangent angle ${\alpha }_t$ and the central slip angle ${\beta }_t$:

$\varphi ={\alpha }_t-{\beta }_t$     (88)

The corresponding derivatives with respect to the reference arc length $s_r$ are expressed as:

${\displaystyle \frac{d\varphi }{d{s}_r}}={\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-{\displaystyle \frac{d{\beta }_t}{d{s}_r}},{\displaystyle \frac{d^2\varphi }{d{s}_r^2}}={\displaystyle \frac{d^2{\alpha }_t}{d{s}_r^2}}-{\displaystyle \frac{d^2{\beta }_t}{d{s}_r^2}}$           (89)

The yaw rate $\omega$ is then derived with respect to time as:

$\omega ={\displaystyle \frac{d\varphi }{dt}}={\displaystyle \frac{d\varphi }{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d\varphi }{d{s}_r}}{V}_r=\left({\displaystyle \frac{d{\alpha }_t}{d{s}_r}}-{\displaystyle \frac{d{\beta }_t}{d{s}_r}}\right)V_r$     (90)

Finally, the angular acceleration $\epsilon$ is obtained by differentiating the yaw rate with respect to time:

$\epsilon ={\displaystyle \frac{d\omega }{dt}}={\displaystyle \frac{d\omega }{d{s}_r}}{\displaystyle \frac{d{s}_r}{dt}}={\displaystyle \frac{d\omega }{d{s}_r}}{V}_r={\displaystyle \frac{d^2\varphi }{d{s}_r^2}}{V}_r^2+{\displaystyle \frac{d{V}_r}{d{s}_r}}\omega$     (91)

where

${\displaystyle \frac{d\omega }{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}\left({\displaystyle \frac{d\varphi }{d{s}_r}}{V}_r\right)={\displaystyle \frac{d^2\varphi }{d{s}_r^2}}{V}_r+{\displaystyle \frac{d{V}_r}{d{s}_r}}{\displaystyle \frac{d\varphi }{d{s}_r}}$     (92)

Derivatives of the Slip Angle. The derivatives of the central slip angle ${\beta }_t$ with respect to the arc length $s_r$ are derived as follows:

${\displaystyle \frac{d{\beta }_t}{d{s}_r}}={\displaystyle \frac{d}{d{s}_r}}\left(\mathit{arcsin}\left(b{K}_t\right)\right)={\displaystyle \frac{b}{\sqrt{1-{\left(b{K}_t\right)}^2}}}{\displaystyle \frac{d{K}_t}{d{s}_r}}=k_{\beta }{\displaystyle \frac{d{K}_t}{d{s}_r}}$     (93)

where the coefficient $k_{\beta }$ and its derivative with respect to $s_r$ are defined as:

$k_{\beta }={\displaystyle \frac{b}{\sqrt{1-{\left(b{K}_t\right)}^2}}},{\displaystyle \frac{d{k}_{\beta }}{d{s}_r}}=k_{\beta }^3{K}_t{\displaystyle \frac{d{K}_t}{d{s}_r}}$          (94)

Consequently, the second derivative of the slip angle with respect to the reference arc length is obtained by differentiating Eq. (93):

${\displaystyle \frac{d^2{\beta }_t}{d{s}_r^2}}={\displaystyle \frac{d}{d{s}_r}}\left(k_{\beta }{\displaystyle \frac{d{K}_t}{d{s}_r}}\right)=k_{\beta }\left(K_t{\left({\displaystyle \frac{d{\beta }_t}{d{s}_r}}\right)}^2+{\displaystyle \frac{d^2{K}_t}{d{s}_r^2}}\right)$             (95)

5. Dynamic Speed Profile Generation and Optimization

5.1. Speed Model

The Speed Model can be realized by a piecewise Lagrange polynomial in Hermite form as well. The previous sections have revealed that in the case of a pre-established reference curve, the best option is to search for redistributions of the speeds along the reference $V_r=V_r\left(s_r\right)$ and perpendicularly to it $V_L=V_L\left(s_r\right)$. Then, it is assumed that the parameter $s_r$ corresponding to the reference's final length also varies.

Two-DOF Model. According to Eq. (35), the second speed derivative is the highest one requiring continuity and smoothness. Then, similarly to Eq. (14) and considering $p\in \left[r,L\right]$, it can be written:

${\displaystyle \frac{d^2{V}_{pi}}{{ds}_r^2}}={\mathit{Q}}_{Vpi}^T{\mathit{S}}_{ri}{\mathit{F}}_{\xi }$, ${\displaystyle \frac{d^3{V}_{pi}}{{ds}_r^3}}={\mathit{Q}}_{Vpi}^T\left({\displaystyle \frac{{\mathit{S}}_{ri}}{S_{ri}}}\right){\displaystyle \frac{d{\mathit{F}}_{\xi }}{d\xi }}$     (96)

where nodal parameters are:

${\mathit{q}}_{Vpi}={\left(\begin{array}{cc}{\left.{\displaystyle \frac{d^2{V}_p}{{ds}_r^2}}\right|}_i& {\left.{\displaystyle \frac{d^3{V}_p}{{ds}_r^3}}\right|}_i\end{array}\right)}^T$, ${\mathit{Q}}_{Vpi}=\left(\begin{array}{c}{\mathit{q}}_{Vpi}\\ {\mathit{q}}_{Vp\left(i+1\right)}\end{array}\right)$     (97)

Thus, the speed's second derivative conjugation between the segments is based on the cubic polynomials transformed to the FE representation by Eqs. (31)-(34). Then, the first derivative of the longitudinal velocity for the $i$-th segment can be found as the first integral:

${\displaystyle \frac{{dV}_{pi}}{d{s}_r}}=\int {\displaystyle \frac{d^2{V}_{pi}}{{ds}_r^2}}{ds}_r+{\left.{\displaystyle \frac{{dV}_{pi}}{d{s}_r}}\right|}_0$     (98)

where the integration constant ${\left.{\displaystyle \frac{{dV}_{pi}}{d{s}_r}}\right|}_0$ is defined from initial conditions.

In turn, the antiderivative is determined as:

$\int {\displaystyle \frac{d^2{V}_{pi}}{{ds}_r^2}}{ds}_r=\int {\mathit{Q}}_{Vpi}^T{\mathit{S}}_{ri}{\mathit{F}}_{\xi }{ds}_r={\mathit{Q}}_{Vpi}^T{\mathit{S}}_{ri}\int {\mathit{F}}_{\xi }{S}_{i}d\xi ={\mathit{Q}}_{Vpi}^T{\mathit{S}}_{ri}{S}_{ri}\int {\mathit{F}}_{\xi }d\xi$     (99)

Further, the longitudinal speed in the $i$-th segment is defined by repeated integration of Eq. (98):

$V_{pi}=\int {\displaystyle \frac{{dV}_{pi}}{d{s}_r}}{ds}_r+V_{pi0}=\int \left(\int {\displaystyle \frac{d^2{V}_{\mathit{pi}}}{{\mathit{ds}}_r^2}}{\mathit{ds}}_r+{\left.{\displaystyle \frac{{\mathit{dV}}_{\mathit{pi}}}{d{s}_r}}\right|}_0\right){ds}_r+V_{pi0}=$     (100)

$=\iint {\displaystyle \frac{d^2{V}_{pi}}{{ds}_r^2}}{ds}_r^2+\int {\left.{\displaystyle \frac{{\mathit{dV}}_{\mathit{pi}}}{d{s}_r}}\right|}_0{ds}_r+V_{pi0}=\iint {\displaystyle \frac{d^2{V}_{pi}}{{ds}_r^2}}{ds}_r^2+{\left.{\displaystyle \frac{{dV}_{pi}}{d{s}_r}}\right|}_0{s}_r+V_{pi0}$where the integration constant $V_{pi0}$ corresponds to the initial speed for i-th segment.

Considering Eq. (99), components of Eq. (100) are found as follows

$\iint {\displaystyle \frac{d^2{V}_{pi}}{{ds}_r^2}}{ds}_r^2={\mathit{Q}}_{Vpi}^T{\mathit{S}}_{ri}{S}_{ri}^2\iint {\mathit{F}}_{\xi }d{\xi }^2$, ${\left.{\displaystyle \frac{{dV}_{pi}}{d{s}_r}}\right|}_0{s}_r=S_i{\left.{\displaystyle \frac{{dV}_{pi}}{d{s}_r}}\right|}_0\xi$     (101)

Combining all segments and considering that the previous segment's last value equals the subsequent segment's initial value, we obtain matrices of speeds and their derivatives.

5.2. Lateral Displacement and Motion Time

Lateral displacement. Based on Eq. (7), we may get the magnitude of the lateral displacement; however, the analytical expression becomes significantly complicated. Therefore, we will use a discrete representation of the displacement $\mathsf{\Delta }L\left(s_r\right)$ and numerical integration for each $i$-th segment $\left[s_{r\left(i-1\right)},s_{r\left(i\right)}\right]$. Then, given Eqs. (64)-(67):

${\Delta L}_i={\displaystyle \underset{s_{r\left(i-1\right)}}{\overset{s_{r\left(i\right)}}{\int }}}{\displaystyle \frac{V_L}{V_r}}d{s}_r=S_r{\displaystyle \underset{{\xi }_{i-1}}{\overset{{\xi }_i}{\int }}}{\displaystyle \frac{V_{Li}\left(S_r\xi \right)}{V_{ri}\left(S_r\xi \right)}}d\xi =S_r{\displaystyle \underset{{\xi }_{i-1}}{\overset{{\xi }_i}{\int }}}{z}_{Li}\left(\xi \right)d\xi ,z_{Li}\left(\xi \right)={\displaystyle \frac{V_{Li}\left({\mathit{Q}}_{VL},S_r,\xi \right)}{V_{ri}\left({\mathit{Q}}_{Vr},S_r,\xi \right)}}$   (102)

Each segment $\left[s_{r\left(i-1\right)},s_{r\left(i\right)}\right]$, in turn, may also be represented as a finite element corresponding to a range $\left[{\xi }_{i-1},{\xi }_i\right]$ and again considered in the segment $\vartheta \in \left[0,1\right]$. Then:

$S_r{\displaystyle \underset{{\xi }_{i-1}}{\overset{{\xi }_i}{\int }}}{z}_{Li}\left(\xi \right)d\xi =S_r{\Delta \xi }_i{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{Li}\left(S_r\xi \left(\vartheta \right)\right)d\vartheta$     (103)

Passing to numerical integration according to Eq. (67), we get:

${\Delta L}_i=S_r{\displaystyle \frac{{\Delta \xi }_i}{2}}{\displaystyle \sum _{k=1}^N}{z}_{Li}\left(S_r{\xi }_i\left({\vartheta }_k\right)\right)w_k=S_r{\displaystyle \frac{{\Delta \xi }_i}{2}}{\mathit{z}}_{Li}\mathit{w}$    (104)

Thus, it is possible to form a vector of displacement increments

$\Delta L={\left(\begin{array}{ccc}{\Delta L}_1& \dots & {\Delta L}_n\end{array}\right)}^T$     (105)

where $n$ is the number of intervals.

The vector of $L$-points can be obtained by accumulating additions to the initial displacement $L_0$:

$L=L_0+{\left(\begin{array}{ccc}0& {\displaystyle \sum _{i=1}^1}{\Delta L}_i& \begin{array}{cc}\dots & {\displaystyle \sum _{i=1}^n}{\Delta L}_i\end{array}\end{array}\right)}^T,L_f=L_0+{\displaystyle \sum _{i=1}^n}{\Delta L}_i$           (106)

Motion time$t_f$ reflects final time costs and may be calculated as follows:

$t_f={\displaystyle \underset{0}{\overset{t_f}{\int }}}dt={\displaystyle \underset{0}{\overset{S_t}{\int }}}{\displaystyle \frac{d{s}_t}{V_t}}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{\displaystyle \frac{d{s}_r}{V_r}}=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\displaystyle \frac{d\xi }{V_r\left(S_r\xi \right)}}=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_t\left(\xi \right)d\xi$     (107)

Discrete time moments are determined using a scheme similar to the lateral displacement in Eqs. (104)-(106), with the only difference that:

${\Delta t}_i={\displaystyle \underset{s_{r\left(i-1\right)}}{\overset{s_{r\left(i\right)}}{\int }}}{\displaystyle \frac{d{s}_r}{V_r}}=S_r{\displaystyle \underset{{\xi }_{i-1}}{\overset{{\xi }_i}{\int }}}{\displaystyle \frac{d\xi }{V_{ri}\left(S_r\xi \right)}}=S_r{\displaystyle \underset{{\xi }_{i-1}}{\overset{{\xi }_i}{\int }}}{z}_{ti}\left(\xi \right)d\xi ,z_{ti}\left(\xi \right)={\displaystyle \frac{1}{V_{ri}\left({\mathit{Q}}_{Vr},S_r,\xi \right)}}$           (108)

As a result, we get a vector of time intervals and the final time value.

$t={\left(\begin{array}{ccc}0& {\displaystyle \sum _{i=1}^1}{\Delta t}_i& \begin{array}{cc}\dots & {\displaystyle \sum _{i=1}^n}{\Delta t}_i\end{array}\end{array}\right)}^T,t_f={\displaystyle \sum _{i=1}^n}{\Delta t}_i$     (109)

5.3. Optimization Criteria

Since this paper focuses on the mathematical basis of motion modeling, the consideration of distances to obstacles (which affect the length of the rail baseline) is omitted. The objective is to formulate a set of cost function components that ensure both good solution convergence and high smoothness of the results. It should be noted that the speeds $V_r$ and $V_L$ are conditionally independent; therefore, using their components completely symmetrically within the objective function is infeasible, as it would induce excessive «competition» between the optimization parameters. Consequently, we focus primarily on the speed $V_r$ along the rail, whose curvature is a priori defined by smooth functions. Thus, it is advisable to consider the following components:

Deflection from the longitudinal target speed$V_{\zeta t}$ may be the main «moving» factor of the AV motion planning model. The integral of the squared speed deviation has the form

$I_{\mathsf{\Delta }{V}_{\zeta }}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{\left(V_{\zeta t}-V_{\zeta }\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left(V_{\zeta t}-V_{\zeta }\right)}^{2}d\xi$     (110)

The integrand $z_{i\Delta V_{\zeta }}$ is expressed through the speed nodal parameters ${\mathit{Q}}_{iVr}$ and ${\mathit{Q}}_{iVL}$ ${\mathit{z}}_{i\Delta V_{\zeta }}\left(\xi \right)={\left(V_{\zeta t}-V_{\zeta }\left({\mathit{Q}}_{iVr},{\mathit{Q}}_{iVL},S_r,\xi \right)\right)}^2$           (111)

Then, considering Eqs. (67)-(69)

$I_{\Delta V_{\zeta }}=S_r{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{i\Delta V_{\zeta }}\left(S_r\xi \left(\vartheta \right)\right)d\vartheta \approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\mathit{z}}_{i\Delta V_{\zeta }}\mathit{w}$     (112)

Deflection from the target final lateral position$L_f$ is relevant when there is no rigid constraint on the final lateral position and it is necessary to follow a desired offset:

$I_{L_f}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{\left(L_f-L\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left(L_t-L\right)}^{2}d\xi$      (113)

${\mathit{z}}_{i{L}_f}\left(\xi \right)={\left(L_f-L\left({\mathit{Q}}_{iVr},{\mathit{Q}}_{iVL},S_r,\xi \right)\right)}^2$ , $I_{L_f}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\mathit{z}}_{i{L}_f}\mathit{w}$

Deflection from the target final angular position${\alpha }_f$ should ensure the desired heading orientation:

$I_{{\alpha }_f}=k_{\alpha }{\left({\alpha }_f-{\alpha }_t\right)}^2$      (114)

where $k_{\alpha }$ is a coefficient.

Integral of longitudinal jerk is associated with longitudinal ride smoothness.

$I_{j_{\zeta }}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{j}_{\zeta }^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{j}_{\zeta }^{2}d\xi$      (115)

${\mathit{z}}_{i{j}_{\zeta }}\left(\xi \right)=j_{\zeta }^2\left({\mathit{Q}}_{iVr},{\mathit{Q}}_{iVL},S_r,\xi \right)$ , $I_{j_{\zeta }}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\mathit{z}}_{i{j}_{\zeta }}\mathit{w}$

Integral of transversal jerk is associated with transversal ride smoothness.

$I_{j_{\mu }}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{j}_{\mu }^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{j}_{\mu }^{2}d\xi$      (116)

${\mathit{z}}_{i{j}_{\mu }}\left(\xi \right)=j_{\mu }^2\left({\mathit{Q}}_{iVr},{\mathit{Q}}_{iVL},S_r,\xi \right)$ , $I_{j_{\mu }}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\mathit{z}}_{i{j}_{\mu }}\mathit{w}$

Integrals of speeds' 3^d derivatives are associated with the smoothness of the speed derivative model itself. For $p\in \left[r,L\right]$, we obtain:

$I_{{d3V}_p}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{\left({\displaystyle \frac{d^3{V}_p}{{ds}_r^3}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d^3{V}_p}{{ds}_r^3}}\right)}^{2}d\xi$      (117)

${\mathit{z}}_{i{d3V}_p}\left(\xi \right)={\left({\displaystyle \frac{d^3{V}_p\left({\mathit{Q}}_{iVr},{\mathit{Q}}_{iVL},S_r,\xi \right)}{{ds}_r^3}}\right)}^2$ , $I_{{d3V}_p}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\mathit{z}}_{i{d3V}_p}\mathit{w}$

Integrals of speeds' 4^th derivatives are among the most important parameters. First, the derivative loses smoothness here and experiences discontinuities. Second, the minimum of this derivative directly affects the smoothness of the trajectory curvature derivatives. The smaller the intersection angles of the $4^{\mathrm{th}}$ derivatives at adjacent nodes, the more stable and monotonic the third derivative, and so forth.

$I_{{d4V}_p}={\displaystyle \underset{0}{\overset{S_r}{\int }}}{\left({\displaystyle \frac{d^4{V}_p}{{ds}_r^4}}\right)}^{2}d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{\left({\displaystyle \frac{d^4{V}_p}{{ds}_r^4}}\right)}^{2}d\xi$      (118)

${\mathit{z}}_{i{d4V}_p}\left(\xi \right)={\left({\displaystyle \frac{d^4{V}_p\left({\mathit{Q}}_{iVr},{\mathit{Q}}_{iVL},S_r,\xi \right)}{{ds}_r^4}}\right)}^2$ , $I_{{d4V}_p}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}{\Delta \xi }_i{\mathit{z}}_{i{d4V}_p}\mathit{w}$

Sums of speeds' 5^th derivatives are represented by discontinuous functions with constant values equivalent to the slopes of the segments in the piecewise-linear $4^{\mathrm{th}}$ derivative. In fact, there are no integrals here, only sums over the number of finite elements (FEs). This parameter is responsible for solution convergence: the closer the steps are to each other, the smoother the piecewise-linear $4^{\mathrm{th}}$ derivative.

$I_{{d5V}_p}=k_{{d5V}_p}{\displaystyle \sum _{i=1}^N}{\left({\displaystyle \frac{d^5{V}_p}{{ds}_r^5}}\right)}^2$      (119)

where $k_{{d5V}_p}$ is a coefficient.

Cost Function$C_f$ is formed as the sum of the weighted criteria indicated above. The minimization condition of the objective function can be expressed as:

$C_f=C_f\left(\mathit{q}\right)=\mathit{W}{\mathit{I}}^T\left(\mathit{q}\right)=\mathit{W}{\mathit{I}}^T\to min$     (120)

where $\mathit{q}$ is the vector of optimization parameters, $\mathit{I}$ is the vector of integral criteria, and $\mathit{W}$ is the vector of weight factors.

Thus, the parameters can be conditionally divided into common ones, factors corresponding to motion smoothness, as well as the derivatives of longitudinal and transversal speeds:

${\mathit{W}}_c=\left(\begin{array}{ccc}{W}_{\mathsf{\Delta }{V}_{\zeta }}& W_{L_f}& W_{{\alpha }_f}\end{array}\right)$, ${\mathit{I}}_c\left(\mathit{q}\right)=\left(\begin{array}{ccc}{I}_{\mathsf{\Delta }{V}_{\zeta }}\left(\mathit{q}\right)& I_{L_f}\left(\mathit{q}\right)& I_{{\alpha }_f}\left(\mathit{q}\right)\end{array}\right)$     (121)

where $W_{\mathsf{\Delta }{V}_{\zeta }}$, $W_{L_f}$, $W_{{\alpha }_f}$ are weight coefficients for the speed quadratic deviations, lateral, and final angular positions, respectively.

${\mathit{W}}_s=\left(\begin{array}{cc}{W}_{j_{\zeta }}& W_{j_{\mu }}\end{array}\right)$, ${\mathit{I}}_s\left(\mathit{q}\right)=\left(\begin{array}{cc}{I}_{j_{\zeta }}\left(\mathit{q}\right)& I_{j_{\mu }}\left(\mathit{q}\right)\end{array}\right)$     (122)

where $W_{j_{\zeta }}$, $W_{j_{\mu }}$ are weight coefficients for longitudinal and lateral jerks, respectively.

${\mathit{W}}_r=\left(\begin{array}{ccc}{W}_{{d3V}_r}& W_{{d4V}_r}& W_{{d4V}_r}\end{array}\right)$, ${\mathit{I}}_r\left(\mathit{q}\right)=\left(\begin{array}{ccc}{I}_{{d3V}_r}\left(\mathit{q}\right)& I_{{d4V}_r}\left(\mathit{q}\right)& I_{{d5V}_r}\left(\mathit{q}\right)\end{array}\right)$     (123)

where $W_{{d3V}_r}$, $W_{{d4V}_r}$, $W_{{d4V}_r}$ are weight coefficients for the speed derivatives along the rail.

${\mathit{W}}_L=\left(\begin{array}{ccc}{W}_{{d3V}_L}& W_{{d4V}_L}& W_{{d4V}_L}\end{array}\right)$, ${\mathit{I}}_L\left(\mathit{q}\right)=\left(\begin{array}{ccc}{I}_{{d3V}_L}\left(\mathit{q}\right)& I_{{d4V}_L}\left(\mathit{q}\right)& I_{{d5V}_L}\left(\mathit{q}\right)\end{array}\right)$     (124)

where $W_{{d3V}_L}$, $W_{{d4V}_L}$, $W_{{d4V}_L}$ are weight coefficients for the speed derivatives across the rail.

Then, resulting vectors are

$\mathit{W}=\left(\begin{array}{ccc}{\mathit{W}}_c& {\mathit{W}}_s& \begin{array}{cc}{\mathit{W}}_r& {\mathit{W}}_L\end{array}\end{array}\right)$, ${\mathit{I}}_L\left(\mathit{q}\right)=\left(\begin{array}{ccc}{\mathit{I}}_c\left(\mathit{q}\right)& {\mathit{I}}_s\left(\mathit{q}\right)& \begin{array}{cc}{\mathit{I}}_r\left(\mathit{q}\right)& {\mathit{I}}_L\left(\mathit{q}\right)\end{array}\end{array}\right)$     (125)

5.4. Constraints

To speed up the optimization procedure, vehicle motion's kinematic, dynamic, and physical parameters may be restricted. This narrows the search space and decreases the number of iterations. Since this article focuses on the functionality of the mathematical framework, we touch upon only the essential constraints. They are introduced to ensure planning accuracy primarily at the terminal sections of a segment, but they can also be supplemented by parameters at local points.

Model Boundary Constraints. In this 2-DOF velocity model, the initial values $V_{r0}$, $V_{l0}$, $d{V}_r/d{s}_r{|}_0$, and $d{V}_L/d{s}_r{|}_0$ can be specified directly or computed from the motion parameters within the vehicle's coordinate system, whereas the 4th derivative is considered separately. Consequently, the second and third derivatives - representing the core model - remain. In this case, since the velocity derivatives are tied to the curvilinear coordinate, a «coupling» (continuity stitching) can be achieved by requiring the final values (subscript $f$) of the current segment (superscript $\left(c\right)$) to match the initial values (subscript $0$) of the next plan (superscript $\left(n\right)$) transformed to the new reference rail. Thus, considering the generalized indexing for both velocities $p\in \left[r,L\right]$, the condition is expressed as:

${\mathit{c}}_{m\left(p\right)}^{\left(n\right)}=\left(\begin{array}{c}{\left.{\displaystyle \frac{d^2{V}_p}{{ds}_r^2}}\right|}_0^{\left(n\right)}\\ {\left.{\displaystyle \frac{d^3{V}_p}{{ds}_r^3}}\right|}_0^{\left(n\right)}\end{array}\right),{\mathit{c}}_{m\left(p\right)}^{\left(c\right)}=\left(\begin{array}{c}{\left.{\displaystyle \frac{d^2{V}_p}{{ds}_r^2}}\right|}_f^{\left(c\right)}\\ {\left.{\displaystyle \frac{d^3{V}_p}{{ds}_r^3}}\right|}_f^{\left(c\right)}\end{array}\right),{\mathit{c}}_{m\left(p\right)}=c_{m\left(p\right)}^{\left(n\right)}-c_{m\left(p\right)}^{\left(c\right)}$     (126)

Grouping the conditions, we obtain the continuity conditions for the 2d- and 3d-order velocity derivatives:

${\mathit{c}}_{bm}=\left(\begin{array}{c}{\mathit{c}}_{mr}\\ {\mathit{c}}_{mL}\end{array}\right)=0$      (127)

Pointwise Boundary Constraints. When optimizing for the boundary nodes, strict conditions are typically imposed, which causes the solution gradient to shift toward the internal nodes. This provides high smoothness, but at the same time, it can lead to an increased slack in the trajectory. To mitigate this phenomenon, it is possible to shift the focus to the actual $\left(a\right)$ lateral velocity $V_{L2}^{\left(a\right)}$ at the second node of the first FE. For this purpose, we can forcibly require a specific desirable $\left(d\right)$ value $V_{L2}^{\left(d\right)}$ to be achieved at this node. Then, the local constraint ${\mathit{c}}_{bp}$ is formulated as:

${\mathit{c}}_p^{\left(a\right)}=V_{L2}^{\left(a\right)},{\mathit{c}}_p^{\left(d\right)}=V_{L2}^{\left(d\right)},{\mathit{c}}_{bp}={\mathit{c}}_p^{\left(a\right)}-{\mathit{c}}_p^{\left(d\right)}$     (128)

Initial Kinematics Boundary Constraints. In cases where complex kinematic parameters in curvilinear motion are unknown (or at the onset of motion), the last measured values can be specified, or a zero-initialization option can be considered. Thus, for the initial values of longitudinal acceleration $a_{\zeta 0}$, longitudinal $j_{\zeta 0}$, and lateral $j_{\mu 0}$ jerks:

${\mathit{c}}_{in}^{\left(a\right)}=\left(\begin{array}{c}{a}_{\zeta 0}^{\left(a\right)}\\ j_{\zeta 0}^{\left(a\right)}\\ j_{\mu 0}^{\left(a\right)}\end{array}\right),{\mathit{c}}_{in}^{\left(d\right)}=\left(\begin{array}{c}{a}_{\zeta 0}^{\left(d\right)}\\ j_{\zeta 0}^{\left(d\right)}\\ j_{\mu 0}^{\left(d\right)}\end{array}\right),{\mathit{c}}_{bin}={\mathit{c}}_{in}^{\left(a\right)}-{\mathit{c}}_{in}^{\left(d\right)}$     (129)

Final Kinematics Boundary Constraints. The completion of a maneuver in curvilinear motion typically implies a final stable position, and therefore certain kinematic parameters may be predetermined. Often, after a maneuver, an additional situational assessment is required, and a new transient process may take some time. Therefore, it is recommended to ensure that the kinematic and model parameters yield such output values that keep the vehicle stable and in a steady state.

Longitudinal Speed. If a vehicle starts a maneuver with a certain longitudinal speed $V_{\zeta }$ relative to its own local coordinates, a constraint can be imposed on the final speed - for example, if it corresponds to the maximum permitted limit. This parameter is especially important when the vehicle must come to a complete stop by the end of the maneuver.

Lateral speed and its derivative. Since the solution is determined in the rail coordinate system, to guarantee the finalization of lateral displacement, it is necessary that the transverse speed $V_L$ and its derivative $d{V}_L/d{s}_r$ at the final point are equal to zero. In this case, the vehicle will continue moving strictly parallel to the rail.

Longitudinal acceleration$a_{\zeta }$, longitudinal$j_{\zeta }$, and lateral$j_{\mu }$jerks. These are complex kinematic parameters that incorporate numerous other kinematic components. However, to ensure full stationarity after the maneuver is complete, we can require that these parameters become zero even for curvilinear motion. Then, it is most convenient to execute the transient process for the next planning stage.

${\mathit{c}}_{bf}^{\left(a\right)}=\left(\begin{array}{c}{V}_{\zeta f}^{\left(a\right)}\\ V_{Lf}^{\left(a\right)}\\ \begin{array}{c}{\left.{\displaystyle \frac{d{V}_L}{d{s}_r}}\right|}_f^{\left(a\right)}\\ a_{\zeta f}^{\left(a\right)}\\ \begin{array}{c}{j}_{\zeta f}^{\left(a\right)}\\ j_{\mu f}^{\left(a\right)}\end{array}\end{array}\end{array}\right),{\mathit{c}}_{bfd}=\left(\begin{array}{c}{V}_{\zeta f}^{\left(d\right)}\\ V_{Lf}^{\left(d\right)}\\ \begin{array}{c}{\left.{\displaystyle \frac{d{V}_L}{d{s}_r}}\right|}_f^{\left(d\right)}\\ a_{\zeta f}^{\left(d\right)}\\ \begin{array}{c}{j}_{\zeta f}^{\left(d\right)}\\ j_{\mu f}^{\left(d\right)}\end{array}\end{array}\end{array}\right),{\mathit{c}}_{bf}={\mathit{c}}_{bfa}-{\mathit{c}}_{bfd}$     (130)

Final Geometry Boundary Constraints. These are among the most crucial constraints as they ensure safety, stability, and the vehicle's precise position at the end of the maneuver.

Final lateral displacement. If this parameter is included in the objective function, the accuracy of the lateral displacement is not always guaranteed. However, since trajectory planning primarily involves shifting by a lane width, it is extremely important to guarantee that at the end of the maneuver, the vehicle remains strictly in the middle of the new driving lane. Therefore, we enforce the problem of tight convergence between the actual and desired values.

Final Angle. Assuming that the marking lines of adjacent driving lanes are represented by curves converging at a single center for each infinitesimally small segment, as shown in Figure 1, regardless of the lane, the final angular position of the vehicle must strictly correspond to the final track angle. This means that the vehicle is properly oriented, and directional stability is guaranteed.

Final curvature. To enhance the stability effect, one can also specify the final curvature of the maneuver trajectory, which can be calculated using Eqs. (8)-(11). Since curvature directly affects the slip angle and the position of the steered wheels, the subsequent transient process will occur as smoothly as possible without jerks.

${\mathit{c}}_{gf}^{\left(a\right)}=\left(\begin{array}{c}{L}_f^{\left(a\right)}\\ {\alpha }_{tf}^{\left(a\right)}\\ K_{tf}^{\left(a\right)}\end{array}\right),{\mathit{c}}_{gf}^{\left(d\right)}=\left(\begin{array}{c}{L}_f^{\left(d\right)}\\ {\alpha }_{tf}^{\left(d\right)}\\ K_{tf}^{\left(d\right)}\end{array}\right),{\mathit{c}}_{bg}=c_{gf}^{\left(a\right)}-c_{gf}^{\left(d\right)}$     (131)

Discrete constraints. Similar to the rail case in Eq. (62), we need to stitch the fourth-order derivatives at the FE nodes. Therefore, we again require the adjacent nodes' values of the 4th-order velocity derivatives along and across the rail to match. Then, introducing the generalized notation for indices $p\in \left[r,L\right]$, we write:

${\mathit{c}}_{dp}^{\left(0\right)}=\left(\begin{array}{c}{\left.{\displaystyle \frac{d^4{V}_p}{{ds}_r^4}}\right|}_0^{\left(2\right)}\\ ⋮\\ {\left.{\displaystyle \frac{d^4{V}_p}{{ds}_r^4}}\right|}_0^{\left(N\right)}\end{array}\right),{\mathit{c}}_{dp}^{\left(f\right)}=\left(\begin{array}{c}{\left.{\displaystyle \frac{d^4{V}_p}{{ds}_r^4}}\right|}_f^{\left(1\right)}\\ ⋮\\ {\left.{\displaystyle \frac{d^4{V}_p}{{ds}_r^4}}\right|}_f^{\left(N-1\right)}\end{array}\right),{\mathit{c}}_{dp}={\mathit{c}}_{dp}^{\left(0\right)}-{\mathit{c}}_{dp}^{\left(f\right)}$      (132)

where $N$ is the number of FEs.

Grouping the conditions, we obtain the continuity conditions for the 4th-order velocity derivatives:

${\mathit{c}}_d=\left(\begin{array}{c}{\mathit{c}}_{dr}\\ {\mathit{c}}_{dL}\end{array}\right)=0$      (133)

Integral constraints. This approach allows collapsing a distributed problem into a pointwise one and significantly reduce the dimension of the equality constraints vector. The idea is that, using an integration approach in Gaussian quadrature common with the optimization, one requires that the integral between the upper and lower limits - which can also be expressed functionally - strictly corresponds to the sum of the integrals between the upper limit and the function, and between the function and the lower limit. If the function does not exceed the boundary limits, the condition is satisfied automatically. Thus, we guarantee that the parameter does not go beyond the established bounds throughout the entire plan.

General approach. Assuming the parameters are smooth functions, the constraints may be expressed via the integral approach based on the Gaussian scheme used above. The base technique for a parameter $\psi$ not exceeding the upper ${\psi }_U$ and lower ${\psi }_L$ boundaries is expressed as follows:

${\displaystyle \underset{0}{\overset{S_r}{\int }}}\left({\psi }_U-{\psi }_L\right)d{s}_r={\displaystyle \underset{0}{\overset{S_r}{\int }}}\left|{\psi }_U-\psi \right|d{s}_r+{\displaystyle \underset{0}{\overset{S_r}{\int }}}\left|\psi -{\psi }_L\right|d{s}_r$     (134)

Substituting the ratio of reference differential and reference speed instead of time differential, each integral may be considered in the reference Frenet coordinates. Integral within the upper and lower bounds (UL):

${\displaystyle \underset{0}{\overset{S_r}{\int }}}\left({\psi }_U-{\psi }_L\right)d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}\left({\psi }_U-{\psi }_L\right)d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{\psi }^{\left(UL\right)}\left(\xi \right)d\xi$     (135)

Similar to Eqs. (67)-(69),

$z_{\psi }^{\left(UL\right)}\left(\xi \right)=\left({\psi }_U-{\psi }_L\right),{\mathit{z}}_{\psi }^{\left(UL\right)}=z_{\psi }^{\left(UL\right)}\left(\xi \left({\mathit{\vartheta }}^T\right)\right),{\mathit{I}}_{\psi }^{\left(UL\right)}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}\mathit{\Delta }{\xi }_i{\mathit{z}}_{i\psi }^{\left(UL\right)}\mathit{w}$     (136)

Integral within the upper bound and ψ-function (Uf):

${\displaystyle \underset{0}{\overset{S_r}{\int }}}\left|{\psi }_U-\psi \right|d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}\left|{\psi }_U-\psi \right|d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{\psi }^{\left(Uf\right)}\left(\xi \right)d\xi$     (137)

where,

$z_{\psi }^{\left(Uf\right)}\left(\xi \right)=\left|{\psi }_U-\psi \right|,{\mathit{z}}_{\psi }^{\left(Uf\right)}=z_{\psi }^{\left(Uf\right)}\left(\xi \left({\mathit{\vartheta }}^T\right)\right),{\mathit{I}}_{\psi }^{\left(Uf\right)}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}\mathit{\Delta }{\xi }_i{\mathit{z}}_{i\psi }^{\left(Uf\right)}\mathit{w}$     (138)

Integral within the $\psi$ -function and lower bound (fL):

${\displaystyle \underset{0}{\overset{S_r}{\int }}}\left|\psi -{\psi }_L\right|d{s}_r=S_r{\displaystyle \underset{0}{\overset{1}{\int }}}\left|\psi -{\psi }_L\right|d\xi =S_r{\displaystyle \underset{0}{\overset{1}{\int }}}{z}_{\psi }^{\left(fL\right)}\left(\xi \right)d\xi$     (139)

where,

$z_{\psi }^{\left(fL\right)}\left(\xi \right)=\left|\psi -{\psi }_L\right|,{\mathit{z}}_{\psi }^{\left(fL\right)}=z_{\psi }^{\left(fL\right)}\left(\xi \left({\mathit{\vartheta }}^T\right)\right),{\mathit{I}}_{\psi }^{\left(fL\right)}\approx {\displaystyle \frac{S_r}{2}}{\displaystyle \sum _{i=1}^N}\mathit{\Delta }{\xi }_i{\mathit{z}}_{i\psi }^{\left(fL\right)}\mathit{w}$     (140)

The requirement of nonlinear equality constraints along the reference S_r occurs as follows

$c_{\psi }={\mathit{I}}_{\psi }^{\left(UL\right)}-\left({\mathit{I}}_{\psi }^{\left(Uf\right)}+{\mathit{I}}_{\psi }^{\left(fL\right)}\right)=0$     (141)

Kinematic parameters. We may impose restrictions on some kinematic parameters grouped in a vector ${\mathit{\psi }}_k$. Then, introducing vectors of upper ${\mathit{\psi }}_{kU}$ and lower ${\mathit{\psi }}_{kL}$ limits, the vector ${\mathit{c}}_{int}^{\left(k\right)}$ of nonlinear integral constraints, can be evaluated using the technique from Eqs. (134)-(141).

${\mathit{\psi }}_k=\left(\begin{array}{c}{V}_{\zeta }\\ a_{\zeta }\\ \begin{array}{c}{a}_{\mu }\\ j_{\zeta }\\ j_{\mu }\end{array}\end{array}\right),{\mathit{\psi }}_{kU}=\left(\begin{array}{c}{V}_{\zeta U}\\ a_{\zeta U}\\ \begin{array}{c}{a}_{\mu U}\\ j_{\zeta U}\\ j_{\mu U}\end{array}\end{array}\right),{\mathit{\psi }}_{kL}=\left(\begin{array}{c}{V}_{\zeta L}\\ a_{\zeta L}\\ \begin{array}{c}{a}_{\mu L}\\ j_{\zeta L}\\ j_{\mu L}\end{array}\end{array}\right),{\mathit{c}}_{int}^{\left(k\right)}=\left(\begin{array}{c}{c}_{V\zeta }\\ c_{a\zeta }\\ \begin{array}{c}{c}_{a\mu }\\ c_{j\zeta }\\ c_{j\mu }\end{array}\end{array}\right)$     (142)

Critical speed$V_{\zeta s}$. The criterion is based on limiting the tire sideslip potential. Supposing all the wheels are driving, the expression occurs as follows

$V_{\zeta s}=\sqrt{g{\phi }_{\mu }\mathit{cos}\left({\beta }_t\right)/K_t}$     (143)

where $g$ = gravity acceleration constant, ${\phi }_{\mu }$ = lateral adhesion coefficient.

If the current total adhesion realized in longitudinal direction is ${\phi }_{\zeta }$ and the maximum adhesion potential corresponds to ${\phi }_{max}$, the lateral limit is

${\phi }_{\mu }={\phi }_{max}\sqrt{1-{\left({\phi }_{\zeta }/{\phi }_{max}\right)}^2}$     (144)

where

${\phi }_{\zeta }=a_{\zeta }/g+f_d+f_r$     (145)

where $f_r$ is the coefficient of total rolling resistance, and $f_d$ is the specific drag force.

Since $V_{\zeta s}$ may vary in a range $[V_{\zeta smin},\infty )$, the maximum allowable speed should be preset lesser. Thus, to satisfy this requirement, a rapidly growing function like $\chi \left(\cdot \right)=\mathit{tanh}\left(\cdot \right)$ may help. Then, the integral condition is as follows

${\displaystyle \underset{0}{\overset{S_r}{\int }}}\chi \left(V_{\zeta s}\right)d{s}_r={\displaystyle \underset{0}{\overset{S_r}{\int }}}\left|\chi \left(V_{\zeta s}\right)-\chi \left(V_{\zeta }\right)\right|d{s}_r+{\displaystyle \underset{0}{\overset{S_r}{\int }}}\chi \left(V_{\zeta }\right)d{s}_r$     (146)

Using Eqs. (143)-(146), physical restrictions on the AV speed may be imposed. Then

${\mathit{\psi }}_s=V_{\zeta },{\mathit{\psi }}_{sU}=V_{\zeta s},{\mathit{\psi }}_{sL}=0,c_{int}^{\left(s\right)}=c_{Vs}$     (147)

Resulting integral constraints

${\mathit{c}}_{int}=\left(\begin{array}{c}{\mathit{c}}_{int}^{\left(k\right)}\\ c_{int}^{\left(s\right)}\end{array}\right)=0$     (148)

The aggregate boundary constraints and the full set of constraints are formed as

${\mathit{c}}_b=\left(\begin{array}{c}{\mathit{c}}_{bm}\\ {\mathit{c}}_{bp}\\ \begin{array}{c}{\mathit{c}}_{bin}\\ {\mathit{c}}_{bf}\\ {\mathit{c}}_{bg}\end{array}\end{array}\right)$, ${\mathit{c}}_{eq}=\left(\begin{array}{c}{\mathit{c}}_b\\ {\mathit{c}}_d\\ {\mathit{c}}_{int}\end{array}\right)$      (149)

The only requirement when selecting constraint parameters is that they do not duplicate or conflict with one another, as this can negatively affect the optimization time. If there are no specific requirements for other parameters, their constraints can be omitted.

6. Simulation

Within the scope and constraints of this study, we consider generating motion plans for an AV moving along a curved section of a three-lane unidirectional road. It is assumed that the AV moves at an initial speed of $V_{\zeta 0}=60$ km/h in the middle lane (Figure 3) and intends to perform a lane change to one of the outer lanes. We employ 10 finite elements per segment and a 5-point Gauss quadrature integration scheme. Optimization is performed using the SQP (Sequential Quadratic Programming) and SQP-legacy algorithms in the MATLAB fmincon function.

6.1. Virtual Rail Generation

As mentioned, having at least 3 centerline points in the driving lane enables the construction of an initial spline that defines the road's curvature. The spline does not require optimization and is rapidly computed via matrix equations. Although it cannot represent higher-order curvature derivatives, it serves as an initial source of basic geometric information for transitioning into the curvilinear coordinate space. To ensure a tight fit of the virtual rail to the spline, it is recommended to relax rigid constraints. Thus, here we maintain only the requirements for matching the coordinates of the endpoint and the final angle, allowing the degrees of freedom to smoothly compensate for the angle mismatch between the spline and the rail (Figure 3). Note that the length of the rail itself also varies slightly during the optimization process relative to the penultimate spline point to achieve a more adaptive approximation search.

The weighting coefficients in Eq. (60) are chosen based on the influencing factors and reflect a trend rather than a final combination, since much depends on how the components of the objective function in Eq. (59) are normalized. Consequently, for this example: $W_s=0.01$, $W_a=4$, $W_{K_r}=0.05$, $W_{d{K}_r}=0.1$, $W_{d2K_r}=0.1$, $W_{d3K_r}=0.1$, $W_{d4K_r}=1$, $W_{d5K_r}=35$, which yields the following results for the rail geometry (Figure 4).

Thus, while the tightness of the rail node fit to the spline is primarily determined by the integral component of curvature (angle), the smoothness of the curvature and its derivatives is established at the stage of discontinuities at nodes, i.e., its highest 5th derivative (Figure 4d). To achieve this, it is necessary to ensure the tightest grouping of «shelf-constants» so that transitions between angles of the 4th derivative, represented by a broken line, are as small as possible. Note also that here the effect of triggered «fictitious» adjacent nodes, provided for in our constraints in Eq. (62), is clearly visible. This explains the conditionally largest coefficient $W_{d5K_r}$.

As mentioned in the rail theory, the third derivative of curvature must be continuous and smooth (Figure 4c). That is why the influence of $W_{d3K_r}$ ranks second. The graphs of the 2nd and 3rd derivatives are the core of the 2-DOF model, yielding high smoothness, which undoubtedly has a positive effect on all parameters during motion plan construction, since they are included in base expressions. However, the influence of the $W_{d4K_r}$ and $W_{d5K_r}$ components themselves need to be reduced so as not to prevent the rail from being flexible during optimization, since the minimum of the derivative is always a straight line. Consequently, to change the monotonicity of curves (Figure 4b), the value of the coefficient $W_{d{K}_r}$ is chosen such as to support the effect of local curvature redistribution without hindering rail flexibility. The coefficient $W_{K_r}$ is excessively sensitive and therefore has a low priority. We do not strive for minimal curvature, as this would increase errors in spline and rail angles, but we also require smoothness of the rail angle function. The factor $W_a$ is the main driving force of optimization and its value is due to the influence of the discrepancy between the rail and spline angles in the natural coordinate system.

Due to the different nature of curvature and the spline, it is impossible to precisely ensure equality of their arcs. The desire to «pull» the rail into the spline causes the adverse effect of disrupting the smoothness of curvature derivatives, since the accumulated error in the finite element (FE) will be compensated by excessive deflections of the rotational degree of freedom at the node. By virtue of the arguments, the value of the coefficient $W_s$ also has a small (or zero), limited positive impact. As a result, we can see that the deviation $e_s$ of the rail angle from the spline angle (Figure 4a) accumulates parabolically to approximately the center of the rail and does not exceed ${0.05}^{\circ }$, which guarantees high convergence in coordinates.

Note that optimization based on angle differences is more advantageous than based on coordinate differences, since it is performed in natural coordinates and does not require additional computational procedures.

6.2. Motion Planning

The obtained rail geometry will now be used to find the optimal distribution plan for kinematic parameters represented by decomposition along and across the rail. Note that the rail itself can also «shrink» to vary the maneuver basis. However, in this work, this variation is assumed to be zero, and the reference generation technique is considered for the entire rail length.

To ensure the vehicle shifts from the center of one lane to the center of another, we set the final lateral coordinate $L_f$ equal to the lane width $W_L=3.75\mathrm{m}$, considering the sign («+» for left and «-» for right). To ensure that the car remains strictly in the middle of the lane after the maneuver, it is necessary to zero out the velocity $V_{Lf}$ and its derivative $d{V}_L/d{s}_r{|}_f$. Similarly, using the transition Eq. (87) from the vehicle's local coordinate system to the rail's curvilinear coordinate system, we can formulate the initial conditions $V_{r0}$, $V_{L0}$, $d{V}_r/d{s}_r{|}_0$, $d{V}_L/d{s}_r{|}_0$. At the same time, we impose the following constraints on the kinematic parameters: for longitudinal acceleration, $a_{\zeta max}=3{\mathrm{m}/\mathrm{s}}^2$ and $a_{\zeta min}=-1{\mathrm{m}/\mathrm{s}}^2$; for lateral acceleration, $a_{\mu max}=-a_{\mu min}=6{\mathrm{m}/\mathrm{s}}^2$; for longitudinal jerk, $j_{\zeta max}=-j_{\zeta min}=13{\mathrm{m}/\mathrm{s}}^3$; for lateral jerk, $j_{\mu max}=-j_{\mu min}=13{\mathrm{m}/\mathrm{s}}^3$.

For this example, let us set the following weight coefficients: $W_{\mathsf{\Delta }{V}_{\zeta }}=5$, $W_{L_f}=0$, $W_{{\alpha }_f}=0$, $W_{j_{\zeta }}=70$, $W_{j_{\mu }}=30$, $W_{{d3V}_r}=0$, $W_{{d3V}_L}=0$, $W_{{d4V}_r}=50$, $W_{{d4V}_L}=50$, $W_{{d5V}_r}=50$, $W_{{d5V}_L}=50$. Based on numerous tests, we found that the relative values of the weight coefficients are justified by the following points. The coefficients $W_{d5V_{\zeta }}$ and $W_{d5V_L}$ have one of the greatest influences on the speed and convergence quality of optimization, since they affect the alignment of the slope angles of the 4th derivative broken lines of velocities. The coefficients $W_{d4V_{\zeta }}$ and $W_{d4V_L}$ are also highly significant factors, as this is precisely where the minimization of «waviness» and the smoothness of lower-order derivatives are established.

Note that at this stage, the coefficients are on a parity basis for «stitching» the derivatives of each velocity model. The coefficients $W_{d3V_{\zeta }}$ and $W_{d3V_L}$ are set to zero so as not to hinder the «flexibility» of the curves of the base 2nd-derivative velocity model, while smoothness is guaranteed by the 4th derivative. The coefficient $W_{j_{\zeta }}$ has the greatest influence and is responsible for minimizing longitudinal jerks in the vehicle coordinate system. The parameter $j_{\zeta }$ incorporates many other kinematic parameters, which influence them as well. The coefficient $W_{j_{\mu }}$ has a moderate value, since the requirements for a simultaneous minimum of jerks in the longitudinal and lateral directions are inherently contradictory. The smoothness of the lateral jerk is inevitably coupled with the longitudinal one, and in this sense, the factors are not independent. The coefficients $W_{L_f}=0$, $W_{{\alpha }_f}=0$ are set to zero, since the desired final values are already specified in the constraints. Non-zero values are relevant if the search for the final position is expressed through soft constraints, and the coefficient $W_{\mathsf{\Delta }{V}_{\zeta }}$ is the main driving factor forcing the car to increase speed. It is assumed that the vehicle can accelerate to a target speed of $V_{\zeta t}=80\mathrm{km}/\mathrm{h}$.

In addition, we impose constraints on the final values of the angle and trajectory curvature, as well as on the continuity of the 4th derivative of velocities at the nodes.

Leftward lane change.

Figure 5. Distribution of speeds and accelerations along the rail length: (a) speeds along and across the rail, (b) accelerations in the rail coordinates, (c) speeds in the vehicle coordinate system, (d) accelerations in the vehicle coordinate system.

Figure 5. Distribution of speeds and accelerations along the rail length: (a) speeds along and across the rail, (b) accelerations in the rail coordinates, (c) speeds in the vehicle coordinate system, (d) accelerations in the vehicle coordinate system.

Figure 6. Distribution of speed derivatives: (a) 1st, (b) 2nd, (c) 3rd, (d) 4th.

Figure 6. Distribution of speed derivatives: (a) 1st, (b) 2nd, (c) 3rd, (d) 4th.

Figure 7. Maneuver smoothness indicators: (a) jerk, (b) angular velocity and acceleration, (c) yaw and central slip angles, (d) relative trajectory angle and lateral displacement.

Figure 7. Maneuver smoothness indicators: (a) jerk, (b) angular velocity and acceleration, (c) yaw and central slip angles, (d) relative trajectory angle and lateral displacement.

Rightward lane change.

Figure 8. Distribution of speeds and accelerations along the rail length: (a) speeds along and across the rail, (b) accelerations in the rail coordinates, (c) speeds in the vehicle coordinate system, (d) accelerations in the vehicle coordinate system.

Figure 8. Distribution of speeds and accelerations along the rail length: (a) speeds along and across the rail, (b) accelerations in the rail coordinates, (c) speeds in the vehicle coordinate system, (d) accelerations in the vehicle coordinate system.

Figure 9. Distribution of speed derivatives: (a) 1st, (b) 2nd, (c) 3rd, (d) 4th.

Figure 9. Distribution of speed derivatives: (a) 1st, (b) 2nd, (c) 3rd, (d) 4th.

Analysis of Results.

Smoothness. The analysis of the obtained graphical dependencies clearly demonstrates the high smoothness of the generated plan. The curves show that the underlying manipulation of speeds and their derivatives in the longitudinal and lateral directions (along and perpendicular to the rail) ensures function continuity and eliminates abrupt jumps. Since the model is initially formulated in terms of second and third derivatives, their targeted optimization ensures that the curves in integration and differentiation plots (including combined kinematic parameters) evolve smoothly, providing inherent stability for the entire system. Minor local inflections in higher-order derivatives observed on the plots outside the knot points (like in Figure 6c, Figure 9c) are purely boundary-related - they arise exclusively from the strict enforcement of boundary conditions at the element ends. However, these effects remain localized and do not violate the overall monotonicity and predictability of the system behavior.

Continuity. The analysis of the graphs also confirms the achievement of the key effect of continuity throughout the entire trajectory structure. Even at the level of higher-order derivatives (up to the fourth), where the curves naturally lose their ideal smoothness and take on a step-like character, full continuity at the knot points is maintained without any breaks. This is achieved through boundary conditions strictly enforced during the optimization process, which require adjacent nodes to coincide, effectively «stitching» the elements together at the knot points and preventing any misalignment (Figure 6d, Figure 9d). It is here, at the level of rigorous high-order nodal alignment, that the primary stability factor of the entire system is established. If this requirement were omitted, discontinuities would inevitably propagate to lower-order levels; however, because the elements are reliably linked at the fourth derivative, the third derivative (which is directly incorporated into the basic model) is automatically rendered continuous, ensuring the predictable and stable behavior of the entire geometry.

Convergence. The analysis of the optimization process and the resulting convergence plots demonstrates high stability and predictability of the algorithm's performance. This is rooted in the model's core structure because the finite element representation based on Hermite polynomials rigorously structures the search space and analytically links parameters via nodal degrees of freedom; the nonlinear programming (SQP) task avoids excessive iterations and prevents the solver from becoming trapped in undesirable local extrema. It should be noted that at this stage, the optimizer was configured based on an assessment of factor influence, reflected through weighting coefficients; nevertheless, even with this approach, the method exhibits excellent convergence. Future research will focus on developing formalized normalization techniques and automated tuning procedures to further enhance the algorithm's efficiency and convergence speed.

Feasibility and Constraint Compliance. The analysis of the obtained graphs also confirms the full feasibility of the trajectories and the strict compliance of all parameters with the specified constraints. First, none of the curves exceed the preset limits, clearly demonstrating the effective performance of the constraints introduced into the model. Second, the parameter values on the graphs fully correspond to the capabilities of their physical implementation within the tracking loop, confirming that the vehicle can execute such trajectories without entering non-physical zones. The maneuver duration is approximately $t=2.7\mathrm{s}$.

Low Slip Angle. The analysis of the side-slip angle distribution graphs (Figure 7c, Figure 10c) demonstrates consistently small values throughout the entire maneuver. This confirms that the motion kinematics are in strict alignment with tire dynamics: the vehicle moves without pronounced side slip, and the velocity vectors remain close to the wheels' longitudinal axis. The low slip angles indicate control stability and show that the trajectory is executed smoothly, without critical lateral loads on the tires.

Strict Final Position. The analysis of the final trajectory segments (Figure 7d, Figure 10d, Figure 11) shows that the algorithm guides the vehicle to the target point with high precision, achieving the specified final position and orientation. The absence of steady-state error at the terminal stage demonstrates the effectiveness of the boundary conditions and terminal constraints, thereby guaranteeing precise execution of the positioning task at the end of the maneuver.

7. Conclusions

This study aimed to develop an advanced mathematical framework for synthesizing a motion plan and references in a natural (curvilinear Frenet) coordinate system. Unlike classical sequential approaches that separate the search for spatial geometry and kinematics profiling into distinct stages, the proposed method implements a unified (integrated) concept. By referencing a spatial guide (virtual rail), optimization is performed simultaneously in two directions - along and across the rail. This makes it entirely possible to abandon the two-stage scheme and the rigid dependency on traffic scenarios during the geometry search phase, thereby ensuring the synchronised distribution of velocities accounting for all constraints and the generation of a comprehensive plan. Based on this study, the advantages of the proposed framework are as follows:

• Mathematical precision. Unlike other planning approaches in the natural Frenet coordinate system, the proposed method is free from simplifications, with all terms represented with high precision. The trajectory plots clearly demonstrate high smoothness in curvature variations and high accuracy of the final position. Despite the apparent complexity of the computational formulas, the optimization process is performed sufficiently fast.
• Dynamic adaptability and criterion balance. In traditional sequential schemes, priority is given to generating a maximally smooth static geometry at the initial stage, which inherently precludes lateral correction - any further adjustments are strictly limited to the longitudinal movement along the pre-fixed path. In contrast, the proposed method implements a fundamentally different paradigm: the spatial trajectory is secondary, serving merely as a result of integration, while the optimization process is centered on achieving optimal dynamics. This simultaneous generation of geometry and kinematics ensures full dynamic elasticity. Consequently, the algorithm can deliberately accept local increases in curvature to achieve superior performance in speed and time, a trade-off that is fundamentally unattainable within the constraints of rigid two-stage concepts.
• Computational performance and optimization parameters. All computational experiments were performed on a hardware platform featuring an Intel Core i7-7500U CPU @ 2.70-2.90 GHz with 8 GB of RAM. The computation time is primarily driven by the constraint set and the required solution accuracy: optimizing the virtual rail takes an average of 0.5 s, while generating the motion plan ranges from 0.5 to 2.4 s. Optimization was carried out using the fmincon solver via the sequential quadratic programming (SQP) algorithm with finite difference gradient approximation. For the rail optimization, the optimality, constraint, and step tolerances were all set to ${10}^{-6}$. For the motion plan generation, the precision was increased by two orders of magnitude with all tolerances set to ${10}^{-8}$, and the maximum number of iterations was capped at 250 (note that convergence is successfully achieved with as few as 50 iterations yielding identical results).
• Mesh and integration point variability. Algorithm performance can be flexibly adjusted over a wide range by varying the number of finite elements and employing Gauss integration schemes with 2 to 8 points. Notably, solution quality is not always strictly proportional to the number of nodes, allowing for an effective trade-off between accuracy and computational speed.
• Constraint flexibility. The proposed framework incorporates numerous constraint factors that can be selectively deactivated to improve computational speed, particularly when handling coupled constraints such as the terminal angle and terminal path curvature.
• Segment matching accuracy. Although this study focuses on a single road segment, the optimized output parameters enable seamless concatenation with subsequent sections. By using the terminal parameters of higher-order derivatives from the current plan as initial conditions for the next plan, trajectory smoothness is automatically guaranteed, even for compound parameters such as jerks.
• Mathematical complexity and fixed rail horizon. Deriving the mathematical framework requires high precision and rigorous multi-stage verification; however, this effort is fully compensated by the quality and speed of prediction. At the current stage, the length of the virtual rail remains fixed (non-telescopic). Consequently, the system is constrained in its ability to adaptively shorten or extend the base trajectory horizon depending on external factors - such as the presence of static and dynamic obstacles (which require real-time horizon adjustments for safe maneuvering) and the tire-road friction coefficient (where low adhesion or safety constraints regarding lead vehicles necessitate a flexible rail length).

While this study establishes the fundamental mathematical framework and simultaneous optimization architecture for advanced Frenet-based motion planning, it opens several promising avenues for future research within this series:

• Comparative Analysis of 2-DOF and 3-DOF Dynamic Models: Future work will extend the framework to a vehicle dynamic model with three degrees of freedom utilizing fifth-degree Hermite polynomials. Although a 3-DOF model introduces more optimization parameters, the increased structural elasticity is expected to permit a significantly reduced number of nodal points, and a dedicated comparative study will evaluate the performance trade-offs between the two formulations.
• Optimization Robustness and Guaranteed Convergence: A rigorous investigation will be conducted to ensure that the proposed optimization process executes within a strictly bounded, finite time and yields predictable results across arbitrary initial conditions, guaranteeing deterministic performance under all operational scenarios.
• Comprehensive Constraint Integration: Upcoming studies will incorporate a much broader spectrum of constraints omitted in the current baseline—specifically focusing on spatial restrictions imposed by static and dynamic obstacles, as well as physical limitations such as tire-road friction boundaries.
• Seamless Multi-Segment Stitching and Lane Change Transitions: Future developments will feature extensive experimentation on managing transient processes during sequential segment stitching. Since the virtual rail and spline are regenerated for each new lane after a maneuver, upcoming work will mathematically and experimentally validate the curvature transition formulas to ensure smooth spatial and temporal continuity across successive planning horizons.