LANE EDGE FUSION SYSTEM FOR AN AUTONOMOUS VEHICLE

Abstract

A lane edge fusion system for an autonomous vehicle includes one or more controllers executing instructions to receive perception data and map data of a roadway the autonomous vehicle is traveling along. The one or more controllers derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data and select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points. The one or more controllers fit an implicit function for the evaluation point based on an implicit moving least squares approach. The one or more controllers build a fused lane edge by setting a point on the implicit curve as one of a plurality fused lane edge points.

Description

INTRODUCTION

The present disclosure relates to a lane edge fusion system for an autonomous vehicle that determines a fused lane edge based on map data and perception data.

An autonomous driving system for a vehicle is a complex system that includes many different aspects. For example, an autonomous driving system may include multiple sensors to gather perception data with respect to the vehicle's surrounding environment. In addition to the sensors, the autonomous driving system may also utilize map data as well.

Map lane edge points are derived from the map data, while perception lane edge points are derived from the perception data. The map lane edge points may be fused together with the perception lane edge points to determine lane edge points that are utilized by the autonomous driving system. However, the map lane edge points and the perception lane edge points may include noisy sample points. Moreover, the variance of the lateral error as measured from the true lane edge may vary over distance for both the noisy map lane edge points and the perception lane edge points. Sometimes the true lane edge forms a curve that goes back on itself and subsequently fails the vertical line test, which makes it difficult to model the true lane edge points as a single function. To address this issue, a parametric function may be used to fuse the map and perception data together, however, it may be challenging to find consistent parameterization with noisy data.

Thus, while autonomous driving systems achieve their intended purpose, there is a need in the art for determining an improved approach to fuse the map lane edge points and the perception lane edge points together.

SUMMARY

According to several aspects, a lane edge fusion system for an autonomous vehicle is disclosed. The lane edge fusion system includes one or more controllers executing instructions to receive perception data and map data of a roadway the autonomous vehicle is traveling along. The one or more controllers derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data. The one or more controllers select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points, where a true position of a lane edge is represented as an implicit curve. The one or more controllers fit an implicit function for the evaluation point based on an implicit moving least squares approach, where the implicit curve is represented by a zero-level set of the implicit function and the implicit function is represented by an equation for a planar circle. The one or more controllers solve for a plurality of coefficients of the implicit function, wherein the plurality of coefficients are a function of the evaluation point. The one or more controllers estimate a covariance of the plurality of coefficients. The one or more controllers determine a point on the implicit curve that is nearest to a given point based on an iterative process. The one or more controllers determine a lateral error variance at the point based on the covariance for the plurality of coefficients. Finally, the one or more controllers build a fused lane edge by setting the point on the implicit curve as one of a plurality fused lane edge points that are fused together to create the fused lane edge.

In another aspect, the implicit function is expressed as:

$f\left(x\right)={b\left(x\right)}^{T}c\left(x\right)$

where ƒ(x) represents the implicit function, b(x) represents a quadratic basis vector, and c(x) is equal to a vector of the plurality of coefficients cx.

In still another aspect, the equation of the planar circle is expressed as:

$f\left(x\right)=c_0+c_{1}x+c_{2}y+c_3\left(x^2+y^2\right)=0$

where c₀, c₁, c₂, c₃ represent the plurality of coefficients, x=x₁, and y=x₂.

In an aspect, the evaluation point includes an error expressed as:

${ϵ}^{\prime }\sim \left(0,\Sigma \right)$

where ϵ′ represents the error, represents the Normal distribution with zero mean, and Σ represents a covariance matrix.

In another aspect, the plurality of coefficients are estimated based on a Lagrangian function including a first loss function and a second loss function.

In still another aspect, the covariance of the plurality of coefficients is estimated based on an optimization problem that minimizes a Lagrangian function and is expressed as:

$c_x=\arg {\min }_{c}L\left(z,c\right)$

where c_x represents the plurality of coefficients, L(z, c) represents the Lagrangian function, and z represents noisy observations.

In an aspect, the covariance of the plurality of coefficients is expressed as:

${\Sigma }_c\approx {\left(BW{\Sigma }_z^{-1}{B}^T\right)}^{-1}BW{\Sigma }_z^{-1}W{B^T\left(BW{\Sigma }_z^{-1}{B}^T\right)}^{-1}$

where Σ_c represents the covariance of the plurality of coefficients, B represents a matrix formed by stacking basis vectors at each point so that B satisfies B=(b₁ b₂ . . . ), W is a diagonal matrix of a positive weighting function w_i, and Σ_z represents a covariance for noisy observations.

In another aspect, the lateral error variance at the point is determined based on:

${\sigma }_x^2=J{\Sigma }_c{J}^T$

where σ_x² is the lateral error variance, Σ_c is the covariance for the plurality of coefficients, and J is a Jacobian matrix with respect to a vector of the plurality of coefficients (c₀, c₁, c₂, c₃) of a function r(c) that represents a radius of a surface given the vector of the plurality of coefficients (c₀, c₁, c₂, c₃).

In still another aspect, the function r(c) is expressed as:

$r\left(c\right)=\sqrt{\frac{c_1^2+c_2^2}{2c_3^2}-c_0/c_3}.$

In an aspect, for each point that is evaluated as part of the iterative process, the plurality of coefficients are solved for based on:

