First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: accelerate EM trajectories from rest

Browse Source
This commit is contained in:
梁薄云
2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs
+8 -2
View File
@@ -167,9 +167,12 @@ public sealed class FullDirectionSegmentScheduleBuilder
LongitudinalConfiguration configuration)
{
double adjustedTime = 0d;
double requestedPreviousTime = times[0];
for (int index = 1; index < times.Count; index++)
{
double requestedDuration = times[index] - times[index - 1];
double requestedTime = times[index];
double requestedDuration = requestedTime - requestedPreviousTime;
requestedPreviousTime = requestedTime;
double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
? configuration.MaximumAccelerationMetersPerSecondSquared
@@ -191,7 +194,10 @@ public sealed class FullDirectionSegmentScheduleBuilder
{
double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
if (previousSlope * nextSlope < 0d)
bool changesDirection = previousSlope * nextSlope < 0d;
bool entersCruise = previousSlope > Tolerance && nextSlope <= Tolerance;
bool leavesCruise = previousSlope >= -Tolerance && nextSlope < -Tolerance;
if (changesDirection || entersCruise || leavesCruise)
stations.Add(index);
}
stations.Add(pathS.Count - 1);