增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,793 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Control.Execution
|
||||
{
|
||||
// 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。
|
||||
public enum PathTrackingCycleResult
|
||||
{
|
||||
Inactive = 0,
|
||||
CommandGenerated = 1,
|
||||
Completed = 2,
|
||||
Faulted = 3
|
||||
}
|
||||
|
||||
// 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。
|
||||
public readonly struct PathTrackingCycleOutput
|
||||
{
|
||||
public PathTrackingCycleOutput(
|
||||
PathTrackingCycleResult result,
|
||||
GcpMotionCommand? command,
|
||||
TrajectoryProjection? projection,
|
||||
double projectionMilliseconds,
|
||||
double controllerComputeMilliseconds)
|
||||
{
|
||||
Result = result;
|
||||
Command = command;
|
||||
Projection = projection;
|
||||
ProjectionMilliseconds = projectionMilliseconds;
|
||||
ControllerComputeMilliseconds =
|
||||
controllerComputeMilliseconds;
|
||||
}
|
||||
|
||||
public PathTrackingCycleResult Result { get; }
|
||||
|
||||
public GcpMotionCommand? Command { get; }
|
||||
|
||||
public TrajectoryProjection? Projection { get; }
|
||||
|
||||
public double ProjectionMilliseconds { get; }
|
||||
|
||||
public double ControllerComputeMilliseconds { get; }
|
||||
}
|
||||
|
||||
// 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。
|
||||
public sealed class PathTrackingCore
|
||||
{
|
||||
private const double ZeroReferenceSpeedToleranceMetersPerSecond =
|
||||
1e-6;
|
||||
private const double StartupRegionMeters = 0.02;
|
||||
private const double StartupPreviewDistanceMeters = 0.05;
|
||||
private const double MaximumStartupSpeedMetersPerSecond = 0.08;
|
||||
private const double ProjectionBackwardSearchDistanceMeters =
|
||||
0.10;
|
||||
private const double ProjectionForwardSearchDistanceMeters =
|
||||
1.00;
|
||||
|
||||
private readonly ILateralController _lateralController;
|
||||
private readonly ILongitudinalController _longitudinalController;
|
||||
private readonly GcpCommandAllocator _gcpAllocator;
|
||||
private readonly double _motionDirectionInBodyRadians;
|
||||
|
||||
private Trajectory2D _trajectory;
|
||||
private double _terminalTravelDirection = 1.0;
|
||||
|
||||
public PathTrackingCore(
|
||||
ILateralController lateralController,
|
||||
ILongitudinalController longitudinalController,
|
||||
GcpCommandAllocator gcpAllocator,
|
||||
double finishDistanceMeters = 0.04,
|
||||
double finishSpeedMetersPerSecond = 0.02,
|
||||
double finishHeadingToleranceRadians =
|
||||
3.0 * Math.PI / 180.0,
|
||||
double maximumDistanceToTrajectoryMeters = 0.30,
|
||||
double terminalBrakingPreviewMeters = 0.02,
|
||||
double terminalApproachDistanceMeters = 0.10,
|
||||
double terminalApproachGainPerSecond = 0.8,
|
||||
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
|
||||
double curvaturePreviewSeconds = 0.20,
|
||||
double maximumCurvaturePreviewMeters = 0.12,
|
||||
double motionDirectionInBodyRadians = 0.0)
|
||||
{
|
||||
_lateralController = lateralController ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(lateralController));
|
||||
_longitudinalController = longitudinalController ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(longitudinalController));
|
||||
_gcpAllocator = gcpAllocator ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(gcpAllocator));
|
||||
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
finishDistanceMeters,
|
||||
nameof(finishDistanceMeters));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
finishSpeedMetersPerSecond,
|
||||
nameof(finishSpeedMetersPerSecond));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
finishHeadingToleranceRadians,
|
||||
nameof(finishHeadingToleranceRadians));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumDistanceToTrajectoryMeters,
|
||||
nameof(maximumDistanceToTrajectoryMeters));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
terminalBrakingPreviewMeters,
|
||||
nameof(terminalBrakingPreviewMeters));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
terminalApproachDistanceMeters,
|
||||
nameof(terminalApproachDistanceMeters));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
terminalApproachGainPerSecond,
|
||||
nameof(terminalApproachGainPerSecond));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumTerminalApproachSpeedMetersPerSecond,
|
||||
nameof(maximumTerminalApproachSpeedMetersPerSecond));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
curvaturePreviewSeconds,
|
||||
nameof(curvaturePreviewSeconds));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
maximumCurvaturePreviewMeters,
|
||||
nameof(maximumCurvaturePreviewMeters));
|
||||
NumericGuard.EnsureFinite(
|
||||
motionDirectionInBodyRadians,
|
||||
nameof(motionDirectionInBodyRadians));
|
||||
|
||||
if (terminalApproachDistanceMeters <=
|
||||
finishDistanceMeters)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(terminalApproachDistanceMeters),
|
||||
"终点单向逼近范围必须大于终点位置容差。");
|
||||
}
|
||||
|
||||
FinishDistanceMeters = finishDistanceMeters;
|
||||
FinishSpeedMetersPerSecond =
|
||||
finishSpeedMetersPerSecond;
|
||||
FinishHeadingToleranceRadians =
|
||||
finishHeadingToleranceRadians;
|
||||
MaximumDistanceToTrajectoryMeters =
|
||||
maximumDistanceToTrajectoryMeters;
|
||||
TerminalBrakingPreviewMeters =
|
||||
terminalBrakingPreviewMeters;
|
||||
TerminalApproachDistanceMeters =
|
||||
terminalApproachDistanceMeters;
|
||||
TerminalApproachGainPerSecond =
|
||||
terminalApproachGainPerSecond;
|
||||
MaximumTerminalApproachSpeedMetersPerSecond =
|
||||
maximumTerminalApproachSpeedMetersPerSecond;
|
||||
CurvaturePreviewSeconds = curvaturePreviewSeconds;
|
||||
MaximumCurvaturePreviewMeters =
|
||||
maximumCurvaturePreviewMeters;
|
||||
_motionDirectionInBodyRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
motionDirectionInBodyRadians);
|
||||
}
|
||||
|
||||
public double FinishDistanceMeters { get; }
|
||||
|
||||
public double FinishSpeedMetersPerSecond { get; }
|
||||
|
||||
public double FinishHeadingToleranceRadians { get; }
|
||||
|
||||
public double MaximumDistanceToTrajectoryMeters { get; }
|
||||
|
||||
public double TerminalBrakingPreviewMeters { get; }
|
||||
|
||||
public double TerminalApproachDistanceMeters { get; }
|
||||
|
||||
public double TerminalApproachGainPerSecond { get; }
|
||||
|
||||
public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
|
||||
|
||||
public double CurvaturePreviewSeconds { get; }
|
||||
|
||||
public double MaximumCurvaturePreviewMeters { get; }
|
||||
|
||||
public bool IsActive { get; private set; }
|
||||
|
||||
public bool IsCompleted { get; private set; }
|
||||
|
||||
public string LastFailureReason { get; private set; } =
|
||||
string.Empty;
|
||||
|
||||
public Exception LastException { get; private set; }
|
||||
|
||||
public TrajectoryProjection? LastProjection { get; private set; }
|
||||
|
||||
public GcpMotionCommand? LastRequestedCommand { get; private set; }
|
||||
|
||||
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
|
||||
|
||||
public double? LastCurvaturePreviewDistanceMeters { get; private set; }
|
||||
|
||||
public double? LastFeedforwardCurvaturePerMeter { get; private set; }
|
||||
|
||||
// 重置跨周期状态，并从轨迹起点开始新的跟踪过程。
|
||||
public void Start(Trajectory2D trajectory)
|
||||
{
|
||||
if (trajectory == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
var terminalTravelDirection =
|
||||
ResolveTerminalTravelDirection(trajectory);
|
||||
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = trajectory;
|
||||
_terminalTravelDirection =
|
||||
terminalTravelDirection;
|
||||
IsActive = true;
|
||||
IsCompleted = false;
|
||||
ClearDiagnostics();
|
||||
}
|
||||
|
||||
// 将受控刚体的位姿和速度转换为本周期GCP命令。
|
||||
public PathTrackingCycleOutput Compute(
|
||||
Pose2D poseInWorld,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
NumericGuard.EnsureFinite(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
NumericGuard.EnsureFinite(
|
||||
actualTwistInBody,
|
||||
nameof(actualTwistInBody));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
if (!IsActive || _trajectory == null)
|
||||
{
|
||||
return new PathTrackingCycleOutput(
|
||||
PathTrackingCycleResult.Inactive,
|
||||
null,
|
||||
LastProjection,
|
||||
0.0,
|
||||
0.0);
|
||||
}
|
||||
|
||||
var projectionStartTimestamp =
|
||||
Stopwatch.GetTimestamp();
|
||||
var projectionCompleted = false;
|
||||
var projectionMilliseconds = 0.0;
|
||||
var controllerComputeStartTimestamp = 0L;
|
||||
|
||||
try
|
||||
{
|
||||
var projection = LastProjection.HasValue
|
||||
? TrajectoryProjector.Project(
|
||||
_trajectory,
|
||||
poseInWorld,
|
||||
LastProjection.Value.ArcLengthMeters,
|
||||
ProjectionBackwardSearchDistanceMeters,
|
||||
ProjectionForwardSearchDistanceMeters)
|
||||
: TrajectoryProjector.Project(
|
||||
_trajectory,
|
||||
poseInWorld);
|
||||
|
||||
projectionMilliseconds =
|
||||
GetElapsedMilliseconds(
|
||||
projectionStartTimestamp);
|
||||
projectionCompleted = true;
|
||||
LastProjection = projection;
|
||||
controllerComputeStartTimestamp =
|
||||
Stopwatch.GetTimestamp();
|
||||
|
||||
if (projection.DistanceToTrajectoryMeters >
|
||||
MaximumDistanceToTrajectoryMeters)
|
||||
{
|
||||
Fail(
|
||||
"受控刚体距离参考轨迹" +
|
||||
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
|
||||
"超过允许值" +
|
||||
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Faulted,
|
||||
null,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
|
||||
if (HasReachedEnd(
|
||||
poseInWorld,
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
projection))
|
||||
{
|
||||
CompleteTrajectory();
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Completed,
|
||||
null,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
|
||||
if (HasStoppedAtUnsatisfiedTerminal(
|
||||
poseInWorld,
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
projection,
|
||||
out var terminalFailureReason))
|
||||
{
|
||||
Fail(terminalFailureReason);
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Faulted,
|
||||
null,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
|
||||
var controlReferenceSpeedMetersPerSecond =
|
||||
ResolveControlReferenceSpeed(
|
||||
poseInWorld,
|
||||
projection);
|
||||
LastControlReferenceSpeedMetersPerSecond =
|
||||
controlReferenceSpeedMetersPerSecond;
|
||||
var curvaturePreviewDistanceMeters =
|
||||
ResolveCurvaturePreviewDistanceMeters(
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
controlReferenceSpeedMetersPerSecond);
|
||||
LastCurvaturePreviewDistanceMeters =
|
||||
curvaturePreviewDistanceMeters;
|
||||
var feedforwardCurvaturePerMeter =
|
||||
ResolveFeedforwardCurvaturePerMeter(
|
||||
projection,
|
||||
curvaturePreviewDistanceMeters);
|
||||
LastFeedforwardCurvaturePerMeter =
|
||||
feedforwardCurvaturePerMeter;
|
||||
var context = new PathTrackingContext(
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
projection,
|
||||
controlReferenceSpeedMetersPerSecond,
|
||||
feedforwardCurvaturePerMeter,
|
||||
deltaTimeSeconds,
|
||||
_motionDirectionInBodyRadians);
|
||||
var lateralCommand =
|
||||
_lateralController.Compute(context);
|
||||
var commandSpeedMetersPerSecond =
|
||||
_longitudinalController
|
||||
.ComputeSpeedMetersPerSecond(context);
|
||||
var command = _gcpAllocator.Allocate(
|
||||
commandSpeedMetersPerSecond,
|
||||
lateralCommand);
|
||||
|
||||
LastRequestedCommand = command;
|
||||
LastFailureReason = string.Empty;
|
||||
LastException = null;
|
||||
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.CommandGenerated,
|
||||
command,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
if (!projectionCompleted)
|
||||
{
|
||||
projectionMilliseconds =
|
||||
GetElapsedMilliseconds(
|
||||
projectionStartTimestamp);
|
||||
}
|
||||
|
||||
Fail(
|
||||
"轨迹跟踪核心计算异常：" +
|
||||
exception.Message,
|
||||
exception);
|
||||
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Faulted,
|
||||
null,
|
||||
LastProjection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
}
|
||||
|
||||
// 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。
|
||||
public void PauseForUnavailableState(string reason)
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
LastRequestedCommand = null;
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
LastException = null;
|
||||
}
|
||||
|
||||
// 将外层执行故障同步到公共核心，并终止当前轨迹。
|
||||
public void Fail(
|
||||
string reason,
|
||||
Exception exception = null)
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = null;
|
||||
IsActive = false;
|
||||
IsCompleted = false;
|
||||
LastRequestedCommand = null;
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
LastException = exception;
|
||||
}
|
||||
|
||||
// 取消当前轨迹并清除全部跟踪状态。
|
||||
public void Cancel()
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = null;
|
||||
_terminalTravelDirection = 1.0;
|
||||
IsActive = false;
|
||||
IsCompleted = false;
|
||||
ClearDiagnostics();
|
||||
}
|
||||
|
||||
private double ResolveControlReferenceSpeed(
|
||||
Pose2D poseInWorld,
|
||||
TrajectoryProjection projection)
|
||||
{
|
||||
if (projection.RemainingDistanceMeters >
|
||||
TerminalApproachDistanceMeters)
|
||||
{
|
||||
return ResolveReferenceSpeedForControl(
|
||||
projection);
|
||||
}
|
||||
|
||||
return ResolveTerminalApproachSpeed(
|
||||
poseInWorld);
|
||||
}
|
||||
|
||||
private double ResolveCurvaturePreviewDistanceMeters(
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
double controlReferenceSpeedMetersPerSecond)
|
||||
{
|
||||
if (CurvaturePreviewSeconds <= 0.0 ||
|
||||
MaximumCurvaturePreviewMeters <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var previewSpeedMetersPerSecond =
|
||||
hasValidVelocityEstimate
|
||||
? CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
actualTwistInBody)
|
||||
: Math.Abs(
|
||||
controlReferenceSpeedMetersPerSecond);
|
||||
|
||||
return Math.Min(
|
||||
MaximumCurvaturePreviewMeters,
|
||||
previewSpeedMetersPerSecond *
|
||||
CurvaturePreviewSeconds);
|
||||
}
|
||||
|
||||
private double ResolveFeedforwardCurvaturePerMeter(
|
||||
TrajectoryProjection projection,
|
||||
double previewDistanceMeters)
|
||||
{
|
||||
var previewArcLengthMeters = Math.Min(
|
||||
_trajectory.TotalLengthMeters,
|
||||
projection.ArcLengthMeters +
|
||||
previewDistanceMeters);
|
||||
|
||||
return _trajectory
|
||||
.SampleAtArcLength(previewArcLengthMeters)
|
||||
.CurvaturePerMeter;
|
||||
}
|
||||
|
||||
private double ResolveTerminalApproachSpeed(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
var distanceToEndMeters =
|
||||
CalculateDistanceToEndMeters(
|
||||
poseInWorld);
|
||||
var headingErrorToEndRadians =
|
||||
CalculateHeadingErrorToEndRadians(
|
||||
poseInWorld);
|
||||
|
||||
if (distanceToEndMeters <=
|
||||
FinishDistanceMeters &&
|
||||
headingErrorToEndRadians <=
|
||||
FinishHeadingToleranceRadians)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var endPose = _trajectory.EndPoint.PoseInWorld;
|
||||
var deltaX = endPose.XMeters -
|
||||
poseInWorld.XMeters;
|
||||
var deltaY = endPose.YMeters -
|
||||
poseInWorld.YMeters;
|
||||
var longitudinalErrorMeters =
|
||||
deltaX * Math.Cos(endPose.YawRadians) +
|
||||
deltaY * Math.Sin(endPose.YawRadians);
|
||||
var remainingAlongTravelMeters =
|
||||
_terminalTravelDirection *
|
||||
longitudinalErrorMeters;
|
||||
|
||||
// 越过终点后不生成与原轨迹方向相反的修正速度。
|
||||
if (remainingAlongTravelMeters <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var speedMagnitudeMetersPerSecond =
|
||||
Math.Min(
|
||||
MaximumTerminalApproachSpeedMetersPerSecond,
|
||||
TerminalApproachGainPerSecond *
|
||||
remainingAlongTravelMeters);
|
||||
|
||||
return _terminalTravelDirection *
|
||||
speedMagnitudeMetersPerSecond;
|
||||
}
|
||||
|
||||
private double ResolveReferenceSpeedForControl(
|
||||
TrajectoryProjection projection)
|
||||
{
|
||||
var currentReferenceSpeed =
|
||||
ApplyTerminalBrakingPreview(
|
||||
projection,
|
||||
projection.ReferencePoint
|
||||
.ReferenceSpeedMetersPerSecond);
|
||||
var isInStartupRegion =
|
||||
projection.ArcLengthMeters <=
|
||||
StartupRegionMeters &&
|
||||
projection.RemainingDistanceMeters >
|
||||
FinishDistanceMeters;
|
||||
|
||||
if (!isInStartupRegion)
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
var previewArcLengthMeters = Math.Min(
|
||||
_trajectory.TotalLengthMeters,
|
||||
projection.ArcLengthMeters +
|
||||
StartupPreviewDistanceMeters);
|
||||
var previewReferenceSpeed =
|
||||
_trajectory
|
||||
.SampleAtArcLength(previewArcLengthMeters)
|
||||
.ReferenceSpeedMetersPerSecond;
|
||||
|
||||
if (Math.Abs(previewReferenceSpeed) <=
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond)
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
var startupReleaseSpeed =
|
||||
Math.Sign(previewReferenceSpeed) *
|
||||
Math.Min(
|
||||
Math.Abs(previewReferenceSpeed),
|
||||
MaximumStartupSpeedMetersPerSecond);
|
||||
|
||||
if (Math.Sign(currentReferenceSpeed) ==
|
||||
Math.Sign(startupReleaseSpeed) &&
|
||||
Math.Abs(currentReferenceSpeed) >=
|
||||
Math.Abs(startupReleaseSpeed))
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
return startupReleaseSpeed;
|
||||
}
|
||||
|
||||
private double ApplyTerminalBrakingPreview(
|
||||
TrajectoryProjection projection,
|
||||
double currentReferenceSpeed)
|
||||
{
|
||||
if (TerminalBrakingPreviewMeters <= 0.0)
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
var previewArcLengthMeters = Math.Min(
|
||||
_trajectory.TotalLengthMeters,
|
||||
projection.ArcLengthMeters +
|
||||
TerminalBrakingPreviewMeters);
|
||||
var previewReferenceSpeed =
|
||||
_trajectory
|
||||
.SampleAtArcLength(previewArcLengthMeters)
|
||||
.ReferenceSpeedMetersPerSecond;
|
||||
var previewIsStop =
|
||||
Math.Abs(previewReferenceSpeed) <=
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||||
var hasSameDirection =
|
||||
Math.Sign(previewReferenceSpeed) ==
|
||||
Math.Sign(currentReferenceSpeed);
|
||||
var previewIsSlower =
|
||||
Math.Abs(previewReferenceSpeed) <
|
||||
Math.Abs(currentReferenceSpeed);
|
||||
|
||||
if (previewIsSlower &&
|
||||
(previewIsStop || hasSameDirection))
|
||||
{
|
||||
return previewReferenceSpeed;
|
||||
}
|
||||
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
private static double ResolveTerminalTravelDirection(
|
||||
Trajectory2D trajectory)
|
||||
{
|
||||
for (var index = trajectory.Count - 1;
|
||||
index >= 0;
|
||||
index--)
|
||||
{
|
||||
var referenceSpeedMetersPerSecond =
|
||||
trajectory[index]
|
||||
.ReferenceSpeedMetersPerSecond;
|
||||
|
||||
if (Math.Abs(referenceSpeedMetersPerSecond) >
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond)
|
||||
{
|
||||
return Math.Sign(
|
||||
referenceSpeedMetersPerSecond);
|
||||
}
|
||||
}
|
||||
|
||||
throw new ArgumentException(
|
||||
"轨迹必须在终点前包含至少一个非零参考速度。",
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
private bool HasReachedEnd(
|
||||
Pose2D poseInWorld,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
TrajectoryProjection projection)
|
||||
{
|
||||
if (!hasValidVelocityEstimate)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return projection.RemainingDistanceMeters <=
|
||||
FinishDistanceMeters &&
|
||||
CalculateDistanceToEndMeters(poseInWorld) <=
|
||||
FinishDistanceMeters &&
|
||||
CalculateHeadingErrorToEndRadians(poseInWorld) <=
|
||||
FinishHeadingToleranceRadians &&
|
||||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
actualTwistInBody) <=
|
||||
FinishSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
private bool HasStoppedAtUnsatisfiedTerminal(
|
||||
Pose2D poseInWorld,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
TrajectoryProjection projection,
|
||||
out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
|
||||
var isTerminalZeroSpeedReference =
|
||||
projection.RemainingDistanceMeters <=
|
||||
FinishDistanceMeters &&
|
||||
Math.Abs(
|
||||
projection.ReferencePoint
|
||||
.ReferenceSpeedMetersPerSecond) <=
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||||
|
||||
if (!isTerminalZeroSpeedReference ||
|
||||
!hasValidVelocityEstimate ||
|
||||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
actualTwistInBody) >
|
||||
FinishSpeedMetersPerSecond)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var positionErrorMeters =
|
||||
CalculateDistanceToEndMeters(
|
||||
poseInWorld);
|
||||
var headingErrorRadians =
|
||||
CalculateHeadingErrorToEndRadians(
|
||||
poseInWorld);
|
||||
|
||||
failureReason =
|
||||
"受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" +
|
||||
$"位置误差={positionErrorMeters:F3}m，" +
|
||||
"航向误差=" +
|
||||
$"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
|
||||
return true;
|
||||
}
|
||||
|
||||
private double CalculateDistanceToEndMeters(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
var endPose = _trajectory.EndPoint.PoseInWorld;
|
||||
var deltaX = poseInWorld.XMeters -
|
||||
endPose.XMeters;
|
||||
var deltaY = poseInWorld.YMeters -
|
||||
endPose.YMeters;
|
||||
|
||||
return Math.Sqrt(
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY);
|
||||
}
|
||||
|
||||
private double CalculateHeadingErrorToEndRadians(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
return Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
_trajectory.EndPoint
|
||||
.PoseInWorld.YawRadians,
|
||||
poseInWorld.YawRadians));
|
||||
}
|
||||
|
||||
private double CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
Twist2D actualTwistInBody)
|
||||
{
|
||||
return Math.Abs(
|
||||
Math.Cos(_motionDirectionInBodyRadians) *
|
||||
actualTwistInBody.VxMetersPerSecond +
|
||||
Math.Sin(_motionDirectionInBodyRadians) *
|
||||
actualTwistInBody.VyMetersPerSecond);
|
||||
}
|
||||
|
||||
private void CompleteTrajectory()
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = null;
|
||||
IsActive = false;
|
||||
IsCompleted = true;
|
||||
LastRequestedCommand = new GcpMotionCommand(
|
||||
0.0,
|
||||
0.0,
|
||||
0.0);
|
||||
LastFailureReason = string.Empty;
|
||||
LastException = null;
|
||||
}
|
||||
|
||||
private void ResetFeedbackControllers()
|
||||
{
|
||||
_lateralController.Reset();
|
||||
_longitudinalController.Reset();
|
||||
}
|
||||
|
||||
private void ClearDiagnostics()
|
||||
{
|
||||
LastProjection = null;
|
||||
LastRequestedCommand = null;
|
||||
LastControlReferenceSpeedMetersPerSecond = null;
|
||||
LastCurvaturePreviewDistanceMeters = null;
|
||||
LastFeedforwardCurvaturePerMeter = null;
|
||||
LastFailureReason = string.Empty;
|
||||
LastException = null;
|
||||
}
|
||||
|
||||
private static PathTrackingCycleOutput CreateOutput(
|
||||
PathTrackingCycleResult result,
|
||||
GcpMotionCommand? command,
|
||||
TrajectoryProjection? projection,
|
||||
double projectionMilliseconds,
|
||||
long controllerComputeStartTimestamp)
|
||||
{
|
||||
var controllerComputeMilliseconds =
|
||||
controllerComputeStartTimestamp == 0L
|
||||
? 0.0
|
||||
: GetElapsedMilliseconds(
|
||||
controllerComputeStartTimestamp);
|
||||
|
||||
return new PathTrackingCycleOutput(
|
||||
result,
|
||||
command,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeMilliseconds);
|
||||
}
|
||||
|
||||
private static double GetElapsedMilliseconds(
|
||||
long startTimestamp)
|
||||
{
|
||||
return (Stopwatch.GetTimestamp() - startTimestamp) *
|
||||
1000.0 /
|
||||
Stopwatch.Frequency;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user