$x_{\mathrm{center}}=-\frac{1}{2c_3}{\left(\begin{array}{cc}{c}_1& c_2\end{array}\right)}^T$

where x_center represents center coordinates of the planar circle and c₁, c₂ represent the plurality of coefficients.

In another aspect, for each point that is evaluated as part of the iterative process, the plurality of coefficients are solved for based on:

$r=\sqrt{x_{\mathrm{center}}^T{x}_{\mathrm{center}}-\frac{c_0}{c_3}}$

where r represents a radius of the planar circle and c₀, c₃ represent the plurality of coefficients.

In still another aspect, a next point on the planar circle nearest to a given point at iteration n is determined based on:

${\tilde{x}}_{n+1}=x_{\mathrm{center}}+\frac{{\tilde{x}}_n-x_{center}}{{\tilde{x}}_n-x_{center}}r$

where {tilde over (X)}_n+1 represents the next point that is selected for evaluation and {tilde over (x)}_n represents a given point at the iteration n.

In an aspect, a gradient constraint is enforced at the zero-level set of the implicit function.

In another aspect, the gradient constraint is expressed as a magnitude squared of a gradient of the implicit function, where the implicit function is equal to 1, and the evaluation point belongs to the zero-level set of the implicit function.

In still another aspect, the fused lane edge defines a shape of a lane located along the roadway that the autonomous vehicle travels along.

In an aspect, a lane edge fusion system for an autonomous vehicle is disclosed, and includes one or more controllers executing instructions to receive perception data and map data of a roadway the autonomous vehicle is traveling along. The one or more controllers derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data. The one or more controllers select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points, wherein a true position of a lane edge is represented as an implicit curve. The one or more controllers fit an implicit function for the evaluation point based on an implicit moving least squares approach, where the implicit curve is represented by a zero-level set of the implicit function and the implicit function is represented by an equation for a planar circle, and where a gradient constraint is enforced at the zero-level set of the implicit function. The one or more controllers solve for a plurality of coefficients of the implicit function, where the plurality of coefficients are a function of the evaluation point and are estimated based on a Lagrangian function including a first loss function and a second loss function. The one or more controllers estimate a covariance of the plurality of coefficients. The one or more controllers determine a point on the implicit curve that is nearest to a given point based on an iterative process. The one or more controllers determine a lateral error variance at the point based on the covariance for the plurality of coefficients. Finally, the one or more controllers build a fused lane edge by setting the point on the implicit curve as one of a plurality fused lane edge points that are fused together to create the fused lane edge, and the fused lane edge defines a shape of a lane located along the roadway that the autonomous vehicle travels along.

In another aspect, the lateral error variance at the point is determined based on:

${\sigma }_x^2=J{\Sigma }_c{J}^T$

where σ_x² is the lateral error variance, Σ_c is the covariance for the plurality of coefficients, and J is a Jacobian matrix with respect to a vector of the plurality of coefficients (c₀, c₁, c₂, c₃) of a function r(c) that represents a radius of a surface given the vector of the plurality of coefficients (c₀, c₁, c₂, c₃).

In yet another aspect, the function r(c) is expressed as:

$r\left(c\right)=\sqrt{\frac{c_1^2+c_2^2}{2c_3^2}-c_0/c_3}.$

In an aspect, the gradient constraint is expressed as a magnitude squared of a gradient of the implicit function, wherein the implicit function is equal to 1, and the evaluation point belongs to the zero-level set of the implicit function.

In another aspect, a lane edge fusion system for an autonomous vehicle is disclosed, and comprises one or more controllers executing instructions to receive perception data and map data of a roadway the autonomous vehicle is traveling along. The one or more controllers derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data. The one or more controllers select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points, where a true position of a lane edge is represented as an implicit curve. The one or more controllers fit an implicit function for the evaluation point based on an implicit moving least squares approach, where the implicit curve is represented by a zero-level set of the implicit function and the implicit function is represented by an equation for a planar circle, and a gradient constraint is enforced at the zero-level set of the implicit function and is expressed as a magnitude squared of a gradient of the implicit function. The implicit function is equal to 1, and the evaluation point belongs to the zero-level set of the implicit function. The one or more controllers solve for a plurality of coefficients of the implicit function, where the plurality of coefficients are a function of the evaluation point and are estimated based on a Lagrangian function including a first loss function and a second loss function. The one or more controllers estimate a covariance of the plurality of coefficients. The one or more controllers determine a point on the implicit curve that is nearest to a given point based on an iterative process. The one or more controllers determine a lateral error variance at the point based on the covariance for the plurality of coefficients wherein the lateral error variance at the point is determined based on:

${\sigma }_x^2=J{\sum }_c{J}^T$

where σ_x² is the lateral error variance, Σ_c is the covariance for the plurality of coefficients, and J is a Jacobian matrix with respect to a vector of the plurality of coefficients (c₀, c₁, c₂, c₃) of a function r(c) that represents a radius of a surface given the vector of the plurality of coefficients (c₀, c₁, c₂, c₃). The one or more controllers build a fused lane edge by setting the point on the implicit curve as one of a plurality fused lane edge points that are fused together to create the fused lane edge, and where the fused lane edge defines a shape of a lane located along the roadway that the autonomous vehicle travels along.

