From 24679e7bc6cea736e168d001ff23363b4d7be0fd Mon Sep 17 00:00:00 2001 From: Cathy Wu Date: Wed, 14 Sep 2016 16:01:58 -0700 Subject: [PATCH 1/2] ACC and midpoint controllers now also move towards the speed of the front vehicle; Midpoint controller is biased to increase speed beyond it's raw tracking to encourage the entire system to speed up; added more knobs: ratio, bias, duration, beta; added simSteps as a class attribute for access by the vehicle controllers. --- python/carfns.py | 52 ++++++++++++++++++++++++++++------------------- python/loopsim.py | 25 ++++++++++++----------- 2 files changed, 44 insertions(+), 33 deletions(-) diff --git a/python/carfns.py b/python/carfns.py index 3b1ea73..f65385a 100644 --- a/python/carfns.py +++ b/python/carfns.py @@ -47,18 +47,18 @@ def carFn((idx, car), sim, step): traci.vehicle.changeLane(car["id"], maxl, 10000) return carFn -def ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.01): +def ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.01, beta = 0.5): """ Basic adaptive cruise control (ACC) controller - :param follow_sec: + :param follow_sec: number of seconds worth of following distance to keep from the front vehicle :param max_speed: 26.8 m/s = 60 mph - :param gain: + :param gain: gain for tracking following distance + :param beta: gain for tracking speed of front vehicle :return: ACCFn to input to a carParams """ def ACCFn((idx, car), sim, step): """ - P controller :param idx: :param car: :param sim: @@ -70,7 +70,8 @@ def ACCFn((idx, car), sim, step): # TODO(cathywu) Setting tau to any value seems to cause collisions # traci.vehicle.setTau(vehID, 0.01) - if step < 250: + if step < sim.simSteps/2: + changeFasterLaneBuilder()((idx, car), sim, step) return try: @@ -89,24 +90,30 @@ def ACCFn((idx, car), sim, step): # print delta, curr_speed, front_speed, curr_speed-front_speed if follow_dist < front_dist and curr_speed < max_speed: # speed up - new_speed = min(curr_speed + gain * delta, max_speed) + new_speed = min(curr_speed + beta * (front_speed-curr_speed) + gain * delta, max_speed) traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec - print step, "FASTER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist + print "t=%d, FASTER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \ + (step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist) elif follow_dist > front_dist: # slow down - new_speed = max(curr_speed + gain * delta, 0) + new_speed = max(curr_speed + beta * (front_speed-curr_speed) + gain * delta, 0) traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec - print step, "SLOWER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist + print "t=%d, SLOWER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \ + (step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist) return ACCFn -def MidpointFnBuilder(max_speed = 26.8, gain = 0.01): +def MidpointFnBuilder(max_speed = 26.8, gain = 0.1, beta = 0.5, duration = 500, bias = 1.0, ratio = 0.5): """ Basic adaptive cruise control (ACC) controller - :param follow_sec: :param max_speed: 26.8 m/s = 60 mph - :param gain: - :return: ACCFn to input to a carParams + :param gain: gain for tracking following distance + :param beta: gain for tracking speed of front vehicle + :param duration: duration for transitioning to new speed (ms) + :param bias: additive speed bias term (m/s) + :param ratio: ratio of distance between front and back vehicles to track + as following distance (default is 0.5=midpoint) + :return: MidpointFn as input to a carParams """ def MidpointFn((idx, car), sim, step): @@ -119,7 +126,8 @@ def MidpointFn((idx, car), sim, step): """ vehID = car["id"] - if step < 250: + if step < sim.simSteps/2: + changeFasterLaneBuilder()((idx, car), sim, step) return try: @@ -133,18 +141,20 @@ def MidpointFn((idx, car), sim, step): curr_speed = car["v"] front_speed = front_car["v"] - follow_dist = (front_dist + back_dist)/2 + follow_dist = (front_dist + back_dist) * ratio delta = front_dist - follow_dist # print delta, curr_speed, front_speed, curr_speed-front_speed if follow_dist < front_dist and curr_speed < max_speed: # speed up - new_speed = min(curr_speed + gain * delta, max_speed) - traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec - print step, "FASTER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist + new_speed = min(curr_speed + beta * (front_speed-curr_speed) + gain * delta + bias, max_speed) + traci.vehicle.slowDown(vehID, new_speed, duration) # 2.5 sec + print "t=%d, FASTER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \ + (step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist) elif follow_dist > front_dist: # slow down - new_speed = max(curr_speed + gain * delta, 0) - traci.vehicle.slowDown(vehID, new_speed, 1000) # 2.5 sec - print step, "SLOWER", curr_speed, new_speed, front_speed, delta, front_dist, follow_dist + new_speed = max(curr_speed + beta * (front_speed-curr_speed) + gain * delta + bias, 0) + traci.vehicle.slowDown(vehID, new_speed, duration) # 2.5 sec + print "t=%d, SLOWER, %0.1f -> %0.1f (%0.1f) | d=%0.2f = %0.2f vs %0.2f" % \ + (step, curr_speed, new_speed, front_speed, delta, front_dist, follow_dist) return MidpointFn diff --git a/python/loopsim.py b/python/loopsim.py index 4fe4410..4aa490a 100644 --- a/python/loopsim.py +++ b/python/loopsim.py @@ -194,7 +194,7 @@ def simulate(self, opts): tag = opts.get("tag", None) paramsList = opts["paramsList"] - simSteps = opts.get("simSteps", 500) + self.simSteps = opts.get("simSteps", 500) if self.label is None: self.label = "-".join([x["name"] + "%03d" % x["count"] @@ -206,7 +206,7 @@ def simulate(self, opts): self._simInit("-" + self.label, [x["name"] for x in paramsList]) self._addTypes(paramsList) self._addCars(paramsList) - self._run(simSteps) + self._run(self.simSteps) def plot(self, show=True, save=False): # Plot results @@ -232,34 +232,35 @@ def plot(self, show=True, save=False): humanParams = { "name" : "human", - "count" : 30, + "count" : 25, "maxSpeed" : 40, - "accel" : 4, - "decel" : 6, - "function" : changeFasterLaneBuilder(), + "accel" : 2.6, + "decel" : 4.5, + # "function" : randomChangeLaneFn, + # "function" : changeFasterLaneBuilder(), "laneSpread" : 0, "speedFactor" : 1.0, "speedDev" : 0.1, "sigma" : 0.5, - "tau" : 2, + "tau" : 4, } robotParams = { "name" : "robot", - "count" : 10, + "count" : 25, "maxSpeed" : 40, "accel" : 4, "decel" : 6, - "function" : MidpointFnBuilder(max_speed = 26.8, gain = 0.1), - # "function" : ACCFnBuilder(follow_sec = 3.0, max_speed = 26.8, gain = 0.1), + # "function" : MidpointFnBuilder(max_speed=40, gain=0.1, beta=0.9, duration=250, bias=1.0, ratio=0.25), + "function" : ACCFnBuilder(follow_sec=1.0, max_speed=40, gain=0.1, beta=0.9), "laneSpread" : 0, - "tau" : 1, + "tau" : 0.5, } opts = { "paramsList" : [humanParams, robotParams], "simSteps" : 500, - "tag" : "Midpoint" + "tag" : "ACC" } defaults.SIM_STEP_LENGTH = 0.5 From 14de45df55d80d5eae8f05f7fd771d0b4e0b326f Mon Sep 17 00:00:00 2001 From: Cathy Wu Date: Wed, 14 Sep 2016 16:04:07 -0700 Subject: [PATCH 2/2] remove default behavior for ACC / Midpoint controllers --- python/carfns.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/carfns.py b/python/carfns.py index f65385a..a6d46d5 100644 --- a/python/carfns.py +++ b/python/carfns.py @@ -71,7 +71,7 @@ def ACCFn((idx, car), sim, step): # traci.vehicle.setTau(vehID, 0.01) if step < sim.simSteps/2: - changeFasterLaneBuilder()((idx, car), sim, step) + # changeFasterLaneBuilder()((idx, car), sim, step) return try: @@ -127,7 +127,7 @@ def MidpointFn((idx, car), sim, step): vehID = car["id"] if step < sim.simSteps/2: - changeFasterLaneBuilder()((idx, car), sim, step) + # changeFasterLaneBuilder()((idx, car), sim, step) return try: