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

贯通车队运行链并支持轨迹自动推导β

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-25 17:59:18 +08:00
parent 95c0b19a26
commit 17092c2766
37 changed files with 3632 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+141 -9
View File
@@ -21,6 +21,11 @@ namespace MultiWheelC
public sealed class TrajectoryTrackingMovement
: MovementDefinition
{
private const double ReferenceSpeedDeadbandMetersPerSecond =
1e-6;
private const double FixedMotionDirectionToleranceRadians =
3.0 * Math.PI / 180.0;
/// <summary>
/// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
/// </summary>
@@ -43,9 +48,18 @@ namespace MultiWheelC
public Action<ParkingGeometricController> CycleObserver;
/// <summary>
/// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。
/// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。
/// </summary>
public double MotionDirectionInBodyRadians;
public double? MotionDirectionInBodyRadians = 0.0;
/// <summary>
/// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。
/// </summary>
public double? ResolvedMotionDirectionInBodyRadians
{
get;
private set;
}
/// <summary>
/// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。
@@ -270,6 +284,12 @@ namespace MultiWheelC
config.ParkingExecutionTimeoutSeconds;
ValidateParameters(executionTimeoutSeconds);
var motionDirectionInBodyRadians =
MotionDirectionInBodyRadians ??
ResolveFixedMotionDirectionInBodyRadians(
Trajectory);
ResolvedMotionDirectionInBodyRadians =
motionDirectionInBodyRadians;
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
@@ -283,7 +303,7 @@ namespace MultiWheelC
new PrepareWheelsForward
{
DirectionRadians =
MotionDirectionInBodyRadians
motionDirectionInBodyRadians
};
foreach (var keepRunning in wheelPreparation.Get())
{
@@ -306,7 +326,7 @@ namespace MultiWheelC
PilotDefinition.Self.CarNum);
adapter.ActivateMotionFrame(
MotionDirectionInBodyRadians);
motionDirectionInBodyRadians);
var stateProvider =
StateProvider ??
@@ -348,7 +368,7 @@ namespace MultiWheelC
new GcpCommandExecutor(
adapter,
maximumGcpAngleRateRadiansPerSecond,
MotionDirectionInBodyRadians);
motionDirectionInBodyRadians);
Controller = new ParkingGeometricController(
stateProvider,
@@ -366,7 +386,7 @@ namespace MultiWheelC
maximumTerminalApproachSpeedMetersPerSecond,
stanleyCurvaturePreviewSeconds,
stanleyMaximumCurvaturePreviewMeters,
MotionDirectionInBodyRadians);
motionDirectionInBodyRadians);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
@@ -478,9 +498,12 @@ namespace MultiWheelC
"新版轨迹跟踪动作没有设置Trajectory。");
}
NumericGuard.EnsureFinite(
MotionDirectionInBodyRadians,
nameof(MotionDirectionInBodyRadians));
if (MotionDirectionInBodyRadians.HasValue)
{
NumericGuard.EnsureFinite(
MotionDirectionInBodyRadians.Value,
nameof(MotionDirectionInBodyRadians));
}
if (double.IsNaN(executionTimeoutSeconds) ||
double.IsInfinity(executionTimeoutSeconds) ||
@@ -491,5 +514,114 @@ namespace MultiWheelC
"轨迹跟踪超时时间必须是正有限值。");
}
}
/// <summary>
/// 根据轨迹切线、参考车身航向和速度符号推导整段轨迹共同使用的固定运动方向。
/// </summary>
private static double ResolveFixedMotionDirectionInBodyRadians(
Trajectory2D trajectory)
{
double? resolvedDirectionRadians = null;
for (var index = 0;
index < trajectory.Count - 1;
index++)
{
var segmentStart = trajectory[index];
var segmentEnd = trajectory[index + 1];
var travelDirection = ResolveSegmentTravelDirection(
segmentStart.ReferenceSpeedMetersPerSecond,
segmentEnd.ReferenceSpeedMetersPerSecond,
index);
if (travelDirection == 0.0)
{
continue;
}
var tangentYawRadians = Math.Atan2(
segmentEnd.PoseInWorld.YMeters -
segmentStart.PoseInWorld.YMeters,
segmentEnd.PoseInWorld.XMeters -
segmentStart.PoseInWorld.XMeters);
var positiveMotionAxisYawRadians =
travelDirection > 0.0
? tangentYawRadians
: AngleMath.NormalizeRadians(
tangentYawRadians + Math.PI);
var referenceBodyYawRadians =
AngleMath.LerpRadians(
segmentStart.PoseInWorld.YawRadians,
segmentEnd.PoseInWorld.YawRadians,
0.5);
var candidateDirectionRadians =
AngleMath.ShortestDifferenceRadians(
positiveMotionAxisYawRadians,
referenceBodyYawRadians);
if (!resolvedDirectionRadians.HasValue)
{
resolvedDirectionRadians =
candidateDirectionRadians;
continue;
}
var directionDifferenceRadians = Math.Abs(
AngleMath.ShortestDifferenceRadians(
candidateDirectionRadians,
resolvedDirectionRadians.Value));
if (directionDifferenceRadians >
FixedMotionDirectionToleranceRadians)
{
throw new InvalidOperationException(
"轨迹无法由一个固定运动坐标系执行：" +
$"第{index + 1}段需要的方向与起始方向相差" +
$"{AngleMath.RadiansToDegrees(directionDifferenceRadians):F2}°。" +
"请拆分轨迹，或显式指定并验证MotionDirectionInBodyRadians。");
}
}
if (!resolvedDirectionRadians.HasValue)
{
throw new InvalidOperationException(
"轨迹没有非零参考速度线段，无法自动确定运动坐标系方向。");
}
return resolvedDirectionRadians.Value;
}
/// <summary>
/// 从相邻轨迹点的有符号参考速度确定该线段的执行方向。
/// </summary>
private static double ResolveSegmentTravelDirection(
double startSpeedMetersPerSecond,
double endSpeedMetersPerSecond,
int segmentStartIndex)
{
var hasStartDirection =
Math.Abs(startSpeedMetersPerSecond) >
ReferenceSpeedDeadbandMetersPerSecond;
var hasEndDirection =
Math.Abs(endSpeedMetersPerSecond) >
ReferenceSpeedDeadbandMetersPerSecond;
if (hasStartDirection &&
hasEndDirection &&
Math.Sign(startSpeedMetersPerSecond) !=
Math.Sign(endSpeedMetersPerSecond))
{
throw new InvalidOperationException(
$"轨迹第{segmentStartIndex + 1}段内参考速度发生正负切换，" +
"无法自动确定固定运动坐标系；请在零速点拆分动作段。");
}
if (hasStartDirection)
{
return Math.Sign(startSpeedMetersPerSecond);
}
return hasEndDirection
? Math.Sign(endSpeedMetersPerSecond)
: 0.0;
}
}
}