Apollo 11.0
自动驾驶开放平台
speed_planner.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2023 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
21#include "modules/planning/tasks/smooth_stop_trajectory_fallback/util/speed_planner.h"
22
23#include "modules/planning/planning_base/common/speed_profile_generator.h"
24
25namespace apollo {
26namespace planning {
27
28void SpeedPlanner::FastStop(double v0, double a0, double max_deceleration, SpeedData* speed_data) {
29 CHECK_NOTNULL(speed_data);
30 if (v0 <= 0.0 || fabs(max_deceleration) <= 0.0) {
31 return;
32 }
33 max_deceleration = -fabs(max_deceleration);
34 double total_time = -v0 / max_deceleration;
35 double s = 0.0;
36 double v = v0;
37 speed_data->AppendSpeedPoint(s, 0.0, v, max_deceleration, 1000.0);
38 for (double t = FLAGS_fallback_time_unit; t < total_time; t += FLAGS_fallback_time_unit) {
39 v = v0 + max_deceleration * t;
40 s = v0 * t + 0.5 * max_deceleration * t * t;
41 speed_data->AppendSpeedPoint(s, t, v, max_deceleration, 0.0);
42 }
43}
44
45void SpeedPlanner::SmoothStop(
46 double v0,
47 double a0,
48 double max_deceleration,
49 double max_pos_jerk,
50 double max_neg_jerk,
51 SpeedData* speed_data) {
52 CHECK_NOTNULL(speed_data);
53 if (v0 <= 0) {
54 return;
55 }
56 // Normalize the sign of input param.
57 max_deceleration = -fabs(max_deceleration);
58 max_pos_jerk = fabs(max_pos_jerk);
59 max_neg_jerk = -fabs(max_neg_jerk);
60 CHECK_GT(0.0, max_deceleration);
61 CHECK_GT(max_pos_jerk, 0.0);
62 CHECK_GT(0.0, max_neg_jerk);
63 double J1 = max_neg_jerk;
64 if (a0 < max_deceleration) {
65 J1 = max_pos_jerk;
66 }
67 double J3 = max_pos_jerk;
68 double T1 = (max_deceleration - a0) / J1;
69 double T3 = -max_deceleration / J3;
70 double square_deceleration = max_deceleration * max_deceleration;
71 double square_a0 = a0 * a0;
72 double dv1 = (square_deceleration - square_a0) / (J1 + J1);
73 double dv3 = -square_deceleration / (J3 + J3);
74 double dv2 = -v0 - dv1 - dv3;
75 if (dv2 < 0.0) {
76 // There is the phase when acceleration is constant.
77 double T2 = dv2 / max_deceleration;
78 double total_time = T1 + T2 + T3;
79 double square_T1 = T1 * T1;
80 double v1 = v0 + a0 * T1 + 0.5 * J1 * square_T1;
81 double s1 = v0 * T1 + 0.5 * a0 * square_T1 + J1 * square_T1 * T1 / 6.0;
82 double v2 = v1 + max_deceleration * T2;
83 double s2 = s1 + v1 * T2 + 0.5 * max_deceleration * T2 * T2;
84 // Add the speed points.
85 double cur_time = FLAGS_fallback_time_unit;
86 double cur_s = 0.0;
87 double cur_v = 0.0;
88 double cur_a = 0.0;
89 double cur_square_t = 0.0;
90 speed_data->AppendSpeedPoint(0.0, 0.0, v0, a0, J1);
91 while (cur_time < T1) {
92 cur_a = a0 + cur_time * J1;
93 cur_square_t = cur_time * cur_time;
94 cur_v = v0 + a0 * cur_time + 0.5 * J1 * cur_square_t;
95 cur_s = v0 * cur_time + 0.5 * a0 * cur_square_t + J1 * cur_square_t * cur_time / 6.0;
96 speed_data->AppendSpeedPoint(cur_s, cur_time, cur_v, cur_a, J1);
97 cur_time += FLAGS_fallback_time_unit;
98 }
99 double delta_time = 0.0;
100 double T1_T2 = T1 + T2;
101 while (cur_time < T1_T2) {
102 delta_time = cur_time - T1;
103 cur_v = v1 + max_deceleration * delta_time;
104 cur_s = s1 + v1 * delta_time + 0.5 * max_deceleration * delta_time * delta_time;
105 speed_data->AppendSpeedPoint(cur_s, cur_time, cur_v, max_deceleration, 0.0);
106 cur_time += FLAGS_fallback_time_unit;
107 }
108 while (cur_time < total_time) {
109 delta_time = cur_time - T1_T2;
110 cur_square_t = delta_time * delta_time;
111 cur_a = max_deceleration + J3 * delta_time;
112 cur_v = v2 + delta_time * max_deceleration + 0.5 * J3 * cur_square_t;
113 cur_s = s2 + v2 * delta_time + 0.5 * max_deceleration * cur_square_t + J3 * cur_square_t * delta_time / 6.0;
114 speed_data->AppendSpeedPoint(cur_s, cur_time, cur_v, cur_a, J3);
115 cur_time += FLAGS_fallback_time_unit;
116 }
117 return;
118 }
119 // No need to reach max_deceleration.
120 square_deceleration = (square_a0 - v0 * 2.0 * J1) / (1.0 - J1 / J3);
121 if (square_deceleration < 0.0) {
122 max_deceleration = 0.0;
123 } else {
124 max_deceleration = -sqrt(square_deceleration);
125 }
126 J1 = max_neg_jerk;
127 if (a0 < max_deceleration) {
128 J1 = max_pos_jerk;
129 }
130 T1 = (max_deceleration - a0) / J1;
131 T3 = -max_deceleration / J3;
132 square_deceleration = max_deceleration * max_deceleration;
133
134 double total_time = T1 + T3;
135 double square_T1 = T1 * T1;
136 double v1 = v0 + a0 * T1 + 0.5 * J1 * square_T1;
137 double s1 = v0 * T1 + 0.5 * a0 * square_T1 + J1 * square_T1 * T1 / 6.0;
138 // Add the speed points.
139 double cur_time = FLAGS_fallback_time_unit;
140 double cur_s = 0.0;
141 double cur_v = 0.0;
142 double cur_a = 0.0;
143 double cur_square_t = 0.0;
144 speed_data->AppendSpeedPoint(0.0, 0.0, v0, a0, J1);
145 while (cur_time < T1) {
146 cur_a = a0 + cur_time * J1;
147 cur_square_t = cur_time * cur_time;
148 cur_v = v0 + a0 * cur_time + 0.5 * J1 * cur_square_t;
149 cur_s = v0 * cur_time + 0.5 * a0 * cur_square_t + J1 * cur_square_t * cur_time / 6.0;
150 speed_data->AppendSpeedPoint(cur_s, cur_time, cur_v, cur_a, J1);
151 cur_time += FLAGS_fallback_time_unit;
152 }
153 double delta_time = 0.0;
154 while (cur_time < total_time) {
155 delta_time = cur_time - T1;
156 cur_square_t = delta_time * delta_time;
157 cur_a = max_deceleration + J3 * delta_time;
158 cur_v = v1 + delta_time * max_deceleration + 0.5 * J3 * cur_square_t;
159 cur_s = s1 + v1 * delta_time + 0.5 * max_deceleration * cur_square_t + J3 * cur_square_t * delta_time / 6.0;
160 speed_data->AppendSpeedPoint(cur_s, cur_time, cur_v, cur_a, J3);
161 cur_time += FLAGS_fallback_time_unit;
162 }
163}
164
165} // namespace planning
166} // namespace apollo
apollo::planning::SpeedData::AppendSpeedPoint
void AppendSpeedPoint(const double s, const double time, const double v, const double a, const double da)
Definition speed_data.cc:48
apollo::planning::SpeedPlanner::SmoothStop
static void SmoothStop(double v0, double a0, double max_deceleration, double max_pos_jerk, double max_neg_jerk, SpeedData *speed_data)
Definition speed_planner.cc:45
apollo::planning::SpeedPlanner::FastStop
static void FastStop(double v0, double a0, double max_deceleration, SpeedData *speed_data)
Definition speed_planner.cc:28
planning
Planning module main class.
apollo
class register implement
Definition arena_queue.h:37
speed_planner.h
speed_profile_generator.h