1082 lines
40 KiB
C#
1082 lines
40 KiB
C#
using System;
|
||
using System.Diagnostics;
|
||
using MultiWheelC.Control.Abstractions;
|
||
using MultiWheelC.Control.Allocation;
|
||
using MultiWheelC.StateEstimation;
|
||
using MultiWheelC.Trajectory;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC.Control.Execution
|
||
{
|
||
/// <summary>
|
||
/// 表示新版停车机器人单周期轨迹控制的执行结果。
|
||
/// </summary>
|
||
public enum ParkingControlCycleResult
|
||
{
|
||
Inactive = 0,
|
||
CommandSent = 1,
|
||
Completed = 2,
|
||
StateUnavailable = 3,
|
||
Faulted = 4
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存一个轨迹控制周期各阶段的高精度耗时和定位帧更新状态。
|
||
/// </summary>
|
||
public readonly struct ParkingControlCycleTiming
|
||
{
|
||
public ParkingControlCycleTiming(
|
||
long cycleIndex,
|
||
double cycleIntervalMilliseconds,
|
||
double stateReadMilliseconds,
|
||
double projectionMilliseconds,
|
||
double controllerComputeMilliseconds,
|
||
double commandSendMilliseconds,
|
||
double totalCycleMilliseconds,
|
||
bool hasStateTimestamp,
|
||
double stateTimestampSeconds,
|
||
bool stateTimestampChanged,
|
||
ParkingControlCycleResult result)
|
||
{
|
||
CycleIndex = cycleIndex;
|
||
CycleIntervalMilliseconds =
|
||
cycleIntervalMilliseconds;
|
||
StateReadMilliseconds = stateReadMilliseconds;
|
||
ProjectionMilliseconds = projectionMilliseconds;
|
||
ControllerComputeMilliseconds =
|
||
controllerComputeMilliseconds;
|
||
CommandSendMilliseconds = commandSendMilliseconds;
|
||
TotalCycleMilliseconds = totalCycleMilliseconds;
|
||
HasStateTimestamp = hasStateTimestamp;
|
||
StateTimestampSeconds = stateTimestampSeconds;
|
||
StateTimestampChanged = stateTimestampChanged;
|
||
Result = result;
|
||
}
|
||
|
||
public long CycleIndex { get; }
|
||
public double CycleIntervalMilliseconds { get; }
|
||
public double StateReadMilliseconds { get; }
|
||
public double ProjectionMilliseconds { get; }
|
||
public double ControllerComputeMilliseconds { get; }
|
||
public double CommandSendMilliseconds { get; }
|
||
public double TotalCycleMilliseconds { get; }
|
||
public bool HasStateTimestamp { get; }
|
||
public double StateTimestampSeconds { get; }
|
||
public bool StateTimestampChanged { get; }
|
||
public ParkingControlCycleResult Result { get; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
|
||
/// </summary>
|
||
public sealed class ParkingGeometricController
|
||
{
|
||
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 IVehicleStateProvider _stateProvider;
|
||
private readonly ILateralController _lateralController;
|
||
private readonly ILongitudinalController _longitudinalController;
|
||
private readonly GcpCommandAllocator _gcpAllocator;
|
||
private readonly GcpCommandExecutor _commandExecutor;
|
||
private readonly double _motionDirectionInBodyRadians;
|
||
|
||
private Trajectory2D _trajectory;
|
||
private double _terminalTravelDirection = 1.0;
|
||
private long _cycleIndex;
|
||
private bool _hasPreviousStateTimestamp;
|
||
private double _previousStateTimestampSeconds;
|
||
|
||
/// <summary>
|
||
/// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
|
||
/// </summary>
|
||
public ParkingGeometricController(
|
||
IVehicleStateProvider stateProvider,
|
||
ILateralController lateralController,
|
||
ILongitudinalController longitudinalController,
|
||
GcpCommandAllocator gcpAllocator,
|
||
GcpCommandExecutor commandExecutor,
|
||
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)
|
||
{
|
||
_stateProvider = stateProvider ??
|
||
throw new ArgumentNullException(
|
||
nameof(stateProvider));
|
||
_lateralController = lateralController ??
|
||
throw new ArgumentNullException(
|
||
nameof(lateralController));
|
||
_longitudinalController = longitudinalController ??
|
||
throw new ArgumentNullException(
|
||
nameof(longitudinalController));
|
||
_gcpAllocator = gcpAllocator ??
|
||
throw new ArgumentNullException(
|
||
nameof(gcpAllocator));
|
||
_commandExecutor = commandExecutor ??
|
||
throw new ArgumentNullException(
|
||
nameof(commandExecutor));
|
||
|
||
EnsureFinitePositive(
|
||
finishDistanceMeters,
|
||
nameof(finishDistanceMeters));
|
||
EnsureFiniteNonNegative(
|
||
finishSpeedMetersPerSecond,
|
||
nameof(finishSpeedMetersPerSecond));
|
||
EnsureFinitePositive(
|
||
finishHeadingToleranceRadians,
|
||
nameof(finishHeadingToleranceRadians));
|
||
EnsureFinitePositive(
|
||
maximumDistanceToTrajectoryMeters,
|
||
nameof(maximumDistanceToTrajectoryMeters));
|
||
EnsureFiniteNonNegative(
|
||
terminalBrakingPreviewMeters,
|
||
nameof(terminalBrakingPreviewMeters));
|
||
EnsureFinitePositive(
|
||
terminalApproachDistanceMeters,
|
||
nameof(terminalApproachDistanceMeters));
|
||
EnsureFinitePositive(
|
||
terminalApproachGainPerSecond,
|
||
nameof(terminalApproachGainPerSecond));
|
||
EnsureFinitePositive(
|
||
maximumTerminalApproachSpeedMetersPerSecond,
|
||
nameof(maximumTerminalApproachSpeedMetersPerSecond));
|
||
EnsureFiniteNonNegative(
|
||
curvaturePreviewSeconds,
|
||
nameof(curvaturePreviewSeconds));
|
||
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);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取终点位置和剩余弧长允许的误差，单位为m。
|
||
/// </summary>
|
||
public double FinishDistanceMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 获取判定轨迹执行完成时允许的最大实际纵向速度，单位为m/s。
|
||
/// </summary>
|
||
public double FinishSpeedMetersPerSecond { get; }
|
||
|
||
/// <summary>
|
||
/// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。
|
||
/// </summary>
|
||
public double FinishHeadingToleranceRadians { get; }
|
||
|
||
/// <summary>
|
||
/// 获取允许车辆偏离参考轨迹的最大距离，单位为m。
|
||
/// </summary>
|
||
public double MaximumDistanceToTrajectoryMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 获取沿轨迹提前读取更低制动参考速度的距离，单位为m。
|
||
/// </summary>
|
||
public double TerminalBrakingPreviewMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 获取切换到终点单向低速逼近的最大剩余弧长，单位为m。
|
||
/// </summary>
|
||
public double TerminalApproachDistanceMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 获取由终点纵向剩余距离生成低速参考的比例增益，单位为1/s。
|
||
/// </summary>
|
||
public double TerminalApproachGainPerSecond { get; }
|
||
|
||
/// <summary>
|
||
/// 获取终点单向逼近参考速度允许的最大绝对值，单位为m/s。
|
||
/// </summary>
|
||
public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
|
||
|
||
/// <summary>
|
||
/// 获取按照车辆纵向速度换算曲率前馈预瞄距离的预测时间，单位为s，0表示关闭。
|
||
/// </summary>
|
||
public double CurvaturePreviewSeconds { get; }
|
||
|
||
/// <summary>
|
||
/// 获取曲率前馈沿轨迹点序允许预瞄的最大距离，单位为m。
|
||
/// </summary>
|
||
public double MaximumCurvaturePreviewMeters { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器当前是否持有并正在执行一条轨迹。
|
||
/// </summary>
|
||
public bool IsActive { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次轨迹是否已经满足终点完成条件。
|
||
/// </summary>
|
||
public bool IsCompleted { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次控制失败原因，正常时为空字符串。
|
||
/// </summary>
|
||
public string LastFailureReason { get; private set; } =
|
||
string.Empty;
|
||
|
||
/// <summary>
|
||
/// 获取最近一次控制异常，正常时为空。
|
||
/// </summary>
|
||
public Exception LastException { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次有效车辆状态。
|
||
/// </summary>
|
||
public VehicleState? LastVehicleState { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次车体中心到参考轨迹的投影结果。
|
||
/// </summary>
|
||
public TrajectoryProjection? LastProjection { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次由控制器和分配器生成、尚未经过角速度限制的GCP运动命令。
|
||
/// </summary>
|
||
public GcpMotionCommand? LastRequestedCommand { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次经过GCP角速度限制后实际发送给底盘的运动命令。
|
||
/// </summary>
|
||
public GcpMotionCommand? LastCommand { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
|
||
/// </summary>
|
||
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近控制周期实际采用的曲率前馈预瞄距离，单位为m。
|
||
/// </summary>
|
||
public double? LastCurvaturePreviewDistanceMeters { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近控制周期沿轨迹预瞄后交给横向控制器的曲率，单位为1/m。
|
||
/// </summary>
|
||
public double? LastFeedforwardCurvaturePerMeter { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。
|
||
/// </summary>
|
||
public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 停止当前底盘并从起点开始执行指定二维轨迹。
|
||
/// </summary>
|
||
public void Start(Trajectory2D trajectory)
|
||
{
|
||
if (trajectory == null)
|
||
{
|
||
throw new ArgumentNullException(
|
||
nameof(trajectory));
|
||
}
|
||
|
||
StopAndResetControllers();
|
||
_trajectory = trajectory;
|
||
_terminalTravelDirection =
|
||
ResolveTerminalTravelDirection(trajectory);
|
||
IsActive = true;
|
||
IsCompleted = false;
|
||
ClearDiagnostics();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
|
||
/// </summary>
|
||
public ParkingControlCycleResult ExecuteCycle(
|
||
double deltaTimeSeconds)
|
||
{
|
||
EnsureFinitePositive(
|
||
deltaTimeSeconds,
|
||
nameof(deltaTimeSeconds));
|
||
|
||
if (!IsActive || _trajectory == null)
|
||
{
|
||
return ParkingControlCycleResult.Inactive;
|
||
}
|
||
|
||
var cycleIndex = ++_cycleIndex;
|
||
var cycleStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
var stateReadMilliseconds = 0.0;
|
||
var projectionMilliseconds = 0.0;
|
||
var controllerComputeMilliseconds = 0.0;
|
||
var commandSendMilliseconds = 0.0;
|
||
var controllerComputeStartTimestamp = 0L;
|
||
var controllerComputeCompleted = false;
|
||
var hasStateTimestamp = false;
|
||
var stateTimestampSeconds = 0.0;
|
||
var stateTimestampChanged = false;
|
||
var cycleResult =
|
||
ParkingControlCycleResult.Faulted;
|
||
|
||
try
|
||
{
|
||
var stateReadStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
bool stateAvailable;
|
||
VehicleState vehicleState;
|
||
try
|
||
{
|
||
stateAvailable =
|
||
_stateProvider.TryGetState(
|
||
out vehicleState);
|
||
}
|
||
finally
|
||
{
|
||
stateReadMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
stateReadStartTimestamp);
|
||
}
|
||
|
||
if (!stateAvailable)
|
||
{
|
||
var stopCommandStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
try
|
||
{
|
||
StopForUnavailableState();
|
||
}
|
||
finally
|
||
{
|
||
commandSendMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
stopCommandStartTimestamp);
|
||
}
|
||
|
||
cycleResult = ParkingControlCycleResult
|
||
.StateUnavailable;
|
||
return cycleResult;
|
||
}
|
||
|
||
LastVehicleState = vehicleState;
|
||
hasStateTimestamp = true;
|
||
stateTimestampSeconds =
|
||
vehicleState.SampleTimestampSeconds;
|
||
stateTimestampChanged =
|
||
!_hasPreviousStateTimestamp ||
|
||
stateTimestampSeconds !=
|
||
_previousStateTimestampSeconds;
|
||
_previousStateTimestampSeconds =
|
||
stateTimestampSeconds;
|
||
_hasPreviousStateTimestamp = true;
|
||
|
||
// 首周期允许全局定位轨迹进度；后续周期仅在上次进度
|
||
// 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。
|
||
var projectionStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
TrajectoryProjection projection;
|
||
try
|
||
{
|
||
projection = LastProjection.HasValue
|
||
? TrajectoryProjector.Project(
|
||
_trajectory,
|
||
vehicleState.PoseInWorld,
|
||
LastProjection.Value.ArcLengthMeters,
|
||
ProjectionBackwardSearchDistanceMeters,
|
||
ProjectionForwardSearchDistanceMeters)
|
||
: TrajectoryProjector.Project(
|
||
_trajectory,
|
||
vehicleState.PoseInWorld);
|
||
}
|
||
finally
|
||
{
|
||
projectionMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
projectionStartTimestamp);
|
||
}
|
||
|
||
LastProjection = projection;
|
||
controllerComputeStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
|
||
if (projection.DistanceToTrajectoryMeters >
|
||
MaximumDistanceToTrajectoryMeters)
|
||
{
|
||
cycleResult = EnterFault(
|
||
"车辆距离参考轨迹" +
|
||
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
|
||
"超过允许值" +
|
||
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
|
||
return cycleResult;
|
||
}
|
||
|
||
if (HasReachedEnd(
|
||
vehicleState,
|
||
projection))
|
||
{
|
||
CompleteTrajectory();
|
||
cycleResult =
|
||
ParkingControlCycleResult.Completed;
|
||
return cycleResult;
|
||
}
|
||
|
||
if (HasStoppedAtUnsatisfiedTerminal(
|
||
vehicleState,
|
||
projection,
|
||
out var terminalFailureReason))
|
||
{
|
||
cycleResult = EnterFault(
|
||
terminalFailureReason);
|
||
return cycleResult;
|
||
}
|
||
|
||
var controlReferenceSpeedMetersPerSecond =
|
||
ResolveControlReferenceSpeed(
|
||
vehicleState,
|
||
projection);
|
||
LastControlReferenceSpeedMetersPerSecond =
|
||
controlReferenceSpeedMetersPerSecond;
|
||
var curvaturePreviewDistanceMeters =
|
||
ResolveCurvaturePreviewDistanceMeters(
|
||
vehicleState,
|
||
controlReferenceSpeedMetersPerSecond);
|
||
var feedforwardCurvaturePerMeter =
|
||
ResolveFeedforwardCurvaturePerMeter(
|
||
projection,
|
||
curvaturePreviewDistanceMeters);
|
||
LastCurvaturePreviewDistanceMeters =
|
||
curvaturePreviewDistanceMeters;
|
||
LastFeedforwardCurvaturePerMeter =
|
||
feedforwardCurvaturePerMeter;
|
||
var context = new PathTrackingContext(
|
||
vehicleState,
|
||
projection,
|
||
controlReferenceSpeedMetersPerSecond,
|
||
feedforwardCurvaturePerMeter,
|
||
deltaTimeSeconds,
|
||
_motionDirectionInBodyRadians);
|
||
var lateralCommand =
|
||
_lateralController.Compute(context);
|
||
var commandSpeedMetersPerSecond =
|
||
_longitudinalController
|
||
.ComputeSpeedMetersPerSecond(context);
|
||
var gcpCommand = _gcpAllocator.Allocate(
|
||
commandSpeedMetersPerSecond,
|
||
lateralCommand);
|
||
LastRequestedCommand = gcpCommand;
|
||
|
||
controllerComputeMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
controllerComputeStartTimestamp);
|
||
controllerComputeCompleted = true;
|
||
|
||
var commandSendStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
bool commandSucceeded;
|
||
try
|
||
{
|
||
commandSucceeded =
|
||
_commandExecutor.Execute(
|
||
gcpCommand,
|
||
deltaTimeSeconds);
|
||
}
|
||
finally
|
||
{
|
||
commandSendMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
commandSendStartTimestamp);
|
||
}
|
||
|
||
if (!commandSucceeded)
|
||
{
|
||
cycleResult = EnterFault(
|
||
string.IsNullOrWhiteSpace(
|
||
_commandExecutor.LastFailureReason)
|
||
? "GCP底盘命令执行失败。"
|
||
: _commandExecutor.LastFailureReason);
|
||
return cycleResult;
|
||
}
|
||
|
||
LastCommand =
|
||
_commandExecutor.LastSentCommand;
|
||
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
cycleResult =
|
||
ParkingControlCycleResult.CommandSent;
|
||
return cycleResult;
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
cycleResult = EnterFault(
|
||
"停车机器人轨迹控制周期异常：" +
|
||
exception.Message,
|
||
exception);
|
||
return cycleResult;
|
||
}
|
||
finally
|
||
{
|
||
if (controllerComputeStartTimestamp != 0L &&
|
||
!controllerComputeCompleted)
|
||
{
|
||
controllerComputeMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
controllerComputeStartTimestamp);
|
||
}
|
||
|
||
LastCycleTiming =
|
||
new ParkingControlCycleTiming(
|
||
cycleIndex,
|
||
deltaTimeSeconds * 1000.0,
|
||
stateReadMilliseconds,
|
||
projectionMilliseconds,
|
||
controllerComputeMilliseconds,
|
||
commandSendMilliseconds,
|
||
GetElapsedMilliseconds(
|
||
cycleStartTimestamp),
|
||
hasStateTimestamp,
|
||
stateTimestampSeconds,
|
||
stateTimestampChanged,
|
||
cycleResult);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 主动取消当前轨迹、立即停车并清除全部控制器状态。
|
||
/// </summary>
|
||
public void Cancel()
|
||
{
|
||
StopAndResetControllers();
|
||
_trajectory = null;
|
||
_terminalTravelDirection = 1.0;
|
||
IsActive = false;
|
||
IsCompleted = false;
|
||
ClearDiagnostics();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在普通空间速度规划和终点单向低速逼近之间选择本周期参考速度。
|
||
/// </summary>
|
||
private double ResolveControlReferenceSpeed(
|
||
VehicleState vehicleState,
|
||
TrajectoryProjection projection)
|
||
{
|
||
if (projection.RemainingDistanceMeters >
|
||
TerminalApproachDistanceMeters)
|
||
{
|
||
return ResolveReferenceSpeedForControl(
|
||
projection);
|
||
}
|
||
|
||
return ResolveTerminalApproachSpeed(
|
||
vehicleState);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据有效实际纵向速度或控制参考速度计算带上限的曲率前馈预瞄距离。
|
||
/// </summary>
|
||
private double ResolveCurvaturePreviewDistanceMeters(
|
||
VehicleState vehicleState,
|
||
double controlReferenceSpeedMetersPerSecond)
|
||
{
|
||
if (CurvaturePreviewSeconds <= 0.0 ||
|
||
MaximumCurvaturePreviewMeters <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var previewSpeedMetersPerSecond =
|
||
vehicleState.HasValidVelocityEstimate
|
||
? CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
vehicleState)
|
||
: Math.Abs(
|
||
controlReferenceSpeedMetersPerSecond);
|
||
|
||
return Math.Min(
|
||
MaximumCurvaturePreviewMeters,
|
||
previewSpeedMetersPerSecond *
|
||
CurvaturePreviewSeconds);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 沿轨迹实际执行点序向前采样专供横向几何前馈使用的曲率。
|
||
/// </summary>
|
||
private double ResolveFeedforwardCurvaturePerMeter(
|
||
TrajectoryProjection projection,
|
||
double previewDistanceMeters)
|
||
{
|
||
var previewArcLengthMeters = Math.Min(
|
||
_trajectory.TotalLengthMeters,
|
||
projection.ArcLengthMeters +
|
||
previewDistanceMeters);
|
||
|
||
return _trajectory
|
||
.SampleAtArcLength(previewArcLengthMeters)
|
||
.CurvaturePerMeter;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。
|
||
/// </summary>
|
||
private double ResolveTerminalApproachSpeed(
|
||
VehicleState vehicleState)
|
||
{
|
||
var distanceToEndMeters =
|
||
CalculateDistanceToEndMeters(
|
||
vehicleState);
|
||
var headingErrorToEndRadians =
|
||
CalculateHeadingErrorToEndRadians(
|
||
vehicleState);
|
||
|
||
// 一旦位置和航向已经进入完成容差，先要求纵向停车；
|
||
// 后续周期在实际速度也满足条件后完成轨迹。
|
||
if (distanceToEndMeters <=
|
||
FinishDistanceMeters &&
|
||
headingErrorToEndRadians <=
|
||
FinishHeadingToleranceRadians)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var endPose =
|
||
_trajectory.EndPoint.PoseInWorld;
|
||
var deltaX =
|
||
endPose.XMeters -
|
||
vehicleState.PoseInWorld.XMeters;
|
||
var deltaY =
|
||
endPose.YMeters -
|
||
vehicleState.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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。
|
||
/// </summary>
|
||
private double ResolveReferenceSpeedForControl(
|
||
TrajectoryProjection projection)
|
||
{
|
||
var currentReferenceSpeed =
|
||
projection.ReferencePoint
|
||
.ReferenceSpeedMetersPerSecond;
|
||
|
||
currentReferenceSpeed =
|
||
ApplyTerminalBrakingPreview(
|
||
projection,
|
||
currentReferenceSpeed);
|
||
|
||
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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 采用前方更低的同方向参考速度，使车辆在终点减速段提前制动。
|
||
/// </summary>
|
||
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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从轨迹终点前最后一个非零参考速度解析单向逼近允许的行驶方向。
|
||
/// </summary>
|
||
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));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
|
||
/// </summary>
|
||
private bool HasReachedEnd(
|
||
VehicleState vehicleState,
|
||
TrajectoryProjection projection)
|
||
{
|
||
if (!vehicleState.HasValidVelocityEstimate)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return projection.RemainingDistanceMeters <=
|
||
FinishDistanceMeters &&
|
||
CalculateDistanceToEndMeters(
|
||
vehicleState) <=
|
||
FinishDistanceMeters &&
|
||
CalculateHeadingErrorToEndRadians(
|
||
vehicleState) <=
|
||
FinishHeadingToleranceRadians &&
|
||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
vehicleState) <=
|
||
FinishSpeedMetersPerSecond;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。
|
||
/// </summary>
|
||
private bool HasStoppedAtUnsatisfiedTerminal(
|
||
VehicleState vehicleState,
|
||
TrajectoryProjection projection,
|
||
out string failureReason)
|
||
{
|
||
failureReason = string.Empty;
|
||
|
||
var isTerminalZeroSpeedReference =
|
||
projection.RemainingDistanceMeters <=
|
||
FinishDistanceMeters &&
|
||
Math.Abs(
|
||
projection.ReferencePoint
|
||
.ReferenceSpeedMetersPerSecond) <=
|
||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||
|
||
if (!isTerminalZeroSpeedReference ||
|
||
!vehicleState.HasValidVelocityEstimate ||
|
||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
vehicleState) >
|
||
FinishSpeedMetersPerSecond)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var positionErrorMeters =
|
||
CalculateDistanceToEndMeters(
|
||
vehicleState);
|
||
var headingErrorRadians =
|
||
CalculateHeadingErrorToEndRadians(
|
||
vehicleState);
|
||
|
||
failureReason =
|
||
"车辆已在终点零速参考处停稳，但终点精度不满足要求：" +
|
||
$"位置误差={positionErrorMeters:F3}m，" +
|
||
"航向误差=" +
|
||
$"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。
|
||
/// </summary>
|
||
private double CalculateDistanceToEndMeters(
|
||
VehicleState vehicleState)
|
||
{
|
||
var endPoint = _trajectory.EndPoint.PoseInWorld;
|
||
var deltaX =
|
||
vehicleState.PoseInWorld.XMeters -
|
||
endPoint.XMeters;
|
||
var deltaY =
|
||
vehicleState.PoseInWorld.YMeters -
|
||
endPoint.YMeters;
|
||
|
||
return Math.Sqrt(
|
||
deltaX * deltaX +
|
||
deltaY * deltaY);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。
|
||
/// </summary>
|
||
private double CalculateHeadingErrorToEndRadians(
|
||
VehicleState vehicleState)
|
||
{
|
||
return Math.Abs(
|
||
AngleMath.ShortestDifferenceRadians(
|
||
_trajectory.EndPoint
|
||
.PoseInWorld.YawRadians,
|
||
vehicleState
|
||
.PoseInWorld.YawRadians));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算车辆沿当前运动坐标系X轴实际速度的绝对值，单位为m/s。
|
||
/// </summary>
|
||
private double CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
VehicleState vehicleState)
|
||
{
|
||
return Math.Abs(
|
||
Math.Cos(_motionDirectionInBodyRadians) *
|
||
vehicleState.TwistInBody.VxMetersPerSecond +
|
||
Math.Sin(_motionDirectionInBodyRadians) *
|
||
vehicleState.TwistInBody.VyMetersPerSecond);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
|
||
/// </summary>
|
||
private void StopForUnavailableState()
|
||
{
|
||
_commandExecutor.Stop();
|
||
_lateralController.Reset();
|
||
_longitudinalController.Reset();
|
||
LastRequestedCommand = null;
|
||
LastCommand = null;
|
||
LastFailureReason =
|
||
"当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
|
||
LastException = null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
|
||
/// </summary>
|
||
private void CompleteTrajectory()
|
||
{
|
||
StopAndResetControllers();
|
||
IsActive = false;
|
||
IsCompleted = true;
|
||
LastRequestedCommand = new GcpMotionCommand(
|
||
0.0,
|
||
0.0,
|
||
0.0);
|
||
LastCommand = new GcpMotionCommand(
|
||
0.0,
|
||
0.0,
|
||
0.0);
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。
|
||
/// </summary>
|
||
private ParkingControlCycleResult EnterFault(
|
||
string reason,
|
||
Exception exception = null)
|
||
{
|
||
StopAndResetControllers();
|
||
IsActive = false;
|
||
IsCompleted = false;
|
||
LastRequestedCommand = null;
|
||
LastCommand = null;
|
||
LastFailureReason = reason;
|
||
LastException = exception;
|
||
return ParkingControlCycleResult.Faulted;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 立即停止底盘并清除横向和纵向控制器的跨周期状态。
|
||
/// </summary>
|
||
private void StopAndResetControllers()
|
||
{
|
||
_commandExecutor.Stop();
|
||
_lateralController.Reset();
|
||
_longitudinalController.Reset();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除上一条轨迹留下的状态、命令和故障诊断信息。
|
||
/// </summary>
|
||
private void ClearDiagnostics()
|
||
{
|
||
_cycleIndex = 0;
|
||
_hasPreviousStateTimestamp = false;
|
||
_previousStateTimestampSeconds = 0.0;
|
||
LastVehicleState = null;
|
||
LastProjection = null;
|
||
LastRequestedCommand = null;
|
||
LastCommand = null;
|
||
LastControlReferenceSpeedMetersPerSecond = null;
|
||
LastCurvaturePreviewDistanceMeters = null;
|
||
LastFeedforwardCurvaturePerMeter = null;
|
||
LastCycleTiming = null;
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将Stopwatch高精度时间戳差转换为毫秒。
|
||
/// </summary>
|
||
private static double GetElapsedMilliseconds(
|
||
long startTimestamp)
|
||
{
|
||
return (Stopwatch.GetTimestamp() - startTimestamp) *
|
||
1000.0 /
|
||
Stopwatch.Frequency;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查控制参数是否为正有限值。
|
||
/// </summary>
|
||
private static void EnsureFinitePositive(
|
||
double value,
|
||
string parameterName)
|
||
{
|
||
if (double.IsNaN(value) ||
|
||
double.IsInfinity(value) ||
|
||
value <= 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
parameterName,
|
||
"轨迹控制器距离和周期参数必须是正有限值。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查控制参数是否为非负有限值。
|
||
/// </summary>
|
||
private static void EnsureFiniteNonNegative(
|
||
double value,
|
||
string parameterName)
|
||
{
|
||
if (double.IsNaN(value) ||
|
||
double.IsInfinity(value) ||
|
||
value < 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
parameterName,
|
||
"轨迹控制器速度参数必须是非负有限值。");
|
||
}
|
||
}
|
||
}
|
||
}
|