794 lines
29 KiB
C#
794 lines
29 KiB
C#
|
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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|