METHODS AND SYSTEMS FOR VEHICLE MOTION PLANNING

1 . A system comprising:

one or more processors; and

a memory including instructions, which when executed by the one or more processors, cause the one or more processors to perform a method comprising:

receiving a vehicle pose, a target goal, and an obstacle;

deriving constraints from the pose, the target goal, and the obstacle;

convexifying a problem based at least in part on the constraints;

generating a trajectory based at least in part on the convexification of the problem;

causing a vehicle to travel based at least in part on the trajectory.

2 . The system of claim 1 , wherein the method further comprises:

retrieving an initial guess based on a concatenated vector of the vehicle pose.

3 . The system of claim 2 , wherein convexifying a problem comprises:

turning nonlinear constraints into penalties in an objective function; and

introducing slack variables for the nonlinear constraints.

4 . The system of claim 3 , wherein the method further comprises:

determining whether a true objective function improves enough with regard to a result of the convexifying the problem;

if the true objective function improves enough, updating true region variables accordingly; and

if the true objective function does not improve enough, reducing a true region.

5 . The system of claim 4 , wherein the method further comprises increasing the penalties.

6 . The system of claim 1 , wherein the method further comprises generating a numerical representation of a goodness of the trajectory.

7 . The system of claim 6 , wherein the representation comprises an objective function.

8 . The system of claim 7 , wherein the objective function comprises terms relating to comfort and satisfaction of the target goal.