完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -1 +1,509 @@
|
||||
// 只在主车激活，完成车队轨迹控制和命令分配
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user