From aeabbf4fb2555d58d767529fc06390f137c97700 Mon Sep 17 00:00:00 2001
From: Markus Quaritsch <markus.quaritsch@tugraz.at>
Date: Tue, 4 Aug 2015 14:19:43 +0200
Subject: [PATCH] include method to calculate deceleration distance for
lookahead (breaking)
---
.../Data/AccelerationCurve.cs | 88 ++++++++++++++++---
.../AccelerationCurveTest.cs | 39 ++++++++
2 files changed, 117 insertions(+), 10 deletions(-)
diff --git a/VectoCore/Models/SimulationComponent/Data/AccelerationCurve.cs b/VectoCore/Models/SimulationComponent/Data/AccelerationCurve.cs
index 43e1c4d478..24024dc4f7 100644
--- a/VectoCore/Models/SimulationComponent/Data/AccelerationCurve.cs
+++ b/VectoCore/Models/SimulationComponent/Data/AccelerationCurve.cs
@@ -1,4 +1,5 @@
-using System.Collections.Generic;
+using System;
+using System.Collections.Generic;
using System.Data;
using System.IO;
using System.Linq;
@@ -47,6 +48,20 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Data
}
public AccelerationEntry Lookup(MeterPerSecond key)
+ {
+ var index = FindIndex(key);
+
+ return new AccelerationEntry {
+ Acceleration =
+ VectoMath.Interpolate(_entries[index - 1].Key, _entries[index].Key, _entries[index - 1].Value.Acceleration,
+ _entries[index].Value.Acceleration, key),
+ Deceleration =
+ VectoMath.Interpolate(_entries[index - 1].Key, _entries[index].Key, _entries[index - 1].Value.Deceleration,
+ _entries[index].Value.Deceleration, key)
+ };
+ }
+
+ protected int FindIndex(MeterPerSecond key)
{
var index = 1;
if (key < _entries[0].Key) {
@@ -58,15 +73,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Data
index = (key > _entries[0].Key) ? _entries.Count - 1 : 1;
}
}
-
- return new AccelerationEntry {
- Acceleration =
- VectoMath.Interpolate(_entries[index - 1].Key, _entries[index].Key, _entries[index - 1].Value.Acceleration,
- _entries[index].Value.Acceleration, key),
- Deceleration =
- VectoMath.Interpolate(_entries[index - 1].Key, _entries[index].Key, _entries[index - 1].Value.Deceleration,
- _entries[index].Value.Deceleration, key)
- };
+ return index;
}
public class AccelerationEntry
@@ -74,5 +81,66 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Data
public MeterPerSquareSecond Acceleration { get; set; }
public MeterPerSquareSecond Deceleration { get; set; }
}
+
+ /// <summary>
+ ///
+ /// </summary>
+ /// <param name="v1">current speed of the vehicle</param>
+ /// <param name="v2">desired speed of the vehicle at the end of acceleration/deceleration phase</param>
+ /// <returns>distance required to accelerate/decelerate the vehicle from v1 to v2 according to the acceleration curve</returns>
+ public Meter ComputeAccelerationDistance(MeterPerSecond v1, MeterPerSecond v2)
+ {
+ var index1 = FindIndex(v1);
+ var index2 = FindIndex(v2);
+
+ var distance = 0.SI<Meter>();
+ for (var i = index2; i <= index1; i++) {
+ distance += ComputeAccelerationSegmentDistance(i, v1, v2);
+ }
+ return distance;
+ }
+
+ /// <summary>
+ ///
+ /// </summary>
+ /// <param name="i">segment of the acceleration curve to use [(i-1) ... i]</param>
+ /// <param name="v1">current speed of the vehicle</param>
+ /// <param name="v2">desired speed of the vehicle at the end of acceleration/deceleration phase</param>
+ /// <returns>distance required to accelerate/decelerate the vehicle from v1 to v2 according to the acceleration curve</returns>
+ private Meter ComputeAccelerationSegmentDistance(int i, MeterPerSecond v1, MeterPerSecond v2)
+ {
+ var leftEntry = _entries[i - 1]; // entry with lower velocity
+ var rightEntry = _entries[i]; // entry with higher velocity
+
+ v2 = VectoMath.Max(v2, leftEntry.Key); // min. velocity within current segment
+ v1 = VectoMath.Min(v1, rightEntry.Key); // max. velocity within current segment
+
+ if (leftEntry.Value.Deceleration.IsEqual(rightEntry.Value.Deceleration)) {
+ // v(t) = a * t + v1 => t = (v2 - v1) / a
+ // s(t) = a/2 * t^2 + v1 * t + s0 {s0 == 0} => s(t)
+ var acceleration = v2 > v1 ? leftEntry.Value.Acceleration : leftEntry.Value.Deceleration;
+ return ((v2 - v1) * (v2 - v1) / 2.0 / acceleration + v1 * (v2 - v1) / acceleration).Cast<Meter>();
+ }
+
+ // a(v) = k * v + d
+ // dv/dt = a(v) = d * v + d ==> v(t) = sgn(k * v1 + d) * exp(-k * c) / k * exp(t * k) - d / k
+ // v(0) = v1 => c = - ln(|v1 * k + d|) / k
+ // v(t) = (v1 + d / k) * exp(t * k) - d / k => t = 1 / k * ln((v2 * k + d) / (v1 * k + d))
+ // s(t) = m / k* exp(t * k) + b * t + c' {m = v1 + d / k, b = -d / k}
+
+ var k = (leftEntry.Value.Deceleration - rightEntry.Value.Deceleration) / (leftEntry.Key - rightEntry.Key);
+ var d = leftEntry.Value.Deceleration - k * leftEntry.Key;
+ if (v2 > v1) {
+ k = (leftEntry.Value.Acceleration - rightEntry.Value.Acceleration) / (leftEntry.Key - rightEntry.Key);
+ d = leftEntry.Value.Acceleration - k * leftEntry.Key;
+ }
+ var m = v1 + d / k;
+ var b = -d / k;
+ var c = 0.SI<Meter>() - m / k;
+ var t = (Math.Log((v2 * k + d).Value() / (v1 * k + d).Value()) / k.Value()).SI<Second>();
+ // TODO @@@quam: remove .SI<> when bug #VECTO-111 is fixed...
+
+ return m / k * Math.Exp((k * t).Value()) + b * t + c;
+ }
}
}
\ No newline at end of file
diff --git a/VectoCoreTest/Models/SimulationComponentData/AccelerationCurveTest.cs b/VectoCoreTest/Models/SimulationComponentData/AccelerationCurveTest.cs
index 8b4a72ee2c..508f6bbf0d 100644
--- a/VectoCoreTest/Models/SimulationComponentData/AccelerationCurveTest.cs
+++ b/VectoCoreTest/Models/SimulationComponentData/AccelerationCurveTest.cs
@@ -79,5 +79,44 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponentData
// EXTRAPOLATE
EqualAcceleration(130, 0.16, -0.103);
}
+
+ [TestMethod]
+ public void ComputeAccelerationDistanceTest()
+ {
+ Data = AccelerationCurveData.ReadFromFile(@"TestData\Components\Truck.vacc");
+
+ // in this part the deceleration is constant
+
+ var result = Data.ComputeAccelerationDistance(25.KMPHtoMeterPerSecond(), 0.KMPHtoMeterPerSecond());
+ Assert.AreEqual(24.11265432099, result.Value(), Tolerance);
+
+ result = Data.ComputeAccelerationDistance(25.KMPHtoMeterPerSecond(), 15.KMPHtoMeterPerSecond());
+ Assert.AreEqual(15.43209876543, result.Value(), Tolerance);
+
+ result = Data.ComputeAccelerationDistance(50.KMPHtoMeterPerSecond(), 0.KMPHtoMeterPerSecond());
+ Assert.AreEqual(96.45061728395, result.Value(), Tolerance);
+
+ result = Data.ComputeAccelerationDistance(50.KMPHtoMeterPerSecond(), 15.KMPHtoMeterPerSecond());
+ Assert.AreEqual(87.77006172840, result.Value(), Tolerance);
+
+ result = Data.ComputeAccelerationDistance(100.KMPHtoMeterPerSecond(), 60.KMPHtoMeterPerSecond());
+ Assert.AreEqual(493.82716049383, result.Value(), Tolerance);
+
+ // decelerate in the non-constant part only
+
+ result = Data.ComputeAccelerationDistance(60.KMPHtoMeterPerSecond(), 50.KMPHtoMeterPerSecond());
+ Assert.AreEqual(59.44491148, result.Value(), Tolerance);
+
+ result = Data.ComputeAccelerationDistance(59.KMPHtoMeterPerSecond(), 55.KMPHtoMeterPerSecond());
+ Assert.AreEqual(27.33155090, result.Value(), Tolerance);
+
+ // decelerate across multiple areas of acceleration curve
+
+ result = Data.ComputeAccelerationDistance(60.KMPHtoMeterPerSecond(), 0.KMPHtoMeterPerSecond());
+ Assert.AreEqual(59.44491148 + 96.45061728395, result.Value(), Tolerance);
+
+ result = Data.ComputeAccelerationDistance(100.KMPHtoMeterPerSecond(), 0.KMPHtoMeterPerSecond());
+ Assert.AreEqual(59.44491148 + 96.45061728395 + 493.82716049383, result.Value(), Tolerance);
+ }
}
}
\ No newline at end of file
--
GitLab