Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSCFModel_ACC.cpp
15 : /// @author Kallirroi Porfyri
16 : /// @date Feb 2018
17 : ///
18 : // ACC car-following model based on [1], [2].
19 : // [1] Milanes, V., and S. E. Shladover. Handling Cut-In Vehicles in Strings
20 : // of Cooperative Adaptive Cruise Control Vehicles. Journal of Intelligent
21 : // Transportation Systems, Vol. 20, No. 2, 2015, pp. 178-191.
22 : // [2] Xiao, L., M. Wang and B. van Arem. Realistic Car-Following Models for
23 : // Microscopic Simulation of Adaptive and Cooperative Adaptive Cruise
24 : // Control Vehicles. Transportation Research Record: Journal of the
25 : // Transportation Research Board, No. 2623, 2017. (DOI: 10.3141/2623-01).
26 : //[3] Xiao, L., Wang, M., Schakel, W., & van Arem, B. (2018). Unravelling
27 : // effects of cooperative adaptive cruise control deactivation on
28 : // traffic flow characteristics at merging bottlenecks. Transportation
29 : // Research Part C: Emerging Technologies, 96, 380–397.
30 : // <https://doi.org/10.1016/j.trc.2018.10.008>
31 : /****************************************************************************/
32 : #include <config.h>
33 :
34 : #include <stdio.h>
35 : #include <iostream>
36 :
37 : #include "MSCFModel_ACC.h"
38 : #include <microsim/MSVehicle.h>
39 : #include <microsim/MSLane.h>
40 : #include <utils/common/RandHelper.h>
41 : #include <utils/common/SUMOTime.h>
42 : #include <utils/xml/SUMOSAXAttributes.h>
43 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
44 : #include <math.h>
45 : #include <microsim/MSNet.h>
46 :
47 : // ===========================================================================
48 : // debug flags
49 : // ===========================================================================
50 : //#define DEBUG_ACC
51 : //#define DEBUG_COND (true)
52 : //#define DEBUG_COND (veh->isSelected())
53 :
54 :
55 : // ===========================================================================
56 : // defaults
57 : // ===========================================================================
58 : #define DEFAULT_SC_GAIN -0.4
59 : #define DEFAULT_GCC_GAIN_SPEED 0.8
60 : #define DEFAULT_GCC_GAIN_SPACE 0.04
61 : #define DEFAULT_GC_GAIN_SPEED 0.07
62 : #define DEFAULT_GC_GAIN_SPACE 0.23
63 : #define DEFAULT_CA_GAIN_SPACE 0.8
64 : #define DEFAULT_CA_GAIN_SPEED 0.23
65 :
66 : // ===========================================================================
67 : // thresholds
68 : // ===========================================================================
69 : #define GAP_THRESHOLD_SPEEDCTRL 120
70 : #define GAP_THRESHOLD_GAPCTRL 100
71 : // override followSpeed when deemed unsafe by the given margin (the value was selected to reduce the number of necessary interventions)
72 : #define DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD 2.0
73 :
74 : /// @todo: add attributes for myCollisionAvoidanceGainSpeed and myCollisionAvoidanceGainSpace
75 :
76 : // ===========================================================================
77 : // method definitions
78 : // ===========================================================================
79 818 : MSCFModel_ACC::MSCFModel_ACC(const MSVehicleType* vtype) :
80 : MSCFModel(vtype),
81 818 : mySpeedControlGain(vtype->getParameter().getCFParam(SUMO_ATTR_SC_GAIN, DEFAULT_SC_GAIN)),
82 818 : myGapClosingControlGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_SPEED, DEFAULT_GCC_GAIN_SPEED)),
83 818 : myGapClosingControlGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_GCC_GAIN_SPACE, DEFAULT_GCC_GAIN_SPACE)),
84 818 : myGapControlGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_SPEED, DEFAULT_GC_GAIN_SPEED)),
85 818 : myGapControlGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_GC_GAIN_SPACE, DEFAULT_GC_GAIN_SPACE)),
86 818 : myCollisionAvoidanceGainSpeed(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_SPEED, DEFAULT_CA_GAIN_SPEED)),
87 818 : myCollisionAvoidanceGainSpace(vtype->getParameter().getCFParam(SUMO_ATTR_CA_GAIN_SPACE, DEFAULT_CA_GAIN_SPACE)),
88 818 : myApplyDriverstate(vtype->getParameter().getCFParam(SUMO_ATTR_APPLYDRIVERSTATE, 0)),
89 1636 : myEmergencyThreshold(vtype->getParameter().getCFParam(SUMO_ATTR_CA_OVERRIDE, DEFAULT_EMERGENCY_OVERRIDE_THRESHOLD)) {
90 : // ACC does not drive very precise and often violates minGap
91 818 : myCollisionMinGapFactor = vtype->getParameter().getCFParam(SUMO_ATTR_COLLISION_MINGAP_FACTOR, 0.1);
92 818 : }
93 :
94 1185 : MSCFModel_ACC::~MSCFModel_ACC() {}
95 :
96 :
97 : void
98 10 : MSCFModel_ACC::ACCVehicleVariables::saveState(OutputDevice& out, const MSCFModel& /*cfm*/) const {
99 10 : out.openTag(SUMO_TAG_CFM_VARIABLES);
100 10 : out.writeAttr(SUMO_ATTR_ID, "ACC");
101 10 : std::ostringstream internals;
102 10 : internals << ACC_ControlMode << " ";
103 10 : internals << lastUpdateTime;
104 10 : out.writeAttr(SUMO_ATTR_STATE, internals.str());
105 10 : out.closeTag();
106 10 : }
107 :
108 :
109 : void
110 10 : MSCFModel_ACC::ACCVehicleVariables::loadState(const SUMOSAXAttributes& attrs) {
111 10 : bool ok = true;
112 10 : const std::string cfmID = attrs.get<std::string>(SUMO_ATTR_ID, nullptr, ok);
113 10 : if (cfmID != "ACC") {
114 0 : throw ProcessError(TLF("incompatible carFollowModel '%' when loading state for ACC", cfmID));
115 : }
116 10 : std::istringstream bis(attrs.getString(SUMO_ATTR_STATE));
117 10 : bis >> ACC_ControlMode;
118 10 : bis >> lastUpdateTime;
119 20 : }
120 :
121 : double
122 31868372 : MSCFModel_ACC::followSpeed(const MSVehicle* const veh, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const pred, const CalcReason /*usage*/) const {
123 31868372 : if (myApplyDriverstate) {
124 52060 : applyHeadwayAndSpeedDifferencePerceptionErrors(veh, speed, gap2pred, predSpeed, predMaxDecel, pred);
125 : }
126 31868372 : const double desSpeed = MIN2(veh->getLane()->getSpeedLimit(), veh->getMaxSpeed());
127 31868372 : const double vACC = _v(veh, gap2pred, speed, predSpeed, desSpeed, true);
128 31868372 : const double vSafe = maximumSafeFollowSpeed(gap2pred, speed, predSpeed, predMaxDecel);
129 31868372 : if (vSafe + myEmergencyThreshold < vACC) {
130 : //ACCVehicleVariables* vars = (ACCVehicleVariables*)veh->getCarFollowVariables();
131 : //std::cout << SIMTIME << " veh=" << veh->getID() << " v=" << speed << " vL=" << predSpeed << " gap=" << gap2pred << " vACC=" << vACC << " vSafe=" << vSafe << " cm=" << vars->ACC_ControlMode << "\n";
132 352638 : return vSafe + myEmergencyThreshold;
133 : }
134 : return vACC;
135 : }
136 :
137 :
138 : double
139 11596357 : MSCFModel_ACC::stopSpeed(const MSVehicle* const veh, const double speed, double gap, double decel, const CalcReason /*usage*/) const {
140 11596357 : if (myApplyDriverstate) {
141 2728 : applyHeadwayPerceptionError(veh, speed, gap);
142 : }
143 : // NOTE: This allows return of smaller values than minNextSpeed().
144 : // Only relevant for the ballistic update: We give the argument headway=TS, to assure that
145 : // the stopping position is approached with a uniform deceleration also for tau!=TS.
146 11596357 : return MIN2(maximumSafeStopSpeed(gap, decel, speed, false, veh->getActionStepLengthSecs()), maxNextSpeed(speed, veh));
147 : }
148 :
149 :
150 : double
151 27519730 : MSCFModel_ACC::getSecureGap(const MSVehicle* const /*veh*/, const MSVehicle* const /*pred*/, const double speed, const double leaderSpeed, const double /* leaderMaxDecel */) const {
152 : // Accel in gap mode should vanish:
153 : // 0 = myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * (g - myHeadwayTime * speed);
154 : // <=> myGapControlGainSpace * g = - myGapControlGainSpeed * (leaderSpeed - speed) + myGapControlGainSpace * myHeadwayTime * speed;
155 : // <=> g = - myGapControlGainSpeed * (leaderSpeed - speed) / myGapControlGainSpace + myHeadwayTime * speed;
156 27519730 : return myGapControlGainSpeed * (speed - leaderSpeed) / myGapControlGainSpace + myHeadwayTime * speed;
157 : }
158 :
159 :
160 : double
161 123364 : MSCFModel_ACC::insertionFollowSpeed(const MSVehicle* const v, double speed, double gap2pred, double predSpeed, double predMaxDecel, const MSVehicle* const /*pred*/) const {
162 : //#ifdef DEBUG_ACC
163 : // std::cout << "MSCFModel_ACC::insertionFollowSpeed(), speed="<<speed<< std::endl;
164 : //#endif
165 : // iterate to find a stationary value for
166 : // speed = followSpeed(v, speed, gap2pred, predSpeed, predMaxDecel, nullptr, CalcReason::FUTURE)
167 : const int max_iter = 50;
168 : int n_iter = 0;
169 : const double tol = 0.1;
170 : const double damping = 0.1;
171 :
172 : double res = speed;
173 3844287 : while (n_iter < max_iter) {
174 : // proposed acceleration
175 3809103 : const double a = SPEED2ACCEL(followSpeed(v, res, gap2pred, predSpeed, predMaxDecel, nullptr, CalcReason::FUTURE) - res);
176 3809103 : res = res + damping * a;
177 : //#ifdef DEBUG_ACC
178 : // std::cout << " n_iter=" << n_iter << ", a=" << a << ", res=" << res << std::endl;
179 : //#endif
180 3809103 : if (fabs(a) < tol) {
181 : break;
182 : } else {
183 3720923 : n_iter++;
184 : }
185 : }
186 123364 : return res;
187 : }
188 :
189 :
190 : /// @todo update interactionGap logic
191 : double
192 0 : MSCFModel_ACC::interactionGap(const MSVehicle* const /*veh */, double /* vL */) const {
193 : /*maximum radar range is ACC is enabled*/
194 0 : return 250;
195 : }
196 :
197 14027197 : double MSCFModel_ACC::accelSpeedControl(double vErr) const {
198 : // Speed control law
199 14027197 : return mySpeedControlGain * vErr;
200 : }
201 :
202 : double
203 17893646 : MSCFModel_ACC::accelGapControl(const MSVehicle* const /* veh */, const double gap2pred, const double speed, const double predSpeed, double vErr) const {
204 : // Gap control law
205 : double gclAccel = 0.0;
206 17893646 : const double deltaVel = predSpeed - speed;
207 :
208 : // see dynamic gap margin definition from (Xiao et. al, 2018)[3], equation 5 reformulated as min/max to avoid discontinuities
209 17893646 : const double d0 = MAX2(0., MIN2(75. / speed - 5., 2.));
210 : // this is equation 4, gap2pred is the difference in vehicle positions minus the length
211 17893646 : const double spacingErr = gap2pred - myHeadwayTime * speed - d0;
212 :
213 :
214 17893646 : if (fabs(spacingErr) < 0.2 && fabs(vErr) < 0.1) {
215 : // gap mode
216 19429 : gclAccel = myGapControlGainSpeed * deltaVel + myGapControlGainSpace * spacingErr;
217 : #ifdef DEBUG_ACC
218 : if (DEBUG_COND) {
219 : std::cout << " applying gap control: spacingErr=" << spacingErr << " speedErr=" << vErr << std::endl;
220 : }
221 : #endif
222 17874217 : } else if (spacingErr < 0) {
223 : // collision avoidance mode
224 3623431 : gclAccel = myCollisionAvoidanceGainSpeed * deltaVel + myCollisionAvoidanceGainSpace * spacingErr;
225 : #ifdef DEBUG_ACC
226 : if (DEBUG_COND) {
227 : std::cout << " applying collision avoidance: spacingErr=" << spacingErr << " speedErr=" << vErr << std::endl;
228 : }
229 : #endif
230 : } else {
231 : // gap closing mode
232 14250786 : gclAccel = myGapClosingControlGainSpeed * deltaVel + myGapClosingControlGainSpace * spacingErr;
233 : #ifdef DEBUG_ACC
234 : if (DEBUG_COND) {
235 : std::cout << " applying gap closing: spacingErr=" << spacingErr << " speedErr=" << vErr << std::endl;
236 : }
237 : #endif
238 : }
239 17893646 : return gclAccel;
240 : }
241 :
242 :
243 : double
244 31920843 : MSCFModel_ACC::_v(const MSVehicle* const veh, const double gap2pred, const double speed,
245 : const double predSpeed, const double desSpeed, const bool /* respectMinGap */) const {
246 :
247 : double accelACC = 0;
248 : double gapLimit_SC = GAP_THRESHOLD_SPEEDCTRL; // lower gap limit in meters to enable speed control law
249 : double gapLimit_GC = GAP_THRESHOLD_GAPCTRL; // upper gap limit in meters to enable gap control law
250 :
251 : #ifdef DEBUG_ACC
252 : if (DEBUG_COND) {
253 : std::cout << SIMTIME << " MSCFModel_ACC::_v() for veh '" << veh->getID() << "'\n"
254 : << " gap=" << gap2pred << " speed=" << speed << " predSpeed=" << predSpeed
255 : << " desSpeed=" << desSpeed << " tau=" << myHeadwayTime << std::endl;
256 : }
257 : #endif
258 :
259 :
260 : /* Velocity error */
261 31920843 : double vErr = speed - desSpeed;
262 : int setControlMode = 0;
263 : ACCVehicleVariables* vars = (ACCVehicleVariables*) veh->getCarFollowVariables();
264 31920843 : if (vars->lastUpdateTime != MSNet::getInstance()->getCurrentTimeStep()) {
265 4270715 : vars->lastUpdateTime = MSNet::getInstance()->getCurrentTimeStep();
266 : setControlMode = 1;
267 : }
268 31920843 : if (gap2pred > gapLimit_SC) {
269 :
270 : #ifdef DEBUG_ACC
271 : if (DEBUG_COND) {
272 : std::cout << " applying speedControl" << std::endl;
273 : }
274 : #endif
275 : // Find acceleration - Speed control law
276 14026812 : accelACC = accelSpeedControl(vErr);
277 : // Set cl to vehicle parameters
278 14026812 : if (setControlMode) {
279 22648 : vars->ACC_ControlMode = 0;
280 : }
281 17894031 : } else if (gap2pred < gapLimit_GC) {
282 : // Find acceleration - Gap control law
283 17456636 : accelACC = accelGapControl(veh, gap2pred, speed, predSpeed, vErr);
284 : // Set cl to vehicle parameters
285 17456636 : if (setControlMode) {
286 4247442 : vars->ACC_ControlMode = 1;
287 : }
288 : } else {
289 : // Follow previous applied law
290 437395 : int cm = vars->ACC_ControlMode;
291 437395 : if (!cm) {
292 :
293 : #ifdef DEBUG_ACC
294 : if (DEBUG_COND) {
295 : std::cout << " applying speedControl" << std::endl;
296 : }
297 : #endif
298 385 : accelACC = accelSpeedControl(vErr);
299 : } else {
300 437010 : accelACC = accelGapControl(veh, gap2pred, speed, predSpeed, vErr);
301 : }
302 :
303 : }
304 :
305 31920843 : double newSpeed = speed + ACCEL2SPEED(accelACC);
306 :
307 : #ifdef DEBUG_ACC
308 : if (DEBUG_COND) {
309 : std::cout << " result: accel=" << accelACC << " newSpeed=" << newSpeed << std::endl;
310 : }
311 : #endif
312 :
313 31920843 : return MAX2(0., newSpeed);
314 : }
315 :
316 :
317 : MSCFModel*
318 0 : MSCFModel_ACC::duplicate(const MSVehicleType* vtype) const {
319 0 : return new MSCFModel_ACC(vtype);
320 : }
|
