Speed planning method and apparatus, electronic device and storage medium
View Patent ↗Provided are a speed planning method and apparatus, an electronic device, and a storage medium. The method includes determining a planned path on which a target object reaches a target position from a current position; in the planned path, performing cubic B-spline curve fitting on a path between the current position and a position with a set distance from the current position to obtain a fitted curve; calculating a radius of curvature of the fitted curve, and calculating a corresponding angular speed based on the radius of curvature and a preset forward speed; based on current obstacle information in a map where the target object is located, judging whether a risk of collision with an obstacle during a process of the target object traveling along the fitted curve at the angular speed exists.
1. A speed planning method, comprising:
determining a planned path on which a target object reaches a target position from a current position;
in the planned path, performing cubic B-spline curve fitting on a path between the current position and a position with a set distance from the current position to obtain a fitted curve;
calculating a radius of curvature of the fitted curve, and calculating a corresponding angular speed based on the radius of curvature and a preset forward speed;
based on current obstacle information in a map where the target object is located, judging whether a risk of collision with an obstacle exists during a process of the target object traveling along the fitted curve at the angular speed; and
based on a judgment result that no risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the angular speed, determining the angular speed as a traveling speed of the target object;
wherein calculating the radius of curvature of the fitted curve comprises:
determining an initial point of the fitted curve and a slope extreme point of the fitted curve respectively; and
based on a slope of the fitted curve at the initial point, a slope of the fitted curve at the slope extreme point, and a distance between the initial point and the slope extreme point, calculating the radius of curvature of the fitted curve;
wherein performing the cubic B-spline curve fitting on the path between the current position and the position with the set distance from the current position in the planned path to obtain the fitted curve comprises:
determining sampling points for performing the cubic B-spline curve fitting based on the planned path;
determining reference points for performing the cubic B-spline curve fitting from the sampling points; and
obtaining the fitted curve according to the reference points and a set fitted curve equation; and
wherein determining the sampling points for performing the cubic B-spline curve fitting based on the planned path comprises:
traversing coordinate points in the planned path, and determining coordinate points whose distance from the current position is smaller than the set distance to be the sampling points, wherein the coordinate points in the planned path are arranged from small to large according to the distance from the current position.
2. The method of claim 1 , further comprising:
based on the judgment result that the risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the angular speed, reducing the preset forward speed, and recalculating a corresponding angular speed based on the radius of curvature and the reduced forward speed;
based on the current obstacle information in the map where the target object is located, judging whether a risk of collision with the obstacle exists during a process of the target object traveling along the fitted curve at the recalculated angular speed; and
based on a judgment result that the risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the recalculated angular speed , continuously reducing the preset forward speed until an angular speed without the risk of collision with the obstacle is obtained.
3. The method of claim 1 , wherein determining the initial point and the slope extreme point of the fitted curve respectively comprises:
selecting a coordinate point closest to the reference point C 0 from coordinate points with a first derivative being zero and a second derivative of the set fitted curve equation f(x) being zero, and marking the coordinate point as a slope extreme point D, wherein the reference point C 0 is the initial point of the fitted curve.
4. The method of claim 1 , wherein based on the current obstacle information in the map where the target object is located, judging whether the risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the angular speed comprises:
determining a path through which the target object passes at the angular speed for a set time;
based on the current obstacle information in the map where the target object is located, judging whether an obstacle exists in the path; and
based on a judgment result that no obstacle exists in the path, determining that no risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the angular speed.
5. An electronic device, comprising a memory, a processor and a computer program stored in the memory and executable by the processor, wherein when executing the computer program, the processor implements the speed planning method of claim 1 .
6. A non-transitory storage medium comprising computer-executable instructions that, when executed by a computer processor, implement the speed planning method of claim 1 .
7. The method of claim 2 , wherein calculating the radius of curvature of the fitted curve comprises:
determining an initial point of the fitted curve and a slope extreme point of the fitted curve respectively; and
based on a slope of the fitted curve at the initial point, a slope of the fitted curve at the slope extreme point, and a distance between the initial point and the slope extreme point, calculating the radius of curvature of the fitted curve.
8. The method of claim 2 , wherein based on the current obstacle information in the map where the target object is located, judging whether the risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the angular speed comprises:
determining a path through which the target object passes at the angular speed for a set time;
based on the current obstacle information in the map where the target object is located, judging whether an obstacle exists in the path; and
based on a judgment result that no obstacle exists in the path, determining that no risk of collision with the obstacle exists during the process of the target object traveling along the fitted curve at the angular speed.
9. An electronic device, comprising a memory, a processor and a computer program stored in the memory and executable by the processor, wherein when executing the computer program, the processor implements the speed planning method of claim 2 .
10. An electronic device, comprising a memory, a processor and a computer program stored in the memory and executable by the processor, wherein when executing the computer program, the processor implements the speed planning method of claim 3 .
11. An electronic device, comprising a memory, a processor and a computer program stored in the memory and executable by the processor, wherein when executing the computer program, the processor implements the speed planning method of claim 4 .
12. A non-transitory storage medium comprising computer-executable instructions that, when executed by a computer processor, implement the speed planning method of claim 2 .
13. The method of claim 1 , wherein the reference points for performing the cubic B-spline curve fitting are determined from the sampling points as follows:
C
0
=
B
0
,
C
1
=
B
floor
(
n
-
1
3
)
,
C
2
=
B
ceil
(
n
-
1
3
*
2
)
,
and
C
3
=
B
n
-
1
,
wherein, the sampling points are expressed as a sampling point set B={B 0 , B 1 , B 2 . . . B n-1 }, C 0 , C 1 , C 2 , and C 3 denote reference points, floor denotes rounding up, and ceil denotes rounding down;
and the set fitted curve equation is as follows:
f
(
x
)
=
1
6
(
y
0
x
0
(
1
-
x
)
3
+
y
1
x
1
(
3
x
3
-
6
x
2
+
4
)
+
y
2
x
2
(
-
3
x
3
+
3
x
2
+
3
x
+
1
)
+
y
3
x
3
x
3
)
,
and
wherein, (x 0 , y 0 ) are coordinates of the reference point C 0 , (x 1 , y 1 ) are coordinates of the reference point C 1 , (x 2 , y 2 ) are coordinates of the reference point C 2 , and (x 3 , y 3 ) are coordinates of the reference point C 3 .
14. The method of claim 3 , wherein based on the slope of the fitted curve at the initial point, the slope of the fitted curve at the slope extreme point, and the distance between the initial point and the slope extreme point, calculating the radius of curvature of the fitted curve comprises:
calculating a slope of the first derivative of the set fitted curve equation f(x) at the reference point C 0 , and marking the slope of the first derivative of the set fitted curve equation f(x) at the reference point C 0 as k 0 ;
calculating a slope of the first derivative of the set fitted curve equation f(x) at the slope extreme point D, and marking the slope of the first derivative of the set fitted curve equation f(x) at the slope extreme point D as k l ;
calculating a distance between the reference point C 0 and the slope extreme point D, and marking the distance between the reference point C 0 and the slope extreme point D as l; and
calculating a radius R of curvature of the fitted curve according to a formula as follows:
R
=
l
2
*
sin
(
k
1
-
k
0
2
)
.