# Efficient Optimization-Based Trajectory Planning for Truck-Trailer Systems
## 2 Methods
### Proposed Solution
#### 2.3.2 Phase 2: Trajectory Planning with Constant Velocity Profile  
The problem of finding a trajectory based on a path supposes a known sequence of points that need to be passed through on the way from the start to the target. The searched result is the vector of control inputs to be applied to the system. To find the velocity, we need to know the total time needed to reach the destination, which is currently unknown. Therefore, the goal of this phase of the trajectory planning is not to find the speed profile yet, but to find the total time required to realize the trajectory. The velocity profile is found in the subsequently Phase 3. In Phase 2, we search for trajectories with a constant velocity profile. In this phase, there is no need to consider vehicle dimensions or collisions with obstacles; therefore the model solution for this phase only considers the truck, and uses the first four differential equations in Equation (2), as referenced later on by Equation (18). Such a simplification is sufficient for the first design of the trajectories, which can serve as a basis for further optimization runs when we have a more complex model of the system. The described truck model has only four states. In this case, for the purpose of incorporating time optimization it is necessary to add a fifth state \(x_{T}\), representing the  
Figure 3: Node graph with initial and final states. total time needed to realize the desired trajectory. We first select an appropriate constant velocity profile \(u_{1}\) for use in Phase 2. This selection adheres to widely accepted kinematic assumptions. Linear velocities below 1-2 m/s (approximately 3.6-7.2 km/h) are generally considered suitable for ensuring minimal dynamic forces such as centripetal acceleration. For urban-like scenarios or controlled environments (e.g., robotics or autonomous systems), a maximum linear velocity of 0.5-1.5 m/s is often applied. In tighter spaces such as parking or obstacle avoidance, the lower end of this range (0.5-1 m/s) is preferred. Based on these considerations, we set \(u_{1}=1\). The total time \(x_{T}\) is determined through an optimization process representing the time required to travel from the initial to the target position. The total travel distance depends on the path determined in Phase 1 while respecting the vehicle's dynamics. Using the constant velocity profile, the travel distance can be expressed as \(u_{1}\cdot x_{T}\). The total time \(x_{T}\) is subsequently utilized in Phase 3, where the velocity profile is refined under the constraints of zero initial and target velocities. To find the total time \(x_{T}\), the simulation step size \(h\) corresponding to the sampling period \(T_{s}\) is supposed to be unitary and to be scaled by the state \(x_{T}\), which is part of decision vector of variables in this phase. The resulting dynamics equations are written in Equation (12). \[\dot{x}_{1} =x_{T}\cdot u_{1}\cdot\cos(x_{4}) \tag{12}\] \[\dot{x}_{2} =x_{T}\cdot u_{1}\cdot\sin(x_{4})\] \[\dot{x}_{3} =x_{T}\cdot(-\frac{1}{\tau}\cdot x_{3}+\frac{1}{\tau}\cdot u_{2})\] \[\dot{x}_{4} =x_{T}\cdot\frac{u_{1}}{d_{0}}\cdot\tan(x_{3})\] \[\dot{x}_{T} =0\]  
Fulfillment of the initial state can be ensured by a simple equality boundary condition, where the corresponding elements of the decision vector are be equal to the given initial state \(\overrightarrow{x}_{0}^{\prime}\). \[\psi= \begin{bmatrix}x_{1}\\ x_{2}\\ x_{3}\\ x_{4}\end{bmatrix} - \begin{bmatrix}x_{0}\\ x_{0}\\ x_{0}\\ x_{0}\end{bmatrix}  \tag{13}\]  
Satisfying the desired final state can also be ensured by similar equality constraints; however, much less restrictive inequality constraints are used here due to the need to fit the system into a predefined target area. This allows for a certain level of relaxation, and significantly increases the chance of finding an admissible solution to the optimization problem. The target area has a rectangular shape containing the truck's target reference point \(R_{0}\), with dimensions denoted by \(x_{\text{tgt}}\) and \(y_{\text{tgt}}\).