Apollo 11.0
自动驾驶开放平台
静态 Public 成员函数 | 所有成员列表
apollo::planning::SpeedPlanner类 参考

#include <speed_planner.h>

apollo::planning::SpeedPlanner 的协作图:

静态 Public 成员函数

static void FastStop (double v0, double a0, double max_deceleration, SpeedData *speed_data)
 
static void SmoothStop (double v0, double a0, double max_deceleration, double max_pos_jerk, double max_neg_jerk, SpeedData *speed_data)
 

详细描述

在文件 speed_planner.h 第 28 行定义.

成员函数说明

◆ FastStop()

void apollo::planning::SpeedPlanner::FastStop ( double  v0,
double  a0,
double  max_deceleration,
SpeedData *  speed_data 
)
static

在文件 speed_planner.cc 第 28 行定义.

28 {
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}

◆ SmoothStop()

void apollo::planning::SpeedPlanner::SmoothStop ( double  v0,
double  a0,
double  max_deceleration,
double  max_pos_jerk,
double  max_neg_jerk,
SpeedData *  speed_data 
)
static

在文件 speed_planner.cc 第 45 行定义.

51 {
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}
该类的文档由以下文件生成: