joylink/rt-sim-training-service
6
0
Fork 0
Code Issues 3 Pull Requests Actions Packages Projects Releases 10 Wiki Activity

速度曲线限速逻辑重写（未完）

Browse Source
This commit is contained in:
joylink_zhangsai 2021-08-23 18:23:47 +08:00
parent e4df304b10
commit 3f05a36687
6 changed files with 97 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/java/club/joylink/rtss/simulation/cbtc/ATS/service/AtsTrainService.java
View File

@ -1,6 +1,5 @@
package club.joylink.rtss.simulation.cbtc.ATS.service;
import club.joylink.rtss.entity.PlanRouting;
import club.joylink.rtss.exception.BusinessExceptionAssertEnum;
import club.joylink.rtss.simulation.cbtc.ATS.service.stage.AtsHeadTrainStageService;
import club.joylink.rtss.simulation.cbtc.ATS.service.stage.AtsPlanTrainStageService;
@ -30,7 +29,6 @@ import org.springframework.util.CollectionUtils;
import org.springframework.util.StringUtils;
import java.time.LocalTime;
import java.time.temporal.ChronoUnit;
import java.util.*;
import java.util.stream.Collectors;
@ -783,7 +781,7 @@ public class AtsTrainService {
boolean targetIsRight = targetTrain.isRight();
targetPosition = CalculateService.calculateNextPositionByStartAndLen(targetTrainTailPosition, !targetIsRight, 2);
}
SectionPosition physicalPosition = CalculateService.calculatePositionOfPhysical(targetPosition);
SectionPosition physicalPosition = targetPosition.convert2PhysicalSectionPosition();
train.setRobotTargetPosition(physicalPosition);
}

18
src/main/java/club/joylink/rtss/simulation/cbtc/data/CalculateService.java
View File

@ -1238,22 +1238,4 @@ public class CalculateService {
}
return acceleratedDistance + varyingAcceleratedDistance;
}
/**
* 计算该位置对应的物理区段位置
*/
public static SectionPosition calculatePositionOfPhysical(SectionPosition sectionPosition) {
Section section = sectionPosition.getSection();
if (section.isPhysical()) {
return sectionPosition;
} else {
Section parent = section.getParent();
if (parent != null) {
return new SectionPosition(parent, section.getMinOffset() + sectionPosition.getOffset());
} else {
return null;
}
}
}
}

2
src/main/java/club/joylink/rtss/simulation/cbtc/data/map/Section.java
View File

@ -976,6 +976,8 @@ public class Section extends MayOutOfOrderDevice {
return this.getSpeedUpLimit() / 3.6f;
} else if (this.isSwitchTrack() && this.getRelSwitch().isReversePosition()) {
return 30 / 3.6f;
} else if (this.getParent() != null) {
return this.getParent().getNeedLimitSpeed();
}
return null;
}

11
src/main/java/club/joylink/rtss/simulation/cbtc/data/map/Switch.java
View File

@ -258,17 +258,6 @@ public class Switch extends MayOutOfOrderDevice {
// }
}
/**
* 设置道岔区段上限速度
*
* @param upLimitSpeed
*/
public void setSpeedUpperLimit(int upLimitSpeed) {
this.a.setSpeedUpLimit(upLimitSpeed);
this.b.setSpeedUpLimit(upLimitSpeed);
this.c.setSpeedUpLimit(upLimitSpeed);
}
public void routeLock(Route route) {
this.routeLock = true;
this.route = route;

14
src/main/java/club/joylink/rtss/simulation/cbtc/data/support/SectionPosition.java
View File

@ -100,6 +100,20 @@ public class SectionPosition {
}
}
public SectionPosition convert2PhysicalSectionPosition() {
Section section = this.getSection();
if (section.isPhysical()) {
return this;
} else {
Section parent = section.getParent();
if (parent != null) {
return new SectionPosition(parent, section.getMinOffset() + this.getOffset());
} else {
return null;
}
}
}
/**
* 该位置位于对应区段的停车范围内
*/

162
src/main/java/club/joylink/rtss/simulation/cbtc/onboard/ATO/SpeedCurve.java
View File

