Method and apparatus for trajectory smoothing in autonomous vehicle control
A practical application of autonomous vehicle control couples vehicle trajectory and road curvature, taking advantage of their interdependency. As a result, if cameras or other lane-informing sensors go out in an autonomous vehicle, it still is possible to use GPS signals and the curvature to keep vehicles safely in their lanes while drivers can be alerted and can decide how to proceed (that is, by continuing autonomous vehicle control, or taking over operation of the vehicle manually).
1. In autonomous vehicle control, a trajectory smoothing method comprising: responsive to route selection, identifying a trajectory; computing a curvature and/or variation of curvature of the trajectory; responsive to curvature or variation of curvature being greater than a predetermined value, performing interpolation to represent the trajectory; applying compressive sensing to provide control signals; and outputting the control signals which control the autonomous vehicle.
2. The method of claim 1 , further comprising:
responsive to availability of updated data for the selected route, updating the route data prior to identifying the trajectory.
3. The method of claim 1 , wherein computing the curvature or variation of curvature comprises:
obtaining initial coordinate values and velocity;
selecting samples of the coordinate values at time t;
estimating a heading angle using interpolation basis functions;
estimating a correction factor;
applying the correction factor to provide corrected coordinate values; and
repeating the selecting, estimating, and applying at an incremented time.
4. The method of claim 3 , further comprising:
responsive to the correction factor being less than a predetermined value, using a small angle approximation as the correction factor before providing the corrected coordinate values.
5. The method of claim 3 , wherein the interpolation basis functions comprise basis splines, wherein the basis splines comprise a series of knots which are ordered points in a domain, wherein the knots have a higher density in areas of higher curvature or variation of curvature, and a lower density in areas of lower curvature or variation of curvature.
6. The method of claim 1 , wherein the applying the compressive sensing comprises:
responsive to computing the curvature or variation in curvature, computing a penalty function for deviation from the curvature or variation in curvature; and
computing a cumulative density function related to the penalty function.
7. The method of claim 6 , wherein the penalty function is
ρ
(
t
)
=
1
1
+
(
2
2
)
tanh
(
α
(
R
-
50
)
)
+
(
3
2
)
tanh
(
α
(
R
-
300
)
)
+
(
3
2
)
tanh
(
α
(
R
-
600
)
)
+
(
11
2
)
tanh
(
α
(
R
-
900
)
)
.
8. The method of claim 6 , wherein the cumulative density function is
F
(
t
)
=
∫
0
t
ρ
(
τ
)
d
τ
∫
01
ρ
(
τ
)
d
τ
.
9. The method of claim 1 , wherein computing the curvature or variation in curvature comprises computing a trajectory curvature selected from the group consisting of
d
θ
dt
=
-
v
(
t
)
κ
(
t
)
where
d
θ
dt
is a change of heading with time;
ν(t) is velocity as a function of time; and
κ(t) is curvature; or
d
θ
ds
=
-
κ
(
s
)
where
d
θ
ds
is a change of heading with respect to arc-length;
s is arc-length; and
κ(s) is curvature.
10. The method of claim 1 , further comprising:
producing one of GPS signals or sensing signals; and
using the one of the GPS signals or the sensing signals with the control signals to output GPS-based control signals to enable control of the autonomous vehicle.
11. An autonomous vehicle control system comprising: a processor; and a non-transitory storage medium storing program instructions which, when executed by the processor, perform a trajectory smoothing method comprising: responsive to route selection, identifying a trajectory; computing a curvature and/or variation of curvature of the trajectory; responsive to curvature or variation of curvature being greater than a predetermined value, performing interpolation to represent the trajectory; applying compressive sensing to produce control signals; and outputting the control signals which control the autonomous vehicle.
12. The system of claim 11 , wherein the method further comprises:
responsive to availability of updated data for the selected route, updating the route data prior to identifying the trajectory.
13. The system of claim 11 , wherein computing the curvature and/or variation of curvature comprises:
obtaining initial coordinate values and velocity;
selecting samples of the coordinate values at time t;
estimating a heading angle using interpolation basis functions;
estimating a correction factor;
applying the correction factor to provide corrected coordinate values; and
repeating the selecting, estimating, and applying at an incremented time.
14. The system of claim 13 , wherein the method further comprises:
responsive to the correction factor being less than a predetermined value, using a small angle approximation as the correction factor before providing the corrected coordinate values.
15. The system of claim 11 , wherein the interpolation basis functions comprise basis splines, wherein the basis splines comprise a series of knots which are ordered points in a domain, wherein the knots have a higher density in areas of higher curvature or variation of curvature, and a lower density in areas of lower curvature or variation of curvature.
16. The system of claim 11 , wherein the applying the compressive sensing comprises:
responsive to computing the curvature and/or variation of curvature, computing a penalty function for deviation from the curvature or variation of curvature; and
computing a cumulative density function related to the penalty function.
17. The system of claim 16 , wherein the penalty function is a smoothed version derived from a lookup-table, such as
ρ
(
t
)
=
1
1
+
(
2
2
)
tanh
(
α
(
R
-
50
)
)
+
(
3
2
)
tanh
(
α
(
R
-
300
)
)
+
(
3
2
)
tanh
(
α
(
R
-
600
)
)
+
(
11
2
)
tanh
(
α
(
R
-
900
)
)
.
18. The system of claim 17 , wherein the cumulative density function is
F
(
t
)
=
∫
0
t
ρ
(
τ
)
d
τ
∫
01
ρ
(
τ
)
d
τ
.
19. The system of claim 11 , wherein computing the curvature and/or variation of curvature comprises computing a trajectory curvature selected from the group consisting of
dθ/dt =−ν( t )κ( t )
where dθ/dt is a change of heading with time;
ν(t) is velocity as a function of time; and
κ(t) is curvature; or
dθ/ds =−κ( s )
where dθ/ds is a change of heading with arc-length;
s is arc-length; and
κ(s) is curvature.
20. The system of claim 11 , further comprising one of communications apparatus or sensor apparatus to produce one of GPS signals or sensing signals which the processor uses with the control signals to output GPS-based control signals to enable control of the autonomous vehicle.