增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
@@ -1,12 +1,11 @@
|
||||
using System;
|
||||
using MultiWheelC.StateEstimation;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Control.Abstractions
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
|
||||
/// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。
|
||||
/// </summary>
|
||||
public readonly struct PathTrackingContext
|
||||
{
|
||||
@@ -14,7 +13,8 @@ namespace MultiWheelC.Control.Abstractions
|
||||
/// 创建横向和纵向控制器共享的只读控制输入快照。
|
||||
/// </summary>
|
||||
public PathTrackingContext(
|
||||
VehicleState vehicleState,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
TrajectoryProjection projection,
|
||||
double controlReferenceSpeedMetersPerSecond,
|
||||
double feedforwardCurvaturePerMeter,
|
||||
@@ -33,8 +33,13 @@ namespace MultiWheelC.Control.Abstractions
|
||||
EnsureFinite(
|
||||
motionDirectionInBodyRadians,
|
||||
nameof(motionDirectionInBodyRadians));
|
||||
NumericGuard.EnsureFinite(
|
||||
actualTwistInBody,
|
||||
nameof(actualTwistInBody));
|
||||
|
||||
VehicleState = vehicleState;
|
||||
ActualTwistInBody = actualTwistInBody;
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
Projection = projection;
|
||||
ControlReferenceSpeedMetersPerSecond =
|
||||
controlReferenceSpeedMetersPerSecond;
|
||||
@@ -47,9 +52,9 @@ namespace MultiWheelC.Control.Abstractions
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取本周期经过校验的实际车辆位姿和速度状态。
|
||||
/// 获取受控刚体坐标系下的实际速度。
|
||||
/// </summary>
|
||||
public VehicleState VehicleState { get; }
|
||||
public Twist2D ActualTwistInBody { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
|
||||
@@ -71,9 +76,9 @@ namespace MultiWheelC.Control.Abstractions
|
||||
/// </summary>
|
||||
public double ActualLongitudinalSpeedMetersPerSecond =>
|
||||
Math.Cos(MotionDirectionInBodyRadians) *
|
||||
VehicleState.TwistInBody.VxMetersPerSecond +
|
||||
ActualTwistInBody.VxMetersPerSecond +
|
||||
Math.Sin(MotionDirectionInBodyRadians) *
|
||||
VehicleState.TwistInBody.VyMetersPerSecond;
|
||||
ActualTwistInBody.VyMetersPerSecond;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
|
||||
@@ -111,10 +116,9 @@ namespace MultiWheelC.Control.Abstractions
|
||||
Projection.RemainingDistanceMeters;
|
||||
|
||||
/// <summary>
|
||||
/// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
|
||||
/// 获取本周期实际速度估计是否可供闭环控制使用。
|
||||
/// </summary>
|
||||
public bool HasValidVelocityEstimate =>
|
||||
VehicleState.HasValidVelocityEstimate;
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查控制周期是否为正有限值。
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
using System;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
|
||||
using MyParking.Shared;
|
||||
// 限制目标角度的最大绝对值，例如不能超过60°。
|
||||
namespace MultiWheelC.Control.Allocation
|
||||
{
|
||||
/// <summary>
|
||||
@@ -13,7 +14,7 @@ namespace MultiWheelC.Control.Allocation
|
||||
/// </summary>
|
||||
public GcpCommandAllocator(double maximumGcpAngleRadians)
|
||||
{
|
||||
EnsureFinitePositive(
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumGcpAngleRadians,
|
||||
nameof(maximumGcpAngleRadians));
|
||||
|
||||
@@ -39,7 +40,7 @@ namespace MultiWheelC.Control.Allocation
|
||||
double speedMetersPerSecond,
|
||||
LateralControlCommand lateralCommand)
|
||||
{
|
||||
EnsureFinite(
|
||||
NumericGuard.EnsureFinite(
|
||||
speedMetersPerSecond,
|
||||
nameof(speedMetersPerSecond));
|
||||
|
||||
@@ -68,37 +69,5 @@ namespace MultiWheelC.Control.Allocation
|
||||
Math.Min(maximumAbsoluteValue, value));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查参数是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"GCP分配参数必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查参数或命令是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"GCP分配参数和命令必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Control.Allocation
|
||||
{
|
||||
@@ -15,13 +15,13 @@ namespace MultiWheelC.Control.Allocation
|
||||
double frontAngleRadians,
|
||||
double rearAngleRadians)
|
||||
{
|
||||
EnsureFinite(
|
||||
NumericGuard.EnsureFinite(
|
||||
speedMetersPerSecond,
|
||||
nameof(speedMetersPerSecond));
|
||||
EnsureFinite(
|
||||
NumericGuard.EnsureFinite(
|
||||
frontAngleRadians,
|
||||
nameof(frontAngleRadians));
|
||||
EnsureFinite(
|
||||
NumericGuard.EnsureFinite(
|
||||
rearAngleRadians,
|
||||
nameof(rearAngleRadians));
|
||||
|
||||
@@ -48,20 +48,5 @@ namespace MultiWheelC.Control.Allocation
|
||||
/// </summary>
|
||||
public double RearAngleRadians { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查底盘中间命令是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"GCP运动命令必须由有限值组成。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,793 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Control.Execution
|
||||
{
|
||||
// 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。
|
||||
public enum PathTrackingCycleResult
|
||||
{
|
||||
Inactive = 0,
|
||||
CommandGenerated = 1,
|
||||
Completed = 2,
|
||||
Faulted = 3
|
||||
}
|
||||
|
||||
// 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。
|
||||
public readonly struct PathTrackingCycleOutput
|
||||
{
|
||||
public PathTrackingCycleOutput(
|
||||
PathTrackingCycleResult result,
|
||||
GcpMotionCommand? command,
|
||||
TrajectoryProjection? projection,
|
||||
double projectionMilliseconds,
|
||||
double controllerComputeMilliseconds)
|
||||
{
|
||||
Result = result;
|
||||
Command = command;
|
||||
Projection = projection;
|
||||
ProjectionMilliseconds = projectionMilliseconds;
|
||||
ControllerComputeMilliseconds =
|
||||
controllerComputeMilliseconds;
|
||||
}
|
||||
|
||||
public PathTrackingCycleResult Result { get; }
|
||||
|
||||
public GcpMotionCommand? Command { get; }
|
||||
|
||||
public TrajectoryProjection? Projection { get; }
|
||||
|
||||
public double ProjectionMilliseconds { get; }
|
||||
|
||||
public double ControllerComputeMilliseconds { get; }
|
||||
}
|
||||
|
||||
// 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。
|
||||
public sealed class PathTrackingCore
|
||||
{
|
||||
private const double ZeroReferenceSpeedToleranceMetersPerSecond =
|
||||
1e-6;
|
||||
private const double StartupRegionMeters = 0.02;
|
||||
private const double StartupPreviewDistanceMeters = 0.05;
|
||||
private const double MaximumStartupSpeedMetersPerSecond = 0.08;
|
||||
private const double ProjectionBackwardSearchDistanceMeters =
|
||||
0.10;
|
||||
private const double ProjectionForwardSearchDistanceMeters =
|
||||
1.00;
|
||||
|
||||
private readonly ILateralController _lateralController;
|
||||
private readonly ILongitudinalController _longitudinalController;
|
||||
private readonly GcpCommandAllocator _gcpAllocator;
|
||||
private readonly double _motionDirectionInBodyRadians;
|
||||
|
||||
private Trajectory2D _trajectory;
|
||||
private double _terminalTravelDirection = 1.0;
|
||||
|
||||
public PathTrackingCore(
|
||||
ILateralController lateralController,
|
||||
ILongitudinalController longitudinalController,
|
||||
GcpCommandAllocator gcpAllocator,
|
||||
double finishDistanceMeters = 0.04,
|
||||
double finishSpeedMetersPerSecond = 0.02,
|
||||
double finishHeadingToleranceRadians =
|
||||
3.0 * Math.PI / 180.0,
|
||||
double maximumDistanceToTrajectoryMeters = 0.30,
|
||||
double terminalBrakingPreviewMeters = 0.02,
|
||||
double terminalApproachDistanceMeters = 0.10,
|
||||
double terminalApproachGainPerSecond = 0.8,
|
||||
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
|
||||
double curvaturePreviewSeconds = 0.20,
|
||||
double maximumCurvaturePreviewMeters = 0.12,
|
||||
double motionDirectionInBodyRadians = 0.0)
|
||||
{
|
||||
_lateralController = lateralController ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(lateralController));
|
||||
_longitudinalController = longitudinalController ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(longitudinalController));
|
||||
_gcpAllocator = gcpAllocator ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(gcpAllocator));
|
||||
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
finishDistanceMeters,
|
||||
nameof(finishDistanceMeters));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
finishSpeedMetersPerSecond,
|
||||
nameof(finishSpeedMetersPerSecond));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
finishHeadingToleranceRadians,
|
||||
nameof(finishHeadingToleranceRadians));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumDistanceToTrajectoryMeters,
|
||||
nameof(maximumDistanceToTrajectoryMeters));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
terminalBrakingPreviewMeters,
|
||||
nameof(terminalBrakingPreviewMeters));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
terminalApproachDistanceMeters,
|
||||
nameof(terminalApproachDistanceMeters));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
terminalApproachGainPerSecond,
|
||||
nameof(terminalApproachGainPerSecond));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
maximumTerminalApproachSpeedMetersPerSecond,
|
||||
nameof(maximumTerminalApproachSpeedMetersPerSecond));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
curvaturePreviewSeconds,
|
||||
nameof(curvaturePreviewSeconds));
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
maximumCurvaturePreviewMeters,
|
||||
nameof(maximumCurvaturePreviewMeters));
|
||||
NumericGuard.EnsureFinite(
|
||||
motionDirectionInBodyRadians,
|
||||
nameof(motionDirectionInBodyRadians));
|
||||
|
||||
if (terminalApproachDistanceMeters <=
|
||||
finishDistanceMeters)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(terminalApproachDistanceMeters),
|
||||
"终点单向逼近范围必须大于终点位置容差。");
|
||||
}
|
||||
|
||||
FinishDistanceMeters = finishDistanceMeters;
|
||||
FinishSpeedMetersPerSecond =
|
||||
finishSpeedMetersPerSecond;
|
||||
FinishHeadingToleranceRadians =
|
||||
finishHeadingToleranceRadians;
|
||||
MaximumDistanceToTrajectoryMeters =
|
||||
maximumDistanceToTrajectoryMeters;
|
||||
TerminalBrakingPreviewMeters =
|
||||
terminalBrakingPreviewMeters;
|
||||
TerminalApproachDistanceMeters =
|
||||
terminalApproachDistanceMeters;
|
||||
TerminalApproachGainPerSecond =
|
||||
terminalApproachGainPerSecond;
|
||||
MaximumTerminalApproachSpeedMetersPerSecond =
|
||||
maximumTerminalApproachSpeedMetersPerSecond;
|
||||
CurvaturePreviewSeconds = curvaturePreviewSeconds;
|
||||
MaximumCurvaturePreviewMeters =
|
||||
maximumCurvaturePreviewMeters;
|
||||
_motionDirectionInBodyRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
motionDirectionInBodyRadians);
|
||||
}
|
||||
|
||||
public double FinishDistanceMeters { get; }
|
||||
|
||||
public double FinishSpeedMetersPerSecond { get; }
|
||||
|
||||
public double FinishHeadingToleranceRadians { get; }
|
||||
|
||||
public double MaximumDistanceToTrajectoryMeters { get; }
|
||||
|
||||
public double TerminalBrakingPreviewMeters { get; }
|
||||
|
||||
public double TerminalApproachDistanceMeters { get; }
|
||||
|
||||
public double TerminalApproachGainPerSecond { get; }
|
||||
|
||||
public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
|
||||
|
||||
public double CurvaturePreviewSeconds { get; }
|
||||
|
||||
public double MaximumCurvaturePreviewMeters { get; }
|
||||
|
||||
public bool IsActive { get; private set; }
|
||||
|
||||
public bool IsCompleted { get; private set; }
|
||||
|
||||
public string LastFailureReason { get; private set; } =
|
||||
string.Empty;
|
||||
|
||||
public Exception LastException { get; private set; }
|
||||
|
||||
public TrajectoryProjection? LastProjection { get; private set; }
|
||||
|
||||
public GcpMotionCommand? LastRequestedCommand { get; private set; }
|
||||
|
||||
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
|
||||
|
||||
public double? LastCurvaturePreviewDistanceMeters { get; private set; }
|
||||
|
||||
public double? LastFeedforwardCurvaturePerMeter { get; private set; }
|
||||
|
||||
// 重置跨周期状态，并从轨迹起点开始新的跟踪过程。
|
||||
public void Start(Trajectory2D trajectory)
|
||||
{
|
||||
if (trajectory == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
var terminalTravelDirection =
|
||||
ResolveTerminalTravelDirection(trajectory);
|
||||
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = trajectory;
|
||||
_terminalTravelDirection =
|
||||
terminalTravelDirection;
|
||||
IsActive = true;
|
||||
IsCompleted = false;
|
||||
ClearDiagnostics();
|
||||
}
|
||||
|
||||
// 将受控刚体的位姿和速度转换为本周期GCP命令。
|
||||
public PathTrackingCycleOutput Compute(
|
||||
Pose2D poseInWorld,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
NumericGuard.EnsureFinite(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
NumericGuard.EnsureFinite(
|
||||
actualTwistInBody,
|
||||
nameof(actualTwistInBody));
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
if (!IsActive || _trajectory == null)
|
||||
{
|
||||
return new PathTrackingCycleOutput(
|
||||
PathTrackingCycleResult.Inactive,
|
||||
null,
|
||||
LastProjection,
|
||||
0.0,
|
||||
0.0);
|
||||
}
|
||||
|
||||
var projectionStartTimestamp =
|
||||
Stopwatch.GetTimestamp();
|
||||
var projectionCompleted = false;
|
||||
var projectionMilliseconds = 0.0;
|
||||
var controllerComputeStartTimestamp = 0L;
|
||||
|
||||
try
|
||||
{
|
||||
var projection = LastProjection.HasValue
|
||||
? TrajectoryProjector.Project(
|
||||
_trajectory,
|
||||
poseInWorld,
|
||||
LastProjection.Value.ArcLengthMeters,
|
||||
ProjectionBackwardSearchDistanceMeters,
|
||||
ProjectionForwardSearchDistanceMeters)
|
||||
: TrajectoryProjector.Project(
|
||||
_trajectory,
|
||||
poseInWorld);
|
||||
|
||||
projectionMilliseconds =
|
||||
GetElapsedMilliseconds(
|
||||
projectionStartTimestamp);
|
||||
projectionCompleted = true;
|
||||
LastProjection = projection;
|
||||
controllerComputeStartTimestamp =
|
||||
Stopwatch.GetTimestamp();
|
||||
|
||||
if (projection.DistanceToTrajectoryMeters >
|
||||
MaximumDistanceToTrajectoryMeters)
|
||||
{
|
||||
Fail(
|
||||
"受控刚体距离参考轨迹" +
|
||||
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
|
||||
"超过允许值" +
|
||||
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Faulted,
|
||||
null,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
|
||||
if (HasReachedEnd(
|
||||
poseInWorld,
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
projection))
|
||||
{
|
||||
CompleteTrajectory();
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Completed,
|
||||
null,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
|
||||
if (HasStoppedAtUnsatisfiedTerminal(
|
||||
poseInWorld,
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
projection,
|
||||
out var terminalFailureReason))
|
||||
{
|
||||
Fail(terminalFailureReason);
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Faulted,
|
||||
null,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
|
||||
var controlReferenceSpeedMetersPerSecond =
|
||||
ResolveControlReferenceSpeed(
|
||||
poseInWorld,
|
||||
projection);
|
||||
LastControlReferenceSpeedMetersPerSecond =
|
||||
controlReferenceSpeedMetersPerSecond;
|
||||
var curvaturePreviewDistanceMeters =
|
||||
ResolveCurvaturePreviewDistanceMeters(
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
controlReferenceSpeedMetersPerSecond);
|
||||
LastCurvaturePreviewDistanceMeters =
|
||||
curvaturePreviewDistanceMeters;
|
||||
var feedforwardCurvaturePerMeter =
|
||||
ResolveFeedforwardCurvaturePerMeter(
|
||||
projection,
|
||||
curvaturePreviewDistanceMeters);
|
||||
LastFeedforwardCurvaturePerMeter =
|
||||
feedforwardCurvaturePerMeter;
|
||||
var context = new PathTrackingContext(
|
||||
actualTwistInBody,
|
||||
hasValidVelocityEstimate,
|
||||
projection,
|
||||
controlReferenceSpeedMetersPerSecond,
|
||||
feedforwardCurvaturePerMeter,
|
||||
deltaTimeSeconds,
|
||||
_motionDirectionInBodyRadians);
|
||||
var lateralCommand =
|
||||
_lateralController.Compute(context);
|
||||
var commandSpeedMetersPerSecond =
|
||||
_longitudinalController
|
||||
.ComputeSpeedMetersPerSecond(context);
|
||||
var command = _gcpAllocator.Allocate(
|
||||
commandSpeedMetersPerSecond,
|
||||
lateralCommand);
|
||||
|
||||
LastRequestedCommand = command;
|
||||
LastFailureReason = string.Empty;
|
||||
LastException = null;
|
||||
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.CommandGenerated,
|
||||
command,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
if (!projectionCompleted)
|
||||
{
|
||||
projectionMilliseconds =
|
||||
GetElapsedMilliseconds(
|
||||
projectionStartTimestamp);
|
||||
}
|
||||
|
||||
Fail(
|
||||
"轨迹跟踪核心计算异常：" +
|
||||
exception.Message,
|
||||
exception);
|
||||
|
||||
return CreateOutput(
|
||||
PathTrackingCycleResult.Faulted,
|
||||
null,
|
||||
LastProjection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeStartTimestamp);
|
||||
}
|
||||
}
|
||||
|
||||
// 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。
|
||||
public void PauseForUnavailableState(string reason)
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
LastRequestedCommand = null;
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
LastException = null;
|
||||
}
|
||||
|
||||
// 将外层执行故障同步到公共核心，并终止当前轨迹。
|
||||
public void Fail(
|
||||
string reason,
|
||||
Exception exception = null)
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = null;
|
||||
IsActive = false;
|
||||
IsCompleted = false;
|
||||
LastRequestedCommand = null;
|
||||
LastFailureReason = reason ?? string.Empty;
|
||||
LastException = exception;
|
||||
}
|
||||
|
||||
// 取消当前轨迹并清除全部跟踪状态。
|
||||
public void Cancel()
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = null;
|
||||
_terminalTravelDirection = 1.0;
|
||||
IsActive = false;
|
||||
IsCompleted = false;
|
||||
ClearDiagnostics();
|
||||
}
|
||||
|
||||
private double ResolveControlReferenceSpeed(
|
||||
Pose2D poseInWorld,
|
||||
TrajectoryProjection projection)
|
||||
{
|
||||
if (projection.RemainingDistanceMeters >
|
||||
TerminalApproachDistanceMeters)
|
||||
{
|
||||
return ResolveReferenceSpeedForControl(
|
||||
projection);
|
||||
}
|
||||
|
||||
return ResolveTerminalApproachSpeed(
|
||||
poseInWorld);
|
||||
}
|
||||
|
||||
private double ResolveCurvaturePreviewDistanceMeters(
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
double controlReferenceSpeedMetersPerSecond)
|
||||
{
|
||||
if (CurvaturePreviewSeconds <= 0.0 ||
|
||||
MaximumCurvaturePreviewMeters <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var previewSpeedMetersPerSecond =
|
||||
hasValidVelocityEstimate
|
||||
? CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
actualTwistInBody)
|
||||
: Math.Abs(
|
||||
controlReferenceSpeedMetersPerSecond);
|
||||
|
||||
return Math.Min(
|
||||
MaximumCurvaturePreviewMeters,
|
||||
previewSpeedMetersPerSecond *
|
||||
CurvaturePreviewSeconds);
|
||||
}
|
||||
|
||||
private double ResolveFeedforwardCurvaturePerMeter(
|
||||
TrajectoryProjection projection,
|
||||
double previewDistanceMeters)
|
||||
{
|
||||
var previewArcLengthMeters = Math.Min(
|
||||
_trajectory.TotalLengthMeters,
|
||||
projection.ArcLengthMeters +
|
||||
previewDistanceMeters);
|
||||
|
||||
return _trajectory
|
||||
.SampleAtArcLength(previewArcLengthMeters)
|
||||
.CurvaturePerMeter;
|
||||
}
|
||||
|
||||
private double ResolveTerminalApproachSpeed(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
var distanceToEndMeters =
|
||||
CalculateDistanceToEndMeters(
|
||||
poseInWorld);
|
||||
var headingErrorToEndRadians =
|
||||
CalculateHeadingErrorToEndRadians(
|
||||
poseInWorld);
|
||||
|
||||
if (distanceToEndMeters <=
|
||||
FinishDistanceMeters &&
|
||||
headingErrorToEndRadians <=
|
||||
FinishHeadingToleranceRadians)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var endPose = _trajectory.EndPoint.PoseInWorld;
|
||||
var deltaX = endPose.XMeters -
|
||||
poseInWorld.XMeters;
|
||||
var deltaY = endPose.YMeters -
|
||||
poseInWorld.YMeters;
|
||||
var longitudinalErrorMeters =
|
||||
deltaX * Math.Cos(endPose.YawRadians) +
|
||||
deltaY * Math.Sin(endPose.YawRadians);
|
||||
var remainingAlongTravelMeters =
|
||||
_terminalTravelDirection *
|
||||
longitudinalErrorMeters;
|
||||
|
||||
// 越过终点后不生成与原轨迹方向相反的修正速度。
|
||||
if (remainingAlongTravelMeters <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var speedMagnitudeMetersPerSecond =
|
||||
Math.Min(
|
||||
MaximumTerminalApproachSpeedMetersPerSecond,
|
||||
TerminalApproachGainPerSecond *
|
||||
remainingAlongTravelMeters);
|
||||
|
||||
return _terminalTravelDirection *
|
||||
speedMagnitudeMetersPerSecond;
|
||||
}
|
||||
|
||||
private double ResolveReferenceSpeedForControl(
|
||||
TrajectoryProjection projection)
|
||||
{
|
||||
var currentReferenceSpeed =
|
||||
ApplyTerminalBrakingPreview(
|
||||
projection,
|
||||
projection.ReferencePoint
|
||||
.ReferenceSpeedMetersPerSecond);
|
||||
var isInStartupRegion =
|
||||
projection.ArcLengthMeters <=
|
||||
StartupRegionMeters &&
|
||||
projection.RemainingDistanceMeters >
|
||||
FinishDistanceMeters;
|
||||
|
||||
if (!isInStartupRegion)
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
var previewArcLengthMeters = Math.Min(
|
||||
_trajectory.TotalLengthMeters,
|
||||
projection.ArcLengthMeters +
|
||||
StartupPreviewDistanceMeters);
|
||||
var previewReferenceSpeed =
|
||||
_trajectory
|
||||
.SampleAtArcLength(previewArcLengthMeters)
|
||||
.ReferenceSpeedMetersPerSecond;
|
||||
|
||||
if (Math.Abs(previewReferenceSpeed) <=
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond)
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
var startupReleaseSpeed =
|
||||
Math.Sign(previewReferenceSpeed) *
|
||||
Math.Min(
|
||||
Math.Abs(previewReferenceSpeed),
|
||||
MaximumStartupSpeedMetersPerSecond);
|
||||
|
||||
if (Math.Sign(currentReferenceSpeed) ==
|
||||
Math.Sign(startupReleaseSpeed) &&
|
||||
Math.Abs(currentReferenceSpeed) >=
|
||||
Math.Abs(startupReleaseSpeed))
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
return startupReleaseSpeed;
|
||||
}
|
||||
|
||||
private double ApplyTerminalBrakingPreview(
|
||||
TrajectoryProjection projection,
|
||||
double currentReferenceSpeed)
|
||||
{
|
||||
if (TerminalBrakingPreviewMeters <= 0.0)
|
||||
{
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
var previewArcLengthMeters = Math.Min(
|
||||
_trajectory.TotalLengthMeters,
|
||||
projection.ArcLengthMeters +
|
||||
TerminalBrakingPreviewMeters);
|
||||
var previewReferenceSpeed =
|
||||
_trajectory
|
||||
.SampleAtArcLength(previewArcLengthMeters)
|
||||
.ReferenceSpeedMetersPerSecond;
|
||||
var previewIsStop =
|
||||
Math.Abs(previewReferenceSpeed) <=
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||||
var hasSameDirection =
|
||||
Math.Sign(previewReferenceSpeed) ==
|
||||
Math.Sign(currentReferenceSpeed);
|
||||
var previewIsSlower =
|
||||
Math.Abs(previewReferenceSpeed) <
|
||||
Math.Abs(currentReferenceSpeed);
|
||||
|
||||
if (previewIsSlower &&
|
||||
(previewIsStop || hasSameDirection))
|
||||
{
|
||||
return previewReferenceSpeed;
|
||||
}
|
||||
|
||||
return currentReferenceSpeed;
|
||||
}
|
||||
|
||||
private static double ResolveTerminalTravelDirection(
|
||||
Trajectory2D trajectory)
|
||||
{
|
||||
for (var index = trajectory.Count - 1;
|
||||
index >= 0;
|
||||
index--)
|
||||
{
|
||||
var referenceSpeedMetersPerSecond =
|
||||
trajectory[index]
|
||||
.ReferenceSpeedMetersPerSecond;
|
||||
|
||||
if (Math.Abs(referenceSpeedMetersPerSecond) >
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond)
|
||||
{
|
||||
return Math.Sign(
|
||||
referenceSpeedMetersPerSecond);
|
||||
}
|
||||
}
|
||||
|
||||
throw new ArgumentException(
|
||||
"轨迹必须在终点前包含至少一个非零参考速度。",
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
private bool HasReachedEnd(
|
||||
Pose2D poseInWorld,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
TrajectoryProjection projection)
|
||||
{
|
||||
if (!hasValidVelocityEstimate)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return projection.RemainingDistanceMeters <=
|
||||
FinishDistanceMeters &&
|
||||
CalculateDistanceToEndMeters(poseInWorld) <=
|
||||
FinishDistanceMeters &&
|
||||
CalculateHeadingErrorToEndRadians(poseInWorld) <=
|
||||
FinishHeadingToleranceRadians &&
|
||||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
actualTwistInBody) <=
|
||||
FinishSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
private bool HasStoppedAtUnsatisfiedTerminal(
|
||||
Pose2D poseInWorld,
|
||||
Twist2D actualTwistInBody,
|
||||
bool hasValidVelocityEstimate,
|
||||
TrajectoryProjection projection,
|
||||
out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
|
||||
var isTerminalZeroSpeedReference =
|
||||
projection.RemainingDistanceMeters <=
|
||||
FinishDistanceMeters &&
|
||||
Math.Abs(
|
||||
projection.ReferencePoint
|
||||
.ReferenceSpeedMetersPerSecond) <=
|
||||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||||
|
||||
if (!isTerminalZeroSpeedReference ||
|
||||
!hasValidVelocityEstimate ||
|
||||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
actualTwistInBody) >
|
||||
FinishSpeedMetersPerSecond)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var positionErrorMeters =
|
||||
CalculateDistanceToEndMeters(
|
||||
poseInWorld);
|
||||
var headingErrorRadians =
|
||||
CalculateHeadingErrorToEndRadians(
|
||||
poseInWorld);
|
||||
|
||||
failureReason =
|
||||
"受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" +
|
||||
$"位置误差={positionErrorMeters:F3}m，" +
|
||||
"航向误差=" +
|
||||
$"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
|
||||
return true;
|
||||
}
|
||||
|
||||
private double CalculateDistanceToEndMeters(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
var endPose = _trajectory.EndPoint.PoseInWorld;
|
||||
var deltaX = poseInWorld.XMeters -
|
||||
endPose.XMeters;
|
||||
var deltaY = poseInWorld.YMeters -
|
||||
endPose.YMeters;
|
||||
|
||||
return Math.Sqrt(
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY);
|
||||
}
|
||||
|
||||
private double CalculateHeadingErrorToEndRadians(
|
||||
Pose2D poseInWorld)
|
||||
{
|
||||
return Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
_trajectory.EndPoint
|
||||
.PoseInWorld.YawRadians,
|
||||
poseInWorld.YawRadians));
|
||||
}
|
||||
|
||||
private double CalculateActualLongitudinalSpeedMetersPerSecond(
|
||||
Twist2D actualTwistInBody)
|
||||
{
|
||||
return Math.Abs(
|
||||
Math.Cos(_motionDirectionInBodyRadians) *
|
||||
actualTwistInBody.VxMetersPerSecond +
|
||||
Math.Sin(_motionDirectionInBodyRadians) *
|
||||
actualTwistInBody.VyMetersPerSecond);
|
||||
}
|
||||
|
||||
private void CompleteTrajectory()
|
||||
{
|
||||
ResetFeedbackControllers();
|
||||
_trajectory = null;
|
||||
IsActive = false;
|
||||
IsCompleted = true;
|
||||
LastRequestedCommand = new GcpMotionCommand(
|
||||
0.0,
|
||||
0.0,
|
||||
0.0);
|
||||
LastFailureReason = string.Empty;
|
||||
LastException = null;
|
||||
}
|
||||
|
||||
private void ResetFeedbackControllers()
|
||||
{
|
||||
_lateralController.Reset();
|
||||
_longitudinalController.Reset();
|
||||
}
|
||||
|
||||
private void ClearDiagnostics()
|
||||
{
|
||||
LastProjection = null;
|
||||
LastRequestedCommand = null;
|
||||
LastControlReferenceSpeedMetersPerSecond = null;
|
||||
LastCurvaturePreviewDistanceMeters = null;
|
||||
LastFeedforwardCurvaturePerMeter = null;
|
||||
LastFailureReason = string.Empty;
|
||||
LastException = null;
|
||||
}
|
||||
|
||||
private static PathTrackingCycleOutput CreateOutput(
|
||||
PathTrackingCycleResult result,
|
||||
GcpMotionCommand? command,
|
||||
TrajectoryProjection? projection,
|
||||
double projectionMilliseconds,
|
||||
long controllerComputeStartTimestamp)
|
||||
{
|
||||
var controllerComputeMilliseconds =
|
||||
controllerComputeStartTimestamp == 0L
|
||||
? 0.0
|
||||
: GetElapsedMilliseconds(
|
||||
controllerComputeStartTimestamp);
|
||||
|
||||
return new PathTrackingCycleOutput(
|
||||
result,
|
||||
command,
|
||||
projection,
|
||||
projectionMilliseconds,
|
||||
controllerComputeMilliseconds);
|
||||
}
|
||||
|
||||
private static double GetElapsedMilliseconds(
|
||||
long startTimestamp)
|
||||
{
|
||||
return (Stopwatch.GetTimestamp() - startTimestamp) *
|
||||
1000.0 /
|
||||
Stopwatch.Frequency;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -20,6 +20,8 @@ namespace MultiWheelC
|
||||
{
|
||||
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
public InPlaceRotationFeedbackMode FeedbackMode =
|
||||
InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
@@ -93,6 +95,12 @@ namespace MultiWheelC
|
||||
var targetWorldAngle =
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
var movementAngleTarget =
|
||||
FeedbackMode ==
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry
|
||||
? RelativeAngleDegrees
|
||||
: targetWorldAngle;
|
||||
|
||||
Console.WriteLine(
|
||||
"原地自转实际参数：" +
|
||||
@@ -106,13 +114,19 @@ namespace MultiWheelC
|
||||
$"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
|
||||
$"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
|
||||
$"起点航向={location.th:F2}°，" +
|
||||
$"目标航向={targetWorldAngle:F2}°。");
|
||||
$"目标航向={targetWorldAngle:F2}°，" +
|
||||
$"反馈模式={FeedbackMode}。");
|
||||
Console.WriteLine(
|
||||
"原地自转CSV保存目录：" +
|
||||
TrackingExperimentRecorder.DefaultOutputDirectory);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotateFilteredPID",
|
||||
controllerName:
|
||||
FeedbackMode ==
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry
|
||||
? "InPlaceRotateWheelOdometry"
|
||||
: "InPlaceRotateFilteredPID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
@@ -130,8 +144,8 @@ namespace MultiWheelC
|
||||
_task = new DriveTask(
|
||||
new MultiWheelRotateInPlace
|
||||
{
|
||||
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
|
||||
AngleTarget = targetWorldAngle,
|
||||
AngleTarget = movementAngleTarget,
|
||||
FeedbackMode = FeedbackMode,
|
||||
Chassis = chassis,
|
||||
StateProvider = stateProvider,
|
||||
CommandAngularSpeedObserver =
|
||||
@@ -165,6 +179,46 @@ namespace MultiWheelC
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
// 读取并校验测试使用的有符号相对旋转角度。
|
||||
protected static bool TryReadRelativeAngleDegrees(
|
||||
out float relativeAngleDegrees)
|
||||
{
|
||||
var input = UI.GetInput(
|
||||
"输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
|
||||
|
||||
if ((!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.CurrentCulture,
|
||||
out relativeAngleDegrees) &&
|
||||
!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.InvariantCulture,
|
||||
out relativeAngleDegrees)) ||
|
||||
float.IsNaN(relativeAngleDegrees) ||
|
||||
float.IsInfinity(relativeAngleDegrees))
|
||||
{
|
||||
Console.WriteLine("旋转角度输入无效，测试已经取消。");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
|
||||
{
|
||||
Console.WriteLine("旋转角度不能为0，测试已经取消。");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) >= 180f)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"输入角度必须满足-180° < angle < 180°。");
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
|
||||
@@ -181,58 +235,37 @@ namespace MultiWheelC
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
var input = UI.GetInput(
|
||||
"输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
|
||||
|
||||
if ((!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.CurrentCulture,
|
||||
out var relativeAngleDegrees) &&
|
||||
!float.TryParse(
|
||||
input,
|
||||
NumberStyles.Float,
|
||||
CultureInfo.InvariantCulture,
|
||||
out relativeAngleDegrees)) ||
|
||||
float.IsNaN(relativeAngleDegrees) ||
|
||||
float.IsInfinity(relativeAngleDegrees))
|
||||
if (!TryReadRelativeAngleDegrees(
|
||||
out var relativeAngleDegrees))
|
||||
{
|
||||
Console.WriteLine("旋转角度输入无效，测试已经取消。");
|
||||
return;
|
||||
}
|
||||
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
|
||||
return;
|
||||
}
|
||||
RelativeAngleDegrees = relativeAngleDegrees;
|
||||
base.Test();
|
||||
}
|
||||
}
|
||||
|
||||
var stateProvider =
|
||||
ParkingVehicleStateProviderFactory.Create(
|
||||
chassis);
|
||||
if (!stateProvider.TryGetState(out _))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"无法读取原地旋转起点状态：" +
|
||||
stateProvider.LastFailureReason);
|
||||
return;
|
||||
}
|
||||
[MovementTest(name = "轮组里程计：输入角度原地相对自转")]
|
||||
public sealed class TestWheelOdometryRotateAngle :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestWheelOdometryRotateAngle()
|
||||
: base(0f, "RotateWheelOdometryCustomAngle")
|
||||
{
|
||||
FeedbackMode =
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry;
|
||||
}
|
||||
|
||||
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
|
||||
/// <summary>
|
||||
/// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。
|
||||
/// </summary>
|
||||
public override void Test()
|
||||
{
|
||||
if (!TryReadRelativeAngleDegrees(
|
||||
out var relativeAngleDegrees))
|
||||
{
|
||||
Console.WriteLine("旋转角度不能为0，测试已经取消。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
|
||||
if (Math.Abs(relativeAngleDegrees) >= 180f)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"输入角度必须满足-180° < angle < 180°；" +
|
||||
"当前最短角控制不支持指定精确±180°的旋转方向。");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -1 +1,201 @@
|
||||
// 输出FleetMotionCommand
|
||||
using System;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Control.Execution;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 表示车队中心单周期轨迹控制的计算结果，不包含通信发送结果。
|
||||
public enum FleetControlCycleResult
|
||||
{
|
||||
Inactive = 0,
|
||||
CommandGenerated = 1,
|
||||
Completed = 2,
|
||||
Faulted = 3
|
||||
}
|
||||
|
||||
// 将公共轨迹核心生成的GCP命令转换为车队坐标系下的刚体速度命令。
|
||||
public sealed class FleetController
|
||||
{
|
||||
private readonly PathTrackingCore _trackingCore;
|
||||
|
||||
// virtualControlPointRadiusMeters必须与横向控制器采用的虚拟车队GCP半径一致。
|
||||
public FleetController(
|
||||
ILateralController lateralController,
|
||||
ILongitudinalController longitudinalController,
|
||||
GcpCommandAllocator gcpAllocator,
|
||||
double virtualControlPointRadiusMeters,
|
||||
double finishDistanceMeters = 0.04,
|
||||
double finishSpeedMetersPerSecond = 0.02,
|
||||
double finishHeadingToleranceRadians =
|
||||
3.0 * Math.PI / 180.0,
|
||||
double maximumDistanceToTrajectoryMeters = 0.30,
|
||||
double terminalBrakingPreviewMeters = 0.02,
|
||||
double terminalApproachDistanceMeters = 0.10,
|
||||
double terminalApproachGainPerSecond = 0.8,
|
||||
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
|
||||
double curvaturePreviewSeconds = 0.20,
|
||||
double maximumCurvaturePreviewMeters = 0.12)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
virtualControlPointRadiusMeters,
|
||||
nameof(virtualControlPointRadiusMeters));
|
||||
|
||||
VirtualControlPointRadiusMeters =
|
||||
virtualControlPointRadiusMeters;
|
||||
_trackingCore = new PathTrackingCore(
|
||||
lateralController,
|
||||
longitudinalController,
|
||||
gcpAllocator,
|
||||
finishDistanceMeters,
|
||||
finishSpeedMetersPerSecond,
|
||||
finishHeadingToleranceRadians,
|
||||
maximumDistanceToTrajectoryMeters,
|
||||
terminalBrakingPreviewMeters,
|
||||
terminalApproachDistanceMeters,
|
||||
terminalApproachGainPerSecond,
|
||||
maximumTerminalApproachSpeedMetersPerSecond,
|
||||
curvaturePreviewSeconds,
|
||||
maximumCurvaturePreviewMeters);
|
||||
}
|
||||
|
||||
// 虚拟车队中心到前、后GCP的距离，单位为m。
|
||||
public double VirtualControlPointRadiusMeters { get; }
|
||||
|
||||
public double FinishDistanceMeters =>
|
||||
_trackingCore.FinishDistanceMeters;
|
||||
|
||||
public double FinishSpeedMetersPerSecond =>
|
||||
_trackingCore.FinishSpeedMetersPerSecond;
|
||||
|
||||
public double FinishHeadingToleranceRadians =>
|
||||
_trackingCore.FinishHeadingToleranceRadians;
|
||||
|
||||
public double MaximumDistanceToTrajectoryMeters =>
|
||||
_trackingCore.MaximumDistanceToTrajectoryMeters;
|
||||
|
||||
public double TerminalBrakingPreviewMeters =>
|
||||
_trackingCore.TerminalBrakingPreviewMeters;
|
||||
|
||||
public double TerminalApproachDistanceMeters =>
|
||||
_trackingCore.TerminalApproachDistanceMeters;
|
||||
|
||||
public double TerminalApproachGainPerSecond =>
|
||||
_trackingCore.TerminalApproachGainPerSecond;
|
||||
|
||||
public double MaximumTerminalApproachSpeedMetersPerSecond =>
|
||||
_trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
|
||||
|
||||
public double CurvaturePreviewSeconds =>
|
||||
_trackingCore.CurvaturePreviewSeconds;
|
||||
|
||||
public double MaximumCurvaturePreviewMeters =>
|
||||
_trackingCore.MaximumCurvaturePreviewMeters;
|
||||
|
||||
public bool IsActive => _trackingCore.IsActive;
|
||||
|
||||
public bool IsCompleted => _trackingCore.IsCompleted;
|
||||
|
||||
public string LastFailureReason =>
|
||||
_trackingCore.LastFailureReason;
|
||||
|
||||
public Exception LastException =>
|
||||
_trackingCore.LastException;
|
||||
|
||||
public FleetState? LastFleetState { get; private set; }
|
||||
|
||||
public TrajectoryProjection? LastProjection =>
|
||||
_trackingCore.LastProjection;
|
||||
|
||||
public GcpMotionCommand? LastGcpCommand =>
|
||||
_trackingCore.LastRequestedCommand;
|
||||
|
||||
public FleetMotionCommand? LastCommand { get; private set; }
|
||||
|
||||
// 重置公共核心，并从轨迹起点开始跟踪虚拟车队中心。
|
||||
public void Start(Trajectory2D trajectory)
|
||||
{
|
||||
_trackingCore.Start(trajectory);
|
||||
LastFleetState = null;
|
||||
LastCommand = null;
|
||||
}
|
||||
|
||||
// 计算一周期车队中心命令；非运行状态、完成或故障时返回停车命令。
|
||||
public FleetControlCycleResult ComputeCommand(
|
||||
FleetState fleetState,
|
||||
double deltaTimeSeconds,
|
||||
out FleetMotionCommand command)
|
||||
{
|
||||
NumericGuard.EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
command = FleetMotionCommand.Stop();
|
||||
|
||||
if (!_trackingCore.IsActive)
|
||||
{
|
||||
return FleetControlCycleResult.Inactive;
|
||||
}
|
||||
|
||||
LastFleetState = fleetState;
|
||||
var output = _trackingCore.Compute(
|
||||
fleetState.FleetPoseInWorld,
|
||||
fleetState.TwistAtFleetOriginInFleet,
|
||||
fleetState.HasValidVelocityEstimate,
|
||||
deltaTimeSeconds);
|
||||
|
||||
if (output.Result ==
|
||||
PathTrackingCycleResult.Completed)
|
||||
{
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.Completed;
|
||||
}
|
||||
|
||||
if (output.Result ==
|
||||
PathTrackingCycleResult.Faulted)
|
||||
{
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.Faulted;
|
||||
}
|
||||
|
||||
if (output.Result !=
|
||||
PathTrackingCycleResult.CommandGenerated ||
|
||||
!output.Command.HasValue)
|
||||
{
|
||||
return FleetControlCycleResult.Inactive;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var twistAtFleetOrigin =
|
||||
GcpKinematics.ToBodyTwist(
|
||||
output.Command.Value,
|
||||
VirtualControlPointRadiusMeters);
|
||||
command = new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
twistAtFleetOrigin);
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.CommandGenerated;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_trackingCore.Fail(
|
||||
"车队中心GCP命令转换异常：" +
|
||||
exception.Message,
|
||||
exception);
|
||||
LastCommand = command;
|
||||
return FleetControlCycleResult.Faulted;
|
||||
}
|
||||
}
|
||||
|
||||
// 取消当前轨迹并使后续周期只生成停车命令。
|
||||
public void Cancel()
|
||||
{
|
||||
_trackingCore.Cancel();
|
||||
LastFleetState = null;
|
||||
LastCommand = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,64 @@
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Fleet
|
||||
{
|
||||
// 一次经过校验的虚拟车队原点状态快照。
|
||||
public readonly struct FleetState
|
||||
{
|
||||
public FleetState(
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D fleetPoseInWorld,
|
||||
Twist2D twistAtFleetOriginInWorld,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
NumericGuard.EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
NumericGuard.EnsureFinite(
|
||||
fleetPoseInWorld,
|
||||
nameof(fleetPoseInWorld));
|
||||
NumericGuard.EnsureFinite(
|
||||
twistAtFleetOriginInWorld,
|
||||
nameof(twistAtFleetOriginInWorld));
|
||||
|
||||
SampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
FleetPoseInWorld = new Pose2D(
|
||||
fleetPoseInWorld.XMeters,
|
||||
fleetPoseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
fleetPoseInWorld.YawRadians));
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
|
||||
// 位姿有效但速度尚未初始化时显式置零，避免控制器误用输入值。
|
||||
TwistAtFleetOriginInWorld =
|
||||
hasValidVelocityEstimate
|
||||
? twistAtFleetOriginInWorld
|
||||
: Twist2D.Zero;
|
||||
|
||||
var worldPoseInFleet =
|
||||
FrameTransform2D.Inverse(
|
||||
FleetPoseInWorld);
|
||||
TwistAtFleetOriginInFleet =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
worldPoseInFleet,
|
||||
TwistAtFleetOriginInWorld);
|
||||
}
|
||||
|
||||
// 状态源单调时钟中的采样时刻，单位为s。
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
// 车队坐标系原点在世界坐标系中的实际位姿。
|
||||
public Pose2D FleetPoseInWorld { get; }
|
||||
|
||||
// 车队原点处的实际刚体速度，在世界坐标系中表达。
|
||||
public Twist2D TwistAtFleetOriginInWorld { get; }
|
||||
|
||||
// 同一刚体速度在车队坐标系中表达，供车队控制器使用。
|
||||
public Twist2D TwistAtFleetOriginInFleet { get; }
|
||||
|
||||
// 速度是否已经初始化并可用于闭环控制。
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
}
|
||||
}
|
||||
@@ -12,15 +12,30 @@ using MultiWheelC.StateEstimation;
|
||||
namespace MultiWheelC
|
||||
{
|
||||
/// <summary>
|
||||
/// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。
|
||||
/// 指定原地自转使用Detour绝对航向或轮组相对角度反馈。
|
||||
/// </summary>
|
||||
public enum InPlaceRotationFeedbackMode
|
||||
{
|
||||
DetourAbsoluteHeading,
|
||||
RelativeWheelOdometry
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将四个舵轮准备到自转姿态，按所选反馈旋转并在完成后等待舵轮回正。
|
||||
/// </summary>
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// Detour模式表示世界目标航向，轮组模式表示有符号相对旋转角度，单位deg。
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置原地自转反馈模式；默认保持现有Detour绝对航向闭环。
|
||||
/// </summary>
|
||||
public InPlaceRotationFeedbackMode FeedbackMode =
|
||||
InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
|
||||
|
||||
// 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
|
||||
public Func<float> ThetaReader;
|
||||
|
||||
@@ -152,10 +167,32 @@ namespace MultiWheelC
|
||||
adapter.LastFailureReason);
|
||||
}
|
||||
|
||||
var useRelativeWheelOdometry =
|
||||
FeedbackMode ==
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry;
|
||||
var wheelStateProvider =
|
||||
useRelativeWheelOdometry
|
||||
? stateProvider as
|
||||
WheelFeedbackVehicleStateProvider
|
||||
: null;
|
||||
if (useRelativeWheelOdometry &&
|
||||
wheelStateProvider == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"轮组相对角度自转需要" +
|
||||
"WheelFeedbackVehicleStateProvider。");
|
||||
}
|
||||
|
||||
var targetAngle =
|
||||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||||
useRelativeWheelOdometry
|
||||
? AngleTarget
|
||||
: (float)AngleMath.NormalizeDegrees(
|
||||
AngleTarget);
|
||||
var currentAngle =
|
||||
ReadCurrentAngleDegrees(stateProvider);
|
||||
useRelativeWheelOdometry
|
||||
? 0f
|
||||
: ReadCurrentAngleDegrees(stateProvider);
|
||||
var cachedCurrentAngle = currentAngle;
|
||||
thPid = new PIDController(
|
||||
() => cachedCurrentAngle,
|
||||
@@ -170,6 +207,10 @@ namespace MultiWheelC
|
||||
pidParameters.SpeedAccPerSec);
|
||||
var lastCommandTime = DateTime.Now;
|
||||
var rotationStarted = DateTime.Now;
|
||||
var accumulatedWheelAngleRadians = 0.0;
|
||||
var previousWheelOmegaRadiansPerSecond = 0.0;
|
||||
var previousWheelTimestampSeconds = 0.0;
|
||||
var hasPreviousWheelSample = false;
|
||||
|
||||
while (true)
|
||||
{
|
||||
@@ -181,15 +222,74 @@ namespace MultiWheelC
|
||||
$"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
|
||||
}
|
||||
|
||||
currentAngle =
|
||||
ReadCurrentAngleDegrees(stateProvider);
|
||||
if (useRelativeWheelOdometry)
|
||||
{
|
||||
if (!wheelStateProvider.TryGetWheelTwist(
|
||||
out var wheelTwist,
|
||||
out var wheelTimestampSeconds))
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
adapter
|
||||
.StopXYThDrivePreserveSteeringState();
|
||||
|
||||
if (hasPreviousWheelSample)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"原地自转期间轮组角速度不可用：" +
|
||||
wheelStateProvider.LastFailureReason);
|
||||
}
|
||||
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (hasPreviousWheelSample)
|
||||
{
|
||||
var wheelDeltaTimeSeconds =
|
||||
wheelTimestampSeconds -
|
||||
previousWheelTimestampSeconds;
|
||||
if (!NumericGuard.IsFinite(
|
||||
wheelDeltaTimeSeconds) ||
|
||||
wheelDeltaTimeSeconds <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"轮组角速度采样时间没有单调递增。");
|
||||
}
|
||||
|
||||
accumulatedWheelAngleRadians +=
|
||||
0.5 *
|
||||
(previousWheelOmegaRadiansPerSecond +
|
||||
wheelTwist
|
||||
.OmegaRadiansPerSecond) *
|
||||
wheelDeltaTimeSeconds;
|
||||
}
|
||||
|
||||
previousWheelOmegaRadiansPerSecond =
|
||||
wheelTwist.OmegaRadiansPerSecond;
|
||||
previousWheelTimestampSeconds =
|
||||
wheelTimestampSeconds;
|
||||
hasPreviousWheelSample = true;
|
||||
currentAngle =
|
||||
(float)AngleMath.RadiansToDegrees(
|
||||
accumulatedWheelAngleRadians);
|
||||
}
|
||||
else
|
||||
{
|
||||
currentAngle =
|
||||
ReadCurrentAngleDegrees(stateProvider);
|
||||
}
|
||||
|
||||
cachedCurrentAngle = currentAngle;
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
var s = thPid.GetResponse(
|
||||
targetAngle,
|
||||
!useRelativeWheelOdometry);
|
||||
var angleErrorDegrees =
|
||||
(float)AngleMath
|
||||
.ShortestDifferenceDegrees(
|
||||
targetAngle,
|
||||
currentAngle);
|
||||
useRelativeWheelOdometry
|
||||
? targetAngle - currentAngle
|
||||
: (float)AngleMath
|
||||
.ShortestDifferenceDegrees(
|
||||
targetAngle,
|
||||
currentAngle);
|
||||
|
||||
// PID进入到位死区后等待其0.3s稳定确认；等待期间
|
||||
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
|
||||
@@ -255,7 +355,8 @@ namespace MultiWheelC
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
adapter.StopXYThDrivePreserveSteeringState();
|
||||
|
||||
if (IsLocalizationRecoveryPending(
|
||||
if (!useRelativeWheelOdometry &&
|
||||
IsLocalizationRecoveryPending(
|
||||
stateProvider))
|
||||
{
|
||||
BeginPostRotationPositionRecovery(
|
||||
@@ -309,7 +410,10 @@ namespace MultiWheelC
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"final rotate to {targetAngle}, wheels forward");
|
||||
useRelativeWheelOdometry
|
||||
? "final relative wheel rotate to " +
|
||||
$"{currentAngle:F2}deg, wheels forward"
|
||||
: $"final rotate to {targetAngle}, wheels forward");
|
||||
}
|
||||
finally
|
||||
{
|
||||
@@ -345,6 +449,33 @@ namespace MultiWheelC
|
||||
rotationTimeoutSeconds,
|
||||
nameof(RotationTimeoutSeconds));
|
||||
|
||||
if (float.IsNaN(AngleTarget) ||
|
||||
float.IsInfinity(AngleTarget))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(AngleTarget),
|
||||
"原地自转目标角度必须是有限值。");
|
||||
}
|
||||
|
||||
if (!Enum.IsDefined(
|
||||
typeof(InPlaceRotationFeedbackMode),
|
||||
FeedbackMode))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(FeedbackMode),
|
||||
"原地自转反馈模式无效。");
|
||||
}
|
||||
|
||||
if (FeedbackMode ==
|
||||
InPlaceRotationFeedbackMode
|
||||
.RelativeWheelOdometry &&
|
||||
Math.Abs(AngleTarget) >= 180f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(AngleTarget),
|
||||
"轮组相对自转角度必须满足-180° < angle < 180°。");
|
||||
}
|
||||
|
||||
if (pidParameters == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
|
||||
@@ -96,67 +96,19 @@ namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
try
|
||||
{
|
||||
var actualCarSpeed =
|
||||
_chassis.GetCarSpeed(true);
|
||||
var rawBodyVxMetersPerSecond =
|
||||
(double)actualCarSpeed.Vx;
|
||||
var rawBodyVyMetersPerSecond =
|
||||
(double)actualCarSpeed.Vy;
|
||||
// CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
|
||||
// 状态估计内部统一转换为rad/s。
|
||||
var rawBodyOmegaRadiansPerSecond =
|
||||
AngleMath.DegreesToRadians(
|
||||
actualCarSpeed.Vw);
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVxMetersPerSecond,
|
||||
"电机反馈车体纵向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVyMetersPerSecond,
|
||||
"电机反馈车体横向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
"电机反馈车体角速度");
|
||||
|
||||
var wheelSpeedTimestampSeconds =
|
||||
_wheelSpeedClock.Elapsed.TotalSeconds;
|
||||
|
||||
UpdateBodyVelocityFilters(
|
||||
rawBodyVxMetersPerSecond,
|
||||
rawBodyVyMetersPerSecond,
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
wheelSpeedTimestampSeconds,
|
||||
out var filteredBodyVxMetersPerSecond,
|
||||
out var filteredBodyVyMetersPerSecond,
|
||||
out var filteredBodyOmegaRadiansPerSecond,
|
||||
ReadFilteredWheelTwist(
|
||||
out var filteredWheelTwist,
|
||||
out _,
|
||||
out var hasValidWheelSpeedEstimate);
|
||||
|
||||
_latestRawWheelBodyVxMetersPerSecond =
|
||||
rawBodyVxMetersPerSecond;
|
||||
_latestFilteredWheelBodyVxMetersPerSecond =
|
||||
filteredBodyVxMetersPerSecond;
|
||||
_latestRawWheelBodyVyMetersPerSecond =
|
||||
rawBodyVyMetersPerSecond;
|
||||
_latestFilteredWheelBodyVyMetersPerSecond =
|
||||
filteredBodyVyMetersPerSecond;
|
||||
_latestRawWheelBodyOmegaRadiansPerSecond =
|
||||
rawBodyOmegaRadiansPerSecond;
|
||||
_latestFilteredWheelBodyOmegaRadiansPerSecond =
|
||||
filteredBodyOmegaRadiansPerSecond;
|
||||
_latestWheelSampleTimestampSeconds =
|
||||
wheelSpeedTimestampSeconds;
|
||||
_latestWheelVelocityValid =
|
||||
hasValidWheelSpeedEstimate;
|
||||
_latestWheelFeedbackReadSucceeded = true;
|
||||
|
||||
// Detour位姿跳变确认期间需要用轮速维持短时运动预测。
|
||||
if (_poseProvider is DetourVehicleStateProvider
|
||||
detourStateProvider)
|
||||
{
|
||||
detourStateProvider.UpdateWheelVelocityEstimate(
|
||||
filteredBodyVxMetersPerSecond,
|
||||
filteredBodyVyMetersPerSecond,
|
||||
filteredBodyOmegaRadiansPerSecond,
|
||||
filteredWheelTwist.VxMetersPerSecond,
|
||||
filteredWheelTwist.VyMetersPerSecond,
|
||||
filteredWheelTwist.OmegaRadiansPerSecond,
|
||||
hasValidWheelSpeedEstimate);
|
||||
}
|
||||
|
||||
@@ -175,13 +127,18 @@ namespace MultiWheelC.StateEstimation
|
||||
poseState.HasValidVelocityEstimate;
|
||||
_hasVelocityDiagnostics = true;
|
||||
|
||||
// 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
|
||||
// 避免轮速差和舵角误差放大Omega噪声。
|
||||
// 轮组反馈有效后统一使用滤波后的平面速度；初始化期间
|
||||
// 暂时保留Detour角速度作为回退值。
|
||||
var omegaRadiansPerSecond =
|
||||
hasValidWheelSpeedEstimate
|
||||
? filteredWheelTwist
|
||||
.OmegaRadiansPerSecond
|
||||
: poseState.TwistInBody
|
||||
.OmegaRadiansPerSecond;
|
||||
var twistInBody = new Twist2D(
|
||||
filteredBodyVxMetersPerSecond,
|
||||
filteredBodyVyMetersPerSecond,
|
||||
poseState.TwistInBody
|
||||
.OmegaRadiansPerSecond);
|
||||
filteredWheelTwist.VxMetersPerSecond,
|
||||
filteredWheelTwist.VyMetersPerSecond,
|
||||
omegaRadiansPerSecond);
|
||||
|
||||
var twistInWorld =
|
||||
FrameTransform2D
|
||||
@@ -210,6 +167,47 @@ namespace MultiWheelC.StateEstimation
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取并滤波轮组反馈速度，不访问Detour；首帧仅建立滤波时间基准并返回false。
|
||||
/// </summary>
|
||||
public bool TryGetWheelTwist(
|
||||
out Twist2D twistInBody,
|
||||
out double sampleTimestampSeconds)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
try
|
||||
{
|
||||
ReadFilteredWheelTwist(
|
||||
out var filteredWheelTwist,
|
||||
out sampleTimestampSeconds,
|
||||
out var hasValidWheelSpeedEstimate);
|
||||
|
||||
if (!hasValidWheelSpeedEstimate)
|
||||
{
|
||||
twistInBody = Twist2D.Zero;
|
||||
LastFailureReason =
|
||||
"轮组速度估计正在建立采样时间基准。";
|
||||
return false;
|
||||
}
|
||||
|
||||
twistInBody = filteredWheelTwist;
|
||||
LastFailureReason = string.Empty;
|
||||
return true;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_latestWheelFeedbackReadSucceeded = false;
|
||||
twistInBody = Twist2D.Zero;
|
||||
sampleTimestampSeconds = 0.0;
|
||||
LastFailureReason =
|
||||
"舵轮电机反馈车体速度解算失败：" +
|
||||
exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
|
||||
/// </summary>
|
||||
@@ -524,6 +522,73 @@ namespace MultiWheelC.StateEstimation
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从底盘读取一次轮组速度，统一转换为SI单位并更新共用低通滤波状态。
|
||||
/// </summary>
|
||||
private void ReadFilteredWheelTwist(
|
||||
out Twist2D filteredTwistInBody,
|
||||
out double sampleTimestampSeconds,
|
||||
out bool hasValidWheelSpeedEstimate)
|
||||
{
|
||||
var actualCarSpeed =
|
||||
_chassis.GetCarSpeed(true);
|
||||
var rawBodyVxMetersPerSecond =
|
||||
(double)actualCarSpeed.Vx;
|
||||
var rawBodyVyMetersPerSecond =
|
||||
(double)actualCarSpeed.Vy;
|
||||
// CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
|
||||
// 状态估计内部统一转换为rad/s。
|
||||
var rawBodyOmegaRadiansPerSecond =
|
||||
AngleMath.DegreesToRadians(
|
||||
actualCarSpeed.Vw);
|
||||
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVxMetersPerSecond,
|
||||
"电机反馈车体纵向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyVyMetersPerSecond,
|
||||
"电机反馈车体横向速度");
|
||||
NumericGuard.EnsureFinite(
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
"电机反馈车体角速度");
|
||||
|
||||
sampleTimestampSeconds =
|
||||
_wheelSpeedClock.Elapsed.TotalSeconds;
|
||||
|
||||
UpdateBodyVelocityFilters(
|
||||
rawBodyVxMetersPerSecond,
|
||||
rawBodyVyMetersPerSecond,
|
||||
rawBodyOmegaRadiansPerSecond,
|
||||
sampleTimestampSeconds,
|
||||
out var filteredBodyVxMetersPerSecond,
|
||||
out var filteredBodyVyMetersPerSecond,
|
||||
out var filteredBodyOmegaRadiansPerSecond,
|
||||
out hasValidWheelSpeedEstimate);
|
||||
|
||||
filteredTwistInBody = new Twist2D(
|
||||
filteredBodyVxMetersPerSecond,
|
||||
filteredBodyVyMetersPerSecond,
|
||||
filteredBodyOmegaRadiansPerSecond);
|
||||
|
||||
_latestRawWheelBodyVxMetersPerSecond =
|
||||
rawBodyVxMetersPerSecond;
|
||||
_latestFilteredWheelBodyVxMetersPerSecond =
|
||||
filteredBodyVxMetersPerSecond;
|
||||
_latestRawWheelBodyVyMetersPerSecond =
|
||||
rawBodyVyMetersPerSecond;
|
||||
_latestFilteredWheelBodyVyMetersPerSecond =
|
||||
filteredBodyVyMetersPerSecond;
|
||||
_latestRawWheelBodyOmegaRadiansPerSecond =
|
||||
rawBodyOmegaRadiansPerSecond;
|
||||
_latestFilteredWheelBodyOmegaRadiansPerSecond =
|
||||
filteredBodyOmegaRadiansPerSecond;
|
||||
_latestWheelSampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_latestWheelVelocityValid =
|
||||
hasValidWheelSpeedEstimate;
|
||||
_latestWheelFeedbackReadSucceeded = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
|
||||
/// </summary>
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user