@ -45,7 +45,6 @@ public class SpeedCurve {
}
/**
*
* @param simulation
* @param train
* @param ma
@ -132,6 +131,7 @@ public class SpeedCurve {
public static final Map<Float, SpeedCalculator> LimitSpeedCalMap = new HashMap<>();
public static final float Release_Limit_Speed = 5; // 释放功能限制速度，单位m/s
public static SpeedCalculator getLimitSpeedCalculator(float limitSpeed) {
SpeedCalculator speedCalculator = LimitSpeedCalMap.get(limitSpeed);
if (speedCalculator == null) {
@ -296,54 +296,23 @@ public class SpeedCurve {
if (totalLen <= 0) {
return SpeedCurve.ZERO;
}
Section tailLogic = tailPosition.getLogicSection();
Section base = tailPosition.getSection();
Section headSection = headPosition.getSection();
float limitSpeed = recommendedSpeedMax;
boolean limit = false;
if (Objects.nonNull(tailPosition.getSection().getNeedLimitSpeed())) {
limitSpeed = Math.min(limitSpeed, tailPosition.getSection().getNeedLimitSpeed() * 0.9f);
limit = true;
} else if (Objects.nonNull(headPosition.getSection().getNeedLimitSpeed())) {
limitSpeed = Math.min(limitSpeed, headPosition.getSection().getNeedLimitSpeed() * 0.9f);
limit = true;
}
SectionPosition endPosition = null;
boolean ahead = false;
float vt = 0;
int count = 0;
if (!limit) {
// 无限速，找前方限速
while (Objects.nonNull(base) && count < 20) {
++count;
Float limitSpeed1 = base.getNeedLimitSpeed();
if (limitSpeed1 == null && !CollectionUtils.isEmpty(base.getLogicList())) { //兼容逻辑区段设置限速
float distanceBetweenLogicAndPhysical = 0; //逻辑区段距物理区段端点的距离（同向端点）
for (Section section : base.getLogicList()) {
limitSpeed1 = section.getNeedLimitSpeed();
if (limitSpeed1 != null) {
float offsetOfLogicSectionOnPhysicalSection = distanceBetweenLogicAndPhysical + (right ? section.getLen() : 0); //逻辑区段的端点在物理区段上的偏移量
SectionPosition logicSectionPosition = new SectionPosition(base, offsetOfLogicSectionOnPhysicalSection);
if (logicSectionPosition.isAheadOf(tailPosition, right)) {
break;
}
}
distanceBetweenLogicAndPhysical += section.getLen();
}
for (int i = 0; i < 20; i++) {
List<Section> logicSections = base.getLogicList();
if (logicSections == null) {
logicSections = Collections.singletonList(base);
}
for (Section logic : logicSections) {
Float needLimitSpeed = logic.getNeedLimitSpeed();
if (needLimitSpeed != null) {
endPosition = new SectionPosition(base, logic.getEndOffsetByDirection(right));
}
if (Objects.nonNull(limitSpeed1)) {
if (ahead) {
// 车头前方
vt = Math.min(limitSpeed, limitSpeed1 * 0.9f);
endPosition = new SectionPosition(base, right ? 0 : base.getLen());
break;
} else {
limitSpeed = Math.min(limitSpeed, limitSpeed1 * 0.9f);
}
}
if (Objects.equals(base, headSection)) {
ahead = true;
}
base = base.getNextRunningSectionOf(right);
}
}
SpeedCurve normalSpeedCurve = buildTargetSpeedCurve(totalLen, v0, 0, limitSpeed);
@ -359,6 +328,75 @@ public class SpeedCurve {
return normalSpeedCurve;
}
// public static SpeedCurve buildTargetSpeedCurve(SectionPosition headPosition,
// SectionPosition tailPosition, boolean right,
// float totalLen, float v0, float recommendedSpeedMax) {
// if (totalLen <= 0) {
// return SpeedCurve.ZERO;
// }
// Section base = tailPosition.getSection();
// Section headSection = headPosition.getSection();
// float limitSpeed = recommendedSpeedMax;
// boolean limit = false;
// if (Objects.nonNull(tailPosition.getSection().getNeedLimitSpeed())) {
// limitSpeed = Math.min(limitSpeed, tailPosition.getSection().getNeedLimitSpeed() * 0.9f);
// limit = true;
// } else if (Objects.nonNull(headPosition.getSection().getNeedLimitSpeed())) {
// limitSpeed = Math.min(limitSpeed, headPosition.getSection().getNeedLimitSpeed() * 0.9f);
// limit = true;
// }
// SectionPosition endPosition = null;
// boolean ahead = false;
// float vt = 0;
// int count = 0;
// if (!limit) {
// // 无限速，找前方限速
// while (Objects.nonNull(base) && count < 20) {
// ++count;
// Float limitSpeed1 = base.getNeedLimitSpeed();
// if (limitSpeed1 == null && !CollectionUtils.isEmpty(base.getLogicList())) { //兼容逻辑区段设置限速
// float distanceBetweenLogicAndPhysical = 0; //逻辑区段距物理区段端点的距离（同向端点）
// for (Section section : base.getLogicList()) {
// limitSpeed1 = section.getNeedLimitSpeed();
// if (limitSpeed1 != null) {
// float offsetOfLogicSectionOnPhysicalSection = distanceBetweenLogicAndPhysical + (right ? section.getLen() : 0); //逻辑区段的端点在物理区段上的偏移量
// SectionPosition logicSectionPosition = new SectionPosition(base, offsetOfLogicSectionOnPhysicalSection);
// if (logicSectionPosition.isAheadOf(tailPosition, right)) {
// break;
// }
// }
// distanceBetweenLogicAndPhysical += section.getLen();
// }
// }
// if (Objects.nonNull(limitSpeed1)) {
// if (ahead) {
// // 车头前方
// vt = Math.min(limitSpeed, limitSpeed1 * 0.9f);
// endPosition = new SectionPosition(base, right ? 0 : base.getLen());
// break;
// } else {
// limitSpeed = Math.min(limitSpeed, limitSpeed1 * 0.9f);
// }
// }
// if (Objects.equals(base, headSection)) {
// ahead = true;
// }
// base = base.getNextRunningSectionOf(right);
// }
// }
// SpeedCurve normalSpeedCurve = buildTargetSpeedCurve(totalLen, v0, 0, limitSpeed);
// if (endPosition != null) {
// Float distance = CalculateService.calculateDistance(headPosition, endPosition, right);
// if (distance != null && distance < totalLen) {
// SpeedCurve limitSpeedCurve = buildTargetSpeedCurve(distance, v0, vt, limitSpeed);
// float normalStartSpeed = normalSpeedCurve.getSpeedOf(normalSpeedCurve.getTotalDistance());
// float limitStartSpeed = limitSpeedCurve.getSpeedOf(limitSpeedCurve.getTotalDistance());
// return normalStartSpeed < limitStartSpeed ? normalSpeedCurve : limitSpeedCurve;
// }
// }
// return normalSpeedCurve;
// }
/**
* 构建目标速度曲线
*
@ -374,11 +412,6 @@ public class SpeedCurve {
return SpeedCurve.ZERO;
}
SpeedCurve speedCurve = new SpeedCurve(targetDistance);
/*float uv = limitSpeed;
if (vt > uv) {
vt = uv;
}
float a1 = VirtualRealityTrain.Stable_Acceleration; // 加速段平均加速度*/
float a2 = VirtualRealityTrain.Max_Braking_Acceleration; // 减速段平均加速度
if (vt >= limitSpeed) {
speedCurve.addCalculator(new SpeedCalculator(0, targetDistance,
@ -396,41 +429,6 @@ public class SpeedCurve {
decelerateV0, vt, -a2, VSFormula.UNIFORM_DECELERATION));
}
return speedCurve;
/*//通过两线相交计算交点y值（即速度）
float vMid = (float) Math.sqrt(2 * a1 * a2 * targetDistance + a2 * Math.pow(v0, 2) + a1 * Math.pow(vt, 2)) / (a1 + a2);
if (vMid > uv) { // 加速-匀速-减速
float la = (float) ((Math.pow(uv, 2) - Math.pow(v0, 2)) / (2 * a1)); // 加速段长度
float ld = (float) (Math.pow(uv, 2) - Math.pow(vt, 2) / (2 * a2)); // 减速段长度
float da = la;
float dd = targetDistance - ld;
speedCurve.addCalculator(new SpeedCalculator(0, da,
v0, uv, a1,
VSFormula.UNIFORM_ACCELERATION));
speedCurve.addCalculator(new SpeedCalculator(da, dd,
uv, uv, 0,
VSFormula.UNIFORM));
speedCurve.addCalculator(new SpeedCalculator(dd, targetDistance,
uv, vt, -a2,
VSFormula.UNIFORM_DECELERATION));
} else {
if (vMid > v0) { // 加速-减速
float dA2 = (float) (Math.pow(vMid, 2) - Math.pow(vt, 2) / (2 * a2)); // 减速段长度
float dMid = targetDistance - dA2;//加速-减速段中点位置
speedCurve.addCalculator(new SpeedCalculator(0, dMid,
v0, vMid, a1,
VSFormula.UNIFORM_ACCELERATION));
speedCurve.addCalculator(new SpeedCalculator(dMid, targetDistance,
vMid, vt, -a2,
VSFormula.UNIFORM_DECELERATION));
} else { // 只减速
speedCurve.addCalculator(new SpeedCalculator(0, targetDistance,
v0, vt, -a2,
VSFormula.UNIFORM_DECELERATION));
}
}
return speedCurve;*/
}
/**