# Multi-vehicle Path Following using Modified Trajectory Shaping Guidance

###### Abstract

In this paper, we formulate a virtual target-based path following guidance law aimed towards multi-vehicle path following problem. The guidance law is well suited to precisely follow circular paths while minting desired distance between two adjacent vehicles where path information is only available to the lead vehicle. We analytically show lateral and longitudnal stability and convergence on the path. This is also validated through simulation and experimental results.

## 1 Introduction

With fast evolving levels of autonomy, autonomous vehicles seek accurate and easily implementable path following guidance methods. Tracking a virtual target on the desired path provides a promising path following guidance framework. A nonlinear guidance logic for path following was proposed by Park et al.[1, 2] wherein the vehicle uses an instantaneous circular maneuver to pursue a virtual target moving at a constant look-ahead distance on the path. The resulting linear analysis of the system shows an asymptotic convergence to straight line and circular paths. It was observed that a small fixed look-ahead distance very small noise could lead to very high maneuvers and a higher fixed look ahead leads to high With a pure pursuit based guidance law, Medagoda and Gibbens [3] used a new virtual target motion logic where the virtual target speed varies with its relative distance to the vehicle. This adaptive approach overcomes the problems arising due to a fixed look-ahead distance.

Missile guidance theory comprises a rich collection of empirical and optimal guidance laws. Using linearized kinematics, Ryoo et al. [4] derived the optimal trajectory shaping missile guidance law against stationary targets. The shaping effect is characterized by the terminal angle of approach at the target. The trajectory shaping guidance command is a function of vehicle heading, line-of-sight angle and the desired terminal approach angle. In a recent work, Ratnoo et al. [5] used the trajectory shaping guidance law for unmanned vehicle path following considering a virtual target on the path. The work shows that even in highly offset initial conditions, trajectory shaping guidance outperforms pure pursuit in terms of path-following errors and also has a faster rate of convergence as compared nonlinear guidance logic. However, as acknowledged in [5] , the trajectory shaping convergence property for curved paths holds only for small look ahead distances relative to the path radius. As the look-ahead distance to path radius ratio increases the vehicle converges to a larger offset froPolytechnic Institute and State Universitym the path radius. In a single vehicle path following scenario the look ahead distance to the virtual target is a design/tuning parameter. For multiple vehicle path following, or platooning, the look ahead distance is an input for maintaining a desired inter-vehicle separation, which could be high due to safety and stability reasons. Also, certain road or weather conditions may also require large look-ahead distances. In a platoon, due to the nature of each vehicle following the one in front of itself, any path following offset amplifies as it propagates through to the end of the platoon. This motivates a need to address the limitation of the trajectory shaping algorithm in accurately converging to path for any desired look ahead distance. The existing platooning work [6, 7] require reference markerPolytechnic Institute and State Universitys and and marker detection capability on each vehicle for path following. In this paper, we use trajectory shaping guidance concept to platooning where only the lead vehicle require path knowledge and other vehicles can follow the path by just following the vehicle in front of it. Although the approach in this paper seems similar to the cyclic pursuit concept [8, 9, 10], both have different outcomes. Cyclic pursuit is a formation control problem with three different formations, namely, rendezvous to a point, evenly spaced circular formation, and evenly spaced logarithmic spirals. However, in addition to precise path convergence, the multiple vehicle path following problem in this paper achieves precise and desired separation on a circular path.

The main contributions of this paper are as follows. First, the convergence properties of the trajectory shaping guidance algorithm are investigated. Its dependence on and a nonzero offset from the path are shown. Secondly, the trajectory shaping guidance algorithm is modified to follow curved paths at zero offset. Finally, the modified trajectory shaping law is then applied to control a platoon of vehicles by making each vehicle the virtual target of the vehicle behind it. It is shown both analytically and with the help of experiments on a multi-robot test-bed that the platoon converges to a constant radius path.

This rest of the paper is organized in the following manner. Section 2 discusses the regular and modified trajectory shaping guidance law. Section 3 presents platooning using modified trajectory shaping guidance and an convergence analysis of the proposed method. Section 4 shows and discusses the simulation and hardware results of the modified trajectory shaping applied to a single vehicle and a platoon. Concluding remarks are presented in Section 5.

## 2 Trajectory Shaping Guidance

Consider a vehicle moving at velocity and a virtual target moving on a given path at velocity as shown in Fig. 1. Trajectory shaping guidance law governs the vehicle’s heading direction so as to achieve a tail-chase approach to the virtual target which is constrained to move along the desired path. The virtual target moves at a distance ahead of the vehicle. The virtual target moves with a relatively lower speed drawing the vehicle closer to the path. As the distance reduces, the virtual target speed increases and finally matches the vehicle’s speed at the desired distance . The states of the system are where is relative target heading, is the relative vehicle heading, here is line-of-sight angle, and are vehicle and virtual target heading angles, respectively. The input vector is where is the curvature of the path and is the commanded velocity of the vehicle following the path. The equations of motion can be written as

(1) |

where is velocity controller gain.

Trajectory shaping guidance [5] will be referred to in this paper as regular trajectory shaping guidance. It controls the vehicle’s lateral acceleration and the virtual target velocity using the following equations

(2) |

(3) |

Note that, the speeds become identical as the vehicle reaches the desired relative distance following the virtual target.

It was shown that the regular trajectory shaping guidance achieves convergence to a straight line and near convergence to a circular path. However, this is under the assumption of small relative heading angles and obtained for . It can be shown that the vehicle will actually converge to an offset of the path radius when is not met. Fig. 1 shows that the desired equilibrium point for the vehicle to be on the circular path at a desired distance is the following.

(4) |

However, it can be shown that for regular trajectory shaping (2) and (1), for straight line , but for circular paths . This means that unless the path is a straight line, the equilibrium point of the regular method is not the desired equilibrium point and thus the vehicle will not converge to the path at the desired distance.

### 2.1 Modified Trajectory Shaping Guidance

In order to obtain accurate path convergence, the lateral acceleration command of the regular trajectory shaping guidance in (2) is modified as follows.

(5) |

As can be seen, the only modification from (2) is to take the sine of the two angles of interest. Throughout this paper the modified method will be referred to as modified trajectory shaping or the sine method. It can be easily verified that at and from (4) for modified trajectory shaping law for both circular and straight line trajectories. This shows that the desired equilibrium point is an actual equilibrium point for the sine method which will result in zero offset.

It will be also shown in the next section that the system around the desired equilibrium point is stable and the vehicle converges on the path with zero offset independent of the value. The modified trajectory shaping will be used for multiple vehicle path following/platooning in the next section.

## 3 Platooning Using Modified Trajectory Shaping

In this section we use the modified/sine trajectory shaping method for platooning. Consider vehicles and a path to follow as shown in Fig. 2. We assume that the first vehicle has knowledge of the path and it follows a virtual target moving on the path. Each vehicle follows the vehicle in front of it ( vehicle) as a virtual target.

The state vector for a vehicle platoon is where . The input vector is includes the curvature of the path and the commanded velocity of the last vehicle. The overall equations of the motion are written as

(6) |

State space equation for the vehicle can be written as

(7) |

where , , , , and

The lateral acceleration command is the lateral controller to minimize path error and the velocity controller is the longitudinal controller for maintaining desired separation between two vehicles. These control equations show (also shown in Fig. 2) that the information needed for the lateral acceleration commands are passed backward through the platoon. The information needed for velocity control are passed forward through the platoon. This asymmetric information flow provides some benefits to the system when dealing with disturbances. For example, if a vehicle receives a lateral disturbance from the path, the vehicles in front of that vehicle won’t be affected laterally, they will remain on the path. If a vehicle receives a longitudinal disturbance, the vehicles behind it won’t be affected longitudinally. This is revisited in the simulation and hardware results of Section 4.

### 3.1 Platooning Stability Analysis

For stability analysis we use linearization around the equilibrium point on the circular path. It can be easily verified that at equilibrium on the circular path

Linearizing (6) at the equilibrium point gives where is the deviation from the equilibrium point and is an almost block diagonal (staircase) matrix

(8) |

where

where . Since , and is real. Due to the asymmetric information flow . There are a total of eigenvalues of and the system will exponentially converge to the equilibrium point on the path when disturbed if all the eigenvalues have negative real parts. After analysis and algebraic manipulations the following eigenvalues are computed

(9) | ||||

where is a function of , for . It can be seen that the above eigenvalues only depend on and are independent of the number vehicles . Since real parts of all the eigenvalues are negative, we can say that the platoon is exponentially stable in the neighborhood of the equilibrium point on the path. This means that if the platoon is disturbed the vehicles will converge back to the path.

## 4 Results

This section discusses the results obtained through simulations and hardware experiments using the modified trajectory shaping guidance law to platooning control. Simulation results will first be analyzed to show path convergence of every vehicle in the platoon. Similar tests were performed on Pololu m3pi robots and also show path convergence of all vehicles.

### 4.1 Simulation Results

The simulations were created using the same framework as used for the simulations in [11]. However, the simulations in this paper more closely relate to highway conditions. Each vehicle is treated as the target for the vehicle behind it. The simulations run for this section were performed with the following parameters: , (unless otherwise specified), and . However to first test the effectiveness of the sine method, several scenarios were simulated for a single vehicle as shown in Fig. 3.

For platooning, convergence was shown analytically to occur for the sine method of trajectory shaping. The regular method has been shown to converge to an offset and each vehicle will converge to an offset of the circle converged to by the vehicle in front of it. Therefore, the offset adds with each vehicle in the platoon. This was tested in simulation as shown in Fig. 4 and in the video https://youtu.be/GLPrj4l3o38.

As can be seen, the sine method had every vehicle converge to the desired path at the desired spacing. The regular method converged to a slight offset that can be seen in the velocity and distance error plots, but is more difficult to see on the path error plot. The last vehicle in the regular method converged to a circle with an offset of . This would equate to vehicle 4 being several lanes over on the highway. (an important property for platooning) It was also desired to see how the system reacts to a disturbance. The acceleration disturbance was created by adding a extremely large lateral acceleration of to a vehicle for second when the simulation reaches seconds. The velocity disturbance was created by add of velocity to a vehicle for second when the simulation reaches seconds. The results presented in Fig. 5(f) and Fig. 6(f) show that the vehicles again converge back to the path.

It can be seen in Fig 5 (c) that for regular case vehicles converge at and therefore the velocity of the vehicles are higher then desired velocity because which leads to smaller turn rate command for same applied lateral acceleration disturbance therefore the maximum deviation on path errors seems higher for the modified trajectory shaping when provided same disturbance.

Simulation videos for ten vehicles are available at https://sharmarajnikant.wordpress.com/projects-2/autonomous-vehicle-platooning/.

Keeping the above results in mind, we can conclude that the sine or modified trajectory shaping method has overall better convergence properties as compared to the regular trajectory shaping method. These results clearly show that the modified trajectory shaping is a better candidate for all platooning scenarios. Note that is not a tuning parameter in the platooning scenario but is an input and sine method is able to achieve any desired distance between vehicles. Therefore we only consider the modified or sine method for obtaining the hardware results that will be detailed in the next section.

### 4.2 Hardware Results

Due to space constraints of the testing facilities at Utah State University, 6 small robots were set to run at a desired velocity of 0.35 m/s on a 1 m radius path. Pololu’s m3pi robot was selected for these experiments. This is a differential steering, two-wheeled vehicle, so the lateral acceleration was mapped to the differential velocity of the right and left side of the robot as shown in (10) and (11) where is the velocity of the right side of the robot, is the velocity of the left side of the robot, is the commanded velocity, and is the distance between the two wheels.

(10) | ||||

(11) |

Using Robot Operating System (ROS), and Utah State University’s (USU) Robust Intelligent Sensing and Control Multi-Agent Analysis Platform (RISC MAAP) [12] the m3pi robot was able to receive position data from a motion capture system, which required each robot to have a unique template of reflective markers as seen in Fig. 7(a). This information was then used to apply the trajectory shaping tests. This system is able to give position data within around 2mm accuracy and heading data within about 2 degrees at about 200 Hz. Fig. 7(b) shows the general layout of how the RISC MAAP System works. The wireless communication was achieved using XBee Series 1 RF Modules. The ROS framework provides for easy passing of state information from the motion capture system to the main computer with ROS. The state information could then be translated into motor commands based on the control algorithms and then sent wirelessly to the m3pi robots.

Using a desired vehicle distance of , the hardware results are shown in Fig. 8. All the velocity, separation, and path errors converge close to zero and all the robots converges on the path.

As a next step we validate the robustness to disturbance by introducing a disturbance of in lateral acceleration. First the disturbance is injected at the front vehicle. The error plots for this case are shown in Fig. 9 that show that the vehicles converge to the desired path after the disturbance.

