Model-based road estimation
A computer-implemented method for estimating a road geometry of a road upon which a vehicle is traveling and related aspects are disclosed. The disclosed embodiments provide a model-based technique for estimating a road model having an arbitrary number of lanes on a road as well as any entrance lanes (i.e. on-ramp or slip roads) and exit lanes (i.e. off ramps or off-slip roads). In more detail, the herein disclosed embodiments utilize a Bayesian approach to multi-lane tracking in various traffic scenarios, such as e.g. highway scenarios. The employed model-based algorithm estimates the lane center curves of multi-lane roads based on vehicle motion data (e.g. speed, angular velocity) and perception data (e.g. camera output, lidar output, radar output, etc.) that includes detected road features (e.g. lane markers, road edges, road barriers, guard rails, road markers, etc.) or other objects (e.g. other road users).
1 . A computer-implemented method for estimating a road geometry of a road upon which a vehicle is traveling, the method comprising:
for each time step out of a plurality of consecutive time steps:
predicting a filter state by determining a probability density of each road model hypothesis out of a plurality of road model hypotheses comprised by the filter state based on vehicle motion data and a prior filter state determined at a preceding time step, wherein the filter state is tracked by a tracking filter and the road geometry is described by the filter state, wherein each road model hypothesis out of the plurality of road model hypotheses is associated with a probability and comprises a parametrization of the road describing a geometry of one or more lanes of the road and wherein each lane of each road model hypothesis is associated with a probability of existence;
associating road features obtained from perception data derived from an output of one or more vehicle-mounted sensors with the parametrization of the road of each road model hypothesis of the predicted filter state to establish a road feature association;
determining an existence probability for each lane of each road model hypothesis based on a prior existence probability for each corresponding lane determined at the preceding time step and the established road feature association;
determining a probability of each road model hypothesis of the plurality of road model hypotheses based on the prior filter state determined at the preceding time step and the determined existence probability of each lane of that road model hypothesis;
selecting the road model hypothesis associated with a highest probability based on the determined probability of each road model hypothesis;
outputting a road model based on the parametrization of the road of the selected road model hypothesis, wherein the road model is output to a path planning function of an automated driving system of the vehicle, the path planning function being configured to generate paths for the vehicle to execute; and
controlling, via control circuitry, steering, throttle, or brakes of the vehicle to execute the paths.
2 . The method according to claim 1 , wherein the parametrization of the road comprises a lane center parametrization for each lane of the road and a width parametrization for each lane of the road.
3 . The method according to claim 1 , wherein the parametrization of the road comprises:
an ego-lane center curvature (k 0 ) of each segment of an ego-lane of a main road,
a width (ω 01 ) of each segment of the ego-lane,
a width (ω 02 ) of each segment of one or more adjacent main road lanes of the main road, and
a pose (x, y, φ 0 ) of the vehicle relative to a starting point of the ego-lane center.
4 . The method according to claim 3 , wherein the parametrization of the road comprises:
a classification of each lane as a main road lane or a branch lane;
a branch-lane distance (a 1 ), of each branch lane, from a starting point of a connecting main road lane center to a connection point of the branch lane to the connecting main road lane center,
a branch-lane angle (φ 1 ), of each branch lane, relative to the connecting main-road lane center at the connecting point,
a branch-lane curvature (k 1 ) of each segment of the one or more branch lanes, and
a branch-lane width (ω 1 ) of each segment of the one or more branch lanes.
5 . The method according to claim 1 , further comprising:
for each time step out of the plurality of consecutive time steps:
adding one or more main road lanes and/or one or more branch lanes to each road model hypothesis of the predicted filter state;
removing any added main road lanes and branch lanes that have no road feature association.
6 . The method according to claim 5 , further comprising:
for each time step out of the plurality of consecutive time steps:
tagging the added one or more main road lanes and/or one or more branch lanes.
7 . The method according to claim 1 , wherein the road features obtained from perception data derived from an output of one or more vehicle-mounted sensors comprises lane marking detections obtained from a lane tracking algorithm configured to output coordinates of lane markers depicted in images output from a vehicle-mounted camera.
8 . The method according to claim 1 , wherein the vehicle motion data comprises a vehicle speed.
9 . The method according to claim 1 , further comprising:
for each time step out of the plurality of consecutive time steps:
forming the road model based on the parametrization of the road of the selected road model hypothesis by converting the road model parametrization to polylines describing lane centers of the lanes of the road.
10 . A non-transitory computer-readable storage medium storing instructions which, when executed by a computing device of a vehicle, causes the computing device to carry out the method according to claim 1 .
11 . An apparatus for estimating a road geometry of a road upon which a vehicle is traveling, the apparatus comprising control circuitry configured to:
for each time step out of a plurality of consecutive time steps:
predict a filter state by determining a probability of each road model hypothesis out of a plurality of road model hypotheses comprised by the filter state based on vehicle motion data and a prior filter state determined at a preceding time step, wherein the filter state is tracked by a tracking filter and the road geometry is described by the filter state, wherein each road model hypothesis out of the plurality of road model hypotheses is associated with a probability and comprises a parametrization of the road describing a geometry of one or more lanes of the road and wherein each lane of each road model hypothesis is associated with a probability of existence;
associate road features obtained from perception data derived from an output of one or more vehicle-mounted sensors with the parametrization of the road of each road model hypothesis of the predicted filter state to establish a road feature association:
determine an existence probability for each lane of each road model hypothesis based on a prior existence probability for each corresponding lane determined at the preceding time step and the established road feature association;
determine a probability of each road model hypothesis of the plurality of road model hypotheses based on the prior filter state determined at the preceding time step and the determined existence probability of each lane of that road model hypothesis;
select the road model hypothesis associated with a highest probability based on the determined probability of each road model hypothesis;
output a road model based on the parametrization of the road of the selected road model hypothesis, wherein the road model is output to a path planning function of an automated driving system of the vehicle, the path planning function being configured to generate paths for the vehicle to execute; and
control, via the control circuitry, steering, throttle, or brakes of the vehicle to execute the paths.
12 . A vehicle comprising an apparatus according to claim 11 .