# Physics-Based Vehicle Simulation Using PD Servo
## 4 Vehicle Simulation
### Trajectory Optimization  
The controls of a real vehicle on a ground path are limited by various physical constraints such as mass, size, wheel motion, etc. For example, the car cannot make sudden turns on sharp curvatures of the input trajectory. In the proposed system, an optimized trajectory that is physically derivable and as similar as possible to the input trajectory is generated via an iterative search method as shown in Figure 5.  
As a preprocessing step, the input trajectory is segmented into several pieces based on the local maximum and minimum points and reflection points. Each of the local maximum and minimum points indicates a moment of gradient switch between positive and negative values, while each of the reflection points indicates a moment of misalignment between the directions of wheels and vehicle body. Figure 6 shows various input trajectories with segmented points. If \(\mathbf{s}_{i}\) is the \(i\)th segment curve on the input trajectory, there is no direction change for wheels on \(\mathbf{s}_{i}\), making the simulation process more efficient than one uses the whole trajectory. Given the start and end points of \(\mathbf{s}_{i}\), \(P_{s}\), and \(P_{e}\), an initial torque value is estimated from Equation (1) and is averaged by the number of frames between the two points, \(N_{\tau}\) frames. To reach \(P_{e}\) with a proper alignment of wheels, the turning direction, \(D\) (m), and its angle, \(\theta_{D}\) (rad), are estimated as follows:  
\[ \begin{array}{l}D=d_{up}\cdot u\times d_{fw}\\ \theta_{D}=cos^{-1}(\frac{u\cdot d_{fw}}{\|u\|\|d_{fw}\|}),\end{array}  \tag{2}\]  
where \(u=P_{e}-P_{c}\), and \(d_{up}\) and \(d_{fw}\) are the up and forward directions of the vehicle at the current frame, respectively. Here, the vehicle turns right if \(D>0\), turns left if \(D<0\), and goes straight if \(D=0\). Let \(\mathbf{\hat{s}}_{i}\) be a curve simulated by the vehicle based on the direction decided by Equation (2) and the velocity controlled by the PD servo. Due to external forces such as gravity and friction, the end of \(\mathbf{\hat{s}}_{i}\) can be different from the end of \(\mathbf{s}_{i}\). To minimize this offset, we introduced an optimization process in a multi-stage scheme as shown in Figure 7. This optimization process is implemented by reducing errors in an iterative way until the similarity between two curves is over a user-specified threshold, \(\delta_{5}\). The details of the optimization scheme will be described in the next paragraph. For the next segment, \(\mathbf{s}_{i+1}\), the end conditions (i.e., the vehicle direction and velocity) of \(\mathbf{s}_{i}\) should be close enough to the start conditions of \(\mathbf{s}_{i+1}\); otherwise, the vehicle cannot make a smooth transition between two segments. Another similarity threshold value, \(\delta_{T}\), is set such that \(\mathbf{\hat{s}}_{i}\) is regenerated by the simulation process if \(\delta_{T}\) is not satisfied. In our experiments, both \(\delta_{5}\) and \(\delta_{T}\) are set at Figure 5: Overview of generation of optimized trajectory using an iterative search method. Figure 6: Segmentation of input trajectories (blue) with local extremes and inflection points (red). Given multiple trajectory segments, the vehicle direction and velocity at the end point of the current segment become the starting point of the next segment. To simulate a vehicle following the input trajectory as closely as possible, the difference between the direction and velocity at the end point of each segment should be minimized between \(\mathbf{s}_{i}\) and \(\mathbf{\dot{s}}_{i}\). When there is a large difference between these values, our approach restarts the simulation at the previous end point until the difference is within the threshold value.