Further areas of applicability will become apparent from the description provided herein. It should be understood that the description and specific examples are intended for purposes of illustration only and are not intended to limit the scope of the present disclosure.

BRIEF DESCRIPTION OF THE DRAWINGS

The drawings described herein are for illustration purposes only and are not intended to limit the scope of the present disclosure in any way.

FIG. 1 is a schematic diagram of a vehicle including the disclosed lane edge fusion system, where the lane edge fusion system includes one or more controllers, according to an exemplary embodiment;

FIG. 2A illustrates a graph including a plurality of map lane edge points, according to an exemplary embodiment;

FIG. 2B illustrates a graph including a plurality of perception lane edge points, according to an exemplary embodiment;

FIG. 2C illustrates a graph including a plurality of lane edge points, according to an exemplary embodiment;

FIG. 3 is a block diagram of the one or more controllers shown in FIG. 1, according to an exemplary embodiment; and

FIG. 4 illustrates a plurality of evaluation points that are positioned with respect to an implicit curve, according to an exemplary embodiment.

DETAILED DESCRIPTION

The following description is merely exemplary in nature and is not intended to limit the present disclosure, application, or uses.

Referring to FIG. 1, an exemplary lane edge fusion system 10 for an autonomous vehicle 12 is illustrated. It is to be appreciated that the autonomous vehicle 12 may be any type of vehicle such as, but not limited to, a sedan, truck, sport utility vehicle, van, or motor home. The autonomous vehicle 12 may be a fully autonomous vehicle including an automated driving system (ADS) for performing all driving tasks or a semi-autonomous vehicle including an advanced driver assistance system (ADAS) for assisting a driver with steering, braking, and/or accelerating.

The lane edge fusion system 10 includes one or more controllers 20 in electronic communication with a plurality of sensors 22 configured to collect perception data 24 indicative of roadway the autonomous vehicle 12 is traveling along. In the non-limiting embodiment as shown in FIG. 1, the plurality of sensors 22 include one or more cameras 30, an inertial measurement unit (IMU) 32, a global positioning system (GPS) 34, radar 36, and LiDAR 38, however, is to be appreciated that additional sensors may be used as well. In addition to receiving the perception data 24 from the plurality of sensors 22, the one or more controllers 20 receives map data 26 indicative of the roadway the autonomous vehicle 12 is traveling along.

Referring to both FIGS. 1 and 2A, a plurality of map lane edge points 40 are derived from the map data 26. As seen in FIG. 2A, the plurality of map lane edge points 40 are plotted based on the global frame coordinate system. Similarly, FIG. 2B illustrates a plurality of perception lane edge points 42, which are plotted based on the ego frame coordinate system. The perception lane edge points 42 are derived from the same underlying true lane edge points as the map data 26. The one or more controllers 20 determine a plurality of fused lane edge points 44, which are illustrated in FIG. 2C, based on the plurality of map lane edge points 40 (FIG. 2A) and the plurality of perception lane edge points 42 (FIG. 2B). The fused lane edge points 44 are fused together to create a fused lane edge 46. The fused lane edge 46 is drawn as a pair of opposing lane edges 46, where a distance 48 between the fused lane edges 46 represent a confidence interval. The fused lane edge 46 is also expressed in the ego frame coordinate system and defines a shape of a lane located along the roadway that the autonomous vehicle 12 travels along. The fused lane edge 46 provides an improved estimation of an actual position of lane edges when compared to estimating the lane edges based on either the perception data 24 or the map data 26 alone.

FIG. 3 is a block diagram of the one or more controllers 20 shown in FIG. 1. The one or more controllers 20 include a transform block 50, a concatenation block 52, a selector block 54, a regression model block 56, a solution block 58, a covariance block 60, a point block 62, and a fusion block 64. The transform block 50 receives the map lane edge points 40 as input. As mentioned above, the map lane edge points 40 are expressed in the global frame coordinate system. Therefore, the transform block 50 transforms the map lane edge points 40, which are expressed in the global frame coordinate system, into the ego frame coordinate system based on a pose transform at perception time t_p. The concatenation block 52 receives the transformed map lane edge points 40 and the perception lane edge points 42 as input and determines a concatenation of the map lane edge points 40 and the perception lane edge points 42. The concatenation block 52 transmits the concatenation of the map lane edge points 40 and the perception lane edge points 42 to the selector block 54.

The selector block 54 then selects either a map lane edge point 40 or a perception lane edge point 42 from the from the concatenation of the map lane edge points 40 and the perception lane edge points 42 as an evaluation point x. The selector block 54 then transmits the evaluation point x to the regression model block 56. FIG. 4 illustrates a plurality of evaluation points x_i, x_j, where the map lane edge points 40 are represented as x_i and the perception lane edge points 42 are represented as x_j. As seen in FIG. 4, an observation error ϵ_i corresponds to the evaluation point x_i and an observation error ϵ_j corresponds to the evaluation point x_i. The observation errors ϵ_i, ϵ_j are measured perpendicular with respect to a corresponding tangent T of the true position of a lane edge, where the true position of the lane edge is drawn as an implicit curve 80. It is to be appreciated that the evaluation points x_i, x_j are heteroskedastic, which means that the variance of the observation errors ϵ_i, ϵ_j vary between the evaluation points x_i, x_j.

