Systems and methods for navigating a vehicle by dynamic map creation based on lane segmentation

1 . A method for navigating an autonomous vehicle, comprising:

receiving sensor data from at least one sensor of the autonomous vehicle, the sensor data representative of a driving surface in a field of view of the autonomous vehicle; and

using a processor:

segmenting a portion of the driving surface in the field of view of the autonomous vehicle by determining a nominal path based at least in part on the sensor data;

assigning a plurality of nodes to at least a portion of the nominal path;

initializing the plurality of nodes with a constant covariance in the lateral and longitudinal direction aligned with an ego pose of the autonomous vehicle, wherein the constant covariance represents an uncertainty estimate for the plurality of nodes that is used in a probabilistic optimization framework to update node positions;

associating the plurality of the nodes assigned to the nominal path with a line to generate at least one segmentation polyline;

extending the at least one segmentation polyline by incrementally associating nodes near to the autonomous vehicle to nodes further from the autonomous vehicle through angle optimization;

determining an updated nominal path by fitting the at least one segmentation polyline to the nominal path, wherein fitting comprises solving an update problem by optimizing lateral displacement of the plurality of nodes to minimize curvature change and maximize match with the nominal path;

generating a trajectory based on the updated nominal path; and

navigating the autonomous vehicle according to the trajectory.

2 . The method of claim 1 , wherein the step of segmenting comprises dilating and skeletonizing the nominal path.

3 . The method of claim 1 , wherein the step of segmenting comprises applying a gaussian blur to the nominal path to generate blurred segmentation.

4 . The method of claim 1 , wherein the step of segmenting comprises segmenting the driving surface with a lane detection segmentation model.

5 . The method of claim 1 , wherein fitting the at least one segmentation polyline comprises minimizing curvature change between two adjacent sets of nodes on the same segmentation line.

6 . The method of claim 5 , wherein a curvature of at least one of the two adjacent sets of nodes is defined by a Menger Curvature function.

7 . The method of claim 5 , wherein at least one of the two adjacent sets of nodes comprises three nodes.

8 . The method of claim 1 , wherein fitting the at least one segmentation polyline comprises maximizing match between the at least one segmentation polyline and the nominal path.

9 . The method of claim 1 , comprising, after the step of segmenting, applying imaginary horizontal lines across the segmented portions of the driving surface to find intercepts of the imaginary horizontal lines and the segmented portions of the driving surface, and identifying the intercepts as a plurality of nodes.

10 . A system for navigating an autonomous vehicle, comprising:

at least one sensor, configured to receive sensor data representative of a driving surface in a field of view of the autonomous vehicle; and

a processor, configured to:

segment a portion of the driving surface in the field of view of the autonomous vehicle by determining a nominal path based at least in part on the sensor data;

assign a plurality of nodes to at least a portion of the nominal path;

initialize the plurality of nodes with a constant covariance in the lateral and longitudinal direction aligned with an ego pose of the autonomous vehicle, wherein the constant covariance represents an uncertainty estimate for the plurality of nodes that is used in a probabilistic optimization framework to update node positions;

associate the plurality of the nodes assigned to the nominal path with a line to generate at least one segmentation polyline;

extend the at least one segmentation polyline by incrementally associating nodes near to the autonomous vehicle to nodes further from the autonomous vehicle through angle optimization;

determine an updated nominal path by fitting the at least one segmentation polyline to the nominal path, wherein fitting comprises solving an update problem by optimizing lateral displacement of the plurality of nodes to minimize curvature change and maximize match with the nominal path;

generate a trajectory based on the updated nominal path; and

navigate the autonomous vehicle according to the trajectory.

11 . The system of claim 10 , wherein the processor is configured to dilate and skeletonize the nominal path.

12 . The system of claim 10 , wherein the processor is configured to apply a gaussian blur to the nominal path to generate blurred segmentation.

13 . The system of claim 10 , wherein the processor is configured to segment the driving surface with a lane detection segmentation model.

14 . The system of claim 10 , wherein the processor is configured to minimize curvature change between two adjacent sets of nodes on the same segmentation line.

15 . The system of claim 14 wherein a curvature of at least one of the two adjacent sets of nodes is defined by a Menger Curvature function.

16 . The system of claim 14 , wherein at least one of the two adjacent sets of nodes comprises three nodes.

17 . The system of claim 10 , wherein the processor is configured to maximize match between the at least one segmentation polyline and the nominal path.

18 . The system of claim 10 , wherein the processor is configured to, after the step of segmenting, apply imaginary horizontal lines across the segmented portions of the driving surface to find intercepts of the imaginary horizontal lines and the segmented portions of the driving surface, and identify the intercepts as a plurality of nodes.