完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -49,6 +49,12 @@ public partial class PilotConfig
|
||||
[FieldMember(desc = "停车控制：Detour自动坐标连续化最大航向变化(deg)")]
|
||||
public float ParkingDetourMaximumAutomaticHeadingShiftDegrees = 5f;
|
||||
|
||||
[FieldMember(desc = "停车控制：Detour缓存帧最大允许时间(s)")]
|
||||
public float ParkingDetourMaximumCachedFrameAgeSeconds = 0.50f;
|
||||
|
||||
[FieldMember(desc = "停车控制：Detour定位质量失效/恢复确认新帧数")]
|
||||
public int ParkingDetourLocalizationQualityConfirmationFrames = 3;
|
||||
|
||||
[FieldMember(desc = "停车控制：Detour线速度滤波时间常数(s)")]
|
||||
public float ParkingDetourLinearVelocityFilterSeconds = 0.15f;
|
||||
|
||||
|
||||
@@ -37,14 +37,21 @@ namespace MultiWheelC.Fleet
|
||||
double terminalApproachGainPerSecond = 0.8,
|
||||
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
|
||||
double curvaturePreviewSeconds = 0.20,
|
||||
double maximumCurvaturePreviewMeters = 0.12)
|
||||
double maximumCurvaturePreviewMeters = 0.12,
|
||||
double motionDirectionInFleetRadians = 0.0)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
virtualControlPointRadiusMeters,
|
||||
nameof(virtualControlPointRadiusMeters));
|
||||
NumericGuard.EnsureFinite(
|
||||
motionDirectionInFleetRadians,
|
||||
nameof(motionDirectionInFleetRadians));
|
||||
|
||||
VirtualControlPointRadiusMeters =
|
||||
virtualControlPointRadiusMeters;
|
||||
MotionDirectionInFleetRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
motionDirectionInFleetRadians);
|
||||
_trackingCore = new PathTrackingCore(
|
||||
lateralController,
|
||||
longitudinalController,
|
||||
@@ -58,12 +65,16 @@ namespace MultiWheelC.Fleet
|
||||
terminalApproachGainPerSecond,
|
||||
maximumTerminalApproachSpeedMetersPerSecond,
|
||||
curvaturePreviewSeconds,
|
||||
maximumCurvaturePreviewMeters);
|
||||
maximumCurvaturePreviewMeters,
|
||||
MotionDirectionInFleetRadians);
|
||||
}
|
||||
|
||||
// 虚拟车队中心到前、后GCP的距离，单位为m。
|
||||
public double VirtualControlPointRadiusMeters { get; }
|
||||
|
||||
// 当前运动坐标系+X轴相对车队坐标系+X轴的方向，单位为rad。
|
||||
public double MotionDirectionInFleetRadians { get; }
|
||||
|
||||
public double FinishDistanceMeters =>
|
||||
_trackingCore.FinishDistanceMeters;
|
||||
|
||||
@@ -169,13 +180,20 @@ namespace MultiWheelC.Fleet
|
||||
|
||||
try
|
||||
{
|
||||
var twistAtFleetOrigin =
|
||||
var twistAtFleetOriginInMotionFrame =
|
||||
GcpKinematics.ToBodyTwist(
|
||||
output.Command.Value,
|
||||
VirtualControlPointRadiusMeters);
|
||||
var twistAtFleetOriginInFleet =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionDirectionInFleetRadians),
|
||||
twistAtFleetOriginInMotionFrame);
|
||||
command = new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
twistAtFleetOrigin);
|
||||
twistAtFleetOriginInFleet);
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.CommandGenerated;
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,181 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MyParking.Shared;
|
||||
// 夹紧后只执行一次 → 建立固定布局
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 建立编队时使用的一辆成员车世界位姿快照。
|
||||
public readonly struct FleetMemberPose
|
||||
{
|
||||
public FleetMemberPose(
|
||||
int vehicleId,
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"编队成员车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
|
||||
VehicleId = vehicleId;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public Pose2D PoseInWorld { get; }
|
||||
}
|
||||
|
||||
// 保存布局建立时的车队世界位姿和固定成员布局。
|
||||
public readonly struct FleetLayoutCaptureResult
|
||||
{
|
||||
public FleetLayoutCaptureResult(
|
||||
Pose2D fleetPoseInWorld,
|
||||
FleetLayout layout)
|
||||
{
|
||||
NumericGuard.EnsureFinite(
|
||||
fleetPoseInWorld,
|
||||
nameof(fleetPoseInWorld));
|
||||
|
||||
FleetPoseInWorld = new Pose2D(
|
||||
fleetPoseInWorld.XMeters,
|
||||
fleetPoseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
fleetPoseInWorld.YawRadians));
|
||||
Layout = layout ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(layout));
|
||||
}
|
||||
|
||||
public Pose2D FleetPoseInWorld { get; }
|
||||
|
||||
public FleetLayout Layout { get; }
|
||||
}
|
||||
|
||||
// 根据同一世界坐标系中的成员位姿建立车队几何中心和固定布局。
|
||||
public static class FleetLayoutCapture
|
||||
{
|
||||
public static FleetLayoutCaptureResult Capture(
|
||||
IReadOnlyList<FleetMemberPose> memberPoses,
|
||||
int leaderVehicleId)
|
||||
{
|
||||
if (memberPoses == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(memberPoses));
|
||||
}
|
||||
|
||||
if (memberPoses.Count == 0)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"建立编队布局至少需要一辆成员车。",
|
||||
nameof(memberPoses));
|
||||
}
|
||||
|
||||
if (leaderVehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(leaderVehicleId),
|
||||
"主车车号必须大于零。");
|
||||
}
|
||||
|
||||
var vehicleIds = new HashSet<int>();
|
||||
var centerXMeters = 0.0;
|
||||
var centerYMeters = 0.0;
|
||||
var leaderFound = false;
|
||||
var leaderYawRadians = 0.0;
|
||||
|
||||
for (var index = 0;
|
||||
index < memberPoses.Count;
|
||||
index++)
|
||||
{
|
||||
var memberPose = memberPoses[index];
|
||||
if (memberPose.VehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(memberPoses),
|
||||
$"第{index}辆成员车的车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
memberPose.PoseInWorld,
|
||||
$"{nameof(memberPoses)}[{index}]." +
|
||||
nameof(FleetMemberPose.PoseInWorld));
|
||||
|
||||
if (!vehicleIds.Add(memberPose.VehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"成员位姿包含重复车号{memberPose.VehicleId}。",
|
||||
nameof(memberPoses));
|
||||
}
|
||||
|
||||
centerXMeters +=
|
||||
memberPose.PoseInWorld.XMeters;
|
||||
centerYMeters +=
|
||||
memberPose.PoseInWorld.YMeters;
|
||||
|
||||
if (memberPose.VehicleId == leaderVehicleId)
|
||||
{
|
||||
leaderFound = true;
|
||||
leaderYawRadians =
|
||||
memberPose.PoseInWorld.YawRadians;
|
||||
}
|
||||
}
|
||||
|
||||
if (!leaderFound)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"成员位姿中不存在主车{leaderVehicleId}。",
|
||||
nameof(leaderVehicleId));
|
||||
}
|
||||
|
||||
centerXMeters /= memberPoses.Count;
|
||||
centerYMeters /= memberPoses.Count;
|
||||
NumericGuard.EnsureFinite(
|
||||
centerXMeters,
|
||||
nameof(centerXMeters));
|
||||
NumericGuard.EnsureFinite(
|
||||
centerYMeters,
|
||||
nameof(centerYMeters));
|
||||
|
||||
var fleetPoseInWorld = new Pose2D(
|
||||
centerXMeters,
|
||||
centerYMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
leaderYawRadians));
|
||||
var worldPoseInFleet =
|
||||
FrameTransform2D.Inverse(
|
||||
fleetPoseInWorld);
|
||||
var vehicleLayouts =
|
||||
new VehicleLayout[memberPoses.Count];
|
||||
|
||||
for (var index = 0;
|
||||
index < memberPoses.Count;
|
||||
index++)
|
||||
{
|
||||
var memberPose = memberPoses[index];
|
||||
var poseInFleet =
|
||||
FrameTransform2D.Compose(
|
||||
worldPoseInFleet,
|
||||
memberPose.PoseInWorld);
|
||||
vehicleLayouts[index] =
|
||||
new VehicleLayout(
|
||||
memberPose.VehicleId,
|
||||
poseInFleet);
|
||||
}
|
||||
|
||||
return new FleetLayoutCaptureResult(
|
||||
fleetPoseInWorld,
|
||||
new FleetLayout(vehicleLayouts));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1 +1,456 @@
|
||||
// 主车、从车都有，负责执行分配给本车的命令
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
/// <summary>表示成员车在一次车队动作中的本地执行阶段。</summary>
|
||||
public enum FleetMemberAgentState
|
||||
{
|
||||
Idle = 0,
|
||||
Preparing = 1,
|
||||
Ready = 2,
|
||||
Active = 3,
|
||||
Faulted = 4
|
||||
}
|
||||
|
||||
/// <summary>区分固定β滚动运动和车辆中心纯自转的准备方式。</summary>
|
||||
public enum FleetMemberPreparationMode
|
||||
{
|
||||
Rolling = 0,
|
||||
Spin = 1
|
||||
}
|
||||
|
||||
/// <summary>负责一辆成员车的舵轮准备、激活和本车速度命令执行。</summary>
|
||||
public sealed class FleetMemberAgent
|
||||
{
|
||||
private const double MotionDeadband = 1e-6;
|
||||
|
||||
private readonly MultiWheelChassisAdapter _adapter;
|
||||
private readonly double _alignmentToleranceRadians;
|
||||
private readonly double _alignmentStableSeconds;
|
||||
|
||||
private double _alignedDurationSeconds;
|
||||
|
||||
/// <summary>创建绑定到一辆多舵轮底盘的成员车执行器。</summary>
|
||||
public FleetMemberAgent(
|
||||
MultiWheelChassisAdapter adapter,
|
||||
double alignmentToleranceRadians,
|
||||
double alignmentStableSeconds)
|
||||
{
|
||||
_adapter = adapter ??
|
||||
throw new ArgumentNullException(nameof(adapter));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
alignmentToleranceRadians,
|
||||
nameof(alignmentToleranceRadians));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
alignmentStableSeconds,
|
||||
nameof(alignmentStableSeconds));
|
||||
|
||||
if (alignmentToleranceRadians > Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(alignmentToleranceRadians),
|
||||
"舵轮到位容差不能大于π。");
|
||||
}
|
||||
|
||||
_alignmentToleranceRadians =
|
||||
alignmentToleranceRadians;
|
||||
_alignmentStableSeconds =
|
||||
alignmentStableSeconds;
|
||||
State = FleetMemberAgentState.Idle;
|
||||
LastFailureReason = string.Empty;
|
||||
}
|
||||
|
||||
public int VehicleId => _adapter.VehicleId;
|
||||
|
||||
public FleetMemberAgentState State { get; private set; }
|
||||
|
||||
public FleetMemberPreparationMode? PreparationMode
|
||||
{
|
||||
get;
|
||||
private set;
|
||||
}
|
||||
|
||||
public long CurrentPlanId { get; private set; }
|
||||
|
||||
public double MotionDirectionInBodyRadians
|
||||
{
|
||||
get;
|
||||
private set;
|
||||
}
|
||||
|
||||
public string LastFailureReason { get; private set; }
|
||||
|
||||
/// <summary>停车并开始准备本车固定β滚动运动系。</summary>
|
||||
public bool BeginRollingPreparation(
|
||||
long planId,
|
||||
double motionDirectionInBodyRadians)
|
||||
{
|
||||
ValidatePlanId(planId);
|
||||
NumericGuard.EnsureFinite(
|
||||
motionDirectionInBodyRadians,
|
||||
nameof(motionDirectionInBodyRadians));
|
||||
|
||||
return BeginPreparation(
|
||||
planId,
|
||||
FleetMemberPreparationMode.Rolling,
|
||||
AngleMath.NormalizeRadians(
|
||||
motionDirectionInBodyRadians));
|
||||
}
|
||||
|
||||
/// <summary>停车并开始准备车辆中心纯自转所需的舵轮方向。</summary>
|
||||
public bool BeginSpinPreparation(long planId)
|
||||
{
|
||||
ValidatePlanId(planId);
|
||||
return BeginPreparation(
|
||||
planId,
|
||||
FleetMemberPreparationMode.Spin,
|
||||
motionDirectionInBodyRadians: 0.0);
|
||||
}
|
||||
|
||||
/// <summary>检查舵轮是否已连续稳定到位；宿主应在准备阶段周期调用。</summary>
|
||||
public FleetMemberAgentState UpdatePreparation(
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
if (State != FleetMemberAgentState.Preparing)
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
bool aligned;
|
||||
try
|
||||
{
|
||||
aligned = UpdateAndCheckAlignment();
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
Fail(exception.Message);
|
||||
return State;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
Fail(exception.Message);
|
||||
return State;
|
||||
}
|
||||
|
||||
if (State == FleetMemberAgentState.Faulted)
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
_alignedDurationSeconds = aligned
|
||||
? _alignedDurationSeconds + deltaTimeSeconds
|
||||
: 0.0;
|
||||
|
||||
if (aligned &&
|
||||
_alignedDurationSeconds >=
|
||||
_alignmentStableSeconds)
|
||||
{
|
||||
State = FleetMemberAgentState.Ready;
|
||||
LastFailureReason = string.Empty;
|
||||
}
|
||||
|
||||
return State;
|
||||
}
|
||||
|
||||
/// <summary>在主车确认全队Ready后激活本车已经准备好的运动方式。</summary>
|
||||
public bool Activate(long planId)
|
||||
{
|
||||
if (planId != CurrentPlanId)
|
||||
{
|
||||
LastFailureReason =
|
||||
"激活任务编号与当前准备任务不一致。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (State == FleetMemberAgentState.Active)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (State != FleetMemberAgentState.Ready ||
|
||||
!PreparationMode.HasValue)
|
||||
{
|
||||
return RejectWhileStopped(
|
||||
"成员车尚未完成舵轮准备。");
|
||||
}
|
||||
|
||||
bool stillAligned;
|
||||
try
|
||||
{
|
||||
stillAligned = ArePreparedWheelsStillAligned();
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
|
||||
if (!stillAligned)
|
||||
{
|
||||
State = FleetMemberAgentState.Preparing;
|
||||
_alignedDurationSeconds = 0.0;
|
||||
return RejectWhileStopped(
|
||||
"成员车在激活前失去舵轮到位状态。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (PreparationMode.Value ==
|
||||
FleetMemberPreparationMode.Rolling)
|
||||
{
|
||||
_adapter.ActivateMotionFrame(
|
||||
MotionDirectionInBodyRadians);
|
||||
}
|
||||
else if (!_adapter.AdoptPreparedSpinForXYTh(
|
||||
_alignmentToleranceRadians))
|
||||
{
|
||||
return Fail(
|
||||
BuildAdapterFailureReason(
|
||||
"无法激活已经准备好的原地自转舵轮。"));
|
||||
}
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
|
||||
State = FleetMemberAgentState.Active;
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>校验任务和车号后执行分配给本车的车体系速度命令。</summary>
|
||||
public bool Execute(
|
||||
long planId,
|
||||
FleetMemberCommand command,
|
||||
TimeSpan? interval = null)
|
||||
{
|
||||
ValidatePlanId(planId);
|
||||
NumericGuard.EnsureFinite(
|
||||
command.TwistInVehicleBody,
|
||||
nameof(command));
|
||||
|
||||
if (planId != CurrentPlanId)
|
||||
{
|
||||
return Fail(
|
||||
"速度命令任务编号与当前激活任务不一致。");
|
||||
}
|
||||
|
||||
if (command.VehicleId != VehicleId)
|
||||
{
|
||||
return Fail(
|
||||
$"速度命令属于车辆{command.VehicleId}，" +
|
||||
$"当前成员车号为{VehicleId}。");
|
||||
}
|
||||
|
||||
if (State != FleetMemberAgentState.Active ||
|
||||
!PreparationMode.HasValue)
|
||||
{
|
||||
return RejectWhileStopped(
|
||||
"成员车尚未激活，不能执行速度命令。");
|
||||
}
|
||||
|
||||
if (!IsCommandCompatibleWithPreparation(
|
||||
command.TwistInVehicleBody))
|
||||
{
|
||||
return Fail(
|
||||
"速度命令与本次舵轮准备方式不一致。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (!_adapter.SendBodyTwist(
|
||||
command.TwistInVehicleBody,
|
||||
interval))
|
||||
{
|
||||
return Fail(
|
||||
BuildAdapterFailureReason(
|
||||
"成员车底盘拒绝执行速度命令。"));
|
||||
}
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>正常取消当前任务并立即停止驱动轮。</summary>
|
||||
public void Stop()
|
||||
{
|
||||
_adapter.StopImmediately();
|
||||
State = FleetMemberAgentState.Idle;
|
||||
PreparationMode = null;
|
||||
CurrentPlanId = 0;
|
||||
MotionDirectionInBodyRadians = 0.0;
|
||||
_alignedDurationSeconds = 0.0;
|
||||
LastFailureReason = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>重置上一动作并下发本次滚动或自转舵轮准备目标。</summary>
|
||||
private bool BeginPreparation(
|
||||
long planId,
|
||||
FleetMemberPreparationMode mode,
|
||||
double motionDirectionInBodyRadians)
|
||||
{
|
||||
try
|
||||
{
|
||||
_adapter.StopImmediately();
|
||||
_adapter.ResetToBodyFrame();
|
||||
|
||||
CurrentPlanId = planId;
|
||||
PreparationMode = mode;
|
||||
MotionDirectionInBodyRadians =
|
||||
motionDirectionInBodyRadians;
|
||||
State = FleetMemberAgentState.Preparing;
|
||||
LastFailureReason = string.Empty;
|
||||
_alignedDurationSeconds = 0.0;
|
||||
|
||||
var accepted = mode ==
|
||||
FleetMemberPreparationMode.Rolling
|
||||
? _adapter.PrepareParallelDirection(
|
||||
motionDirectionInBodyRadians)
|
||||
: _adapter.PrepareSpin(
|
||||
alignmentToleranceDegrees:
|
||||
AngleMath.RadiansToDegrees(
|
||||
_alignmentToleranceRadians));
|
||||
|
||||
if (!accepted)
|
||||
{
|
||||
return Fail(
|
||||
BuildAdapterFailureReason(
|
||||
"成员车底盘拒绝舵轮准备目标。"));
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
return Fail(exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>更新当前准备目标并读取舵轮到位状态。</summary>
|
||||
private bool UpdateAndCheckAlignment()
|
||||
{
|
||||
if (PreparationMode ==
|
||||
FleetMemberPreparationMode.Rolling)
|
||||
{
|
||||
return _adapter.AreParallelWheelsAligned(
|
||||
MotionDirectionInBodyRadians,
|
||||
_alignmentToleranceRadians);
|
||||
}
|
||||
|
||||
if (!_adapter.PrepareSpin(
|
||||
alignmentToleranceDegrees:
|
||||
AngleMath.RadiansToDegrees(
|
||||
_alignmentToleranceRadians)))
|
||||
{
|
||||
Fail(
|
||||
BuildAdapterFailureReason(
|
||||
"成员车底盘无法继续更新原地自转准备。"));
|
||||
return false;
|
||||
}
|
||||
|
||||
return _adapter.AreSpinWheelsAligned;
|
||||
}
|
||||
|
||||
/// <summary>确认舵轮在全队释放前仍保持到位。</summary>
|
||||
private bool ArePreparedWheelsStillAligned()
|
||||
{
|
||||
return PreparationMode ==
|
||||
FleetMemberPreparationMode.Rolling
|
||||
? _adapter.AreParallelWheelsAligned(
|
||||
MotionDirectionInBodyRadians,
|
||||
_alignmentToleranceRadians)
|
||||
: _adapter.AreSpinWheelsAligned;
|
||||
}
|
||||
|
||||
/// <summary>禁止滚动准备执行纯自转，也禁止自转准备执行平移。</summary>
|
||||
private bool IsCommandCompatibleWithPreparation(
|
||||
Twist2D bodyTwist)
|
||||
{
|
||||
var linearSpeed = Math.Sqrt(
|
||||
bodyTwist.VxMetersPerSecond *
|
||||
bodyTwist.VxMetersPerSecond +
|
||||
bodyTwist.VyMetersPerSecond *
|
||||
bodyTwist.VyMetersPerSecond);
|
||||
var hasLinearMotion =
|
||||
linearSpeed > MotionDeadband;
|
||||
var hasAngularMotion =
|
||||
Math.Abs(
|
||||
bodyTwist.OmegaRadiansPerSecond) >
|
||||
MotionDeadband;
|
||||
|
||||
if (!hasLinearMotion && !hasAngularMotion)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return PreparationMode ==
|
||||
FleetMemberPreparationMode.Rolling
|
||||
? hasLinearMotion
|
||||
: !hasLinearMotion && hasAngularMotion;
|
||||
}
|
||||
|
||||
/// <summary>拒绝未满足执行条件的命令并保持车辆零速。</summary>
|
||||
private bool RejectWhileStopped(string reason)
|
||||
{
|
||||
_adapter.StopImmediately();
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>锁存成员车故障并立即清零驱动轮速度。</summary>
|
||||
private bool Fail(string reason)
|
||||
{
|
||||
_adapter.StopImmediately();
|
||||
State = FleetMemberAgentState.Faulted;
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>优先返回底盘提供的具体失败原因。</summary>
|
||||
private string BuildAdapterFailureReason(
|
||||
string fallbackReason)
|
||||
{
|
||||
return string.IsNullOrWhiteSpace(
|
||||
_adapter.LastFailureReason)
|
||||
? fallbackReason
|
||||
: _adapter.LastFailureReason;
|
||||
}
|
||||
|
||||
/// <summary>拒绝零值和负值任务编号。</summary>
|
||||
private static void ValidatePlanId(long planId)
|
||||
{
|
||||
if (planId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(planId),
|
||||
"车队动作任务编号必须大于零。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,433 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 将小范围成员布局误差转换为不改变车队整体刚体运动的相对速度修正。
|
||||
public sealed class FleetMemberCommandCorrector
|
||||
{
|
||||
private const double GeometryTolerance = 1e-12;
|
||||
|
||||
private readonly double _longitudinalPositionGainPerSecond;
|
||||
private readonly double _lateralPositionGainPerSecond;
|
||||
private readonly double _yawGainPerSecond;
|
||||
private readonly double _positionErrorDeadbandMeters;
|
||||
private readonly double _yawErrorDeadbandRadians;
|
||||
private readonly double _maximumLinearCorrectionMetersPerSecond;
|
||||
private readonly double _maximumAngularCorrectionRadiansPerSecond;
|
||||
|
||||
public FleetMemberCommandCorrector(
|
||||
double longitudinalPositionGainPerSecond,
|
||||
double lateralPositionGainPerSecond,
|
||||
double yawGainPerSecond,
|
||||
double positionErrorDeadbandMeters,
|
||||
double yawErrorDeadbandRadians,
|
||||
double maximumLinearCorrectionMetersPerSecond,
|
||||
double maximumAngularCorrectionRadiansPerSecond)
|
||||
{
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
longitudinalPositionGainPerSecond,
|
||||
nameof(longitudinalPositionGainPerSecond));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
lateralPositionGainPerSecond,
|
||||
nameof(lateralPositionGainPerSecond));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
yawGainPerSecond,
|
||||
nameof(yawGainPerSecond));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
positionErrorDeadbandMeters,
|
||||
nameof(positionErrorDeadbandMeters));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
yawErrorDeadbandRadians,
|
||||
nameof(yawErrorDeadbandRadians));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumLinearCorrectionMetersPerSecond,
|
||||
nameof(maximumLinearCorrectionMetersPerSecond));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumAngularCorrectionRadiansPerSecond,
|
||||
nameof(maximumAngularCorrectionRadiansPerSecond));
|
||||
|
||||
if (yawErrorDeadbandRadians > Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(yawErrorDeadbandRadians),
|
||||
"成员航向误差死区不能大于π。");
|
||||
}
|
||||
|
||||
_longitudinalPositionGainPerSecond =
|
||||
longitudinalPositionGainPerSecond;
|
||||
_lateralPositionGainPerSecond =
|
||||
lateralPositionGainPerSecond;
|
||||
_yawGainPerSecond = yawGainPerSecond;
|
||||
_positionErrorDeadbandMeters =
|
||||
positionErrorDeadbandMeters;
|
||||
_yawErrorDeadbandRadians =
|
||||
yawErrorDeadbandRadians;
|
||||
_maximumLinearCorrectionMetersPerSecond =
|
||||
maximumLinearCorrectionMetersPerSecond;
|
||||
_maximumAngularCorrectionRadiansPerSecond =
|
||||
maximumAngularCorrectionRadiansPerSecond;
|
||||
}
|
||||
|
||||
public IReadOnlyList<FleetMemberCommand> Correct(
|
||||
FleetLayout layout,
|
||||
IReadOnlyList<FleetMemberCommand> baseCommands,
|
||||
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
||||
bool applyRelativeCorrection = true)
|
||||
{
|
||||
if (layout == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(layout));
|
||||
}
|
||||
|
||||
if (baseCommands == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(baseCommands));
|
||||
}
|
||||
|
||||
if (memberErrors == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(memberErrors));
|
||||
}
|
||||
|
||||
if (baseCommands.Count != layout.VehicleCount)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"成员基础命令数量必须与车队布局一致。",
|
||||
nameof(baseCommands));
|
||||
}
|
||||
|
||||
if (memberErrors.Count != layout.VehicleCount)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"成员布局误差数量必须与车队布局一致。",
|
||||
nameof(memberErrors));
|
||||
}
|
||||
|
||||
var commandsByVehicleId =
|
||||
IndexCommands(baseCommands);
|
||||
var errorsByVehicleId =
|
||||
IndexErrors(memberErrors);
|
||||
var orderedCommands =
|
||||
new FleetMemberCommand[layout.VehicleCount];
|
||||
var orderedErrors =
|
||||
new FleetMemberLayoutError[layout.VehicleCount];
|
||||
var rawCorrectionsInFleet =
|
||||
new Twist2D[layout.VehicleCount];
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicleLayout = layout.Vehicles[index];
|
||||
if (!commandsByVehicleId.TryGetValue(
|
||||
vehicleLayout.VehicleId,
|
||||
out var baseCommand))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"缺少车辆{vehicleLayout.VehicleId}的基础命令。",
|
||||
nameof(baseCommands));
|
||||
}
|
||||
|
||||
if (!errorsByVehicleId.TryGetValue(
|
||||
vehicleLayout.VehicleId,
|
||||
out var memberError))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"缺少车辆{vehicleLayout.VehicleId}的布局误差。",
|
||||
nameof(memberErrors));
|
||||
}
|
||||
|
||||
orderedCommands[index] = baseCommand;
|
||||
orderedErrors[index] = memberError;
|
||||
rawCorrectionsInFleet[index] =
|
||||
applyRelativeCorrection
|
||||
? CalculateRawCorrectionInFleet(
|
||||
vehicleLayout,
|
||||
memberError)
|
||||
: Twist2D.Zero;
|
||||
}
|
||||
|
||||
var relativeCorrectionsInFleet =
|
||||
RemoveCommonRigidMotion(
|
||||
layout,
|
||||
rawCorrectionsInFleet);
|
||||
var correctedCommands =
|
||||
new FleetMemberCommand[layout.VehicleCount];
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicleLayout = layout.Vehicles[index];
|
||||
var baseTwistInFleet =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
vehicleLayout.PoseInFleet,
|
||||
orderedCommands[index].TwistInVehicleBody);
|
||||
var correctionInFleet = LimitCorrection(
|
||||
relativeCorrectionsInFleet[index]);
|
||||
var correctedTwistInFleet = Add(
|
||||
baseTwistInFleet,
|
||||
correctionInFleet);
|
||||
|
||||
// 使用成员当前相对姿态表达最终命令，避免小航向误差造成坐标表达偏差。
|
||||
var actualPoseInFleet =
|
||||
FrameTransform2D.Compose(
|
||||
vehicleLayout.PoseInFleet,
|
||||
orderedErrors[index]
|
||||
.ActualPoseInExpectedVehicleFrame);
|
||||
var fleetPoseInActualVehicle =
|
||||
FrameTransform2D.Inverse(
|
||||
actualPoseInFleet);
|
||||
var correctedTwistInVehicleBody =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
fleetPoseInActualVehicle,
|
||||
correctedTwistInFleet);
|
||||
|
||||
correctedCommands[index] =
|
||||
new FleetMemberCommand(
|
||||
vehicleLayout.VehicleId,
|
||||
correctedTwistInVehicleBody);
|
||||
}
|
||||
|
||||
return Array.AsReadOnly(correctedCommands);
|
||||
}
|
||||
|
||||
private Twist2D CalculateRawCorrectionInFleet(
|
||||
VehicleLayout vehicleLayout,
|
||||
FleetMemberLayoutError memberError)
|
||||
{
|
||||
var error =
|
||||
memberError.ActualPoseInExpectedVehicleFrame;
|
||||
var correctionInExpectedVehicle = new Twist2D(
|
||||
-_longitudinalPositionGainPerSecond *
|
||||
ApplyDeadband(
|
||||
error.XMeters,
|
||||
_positionErrorDeadbandMeters),
|
||||
-_lateralPositionGainPerSecond *
|
||||
ApplyDeadband(
|
||||
error.YMeters,
|
||||
_positionErrorDeadbandMeters),
|
||||
-_yawGainPerSecond *
|
||||
ApplyDeadband(
|
||||
error.YawRadians,
|
||||
_yawErrorDeadbandRadians));
|
||||
|
||||
return FrameTransform2D.TransformTwistAtSamePoint(
|
||||
vehicleLayout.PoseInFleet,
|
||||
correctionInExpectedVehicle);
|
||||
}
|
||||
|
||||
private static Twist2D[] RemoveCommonRigidMotion(
|
||||
FleetLayout layout,
|
||||
IReadOnlyList<Twist2D> rawCorrectionsInFleet)
|
||||
{
|
||||
var count = layout.VehicleCount;
|
||||
var meanX = 0.0;
|
||||
var meanY = 0.0;
|
||||
var meanVx = 0.0;
|
||||
var meanVy = 0.0;
|
||||
var meanOmega = 0.0;
|
||||
|
||||
for (var index = 0; index < count; index++)
|
||||
{
|
||||
var position = layout.Vehicles[index].PoseInFleet;
|
||||
var correction = rawCorrectionsInFleet[index];
|
||||
meanX += position.XMeters;
|
||||
meanY += position.YMeters;
|
||||
meanVx += correction.VxMetersPerSecond;
|
||||
meanVy += correction.VyMetersPerSecond;
|
||||
meanOmega += correction.OmegaRadiansPerSecond;
|
||||
}
|
||||
|
||||
meanX /= count;
|
||||
meanY /= count;
|
||||
meanVx /= count;
|
||||
meanVy /= count;
|
||||
meanOmega /= count;
|
||||
|
||||
var rotationalNumerator = 0.0;
|
||||
var rotationalDenominator = 0.0;
|
||||
for (var index = 0; index < count; index++)
|
||||
{
|
||||
var position = layout.Vehicles[index].PoseInFleet;
|
||||
var correction = rawCorrectionsInFleet[index];
|
||||
var centeredX = position.XMeters - meanX;
|
||||
var centeredY = position.YMeters - meanY;
|
||||
var centeredVx =
|
||||
correction.VxMetersPerSecond - meanVx;
|
||||
var centeredVy =
|
||||
correction.VyMetersPerSecond - meanVy;
|
||||
|
||||
rotationalNumerator +=
|
||||
-centeredY * centeredVx +
|
||||
centeredX * centeredVy;
|
||||
rotationalDenominator +=
|
||||
centeredX * centeredX +
|
||||
centeredY * centeredY;
|
||||
}
|
||||
|
||||
var commonOmegaFromTranslation =
|
||||
rotationalDenominator <= GeometryTolerance
|
||||
? 0.0
|
||||
: rotationalNumerator /
|
||||
rotationalDenominator;
|
||||
var commonVxAtFleetOrigin =
|
||||
meanVx +
|
||||
commonOmegaFromTranslation * meanY;
|
||||
var commonVyAtFleetOrigin =
|
||||
meanVy -
|
||||
commonOmegaFromTranslation * meanX;
|
||||
var relativeCorrections = new Twist2D[count];
|
||||
|
||||
for (var index = 0; index < count; index++)
|
||||
{
|
||||
var position = layout.Vehicles[index].PoseInFleet;
|
||||
var correction = rawCorrectionsInFleet[index];
|
||||
var commonVxAtMember =
|
||||
commonVxAtFleetOrigin -
|
||||
commonOmegaFromTranslation *
|
||||
position.YMeters;
|
||||
var commonVyAtMember =
|
||||
commonVyAtFleetOrigin +
|
||||
commonOmegaFromTranslation *
|
||||
position.XMeters;
|
||||
|
||||
relativeCorrections[index] = new Twist2D(
|
||||
correction.VxMetersPerSecond -
|
||||
commonVxAtMember,
|
||||
correction.VyMetersPerSecond -
|
||||
commonVyAtMember,
|
||||
correction.OmegaRadiansPerSecond -
|
||||
meanOmega);
|
||||
}
|
||||
|
||||
return relativeCorrections;
|
||||
}
|
||||
|
||||
private Twist2D LimitCorrection(Twist2D correction)
|
||||
{
|
||||
var linearMagnitude = Math.Sqrt(
|
||||
correction.VxMetersPerSecond *
|
||||
correction.VxMetersPerSecond +
|
||||
correction.VyMetersPerSecond *
|
||||
correction.VyMetersPerSecond);
|
||||
var linearScale =
|
||||
linearMagnitude <=
|
||||
_maximumLinearCorrectionMetersPerSecond
|
||||
? 1.0
|
||||
: _maximumLinearCorrectionMetersPerSecond /
|
||||
linearMagnitude;
|
||||
var limitedOmega = Math.Max(
|
||||
-_maximumAngularCorrectionRadiansPerSecond,
|
||||
Math.Min(
|
||||
_maximumAngularCorrectionRadiansPerSecond,
|
||||
correction.OmegaRadiansPerSecond));
|
||||
|
||||
return new Twist2D(
|
||||
correction.VxMetersPerSecond * linearScale,
|
||||
correction.VyMetersPerSecond * linearScale,
|
||||
limitedOmega);
|
||||
}
|
||||
|
||||
private static Dictionary<int, FleetMemberCommand>
|
||||
IndexCommands(
|
||||
IReadOnlyList<FleetMemberCommand> commands)
|
||||
{
|
||||
var indexed =
|
||||
new Dictionary<int, FleetMemberCommand>(
|
||||
commands.Count);
|
||||
|
||||
for (var index = 0; index < commands.Count; index++)
|
||||
{
|
||||
var command = commands[index];
|
||||
if (command.VehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(commands),
|
||||
$"第{index}个成员命令的车号无效。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
command.TwistInVehicleBody,
|
||||
$"{nameof(commands)}[{index}]." +
|
||||
nameof(FleetMemberCommand.TwistInVehicleBody));
|
||||
|
||||
if (indexed.ContainsKey(command.VehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"成员命令包含重复车号{command.VehicleId}。",
|
||||
nameof(commands));
|
||||
}
|
||||
|
||||
indexed.Add(command.VehicleId, command);
|
||||
}
|
||||
|
||||
return indexed;
|
||||
}
|
||||
|
||||
private static Dictionary<int, FleetMemberLayoutError>
|
||||
IndexErrors(
|
||||
IReadOnlyList<FleetMemberLayoutError> errors)
|
||||
{
|
||||
var indexed =
|
||||
new Dictionary<int, FleetMemberLayoutError>(
|
||||
errors.Count);
|
||||
|
||||
for (var index = 0; index < errors.Count; index++)
|
||||
{
|
||||
var error = errors[index];
|
||||
if (error.VehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(errors),
|
||||
$"第{index}个布局误差的车号无效。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
error.ActualPoseInExpectedVehicleFrame,
|
||||
$"{nameof(errors)}[{index}]." +
|
||||
nameof(FleetMemberLayoutError
|
||||
.ActualPoseInExpectedVehicleFrame));
|
||||
|
||||
if (indexed.ContainsKey(error.VehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"成员布局误差包含重复车号{error.VehicleId}。",
|
||||
nameof(errors));
|
||||
}
|
||||
|
||||
indexed.Add(error.VehicleId, error);
|
||||
}
|
||||
|
||||
return indexed;
|
||||
}
|
||||
|
||||
private static double ApplyDeadband(
|
||||
double value,
|
||||
double deadband)
|
||||
{
|
||||
var magnitude = Math.Abs(value);
|
||||
if (magnitude <= deadband)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
return Math.Sign(value) * (magnitude - deadband);
|
||||
}
|
||||
|
||||
private static Twist2D Add(
|
||||
Twist2D first,
|
||||
Twist2D second)
|
||||
{
|
||||
return new Twist2D(
|
||||
first.VxMetersPerSecond +
|
||||
second.VxMetersPerSecond,
|
||||
first.VyMetersPerSecond +
|
||||
second.VyMetersPerSecond,
|
||||
first.OmegaRadiansPerSecond +
|
||||
second.OmegaRadiansPerSecond);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,372 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
/// <summary>表示主车侧车队运动准备的当前阶段。</summary>
|
||||
public enum FleetPreparationCoordinatorState
|
||||
{
|
||||
Idle = 0,
|
||||
WaitingForMembers = 1,
|
||||
ReadyToActivate = 2,
|
||||
ActivationAuthorized = 3,
|
||||
Faulted = 4
|
||||
}
|
||||
|
||||
/// <summary>保存一次滚动准备中分配给指定成员车的本地β目标。</summary>
|
||||
public readonly struct FleetMemberPreparationTarget
|
||||
{
|
||||
/// <summary>创建一条属于指定任务和成员车的准备目标。</summary>
|
||||
public FleetMemberPreparationTarget(
|
||||
long planId,
|
||||
int vehicleId,
|
||||
double motionDirectionInBodyRadians)
|
||||
{
|
||||
if (planId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(planId),
|
||||
"车队动作任务编号必须大于零。");
|
||||
}
|
||||
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"成员车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
motionDirectionInBodyRadians,
|
||||
nameof(motionDirectionInBodyRadians));
|
||||
|
||||
PlanId = planId;
|
||||
VehicleId = vehicleId;
|
||||
MotionDirectionInBodyRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
motionDirectionInBodyRadians);
|
||||
}
|
||||
|
||||
public long PlanId { get; }
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public double MotionDirectionInBodyRadians { get; }
|
||||
}
|
||||
|
||||
/// <summary>保存成员车对某次准备任务上报的本地状态。</summary>
|
||||
public readonly struct FleetMemberPreparationStatus
|
||||
{
|
||||
/// <summary>创建一条成员车准备状态报告。</summary>
|
||||
public FleetMemberPreparationStatus(
|
||||
long planId,
|
||||
int vehicleId,
|
||||
FleetMemberAgentState state,
|
||||
string failureReason = "")
|
||||
{
|
||||
if (planId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(planId),
|
||||
"车队动作任务编号必须大于零。");
|
||||
}
|
||||
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"成员车号必须大于零。");
|
||||
}
|
||||
|
||||
if (!Enum.IsDefined(
|
||||
typeof(FleetMemberAgentState),
|
||||
state))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(state),
|
||||
"成员车准备状态无效。");
|
||||
}
|
||||
|
||||
PlanId = planId;
|
||||
VehicleId = vehicleId;
|
||||
State = state;
|
||||
FailureReason = failureReason ?? string.Empty;
|
||||
}
|
||||
|
||||
public long PlanId { get; }
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public FleetMemberAgentState State { get; }
|
||||
|
||||
public string FailureReason { get; }
|
||||
}
|
||||
|
||||
/// <summary>在主车侧分配成员β并管理全队Ready统一激活屏障。</summary>
|
||||
public sealed class FleetPreparationCoordinator
|
||||
{
|
||||
private static readonly IReadOnlyList<
|
||||
FleetMemberPreparationTarget>
|
||||
EmptyTargets = Array.AsReadOnly(
|
||||
Array.Empty<FleetMemberPreparationTarget>());
|
||||
|
||||
private readonly Dictionary<int, FleetMemberAgentState>
|
||||
_memberStates =
|
||||
new Dictionary<int, FleetMemberAgentState>();
|
||||
|
||||
private IReadOnlyList<FleetMemberPreparationTarget>
|
||||
_targets = EmptyTargets;
|
||||
|
||||
/// <summary>创建尚未激活准备任务的主车侧协调器。</summary>
|
||||
public FleetPreparationCoordinator()
|
||||
{
|
||||
State = FleetPreparationCoordinatorState.Idle;
|
||||
LastFailureReason = string.Empty;
|
||||
}
|
||||
|
||||
public FleetPreparationCoordinatorState State
|
||||
{
|
||||
get;
|
||||
private set;
|
||||
}
|
||||
|
||||
public long CurrentPlanId { get; private set; }
|
||||
|
||||
public double MotionDirectionInFleetRadians
|
||||
{
|
||||
get;
|
||||
private set;
|
||||
}
|
||||
|
||||
public IReadOnlyList<FleetMemberPreparationTarget>
|
||||
Targets => _targets;
|
||||
|
||||
public string LastFailureReason { get; private set; }
|
||||
|
||||
/// <summary>根据车队固定布局为全部成员建立本次滚动β准备目标。</summary>
|
||||
public void StartRollingPreparation(
|
||||
long planId,
|
||||
FleetLayout layout,
|
||||
double motionDirectionInFleetRadians)
|
||||
{
|
||||
if (planId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(planId),
|
||||
"车队动作任务编号必须大于零。");
|
||||
}
|
||||
|
||||
if (layout == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(layout));
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
motionDirectionInFleetRadians,
|
||||
nameof(motionDirectionInFleetRadians));
|
||||
|
||||
var normalizedFleetDirection =
|
||||
AngleMath.NormalizeRadians(
|
||||
motionDirectionInFleetRadians);
|
||||
var targets =
|
||||
new FleetMemberPreparationTarget[
|
||||
layout.VehicleCount];
|
||||
|
||||
_memberStates.Clear();
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicle = layout.Vehicles[index];
|
||||
var rawDirectionInBody =
|
||||
AngleMath.NormalizeRadians(
|
||||
normalizedFleetDirection -
|
||||
vehicle.PoseInFleet.YawRadians);
|
||||
var equivalentDirectionInBody =
|
||||
SelectSteeringAxisEquivalent(
|
||||
rawDirectionInBody);
|
||||
|
||||
targets[index] =
|
||||
new FleetMemberPreparationTarget(
|
||||
planId,
|
||||
vehicle.VehicleId,
|
||||
equivalentDirectionInBody);
|
||||
_memberStates.Add(
|
||||
vehicle.VehicleId,
|
||||
FleetMemberAgentState.Idle);
|
||||
}
|
||||
|
||||
CurrentPlanId = planId;
|
||||
MotionDirectionInFleetRadians =
|
||||
normalizedFleetDirection;
|
||||
_targets = Array.AsReadOnly(targets);
|
||||
State =
|
||||
FleetPreparationCoordinatorState
|
||||
.WaitingForMembers;
|
||||
LastFailureReason = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>接收一辆成员车的状态并重新计算全队Ready状态。</summary>
|
||||
public FleetPreparationCoordinatorState
|
||||
ReportMemberStatus(
|
||||
FleetMemberPreparationStatus status)
|
||||
{
|
||||
if (State ==
|
||||
FleetPreparationCoordinatorState.Idle ||
|
||||
State ==
|
||||
FleetPreparationCoordinatorState.Faulted)
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
if (status.PlanId != CurrentPlanId)
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
if (!_memberStates.ContainsKey(status.VehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"车辆{status.VehicleId}不属于当前车队布局。",
|
||||
nameof(status));
|
||||
}
|
||||
|
||||
if (status.State ==
|
||||
FleetMemberAgentState.Faulted)
|
||||
{
|
||||
return Fail(
|
||||
string.IsNullOrWhiteSpace(
|
||||
status.FailureReason)
|
||||
? $"车辆{status.VehicleId}准备失败。"
|
||||
: $"车辆{status.VehicleId}准备失败：" +
|
||||
status.FailureReason);
|
||||
}
|
||||
|
||||
if (State ==
|
||||
FleetPreparationCoordinatorState
|
||||
.ActivationAuthorized)
|
||||
{
|
||||
return State;
|
||||
}
|
||||
|
||||
if (status.State ==
|
||||
FleetMemberAgentState.Active)
|
||||
{
|
||||
return Fail(
|
||||
$"车辆{status.VehicleId}在全队统一激活前已经进入Active。");
|
||||
}
|
||||
|
||||
_memberStates[status.VehicleId] = status.State;
|
||||
State = AreAllMembersReady()
|
||||
? FleetPreparationCoordinatorState
|
||||
.ReadyToActivate
|
||||
: FleetPreparationCoordinatorState
|
||||
.WaitingForMembers;
|
||||
LastFailureReason = string.Empty;
|
||||
return State;
|
||||
}
|
||||
|
||||
/// <summary>在全部成员Ready后授权外层向全队广播同一任务的激活命令。</summary>
|
||||
public bool TryAuthorizeActivation(long planId)
|
||||
{
|
||||
if (planId != CurrentPlanId)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (State ==
|
||||
FleetPreparationCoordinatorState
|
||||
.ActivationAuthorized)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if (State !=
|
||||
FleetPreparationCoordinatorState
|
||||
.ReadyToActivate)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
State = FleetPreparationCoordinatorState
|
||||
.ActivationAuthorized;
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>查找指定成员车在当前任务中的本地β准备目标。</summary>
|
||||
public bool TryGetTarget(
|
||||
int vehicleId,
|
||||
out FleetMemberPreparationTarget target)
|
||||
{
|
||||
for (var index = 0;
|
||||
index < _targets.Count;
|
||||
index++)
|
||||
{
|
||||
if (_targets[index].VehicleId == vehicleId)
|
||||
{
|
||||
target = _targets[index];
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
target = default;
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>取消当前准备任务并清除成员状态和β目标。</summary>
|
||||
public void Cancel()
|
||||
{
|
||||
_memberStates.Clear();
|
||||
_targets = EmptyTargets;
|
||||
CurrentPlanId = 0;
|
||||
MotionDirectionInFleetRadians = 0.0;
|
||||
State = FleetPreparationCoordinatorState.Idle;
|
||||
LastFailureReason = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>判断当前任务中的每辆成员车是否都已报告Ready。</summary>
|
||||
private bool AreAllMembersReady()
|
||||
{
|
||||
foreach (var state in _memberStates.Values)
|
||||
{
|
||||
if (state != FleetMemberAgentState.Ready)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return _memberStates.Count > 0;
|
||||
}
|
||||
|
||||
/// <summary>将有向β转换为±90°内的等效滚动轴，反向运动由轮速符号表达。</summary>
|
||||
private static double SelectSteeringAxisEquivalent(
|
||||
double directionRadians)
|
||||
{
|
||||
var equivalent = AngleMath.NormalizeRadians(
|
||||
directionRadians);
|
||||
|
||||
if (equivalent > Math.PI / 2.0)
|
||||
{
|
||||
equivalent -= Math.PI;
|
||||
}
|
||||
else if (equivalent < -Math.PI / 2.0)
|
||||
{
|
||||
equivalent += Math.PI;
|
||||
}
|
||||
|
||||
return AngleMath.NormalizeRadians(equivalent);
|
||||
}
|
||||
|
||||
/// <summary>锁存准备故障，等待外层停止所有成员并取消任务。</summary>
|
||||
private FleetPreparationCoordinatorState Fail(
|
||||
string reason)
|
||||
{
|
||||
State = FleetPreparationCoordinatorState.Faulted;
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
return State;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1 +0,0 @@
|
||||
// 检查通信超时、成员故障、定位状态、夹臂状态和相对误差
|
||||
@@ -1 +1,579 @@
|
||||
// 主车汇总各车状态、时间对齐、计算车队中心和相对布局误差；输出FleetState
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 一辆成员车在主车统一时间轴上的状态样本。
|
||||
public readonly struct FleetMemberStateSample
|
||||
{
|
||||
public FleetMemberStateSample(
|
||||
int vehicleId,
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D poseInWorld,
|
||||
Twist2D twistAtVehicleOriginInWorld,
|
||||
bool isStateAvailable,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"编队成员车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
NumericGuard.EnsureFinite(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
NumericGuard.EnsureFinite(
|
||||
twistAtVehicleOriginInWorld,
|
||||
nameof(twistAtVehicleOriginInWorld));
|
||||
|
||||
VehicleId = vehicleId;
|
||||
SampleTimestampSeconds = sampleTimestampSeconds;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
TwistAtVehicleOriginInWorld =
|
||||
twistAtVehicleOriginInWorld;
|
||||
IsStateAvailable = isStateAvailable;
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
// 该时间戳必须已经换算到主车/协调器的单调时间轴。
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
// 成员车体中心处的实际速度，在世界坐标系中表达。
|
||||
public Twist2D TwistAtVehicleOriginInWorld { get; }
|
||||
|
||||
public bool IsStateAvailable { get; }
|
||||
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
}
|
||||
|
||||
// 成员实际位姿相对固定布局目标位姿的误差。
|
||||
public readonly struct FleetMemberLayoutError
|
||||
{
|
||||
public FleetMemberLayoutError(
|
||||
int vehicleId,
|
||||
Pose2D actualPoseInExpectedVehicleFrame)
|
||||
{
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
"编队成员车号必须大于零。");
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
actualPoseInExpectedVehicleFrame,
|
||||
nameof(actualPoseInExpectedVehicleFrame));
|
||||
|
||||
VehicleId = vehicleId;
|
||||
ActualPoseInExpectedVehicleFrame =
|
||||
new Pose2D(
|
||||
actualPoseInExpectedVehicleFrame.XMeters,
|
||||
actualPoseInExpectedVehicleFrame.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
actualPoseInExpectedVehicleFrame
|
||||
.YawRadians));
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
// 期望成员车体系中表达的实际成员位姿；理想刚体布局时为Identity。
|
||||
public Pose2D ActualPoseInExpectedVehicleFrame { get; }
|
||||
}
|
||||
|
||||
// 一次车队状态估计的结果；不可用时不提供FleetState。
|
||||
public sealed class FleetStateEstimateResult
|
||||
{
|
||||
private static readonly IReadOnlyList<FleetMemberLayoutError>
|
||||
EmptyMemberErrors = Array.AsReadOnly(
|
||||
Array.Empty<FleetMemberLayoutError>());
|
||||
|
||||
private FleetStateEstimateResult(
|
||||
bool isAvailable,
|
||||
FleetState? state,
|
||||
IReadOnlyList<FleetMemberLayoutError> memberErrors,
|
||||
string unavailableReason)
|
||||
{
|
||||
IsAvailable = isAvailable;
|
||||
State = state;
|
||||
MemberErrors = memberErrors;
|
||||
UnavailableReason = unavailableReason;
|
||||
}
|
||||
|
||||
public bool IsAvailable { get; }
|
||||
|
||||
public FleetState? State { get; }
|
||||
|
||||
public IReadOnlyList<FleetMemberLayoutError> MemberErrors { get; }
|
||||
|
||||
public string UnavailableReason { get; }
|
||||
|
||||
internal static FleetStateEstimateResult Available(
|
||||
FleetState state,
|
||||
FleetMemberLayoutError[] memberErrors)
|
||||
{
|
||||
return new FleetStateEstimateResult(
|
||||
true,
|
||||
state,
|
||||
Array.AsReadOnly(memberErrors),
|
||||
string.Empty);
|
||||
}
|
||||
|
||||
internal static FleetStateEstimateResult Unavailable(
|
||||
string reason)
|
||||
{
|
||||
return new FleetStateEstimateResult(
|
||||
false,
|
||||
null,
|
||||
EmptyMemberErrors,
|
||||
reason ?? string.Empty);
|
||||
}
|
||||
}
|
||||
|
||||
// 从各成员状态反算并融合车队虚拟中心状态。
|
||||
public sealed class FleetStateEstimator
|
||||
{
|
||||
private const double TimestampToleranceSeconds = 1e-9;
|
||||
private const double MinimumCircularMeanMagnitude = 1e-12;
|
||||
|
||||
private readonly double _maximumMemberStateAgeSeconds;
|
||||
private readonly double _maximumPositionDisagreementMeters;
|
||||
private readonly double _maximumYawDisagreementRadians;
|
||||
|
||||
public FleetStateEstimator(
|
||||
double maximumMemberStateAgeSeconds,
|
||||
double maximumPositionDisagreementMeters,
|
||||
double maximumYawDisagreementRadians)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumMemberStateAgeSeconds,
|
||||
nameof(maximumMemberStateAgeSeconds));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumPositionDisagreementMeters,
|
||||
nameof(maximumPositionDisagreementMeters));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumYawDisagreementRadians,
|
||||
nameof(maximumYawDisagreementRadians));
|
||||
|
||||
if (maximumYawDisagreementRadians > Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(maximumYawDisagreementRadians),
|
||||
"车队候选航向差阈值不能大于π。");
|
||||
}
|
||||
|
||||
_maximumMemberStateAgeSeconds =
|
||||
maximumMemberStateAgeSeconds;
|
||||
_maximumPositionDisagreementMeters =
|
||||
maximumPositionDisagreementMeters;
|
||||
_maximumYawDisagreementRadians =
|
||||
maximumYawDisagreementRadians;
|
||||
}
|
||||
|
||||
public FleetStateEstimateResult Estimate(
|
||||
FleetLayout layout,
|
||||
IReadOnlyList<FleetMemberStateSample> memberStates,
|
||||
double targetTimestampSeconds)
|
||||
{
|
||||
if (layout == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(layout));
|
||||
}
|
||||
|
||||
if (memberStates == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(memberStates));
|
||||
}
|
||||
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
targetTimestampSeconds,
|
||||
nameof(targetTimestampSeconds));
|
||||
|
||||
if (memberStates.Count != layout.VehicleCount)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"成员状态数量{memberStates.Count}与布局数量" +
|
||||
$"{layout.VehicleCount}不一致。");
|
||||
}
|
||||
|
||||
var statesByVehicleId =
|
||||
new Dictionary<int, FleetMemberStateSample>(
|
||||
memberStates.Count);
|
||||
|
||||
for (var index = 0;
|
||||
index < memberStates.Count;
|
||||
index++)
|
||||
{
|
||||
var memberState = memberStates[index];
|
||||
if (memberState.VehicleId <= 0)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"第{index}个成员状态的车号无效。");
|
||||
}
|
||||
|
||||
if (!layout.TryGetVehicle(
|
||||
memberState.VehicleId,
|
||||
out _))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"成员状态包含布局外车辆" +
|
||||
$"{memberState.VehicleId}。");
|
||||
}
|
||||
|
||||
if (statesByVehicleId.ContainsKey(
|
||||
memberState.VehicleId))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"成员状态包含重复车号" +
|
||||
$"{memberState.VehicleId}。");
|
||||
}
|
||||
|
||||
statesByVehicleId.Add(
|
||||
memberState.VehicleId,
|
||||
memberState);
|
||||
}
|
||||
|
||||
var alignedMembers =
|
||||
new AlignedMemberState[layout.VehicleCount];
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicleLayout = layout.Vehicles[index];
|
||||
if (!statesByVehicleId.TryGetValue(
|
||||
vehicleLayout.VehicleId,
|
||||
out var memberState))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
$"缺少车辆{vehicleLayout.VehicleId}的状态。");
|
||||
}
|
||||
|
||||
var alignmentResult = AlignMemberState(
|
||||
memberState,
|
||||
targetTimestampSeconds,
|
||||
out var alignedPoseInWorld);
|
||||
if (alignmentResult != null)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
alignmentResult);
|
||||
}
|
||||
|
||||
var candidateFleetPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
alignedPoseInWorld,
|
||||
FrameTransform2D.Inverse(
|
||||
vehicleLayout.PoseInFleet));
|
||||
|
||||
alignedMembers[index] =
|
||||
new AlignedMemberState(
|
||||
vehicleLayout,
|
||||
memberState,
|
||||
alignedPoseInWorld,
|
||||
candidateFleetPoseInWorld);
|
||||
}
|
||||
|
||||
var disagreementReason =
|
||||
FindCandidateDisagreement(alignedMembers);
|
||||
if (disagreementReason != null)
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
disagreementReason);
|
||||
}
|
||||
|
||||
if (!TryAverageCandidateFleetPose(
|
||||
alignedMembers,
|
||||
out var fleetPoseInWorld))
|
||||
{
|
||||
return FleetStateEstimateResult.Unavailable(
|
||||
"成员候选航向无法形成唯一的车队平均航向。");
|
||||
}
|
||||
|
||||
var hasValidVelocityEstimate =
|
||||
TryAverageFleetOriginTwist(
|
||||
alignedMembers,
|
||||
fleetPoseInWorld,
|
||||
out var twistAtFleetOriginInWorld);
|
||||
var fleetState = new FleetState(
|
||||
targetTimestampSeconds,
|
||||
fleetPoseInWorld,
|
||||
twistAtFleetOriginInWorld,
|
||||
hasValidVelocityEstimate);
|
||||
var memberErrors = CalculateMemberErrors(
|
||||
alignedMembers,
|
||||
fleetPoseInWorld);
|
||||
|
||||
return FleetStateEstimateResult.Available(
|
||||
fleetState,
|
||||
memberErrors);
|
||||
}
|
||||
|
||||
private string AlignMemberState(
|
||||
FleetMemberStateSample memberState,
|
||||
double targetTimestampSeconds,
|
||||
out Pose2D alignedPoseInWorld)
|
||||
{
|
||||
alignedPoseInWorld = memberState.PoseInWorld;
|
||||
|
||||
if (!memberState.IsStateAvailable)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}状态不可用。";
|
||||
}
|
||||
|
||||
var ageSeconds =
|
||||
targetTimestampSeconds -
|
||||
memberState.SampleTimestampSeconds;
|
||||
|
||||
if (ageSeconds < -TimestampToleranceSeconds)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}的状态时间晚于" +
|
||||
"本次估计目标时间。";
|
||||
}
|
||||
|
||||
if (ageSeconds > _maximumMemberStateAgeSeconds)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}的状态已过期：" +
|
||||
$"{ageSeconds:F3}s。";
|
||||
}
|
||||
|
||||
if (ageSeconds <= TimestampToleranceSeconds)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
if (!memberState.HasValidVelocityEstimate)
|
||||
{
|
||||
return $"车辆{memberState.VehicleId}缺少时间对齐所需的" +
|
||||
"有效速度。";
|
||||
}
|
||||
|
||||
var twist = memberState.TwistAtVehicleOriginInWorld;
|
||||
alignedPoseInWorld = new Pose2D(
|
||||
memberState.PoseInWorld.XMeters +
|
||||
twist.VxMetersPerSecond * ageSeconds,
|
||||
memberState.PoseInWorld.YMeters +
|
||||
twist.VyMetersPerSecond * ageSeconds,
|
||||
AngleMath.NormalizeRadians(
|
||||
memberState.PoseInWorld.YawRadians +
|
||||
twist.OmegaRadiansPerSecond * ageSeconds));
|
||||
return null;
|
||||
}
|
||||
|
||||
private string FindCandidateDisagreement(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers)
|
||||
{
|
||||
for (var firstIndex = 0;
|
||||
firstIndex < alignedMembers.Count;
|
||||
firstIndex++)
|
||||
{
|
||||
var first = alignedMembers[firstIndex];
|
||||
for (var secondIndex = firstIndex + 1;
|
||||
secondIndex < alignedMembers.Count;
|
||||
secondIndex++)
|
||||
{
|
||||
var second = alignedMembers[secondIndex];
|
||||
var dx =
|
||||
first.CandidateFleetPoseInWorld.XMeters -
|
||||
second.CandidateFleetPoseInWorld.XMeters;
|
||||
var dy =
|
||||
first.CandidateFleetPoseInWorld.YMeters -
|
||||
second.CandidateFleetPoseInWorld.YMeters;
|
||||
var positionDifferenceMeters =
|
||||
Math.Sqrt(dx * dx + dy * dy);
|
||||
var yawDifferenceRadians = Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
first.CandidateFleetPoseInWorld
|
||||
.YawRadians,
|
||||
second.CandidateFleetPoseInWorld
|
||||
.YawRadians));
|
||||
|
||||
if (positionDifferenceMeters >
|
||||
_maximumPositionDisagreementMeters)
|
||||
{
|
||||
return $"车辆{first.VehicleLayout.VehicleId}与" +
|
||||
$"车辆{second.VehicleLayout.VehicleId}反算的" +
|
||||
$"车队中心相差{positionDifferenceMeters:F3}m，" +
|
||||
"超过允许值。";
|
||||
}
|
||||
|
||||
if (yawDifferenceRadians >
|
||||
_maximumYawDisagreementRadians)
|
||||
{
|
||||
return $"车辆{first.VehicleLayout.VehicleId}与" +
|
||||
$"车辆{second.VehicleLayout.VehicleId}反算的" +
|
||||
$"车队航向相差" +
|
||||
$"{AngleMath.RadiansToDegrees(yawDifferenceRadians):F2}°，" +
|
||||
"超过允许值。";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
private static bool TryAverageCandidateFleetPose(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers,
|
||||
out Pose2D fleetPoseInWorld)
|
||||
{
|
||||
var xMeters = 0.0;
|
||||
var yMeters = 0.0;
|
||||
var yawCosineSum = 0.0;
|
||||
var yawSineSum = 0.0;
|
||||
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
var candidate =
|
||||
alignedMembers[index]
|
||||
.CandidateFleetPoseInWorld;
|
||||
xMeters += candidate.XMeters;
|
||||
yMeters += candidate.YMeters;
|
||||
yawCosineSum += Math.Cos(candidate.YawRadians);
|
||||
yawSineSum += Math.Sin(candidate.YawRadians);
|
||||
}
|
||||
|
||||
var count = alignedMembers.Count;
|
||||
var circularMeanMagnitude = Math.Sqrt(
|
||||
yawCosineSum * yawCosineSum +
|
||||
yawSineSum * yawSineSum);
|
||||
if (circularMeanMagnitude <
|
||||
MinimumCircularMeanMagnitude)
|
||||
{
|
||||
fleetPoseInWorld = Pose2D.Identity;
|
||||
return false;
|
||||
}
|
||||
|
||||
fleetPoseInWorld = new Pose2D(
|
||||
xMeters / count,
|
||||
yMeters / count,
|
||||
Math.Atan2(yawSineSum, yawCosineSum));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAverageFleetOriginTwist(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers,
|
||||
Pose2D fleetPoseInWorld,
|
||||
out Twist2D twistAtFleetOriginInWorld)
|
||||
{
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
if (!alignedMembers[index]
|
||||
.MemberState
|
||||
.HasValidVelocityEstimate)
|
||||
{
|
||||
twistAtFleetOriginInWorld = Twist2D.Zero;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
var vxMetersPerSecond = 0.0;
|
||||
var vyMetersPerSecond = 0.0;
|
||||
var omegaRadiansPerSecond = 0.0;
|
||||
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
var member = alignedMembers[index];
|
||||
var twist = member.MemberState
|
||||
.TwistAtVehicleOriginInWorld;
|
||||
var memberXFromFleetOrigin =
|
||||
member.AlignedPoseInWorld.XMeters -
|
||||
fleetPoseInWorld.XMeters;
|
||||
var memberYFromFleetOrigin =
|
||||
member.AlignedPoseInWorld.YMeters -
|
||||
fleetPoseInWorld.YMeters;
|
||||
|
||||
vxMetersPerSecond +=
|
||||
twist.VxMetersPerSecond +
|
||||
twist.OmegaRadiansPerSecond *
|
||||
memberYFromFleetOrigin;
|
||||
vyMetersPerSecond +=
|
||||
twist.VyMetersPerSecond -
|
||||
twist.OmegaRadiansPerSecond *
|
||||
memberXFromFleetOrigin;
|
||||
omegaRadiansPerSecond +=
|
||||
twist.OmegaRadiansPerSecond;
|
||||
}
|
||||
|
||||
var count = alignedMembers.Count;
|
||||
twistAtFleetOriginInWorld = new Twist2D(
|
||||
vxMetersPerSecond / count,
|
||||
vyMetersPerSecond / count,
|
||||
omegaRadiansPerSecond / count);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static FleetMemberLayoutError[] CalculateMemberErrors(
|
||||
IReadOnlyList<AlignedMemberState> alignedMembers,
|
||||
Pose2D fleetPoseInWorld)
|
||||
{
|
||||
var errors =
|
||||
new FleetMemberLayoutError[alignedMembers.Count];
|
||||
|
||||
for (var index = 0;
|
||||
index < alignedMembers.Count;
|
||||
index++)
|
||||
{
|
||||
var member = alignedMembers[index];
|
||||
var expectedPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
fleetPoseInWorld,
|
||||
member.VehicleLayout.PoseInFleet);
|
||||
var actualPoseInExpectedVehicleFrame =
|
||||
FrameTransform2D.Compose(
|
||||
FrameTransform2D.Inverse(
|
||||
expectedPoseInWorld),
|
||||
member.AlignedPoseInWorld);
|
||||
|
||||
errors[index] = new FleetMemberLayoutError(
|
||||
member.VehicleLayout.VehicleId,
|
||||
actualPoseInExpectedVehicleFrame);
|
||||
}
|
||||
|
||||
return errors;
|
||||
}
|
||||
|
||||
private readonly struct AlignedMemberState
|
||||
{
|
||||
public AlignedMemberState(
|
||||
VehicleLayout vehicleLayout,
|
||||
FleetMemberStateSample memberState,
|
||||
Pose2D alignedPoseInWorld,
|
||||
Pose2D candidateFleetPoseInWorld)
|
||||
{
|
||||
VehicleLayout = vehicleLayout;
|
||||
MemberState = memberState;
|
||||
AlignedPoseInWorld = alignedPoseInWorld;
|
||||
CandidateFleetPoseInWorld =
|
||||
candidateFleetPoseInWorld;
|
||||
}
|
||||
|
||||
public VehicleLayout VehicleLayout { get; }
|
||||
|
||||
public FleetMemberStateSample MemberState { get; }
|
||||
|
||||
public Pose2D AlignedPoseInWorld { get; }
|
||||
|
||||
public Pose2D CandidateFleetPoseInWorld { get; }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,6 +6,16 @@ using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>表示Detour位姿相对已标注地图关键帧的当前可信级别。</summary>
|
||||
public enum DetourLocalizationQuality
|
||||
{
|
||||
Unknown = 0,
|
||||
Normal = 1,
|
||||
Degraded = 2,
|
||||
Poor = 3,
|
||||
Lost = 4
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取Detour位姿，并将可能发生重定位的原始世界坐标转换为当前任务使用的连续坐标。
|
||||
/// </summary>
|
||||
@@ -55,6 +65,10 @@ namespace MultiWheelC.StateEstimation
|
||||
public const double
|
||||
DefaultMaximumAutomaticHeadingShiftRadians =
|
||||
5.0 * Math.PI / 180.0;
|
||||
public const double DefaultMaximumCachedFrameAgeSeconds =
|
||||
0.50;
|
||||
public const int
|
||||
DefaultLocalizationQualityConfirmationFrameCount = 3;
|
||||
|
||||
private const double MillimetersPerMeter = 1000.0;
|
||||
private const double PositionEqualityToleranceMeters = 1e-9;
|
||||
@@ -64,6 +78,9 @@ namespace MultiWheelC.StateEstimation
|
||||
private const double
|
||||
InPlaceRotationAngularSpeedThresholdRadiansPerSecond =
|
||||
Math.PI / 180.0;
|
||||
private const double NormalLocalizationStepUpperBound = 5.0;
|
||||
private const double DegradedLocalizationStepUpperBound = 10.0;
|
||||
private const double LostLocalizationStepLowerBound = 99.0;
|
||||
|
||||
private readonly object _syncRoot = new object();
|
||||
private readonly Stopwatch _clock = Stopwatch.StartNew();
|
||||
@@ -83,6 +100,9 @@ namespace MultiWheelC.StateEstimation
|
||||
private readonly double _maximumAutomaticFrameShiftMeters;
|
||||
private readonly double
|
||||
_maximumAutomaticHeadingShiftRadians;
|
||||
private readonly double _maximumCachedFrameAgeSeconds;
|
||||
private readonly int
|
||||
_localizationQualityConfirmationFrameCount;
|
||||
|
||||
private Pose2D _acceptedPoseInControl;
|
||||
private double _acceptedTimestampSeconds;
|
||||
@@ -94,8 +114,15 @@ namespace MultiWheelC.StateEstimation
|
||||
private long _lastObservedTickRaw;
|
||||
private bool _hasDetourSourceFrameInterval;
|
||||
private double _lastDetourSourceFrameIntervalSeconds;
|
||||
private double _lastNewDetourFrameTimestampSeconds;
|
||||
private Pose2D _controlFromDetour = Pose2D.Identity;
|
||||
|
||||
private int _lostLocalizationStepConsecutiveFrameCount;
|
||||
private int _normalLocalizationStepRecoveryFrameCount;
|
||||
private bool _localizationStepUnavailable;
|
||||
private bool _localizationStepPredictionActive;
|
||||
private bool _cachedFramePredictionActive;
|
||||
|
||||
private bool _hasMotionPrediction;
|
||||
private Pose2D _predictedPoseInControl;
|
||||
private double _predictionTimestampSeconds;
|
||||
@@ -144,7 +171,9 @@ namespace MultiWheelC.StateEstimation
|
||||
DefaultJumpConfirmationFrameCount,
|
||||
DefaultJumpConfirmationTimeoutSeconds,
|
||||
DefaultMaximumAutomaticFrameShiftMeters,
|
||||
DefaultMaximumAutomaticHeadingShiftRadians)
|
||||
DefaultMaximumAutomaticHeadingShiftRadians,
|
||||
DefaultMaximumCachedFrameAgeSeconds,
|
||||
DefaultLocalizationQualityConfirmationFrameCount)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -175,7 +204,9 @@ namespace MultiWheelC.StateEstimation
|
||||
DefaultJumpConfirmationFrameCount,
|
||||
DefaultJumpConfirmationTimeoutSeconds,
|
||||
DefaultMaximumAutomaticFrameShiftMeters,
|
||||
DefaultMaximumAutomaticHeadingShiftRadians)
|
||||
DefaultMaximumAutomaticHeadingShiftRadians,
|
||||
DefaultMaximumCachedFrameAgeSeconds,
|
||||
DefaultLocalizationQualityConfirmationFrameCount)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -197,6 +228,46 @@ namespace MultiWheelC.StateEstimation
|
||||
double jumpConfirmationTimeoutSeconds,
|
||||
double maximumAutomaticFrameShiftMeters,
|
||||
double maximumAutomaticHeadingShiftRadians)
|
||||
: this(
|
||||
velocityEstimator,
|
||||
maximumLinearSpeedMetersPerSecond,
|
||||
maximumAngularSpeedRadiansPerSecond,
|
||||
positionJumpMarginMeters,
|
||||
headingJumpMarginRadians,
|
||||
velocityPositionResidualMeters,
|
||||
velocityHeadingResidualRadians,
|
||||
stationaryConfirmationSeconds,
|
||||
headingOutlierConfirmationFrameCount,
|
||||
headingOutlierPredictionTimeoutSeconds,
|
||||
jumpConfirmationFrameCount,
|
||||
jumpConfirmationTimeoutSeconds,
|
||||
maximumAutomaticFrameShiftMeters,
|
||||
maximumAutomaticHeadingShiftRadians,
|
||||
DefaultMaximumCachedFrameAgeSeconds,
|
||||
DefaultLocalizationQualityConfirmationFrameCount)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建同时配置缓存停更和定位质量确认参数的Detour状态源。
|
||||
/// </summary>
|
||||
public DetourVehicleStateProvider(
|
||||
VelocityEstimator2D velocityEstimator,
|
||||
double maximumLinearSpeedMetersPerSecond,
|
||||
double maximumAngularSpeedRadiansPerSecond,
|
||||
double positionJumpMarginMeters,
|
||||
double headingJumpMarginRadians,
|
||||
double velocityPositionResidualMeters,
|
||||
double velocityHeadingResidualRadians,
|
||||
double stationaryConfirmationSeconds,
|
||||
int headingOutlierConfirmationFrameCount,
|
||||
double headingOutlierPredictionTimeoutSeconds,
|
||||
int jumpConfirmationFrameCount,
|
||||
double jumpConfirmationTimeoutSeconds,
|
||||
double maximumAutomaticFrameShiftMeters,
|
||||
double maximumAutomaticHeadingShiftRadians,
|
||||
double maximumCachedFrameAgeSeconds,
|
||||
int localizationQualityConfirmationFrameCount)
|
||||
{
|
||||
_velocityEstimator = velocityEstimator ??
|
||||
throw new ArgumentNullException(
|
||||
@@ -235,6 +306,9 @@ namespace MultiWheelC.StateEstimation
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumAutomaticHeadingShiftRadians,
|
||||
nameof(maximumAutomaticHeadingShiftRadians));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumCachedFrameAgeSeconds,
|
||||
nameof(maximumCachedFrameAgeSeconds));
|
||||
|
||||
if (jumpConfirmationFrameCount < 2)
|
||||
{
|
||||
@@ -250,6 +324,13 @@ namespace MultiWheelC.StateEstimation
|
||||
"Detour航向异常至少需要两个新帧确认。");
|
||||
}
|
||||
|
||||
if (localizationQualityConfirmationFrameCount < 2)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(localizationQualityConfirmationFrameCount),
|
||||
"Detour定位质量失效和恢复至少需要两个新帧确认。");
|
||||
}
|
||||
|
||||
_maximumLinearSpeedMetersPerSecond =
|
||||
maximumLinearSpeedMetersPerSecond;
|
||||
_maximumAngularSpeedRadiansPerSecond =
|
||||
@@ -274,6 +355,10 @@ namespace MultiWheelC.StateEstimation
|
||||
maximumAutomaticFrameShiftMeters;
|
||||
_maximumAutomaticHeadingShiftRadians =
|
||||
maximumAutomaticHeadingShiftRadians;
|
||||
_maximumCachedFrameAgeSeconds =
|
||||
maximumCachedFrameAgeSeconds;
|
||||
_localizationQualityConfirmationFrameCount =
|
||||
localizationQualityConfirmationFrameCount;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -289,15 +374,22 @@ namespace MultiWheelC.StateEstimation
|
||||
string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次读取到的Detour源时间戳原值。
|
||||
/// 获取最近一次共享对象提交时的DateTime.Ticks原值。
|
||||
/// </summary>
|
||||
public long? LastDetourTickRaw { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次Detour l_step原值；其精确定义仍由Detour接口文档确认。
|
||||
/// 获取最近一次Detour l_step原值，即到已标注地图关键帧的图上步数。
|
||||
/// </summary>
|
||||
public double? LastDetourLocalizationStep { get; private set; }
|
||||
|
||||
/// <summary>获取最近一帧Detour定位质量的瞬时分级。</summary>
|
||||
public DetourLocalizationQuality LocalizationQuality
|
||||
{
|
||||
get;
|
||||
private set;
|
||||
} = DetourLocalizationQuality.Unknown;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否正在确认疑似Detour坐标跳变。
|
||||
/// </summary>
|
||||
@@ -435,10 +527,35 @@ namespace MultiWheelC.StateEstimation
|
||||
LastDetourTickRaw = observation.TickRaw;
|
||||
LastDetourLocalizationStep =
|
||||
observation.LocalizationStep;
|
||||
LocalizationQuality =
|
||||
ClassifyLocalizationStep(
|
||||
observation.LocalizationStep);
|
||||
|
||||
if (!_hasObservedDetourFrame)
|
||||
{
|
||||
SetLastObservedDetourFrame(observation);
|
||||
_lastNewDetourFrameTimestampSeconds =
|
||||
timestampSeconds;
|
||||
|
||||
if (LocalizationQuality ==
|
||||
DetourLocalizationQuality.Lost)
|
||||
{
|
||||
_localizationStepUnavailable = true;
|
||||
_lostLocalizationStepConsecutiveFrameCount =
|
||||
_localizationQualityConfirmationFrameCount;
|
||||
InvalidateHeadingObservation(
|
||||
"Detour初始l_step达到丢失定位范围，航向暂不可用。");
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour初始l_step=" +
|
||||
observation.LocalizationStep
|
||||
.ToString(
|
||||
"F0",
|
||||
CultureInfo.InvariantCulture) +
|
||||
"，定位状态不可用。";
|
||||
return false;
|
||||
}
|
||||
|
||||
AcceptReliableHeadingObservation(observation);
|
||||
state = AcceptPoseAfterReset(
|
||||
observation.PoseInDetour,
|
||||
@@ -471,6 +588,9 @@ namespace MultiWheelC.StateEstimation
|
||||
var previousPoseInDetour =
|
||||
_lastObservedPoseInDetour;
|
||||
SetLastObservedDetourFrame(observation);
|
||||
_lastNewDetourFrameTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_cachedFramePredictionActive = false;
|
||||
|
||||
if (!NumericGuard.IsFinite(
|
||||
sourceDeltaTimeSeconds) ||
|
||||
@@ -488,6 +608,14 @@ namespace MultiWheelC.StateEstimation
|
||||
sourceDeltaTimeSeconds;
|
||||
_hasDetourSourceFrameInterval = true;
|
||||
|
||||
if (HandleLocalizationStepQuality(
|
||||
observation,
|
||||
timestampSeconds,
|
||||
out state))
|
||||
{
|
||||
return !_localizationStepUnavailable;
|
||||
}
|
||||
|
||||
var poseInControl =
|
||||
FrameTransform2D.TransformPose(
|
||||
_controlFromDetour,
|
||||
@@ -605,6 +733,18 @@ namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if ((_cachedFramePredictionActive ||
|
||||
_localizationStepPredictionActive) &&
|
||||
_hasReliableHeadingObservation &&
|
||||
_latestWheelVelocityValid &&
|
||||
_hasMotionPrediction)
|
||||
{
|
||||
headingRadians =
|
||||
GetPredictedPoseInControl().YawRadians;
|
||||
LastHeadingFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_headingOutlierCandidateActive)
|
||||
{
|
||||
var candidateAgeSeconds =
|
||||
@@ -742,8 +882,15 @@ namespace MultiWheelC.StateEstimation
|
||||
_lastObservedTickRaw = 0L;
|
||||
_hasDetourSourceFrameInterval = false;
|
||||
_lastDetourSourceFrameIntervalSeconds = 0.0;
|
||||
_lastNewDetourFrameTimestampSeconds = 0.0;
|
||||
_controlFromDetour = Pose2D.Identity;
|
||||
|
||||
_lostLocalizationStepConsecutiveFrameCount = 0;
|
||||
_normalLocalizationStepRecoveryFrameCount = 0;
|
||||
_localizationStepUnavailable = false;
|
||||
_localizationStepPredictionActive = false;
|
||||
_cachedFramePredictionActive = false;
|
||||
|
||||
_hasMotionPrediction = false;
|
||||
_predictedPoseInControl = Pose2D.Identity;
|
||||
_predictionTimestampSeconds = 0.0;
|
||||
@@ -765,15 +912,158 @@ namespace MultiWheelC.StateEstimation
|
||||
AutomaticFrameShiftCount = 0;
|
||||
LastDetourTickRaw = null;
|
||||
LastDetourLocalizationStep = null;
|
||||
LocalizationQuality =
|
||||
DetourLocalizationQuality.Unknown;
|
||||
LastFailureReason = string.Empty;
|
||||
LastHeadingFailureReason = string.Empty;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>按l_step确认定位丢失，并要求连续正常新帧后才恢复。</summary>
|
||||
private bool HandleLocalizationStepQuality(
|
||||
DetourObservation observation,
|
||||
double timestampSeconds,
|
||||
out VehicleState state)
|
||||
{
|
||||
if (LocalizationQuality ==
|
||||
DetourLocalizationQuality.Lost)
|
||||
{
|
||||
_normalLocalizationStepRecoveryFrameCount = 0;
|
||||
_lostLocalizationStepConsecutiveFrameCount++;
|
||||
|
||||
if (_lostLocalizationStepConsecutiveFrameCount >=
|
||||
_localizationQualityConfirmationFrameCount)
|
||||
{
|
||||
_localizationStepUnavailable = true;
|
||||
_localizationStepPredictionActive = false;
|
||||
InvalidateHeadingObservation(
|
||||
"Detour l_step连续达到丢失定位范围，航向暂不可用。");
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour l_step=" +
|
||||
observation.LocalizationStep.ToString(
|
||||
"F0",
|
||||
CultureInfo.InvariantCulture) +
|
||||
"已连续" +
|
||||
_lostLocalizationStepConsecutiveFrameCount +
|
||||
"个新帧达到丢失定位范围，车辆状态暂不可用。";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!_hasMotionPrediction ||
|
||||
!_latestWheelVelocityValid ||
|
||||
!_hasReliableHeadingObservation)
|
||||
{
|
||||
_localizationStepUnavailable = true;
|
||||
_localizationStepPredictionActive = false;
|
||||
InvalidateHeadingObservation(
|
||||
"Detour定位质量异常且缺少可靠轮组预测，航向暂不可用。");
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour定位质量异常且缺少可靠轮组预测，车辆状态暂不可用。";
|
||||
return true;
|
||||
}
|
||||
|
||||
_localizationStepPredictionActive = true;
|
||||
state = CreatePredictedState(timestampSeconds);
|
||||
LastFailureReason =
|
||||
"Detour l_step达到丢失定位范围，正在等待连续新帧确认，" +
|
||||
"当前使用轮组速度短时预测位姿。";
|
||||
return true;
|
||||
}
|
||||
|
||||
_lostLocalizationStepConsecutiveFrameCount = 0;
|
||||
_localizationStepPredictionActive = false;
|
||||
|
||||
if (!_localizationStepUnavailable)
|
||||
{
|
||||
_normalLocalizationStepRecoveryFrameCount = 0;
|
||||
state = default;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (LocalizationQuality ==
|
||||
DetourLocalizationQuality.Normal)
|
||||
{
|
||||
_normalLocalizationStepRecoveryFrameCount++;
|
||||
}
|
||||
else
|
||||
{
|
||||
_normalLocalizationStepRecoveryFrameCount = 0;
|
||||
}
|
||||
|
||||
if (_normalLocalizationStepRecoveryFrameCount <
|
||||
_localizationQualityConfirmationFrameCount)
|
||||
{
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour定位质量正在恢复确认(" +
|
||||
_normalLocalizationStepRecoveryFrameCount +
|
||||
"/" +
|
||||
_localizationQualityConfirmationFrameCount +
|
||||
")，车辆状态暂不可用。";
|
||||
return true;
|
||||
}
|
||||
|
||||
_localizationStepUnavailable = false;
|
||||
_normalLocalizationStepRecoveryFrameCount = 0;
|
||||
AcceptReliableHeadingObservation(observation);
|
||||
state = AcceptPoseAfterReset(
|
||||
FrameTransform2D.TransformPose(
|
||||
_controlFromDetour,
|
||||
observation.PoseInDetour),
|
||||
timestampSeconds);
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
|
||||
private bool HandleRepeatedDetourFrame(
|
||||
double timestampSeconds,
|
||||
out VehicleState state)
|
||||
{
|
||||
var cachedFrameAgeSeconds =
|
||||
timestampSeconds -
|
||||
_lastNewDetourFrameTimestampSeconds;
|
||||
|
||||
if (!NumericGuard.IsFinite(cachedFrameAgeSeconds) ||
|
||||
cachedFrameAgeSeconds < 0.0)
|
||||
{
|
||||
InvalidateHeadingObservation(
|
||||
"Detour缓存帧本机计时无效，航向暂不可用。");
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour缓存帧本机计时无效，车辆状态暂不可用。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_localizationStepUnavailable)
|
||||
{
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour定位质量尚未通过连续健康新帧恢复确认。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (cachedFrameAgeSeconds >
|
||||
_maximumCachedFrameAgeSeconds)
|
||||
{
|
||||
_cachedFramePredictionActive = false;
|
||||
InvalidateHeadingObservation(
|
||||
"Detour源tick超过缓存停更上限，航向暂不可用。");
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour源tick已连续" +
|
||||
cachedFrameAgeSeconds.ToString(
|
||||
"F3",
|
||||
CultureInfo.InvariantCulture) +
|
||||
"s未更新，超过" +
|
||||
_maximumCachedFrameAgeSeconds.ToString(
|
||||
"F3",
|
||||
CultureInfo.InvariantCulture) +
|
||||
"s上限，车辆状态暂不可用。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_jumpCandidateActive)
|
||||
{
|
||||
return ReturnCandidateState(
|
||||
@@ -781,6 +1071,16 @@ namespace MultiWheelC.StateEstimation
|
||||
out state);
|
||||
}
|
||||
|
||||
if (IsVehicleMovingFromWheelFeedback())
|
||||
{
|
||||
_cachedFramePredictionActive = true;
|
||||
state = CreatePredictedState(timestampSeconds);
|
||||
LastFailureReason =
|
||||
"Detour源tick暂未更新，当前使用轮组速度短时预测位姿。";
|
||||
return true;
|
||||
}
|
||||
|
||||
_cachedFramePredictionActive = false;
|
||||
state = HandleRepeatedPose(timestampSeconds);
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
@@ -974,6 +1274,54 @@ namespace MultiWheelC.StateEstimation
|
||||
InPlaceRotationAngularSpeedThresholdRadiansPerSecond;
|
||||
}
|
||||
|
||||
/// <summary>判断轮组反馈是否表明车辆仍在平移或转动。</summary>
|
||||
private bool IsVehicleMovingFromWheelFeedback()
|
||||
{
|
||||
if (!_latestWheelVelocityValid)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var linearSpeedMetersPerSecond = Math.Sqrt(
|
||||
_latestWheelTwistInBody.VxMetersPerSecond *
|
||||
_latestWheelTwistInBody.VxMetersPerSecond +
|
||||
_latestWheelTwistInBody.VyMetersPerSecond *
|
||||
_latestWheelTwistInBody.VyMetersPerSecond);
|
||||
|
||||
return linearSpeedMetersPerSecond >
|
||||
InPlaceRotationLinearSpeedThresholdMetersPerSecond ||
|
||||
Math.Abs(
|
||||
_latestWheelTwistInBody
|
||||
.OmegaRadiansPerSecond) >
|
||||
InPlaceRotationAngularSpeedThresholdRadiansPerSecond;
|
||||
}
|
||||
|
||||
/// <summary>按照Detour源码中l_step到可信锚点的图距离进行质量分级。</summary>
|
||||
public static DetourLocalizationQuality
|
||||
ClassifyLocalizationStep(double localizationStep)
|
||||
{
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
localizationStep,
|
||||
nameof(localizationStep));
|
||||
|
||||
if (localizationStep >=
|
||||
LostLocalizationStepLowerBound)
|
||||
{
|
||||
return DetourLocalizationQuality.Lost;
|
||||
}
|
||||
|
||||
if (localizationStep >
|
||||
DegradedLocalizationStepUpperBound)
|
||||
{
|
||||
return DetourLocalizationQuality.Poor;
|
||||
}
|
||||
|
||||
return localizationStep >
|
||||
NormalLocalizationStepUpperBound
|
||||
? DetourLocalizationQuality.Degraded
|
||||
: DetourLocalizationQuality.Normal;
|
||||
}
|
||||
|
||||
private void ClearJumpCandidate()
|
||||
{
|
||||
_jumpCandidateActive = false;
|
||||
@@ -1015,7 +1363,7 @@ namespace MultiWheelC.StateEstimation
|
||||
NumericGuard.EnsureFinite(
|
||||
yawDegrees,
|
||||
"DetourTheta");
|
||||
NumericGuard.EnsureFinite(
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
localizationStep,
|
||||
"DetourLStep");
|
||||
|
||||
@@ -1459,6 +1807,21 @@ namespace MultiWheelC.StateEstimation
|
||||
return "Uninitialized";
|
||||
}
|
||||
|
||||
if (_localizationStepUnavailable)
|
||||
{
|
||||
return "LocalizationUnavailable";
|
||||
}
|
||||
|
||||
if (_localizationStepPredictionActive)
|
||||
{
|
||||
return "LocalizationStepPrediction";
|
||||
}
|
||||
|
||||
if (_cachedFramePredictionActive)
|
||||
{
|
||||
return "CachedFramePrediction";
|
||||
}
|
||||
|
||||
if (_jumpCandidateActive)
|
||||
{
|
||||
var candidateAgeSeconds =
|
||||
@@ -1482,6 +1845,18 @@ namespace MultiWheelC.StateEstimation
|
||||
return "HeadingUnavailable";
|
||||
}
|
||||
|
||||
if (LocalizationQuality ==
|
||||
DetourLocalizationQuality.Poor)
|
||||
{
|
||||
return "LocalizationPoor";
|
||||
}
|
||||
|
||||
if (LocalizationQuality ==
|
||||
DetourLocalizationQuality.Degraded)
|
||||
{
|
||||
return "LocalizationDegraded";
|
||||
}
|
||||
|
||||
return string.IsNullOrWhiteSpace(LastFailureReason)
|
||||
? "Healthy"
|
||||
: "Unavailable";
|
||||
|
||||
@@ -64,7 +64,10 @@ namespace MultiWheelC.StateEstimation
|
||||
config.ParkingDetourMaximumAutomaticFrameShift,
|
||||
AngleMath.DegreesToRadians(
|
||||
config
|
||||
.ParkingDetourMaximumAutomaticHeadingShiftDegrees));
|
||||
.ParkingDetourMaximumAutomaticHeadingShiftDegrees),
|
||||
config.ParkingDetourMaximumCachedFrameAgeSeconds,
|
||||
config
|
||||
.ParkingDetourLocalizationQualityConfirmationFrames);
|
||||
|
||||
return new WheelFeedbackVehicleStateProvider(
|
||||
detourStateProvider,
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user