Referring to both FIGS. 3 and 4, the regression model block 56 fits an implicit function ƒ(x)=0 for a given evaluation point x ϵ², where the evaluation point x represents a map lane edge point 40, a perception lane edge point 42, or an arbitrary point that is proximate in distance to the implicit curve 80. Specifically, the implicit function ƒ(x) is fit based on an implicit moving least squares (MLS) approach that locally fits the map lane edge points 40 and the perception lane edge points 42 that surround a selected evaluation point x. The implicit curve 80 is represented as a zero-level set of the implicit function ƒ(x). That is, the true position of the lane edge is represented as the zero-level set of the implicit function ƒ(x). The zero-level set of the implicit function ƒ(x) is expressed in Equation 1, and the implicit function ƒ(x) is expressed in Equation 2 as:

$\begin{array}{cc}=\left\{xϵ{ℝ}^2|f\left(x\right)=0\right\}& \mathrm{Equation}1\end{array}$ $\begin{array}{cc}f\left(x\right)={b\left(x\right)}^{T}c\left(x\right)& \mathrm{Equation}2\end{array}$

where b(x) is a quadratic basis vector and c(x)=c_x is a vector of a plurality of coefficients. The transpose of c(x) is equal to c_x^T=(c₀ c₁ c₂ c₃), where c₀, c₁, c₂, c₃ represent the plurality of coefficients that are solved for based on the evaluation point x. It is to be appreciated that the plurality of coefficients c_x are a function of the evaluation point x, which is a consequence of the implicit MLS approach. Therefore, the value of the plurality of coefficients c₀, c₁, c₂, c₃ change based on the specific evaluation point x_i currently being evaluated.

The regression model block 56 selects the quadratic basis vector b(x) as Equation 3:

$\begin{array}{cc}b\left(x\right)=\left(\begin{array}{c}1\\ x_1\\ x_2\\ x_1+x_2\end{array}\right)& \mathrm{Equation}3\end{array}$

where x₁ represents the first component of vector x (e.g., the x-axis value) and x₂ represents the second component (e.g., the y-axis value). The quadratic basis vector b(x) is selected to result in the implicit function ƒ(x) that represents the equation for a planar circle, which may be written in the form of Equation 4 as:

$\begin{array}{cc}f\left(x\right)=c_0+c_{1}x+c_{2}y+c_3\left(x^2+y^2\right)=0,& \mathrm{Equation}4\end{array}$

where x=x₁ and y=x₂. It is to be appreciated that representing the implicit function by an equation for a planar circle is simple, does not require parameterization, and is relatively simple in nature to solve.

A gradient constraint is enforced at the zero-level set , which is the implicit curve 80, to avoid a trivial solution c_x=0. Specifically, the gradient constraint is expressed as a magnitude squared of a gradient of the implicit function ƒ(x) that is equal to 1 where the evaluation point x belongs to the zero-level set z of the implicit function ƒ(x), and is expressed in Equation 5 as:

$\begin{array}{cc}{\nabla f\left(x\right)}^2=1& \mathrm{Equation}5\end{array}$ $\mathrm{where}$ $xϵ,$ $\mathrm{and}$ $=\left\{x|f\left(x\right)=0\right\}$

The magnitude squared of a gradient of the implicit function ƒ(x) is equivalent to a constraint based on the plurality of coefficients c_x, which is expressed by Equation 6 as:

$\begin{array}{cc}{c}_x^T{\mathrm{Uc}}_x=1& \mathrm{Equation}6\end{array}$

where U is represented by a 4×4 matrix in Equation 7 as:

$\begin{array}{cc}U=\left(\begin{array}{cccc}0& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ -4& 0& 0& 0\end{array}\right)& \mathrm{Equation}7\end{array}$

The regression model block 56 then builds an error model, where the evaluation point x includes an error E′, and is expressed in Equation 8 as:

$\begin{array}{cc}{ϵ}^{\prime }\sim 𝒩\left(0,\sum \right)& \mathrm{Equation}8\end{array}$

where represents the Normal distribution with zero mean and Σ represents a covariance matrix. Assuming that the magnitude of the error ∥ϵ′∥ is negligible, the error ϵ′ of the evaluation point x may be expressed in Equations 9 and 10 as:

$\begin{array}{cc}f\left(x+{ϵ}^{\prime }\right)\approx f\left(x\right)+\nabla f^T{ϵ}^{\prime }& \mathrm{Equation}9\end{array}$ $\begin{array}{cc}f\left(x+{ϵ}^{\prime }\right)\approx f\left(x\right)+ϵ& \mathrm{Equation}10\end{array}$

where ϵ represents the observation error, such as the observation errors ϵ_i, ϵ_j (shown in FIG. 4) and is the linearized scalar error in the implicit function ƒ (x). If the observation error ϵ is negligible, then a scalar error ϵ may be represented by Equations 11 and 12 as:

$\begin{array}{cc}𝔼\left[f\left(x+{ϵ}^{\prime }\right)-f\left(x\right)\right]\approx 0& \mathrm{Equations}11\mathrm{and}12\end{array}$ $\mathrm{so}$ $ϵ\sim 𝒩\left(0,{\sigma }^2\right)$

where σ² is lateral error variance. The lateral error variance σ² may be expressed in Equation 13 as:

