System and method for collision avoidance maneuver path determination with jerk limit
View Patent ↗In a vehicle, an optimal path curvature limited by one or more constraints may be determined. The constraints may be related to lateral jerk and one or more vehicle dynamics constraints. Based on the optimal path curvature, an optimal vehicle path around an object may be determined. The optimal vehicle path may be output to a collision avoidance control system. The collision avoidance control system may cause the vehicle to take a certain path.
1. A method for performing a vehicular, collision avoidance maneuver comprising:
using a processor in the vehicle to determine a travel path from sensor data responsively to detection of a threshold distance between a vehicle and an object, the travel path having a series of path segments defined by alternating cubic and quadratic polynomial equations,
wherein each of the cubic equations has a second derivative whose slope defines a trajectory curvature rate constraint,
wherein each of the quadratic equations has a second derivative whose constant value defines a trajectory curvature limit,
wherein lateral position y and longitudinal position x are measured from a center of gravity of the vehicle such that:
a first path segment of the series of path segments is defined as:
y
1
(
x
)
=
a
1
x
3
6
,
x
∈
[
0
,
x
1
]
,
wherein
a
1
=
η
max
V
3
,
wherein η max is lateral acceleration rate constraint and V is vehicle longitudinal velocity;
a second path segment of the series of path segments is defined as:
y
2
(
x
)
=
b
2
(
x
-
x
1
)
2
2
+
c
2
(
x
-
x
1
)
+
d
2
,
x
∈
[
x
1
,
x
2
]
,
wherein d 2 =⅙a 1 x 1 3 , c 2 =½a 1 x 1 2 , and b 2 =a 1 x 1 ;
a third path segment of the series of path segments is defined as:
y
3
(
x
)
=
a
3
(
x
-
x
2
)
3
6
+
b
3
(
x
-
x
2
)
2
2
+
c
3
(
x
-
x
2
)
+
d
3
,
x
∈
[
x
2
,
x
3
]
,
wherein d 3 =½b 2 (x 2 −x 1 ) 2 +c 2 (x 2 −x 1 )+d 2 , and c 3 =b 2 (x 2 −x 1 )+c 2 , b 3 =b 2 , and a 3 =−a 1 ;
a fourth path segment of the series of path segments is defined as:
y
4
(
x
)
=
b
4
(
x
-
x
3
)
2
2
+
c
4
(
x
-
x
3
)
+
d
4
,
x
∈
[
x
3
,
x
4
]
,
wherein d 4 =⅙a 3 (x 3 −x 2 ) 3 +½b 3 (x 3 −x 2 )+c 3 (x 3 −x 2 )+d 3 , c 4 =½a 3 (x 3 −x 2 ) 2 +b 3 (x 3 −x 2 )+c 3 , and b 4 =a 3 (x 3 −x 2 )+b 3 ;
a fifth path segment of the series of path segments is defined as:
y
5
(
x
)
=
a
5
(
x
-
x
4
)
3
6
+
b
5
(
x
-
x
4
)
2
2
+
c
5
(
x
-
x
4
)
+
d
5
,
x
∈
[
x
4
,
x
5
]
,
wherein d 5 =½b 4 (x 4 −x 3 ) 2 +c 4 (x 4 −x 3 )+d 4 , c 5 =b 4 (x 4 −x 3 )+c 4 , b 5 =b 4 ,and a 5 =a 1 ; and
guiding the vehicle in accordance with the series of path segments.
2. The method of claim 1 , wherein the processor is further configured to determine a sixth path segment of the series of path segments, the sixth path segment defined as y 6 (x)=y lane (x), x>x 5 , wherein y lane =½βx 5 2 +γx 5 +δ, wherein β is lane curvature, γ is lane heading, and δ is lane offset.
3. The method of claim 1 , wherein the lateral acceleration rate constraint has a value ranging between 1 g/s to about 2 g/s.
4. The method of claim 1 , wherein the lateral acceleration constraint is implemented as 6 m/s 2 .
5. The method of claim 3 , wherein the lateral acceleration constraint is implemented as 6 m/s 2 .
6. A system for determining a travel path for a vehicular collision avoidance maneuver comprising:
a processor in a vehicle configured to determine a travel path from sensor data responsively to detection of a threshold distance between the vehicle and an object, the travel path having a series of path segments defined by alternating cubic and quadratic polynomial equations,
wherein each of the cubic equations has a second derivative whose slope defines a trajectory curvature rate constraint,
wherein each of the quadratic equations has a second derivative whose constant value defines a trajectory curvature limit,
wherein lateral position y and longitudinal position x are measured from a center of gravity of the vehicle such that:
a first path segment of the series of path segments is defined as:
y
1
(
x
)
=
a
1
x
3
6
,
x
∈
[
0
,
x
1
]
,
wherein
a
1
=
η
max
V
3
,
wherein η max is lateral acceleration rate constraint and V is vehicle longitudinal velocity;
a second path segment of the series of path segments is defined as:
y
2
(
x
)
=
b
2
(
x
-
x
1
)
2
2
+
c
2
(
x
-
x
1
)
+
d
2
,
x
∈
[
x
1
,
x
2
]
,
wherein d 2 =⅙a 1 x 3 1 , c 2 =½a 1 x 1 2 , and b 2 =a 1 x 1 ;
a third path segment of the series of path segments is defined as:
y
3
(
x
)
=
a
3
(
x
-
x
2
)
3
6
+
b
3
(
x
-
x
2
)
2
2
+
c
3
(
x
-
x
2
)
+
d
3
,
x
∈
[
x
2
,
x
3
]
,
wherein d 3 =½b 2 (x 2 −x 1 ) 2 +c 2 (x 2 −x 1 )+d 2 , and c 3 =b 2 (x 2 −x 1 )+c 2 , b 3 =b 2 , and a 3 =−a 1 ;
a fourth path segment of the series of path segments is defined as:
y
4
(
x
)
=
b
4
(
x
-
x
3
)
2
2
+
c
4
(
x
-
x
3
)
+
d
4
,
x
∈
[
x
3
,
x
4
]
,
wherein d 4 =⅙a 3 (x 3 −x 2 ) 3 +½b 3 (x 3 −x 2 )+c 3 (x 3 −x 2 )+d 3 , c 4 =½a 3 (x 3 −x 2 ) 2 +b 3 (x 3 −x 2 )+c 3 , and b 4 =a 3 (x 3 −x 2 )+b 3 ;
a fifth path segment of the series of path segments is defined as:
y
5
(
x
)
=
a
5
(
x
-
x
4
)
3
6
+
b
5
(
x
-
x
4
)
2
2
+
c
5
(
x
-
x
4
)
+
d
5
,
x
∈
[
x
4
,
x
5
]
,
wherein d 5 =½b 4 (x 4 −x 3 ) 2 +c 4 (x 4 −x 3 )+d 4 , and c 5 =b 4 (x 4 −x 3 )+c 4 , and b 5 =b 4 , and a 5 =a 1 ; and
an automated steering control system configured to actuate steering in accordance with the series of path segments.
7. The system of claim 6 , wherein the processor is further configured to determine a sixth path segment of the series of path segments defined as y 6 (x)=y lane (x), x>x 5 , wherein y lane =½βx 5 2 +γx 5 +δ, wherein β is lane curvature, γ is lane heading, and δ is lane offset.
8. The system of claim 6 , wherein the lateral acceleration rate constraint has a value ranging between 1 g/s to about 2 g/s.
9. The system of claim 6 , wherein the lateral acceleration constraint is implemented as 6 m/s 2 .
10. The system of claim 8 wherein the lateral acceleration constraint is implemented as 6 m/s 2 .