CPC B60W 60/00272 (2020.02) [B60W 60/00274 (2020.02); B60W 2554/4044 (2020.02); B60W 2754/10 (2020.02)] | 24 Claims |
1. A method of maintaining vehicle formation, the method comprising:
receiving a desired cross track offset distance and a desired along track offset distance between a lead vehicle and a follower vehicle;
receiving a current position, a current yaw rate, and a current speed of the lead vehicle;
determining a current turn radius of the lead vehicle based on the current yaw rate and the current speed of the lead vehicle, wherein a current trajectory of the lead vehicle is along an arc of a first turn circle with the current turn radius;
determining a projected turn radius of the follower vehicle based on the current turn radius of the lead vehicle, the desired cross track offset distance, and the desired along track offset distance;
determining a commanded curvature and a next speed of the follower vehicle based on a current position of the follower vehicle with respect to the current position of the lead vehicle and the projected turn radius of the follower vehicle:
causing the follower vehicle to move along an arc of a second turn circle having the projected turn radius while maintaining the desired cross track offset distance and the desired along track offset distance from the lead vehicle;
maintaining the desired cross track offset distance during the turn along the arc; and
outputting the next speed and the commanded curvature to a control system of the follower vehicle for navigation of the follower vehicle.
|