$\begin{array}{cc}{\sigma }^2=\nabla f^T\sum \nabla f& \mathrm{Equation}13\end{array}$

It is to be appreciated that when the evaluation point x belongs to the zero-level set , or x ϵ, then the gradient constraint of ∥∇ƒ (x)∥²=1 is satisfied, and the lateral error variance σ² represents the lateral error variance that is projected perpendicular to the implicit curve 80 shown in FIG. 4.

The solution block 58 then solves for the plurality of coefficients c_x of the implicit function ƒ(x). Specifically, in one embodiment, the solution block 58 estimates a value of the plurality of coefficients c_x of the implicit function ƒ(x) by solving an optimization problem that minimizes a Lagrangian function, which is expressed in Equation 14 as:

$\begin{array}{cc}=\arg {\min }_{c_x}L\left(c_x\right)& \mathrm{Equation}14\end{array}$

where L(c_x) represents the Lagrangian function. The Lagrangian function L(c_x) is expressed in Equations 15 and 16 as:

$\begin{array}{c}L\left(c_x\right)=\frac{1}{2}\sum _i{w}_i{f\left(x_i\right)}_{{\sigma }_i^2}^2+\lambda \left(1-c_x^T{\mathrm{Uc}}_x\right)\\ =\frac{1}{2}\sum _i{w}_i{{\sigma }_i^{-2}\left[{b\left(x_i\right)}^T{c}_x\right]}^2+\lambda \left(1-c_x^T{\mathrm{Uc}}_x\right)\end{array}$

where w_i=k(x, x_i) is a value of a positive weighting function that is dependent on the evaluation point x, k represents a squared-exponential function, and λ is the Lagrangian multiplier. It is to be appreciated that only a subset of observations within a predetermined distance of the evaluation point x have a non-zero weight. Consequently, certain values w_i of the positive weighting function and the quadratic basis vector b(x_i) will not require evaluation, and the optimization problem for the plurality of coefficients c_x of the implicit function ƒ(x) is solved based on Equation 17:

$\begin{array}{cc}=\frac{v_1}{\sqrt{v_1^T\mathrm{Uv}}}& \mathrm{Equation}17\end{array}$

where v₁ is an eigenvector corresponding to a smallest possible eigenvalue 11 of a 4×4 matrix, which is represented as ψ and is solved in Equation 18 as:

$\begin{array}{cc}\psi ={\left(U+U^T\right)}^{-1}{\sum }_I{w}_i{\sigma }_i^{-2}b\left(x_i\right){b\left(x_i\right)}^T& \mathrm{Equation}18\end{array}$

and ĉ_x is an estimated value of the plurality of coefficients c_x.

The above-mentioned approach for estimating the value of the plurality of coefficients c_x of the implicit function ƒ(x) by solving the optimization problem that minimizes a Lagrangian function does not account for outliers in the map lane edge points x_i and the perception lane edge points x_j (shown in FIG. 4). Therefore, in an alternative approach for estimating the value of the plurality of coefficients c_x of the implicit function ƒ(x), a first set of observed map lane edge points N₁ having the potential to contain outliers and a second set of observed map lane edge points N₂ that only contain inliers are considered, where a set of observed map lane edge points are expressed as {x_i}_i=0^N1+N2−1. A first loss function p⁽¹⁾ (e) is provided for the first set of observed map lane edge points N₁ and a second loss function p⁽²⁾(e) is provided for the second set of observed map lane edge points N₂. The Lagrangian function L(c_x) is expressed in Equations 19 and 20 as:

$\begin{array}{cc}L\left(c_x\right)=\frac{1}{2}\sum _i{w}_i{\rho }_i({f\left(x_i\right)}_{{\sum }_i}+\lambda \left(1-c_x^T{\mathrm{Uc}}_x\right)& \mathrm{Equation}20\end{array}$ $\mathrm{where}$ ${\rho }_i\left(e\right)=\{\begin{array}{cc}{\rho }^{\left(1\right)}\left(e\right)& 0\le i<N_1\\ {\rho }^{\left(2\right)}\left(e\right)& N_1\le i<N_2\end{array}$

The Lagrangian function L(c_x) including the first and second loss functions p⁽¹⁾(e), p⁽²⁾(e) is solved for based on an iterative reweighted least squares approach, where the Lagrangian function L(c_x) is converted into a standard weighted least squares problem including a variance that is determined based on the lateral error variance σ² and an iterative reweighted least squares weight s_i, which is expressed in Equation 21 as:

$\begin{array}{cc}{\sigma }_{\mathrm{LS},i}^{2,\left(n\right)}=\frac{{\sigma }^2}{s_i}& \mathrm{Equation}21\end{array}$

where σ_Ls,i^2,(π) represents the variance of the equivalent linear least squares problem at the current iteration. The iterative reweighted least squares weight s_i is determined based on the magnitude of a Mahalanobis distance ∥e_i∥ and a derivative of the loss function p′(|ē_iⁿ⁻¹|) is expressed in Equation 22. The Mahalanobis distance ∥e_i∥ is based on the lateral error variance σ_i, a transform of the quadratic basis vector b(x), and the estimated value of the plurality of coefficients c_x, and is expressed in Equation 23 as:

$\begin{array}{cc}{s}_i=\frac{{\rho }^{\prime }\left({\overline{e}}_i^{n-1}\right)}{{\overline{e}}_i^{n-1}}& \mathrm{Equation}22\end{array}$ $\begin{array}{cc}\stackrel{\_}{e_{\iota }}=\frac{❘b_i^T❘}{{\sigma }_i}& \mathrm{Equation}23\end{array}$

where n represents the current iteration number. The solution block 58 then solves for the plurality of coefficients c_x of the implicit function ƒ(x) based on the iterative reweighted least squares approach until convergence, where the magnitude of the estimated value of the plurality of coefficients c_x changes less than a predetermined threshold amount. The predetermined threshold amount is determined based on specific accuracy requirements of an autonomous driving system of the autonomous vehicle 12.

The covariance block 60 then estimates a covariance of the plurality of coefficients c_x based on an optimization problem that minimizes a Lagrangian function, which is expressed in Equation 24 as:

$\begin{array}{cc}{c}_x=\arg {\min }_{c}L\left(z,c\right)& \mathrm{Equation}24\end{array}$

where L(z, c) represents the Lagrangian function and z represents noisy observations. The covariance for the plurality of coefficients c_x is solved for based on Equation 25:

$\begin{array}{cc}{\sum }_c\approx {\left(\frac{{\partial }^{2}L}{\partial c^2}\right)}^{-1}\left(\frac{{\partial }^{2}L}{\partial z\partial c}\right){\sum }_Z{\left(\frac{{\partial }^{2}L}{\partial z\partial c}\right)}^T{\left(\frac{{\partial }^{2}L}{\partial c^2}\right)}^{-1}& \mathrm{Equation}25\end{array}$

where Σ_c is the covariance for the plurality of coefficients c_x, Σ_z is the covariance for the noisy observations z, and the partial derivatives and evaluated at the expected values of the noisy observations z. The Lagrangian function L(z, c) is determined based on the last iteration of an iterative reweighted least squares approach, and expressed in Equation 26 as:

$\begin{array}{cc}L\left(z,c\right)=\frac{1}{2}{\left(B^{T}c-z\right)}^{T}W{\sum }_z^{-1}\left(B^{T}c-z\right)+\lambda \left(1-c^T\mathrm{Uc}\right)& \mathrm{Equation}26\end{array}$

where Equation 26 has utilized a linearized error model to include additive pseudo observations z˜(0, Σ_z), W is the diagonal matrix of the positive weighting function w_i, or W=diag({w_i}), B represents a matrix formed by stacking basis vectors at each point so that B satisfies B=(b₁ b₂ . . . ), and the covariance for the noisy observations 22 is the diagonal matrix of the lateral error variance σ_i squared, or Σ_z=diag({σ_i²}).

The partial derivatives in Equation 25 are expressed in Equations 27, 28, and 29 as:

$\begin{array}{cc}\frac{\partial L}{\partial c}={\left(B^{T}c-z\right)}^T{\Phi }^T{B}^T-\lambda c^T\left(U+U^T\right)& \mathrm{Equation}27\end{array}$ $\begin{array}{cc}\frac{{\partial }^{2}L}{\partial c^2}=B\Phi B^T-\lambda \left(U+U^T\right)& \mathrm{Equation}28\end{array}$ $\begin{array}{cc}\frac{{\partial }^{2}L}{\partial z\partial c}=-B\Phi & \mathrm{Equation}29\end{array}$

where ϕ represents a constant that provides conciseness to the partial derivatives and is expressed in Equation 30 as:

$\begin{array}{cc}\Phi =\frac{1}{2}\left(W{\sum }_z^{-1}+W{\sum }_z^{-1}\right)=W{\sum }_z^{-1}& \mathrm{Equation}30\end{array}$

Accordingly, Equations 25, 27, 28, 29, and 30 are combined to create an equation that estimates the covariance Σ_c for the plurality of coefficients c_x is expressed in Equation 31 as:

$\begin{array}{cc}{\sum }_c\approx K^{-1}\mathrm{BW}{\sum }_z^{-1}{\mathrm{WB}}^T{K}^{-1}& \mathrm{Equation}31\end{array}$

where K represents a constant that provides conciseness to the covariance Σ_c and is expressed in Equation 32:

$\begin{array}{cc}K=\mathrm{BW}{\sum }_z^{-1}{B}^T-{\lambda }_1\left(U+U^T\right)& \mathrm{Equation}32\end{array}$

where λ₁ represents the Lagrangian multiplier for the smallest eigenvalue and λ₁=λ. Equations 31 and 32 are combined together to create an equation that estimates the covariance Σ_c for the plurality of coefficients c_x is expressed in Equation 33 as:

$\begin{array}{cc}{\sum }_c\approx {\left(\mathrm{BW}{\sum }_z^{-1}{B}^T\right)}^{-1}\mathrm{BW}{\sum }_z^{-1}W{B^T\left(\mathrm{BW}{\sum }_z^{-1}{B}^T\right)}^{-1}& \mathrm{Equation}33\end{array}$

Once the covariance block 60 estimates the covariance Σ_c for the plurality of coefficients c_x, the point block 62 then determines a point x on the implicit curve 80 defined by the zero-level set z of the implicit function ƒ(x) that is nearest to a given point {tilde over (x)}. The point x represents one of the fused lane edge points 44 (seen in FIG. 2C) that create the fused lane edge 46. The given point {tilde over (x)} represents a starting point of an iterative process for calculating the point x. The point x is determined by finding a local fit of the implicit curve 80 by solving for Equation 34, which is expressed as:

$\begin{array}{cc}{f}_0\left(\right)=b(T^{}& \mathrm{Equation}34\end{array}$ $\mathrm{where}$ $=x_0$

where ƒ₀() represents the implicit function ƒ(x) evaluated at an initial given point , where the initial given point is equal to an initial point x₀. It is to be appreciated that since the implicit function ƒ(x) is represented by an equation for a planar circle (Equation 4), the next given point for which the implicit function ƒ(x) evaluated at an initial given point is set to zero, or ƒ₀()=0. The point x is calculated iteratively based on an iterative process by finding the local fit of the implicit curve 80 until the point x is approximately equal to the given point {tilde over (x)}, or x≈{tilde over (x)}_m, where {tilde over (x)}_n represents a given point % at iteration n.

A lateral error variance σ_x² at the point x is determined based on the covariance Σ_c for the plurality of coefficients c_x is expressed in Equation 35 as:

$\begin{array}{cc}{\sigma }_x^2=J{\sum }_c{J}^T& \mathrm{Equation}35\end{array}$

where J is the Jacobian matrix with respect to the vector of the plurality of coefficients (c₀, c₁, c₂, c₃) of a function r(c) that represents a radius of a surface given the vector of the plurality of coefficients (c₀, c₁, c₂, c₃) expressed in Equation 36 as:

$\begin{array}{cc}r\left(c\right)=\sqrt{\frac{c_1^2+c_2^2}{2c_3^2}-c_0/c_3}& \mathrm{Equation}36\end{array}$

For each point x that is evaluated as part of the iterative process, the plurality of coefficients c₀, c₁, c₂, c₃ that are part of the implicit function ƒ(x) are solved for as a function of the evaluation point x based on a least squares approach, where the coefficients c₀, c₁, c₂, c₃ are solved based on center coordinates x_center and a radius r of the planar circle that represents the implicit function ƒ(x), and are expressed in Equations 37 and 38 as:

$\begin{array}{cc}{x}_{\mathrm{center}}=-\frac{1}{2c_3}{\left(c_1{c}_2\right)}^T& \mathrm{Equation}37\end{array}$

$\begin{array}{cc}r=\sqrt{x_{\mathrm{center}}^T{x}_{\mathrm{center}}-\frac{c_0}{c_3}}& \mathrm{Equation}38\end{array}$

where a next point {tilde over (x)}_n+1 on the planar circle representing the implicit function ƒ(x) that is nearest to {tilde over (x)}_n is determined based on Equation 39 as:

$\begin{array}{cc}{\tilde{x}}_{n+1}=x_{\mathrm{center}}+\frac{{\tilde{x}}_n-x_{\mathrm{center}}}{{\tilde{x}}_n-x_{\mathrm{center}}}r& \mathrm{Equation}39\end{array}$

where the next point {tilde over (x)}_n+1 represents the next point that is selected for evaluation by the selector block 54.

The fusion block 64 receives the lateral error variance σ_x² at the point x and the point x from the point block 62. The fusion block 64 builds the fused lane edge 46 (FIG. 2C) by setting the point x as one of the fused lane edge points 44. The fusion block 64 continues to add fused lane edge points 44 to the fused lane edge 46 until all the map lane edge points 40 and the perception lane edge points 42 have been considered. It is to be appreciated that in embodiments, a latency computation may be performed. Specifically, the fusion block 68 may determine a delta transform that compensates for latency based on a difference between the pose transform at the current time t_n and the pose transform at the perception time t_p, where the delta transform is used to propagate to the current time t_n.

Referring generally to the figures, the disclosed lane edge fusion system provides various technical effects and benefits. Specifically, the disclosed lane edge fusion system may build the fused lane edge without ordering, associating, or parameterizing the input points. Furthermore, the disclosed lane edge fusion system accounts for the heteroskedastic nature of the evaluation points, which include variance in the observation errors between the evaluation points. The true position of the lane edge is represented by an implicit function that is an equation for a planar circle, which is simple, does not require parameterization, and is relatively simple in nature to solve. Moreover, it is also to be appreciated that the disclosed lane edge fusion system may be used with two-dimensional data (top-down) as well as three-dimensional data (with elevation).

The controllers may refer to, or be part of an electronic circuit, a combinational logic circuit, a field programmable gate array (FPGA), a processor (shared, dedicated, or group) that executes code, or a combination of some or all of the above, such as in a system-on-chip. Additionally, the controllers may be microprocessor-based such as a computer having a at least one processor, memory (RAM and/or ROM), and associated input and output buses. The processor may operate under the control of an operating system that resides in memory. The operating system may manage computer resources so that computer program code embodied as one or more computer software applications, such as an application residing in memory, may have instructions executed by the processor. In an alternative embodiment, the processor may execute the application directly, in which case the operating system may be omitted.

The description of the present disclosure is merely exemplary in nature and variations that do not depart from the gist of the present disclosure are intended to be within the scope of the present disclosure. Such variations are not to be regarded as a departure from the spirit and scope of the present disclosure.

Claims

1. A lane edge fusion system for an autonomous vehicle, the lane edge fusion system comprising: one or more controllers executing instructions to: receive perception data and map data of a roadway the autonomous vehicle is traveling along;derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data;select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points, wherein a true position of a lane edge is represented as an implicit curve;fit an implicit function for the evaluation point based on an implicit moving least squares approach, wherein the implicit curve is represented by a zero-level set of the implicit function and the implicit function is represented by an equation for a planar circle;solve for a plurality of coefficients of the implicit function, wherein the plurality of coefficients are a function of the evaluation point;estimate a covariance of the plurality of coefficients;determine a point on the implicit curve that is nearest to a given point based on an iterative process;determine a lateral error variance at the point based on the covariance for the plurality of coefficients; andbuild a fused lane edge by setting the point on the implicit curve as one of a plurality fused lane edge points that are fused together to create the fused lane edge.

2. The lane edge fusion system of claim 1, wherein the implicit function is expressed as:

3. The lane edge fusion system of claim 2, wherein the equation of the planar circle is expressed as:

4. The lane edge fusion system of claim 1, wherein the evaluation point includes an error expressed as:

5. The lane edge fusion system of claim 1, wherein the plurality of coefficients are estimated based on a Lagrangian function including a first loss function and a second loss function.

6. The lane edge fusion system of claim 1, wherein the covariance of the plurality of coefficients is estimated based on an optimization problem that minimizes a Lagrangian function and is expressed as:

7. The lane edge fusion system of claim 8, wherein the covariance of the plurality of coefficients is expressed as:

8. The lane edge fusion system of claim 1, wherein the lateral error variance at the point is determined based on:

9. The lane edge fusion system of claim 8, wherein the function r(c) is expressed as:

10. The lane edge fusion system of claim 1, wherein for each point that is evaluated as part of the iterative process, the plurality of coefficients are solved for based on:

11. The lane edge fusion system of claim 10, wherein for each point that is evaluated as part of the iterative process, the plurality of coefficients are solved for based on:

12. The lane edge fusion system of claim 11, wherein a next point on the planar circle nearest to a given point at iteration n is determined based on:

13. The lane edge fusion system of claim 1, wherein a gradient constraint is enforced at the zero-level set of the implicit function.

14. The lane edge fusion system of claim 13, wherein the gradient constraint is expressed as a magnitude squared of a gradient of the implicit function, wherein the implicit function is equal to 1, and the evaluation point belongs to the zero-level set of the implicit function.

15. The lane edge fusion system of claim 1, wherein the fused lane edge defines a shape of a lane located along the roadway that the autonomous vehicle travels along.

16. A lane edge fusion system for an autonomous vehicle, the lane edge fusion system comprising: one or more controllers executing instructions to: receive perception data and map data of a roadway the autonomous vehicle is traveling along;derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data;select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points, wherein a true position of a lane edge is represented as an implicit curve;fit an implicit function for the evaluation point based on an implicit moving least squares approach, wherein the implicit curve is represented by a zero-level set of the implicit function and the implicit function is represented by an equation for a planar circle, and wherein a gradient constraint is enforced at the zero-level set of the implicit function;solve for a plurality of coefficients of the implicit function, wherein the plurality of coefficients are a function of the evaluation point and are estimated based on a Lagrangian function including a first loss function and a second loss function;estimate a covariance of the plurality of coefficients;determine a point on the implicit curve that is nearest to a given point based on an iterative process;determine a lateral error variance at the point based on the covariance for the plurality of coefficients; andbuild a fused lane edge by setting the point on the implicit curve as one of a plurality fused lane edge points that are fused together to create the fused lane edge, and wherein the fused lane edge defines a shape of a lane located along the roadway that the autonomous vehicle travels along.

17. The lane edge fusion system of claim 16, wherein the lateral error variance at the point is determined based on:

18. The lane edge fusion system of claim 17, wherein the function r(c) is expressed as:

19. The lane edge fusion system of claim 16, wherein the gradient constraint is expressed as a magnitude squared of a gradient of the implicit function, wherein the implicit function is equal to 1, and the evaluation point belongs to the zero-level set of the implicit function.

20. A lane edge fusion system for an autonomous vehicle, the lane edge fusion system comprising: one or more controllers executing instructions to: receive perception data and map data of a roadway the autonomous vehicle is traveling along;derive a plurality of map lane edge points from the map data and a plurality of perception lane edge points from the perception data;select an evaluation point based on the plurality of map lane edge points and the plurality of perception lane edge points, wherein a true position of a lane edge is represented as an implicit curve;fit an implicit function for the evaluation point based on an implicit moving least squares approach, wherein the implicit curve is represented by a zero-level set of the implicit function and the implicit function is represented by an equation for a planar circle, and wherein a gradient constraint is enforced at the zero-level set of the implicit function and is expressed as a magnitude squared of a gradient of the implicit function, wherein the implicit function is equal to 1, and the evaluation point belongs to the zero-level set of the implicit function;solve for a plurality of coefficients of the implicit function, wherein the plurality of coefficients are a function of the evaluation point and are estimated based on a Lagrangian function including a first loss function and a second loss function;estimate a covariance of the plurality of coefficients;determine a point on the implicit curve that is nearest to a given point based on an iterative process;determine a lateral error variance at the point based on the covariance for the plurality of coefficients, wherein the lateral error variance at the point is determined based on: