AUTONOMOUS LANE MERGING SYSTEM AND METHOD

1 . A computer-implemented method of performing an autonomous lane merging of a vehicle, the method comprising:

identifying one or more objects in a plurality of lanes in the vicinity of the vehicle;

identifying one or more gaps among the one or more objects;

determining a mode of the vehicle;

determining a terminal state of a planned trajectory of the vehicle based on the identified objects, gaps, and the mode of the vehicle;

performing a sanity check based on the terminal state of the planned trajectory;

generating the planned trajectory of the vehicle if the sanity check passes;

providing the planned trajectory to a controller of the vehicle to autonomously move the vehicle according to the planned trajectory.

2 . The computer-implemented method of claim 1 , further comprising validating the planned trajectory before providing the planned trajectory to the controller.

3 . The computer-implemented method of claim 2 , wherein validating the planned trajectory comprises performing speed validation, acceleration validation, curvature validation, and collision check validation.

4 . The computer-implemented method of claim 1 , wherein identifying the one or more objects comprises calculating each object's position in the vehicle's body frame.

5 . The computer-implemented method of claim 1 , wherein identifying the one or more gaps comprises:

sorting the one or more objects into different groups based on a lane ID associated with each object, and

sorting the different groups based on longitudinal distance of each different group.

6 . The computer-implemented method of claim 5 , wherein identifying the one or more gaps further comprises:

predicting each object's longitudinal and lateral states for a given time; and

calculating the gaps in each lane; and

forming different groups of gaps.

7 . The computer-implemented method of claim 1 , wherein the mode of the vehicle comprises one of a velocity-keeping mode, a car-following mode, and a merging mode.

8 . The computer-implemented method of claim 7 , wherein determining the mode of the vehicle comprises:

determining whether a merge signal is initiated by a user of the vehicle;

if the merge signal is initiated, entering the merging mode;

if the merge signal is not initiated, determining if there is an object observed in front of the vehicle and if a longitudinal velocity of the observed object is less than a target speed;

if there is an object observed in front of the vehicle and the longitudinal velocity of the observed object is less than the target speed, entering the car-following mode;

if either there is no observed object or the longitudinal velocity of the observed object is no less than the target speed, entering the velocity-keeping mode.

9 . The computer-implemented method of claim 7 , wherein performing a sanity check based on the terminal state of the planned trajectory comprises:

determining that the vehicle is in the velocity-keeping mode; and

determining whether a constant maximum acceleration of the vehicle cannot reach the target speed within a maximum sampling time.

10 . The computer-implemented method of claim 7 , wherein performing a sanity check based on the terminal state of the planned trajectory comprises:

determining that the vehicle is in a car-following mode; and

calculating a distance from a current position of the vehicle to a target position and a difference in a current velocity of the vehicle and a target velocity.

11 . The computer-implemented method of claim 7 , wherein generating the planned trajectory of the vehicle comprises:

determining that the vehicle is in the vehicle-following mode; and

using quartic polynomial to generate the planned trajectory based on a given initial state and a target state.

12 . The computer-implemented method of claim 7 , wherein generating the planned trajectory of the vehicle comprises:

determining that the vehicle is in the car-following mode; and

using quintic polynomial to generate the planned trajectory based on a given initial state and a target state.

13 . A vehicle comprising:

one or more sensors configured to detect objects in the vicinity of the vehicle;

an object processor configured to identify one or more objects in a plurality of lanes in the vicinity of the vehicle and further configured to identify one or more gaps among the one or more objects;

a mode manager configured to determine a mode of the vehicle;

a behavior planner configured to determine a terminal state of a planned trajectory of the vehicle based on the identified objects, gaps, and the mode of the vehicle;

a sanity check layer configured to perform a sanity check based on the terminal state of the planned trajectory;

a trajectory generation module configured to generate the planned trajectory of the vehicle if the sanity check passes; and

a controller of the vehicle configured to autonomously move the vehicle according to the planned trajectory.

14 . The vehicle of claim 13 , further comprising:

an anchor point generation module configure to set up a plurality of anchor points associated with each lane for sampling and set a terminal lateral velocity and acceleration of the vehicle to zero.

15 . The vehicle of claim 14 , further comprising:

a vehicle state estimator configured to estimate a state of the vehicle, the state comprising at least one of a velocity, speed, direction, acceleration rate, and decelerate rate of the vehicle.

16 . The vehicle of claim 14 , further comprising:

a trajectory evaluation module configured to validate the planned trajectory before providing the planned trajectory to the controller.

17 . The vehicle of claim 14 , wherein the controller is configured to control the operation of one or more of brakes, steering output, and speed of the vehicle via controlling an actuation system.

18 . The vehicle of claim 14 , wherein the one or more sensors comprise at least one of a camera, a radar, and a LIDAR.

19 . The vehicle of claim 14 , wherein the mode of the vehicle comprises one of a velocity-keeping mode, a car-following mode, and a merging mode.

20 . A highway Autonomous Driver Assistance System (ADAS)

one or more sensors configured to detect objects in the vicinity of the vehicle;

a processor; and

a non-transitory storage configured to store instructions, which when executed by the processor, cause the processor to perform a method comprising:

identifying one or more objects in a plurality of lanes in the vicinity of the vehicle;

identifying one or more gaps among the one or more objects;

determining a mode of the vehicle;

determining a terminal state of a planned trajectory of the vehicle based on the identified objects, gaps, and the mode of the vehicle;

performing a sanity check based on the terminal state of the planned trajectory;

generating the planned trajectory of the vehicle if the sanity check passes;

providing the planned trajectory to a controller of the vehicle to autonomously move the vehicle according to the planned trajectory.