Snapshots of this experiment are shown in Figure 10 and complete experimental video can be found at https://youtu.be/GLPrj4l3o38.

In the second case disturbance is injected in the middle of the platoon in Vehicle 3. The results shown in Fig. 11 show that the path errors of Vehicle 1 and Vehicle 2 are unaffected by the disturbance. However, they do slow down to allow the others cars to catch up again. This is due to the flow of information shown in Fig. 2. The lateral acceleration of each vehicle is only a function of the vehicle in front of it, so any lateral disturbance input to the vehicle would not affect its predecessors.

Snapshots of this experiment are shown in Figure 12 and complete experimental video can be found at https://youtu.be/GLPrj4l3o38.

## 5 Conclusions

In this paper we first show that the regular trajectory shaping guidance law converges at nonzero offset from the path and is dependent on the value of . The trajectory shaping guidance law is modified such that the vehicle converges to circular and straight line paths at zero offset and is independent of the value. It is shown that the modified trajectory shaping guidance is a great candidate for multiple vehicle path following/platooning where only the lead vehicle has the knowledge of the path. The modified trajectory shaping guidance algorithm is validated for path following and platooning using simulation and experimental results.

For future work, we will investigate string stability, not investigated in this paper, to find out conditions for the string stability.

## References

- [1] S. Park, J. Deyst, and J. P. How, “A New Nonlinear Guidance Logic for Trajectory Tracking,” in Proceedings of the AIAA Guidance Navigation and Control Conference and Exhibit. Providence: AIAA, 2004, pp. 1–16. [Online]. Available: http://acl.mit.edu/papers/gnc{_}park{_}deyst{_}how.pdf
- [2] ——, “Performance and lyapunov stability of a nonlinear path following guidance method,” Journal of Guidance, Control, and Dynamics, vol. 30, no. 6, pp. 1718–1728, 2007.
- [3] E. D. B. Medagoda and P. W. Gibbens, “Synthetic-Waypoint Guidance Algorithm for Following a Desired Flight Trajectory,” Journal of Guidance, Control, and Dynamics, vol. 33, no. 2, pp. 601–606, 2010.
- [4] C.-K. Ryoo, H. Cho, and M.-J. Tahk, “Optimal Guidance Laws with Terminal Impact Angle Constraint,” Journal of Guidance, Control, and Dynamics, vol. 28, no. 4, pp. 724–732, 2005.
- [5] A. Ratnoo, S. Y. Hayoun, A. Granot, and T. Shima, “Path Following using Trajectory Shaping Guidance,” Journal of Guidance, Control, and Dynamics, vol. 38, no. 1, pp. 106–116, 2014.
- [6] R. Rajamani, H. S. Tan, B. K. Law, and W. B. Zhang, “Demonstration of integrated longitudinal and lateral control for the operation of automated vehicles in platoons,” IEEE Transactions on Control Systems Technology, vol. 8, no. 4, pp. 695–708, 2000.
- [7] H. Fritz, “Longitudinal and lateral control of heavy duty trucks for automated vehicle following in mixed traffic: experimental results from the CHAUFFEUR project,” in Proceedings of the 1999 IEEE International Conference on Control Applications (Cat. No.99CH36328), vol. 2, 1999, pp. 1348–1352. [Online]. Available: http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=801168
- [8] M. Pavone and E. Frazzoli, “Decentralized policies for geometric pattern formation and path coverage,” Journal of Dynamic Systems, Measurement, and Control, vol. 129, no. 5, pp. 633–643, 2007.
- [9] K. S. Galloway, E. W. Justh, and P. Krishnaprasad, “Geometry of cyclic pursuit,” in Decision and Control, 2009 held jointly with the 2009 28th Chinese Control Conference. CDC/CCC 2009. Proceedings of the 48th IEEE Conference on. IEEE, 2009, pp. 7485–7490.
- [10] T.-H. Kim and T. Sugie, “Cooperative control for target-capturing task based on a cyclic pursuit strategy,” Automatica, vol. 43, no. 8, pp. 1426–1431, 2007.
- [11] I. T. Erekson, “Modified Trajectory Shaping Guidance for Autonomous Path Following Control of Platooning Ground Vehicles,” Master’s thesis, Utah State University, 2016.
- [12] D. S. Maughan, “Robust Intelligent Sensing and Control Multi-Agent Analysis Platform,” Master’s thesis, Utah State University, 2016.