621 lines
22 KiB
C#
621 lines
22 KiB
C#
using System;
|
||
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>
|
||
/// 组织状态读取、轨迹投影、横纵向控制、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 readonly IVehicleStateProvider _stateProvider;
|
||
private readonly ILateralController _lateralController;
|
||
private readonly ILongitudinalController _longitudinalController;
|
||
private readonly GcpCommandAllocator _gcpAllocator;
|
||
private readonly GcpCommandExecutor _commandExecutor;
|
||
|
||
private Trajectory2D _trajectory;
|
||
|
||
/// <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)
|
||
{
|
||
_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));
|
||
|
||
FinishDistanceMeters = finishDistanceMeters;
|
||
FinishSpeedMetersPerSecond =
|
||
finishSpeedMetersPerSecond;
|
||
FinishHeadingToleranceRadians =
|
||
finishHeadingToleranceRadians;
|
||
MaximumDistanceToTrajectoryMeters =
|
||
maximumDistanceToTrajectoryMeters;
|
||
TerminalBrakingPreviewMeters =
|
||
terminalBrakingPreviewMeters;
|
||
}
|
||
|
||
/// <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>
|
||
/// 获取控制器当前是否持有并正在执行一条轨迹。
|
||
/// </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? LastCommand { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
|
||
/// </summary>
|
||
public double? LastReferenceSpeedMetersPerSecond { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 停止当前底盘并从起点开始执行指定二维轨迹。
|
||
/// </summary>
|
||
public void Start(Trajectory2D trajectory)
|
||
{
|
||
if (trajectory == null)
|
||
{
|
||
throw new ArgumentNullException(
|
||
nameof(trajectory));
|
||
}
|
||
|
||
StopAndResetControllers();
|
||
_trajectory = trajectory;
|
||
IsActive = true;
|
||
IsCompleted = false;
|
||
ClearDiagnostics();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
|
||
/// </summary>
|
||
public ParkingControlCycleResult ExecuteCycle(
|
||
double deltaTimeSeconds)
|
||
{
|
||
EnsureFinitePositive(
|
||
deltaTimeSeconds,
|
||
nameof(deltaTimeSeconds));
|
||
|
||
if (!IsActive || _trajectory == null)
|
||
{
|
||
return ParkingControlCycleResult.Inactive;
|
||
}
|
||
|
||
try
|
||
{
|
||
if (!_stateProvider.TryGetState(
|
||
out var vehicleState))
|
||
{
|
||
StopForUnavailableState();
|
||
return ParkingControlCycleResult
|
||
.StateUnavailable;
|
||
}
|
||
|
||
LastVehicleState = vehicleState;
|
||
|
||
var projection = TrajectoryProjector.Project(
|
||
_trajectory,
|
||
vehicleState.PoseInWorld);
|
||
LastProjection = projection;
|
||
|
||
if (projection.DistanceToTrajectoryMeters >
|
||
MaximumDistanceToTrajectoryMeters)
|
||
{
|
||
return EnterFault(
|
||
"车辆距离参考轨迹" +
|
||
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
|
||
"超过允许值" +
|
||
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
|
||
}
|
||
|
||
if (HasReachedEnd(
|
||
vehicleState,
|
||
projection))
|
||
{
|
||
CompleteTrajectory();
|
||
return ParkingControlCycleResult.Completed;
|
||
}
|
||
|
||
if (HasStoppedAtUnsatisfiedTerminal(
|
||
vehicleState,
|
||
projection,
|
||
out var terminalFailureReason))
|
||
{
|
||
return EnterFault(
|
||
terminalFailureReason);
|
||
}
|
||
|
||
var referenceSpeedMetersPerSecond =
|
||
ResolveReferenceSpeedForControl(
|
||
projection);
|
||
LastReferenceSpeedMetersPerSecond =
|
||
referenceSpeedMetersPerSecond;
|
||
var context = new PathTrackingContext(
|
||
vehicleState,
|
||
projection,
|
||
referenceSpeedMetersPerSecond,
|
||
deltaTimeSeconds);
|
||
var lateralCommand =
|
||
_lateralController.Compute(context);
|
||
var commandSpeedMetersPerSecond =
|
||
_longitudinalController
|
||
.ComputeSpeedMetersPerSecond(context);
|
||
var gcpCommand = _gcpAllocator.Allocate(
|
||
commandSpeedMetersPerSecond,
|
||
lateralCommand);
|
||
|
||
if (!_commandExecutor.Execute(
|
||
gcpCommand,
|
||
deltaTimeSeconds))
|
||
{
|
||
return EnterFault(
|
||
string.IsNullOrWhiteSpace(
|
||
_commandExecutor.LastFailureReason)
|
||
? "GCP底盘命令执行失败。"
|
||
: _commandExecutor.LastFailureReason);
|
||
}
|
||
|
||
LastCommand =
|
||
_commandExecutor.LastSentCommand;
|
||
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
return ParkingControlCycleResult.CommandSent;
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
return EnterFault(
|
||
"停车机器人轨迹控制周期异常：" +
|
||
exception.Message,
|
||
exception);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 主动取消当前轨迹、立即停车并清除全部控制器状态。
|
||
/// </summary>
|
||
public void Cancel()
|
||
{
|
||
StopAndResetControllers();
|
||
_trajectory = null;
|
||
IsActive = false;
|
||
IsCompleted = false;
|
||
ClearDiagnostics();
|
||
}
|
||
|
||
/// <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 bool HasReachedEnd(
|
||
VehicleState vehicleState,
|
||
TrajectoryProjection projection)
|
||
{
|
||
if (!vehicleState.HasValidVelocityEstimate)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return projection.RemainingDistanceMeters <=
|
||
FinishDistanceMeters &&
|
||
CalculateDistanceToEndMeters(
|
||
vehicleState) <=
|
||
FinishDistanceMeters &&
|
||
CalculateHeadingErrorToEndRadians(
|
||
vehicleState) <=
|
||
FinishHeadingToleranceRadians &&
|
||
CalculateActualLinearSpeedMetersPerSecond(
|
||
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 ||
|
||
CalculateActualLinearSpeedMetersPerSecond(
|
||
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>
|
||
/// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
|
||
/// </summary>
|
||
private static double CalculateActualLinearSpeedMetersPerSecond(
|
||
VehicleState vehicleState)
|
||
{
|
||
return Math.Sqrt(
|
||
vehicleState.TwistInBody.VxMetersPerSecond *
|
||
vehicleState.TwistInBody.VxMetersPerSecond +
|
||
vehicleState.TwistInBody.VyMetersPerSecond *
|
||
vehicleState.TwistInBody.VyMetersPerSecond);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
|
||
/// </summary>
|
||
private void StopForUnavailableState()
|
||
{
|
||
_commandExecutor.Stop();
|
||
_lateralController.Reset();
|
||
_longitudinalController.Reset();
|
||
LastCommand = null;
|
||
LastFailureReason =
|
||
"当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
|
||
LastException = null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
|
||
/// </summary>
|
||
private void CompleteTrajectory()
|
||
{
|
||
StopAndResetControllers();
|
||
IsActive = false;
|
||
IsCompleted = true;
|
||
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;
|
||
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()
|
||
{
|
||
LastVehicleState = null;
|
||
LastProjection = null;
|
||
LastCommand = null;
|
||
LastReferenceSpeedMetersPerSecond = null;
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
}
|
||
|
||
/// <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,
|
||
"轨迹控制器速度参数必须是非负有限值。");
|
||
}
|
||
}
|
||
}
|
||
}
|