514 lines
18 KiB
C#
514 lines
18 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.Trajectory;
|
|
using MyParking.Shared;
|
|
// 负责运动中：每周期计算每辆车的速度命令
|
|
namespace MultiWheelC.Fleet
|
|
{
|
|
// 主车单周期车队协调结果，不表示通信或成员底盘执行结果。
|
|
public enum FleetCoordinationCycleResult
|
|
{
|
|
Inactive = 0,
|
|
WaitingForState = 1,
|
|
CommandGenerated = 2,
|
|
Completed = 3,
|
|
Faulted = 4
|
|
}
|
|
|
|
// 保存本周期的状态估计、车队命令和成员基础命令。
|
|
public sealed class FleetCoordinationCycleOutput
|
|
{
|
|
internal FleetCoordinationCycleOutput(
|
|
FleetState? state,
|
|
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
|
FleetMotionCommand fleetCommand,
|
|
IReadOnlyList<FleetMemberCommand> baseMemberCommands,
|
|
IReadOnlyList<FleetMemberCommand> memberCommands,
|
|
double speedScale,
|
|
string reason)
|
|
{
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
speedScale,
|
|
nameof(speedScale));
|
|
if (speedScale > 1.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(speedScale),
|
|
"车队统一速度比例不能大于1。");
|
|
}
|
|
|
|
State = state;
|
|
MemberErrors = memberErrors ??
|
|
throw new ArgumentNullException(
|
|
nameof(memberErrors));
|
|
FleetCommand = fleetCommand;
|
|
BaseMemberCommands = baseMemberCommands ??
|
|
throw new ArgumentNullException(
|
|
nameof(baseMemberCommands));
|
|
MemberCommands = memberCommands ??
|
|
throw new ArgumentNullException(
|
|
nameof(memberCommands));
|
|
SpeedScale = speedScale;
|
|
Reason = reason ?? string.Empty;
|
|
}
|
|
|
|
public FleetState? State { get; }
|
|
|
|
public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }
|
|
|
|
public FleetMotionCommand FleetCommand { get; }
|
|
|
|
// 仅由车队刚体命令分解得到，尚未叠加成员相对布局纠偏。
|
|
public IReadOnlyList<FleetMemberCommand> BaseMemberCommands { get; }
|
|
|
|
// 已转换到成员当前车体系并叠加小范围相对布局纠偏的最终命令。
|
|
public IReadOnlyList<FleetMemberCommand> MemberCommands { get; }
|
|
|
|
// 因成员布局误差施加到Vx、Vy和Omega的统一比例，范围为[0,1]。
|
|
public double SpeedScale { get; }
|
|
|
|
public string Reason { get; }
|
|
}
|
|
|
|
// 在主车上串联车队状态估计、中心轨迹控制和成员命令分解。
|
|
public sealed class FleetCoordinator
|
|
{
|
|
private static readonly IReadOnlyList<FleetMemberLayoutError>
|
|
EmptyMemberErrors = Array.AsReadOnly(
|
|
Array.Empty<FleetMemberLayoutError>());
|
|
private static readonly IReadOnlyList<FleetMemberCommand>
|
|
EmptyMemberCommands = Array.AsReadOnly(
|
|
Array.Empty<FleetMemberCommand>());
|
|
|
|
private readonly FleetStateEstimator _stateEstimator;
|
|
private readonly FleetController _fleetController;
|
|
private readonly FleetMemberCommandCorrector
|
|
_memberCommandCorrector;
|
|
private readonly double _memberPositionErrorWarningMeters;
|
|
private readonly double _maximumMemberPositionErrorMeters;
|
|
private readonly double _memberYawErrorWarningRadians;
|
|
private readonly double _maximumMemberYawErrorRadians;
|
|
|
|
private FleetLayout _activeLayout;
|
|
private bool _isCompleted;
|
|
private bool _isFaulted;
|
|
|
|
public FleetCoordinator(
|
|
FleetStateEstimator stateEstimator,
|
|
FleetController fleetController,
|
|
FleetMemberCommandCorrector memberCommandCorrector,
|
|
double memberPositionErrorWarningMeters,
|
|
double maximumMemberPositionErrorMeters,
|
|
double memberYawErrorWarningRadians,
|
|
double maximumMemberYawErrorRadians)
|
|
{
|
|
_stateEstimator = stateEstimator ??
|
|
throw new ArgumentNullException(
|
|
nameof(stateEstimator));
|
|
_fleetController = fleetController ??
|
|
throw new ArgumentNullException(
|
|
nameof(fleetController));
|
|
_memberCommandCorrector =
|
|
memberCommandCorrector ??
|
|
throw new ArgumentNullException(
|
|
nameof(memberCommandCorrector));
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
memberPositionErrorWarningMeters,
|
|
nameof(memberPositionErrorWarningMeters));
|
|
NumericGuard.EnsureFinitePositive(
|
|
maximumMemberPositionErrorMeters,
|
|
nameof(maximumMemberPositionErrorMeters));
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
memberYawErrorWarningRadians,
|
|
nameof(memberYawErrorWarningRadians));
|
|
NumericGuard.EnsureFinitePositive(
|
|
maximumMemberYawErrorRadians,
|
|
nameof(maximumMemberYawErrorRadians));
|
|
|
|
if (memberPositionErrorWarningMeters >=
|
|
maximumMemberPositionErrorMeters)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(memberPositionErrorWarningMeters),
|
|
"成员位置误差警告阈值必须小于停止阈值。");
|
|
}
|
|
|
|
if (memberYawErrorWarningRadians >=
|
|
maximumMemberYawErrorRadians)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(memberYawErrorWarningRadians),
|
|
"成员航向误差警告阈值必须小于停止阈值。");
|
|
}
|
|
|
|
if (maximumMemberYawErrorRadians > Math.PI)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(maximumMemberYawErrorRadians),
|
|
"成员航向误差上限不能大于π。");
|
|
}
|
|
|
|
_memberPositionErrorWarningMeters =
|
|
memberPositionErrorWarningMeters;
|
|
_maximumMemberPositionErrorMeters =
|
|
maximumMemberPositionErrorMeters;
|
|
_memberYawErrorWarningRadians =
|
|
memberYawErrorWarningRadians;
|
|
_maximumMemberYawErrorRadians =
|
|
maximumMemberYawErrorRadians;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
public FleetLayout ActiveLayout => _activeLayout;
|
|
|
|
public bool IsActive =>
|
|
_activeLayout != null &&
|
|
!_isCompleted &&
|
|
!_isFaulted &&
|
|
_fleetController.IsActive;
|
|
|
|
public bool IsCompleted => _isCompleted;
|
|
|
|
public bool IsFaulted => _isFaulted;
|
|
|
|
public string LastFailureReason { get; private set; }
|
|
|
|
// 运动前成员β准备必须与车队控制器采用同一个车队运动方向。
|
|
public double MotionDirectionInFleetRadians =>
|
|
_fleetController.MotionDirectionInFleetRadians;
|
|
|
|
public void Start(
|
|
FleetLayout layout,
|
|
Trajectory2D trajectory)
|
|
{
|
|
if (layout == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(layout));
|
|
}
|
|
|
|
if (trajectory == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(trajectory));
|
|
}
|
|
|
|
_fleetController.Start(trajectory);
|
|
_activeLayout = layout;
|
|
_isCompleted = false;
|
|
_isFaulted = false;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
public FleetCoordinationCycleResult ExecuteCycle(
|
|
IReadOnlyList<FleetMemberStateSample> memberStates,
|
|
double targetTimestampSeconds,
|
|
double deltaTimeSeconds,
|
|
out FleetCoordinationCycleOutput output)
|
|
{
|
|
if (memberStates == null)
|
|
{
|
|
throw new ArgumentNullException(nameof(memberStates));
|
|
}
|
|
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
targetTimestampSeconds,
|
|
nameof(targetTimestampSeconds));
|
|
NumericGuard.EnsureFinitePositive(
|
|
deltaTimeSeconds,
|
|
nameof(deltaTimeSeconds));
|
|
|
|
if (_activeLayout == null)
|
|
{
|
|
output = CreateOutput(
|
|
null,
|
|
EmptyMemberErrors,
|
|
EmptyMemberCommands,
|
|
"车队布局尚未激活。");
|
|
return FleetCoordinationCycleResult.Inactive;
|
|
}
|
|
|
|
if (_isFaulted)
|
|
{
|
|
output = CreateStopOutput(
|
|
null,
|
|
EmptyMemberErrors,
|
|
LastFailureReason);
|
|
return FleetCoordinationCycleResult.Faulted;
|
|
}
|
|
|
|
if (_isCompleted)
|
|
{
|
|
output = CreateStopOutput(
|
|
null,
|
|
EmptyMemberErrors,
|
|
string.Empty);
|
|
return FleetCoordinationCycleResult.Completed;
|
|
}
|
|
|
|
if (!_fleetController.IsActive)
|
|
{
|
|
output = CreateStopOutput(
|
|
null,
|
|
EmptyMemberErrors,
|
|
"车队中心控制器尚未启动或已经取消。");
|
|
return FleetCoordinationCycleResult.Inactive;
|
|
}
|
|
|
|
var estimate = _stateEstimator.Estimate(
|
|
_activeLayout,
|
|
memberStates,
|
|
targetTimestampSeconds);
|
|
if (!estimate.IsAvailable ||
|
|
!estimate.State.HasValue)
|
|
{
|
|
output = CreateStopOutput(
|
|
null,
|
|
estimate.MemberErrors,
|
|
estimate.UnavailableReason);
|
|
return FleetCoordinationCycleResult.WaitingForState;
|
|
}
|
|
|
|
var state = estimate.State.Value;
|
|
var speedScale = CalculateLayoutSpeedScale(
|
|
estimate.MemberErrors,
|
|
out var layoutFailureReason,
|
|
out var limitingReason);
|
|
if (layoutFailureReason != null)
|
|
{
|
|
return Fail(
|
|
state,
|
|
estimate.MemberErrors,
|
|
layoutFailureReason,
|
|
out output);
|
|
}
|
|
|
|
var controlResult =
|
|
_fleetController.ComputeCommand(
|
|
state,
|
|
deltaTimeSeconds,
|
|
out var fleetCommand);
|
|
|
|
if (controlResult ==
|
|
FleetControlCycleResult.CommandGenerated)
|
|
{
|
|
var scaledFleetCommand = ScaleFleetCommand(
|
|
fleetCommand,
|
|
speedScale);
|
|
var baseMemberCommands =
|
|
FleetKinematics.Decompose(
|
|
_activeLayout,
|
|
scaledFleetCommand);
|
|
var memberCommands =
|
|
_memberCommandCorrector.Correct(
|
|
_activeLayout,
|
|
baseMemberCommands,
|
|
estimate.MemberErrors,
|
|
applyRelativeCorrection:
|
|
speedScale >= 1.0);
|
|
output = new FleetCoordinationCycleOutput(
|
|
state,
|
|
estimate.MemberErrors,
|
|
scaledFleetCommand,
|
|
baseMemberCommands,
|
|
memberCommands,
|
|
speedScale,
|
|
limitingReason);
|
|
return FleetCoordinationCycleResult.CommandGenerated;
|
|
}
|
|
|
|
if (controlResult ==
|
|
FleetControlCycleResult.Completed)
|
|
{
|
|
_isCompleted = true;
|
|
output = CreateStopOutput(
|
|
state,
|
|
estimate.MemberErrors,
|
|
string.Empty);
|
|
return FleetCoordinationCycleResult.Completed;
|
|
}
|
|
|
|
if (controlResult ==
|
|
FleetControlCycleResult.Faulted)
|
|
{
|
|
var reason = string.IsNullOrWhiteSpace(
|
|
_fleetController.LastFailureReason)
|
|
? "车队中心轨迹控制失败。"
|
|
: _fleetController.LastFailureReason;
|
|
return Fail(
|
|
state,
|
|
estimate.MemberErrors,
|
|
reason,
|
|
out output,
|
|
cancelController: false);
|
|
}
|
|
|
|
output = CreateStopOutput(
|
|
state,
|
|
estimate.MemberErrors,
|
|
"车队中心控制器当前未生成命令。");
|
|
return FleetCoordinationCycleResult.Inactive;
|
|
}
|
|
|
|
public void Cancel()
|
|
{
|
|
_fleetController.Cancel();
|
|
_isCompleted = false;
|
|
_isFaulted = false;
|
|
LastFailureReason = string.Empty;
|
|
}
|
|
|
|
private double CalculateLayoutSpeedScale(
|
|
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
|
out string failureReason,
|
|
out string limitingReason)
|
|
{
|
|
var speedScale = 1.0;
|
|
failureReason = null;
|
|
limitingReason = string.Empty;
|
|
|
|
for (var index = 0;
|
|
index < memberErrors.Count;
|
|
index++)
|
|
{
|
|
var memberError = memberErrors[index];
|
|
var poseError =
|
|
memberError.ActualPoseInExpectedVehicleFrame;
|
|
var positionErrorMeters = Math.Sqrt(
|
|
poseError.XMeters * poseError.XMeters +
|
|
poseError.YMeters * poseError.YMeters);
|
|
var yawErrorRadians =
|
|
Math.Abs(poseError.YawRadians);
|
|
|
|
if (positionErrorMeters >=
|
|
_maximumMemberPositionErrorMeters)
|
|
{
|
|
failureReason =
|
|
$"车辆{memberError.VehicleId}相对布局位置误差" +
|
|
$"{positionErrorMeters:F3}m达到停止阈值。";
|
|
return 0.0;
|
|
}
|
|
|
|
if (yawErrorRadians >=
|
|
_maximumMemberYawErrorRadians)
|
|
{
|
|
failureReason =
|
|
$"车辆{memberError.VehicleId}相对布局航向误差" +
|
|
$"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°" +
|
|
"达到停止阈值。";
|
|
return 0.0;
|
|
}
|
|
|
|
var positionScale = CalculateScale(
|
|
positionErrorMeters,
|
|
_memberPositionErrorWarningMeters,
|
|
_maximumMemberPositionErrorMeters);
|
|
if (positionScale < speedScale)
|
|
{
|
|
speedScale = positionScale;
|
|
limitingReason =
|
|
$"车辆{memberError.VehicleId}相对布局位置误差" +
|
|
$"{positionErrorMeters:F3}m，车队统一速度比例" +
|
|
$"降至{speedScale:F3}。";
|
|
}
|
|
|
|
var yawScale = CalculateScale(
|
|
yawErrorRadians,
|
|
_memberYawErrorWarningRadians,
|
|
_maximumMemberYawErrorRadians);
|
|
if (yawScale < speedScale)
|
|
{
|
|
speedScale = yawScale;
|
|
limitingReason =
|
|
$"车辆{memberError.VehicleId}相对布局航向误差" +
|
|
$"{AngleMath.RadiansToDegrees(yawErrorRadians):F2}°，" +
|
|
$"车队统一速度比例降至{speedScale:F3}。";
|
|
}
|
|
}
|
|
|
|
return speedScale;
|
|
}
|
|
|
|
private static double CalculateScale(
|
|
double errorMagnitude,
|
|
double warningThreshold,
|
|
double stopThreshold)
|
|
{
|
|
if (errorMagnitude <= warningThreshold)
|
|
{
|
|
return 1.0;
|
|
}
|
|
|
|
return (stopThreshold - errorMagnitude) /
|
|
(stopThreshold - warningThreshold);
|
|
}
|
|
|
|
private static FleetMotionCommand ScaleFleetCommand(
|
|
FleetMotionCommand command,
|
|
double speedScale)
|
|
{
|
|
var twist = command.TwistAtReferencePoint;
|
|
return new FleetMotionCommand(
|
|
command.ReferencePointInFleet,
|
|
new Twist2D(
|
|
twist.VxMetersPerSecond * speedScale,
|
|
twist.VyMetersPerSecond * speedScale,
|
|
twist.OmegaRadiansPerSecond * speedScale));
|
|
}
|
|
|
|
private FleetCoordinationCycleResult Fail(
|
|
FleetState state,
|
|
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
|
string reason,
|
|
out FleetCoordinationCycleOutput output,
|
|
bool cancelController = true)
|
|
{
|
|
if (cancelController)
|
|
{
|
|
_fleetController.Cancel();
|
|
}
|
|
|
|
_isFaulted = true;
|
|
_isCompleted = false;
|
|
LastFailureReason = reason ?? string.Empty;
|
|
output = CreateStopOutput(
|
|
state,
|
|
memberErrors,
|
|
LastFailureReason);
|
|
return FleetCoordinationCycleResult.Faulted;
|
|
}
|
|
|
|
private FleetCoordinationCycleOutput CreateStopOutput(
|
|
FleetState? state,
|
|
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
|
string reason)
|
|
{
|
|
var stopCommands = _activeLayout == null
|
|
? EmptyMemberCommands
|
|
: FleetKinematics.Decompose(
|
|
_activeLayout,
|
|
FleetMotionCommand.Stop());
|
|
|
|
return CreateOutput(
|
|
state,
|
|
memberErrors,
|
|
stopCommands,
|
|
reason);
|
|
}
|
|
|
|
private static FleetCoordinationCycleOutput CreateOutput(
|
|
FleetState? state,
|
|
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
|
IReadOnlyList<FleetMemberCommand> memberCommands,
|
|
string reason)
|
|
{
|
|
return new FleetCoordinationCycleOutput(
|
|
state,
|
|
memberErrors,
|
|
FleetMotionCommand.Stop(),
|
|
memberCommands,
|
|
memberCommands,
|
|
0.0,
|
|
reason);
|
|
}
|
|
}
|
|
}
|