Path following in vehicle formations

1. A method of maintaining vehicle formation, the method comprising:

receiving a desired along path distance between a lead vehicle and a follower vehicle;

receiving a plurality of waypoints corresponding to a plurality of positions along a path of the lead vehicle up to a current time, each respective waypoint including a position and a waypoint speed of the lead vehicle at the respective position, wherein:

the plurality of waypoints are sequentially dropped along the path of the lead vehicle; and

a frequency at which the plurality of waypoints are dropped is proportional to a weighted sum of metrics that includes a first weight for a time elapsed, a second weight for the along path distance, and a third weight for a heading change, wherein the first weight, the second weight, and the third weight are dynamically adjustable;

determining a dynamic path for the follower vehicle by spline fitting the plurality of positions of the plurality of waypoints;

determining a commanded curvature of the follower vehicle based on the dynamic path at a current position of the follower vehicle;

determining a current along path distance between the lead vehicle and the follower vehicle;

determining an along path error based on the current along path distance and the desired along path distance;

determining a next speed of the follower vehicle based on the along path error and the respective waypoint speed of the respective waypoint that is adjacent to a current position of the follower vehicle; and

outputting the commanded curvature and the next speed to a control system of the follower vehicle to control navigation of the follower vehicle, so that the follower vehicle follows the dynamic path.

2. The method of claim 1 wherein the plurality of waypoints includes a first set of waypoints behind the follower vehicle, and a second set of waypoints ahead of the follower vehicle.

3. The method of claim 2 wherein the first set of waypoints and the second set of waypoints have substantially a same number of waypoints.

4. The method of claim 1 wherein the plurality of waypoints are sequentially dropped along the path of the lead vehicle, and wherein a frequency of waypoint dropping relates to at least one of a time elapsed between a pair of adjacent waypoints, an along path distance between the pair of adjacent waypoints, or the heading change between the pair of adjacent waypoints.

5. The method of claim 1 wherein the plurality of waypoints are sequentially dropped along the path of the lead vehicle, and wherein a frequency of waypoint dropping is inversely proportional to a weighted sum of a time elapsed between a pair of adjacent waypoints, an along path distance between the pair of adjacent waypoints, and the heading change between the pair of adjacent waypoints.

6. The method of claim 1 wherein determining the next speed of the follower vehicle is further based on a current speed of the lead vehicle.

7. The method of claim 1 wherein determining the next speed of the follower vehicle is further based on whether a current radial distance between the lead vehicle and the follower vehicle is less than a minimum radial distance.

8. The method of claim 1 further comprising:

in response to determining that the current along path distance between the lead vehicle and the follower vehicle is greater than a maximum along path distance, sending a command to the control system of the follower vehicle to disengage path following.

9. The method of claim 1 further comprising:

upon determining that a difference between an initial heading of the follower vehicle differs and an initial heading of the lead vehicle is greater than a threshold, sending a command to the control system of the follower vehicle to not to engage path following.

10. The method of claim 1 further comprising:

upon determining that an initial position of the follower vehicle is outside a section of a circle behind the lead vehicle having a predetermined central angle, sending a command to the control system of the follower vehicle to not to engage path following.

11. The method of claim 1 further comprising:

upon determining that the follower vehicle is unable to follow the dynamic path, sending a command to the control system of the follower vehicle to disengage path following.

12. A method of maintaining vehicle formation, the method comprising:

receiving a desired along path distance between a lead vehicle and a follower vehicle;

receiving a plurality of waypoints corresponding to a plurality of positions along a path of the lead vehicle up to a current time, each waypoint including a respective position of the lead vehicle, wherein:

the plurality of waypoints are sequentially dropped along the path of the lead vehicle; and

a frequency at which the plurality of waypoints are dropped is proportional to a weighted sum of metrics that includes a first weight for a time elapsed, a second weight for the along path distance, and a third weight for a heading change, wherein the first weight, the second weight, and the third weight are dynamically adjustable;

determining a dynamic path for the follower vehicle by spline fitting the plurality of positions of the plurality of waypoints;

determining a commanded curvature of the follower vehicle based on a curvature of the dynamic path at a current position of the follower vehicle;

determining a current along path distance between the lead vehicle and the follower vehicle;

determining an along path error based on the current along path distance and the desired along path distance;

determining a next speed of the follower vehicle based on the along path error; and

outputting the commanded curvature and the next speed to a control system of the follower vehicle to control navigation of the follower vehicle, so that the follower vehicle follows the dynamic path.

13. The method of claim 12 wherein the plurality of waypoints includes a first set of waypoints behind the follower vehicle, and a second set of waypoints ahead of the follower vehicle.

14. The method of claim 13 wherein the first set of waypoints and the second set of waypoints have substantially a same number of waypoints.

15. The method of claim 12 wherein the plurality of waypoints are sequentially dropped along the path of the lead vehicle, and wherein a frequency of waypoint dropping relates to at least one of a time elapsed between a pair of adjacent waypoints, an along path distance between the pair of adjacent waypoints, or the heading change between the pair of adjacent waypoints.

16. The method of claim 12 wherein each respective waypoint further includes a respective waypoint speed of the lead vehicle at the respective position of the respective waypoint, and determining the next speed of the follower vehicle is further based on the respective waypoint speed of the respective waypoint that is adjacent to a current position of the follower vehicle.

17. The method of claim 16 wherein determining the next speed of the follower vehicle is further based on a current speed of the lead vehicle.

18. The method of claim 12 further comprising:

upon determining that the current along path distance between the lead vehicle and the follower vehicle is greater than a maximum along path distance, sending a command to the control system of the follower vehicle to disengage path following.

19. The method of claim 12 further comprising:

upon determining that a difference between an initial heading of the follower vehicle differs and an initial heading of the lead vehicle is greater than a threshold, sending a command to the control system of the follower vehicle to not to engage path following.

20. The method of claim 12 further comprising:

upon determining that an initial position of the follower vehicle is outside a section of a circle behind the lead vehicle having a predetermined central angle, sending a command to the control system of the follower vehicle to not to engage path following.

21. The method of claim 12 further comprising:

upon determining that the follower vehicle is unable to follow the dynamic path, sending a command to the control system of the follower vehicle to disengage path following.