diff --git a/MedullaAdapter/MedullaAdapter.csproj b/MedullaAdapter/MedullaAdapter.csproj
index e374356..c73779d 100644
--- a/MedullaAdapter/MedullaAdapter.csproj
+++ b/MedullaAdapter/MedullaAdapter.csproj
@@ -51,6 +51,9 @@
+
+
diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index b4d199e..9ff7b74 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
index 0fca26f..fb1eff4 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
index 246e2b6..d91a710 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC/AGV.cs b/MultiWheelC/AGV.cs
deleted file mode 100644
index 6592668..0000000
--- a/MultiWheelC/AGV.cs
+++ /dev/null
@@ -1,21 +0,0 @@
-using ClumsyCore;
-using MDCSToolBox.Clumsy.AgvInterfaces;
-using MDCSToolBox.Clumsy.MotionControllers;
-
-namespace MultiWheelC
-{
- public class AGV : MultiWheelInterface
- {
- public override AbstractGeometricController GetController()
- => new ChassisController().Get();
- public override MultiWheelMagTracker GetMagController()
- => new MultiWheelMagTracker();
- public override NaiveMagnetController GetNaiveMagnetController()
- => new NaiveMagnetController();
-
- public void Sleep(float seconds)
- {
- new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
- }
- }
-}
diff --git a/MultiWheelC/ChassisController.cs b/MultiWheelC/ChassisController.cs
deleted file mode 100644
index 1237da3..0000000
--- a/MultiWheelC/ChassisController.cs
+++ /dev/null
@@ -1,45 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.Pilot;
-using MDCSToolBox.Clumsy.MotionControllers;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-
-namespace MultiWheelC;
-
-public class ChassisController : MovementDefinition
-{
- public float BaseSpeed = Configuration.conf.basicSpeed;
-
- // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
- public override MultiWheelGeometricController Get()
- {
- return new MultiWheelGeometricController
- {
- Chassis = BasicPilotBase.Chassis,
- BaseSpeed = BaseSpeed,
- SlowDistance = PilotDefinition.Conf.SlowDistance,
- SlowingPow = PilotDefinition.Conf.SlowingPow,
- FinishDistance = PilotDefinition.Conf.FinishDistance,
- FinishSpeed = PilotDefinition.Conf.FinishSpeed,
- FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
- FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
- FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
- NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
- DebugMode = PilotDefinition.Conf.MotionDebugPrint,
- DebugCurvature = PilotDefinition.Conf.DebugCurvature,
- PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
- SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
- SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
- SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
- SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
- SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
- MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
- MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
- GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
- DthLinearFac = PilotDefinition.Conf.DthLinearFac,
- DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
- BiasFac = PilotDefinition.Conf.BiasFac,
- BiasThreshold = PilotDefinition.Conf.BiasThreshold,
- };
- }
-}
\ No newline at end of file
diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs
index 578468a..8495b80 100644
--- a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs
+++ b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs
@@ -15,20 +15,20 @@ namespace MultiWheelC.Control.Abstractions
public PathTrackingContext(
VehicleState vehicleState,
TrajectoryProjection projection,
- double referenceSpeedMetersPerSecond,
+ double controlReferenceSpeedMetersPerSecond,
double deltaTimeSeconds)
{
EnsureFinite(
- referenceSpeedMetersPerSecond,
- nameof(referenceSpeedMetersPerSecond));
+ controlReferenceSpeedMetersPerSecond,
+ nameof(controlReferenceSpeedMetersPerSecond));
EnsureFinitePositive(
deltaTimeSeconds,
nameof(deltaTimeSeconds));
VehicleState = vehicleState;
Projection = projection;
- ReferenceSpeedMetersPerSecond =
- referenceSpeedMetersPerSecond;
+ ControlReferenceSpeedMetersPerSecond =
+ controlReferenceSpeedMetersPerSecond;
DeltaTimeSeconds = deltaTimeSeconds;
}
@@ -48,9 +48,9 @@ namespace MultiWheelC.Control.Abstractions
public double DeltaTimeSeconds { get; }
///
- /// 获取轨迹投影点要求的有符号参考速度，单位为m/s。
+ /// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
///
- public double ReferenceSpeedMetersPerSecond { get; }
+ public double ControlReferenceSpeedMetersPerSecond { get; }
///
/// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
@@ -60,7 +60,7 @@ namespace MultiWheelC.Control.Abstractions
.VxMetersPerSecond;
///
- /// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。
+ /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
///
public double ReferenceCurvaturePerMeter =>
Projection.ReferencePoint
diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
index d8f6be4..633db00 100644
--- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs
+++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
@@ -29,6 +29,10 @@ namespace MultiWheelC.Control.Execution
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 IVehicleStateProvider _stateProvider;
private readonly ILateralController _lateralController;
@@ -103,7 +107,7 @@ namespace MultiWheelC.Control.Execution
public double FinishDistanceMeters { get; }
///
- /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
+ /// 获取判定轨迹执行完成时允许的最大实际纵向速度，单位为m/s。
///
public double FinishSpeedMetersPerSecond { get; }
@@ -161,7 +165,7 @@ namespace MultiWheelC.Control.Execution
///
/// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
///
- public double? LastReferenceSpeedMetersPerSecond { get; private set; }
+ public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
///
/// 停止当前底盘并从起点开始执行指定二维轨迹。
@@ -208,9 +212,18 @@ namespace MultiWheelC.Control.Execution
LastVehicleState = vehicleState;
- var projection = TrajectoryProjector.Project(
- _trajectory,
- vehicleState.PoseInWorld);
+ // 首周期允许全局定位轨迹进度；后续周期仅在上次进度
+ // 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。
+ var projection = LastProjection.HasValue
+ ? TrajectoryProjector.Project(
+ _trajectory,
+ vehicleState.PoseInWorld,
+ LastProjection.Value.ArcLengthMeters,
+ ProjectionBackwardSearchDistanceMeters,
+ ProjectionForwardSearchDistanceMeters)
+ : TrajectoryProjector.Project(
+ _trajectory,
+ vehicleState.PoseInWorld);
LastProjection = projection;
if (projection.DistanceToTrajectoryMeters >
@@ -240,15 +253,15 @@ namespace MultiWheelC.Control.Execution
terminalFailureReason);
}
- var referenceSpeedMetersPerSecond =
+ var controlReferenceSpeedMetersPerSecond =
ResolveReferenceSpeedForControl(
projection);
- LastReferenceSpeedMetersPerSecond =
- referenceSpeedMetersPerSecond;
+ LastControlReferenceSpeedMetersPerSecond =
+ controlReferenceSpeedMetersPerSecond;
var context = new PathTrackingContext(
vehicleState,
projection,
- referenceSpeedMetersPerSecond,
+ controlReferenceSpeedMetersPerSecond,
deltaTimeSeconds);
var lateralCommand =
_lateralController.Compute(context);
@@ -421,7 +434,7 @@ namespace MultiWheelC.Control.Execution
CalculateHeadingErrorToEndRadians(
vehicleState) <=
FinishHeadingToleranceRadians &&
- CalculateActualLinearSpeedMetersPerSecond(
+ CalculateActualLongitudinalSpeedMetersPerSecond(
vehicleState) <=
FinishSpeedMetersPerSecond;
}
@@ -446,7 +459,7 @@ namespace MultiWheelC.Control.Execution
if (!isTerminalZeroSpeedReference ||
!vehicleState.HasValidVelocityEstimate ||
- CalculateActualLinearSpeedMetersPerSecond(
+ CalculateActualLongitudinalSpeedMetersPerSecond(
vehicleState) >
FinishSpeedMetersPerSecond)
{
@@ -502,16 +515,13 @@ namespace MultiWheelC.Control.Execution
}
///
- /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
+ /// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
///
- private static double CalculateActualLinearSpeedMetersPerSecond(
+ private static double CalculateActualLongitudinalSpeedMetersPerSecond(
VehicleState vehicleState)
{
- return Math.Sqrt(
- vehicleState.TwistInBody.VxMetersPerSecond *
- vehicleState.TwistInBody.VxMetersPerSecond +
- vehicleState.TwistInBody.VyMetersPerSecond *
- vehicleState.TwistInBody.VyMetersPerSecond);
+ return Math.Abs(
+ vehicleState.TwistInBody.VxMetersPerSecond);
}
///
@@ -578,7 +588,7 @@ namespace MultiWheelC.Control.Execution
LastVehicleState = null;
LastProjection = null;
LastCommand = null;
- LastReferenceSpeedMetersPerSecond = null;
+ LastControlReferenceSpeedMetersPerSecond = null;
LastFailureReason = string.Empty;
LastException = null;
}
diff --git a/MultiWheelC/Control/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
index 9c281a7..da60f64 100644
--- a/MultiWheelC/Control/Lateral/StanleyLateralController.cs
+++ b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
@@ -99,10 +99,13 @@ namespace MultiWheelC.Control.Lateral
MinimumSpeedMetersPerSecond);
var travelDirection = SelectTravelDirection(context);
- // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
- var feedforwardAngleRadians = Math.Atan(
- context.ReferenceCurvaturePerMeter *
- ControlPointRadiusMeters);
+ // 参考曲率按轨迹点序的实际行进方向定义；倒车时底盘有符号
+ // 纵向速度反向，因此GCP曲率前馈也必须反向才能保持相同几何曲率。
+ var feedforwardAngleRadians =
+ travelDirection *
+ Math.Atan(
+ context.ReferenceCurvaturePerMeter *
+ ControlPointRadiusMeters);
// 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
var crossTrackCorrectionRadians =
@@ -155,7 +158,7 @@ namespace MultiWheelC.Control.Lateral
.ActualLongitudinalSpeedMetersPerSecond;
}
- return context.ReferenceSpeedMetersPerSecond;
+ return context.ControlReferenceSpeedMetersPerSecond;
}
///
@@ -166,11 +169,11 @@ namespace MultiWheelC.Control.Lateral
{
const double directionDeadbandMetersPerSecond = 1e-6;
- if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
+ if (Math.Abs(context.ControlReferenceSpeedMetersPerSecond) >
directionDeadbandMetersPerSecond)
{
return Math.Sign(
- context.ReferenceSpeedMetersPerSecond);
+ context.ControlReferenceSpeedMetersPerSecond);
}
if (context.HasValidVelocityEstimate &&
diff --git a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs
index ecf61f0..c84f5a4 100644
--- a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs
+++ b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs
@@ -85,16 +85,16 @@ namespace MultiWheelC.Control.Longitudinal
_feedbackPid.LastDerivativeOutput;
///
- /// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。
+ /// 根据本周期控制参考速度和实际纵向速度计算底盘命令速度。
///
public double ComputeSpeedMetersPerSecond(
PathTrackingContext context)
{
- var referenceSpeedMetersPerSecond =
- context.ReferenceSpeedMetersPerSecond;
+ var controlReferenceSpeedMetersPerSecond =
+ context.ControlReferenceSpeedMetersPerSecond;
// 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
- if (Math.Abs(referenceSpeedMetersPerSecond) <=
+ if (Math.Abs(controlReferenceSpeedMetersPerSecond) <=
ReferenceStopDeadbandMetersPerSecond)
{
Reset();
@@ -106,11 +106,11 @@ namespace MultiWheelC.Control.Longitudinal
{
Reset();
return LimitReferenceSpeed(
- referenceSpeedMetersPerSecond);
+ controlReferenceSpeedMetersPerSecond);
}
var speedErrorMetersPerSecond =
- referenceSpeedMetersPerSecond -
+ controlReferenceSpeedMetersPerSecond -
context.ActualLongitudinalSpeedMetersPerSecond;
// Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
@@ -120,24 +120,24 @@ namespace MultiWheelC.Control.Longitudinal
{
Reset();
return LimitReferenceSpeed(
- referenceSpeedMetersPerSecond);
+ controlReferenceSpeedMetersPerSecond);
}
GetCorrectionOutputRange(
- referenceSpeedMetersPerSecond,
+ controlReferenceSpeedMetersPerSecond,
out var minimumCorrectionMetersPerSecond,
out var maximumCorrectionMetersPerSecond);
var correctionMetersPerSecond =
_feedbackPid.Update(
- referenceSpeedMetersPerSecond,
+ controlReferenceSpeedMetersPerSecond,
context
.ActualLongitudinalSpeedMetersPerSecond,
context.DeltaTimeSeconds,
minimumCorrectionMetersPerSecond,
maximumCorrectionMetersPerSecond);
- return referenceSpeedMetersPerSecond +
+ return controlReferenceSpeedMetersPerSecond +
correctionMetersPerSecond;
}
diff --git a/MultiWheelC/Experiments/ClampTests.cs b/MultiWheelC/Experiments/ClampTests.cs
deleted file mode 100644
index 0fcaa41..0000000
--- a/MultiWheelC/Experiments/ClampTests.cs
+++ /dev/null
@@ -1,90 +0,0 @@
-
-
-// using System;
-// using ClumsyCore;
-// using ClumsyCore.Pilot;
-// using FundamentalLib;
-// using MDCSToolBox.Clumsy.Movements;
-// using MDCSToolBox.Clumsy.Pilot;
-
-// namespace MultiWheelC
-// {
-// public abstract class ClampMovementTestBase : MovementTest
-// {
-// public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
-
-// private DriveTask _task;
-// protected abstract bool Close { get; }
-
-// // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
-// public override void Test()
-// {
-// var leftTarget = Close
-// ? PilotDefinition.Self.LeftArmUpperPos
-// : PilotDefinition.Self.LeftArmLowerPos;
-// var rightTarget = Close
-// ? PilotDefinition.Self.RightArmUpperPos
-// : PilotDefinition.Self.RightArmLowerPos;
-
-// if (float.IsNaN(leftTarget) ||
-// float.IsInfinity(leftTarget) ||
-// float.IsNaN(rightTarget) ||
-// float.IsInfinity(rightTarget))
-// {
-// Console.WriteLine(
-// "夹臂目标位置无效，取消夹臂运动测试。");
-// return;
-// }
-
-// // 防止重复点击时上一项夹臂任务仍在运行。
-// TestStop();
-// Console.WriteLine(
-// $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
-// $"左目标={leftTarget}，右目标={rightTarget}");
-
-// var task = new DriveTask(
-// new ClampToTarget
-// {
-// LeftClampTarget = leftTarget,
-// RightClampTarget = rightTarget,
-// TimeoutSeconds = TimeoutSeconds
-// }.Get());
-// _task = task;
-
-// try
-// {
-// task.Wait();
-// }
-// finally
-// {
-// PilotDefinition.Self.SpeedLeftArm = 0f;
-// PilotDefinition.Self.SpeedRightArm = 0f;
-// if (ReferenceEquals(_task, task))
-// _task = null;
-// }
-// }
-
-// // 停止夹臂任务并立即清零左右夹臂下发速度。
-// public override void TestStop()
-// {
-// _task?.Stop();
-// _task = null;
-// PilotDefinition.Self.SpeedLeftArm = 0f;
-// PilotDefinition.Self.SpeedRightArm = 0f;
-// }
-// }
-
-// [MovementTest(name = "夹臂关闭测试")]
-// public sealed class TestClampCloseMovement
-// : ClampMovementTestBase
-// {
-// protected override bool Close => false;
-// }
-
-// [MovementTest(name = "夹臂启动测试")]
-// public sealed class TestClampOpenMovement
-// : ClampMovementTestBase
-// {
-// protected override bool Close => true;
-// }
-// }
diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
index cb612d9..e335698 100644
--- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+++ b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
@@ -59,11 +59,6 @@ namespace MultiWheelC
return;
}
- if (!MovementTestPreparation.AreWheelsForward())
- {
- return;
- }
-
var chassis = PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
@@ -72,22 +67,18 @@ namespace MultiWheelC
return;
}
- var detourStateProvider =
- new DetourVehicleStateProvider();
- if (!detourStateProvider.TryGetState(
+ var stateProvider =
+ ParkingVehicleStateProviderFactory.Create(
+ chassis);
+ if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
- "无法读取组合运动起点位姿：" +
- detourStateProvider.LastFailureReason);
+ "无法读取组合运动起点状态：" +
+ stateProvider.LastFailureReason);
return;
}
- var stateProvider =
- new WheelFeedbackVehicleStateProvider(
- detourStateProvider,
- chassis);
-
var planStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
@@ -176,15 +167,6 @@ namespace MultiWheelC
{
Segments = plan,
StateProvider = stateProvider,
- ConfigureTrackingMovement = tracking =>
- {
- tracking.StanleyUsesActualSpeed = true;
- tracking.MaximumCommandSpeedMetersPerSecond =
- StraightMaximumSpeedMetersPerSecond;
- tracking
- .LongitudinalSpeedErrorDeadbandMetersPerSecond =
- 0.025;
- },
SegmentStarted = (index, segment) =>
{
_recorder?.ClearControlReference();
@@ -330,14 +312,14 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
- controller.LastReferenceSpeedMetersPerSecond.HasValue)
+ controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
_recorder?.UpdateControlReference(
projection.ArcLengthMeters,
controller
- .LastReferenceSpeedMetersPerSecond.Value,
+ .LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index 0c8371e..950d528 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -150,11 +150,6 @@ namespace MultiWheelC
return;
}
- if (!MovementTestPreparation.AreWheelsForward())
- {
- return;
- }
-
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -164,22 +159,20 @@ namespace MultiWheelC
return;
}
- var detourStateProvider =
- new DetourVehicleStateProvider();
- if (!detourStateProvider.TryGetState(
+ var stateProvider =
+ ParkingVehicleStateProviderFactory.Create(
+ chassis);
+ if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
- "无法读取有效Detour起点位姿：" +
- detourStateProvider.LastFailureReason);
+ "无法读取有效停车状态起点位姿：" +
+ stateProvider.LastFailureReason);
_stateProvider = null;
return;
}
- _stateProvider =
- new WheelFeedbackVehicleStateProvider(
- detourStateProvider,
- chassis);
+ _stateProvider = stateProvider;
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
@@ -227,8 +220,6 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
- StanleyUsesActualSpeed = true,
- MaximumCommandSpeedMetersPerSecond = 0.50,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -358,13 +349,13 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
- controller.LastReferenceSpeedMetersPerSecond.HasValue)
+ controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
recorder.UpdateControlReference(
projection.ArcLengthMeters,
- controller.LastReferenceSpeedMetersPerSecond.Value,
+ controller.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
@@ -511,11 +502,6 @@ namespace MultiWheelC
return;
}
- if (!MovementTestPreparation.AreWheelsForward())
- {
- return;
- }
-
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -525,22 +511,20 @@ namespace MultiWheelC
return;
}
- var detourStateProvider =
- new DetourVehicleStateProvider();
- if (!detourStateProvider.TryGetState(
+ var stateProvider =
+ ParkingVehicleStateProviderFactory.Create(
+ chassis);
+ if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
- "无法读取有效Detour起点位姿：" +
- detourStateProvider.LastFailureReason);
+ "无法读取有效停车状态起点位姿：" +
+ stateProvider.LastFailureReason);
_stateProvider = null;
return;
}
- _stateProvider =
- new WheelFeedbackVehicleStateProvider(
- detourStateProvider,
- chassis);
+ _stateProvider = stateProvider;
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
@@ -593,9 +577,6 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
- StanleyUsesActualSpeed = true,
- MaximumCommandSpeedMetersPerSecond =
- StraightMaximumSpeedMetersPerSecond,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -725,13 +706,13 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
- controller.LastReferenceSpeedMetersPerSecond.HasValue)
+ controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
recorder.UpdateControlReference(
projection.ArcLengthMeters,
- controller.LastReferenceSpeedMetersPerSecond.Value,
+ controller.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
diff --git a/MultiWheelC/Experiments/PathTrackingTests.cs b/MultiWheelC/Experiments/PathTrackingTests.cs
deleted file mode 100644
index a3f82d9..0000000
--- a/MultiWheelC/Experiments/PathTrackingTests.cs
+++ /dev/null
@@ -1,635 +0,0 @@
-
-
-using System;
-using System.Collections.Generic;
-using System.Numerics;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using FundamentalLib;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-using MDCSToolBox.Clumsy.Tracks;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
- [MovementTest(name = "SendMotion：连续前进4m")]
- public class TestForward4m : MovementTest
- {
- public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
- public float CruiseSpeed = 0.3f; // 巡航速度上限，单位m/s。
- public int TrialNumber = 1; // 重复实验编号。
- private DriveTask _task;
- private TrackingExperimentRecorder _recorder;
- // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
- public override void Test()
- {
- if (!MovementTestPreparation.AreWheelsForward())
- {
- return;
- }
-
- var location = DetourInterface.getCartLocation();
- if (double.IsNaN(location.x) ||
- double.IsInfinity(location.x) ||
- double.IsNaN(location.y) ||
- double.IsInfinity(location.y) ||
- double.IsNaN(location.th) ||
- double.IsInfinity(location.th))
- {
- Console.WriteLine(
- "Detour当前位姿无效，取消连续前进4m测试。");
- return;
- }
- var source = new Vector2((float)location.x, (float)location.y);
- // Detour航向单位是度，三角函数需要弧度。
- var headingRadians =
- AngleMath.DegreesToRadians(location.th);
- var destination = new Vector2(
- source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
- source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
- _recorder =
- new TrackingExperimentRecorder(
- controllerName: "LegacyGeometricController",
- trajectoryName: "LegacyStraight4m",
- trialNumber: TrialNumber,
- referenceStart: source,
- referenceEnd: destination,
- referenceSpeed: CruiseSpeed);
- _recorder.Start();
- try
- {
- _task = new DriveTask(
- new DstTracker
- {
- Src = source,
- Dst = destination,
- CarDirectionBias = 0f,
- MaxSpeed = CruiseSpeed
- }.Get());
- _task.Wait();
- // 保留少量停车后数据，便于观察速度是否回到零。
- Thread.Sleep(300);
- }
- finally
- {
- _task?.Stop();
- _recorder?.UpdateCommand(0f, 0f);
- _recorder?.StopAndSave();
- _task = null;
- _recorder = null;
- }
- }
-
- public override void TestStop()
- {
- _task?.Stop();
- _recorder?.UpdateCommand(0f, 0f);
- _recorder?.StopAndSave();
- }
- }
-
- [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
- public class TestArcMovement : MovementTest
- {
- public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
- public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
- public int TrialNumber = 1; // 重复实验编号。
-
- private DriveTask _task;
- private TrackingExperimentRecorder _recorder;
-
- // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
- public override void Test()
- {
- if (float.IsNaN(RadiusMillimeters) ||
- float.IsInfinity(RadiusMillimeters) ||
- RadiusMillimeters <= 0f ||
- float.IsNaN(CruiseSpeed) ||
- float.IsInfinity(CruiseSpeed) ||
- CruiseSpeed <= 0f)
- {
- Console.WriteLine("圆弧运动测试参数无效。");
- return;
- }
-
- if (!MovementTestPreparation.AreWheelsForward())
- {
- return;
- }
-
- var location = DetourInterface.getCartLocation();
- if (double.IsNaN(location.x) ||
- double.IsInfinity(location.x) ||
- double.IsNaN(location.y) ||
- double.IsInfinity(location.y) ||
- double.IsNaN(location.th) ||
- double.IsInfinity(location.th))
- {
- Console.WriteLine(
- "Detour当前位姿无效，取消圆弧运动测试。");
- return;
- }
-
- var source =
- new Vector2((float)location.x, (float)location.y);
- var headingRadians =
- AngleMath.DegreesToRadians(location.th);
-
- // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
- var center = new Vector2(
- source.X -
- RadiusMillimeters *
- (float)Math.Sin(headingRadians),
- source.Y +
- RadiusMillimeters *
- (float)Math.Cos(headingRadians));
-
- // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
- var startRadialAngleDegrees =
- (float)location.th - 90f;
-
- var controller = new ChassisController
- {
- BaseSpeed = CruiseSpeed
- }.Get();
- controller.FinishSpeed = 0f;
-
- var arc = new CircularArcTrack(
- center,
- RadiusMillimeters,
- startRadialAngleDegrees,
- startRadialAngleDegrees + 90f,
- direction: 1)
- {
- Speed = CruiseSpeed,
- CarDirectionBias = 0f
- };
-
- // 左转90°后，圆心到终点的径向方向等于起始车头方向。
- var destination = center + new Vector2(
- RadiusMillimeters *
- (float)Math.Cos(headingRadians),
- RadiusMillimeters *
- (float)Math.Sin(headingRadians));
-
- if (!controller.AddTrack(arc, "LeftArc90Degrees"))
- {
- Console.WriteLine(
- "左转90°圆弧轨迹添加失败，取消测试。");
- return;
- }
-
- _recorder = new TrackingExperimentRecorder(
- controllerName: "LegacyGeometricController",
- trajectoryName:
- $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
- trialNumber: TrialNumber,
- referenceStart: source,
- referenceEnd: destination,
- referenceSpeed: CruiseSpeed);
- _recorder.Start();
-
- try
- {
- _task = new DriveTask(controller.Track());
- _task.Wait();
-
- // 保留少量停车后的样本，用于观察速度是否回到零。
- Thread.Sleep(300);
- }
- finally
- {
- _task?.Stop();
- _recorder?.UpdateCommand(0f, 0f);
- _recorder?.StopAndSave();
- _task = null;
- _recorder = null;
- }
- }
-
- // 停止圆弧运动并保存当前已经采集的实验数据。
- public override void TestStop()
- {
- _task?.Stop();
- _recorder?.UpdateCommand(0f, 0f);
- _recorder?.StopAndSave();
- }
- }
-
- [MovementTest(name = "SendMotion：蟹行直线4m")]
- public class TestCrabForward4m : MovementTest
- {
- public float DistanceMillimeters = 4000f;
- public float CruiseSpeed = 0.2f;
- public int TrialNumber = 1;
-
- private DriveTask _task;
- private TrackingExperimentRecorder _recorder;
-
- // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
- public override void Test()
- {
- if (!TryReadStartPose(
- out var source,
- out var bodyYawRadians))
- return;
-
- var motionYaw =
- bodyYawRadians + Math.PI / 2.0;
- var destination = new Vector2(
- source.X +
- DistanceMillimeters *
- (float)Math.Cos(motionYaw),
- source.Y +
- DistanceMillimeters *
- (float)Math.Sin(motionYaw));
-
- var tracker = new CrabMotionFrameTracker
- {
- CommandBackend =
- CrabMotionFrameTracker
- .ChassisCommandBackend
- .SendMotion,
- PathKind =
- CrabMotionFrameTracker
- .ReferencePathKind.Straight,
- StartPosition = source,
- InitialBodyYawRadians =
- bodyYawRadians,
- LengthMillimeters =
- DistanceMillimeters,
- CruiseSpeed = CruiseSpeed
- };
-
- _recorder = new TrackingExperimentRecorder(
- controllerName:
- "CrabSendMotionTracker",
- trajectoryName:
- "CrabStraight4m",
- trialNumber: TrialNumber,
- referenceStart: source,
- referenceEnd: destination,
- referenceSpeed: CruiseSpeed,
- referenceMotionFrameYawDegrees: 90f);
- tracker.CommandObserver =
- (vx, vy, omega) =>
- _recorder?.UpdateBodyCommand(
- vx,
- vy,
- omega);
- _recorder.Start();
-
- try
- {
- _task = new DriveTask(tracker.Get());
- _task.Wait();
- Thread.Sleep(300);
- }
- finally
- {
- _task?.Stop();
- _recorder?.UpdateBodyCommand(
- 0f,
- 0f,
- 0f);
- _recorder?.StopAndSave();
- _task = null;
- _recorder = null;
- }
- }
-
- public override void TestStop()
- {
- _task?.Stop();
- _recorder?.UpdateBodyCommand(
- 0f,
- 0f,
- 0f);
- _recorder?.StopAndSave();
- }
-
- // 读取并校验测试开始时的Detour世界位姿。
- private static bool TryReadStartPose(
- out Vector2 source,
- out double bodyYawRadians)
- {
- var location =
- DetourInterface.getCartLocation();
- if (double.IsNaN(location.x) ||
- double.IsInfinity(location.x) ||
- double.IsNaN(location.y) ||
- double.IsInfinity(location.y) ||
- double.IsNaN(location.th) ||
- double.IsInfinity(location.th))
- {
- Console.WriteLine(
- "Detour当前位姿无效，取消蟹行直线测试。");
- source = Vector2.Zero;
- bodyYawRadians = 0.0;
- return false;
- }
-
- source = new Vector2(
- (float)location.x,
- (float)location.y);
- bodyYawRadians =
- AngleMath.DegreesToRadians(location.th);
- return true;
- }
- }
-
- [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
- public class TestCrabLeftArc90 : MovementTest
- {
- public float RadiusMillimeters = 2000f;
- public float CruiseSpeed = 0.2f;
- public int TrialNumber = 1;
-
- private DriveTask _task;
- private TrackingExperimentRecorder _recorder;
-
- // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
- public override void Test()
- {
- var location =
- DetourInterface.getCartLocation();
- if (double.IsNaN(location.x) ||
- double.IsInfinity(location.x) ||
- double.IsNaN(location.y) ||
- double.IsInfinity(location.y) ||
- double.IsNaN(location.th) ||
- double.IsInfinity(location.th))
- {
- Console.WriteLine(
- "Detour当前位姿无效，取消蟹行圆弧测试。");
- return;
- }
-
- var source = new Vector2(
- (float)location.x,
- (float)location.y);
- var bodyYawRadians =
- AngleMath.DegreesToRadians(location.th);
- var tracker = new CrabMotionFrameTracker
- {
- CommandBackend =
- CrabMotionFrameTracker
- .ChassisCommandBackend
- .SendMotion,
- PathKind =
- CrabMotionFrameTracker
- .ReferencePathKind.LeftArc,
- StartPosition = source,
- InitialBodyYawRadians =
- bodyYawRadians,
- RadiusMillimeters =
- RadiusMillimeters,
- ArcSweepRadians = Math.PI / 2.0,
- CruiseSpeed = CruiseSpeed
- };
- var destination =
- tracker.GetArcDestination();
-
- _recorder = new TrackingExperimentRecorder(
- controllerName:
- "CrabSendMotionTracker",
- trajectoryName:
- $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
- trialNumber: TrialNumber,
- referenceStart: source,
- referenceEnd: destination,
- referenceSpeed: CruiseSpeed,
- referenceMotionFrameYawDegrees: 90f);
- tracker.CommandObserver =
- (vx, vy, omega) =>
- _recorder?.UpdateBodyCommand(
- vx,
- vy,
- omega);
- _recorder.Start();
-
- try
- {
- _task = new DriveTask(tracker.Get());
- _task.Wait();
- Thread.Sleep(300);
- }
- finally
- {
- _task?.Stop();
- _recorder?.UpdateBodyCommand(
- 0f,
- 0f,
- 0f);
- _recorder?.StopAndSave();
- _task = null;
- _recorder = null;
- }
- }
-
- public override void TestStop()
- {
- _task?.Stop();
- _recorder?.UpdateBodyCommand(
- 0f,
- 0f,
- 0f);
- _recorder?.StopAndSave();
- }
- }
-
- [MovementTest(name = "SendMotion：4m S型曲线")]
- public class TestSCurve4m : MovementTest
- {
- public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
- public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
- public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
- public int TrialNumber = 1; // 重复实验编号。
-
- private DriveTask _task;
- private TrackingExperimentRecorder _recorder;
-
- // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
- public override void Test()
- {
- if (float.IsNaN(LengthMillimeters) ||
- float.IsInfinity(LengthMillimeters) ||
- LengthMillimeters <= 0f ||
- float.IsNaN(LateralOffsetMillimeters) ||
- float.IsInfinity(LateralOffsetMillimeters) ||
- LateralOffsetMillimeters <= 0f ||
- float.IsNaN(CruiseSpeed) ||
- float.IsInfinity(CruiseSpeed) ||
- CruiseSpeed <= 0f)
- {
- Console.WriteLine("S型曲线测试参数无效。");
- return;
- }
-
- if (!MovementTestPreparation.AreWheelsForward())
- return;
-
- var location = DetourInterface.getCartLocation();
- if (double.IsNaN(location.x) ||
- double.IsInfinity(location.x) ||
- double.IsNaN(location.y) ||
- double.IsInfinity(location.y) ||
- double.IsNaN(location.th) ||
- double.IsInfinity(location.th))
- {
- Console.WriteLine(
- "Detour当前位姿无效，取消4m S型曲线测试。");
- return;
- }
-
- var source =
- new Vector2((float)location.x, (float)location.y);
- var headingRadians =
- AngleMath.DegreesToRadians(location.th);
- var length = LengthMillimeters;
- var offset = LateralOffsetMillimeters;
-
- // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
- // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
- var firstControlPoints = new List
- {
- LocalToWorld(source, headingRadians, 0f, 0f),
- LocalToWorld(
- source, headingRadians,
- length / 12f, 0f),
- LocalToWorld(
- source, headingRadians,
- length / 6f, offset),
- LocalToWorld(
- source, headingRadians,
- length * 0.25f, offset)
- };
- var secondControlPoints = new List
- {
- LocalToWorld(
- source, headingRadians,
- length * 0.25f, offset),
- LocalToWorld(
- source, headingRadians,
- length / 3f, offset),
- LocalToWorld(
- source, headingRadians,
- length * 2f / 3f, -offset),
- LocalToWorld(
- source, headingRadians,
- length * 0.75f, -offset)
- };
- var thirdControlPoints = new List
- {
- LocalToWorld(
- source, headingRadians,
- length * 0.75f, -offset),
- LocalToWorld(
- source, headingRadians,
- length * 5f / 6f, -offset),
- LocalToWorld(
- source, headingRadians,
- length * 11f / 12f, 0f),
- LocalToWorld(
- source, headingRadians,
- length, 0f)
- };
-
- var firstTrack = new BezierTrack(firstControlPoints)
- {
- Speed = CruiseSpeed,
- CarDirectionBias = 0f
- };
- var secondTrack = new BezierTrack(secondControlPoints)
- {
- Speed = CruiseSpeed,
- CarDirectionBias = 0f
- };
- var thirdTrack = new BezierTrack(thirdControlPoints)
- {
- Speed = CruiseSpeed,
- CarDirectionBias = 0f
- };
-
- var controller = new ChassisController
- {
- BaseSpeed = CruiseSpeed
- }.Get();
- controller.FinishSpeed = 0f;
-
- if (!controller.AddTrack(
- firstTrack,
- "SCurve4m-Part1") ||
- !controller.AddTrack(
- secondTrack,
- "SCurve4m-Part2") ||
- !controller.AddTrack(
- thirdTrack,
- "SCurve4m-Part3"))
- {
- Console.WriteLine(
- "4m S型曲线轨迹添加失败，取消测试。");
- return;
- }
-
- var destination =
- LocalToWorld(
- source,
- headingRadians,
- length,
- 0f);
- _recorder = new TrackingExperimentRecorder(
- controllerName: "LegacyGeometricController",
- trajectoryName:
- $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
- trialNumber: TrialNumber,
- referenceStart: source,
- referenceEnd: destination,
- referenceSpeed: CruiseSpeed);
- _recorder.Start();
-
- try
- {
- _task = new DriveTask(controller.Track());
- _task.Wait();
-
- // 保留少量停止后的数据，用于观察速度是否回到零。
- Thread.Sleep(300);
- }
- finally
- {
- _task?.Stop();
- _recorder?.UpdateCommand(0f, 0f);
- _recorder?.StopAndSave();
- _task = null;
- _recorder = null;
- }
- }
-
- // 停止S型曲线测试并保存当前已经采集的数据。
- public override void TestStop()
- {
- _task?.Stop();
- _recorder?.UpdateCommand(0f, 0f);
- _recorder?.StopAndSave();
- }
-
- // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
- private static Vector2 LocalToWorld(
- Vector2 origin,
- double headingRadians,
- float localX,
- float localY)
- {
- var cos = (float)Math.Cos(headingRadians);
- var sin = (float)Math.Sin(headingRadians);
-
- return new Vector2(
- origin.X + localX * cos - localY * sin,
- origin.Y + localX * sin + localY * cos);
- }
- }
-}
diff --git a/MultiWheelC/Experiments/RotationTests.cs b/MultiWheelC/Experiments/RotationTests.cs
index 08b3a45..41e9a4d 100644
--- a/MultiWheelC/Experiments/RotationTests.cs
+++ b/MultiWheelC/Experiments/RotationTests.cs
@@ -10,7 +10,6 @@ using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
-using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;
namespace MultiWheelC
@@ -109,29 +108,6 @@ namespace MultiWheelC
{
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
AngleTarget = targetWorldAngle,
- PidparamsRead = () => new PIDParams
- {
- Kp =
- config.InPlaceRotateKp,
- Ki =
- config.InPlaceRotateKi,
- Kd =
- config.InPlaceRotateKd,
- DeadZone =
- config.InPlaceRotateArriveDeg,
- SpeedAccPerSec =
- config.InPlaceRotateAcc,
- OutputUpperThreshold =
- config.InPlaceRotateMaxSpeed,
- MaxI =
- config.InPlaceRotateMaxI
- },
- MinimumAngularSpeedDegreesPerSecond =
- config.InPlaceRotateMinimumSpeed,
- WheelAlignmentToleranceDegrees =
- config.InPlaceRotateWheelAlignDeg,
- RotationTimeoutSeconds =
- config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
diff --git a/MultiWheelC/Experiments/TestTrajectoryFactory.cs b/MultiWheelC/Experiments/TestTrajectoryFactory.cs
index e0f66b4..aad3f1b 100644
--- a/MultiWheelC/Experiments/TestTrajectoryFactory.cs
+++ b/MultiWheelC/Experiments/TestTrajectoryFactory.cs
@@ -13,7 +13,7 @@ namespace MultiWheelC
private const double StraightLengthMeters = 4.0;
///
- /// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。
+ /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的4m直线轨迹。
///
public static Trajectory2D CreateStraight4Meters(
Pose2D startPoseInWorld,
@@ -32,7 +32,7 @@ namespace MultiWheelC
}
///
- /// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
+ /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
///
public static Trajectory2D CreateStraight(
Pose2D startPoseInWorld,
@@ -42,22 +42,22 @@ namespace MultiWheelC
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
{
- EnsureFinitePose(
+ NumericGuard.EnsureFinite(
startPoseInWorld,
nameof(startPoseInWorld));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
lengthMeters,
nameof(lengthMeters));
- EnsureFinitePositive(
+ var travelDirection = GetTravelDirection(
cruiseSpeedMetersPerSecond,
nameof(cruiseSpeedMetersPerSecond));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
decelerationMetersPerSecondSquared,
nameof(decelerationMetersPerSecondSquared));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
@@ -73,10 +73,10 @@ namespace MultiWheelC
pointSpacingMeters);
var points = new List(
segmentCount + 1);
- var directionX = Math.Cos(
- startPoseInWorld.YawRadians);
- var directionY = Math.Sin(
- startPoseInWorld.YawRadians);
+ var directionX = travelDirection *
+ Math.Cos(startPoseInWorld.YawRadians);
+ var directionY = travelDirection *
+ Math.Sin(startPoseInWorld.YawRadians);
for (var index = 0;
index <= segmentCount;
@@ -116,7 +116,7 @@ namespace MultiWheelC
}
///
- /// 从当前位姿生成“3m直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
+ /// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
///
public static Trajectory2D CreateStraightLeftSemicircleStraight(
Pose2D startPoseInWorld,
@@ -143,7 +143,7 @@ namespace MultiWheelC
}
///
- /// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
+ /// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
///
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
Pose2D startPoseInWorld,
@@ -157,34 +157,33 @@ namespace MultiWheelC
double decelerationMetersPerSecondSquared,
double pointSpacingMeters)
{
- EnsureFinitePose(
+ NumericGuard.EnsureFinite(
startPoseInWorld,
nameof(startPoseInWorld));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
straightLengthMeters,
nameof(straightLengthMeters));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
turnRadiusMeters,
nameof(turnRadiusMeters));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
turnAngleRadians,
nameof(turnAngleRadians));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
curvatureTransitionLengthMeters,
nameof(curvatureTransitionLengthMeters));
- EnsureFinitePositive(
+ var travelDirection = GetCommonTravelDirection(
straightMaximumSpeedMetersPerSecond,
- nameof(straightMaximumSpeedMetersPerSecond));
- EnsureFinitePositive(
+ nameof(straightMaximumSpeedMetersPerSecond),
turnMaximumSpeedMetersPerSecond,
nameof(turnMaximumSpeedMetersPerSecond));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
decelerationMetersPerSecondSquared,
nameof(decelerationMetersPerSecondSquared));
- EnsureFinitePositive(
+ NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
@@ -245,8 +244,10 @@ namespace MultiWheelC
turnStartArcLengthMeters &&
arcLengthMeters <=
turnEndArcLengthMeters
- ? turnMaximumSpeedMetersPerSecond
- : straightMaximumSpeedMetersPerSecond;
+ ? Math.Abs(
+ turnMaximumSpeedMetersPerSecond)
+ : Math.Abs(
+ straightMaximumSpeedMetersPerSecond);
}
ApplyAccelerationAndBrakingLimits(
@@ -256,6 +257,14 @@ namespace MultiWheelC
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared);
+ for (var index = 0;
+ index < referenceSpeeds.Length;
+ index++)
+ {
+ referenceSpeeds[index] *=
+ travelDirection;
+ }
+
var points = new List(
sampleArcLengths.Count);
var startCos = Math.Cos(
@@ -296,10 +305,10 @@ namespace MultiWheelC
if (Math.Abs(segmentCurvaturePerMeter) <=
1e-12)
{
- localX +=
+ localX += travelDirection *
Math.Cos(localYawRadians) *
segmentLengthMeters;
- localY +=
+ localY += travelDirection *
Math.Sin(localYawRadians) *
segmentLengthMeters;
}
@@ -308,11 +317,11 @@ namespace MultiWheelC
var nextYawRadians =
localYawRadians +
segmentYawChangeRadians;
- localX +=
+ localX += travelDirection *
(Math.Sin(nextYawRadians) -
Math.Sin(localYawRadians)) /
segmentCurvaturePerMeter;
- localY +=
+ localY += travelDirection *
(Math.Cos(localYawRadians) -
Math.Cos(nextYawRadians)) /
segmentCurvaturePerMeter;
@@ -490,7 +499,7 @@ namespace MultiWheelC
}
///
- /// 对逐点速度上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
+ /// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
///
private static void ApplyAccelerationAndBrakingLimits(
IReadOnlyList arcLengthsMeters,
@@ -547,7 +556,7 @@ namespace MultiWheelC
}
///
- /// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
+ /// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
///
private static double CalculateReferenceSpeed(
double arcLengthMeters,
@@ -566,52 +575,64 @@ namespace MultiWheelC
decelerationMetersPerSecondSquared *
Math.Max(0.0, remainingDistanceMeters));
- return Math.Min(
+ var travelDirection = GetTravelDirection(
cruiseSpeedMetersPerSecond,
+ nameof(cruiseSpeedMetersPerSecond));
+ var speedMagnitude = Math.Min(
+ Math.Abs(cruiseSpeedMetersPerSecond),
Math.Min(
accelerationLimitedSpeed,
brakingLimitedSpeed));
+
+ return travelDirection * speedMagnitude;
}
///
- /// 检查世界坐标系起点位姿是否全部为有限值。
+ /// 获取非零有符号速度表示的前进或倒车方向。
///
- private static void EnsureFinitePose(
- Pose2D pose,
+ private static double GetTravelDirection(
+ double signedSpeedMetersPerSecond,
string parameterName)
{
- if (!IsFinite(pose.XMeters) ||
- !IsFinite(pose.YMeters) ||
- !IsFinite(pose.YawRadians))
+ NumericGuard.EnsureFinite(
+ signedSpeedMetersPerSecond,
+ parameterName);
+
+ if (signedSpeedMetersPerSecond == 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
- "直线测试轨迹的起点位姿必须由有限值组成。");
+ "测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
}
+
+ return Math.Sign(
+ signedSpeedMetersPerSecond);
}
///
- /// 检查测试轨迹参数是否为正有限值。
+ /// 确保直线段和转弯段速度使用相同的前进或倒车方向。
///
- private static void EnsureFinitePositive(
- double value,
- string parameterName)
+ private static double GetCommonTravelDirection(
+ double firstSpeedMetersPerSecond,
+ string firstParameterName,
+ double secondSpeedMetersPerSecond,
+ string secondParameterName)
{
- if (!IsFinite(value) || value <= 0.0)
+ var firstDirection = GetTravelDirection(
+ firstSpeedMetersPerSecond,
+ firstParameterName);
+ var secondDirection = GetTravelDirection(
+ secondSpeedMetersPerSecond,
+ secondParameterName);
+
+ if (firstDirection != secondDirection)
{
- throw new ArgumentOutOfRangeException(
- parameterName,
- "直线测试轨迹的速度、加速度和点间距必须是正有限值。");
+ throw new ArgumentException(
+ "同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
+ firstParameterName);
}
- }
- ///
- /// 判断数值是否可用于轨迹计算。
- ///
- private static bool IsFinite(double value)
- {
- return !double.IsNaN(value) &&
- !double.IsInfinity(value);
+ return firstDirection;
}
}
}
diff --git a/MultiWheelC/Experiments/WheelPreparationTests.cs b/MultiWheelC/Experiments/WheelPreparationTests.cs
index 7444cfd..4faa430 100644
--- a/MultiWheelC/Experiments/WheelPreparationTests.cs
+++ b/MultiWheelC/Experiments/WheelPreparationTests.cs
@@ -3,17 +3,20 @@
using System;
using ClumsyCore;
using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
-using MyParking.Shared;
namespace MultiWheelC
{
+ ///
+ /// 为需要显式执行舵轮回正的测试管理准备动作及其DriveTask生命周期。
+ ///
internal static class MovementTestPreparation
{
- // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
+ ///
+ /// 执行舵轮回正动作并返回四轮是否已经稳定朝向车体前方。
+ ///
public static bool AlignWheelsForward(
ref DriveTask activeTask)
{
@@ -40,47 +43,11 @@ namespace MultiWheelC
}
}
- // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
- public static bool AreWheelsForward(
- float toleranceDegrees = 2f)
- {
- var chassis =
- PilotDefinition.Chassis as MultiWheelChassis;
- if (chassis == null)
- {
- Console.WriteLine(
- "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
- return false;
- }
-
- try
- {
- var adapter = new MultiWheelChassisAdapter(
- chassis,
- PilotDefinition.Self.CarNum);
- var toleranceRadians =
- AngleMath.DegreesToRadians(toleranceDegrees);
-
- if (adapter.AreParallelWheelsAligned(
- 0.0,
- toleranceRadians))
- {
- return true;
- }
-
- Console.WriteLine(
- "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
- return false;
- }
- catch (Exception ex)
- {
- Console.WriteLine(
- $"检查舵轮方向失败：{ex.Message}");
- return false;
- }
- }
}
+ ///
+ /// 提供可从测试界面单独触发的四舵轮回正动作。
+ ///
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
public class AlignWheelsForwardTest : MovementTest
{
diff --git a/MultiWheelC/Movements/ClampToTarget.cs b/MultiWheelC/Movements/ClampToTarget.cs
deleted file mode 100644
index d6e06bc..0000000
--- a/MultiWheelC/Movements/ClampToTarget.cs
+++ /dev/null
@@ -1,91 +0,0 @@
-using System;
-using System.Collections.Generic;
-using ClumsyCore.Pilot;
-using MDCSToolBox.Commons.Controllers;
-
-namespace MultiWheelC
-{
- public class ClampToTarget : MovementDefinition
- {
- public float LeftClampTarget;
- public float RightClampTarget;
- public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
- public float ClampKp = PilotDefinition.Conf.ClampControlKp;
- public float ClampKi = PilotDefinition.Conf.ClampControlKi;
- public float ClampKd = PilotDefinition.Conf.ClampControlKd;
- public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
- public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
- public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
- public float TimeoutSeconds = 30f;
- private PIDController leftpid, rightpid;
-
- // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
- public override IEnumerable Get()
- {
- try
- {
- leftpid = new PIDController(
- () => PilotDefinition.Self.ActualPosLeftArm,
- ClampKp, ClampKi, ClampKd, ClampMaxI,
- ClampDeadZone, MaxClampSpeed)
- {
- SpeedAccPerSec = ClampSpeedAcc
- };
-
- rightpid = new PIDController(
- () => PilotDefinition.Self.ActualPosRightArm,
- ClampKp, ClampKi, ClampKd, ClampMaxI,
- ClampDeadZone, MaxClampSpeed)
- {
- SpeedAccPerSec = ClampSpeedAcc
- };
-
- var startTime = DateTime.UtcNow;
- while (true)
- {
- if (TimeoutSeconds > 0f &&
- (DateTime.UtcNow - startTime).TotalSeconds >
- TimeoutSeconds)
- {
- Console.WriteLine(
- $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
- "停止左右夹臂。");
- yield break;
- }
-
- var leftspeed =
- leftpid.GetResponse(LeftClampTarget);
- var rightspeed =
- rightpid.GetResponse(RightClampTarget);
- Console.WriteLine(
- $"left arm speed:{leftspeed} " +
- $"right arm speed:{rightspeed}");
-
- PilotDefinition.Self.SpeedLeftArm = leftspeed;
- PilotDefinition.Self.SpeedRightArm = rightspeed;
-
- var leftArrived = leftpid.IsArrived();
- var rightArrived = rightpid.IsArrived();
- if (leftArrived)
- PilotDefinition.Self.SpeedLeftArm = 0f;
- if (rightArrived)
- PilotDefinition.Self.SpeedRightArm = 0f;
-
- if (leftArrived && rightArrived)
- break;
-
- yield return true;
- }
-
- Console.WriteLine(
- $"left clamp to target:{LeftClampTarget} " +
- $"right clamp to target:{RightClampTarget}");
- }
- finally
- {
- PilotDefinition.Self.SpeedLeftArm = 0f;
- PilotDefinition.Self.SpeedRightArm = 0f;
- }
- }
- }
-}
diff --git a/MultiWheelC/Movements/CrabMotionFrameTracker.cs b/MultiWheelC/Movements/CrabMotionFrameTracker.cs
deleted file mode 100644
index 6eaa1ce..0000000
--- a/MultiWheelC/Movements/CrabMotionFrameTracker.cs
+++ /dev/null
@@ -1,817 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MyParking.Shared;
-using System;
-using System.Collections.Generic;
-using System.Numerics;
-
-namespace MultiWheelC
-{
- // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
- public sealed class CrabMotionFrameTracker : MovementDefinition
- {
- public enum ReferencePathKind
- {
- Straight = 0,
- LeftArc = 1,
- SCurve = 2
- }
-
- public enum ChassisCommandBackend
- {
- SendXYThSpeed = 0,
- SendMotion = 1
- }
-
- public ReferencePathKind PathKind;
- public ChassisCommandBackend CommandBackend =
- ChassisCommandBackend.SendMotion;
- public Vector2 StartPosition;
- public double InitialBodyYawRadians;
- public float LengthMillimeters = 4000f;
- public float RadiusMillimeters = 2000f;
- public float SCurveLateralOffsetMillimeters = 400f;
- public double ArcSweepRadians = Math.PI / 2.0;
- public float CruiseSpeed = 0.2f;
- public float SlowDistanceMillimeters = 600f;
- public float FinishDistanceMillimeters = 30f;
- public float MinimumSpeed = 0.04f;
- public double LateralGainPerSecond = 0.8;
- public double MaximumLateralCorrection = 0.12;
- public double HeadingGainPerSecond = 1.5;
- public double MaximumAngularSpeedRadiansPerSecond =
- AngleMath.DegreesToRadians(30.0);
- public double MaximumVirtualSteeringRadians =
- AngleMath.DegreesToRadians(30.0);
- public float WheelAlignmentToleranceDegrees = 2f;
- public float WheelAlignmentStableSeconds = 0.3f;
- public float WheelAlignmentTimeoutSeconds = 10f;
- public float TrackingTimeoutSeconds = 60f;
- public Action CommandObserver;
-
- // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
- public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
- private double _lastSCurveProgress;
-
- public override IEnumerable Get()
- {
- ValidateParameters();
-
- var chassis =
- PilotDefinition.Chassis as MultiWheelChassis;
- if (chassis == null)
- throw new InvalidOperationException(
- "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
-
- var adapter = new MultiWheelChassisAdapter(
- chassis,
- PilotDefinition.Self.CarNum);
- adapter.ResetToBodyFrame();
-
- var lastCommandTime = DateTime.Now;
-
- try
- {
- // 模式切换阶段只转舵轮，驱动速度始终保持为零。
- var alignmentStarted = DateTime.Now;
- DateTime? stableSince = null;
- while (true)
- {
- if (!adapter.PrepareParallelDirection(
- MotionFrameYawInBodyRadians))
- throw new InvalidOperationException(
- "无法生成运动坐标系对应的舵轮准备姿态。");
-
- var aligned =
- adapter.AreParallelWheelsAligned(
- MotionFrameYawInBodyRadians,
- AngleMath.DegreesToRadians(
- WheelAlignmentToleranceDegrees));
-
- if (aligned)
- {
- if (stableSince == null)
- stableSince = DateTime.Now;
-
- if ((DateTime.Now - stableSince.Value)
- .TotalSeconds >=
- WheelAlignmentStableSeconds)
- break;
- }
- else
- {
- stableSince = null;
- }
-
- if ((DateTime.Now - alignmentStarted)
- .TotalSeconds >
- WheelAlignmentTimeoutSeconds)
- throw new TimeoutException(
- "舵轮在限定时间内未稳定到达运动坐标系初始方向。");
-
- yield return true;
- }
-
- if (CommandBackend ==
- ChassisCommandBackend.SendMotion)
- {
- // 舵轮已按真实机械角度完成预对齐；
- // 现在由Shared适配层激活SendMotion虚拟运动坐标系。
- adapter.ActivateMotionFrame(
- MotionFrameYawInBodyRadians);
- }
-
- var trackingStarted = DateTime.Now;
- while (true)
- {
- if ((DateTime.Now - trackingStarted)
- .TotalSeconds >
- TrackingTimeoutSeconds)
- throw new TimeoutException(
- "蟹行轨迹在限定时间内未完成。");
-
- var location =
- DetourInterface.getCartLocation();
- if (!IsFinite(location.x) ||
- !IsFinite(location.y) ||
- !IsFinite(location.th))
- throw new InvalidOperationException(
- "蟹行轨迹测试期间Detour位姿无效。");
-
- var currentPosition = new Vector2(
- (float)location.x,
- (float)location.y);
- var currentBodyYaw =
- AngleMath.DegreesToRadians(location.th);
-
- CalculateReference(
- currentPosition,
- out var tangentYaw,
- out var referencePoint,
- out var remainingMillimeters,
- out var referenceCurvature);
-
- if (remainingMillimeters <=
- FinishDistanceMillimeters)
- break;
-
- var speed =
- CalculateSpeed(remainingMillimeters);
- var tangent = new Vector2(
- (float)Math.Cos(tangentYaw),
- (float)Math.Sin(tangentYaw));
- var leftNormal = new Vector2(
- -tangent.Y,
- tangent.X);
- var positionError =
- currentPosition - referencePoint;
- var lateralErrorMeters =
- Vector2.Dot(
- positionError,
- leftNormal) / 1000.0;
- var normalCorrection =
- Limit(
- -LateralGainPerSecond *
- lateralErrorMeters,
- MaximumLateralCorrection);
-
- // 先在世界坐标中组合切向速度与横向纠偏速度。
- var worldVx =
- tangent.X * speed +
- leftNormal.X * (float)normalCorrection;
- var worldVy =
- tangent.Y * speed +
- leftNormal.Y * (float)normalCorrection;
-
- // 将世界速度表达为当前蟹行运动坐标系速度。
- var motionYaw =
- currentBodyYaw +
- MotionFrameYawInBodyRadians;
- var motionCos = Math.Cos(motionYaw);
- var motionSin = Math.Sin(motionYaw);
- var vxInMotion =
- motionCos * worldVx +
- motionSin * worldVy;
- var vyInMotion =
- -motionSin * worldVx +
- motionCos * worldVy;
-
- var desiredBodyYaw =
- tangentYaw -
- MotionFrameYawInBodyRadians;
- var headingError =
- AngleMath.ShortestDifferenceRadians(
- desiredBodyYaw,
- currentBodyYaw);
- var omega =
- speed * referenceCurvature +
- HeadingGainPerSecond * headingError;
- omega = Limit(
- omega,
- MaximumAngularSpeedRadiansPerSecond);
-
- var now = DateTime.Now;
- var interval = now - lastCommandTime;
- lastCommandTime = now;
-
- bool commandAccepted;
- Twist2D bodyTwist;
- if (CommandBackend ==
- ChassisCommandBackend.SendMotion)
- {
- // 运动坐标系相对车体系旋转+90°：
- // 运动系正向速度会转换成车体系+Y速度。
- bodyTwist =
- FrameTransform2D
- .TransformTwistAtSamePoint(
- new Pose2D(
- 0.0,
- 0.0,
- MotionFrameYawInBodyRadians),
- new Twist2D(
- vxInMotion,
- vyInMotion,
- omega));
-
- // 将运动坐标系原点和前后几何控制点处的速度，
- // 转换为SendMotion需要的前后轴方向。
- var controlPointRadiusMeters =
- Math.Max(
- chassis.ControlPointRadius /
- 1000.0,
- 0.001);
- var frontVelocityY =
- vyInMotion +
- omega *
- controlPointRadiusMeters;
- var rearVelocityY =
- vyInMotion -
- omega *
- controlPointRadiusMeters;
- var frontSteeringRadians =
- Math.Atan2(
- frontVelocityY,
- vxInMotion);
- var rearSteeringRadians =
- Math.Atan2(
- rearVelocityY,
- vxInMotion);
-
- // 蟹行测试绕过M层ManualControl并直接调用SendMotion，
- // 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
- if (IsCrabMotionFrame())
- {
- var geometryRatio =
- adapter.HalfTrackWidthMeters /
- adapter.HalfWheelBaseMeters;
-
- frontSteeringRadians =
- ConvertToCrabSteering(
- frontSteeringRadians,
- geometryRatio);
- rearSteeringRadians =
- ConvertToCrabSteering(
- rearSteeringRadians,
- geometryRatio);
- }
-
- var frontThetaDegrees =
- (float)AngleMath.RadiansToDegrees(
- frontSteeringRadians);
- var rearThetaDegrees =
- (float)AngleMath.RadiansToDegrees(
- rearSteeringRadians);
- var motionSpeed =
- (float)Math.Sqrt(
- vxInMotion * vxInMotion +
- vyInMotion * vyInMotion);
-
- commandAccepted =
- chassis.SendMotion(
- motionSpeed,
- frontThetaDegrees,
- rearThetaDegrees,
- interval);
- }
- else if (CommandBackend ==
- ChassisCommandBackend
- .SendXYThSpeed)
- {
- // 安全XYTh后端根据舵角误差统一压低驱动轮速。
- bodyTwist =
- FrameTransform2D
- .TransformTwistAtSamePoint(
- new Pose2D(
- 0.0,
- 0.0,
- MotionFrameYawInBodyRadians),
- new Twist2D(
- vxInMotion,
- vyInMotion,
- omega));
- var command = new ChassisCommand(
- PilotDefinition.Self.CarNum,
- bodyTwist);
- commandAccepted =
- adapter.Send(
- command,
- interval);
- }
- else
- {
- throw new InvalidOperationException(
- $"不支持的底盘命令后端：{CommandBackend}。");
- }
-
- if (!commandAccepted)
- throw new InvalidOperationException(
- "运动坐标系轨迹底盘解算失败：" +
- chassis
- .LastMotionDecomposeFailureReason);
-
- CommandObserver?.Invoke(
- (float)bodyTwist.VxMetersPerSecond,
- (float)bodyTwist.VyMetersPerSecond,
- (float)bodyTwist
- .OmegaRadiansPerSecond);
-
- yield return true;
- }
- }
- finally
- {
- adapter.StopImmediately();
- if (CommandBackend ==
- ChassisCommandBackend.SendMotion)
- {
- // 测试退出后恢复真实车体坐标系，避免影响后续测试。
- adapter.ResetToBodyFrame();
- }
- CommandObserver?.Invoke(0f, 0f, 0f);
- }
-
- yield return false;
- }
-
- // 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
- private bool IsCrabMotionFrame()
- {
- return Math.Abs(
- AngleMath.ShortestDifferenceRadians(
- Math.PI / 2.0,
- MotionFrameYawInBodyRadians)) <
- 1e-6;
- }
-
- // 按车体几何比例缩小蟹行转角。
- // +90°运动坐标系已经完成方向映射，此处不能再次反号。
- private double ConvertToCrabSteering(
- double normalSteeringRadians,
- double geometryRatio)
- {
- var crabSteeringRadians =
- Math.Atan(
- geometryRatio *
- Math.Tan(
- normalSteeringRadians));
-
- return Limit(
- crabSteeringRadians,
- MaximumVirtualSteeringRadians);
- }
-
- // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
- private void CalculateReference(
- Vector2 currentPosition,
- out double tangentYaw,
- out Vector2 referencePoint,
- out float remainingMillimeters,
- out double curvaturePerMeter)
- {
- var initialMotionYaw =
- InitialBodyYawRadians +
- MotionFrameYawInBodyRadians;
-
- if (PathKind == ReferencePathKind.Straight)
- {
- var tangent = new Vector2(
- (float)Math.Cos(initialMotionYaw),
- (float)Math.Sin(initialMotionYaw));
- var relative = currentPosition - StartPosition;
- var progress =
- Vector2.Dot(relative, tangent);
- var clampedProgress =
- Math.Max(
- 0f,
- Math.Min(progress, LengthMillimeters));
-
- tangentYaw = initialMotionYaw;
- referencePoint =
- StartPosition +
- tangent * clampedProgress;
- remainingMillimeters =
- Math.Max(
- 0f,
- LengthMillimeters - progress);
- curvaturePerMeter = 0.0;
- return;
- }
-
- if (PathKind == ReferencePathKind.SCurve)
- {
- CalculateSCurveReference(
- currentPosition,
- initialMotionYaw,
- out tangentYaw,
- out referencePoint,
- out remainingMillimeters,
- out curvaturePerMeter);
- return;
- }
-
- var center = GetArcCenter();
- var startRadialYaw =
- initialMotionYaw - Math.PI / 2.0;
- var radial = currentPosition - center;
- var currentRadialYaw =
- Math.Atan2(radial.Y, radial.X);
- var progressRadians =
- AngleMath.NormalizeRadians(
- currentRadialYaw - startRadialYaw);
-
- // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
- if (progressRadians < 0.0)
- progressRadians = 0.0;
-
- var clampedProgressRadians =
- Math.Min(
- progressRadians,
- ArcSweepRadians);
- var referenceRadialYaw =
- startRadialYaw +
- clampedProgressRadians;
- referencePoint = center + new Vector2(
- RadiusMillimeters *
- (float)Math.Cos(referenceRadialYaw),
- RadiusMillimeters *
- (float)Math.Sin(referenceRadialYaw));
- tangentYaw =
- referenceRadialYaw + Math.PI / 2.0;
- remainingMillimeters =
- (float)Math.Max(
- 0.0,
- (ArcSweepRadians - progressRadians) *
- RadiusMillimeters);
- curvaturePerMeter =
- 1000.0 / RadiusMillimeters;
- }
-
- // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
- private void CalculateSCurveReference(
- Vector2 currentPosition,
- double initialMotionYaw,
- out double tangentYaw,
- out Vector2 referencePoint,
- out float remainingMillimeters,
- out double curvaturePerMeter)
- {
- const int nearestPointSamples = 200;
- var searchStart =
- Math.Max(
- 0.0,
- _lastSCurveProgress - 0.02);
- var bestProgress = _lastSCurveProgress;
- var bestDistanceSquared = double.MaxValue;
-
- for (var i = 0;
- i <= nearestPointSamples;
- i++)
- {
- var progress =
- searchStart +
- (1.0 - searchStart) *
- i / nearestPointSamples;
- EvaluateSCurve(
- progress,
- out var localPoint,
- out _,
- out _);
- var worldPoint =
- LocalPathPointToWorld(
- localPoint,
- initialMotionYaw);
- var distanceSquared =
- Vector2.DistanceSquared(
- currentPosition,
- worldPoint);
-
- if (distanceSquared <
- bestDistanceSquared)
- {
- bestDistanceSquared =
- distanceSquared;
- bestProgress = progress;
- }
- }
-
- // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
- _lastSCurveProgress =
- Math.Max(
- _lastSCurveProgress,
- bestProgress);
- EvaluateSCurve(
- _lastSCurveProgress,
- out var bestLocalPoint,
- out var firstDerivative,
- out var secondDerivative);
- referencePoint =
- LocalPathPointToWorld(
- bestLocalPoint,
- initialMotionYaw);
- tangentYaw =
- initialMotionYaw +
- Math.Atan2(
- firstDerivative.Y,
- firstDerivative.X);
-
- var derivativeMagnitude =
- Math.Sqrt(
- firstDerivative.X *
- firstDerivative.X +
- firstDerivative.Y *
- firstDerivative.Y);
- if (derivativeMagnitude < 1e-6)
- {
- curvaturePerMeter = 0.0;
- }
- else
- {
- // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
- curvaturePerMeter =
- (firstDerivative.X *
- secondDerivative.Y -
- firstDerivative.Y *
- secondDerivative.X) *
- 1000.0 /
- Math.Pow(
- derivativeMagnitude,
- 3.0);
- }
-
- remainingMillimeters =
- ApproximateSCurveRemainingLength(
- _lastSCurveProgress);
- }
-
- // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
- private void EvaluateSCurve(
- double progress,
- out Vector2 point,
- out Vector2 firstDerivative,
- out Vector2 secondDerivative)
- {
- progress =
- Math.Max(
- 0.0,
- Math.Min(progress, 1.0));
-
- Vector2 p0;
- Vector2 p1;
- Vector2 p2;
- Vector2 p3;
- double t;
-
- if (progress <= 0.25)
- {
- t = progress * 4.0;
- p0 = new Vector2(0f, 0f);
- p1 = new Vector2(
- LengthMillimeters / 12f,
- 0f);
- p2 = new Vector2(
- LengthMillimeters / 6f,
- SCurveLateralOffsetMillimeters);
- p3 = new Vector2(
- LengthMillimeters * 0.25f,
- SCurveLateralOffsetMillimeters);
- }
- else if (progress <= 0.75)
- {
- t = (progress - 0.25) * 2.0;
- p0 = new Vector2(
- LengthMillimeters * 0.25f,
- SCurveLateralOffsetMillimeters);
- p1 = new Vector2(
- LengthMillimeters / 3f,
- SCurveLateralOffsetMillimeters);
- p2 = new Vector2(
- LengthMillimeters * 2f / 3f,
- -SCurveLateralOffsetMillimeters);
- p3 = new Vector2(
- LengthMillimeters * 0.75f,
- -SCurveLateralOffsetMillimeters);
- }
- else
- {
- t = (progress - 0.75) * 4.0;
- p0 = new Vector2(
- LengthMillimeters * 0.75f,
- -SCurveLateralOffsetMillimeters);
- p1 = new Vector2(
- LengthMillimeters * 5f / 6f,
- -SCurveLateralOffsetMillimeters);
- p2 = new Vector2(
- LengthMillimeters * 11f / 12f,
- 0f);
- p3 = new Vector2(
- LengthMillimeters,
- 0f);
- }
-
- var oneMinusT = 1.0 - t;
- point =
- p0 * (float)(
- oneMinusT *
- oneMinusT *
- oneMinusT) +
- p1 * (float)(
- 3.0 *
- oneMinusT *
- oneMinusT *
- t) +
- p2 * (float)(
- 3.0 *
- oneMinusT *
- t *
- t) +
- p3 * (float)(t * t * t);
- firstDerivative =
- (p1 - p0) *
- (float)(
- 3.0 *
- oneMinusT *
- oneMinusT) +
- (p2 - p1) *
- (float)(
- 6.0 *
- oneMinusT *
- t) +
- (p3 - p2) *
- (float)(3.0 * t * t);
- secondDerivative =
- (p2 - 2f * p1 + p0) *
- (float)(6.0 * oneMinusT) +
- (p3 - 2f * p2 + p1) *
- (float)(6.0 * t);
- }
-
- // 通过分段采样估算从当前S曲线进度到终点的实际弧长。
- private float ApproximateSCurveRemainingLength(
- double startProgress)
- {
- const int lengthSamples = 100;
- EvaluateSCurve(
- startProgress,
- out var previousPoint,
- out _,
- out _);
- var length = 0f;
-
- for (var i = 1;
- i <= lengthSamples;
- i++)
- {
- var progress =
- startProgress +
- (1.0 - startProgress) *
- i / lengthSamples;
- EvaluateSCurve(
- progress,
- out var point,
- out _,
- out _);
- length +=
- Vector2.Distance(
- previousPoint,
- point);
- previousPoint = point;
- }
-
- return length;
- }
-
- // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
- private Vector2 LocalPathPointToWorld(
- Vector2 localPoint,
- double initialMotionYaw)
- {
- var cos =
- (float)Math.Cos(initialMotionYaw);
- var sin =
- (float)Math.Sin(initialMotionYaw);
-
- return StartPosition + new Vector2(
- localPoint.X * cos -
- localPoint.Y * sin,
- localPoint.X * sin +
- localPoint.Y * cos);
- }
-
- // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
- public Vector2 GetArcCenter()
- {
- var initialMotionYaw =
- InitialBodyYawRadians +
- MotionFrameYawInBodyRadians;
- return StartPosition + new Vector2(
- -RadiusMillimeters *
- (float)Math.Sin(initialMotionYaw),
- RadiusMillimeters *
- (float)Math.Cos(initialMotionYaw));
- }
-
- // 获取圆弧测试的理论终点。
- public Vector2 GetArcDestination()
- {
- var initialMotionYaw =
- InitialBodyYawRadians +
- MotionFrameYawInBodyRadians;
- var startRadialYaw =
- initialMotionYaw - Math.PI / 2.0;
- var endRadialYaw =
- startRadialYaw + ArcSweepRadians;
- var center = GetArcCenter();
-
- return center + new Vector2(
- RadiusMillimeters *
- (float)Math.Cos(endRadialYaw),
- RadiusMillimeters *
- (float)Math.Sin(endRadialYaw));
- }
-
- // 根据剩余路径长度生成终点减速速度。
- private float CalculateSpeed(
- float remainingMillimeters)
- {
- if (remainingMillimeters >=
- SlowDistanceMillimeters)
- return CruiseSpeed;
-
- var ratio =
- remainingMillimeters /
- Math.Max(
- SlowDistanceMillimeters,
- 1f);
- return Math.Max(
- MinimumSpeed,
- CruiseSpeed * ratio);
- }
-
- private void ValidateParameters()
- {
- if (CruiseSpeed <= 0f ||
- !IsFinite(CruiseSpeed) ||
- LengthMillimeters <= 0f ||
- !IsFinite(LengthMillimeters) ||
- RadiusMillimeters <= 0f ||
- !IsFinite(RadiusMillimeters) ||
- SCurveLateralOffsetMillimeters <= 0f ||
- !IsFinite(
- SCurveLateralOffsetMillimeters) ||
- ArcSweepRadians <= 0.0 ||
- !IsFinite(ArcSweepRadians) ||
- SlowDistanceMillimeters <= 0f ||
- !IsFinite(SlowDistanceMillimeters) ||
- FinishDistanceMillimeters < 0f ||
- !IsFinite(FinishDistanceMillimeters) ||
- TrackingTimeoutSeconds <= 0f ||
- !IsFinite(TrackingTimeoutSeconds) ||
- MaximumVirtualSteeringRadians <= 0.0 ||
- MaximumVirtualSteeringRadians >=
- Math.PI / 2.0 ||
- !IsFinite(
- MaximumVirtualSteeringRadians))
- throw new ArgumentOutOfRangeException(
- "蟹行轨迹测试参数无效。");
- }
-
- private static double Limit(
- double value,
- double absoluteLimit)
- {
- return Math.Max(
- -absoluteLimit,
- Math.Min(value, absoluteLimit));
- }
-
- private static bool IsFinite(double value)
- {
- return
- !double.IsNaN(value) &&
- !double.IsInfinity(value);
- }
- }
-}
diff --git a/MultiWheelC/Movements/DriverMovements.cs b/MultiWheelC/Movements/DriverMovements.cs
deleted file mode 100644
index 841ef5b..0000000
--- a/MultiWheelC/Movements/DriverMovements.cs
+++ /dev/null
@@ -1,123 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Threading;
-using ClumsyCore.Pilot;
-
-namespace MultiWheelC
-{
- public class Sleep : MovementDefinition
- {
- public float Second = 2f;
-
- public override IEnumerable Get()
- {
- if (Second <= 0)
- {
- yield return false;
- yield break;
- }
-
- var endTime = DateTime.UtcNow.AddSeconds(Second);
- while (DateTime.UtcNow < endTime)
- {
- Thread.Sleep(50);
- yield return true;
- }
-
- yield return false;
- }
- }
-
- public class DriverAble : MovementDefinition
- {
- public int WaitTimeoutMs = 2000;
- public int PollIntervalMs = 50;
-
- // C层单车硬件：请求全部驱动轮复位并恢复使能。
- public override IEnumerable Get()
- {
- PilotDefinition.Self.ResetFromC = true;
-
- try
- {
- var start = DateTime.Now;
- var timeoutMs = Math.Max(0, WaitTimeoutMs);
- var pollMs = Math.Max(1, PollIntervalMs);
-
- // 至少保留一个调度周期，确保M层能收到复位请求。
- yield return true;
-
- while (!PilotDefinition.Self.WheelAbleState &&
- (DateTime.Now - start).TotalMilliseconds < timeoutMs)
- {
- Thread.Sleep(pollMs);
- yield return true;
- }
- }
- finally
- {
- PilotDefinition.Self.ResetFromC = false;
- }
- }
- }
- public class DriverDisable : MovementDefinition
- {
- public int WaitTimeoutMs = 3000;
- public int PollIntervalMs = 20;
-
- // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
- public override IEnumerable Get()
- {
- var timeoutMs = Math.Max(0, WaitTimeoutMs);
- var pollMs = Math.Max(1, PollIntervalMs);
- var startTime = DateTime.UtcNow;
- var success = false;
-
- PilotDefinition.Self.DisableFromC = true;
-
- try
- {
- // 至少保持一个C层调度周期，确保M层能收到下使能请求。
- yield return true;
-
- success = !PilotDefinition.Self.WheelAbleState;
-
- while (!success &&
- (DateTime.UtcNow - startTime).TotalMilliseconds <
- timeoutMs)
- {
- Thread.Sleep(pollMs);
-
- success =
- !PilotDefinition.Self.WheelAbleState;
-
- if (!success)
- {
- yield return true;
- }
- }
- }
- finally
- {
- // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
- PilotDefinition.Self.DisableFromC = false;
- }
- if (success)
- {
- Console.WriteLine(
- $"驱动器下使能完成，" +
- $"WheelAbleState=" +
- $"{PilotDefinition.Self.WheelAbleState}");
- }
- else
- {
- Console.WriteLine(
- $"驱动器下使能超时，" +
- $"WheelAbleState=" +
- $"{PilotDefinition.Self.WheelAbleState}，" +
- $"等待{timeoutMs}ms");
- }
- yield return false;
- }
- }
-}
diff --git a/MultiWheelC/Movements/DstTracker.cs b/MultiWheelC/Movements/DstTracker.cs
deleted file mode 100644
index 7630735..0000000
--- a/MultiWheelC/Movements/DstTracker.cs
+++ /dev/null
@@ -1,55 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Numerics;
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MDCSToolBox.Clumsy.Tracks;
-
-namespace MultiWheelC
-{
- //在世界坐标系下，从路径起点追踪到终点并停车
- public class DstTracker : MovementDefinition
- {
- public Vector2 Src;
- public Vector2 Dst;
- // 本次轨迹的巡航速度上限，单位m/s。
- public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
- public float CarDirectionBias = 0f;
- public Painter Painter = UI.GetPainter("DstTracker");
- public override IEnumerable Get()
- {
- var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
- DriveTask task = null;
- try
- {
- Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
- Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
-
- var tracker = new ChassisController
- {
- BaseSpeed = MaxSpeed
- }.Get();
- // 要求路径末端速度下降到零。
- tracker.FinishSpeed = 0f;
- var linePath = new LineTrack(Src, Dst)
- {
- CarDirectionBias = CarDirectionBias,
- Speed = MaxSpeed
- };
- tracker.AddTrack(linePath);
- task = new DriveTask(tracker.Track());
- task.Wait();
- yield return false;
- }
- finally
- {
- task?.Stop();
- chassis.PredefinedDriveStop();
- }
- }
-
- }
-}
diff --git a/MultiWheelC/Movements/LineTracking.cs b/MultiWheelC/Movements/LineTracking.cs
deleted file mode 100644
index f7ecae5..0000000
--- a/MultiWheelC/Movements/LineTracking.cs
+++ /dev/null
@@ -1,178 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Numerics;
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using FundamentalLib;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
- // C层单车底盘：按照车轮里程行驶指定的相对距离。
- public class LineTracking : MovementDefinition
- {
- // 相对动作启动位置的行驶距离，单位mm。
- // 正数表示前进，负数表示后退。
- public float TargetDistance;
- public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
- public float Kp = PilotDefinition.Conf.LineTrackKp;
- public float Ki = PilotDefinition.Conf.LineTrackKi;
- public float Kd = PilotDefinition.Conf.LineTrackKd;
- public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
- public int SrcId = -1;
- public int DstId = -1;
- public Action LeaveSrcFunction;
- // 接近目标后是否保留速度，交给下一个动作接管。
- public bool EnableHandover;
- // 进入动作衔接的剩余距离，单位mm。
- public float HandoverDistance = 80f;
- // HandoverSpeed小于0时，使用MaxSpeed的此比例。
- public float HandoverSpeedRatio = 0.5f;
- // 大于等于0时，直接作为衔接速度，单位m/s。
- public float HandoverSpeed = -1f;
- public float MinHandoverSpeed = 0.05f;
- private PIDController _pid;
- // 读取当前单车直线行驶里程，单位mm。
- private static float ReadPosition()
- {
- return
- (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
- }
-
- // 根据动作启动位置和目标距离执行直线里程闭环。
- public override IEnumerable Get()
- {
- if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
- {
- throw new ArgumentOutOfRangeException(
- nameof(TargetDistance),
- "目标行驶距离必须是有限值。");
- }
-
- if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
- {
- throw new ArgumentOutOfRangeException(
- nameof(MaxSpeed),
- "最大速度必须是大于零的有限值。");
- }
- var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
- // 每次启动动作时重新读取起始编码器位置。
- var startPosition = ReadPosition();
- // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
- var targetPosition = startPosition + TargetDistance;
- _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
- {
- SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
- };
- var handoverRequested = false;
- var keepHandoverSpeed = false;
- DLog.Log(
- $"直线里程动作：" +
- $"起点={startPosition:F1}mm，" +
- $"距离={TargetDistance:F1}mm，" +
- $"目标={targetPosition:F1}mm",
- "straight_line");
- try
- {
- while (true)
- {
- var currentPosition = ReadPosition();
- var remainingDistance = targetPosition - currentPosition;
- // 接近目标后，保留一定速度交给后续动作。
- if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
- {
- var direction = Math.Sign(remainingDistance);
- if (direction == 0)
- {
- direction = Math.Sign(TargetDistance);
- }
- var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
- var maximumSpeed = Math.Abs(MaxSpeed);
- var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
- var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
- var handoverSpeed = limitedSpeed * direction;
- chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
- handoverRequested = true;
- // 保持一个调度周期，让速度命令实际生效。
- yield return true;
- break;
- }
- var speed = _pid.GetResponse(targetPosition);
- chassis.SendXYThSpeed(speed, 0f, 0f);
- if (_pid.IsArrived())
- {
- break;
- }
- yield return true;
- }
- if (SrcId != -1 &&
- LeaveSrcFunction != null)
- {
- LeaveSrcFunction(SrcId);
- DLog.Log($"释放放车点{SrcId}", "straight_line");
- }
- // 只有正常完成动作衔接时才允许保留非零速度。
- keepHandoverSpeed = handoverRequested;
- }
- finally
- {
- // 普通完成、人工停止或异常退出时都必须停车。
- if (!keepHandoverSpeed)
- {
- chassis.SendXYThSpeed(0f, 0f, 0f);
- }
- }
- yield return false;
- }
- }
-
-
-
-
- //直线行走基于detour
- public class LineTracking_based_detour : MovementDefinition
- {
- public float LineDistance = 1000f;
- public int SrcId = -1;
- public int DstId = -1;
- public Action LeaveSrcFunction = null;
- public Painter painter = UI.GetPainter("Line", false);
- // C层单车轨迹：执行早期版本的两点直线跟踪动作。
- public override IEnumerable Get()
- {
- var curpose = DetourInterface.getCartLocation();
- Console.WriteLine($"curpose.th:{curpose.th}");
- var src = new Vector2((float)curpose.x, (float)curpose.y);
- var headingRadians =
- AngleMath.DegreesToRadians(curpose.th);
- var dst = new Vector2(
- (float)(curpose.x +
- LineDistance * Math.Cos(headingRadians)),
- (float)(curpose.y +
- LineDistance * Math.Sin(headingRadians)));
- // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
- // (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
- Console.WriteLine($"src:{src.X} {src.Y}");
- Console.WriteLine($"dst:{dst.X} {dst.Y}");
- painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
-
- var tracker = new ChassisController().Get();
- var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
- tracker.AddTrack(linePath);
- var _dt = new DriveTask(tracker.Track());
- _dt.Wait();
- if (SrcId != -1 && LeaveSrcFunction != null)
- {
- LeaveSrcFunction(SrcId);
- DLog.Log($"释放放车点{SrcId}", "straight_line");
- }
- yield return false;
- }
- }
-}
diff --git a/MultiWheelC/Movements/MotionPlanExecutor.cs b/MultiWheelC/Movements/MotionPlanExecutor.cs
index f1f4dcf..f66a010 100644
--- a/MultiWheelC/Movements/MotionPlanExecutor.cs
+++ b/MultiWheelC/Movements/MotionPlanExecutor.cs
@@ -2,7 +2,7 @@ using System;
using System.Collections.Generic;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
-using MDCSToolBox.Commons.Controllers;
+using CommonUsage.Chassis;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
@@ -81,7 +81,7 @@ namespace MultiWheelC
public IReadOnlyList Segments;
///
- /// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
+ /// 获取或设置所有动作段共享的车辆状态源；为空时组合Detour位姿与电机反馈速度。
///
public IVehicleStateProvider StateProvider;
@@ -118,23 +118,26 @@ namespace MultiWheelC
///
public override IEnumerable Get()
{
- if (Segments == null || Segments.Count == 0)
+ var segments = ValidateAndSnapshotSegments();
+
+ var chassis =
+ PilotDefinition.Chassis as MultiWheelChassis;
+ if (chassis == null)
{
throw new InvalidOperationException(
- "组合运动计划至少需要包含一个动作段。");
+ "当前底盘不是MultiWheelChassis，无法执行组合运动计划。");
}
var stateProvider =
StateProvider ??
- new DetourVehicleStateProvider();
+ ParkingVehicleStateProviderFactory.Create(
+ chassis);
for (var index = 0;
- index < Segments.Count;
+ index < segments.Count;
index++)
{
- var segment = Segments[index] ??
- throw new InvalidOperationException(
- $"组合运动计划第{index}段为空。");
+ var segment = segments[index];
SegmentStarted?.Invoke(index, segment);
@@ -186,7 +189,6 @@ namespace MultiWheelC
if (segment is RotateInPlaceMotionPlanSegment rotate)
{
- var config = PilotDefinition.Conf;
var movement =
new MultiWheelRotateInPlace
{
@@ -194,25 +196,6 @@ namespace MultiWheelC
(float)AngleMath.RadiansToDegrees(
rotate.TargetYawRadians),
StateProvider = stateProvider,
- PidparamsRead = () => new PIDParams
- {
- Kp = config.InPlaceRotateKp,
- Ki = config.InPlaceRotateKi,
- Kd = config.InPlaceRotateKd,
- DeadZone =
- config.InPlaceRotateArriveDeg,
- SpeedAccPerSec =
- config.InPlaceRotateAcc,
- OutputUpperThreshold =
- config.InPlaceRotateMaxSpeed,
- MaxI = config.InPlaceRotateMaxI
- },
- MinimumAngularSpeedDegreesPerSecond =
- config.InPlaceRotateMinimumSpeed,
- WheelAlignmentToleranceDegrees =
- config.InPlaceRotateWheelAlignDeg,
- RotationTimeoutSeconds =
- config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandDegreesPerSecond =>
RotationCommandObserver?.Invoke(
@@ -242,5 +225,42 @@ namespace MultiWheelC
// 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
yield return false;
}
+
+ ///
+ /// 在车辆动作开始前验证全部动作段，并创建本次执行使用的稳定快照。
+ ///
+ private IReadOnlyList
+ ValidateAndSnapshotSegments()
+ {
+ if (Segments == null || Segments.Count == 0)
+ {
+ throw new InvalidOperationException(
+ "组合运动计划至少需要包含一个动作段。");
+ }
+
+ var segments =
+ new MotionPlanSegment[Segments.Count];
+
+ for (var index = 0;
+ index < Segments.Count;
+ index++)
+ {
+ var segment = Segments[index] ??
+ throw new InvalidOperationException(
+ $"组合运动计划第{index}段为空。");
+
+ if (!(segment is TrackMotionPlanSegment) &&
+ !(segment is RotateInPlaceMotionPlanSegment))
+ {
+ throw new NotSupportedException(
+ "组合运动计划不支持动作段类型：" +
+ $"{segment.GetType().FullName}。");
+ }
+
+ segments[index] = segment;
+ }
+
+ return segments;
+ }
}
}
diff --git a/MultiWheelC/Movements/PrepareWheelsForward.cs b/MultiWheelC/Movements/PrepareWheelsForward.cs
index b933fa3..6beb03a 100644
--- a/MultiWheelC/Movements/PrepareWheelsForward.cs
+++ b/MultiWheelC/Movements/PrepareWheelsForward.cs
@@ -9,16 +9,54 @@ using MyParking.Shared;
namespace MultiWheelC
{
- // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
+ ///
+ /// 停车并等待四个舵轮稳定回到车体前向0°。
+ ///
public class PrepareWheelsForward : MovementDefinition
{
- public float ToleranceDegrees = 2f;
- public float StableSeconds = 0.3f;
- public float TimeoutSeconds = 10f;
+ ///
+ /// 获取或设置本次动作的回正到位容差覆盖值，单位为deg；为空时读取车辆配置。
+ ///
+ public float? ToleranceDegrees;
+
+ ///
+ /// 获取或设置本次动作的稳定确认时间覆盖值，单位为s；为空时读取车辆配置。
+ ///
+ public float? StableSeconds;
+
+ ///
+ /// 获取或设置本次动作的超时覆盖值，单位为s；为空时读取车辆配置，0表示关闭超时。
+ ///
+ public float? TimeoutSeconds;
+
public bool Completed { get; private set; }
+ ///
+ /// 读取一次有效配置并等待全部舵轮在容差内稳定保持车体前向0°。
+ ///
public override IEnumerable Get()
{
+ var config = PilotDefinition.Conf;
+ var toleranceDegrees =
+ ToleranceDegrees ??
+ config.ParkingWheelForwardToleranceDegrees;
+ var stableSeconds =
+ StableSeconds ??
+ config.ParkingWheelForwardStableSeconds;
+ var timeoutSeconds =
+ TimeoutSeconds ??
+ config.ParkingWheelForwardTimeoutSeconds;
+
+ NumericGuard.EnsureFiniteNonNegative(
+ toleranceDegrees,
+ nameof(ToleranceDegrees));
+ NumericGuard.EnsureFiniteNonNegative(
+ stableSeconds,
+ nameof(StableSeconds));
+ NumericGuard.EnsureFiniteNonNegative(
+ timeoutSeconds,
+ nameof(TimeoutSeconds));
+
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -32,7 +70,7 @@ namespace MultiWheelC
PilotDefinition.Self.CarNum);
adapter.ResetToBodyFrame();
var toleranceRadians =
- AngleMath.DegreesToRadians(ToleranceDegrees);
+ AngleMath.DegreesToRadians(toleranceDegrees);
var startTime = DateTime.UtcNow;
DateTime? alignedSince = null;
@@ -59,7 +97,7 @@ namespace MultiWheelC
if ((DateTime.UtcNow -
alignedSince.Value).TotalSeconds >=
- StableSeconds)
+ stableSeconds)
{
Completed = true;
yield break;
@@ -70,12 +108,12 @@ namespace MultiWheelC
alignedSince = null;
}
- if (TimeoutSeconds > 0f &&
+ if (timeoutSeconds > 0f &&
(DateTime.UtcNow - startTime).TotalSeconds >
- TimeoutSeconds)
+ timeoutSeconds)
{
throw new TimeoutException(
- $"舵轮回正超过{TimeoutSeconds:F1}s，" +
+ $"舵轮回正超过{timeoutSeconds:F1}s，" +
"测试已经取消。");
}
diff --git a/MultiWheelC/Movements/RotateInPlace.cs b/MultiWheelC/Movements/RotateInPlace.cs
index c35a517..64b4836 100644
--- a/MultiWheelC/Movements/RotateInPlace.cs
+++ b/MultiWheelC/Movements/RotateInPlace.cs
@@ -11,6 +11,9 @@ using MultiWheelC.StateEstimation;
namespace MultiWheelC
{
+ ///
+ /// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。
+ ///
public class MultiWheelRotateInPlace : MovementDefinition
{
///
@@ -18,23 +21,26 @@ namespace MultiWheelC
///
public float AngleTarget;
- // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
+ // 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
public Func ThetaReader;
- public IVehicleStateProvider StateProvider =
- new DetourVehicleStateProvider();
+ public IVehicleStateProvider StateProvider;
- public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+ public MultiWheelChassis Chassis =
+ PilotDefinition.Chassis as MultiWheelChassis;
- public Func PidparamsRead = () => new PIDParams() { };
+ ///
+ /// 获取或设置本次动作的PID参数读取覆盖；为空时读取车辆配置。
+ ///
+ public Func PidparamsRead;
public PIDController thPid;
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
public Action CommandAngularSpeedObserver;
- // 自转前舵轮实际角度允许误差，单位deg。
- public float WheelAlignmentToleranceDegrees = 2f;
+ // 自转前舵轮实际角度允许误差覆盖值，单位deg；为空时读取车辆配置。
+ public float? WheelAlignmentToleranceDegrees;
// 自转舵轮连续保持到位的时间，单位s。
public float WheelAlignmentStableSeconds = 0.3f;
@@ -42,20 +48,56 @@ namespace MultiWheelC
// 自转舵轮准备超时时间，单位s。
public float WheelAlignmentTimeoutSeconds = 10f;
- // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
- public float MinimumAngularSpeedDegreesPerSecond = 1f;
+ // 航向尚未到位时允许下发的最小有效角速度覆盖值，单位deg/s；为空时读取车辆配置。
+ public float? MinimumAngularSpeedDegreesPerSecond;
- // 舵轮到位后执行航向闭环允许的最长时间，单位s。
- public float RotationTimeoutSeconds = 15f;
+ // 舵轮到位后执行航向闭环允许的最长时间覆盖值，单位s；为空时读取车辆配置。
+ public float? RotationTimeoutSeconds;
- // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
+ ///
+ /// 读取一次有效配置，闭环旋转到目标航向并在正常完成后等待舵轮稳定回正。
+ ///
public override IEnumerable Get()
{
if (Chassis == null)
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
- ValidateParameters();
+ var config = PilotDefinition.Conf;
+ var pidParameters =
+ PidparamsRead == null
+ ? new PIDParams
+ {
+ Kp = config.InPlaceRotateKp,
+ Ki = config.InPlaceRotateKi,
+ Kd = config.InPlaceRotateKd,
+ MaxI = config.InPlaceRotateMaxI,
+ DeadZone = config.InPlaceRotateArriveDeg,
+ SpeedAccPerSec = config.InPlaceRotateAcc,
+ OutputUpperThreshold =
+ config.InPlaceRotateMaxSpeed
+ }
+ : PidparamsRead();
+ var wheelAlignmentToleranceDegrees =
+ WheelAlignmentToleranceDegrees ??
+ config.InPlaceRotateWheelAlignDeg;
+ var minimumAngularSpeedDegreesPerSecond =
+ MinimumAngularSpeedDegreesPerSecond ??
+ config.InPlaceRotateMinimumSpeed;
+ var rotationTimeoutSeconds =
+ RotationTimeoutSeconds ??
+ config.InPlaceRotateTimeoutSec;
+ var stateProvider =
+ StateProvider ??
+ ParkingVehicleStateProviderFactory.Create(
+ Chassis,
+ config);
+
+ ValidateParameters(
+ pidParameters,
+ wheelAlignmentToleranceDegrees,
+ minimumAngularSpeedDegreesPerSecond,
+ rotationTimeoutSeconds);
var adapter = new MultiWheelChassisAdapter(
Chassis,
@@ -70,7 +112,7 @@ namespace MultiWheelC
{
if (!adapter.PrepareSpin(
alignmentToleranceDegrees:
- WheelAlignmentToleranceDegrees))
+ wheelAlignmentToleranceDegrees))
throw new InvalidOperationException(
"无法生成原地自转舵轮目标：" +
adapter.LastFailureReason);
@@ -101,7 +143,7 @@ namespace MultiWheelC
var alignmentToleranceRadians =
AngleMath.DegreesToRadians(
- WheelAlignmentToleranceDegrees);
+ wheelAlignmentToleranceDegrees);
if (!adapter.AdoptPreparedSpinForXYTh(
alignmentToleranceRadians))
{
@@ -112,14 +154,20 @@ namespace MultiWheelC
var targetAngle =
(float)AngleMath.NormalizeDegrees(AngleTarget);
- var p = PidparamsRead();
- var currentAngle = ReadCurrentAngleDegrees();
+ var currentAngle =
+ ReadCurrentAngleDegrees(stateProvider);
var cachedCurrentAngle = currentAngle;
thPid = new PIDController(
() => cachedCurrentAngle,
- p.Kp);
- thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
- p.OutputUpperThreshold, p.SpeedAccPerSec);
+ pidParameters.Kp);
+ thPid.ChangeParameters(
+ pidParameters.Kp,
+ pidParameters.Ki,
+ pidParameters.Kd,
+ pidParameters.MaxI,
+ pidParameters.DeadZone,
+ pidParameters.OutputUpperThreshold,
+ pidParameters.SpeedAccPerSec);
var lastCommandTime = DateTime.Now;
var rotationStarted = DateTime.Now;
@@ -127,13 +175,14 @@ namespace MultiWheelC
{
if ((DateTime.Now - rotationStarted)
.TotalSeconds >
- RotationTimeoutSeconds)
+ rotationTimeoutSeconds)
{
throw new TimeoutException(
- $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
+ $"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
}
- currentAngle = ReadCurrentAngleDegrees();
+ currentAngle =
+ ReadCurrentAngleDegrees(stateProvider);
cachedCurrentAngle = currentAngle;
var s = thPid.GetResponse(targetAngle, true);
var angleErrorDegrees =
@@ -145,7 +194,7 @@ namespace MultiWheelC
// PID进入到位死区后等待其0.3s稳定确认；等待期间
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
if (Math.Abs(angleErrorDegrees) <=
- p.DeadZone)
+ pidParameters.DeadZone)
{
CommandAngularSpeedObserver?.Invoke(0f);
adapter
@@ -162,10 +211,10 @@ namespace MultiWheelC
// 避免接近目标时反复出现微小命令但车辆实际不动。
if (Math.Abs(s) > 1e-6f &&
Math.Abs(s) <
- MinimumAngularSpeedDegreesPerSecond)
+ minimumAngularSpeedDegreesPerSecond)
{
s = Math.Sign(angleErrorDegrees) *
- MinimumAngularSpeedDegreesPerSecond;
+ minimumAngularSpeedDegreesPerSecond;
}
// PID加速限制在首周期可能暂时输出零；此时保留
@@ -204,7 +253,30 @@ namespace MultiWheelC
yield return true;
}
- Console.WriteLine($"final rotate to {targetAngle}");
+ CommandAngularSpeedObserver?.Invoke(0f);
+ adapter.StopXYThDrivePreserveSteeringState();
+
+ // 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
+ var wheelPreparation =
+ new PrepareWheelsForward();
+ foreach (var keepRunning in wheelPreparation.Get())
+ {
+ if (!keepRunning)
+ {
+ break;
+ }
+
+ yield return true;
+ }
+
+ if (!wheelPreparation.Completed)
+ {
+ throw new InvalidOperationException(
+ "原地自转完成后舵轮未能稳定回到车头方向。");
+ }
+
+ Console.WriteLine(
+ $"final rotate to {targetAngle}, wheels forward");
}
finally
{
@@ -216,10 +288,14 @@ namespace MultiWheelC
///
/// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
///
- private void ValidateParameters()
+ private void ValidateParameters(
+ PIDParams pidParameters,
+ float wheelAlignmentToleranceDegrees,
+ float minimumAngularSpeedDegreesPerSecond,
+ float rotationTimeoutSeconds)
{
EnsureFinitePositive(
- WheelAlignmentToleranceDegrees,
+ wheelAlignmentToleranceDegrees,
nameof(WheelAlignmentToleranceDegrees),
allowZero: true);
EnsureFinitePositive(
@@ -230,13 +306,12 @@ namespace MultiWheelC
WheelAlignmentTimeoutSeconds,
nameof(WheelAlignmentTimeoutSeconds));
EnsureFinitePositive(
- MinimumAngularSpeedDegreesPerSecond,
+ minimumAngularSpeedDegreesPerSecond,
nameof(MinimumAngularSpeedDegreesPerSecond));
EnsureFinitePositive(
- RotationTimeoutSeconds,
+ rotationTimeoutSeconds,
nameof(RotationTimeoutSeconds));
- var pidParameters = PidparamsRead();
if (pidParameters == null)
{
throw new InvalidOperationException(
@@ -256,7 +331,7 @@ namespace MultiWheelC
pidParameters.Kp,
"PidparamsRead.Kp");
- if (MinimumAngularSpeedDegreesPerSecond >
+ if (minimumAngularSpeedDegreesPerSecond >
pidParameters.OutputUpperThreshold)
{
throw new InvalidOperationException(
@@ -267,7 +342,8 @@ namespace MultiWheelC
///
/// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
///
- private float ReadCurrentAngleDegrees()
+ private float ReadCurrentAngleDegrees(
+ IVehicleStateProvider stateProvider)
{
if (ThetaReader != null)
{
@@ -283,20 +359,40 @@ namespace MultiWheelC
angleDegrees);
}
- if (StateProvider == null ||
- !StateProvider.TryGetState(out var state))
+ if (stateProvider == null ||
+ !stateProvider.TryGetState(out var state))
{
throw new InvalidOperationException(
"无法从Detour状态源读取有效车辆航向。" +
- (StateProvider is DetourVehicleStateProvider provider
- ? provider.LastFailureReason
- : ""));
+ GetStateProviderFailureReason(
+ stateProvider));
}
return (float)AngleMath.RadiansToDegrees(
state.PoseInWorld.YawRadians);
}
+ ///
+ /// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
+ ///
+ private static string GetStateProviderFailureReason(
+ IVehicleStateProvider stateProvider)
+ {
+ if (stateProvider is
+ WheelFeedbackVehicleStateProvider wheelProvider)
+ {
+ return wheelProvider.LastFailureReason;
+ }
+
+ if (stateProvider is
+ DetourVehicleStateProvider detourProvider)
+ {
+ return detourProvider.LastFailureReason;
+ }
+
+ return string.Empty;
+ }
+
///
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
///
diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
index a69d0e7..795d939 100644
--- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
@@ -4,6 +4,7 @@ using System.Diagnostics;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
+using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Control.Execution;
using MultiWheelC.Control.Lateral;
@@ -26,120 +27,120 @@ namespace MultiWheelC
public Trajectory2D Trajectory;
///
- /// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。
+ /// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
///
public IVehicleStateProvider StateProvider;
+ ///
+ /// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
+ ///
+ public Func
+ LateralControllerFactory;
+
///
/// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
///
public Action CycleObserver;
///
- /// Stanley横向误差增益，单位为1/s。
+ /// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
///
- public double StanleyCrossTrackGainPerSecond = 0.4;
+ public double? StanleyCrossTrackGainPerSecond;
///
- /// Stanley航向误差增益。
+ /// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
///
- public double StanleyHeadingErrorGain = 1.0;
+ public double? StanleyHeadingErrorGain;
///
- /// Stanley低速分母保护速度，单位为m/s。
+ /// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
///
- public double StanleyMinimumSpeedMetersPerSecond = 0.15;
+ public double? StanleyMinimumSpeedMetersPerSecond;
///
- /// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
+ /// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
///
- public bool StanleyUsesActualSpeed = true;
+ public bool? StanleyUsesActualSpeed;
///
- /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+ /// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
///
- public double MaximumCrossTrackCorrectionRadians =
- AngleMath.DegreesToRadians(10.0);
+ public double? MaximumCrossTrackCorrectionRadians;
///
- /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+ /// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
///
- public double MaximumHeadingCorrectionRadians =
- AngleMath.DegreesToRadians(10.0);
+ public double? MaximumHeadingCorrectionRadians;
///
- /// 纵向速度外环比例增益。
+ /// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
///
- public double LongitudinalKp = 0.5;
+ public double? LongitudinalKp;
///
- /// 纵向速度外环积分增益，单位为1/s。
+ /// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
///
- public double LongitudinalKiPerSecond;
+ public double? LongitudinalKiPerSecond;
///
- /// 纵向速度外环微分增益，单位为s。
+ /// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
///
- public double LongitudinalKdSeconds;
+ public double? LongitudinalKdSeconds;
///
- /// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
+ /// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
///
- public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
+ public double? MaximumIntegralCorrectionMetersPerSecond;
///
- /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+ /// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
///
- public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
- 0.025;
+ public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;
///
- /// 底盘纵向命令速度绝对值上限，单位为m/s。
+ /// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
///
- public double MaximumCommandSpeedMetersPerSecond = 0.50;
+ public double? MaximumCommandSpeedMetersPerSecond;
///
- /// 前后GCP允许的最大转角绝对值，单位为rad。
+ /// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
///
- public double MaximumGcpAngleRadians =
- AngleMath.DegreesToRadians(45.0);
+ public double? MaximumGcpAngleRadians;
///
- /// 前后GCP目标转角最大变化率，单位为rad/s。
+ /// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
///
- public double MaximumGcpAngleRateRadiansPerSecond =
- AngleMath.DegreesToRadians(15.0);
+ public double? MaximumGcpAngleRateRadiansPerSecond;
///
- /// 终点位置和剩余弧长的完成容差，单位为m。
+ /// 获取或设置本次动作的终点距离容差覆盖值，单位为m；为空时读取车辆配置。
///
- public double FinishDistanceMeters = 0.03;
+ public double? FinishDistanceMeters;
///
- /// 终点停稳判定允许的实际线速度，单位为m/s。
+ /// 获取或设置本次动作的终点速度容差覆盖值，单位为m/s；为空时读取车辆配置。
///
- public double FinishSpeedMetersPerSecond = 0.02;
+ public double? FinishSpeedMetersPerSecond;
///
- /// 终点航向完成容差，单位为rad。
+ /// 获取或设置本次动作的终点航向容差覆盖值，单位为rad；为空时读取车辆配置。
///
- public double FinishHeadingToleranceRadians =
- AngleMath.DegreesToRadians(3.0);
+ public double? FinishHeadingToleranceRadians;
///
- /// 终点减速阶段提前读取参考速度的距离，单位为m。
+ /// 获取或设置本次动作的终点制动预瞄距离覆盖值，单位为m；为空时读取车辆配置。
///
- public double TerminalBrakingPreviewMeters = 0.02;
+ public double? TerminalBrakingPreviewMeters;
///
- /// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
+ /// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
///
- public double MaximumDistanceToTrajectoryMeters = 0.30;
+ public double? MaximumDistanceToTrajectoryMeters;
///
- /// 单次轨迹动作允许的最长执行时间，单位为s。
+ /// 获取或设置本次动作的执行超时覆盖值，单位为s；为空时读取车辆配置。
///
- public double ExecutionTimeoutSeconds = 120.0;
+ public double? ExecutionTimeoutSeconds;
///
/// 获取本次动作创建的控制器，尚未开始时为空。
@@ -147,11 +148,78 @@ namespace MultiWheelC
public ParkingGeometricController Controller { get; private set; }
///
- /// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
+ /// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
///
public override IEnumerable Get()
{
- ValidateParameters();
+ var config = PilotDefinition.Conf;
+ var stanleyCrossTrackGainPerSecond =
+ StanleyCrossTrackGainPerSecond ??
+ config.ParkingStanleyCrossTrackGain;
+ var stanleyHeadingErrorGain =
+ StanleyHeadingErrorGain ??
+ config.ParkingStanleyHeadingGain;
+ var stanleyMinimumSpeedMetersPerSecond =
+ StanleyMinimumSpeedMetersPerSecond ??
+ config.ParkingStanleyMinimumSpeed;
+ var stanleyUsesActualSpeed =
+ StanleyUsesActualSpeed ??
+ config.ParkingStanleyUseActualSpeed;
+ var maximumCrossTrackCorrectionRadians =
+ MaximumCrossTrackCorrectionRadians ??
+ AngleMath.DegreesToRadians(
+ config.ParkingMaximumCrossTrackCorrectionDegrees);
+ var maximumHeadingCorrectionRadians =
+ MaximumHeadingCorrectionRadians ??
+ AngleMath.DegreesToRadians(
+ config.ParkingMaximumHeadingCorrectionDegrees);
+ var longitudinalKp =
+ LongitudinalKp ??
+ config.ParkingLongitudinalKp;
+ var longitudinalKiPerSecond =
+ LongitudinalKiPerSecond ??
+ config.ParkingLongitudinalKi;
+ var longitudinalKdSeconds =
+ LongitudinalKdSeconds ??
+ config.ParkingLongitudinalKd;
+ var maximumIntegralCorrectionMetersPerSecond =
+ MaximumIntegralCorrectionMetersPerSecond ??
+ config.ParkingMaximumIntegralCorrection;
+ var maximumCommandSpeedMetersPerSecond =
+ MaximumCommandSpeedMetersPerSecond ??
+ config.ParkingMaximumCommandSpeed;
+ var longitudinalSpeedErrorDeadbandMetersPerSecond =
+ LongitudinalSpeedErrorDeadbandMetersPerSecond ??
+ config.ParkingLongitudinalSpeedErrorDeadband;
+ var maximumGcpAngleRadians =
+ MaximumGcpAngleRadians ??
+ AngleMath.DegreesToRadians(
+ config.ParkingMaximumGcpAngleDegrees);
+ var maximumGcpAngleRateRadiansPerSecond =
+ MaximumGcpAngleRateRadiansPerSecond ??
+ AngleMath.DegreesToRadians(
+ config.ParkingMaximumGcpAngleRateDegreesPerSecond);
+ var finishDistanceMeters =
+ FinishDistanceMeters ??
+ config.ParkingFinishDistance;
+ var finishSpeedMetersPerSecond =
+ FinishSpeedMetersPerSecond ??
+ config.ParkingFinishSpeed;
+ var finishHeadingToleranceRadians =
+ FinishHeadingToleranceRadians ??
+ AngleMath.DegreesToRadians(
+ config.ParkingFinishHeadingToleranceDegrees);
+ var terminalBrakingPreviewMeters =
+ TerminalBrakingPreviewMeters ??
+ config.ParkingTerminalBrakingPreview;
+ var maximumDistanceToTrajectoryMeters =
+ MaximumDistanceToTrajectoryMeters ??
+ config.ParkingMaximumDistanceToTrajectory;
+ var executionTimeoutSeconds =
+ ExecutionTimeoutSeconds ??
+ config.ParkingExecutionTimeoutSeconds;
+
+ ValidateParameters(executionTimeoutSeconds);
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
@@ -161,6 +229,24 @@ namespace MultiWheelC
"当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
}
+ var wheelPreparation =
+ new PrepareWheelsForward();
+ foreach (var keepRunning in wheelPreparation.Get())
+ {
+ if (!keepRunning)
+ {
+ break;
+ }
+
+ yield return true;
+ }
+
+ if (!wheelPreparation.Completed)
+ {
+ throw new InvalidOperationException(
+ "轨迹跟踪开始前舵轮未能稳定回到车头方向。");
+ }
+
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
@@ -170,34 +256,44 @@ namespace MultiWheelC
var stateProvider =
StateProvider ??
- new DetourVehicleStateProvider();
+ ParkingVehicleStateProviderFactory.Create(
+ chassis,
+ config);
var controlPointRadiusMeters =
chassis.ControlPointRadius / 1000.0;
var lateralController =
- new StanleyLateralController(
- controlPointRadiusMeters,
- StanleyCrossTrackGainPerSecond,
- StanleyHeadingErrorGain,
- StanleyMinimumSpeedMetersPerSecond,
- StanleyUsesActualSpeed,
- MaximumCrossTrackCorrectionRadians,
- MaximumHeadingCorrectionRadians);
+ LateralControllerFactory == null
+ ? new StanleyLateralController(
+ controlPointRadiusMeters,
+ stanleyCrossTrackGainPerSecond,
+ stanleyHeadingErrorGain,
+ stanleyMinimumSpeedMetersPerSecond,
+ stanleyUsesActualSpeed,
+ maximumCrossTrackCorrectionRadians,
+ maximumHeadingCorrectionRadians)
+ : LateralControllerFactory(
+ controlPointRadiusMeters);
+ if (lateralController == null)
+ {
+ throw new InvalidOperationException(
+ "横向控制器创建委托不能返回空值。");
+ }
var longitudinalController =
new PidLongitudinalController(
- LongitudinalKp,
- LongitudinalKiPerSecond,
- LongitudinalKdSeconds,
- MaximumIntegralCorrectionMetersPerSecond,
- MaximumCommandSpeedMetersPerSecond,
- LongitudinalSpeedErrorDeadbandMetersPerSecond);
+ longitudinalKp,
+ longitudinalKiPerSecond,
+ longitudinalKdSeconds,
+ maximumIntegralCorrectionMetersPerSecond,
+ maximumCommandSpeedMetersPerSecond,
+ longitudinalSpeedErrorDeadbandMetersPerSecond);
var gcpAllocator =
new GcpCommandAllocator(
- MaximumGcpAngleRadians);
+ maximumGcpAngleRadians);
var commandExecutor =
new GcpCommandExecutor(
adapter,
- MaximumGcpAngleRateRadiansPerSecond);
+ maximumGcpAngleRateRadiansPerSecond);
Controller = new ParkingGeometricController(
stateProvider,
@@ -205,11 +301,11 @@ namespace MultiWheelC
longitudinalController,
gcpAllocator,
commandExecutor,
- FinishDistanceMeters,
- FinishSpeedMetersPerSecond,
- FinishHeadingToleranceRadians,
- MaximumDistanceToTrajectoryMeters,
- TerminalBrakingPreviewMeters);
+ finishDistanceMeters,
+ finishSpeedMetersPerSecond,
+ finishHeadingToleranceRadians,
+ maximumDistanceToTrajectoryMeters,
+ terminalBrakingPreviewMeters);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
@@ -221,10 +317,10 @@ namespace MultiWheelC
while (true)
{
if (clock.Elapsed.TotalSeconds >
- ExecutionTimeoutSeconds)
+ executionTimeoutSeconds)
{
throw new TimeoutException(
- $"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
+ $"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
}
var currentCycleSeconds =
@@ -291,7 +387,8 @@ namespace MultiWheelC
///
/// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
///
- private void ValidateParameters()
+ private void ValidateParameters(
+ double executionTimeoutSeconds)
{
if (Trajectory == null)
{
@@ -299,9 +396,9 @@ namespace MultiWheelC
"新版轨迹跟踪动作没有设置Trajectory。");
}
- if (double.IsNaN(ExecutionTimeoutSeconds) ||
- double.IsInfinity(ExecutionTimeoutSeconds) ||
- ExecutionTimeoutSeconds <= 0.0)
+ if (double.IsNaN(executionTimeoutSeconds) ||
+ double.IsInfinity(executionTimeoutSeconds) ||
+ executionTimeoutSeconds <= 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(ExecutionTimeoutSeconds),
diff --git a/MultiWheelC/PilotConfig.cs b/MultiWheelC/PilotConfig.cs
index f1ae1c2..0e97ae5 100644
--- a/MultiWheelC/PilotConfig.cs
+++ b/MultiWheelC/PilotConfig.cs
@@ -4,7 +4,10 @@ using Newtonsoft.Json;
namespace MultiWheelC;
-public class PilotConfig : MultiWheelPilotConfig
+///
+/// 定义由MDCS显示、持久化并随车辆部署的运行参数。
+///
+public partial class PilotConfig : MultiWheelPilotConfig
{
#region 单车-轨迹跟踪 LineTracking
@@ -18,53 +21,6 @@ public class PilotConfig : MultiWheelPilotConfig
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
#endregion
- #region 单车-原地旋转 暂时没用上
- [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
- public float InPlaceRotateTargetWorldDeg = 90f;
-
- [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
- public float InPlaceRotateSpeed = 30f;
-
- [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
- public float InPlaceRotateArriveDeg = 1.5f;
-
- [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
- public float InPlaceRotateWheelAlignDeg = 2f;
-
- [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
- public float InPlaceRotateActiveWheelAlignDeg = 10f;
-
- #endregion
-
- #region 单车-临时
- [FieldMember(desc = "原地旋转Kp")]
- public float InPlaceRotateKp = 1.05f;
-
- [FieldMember(desc = "原地旋转Ki")]
- public float InPlaceRotateKi = 0f;
-
- [FieldMember(desc = "原地旋转Kd")]
- public float InPlaceRotateKd = 0f;
-
- [FieldMember(desc = "原地旋转积分限幅")]
- public float InPlaceRotateMaxI = 0f;
-
- [FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
- public float InPlaceRotateMinimumSpeed = 1f;
-
- [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
- public float InPlaceRotateMaxSpeed = 47.5f;
-
- [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
- public float InPlaceRotateAcc = 60f;
-
- [FieldMember(desc = "原地旋转超时(s)")]
- public float InPlaceRotateTimeoutSec = 15f;
- #endregion
-
-
-
-
#region 单车-钻车与夹抱
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
diff --git a/MultiWheelC/Trajectory/Trajectory2D.cs b/MultiWheelC/Trajectory/Trajectory2D.cs
index a991bde..687ab5f 100644
--- a/MultiWheelC/Trajectory/Trajectory2D.cs
+++ b/MultiWheelC/Trajectory/Trajectory2D.cs
@@ -7,18 +7,22 @@ using MyParking.Shared;
namespace MultiWheelC.Trajectory
{
///
- /// 保存一条经过基本合法性检查的只读二维参考轨迹。
+ /// 保存一条按预定执行点序排列、以累计弧长参数化的只读二维参考轨迹。
///
public sealed class Trajectory2D
{
private const double StartArcLengthToleranceMeters = 1e-9;
private const double MinimumSegmentLengthMeters = 1e-6;
+ private const double ArcLengthConsistencyAbsoluteToleranceMeters =
+ 1e-6;
+ private const double ArcLengthConsistencyRelativeTolerance =
+ 0.01;
private readonly TrajectoryPoint[] _points;
private readonly ReadOnlyCollection _readOnlyPoints;
///
- /// 复制并验证按累计弧长升序排列的参考轨迹点。
+ /// 复制并验证按实际执行顺序及累计弧长升序排列的参考轨迹点。
///
public Trajectory2D(
IEnumerable points)
@@ -102,7 +106,7 @@ namespace MultiWheelC.Trajectory
public double GetRemainingDistanceMeters(
double arcLengthMeters)
{
- EnsureFinite(
+ NumericGuard.EnsureFinite(
arcLengthMeters,
nameof(arcLengthMeters));
@@ -121,7 +125,7 @@ namespace MultiWheelC.Trajectory
public TrajectoryPoint SampleAtArcLength(
double arcLengthMeters)
{
- EnsureFinite(
+ NumericGuard.EnsureFinite(
arcLengthMeters,
nameof(arcLengthMeters));
@@ -148,6 +152,48 @@ namespace MultiWheelC.Trajectory
(segmentEnd.ArcLengthMeters -
segmentStart.ArcLengthMeters);
+ return InterpolateSegment(
+ segmentStartIndex,
+ interpolationRatio);
+ }
+
+ ///
+ /// 在指定线段上统一插值位置、车头航向、曲率和有符号参考速度。
+ ///
+ internal TrajectoryPoint InterpolateSegment(
+ int segmentStartIndex,
+ double interpolationRatio)
+ {
+ if (segmentStartIndex < 0 ||
+ segmentStartIndex >= _points.Length - 1)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(segmentStartIndex),
+ "轨迹插值线段索引必须指向一条有效线段的起点。");
+ }
+
+ NumericGuard.EnsureFinite(
+ interpolationRatio,
+ nameof(interpolationRatio));
+
+ if (interpolationRatio < 0.0 ||
+ interpolationRatio > 1.0)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(interpolationRatio),
+ "轨迹线段插值比例必须位于[0,1]范围内。");
+ }
+
+ var segmentStart =
+ _points[segmentStartIndex];
+ var segmentEnd =
+ _points[segmentStartIndex + 1];
+ var arcLengthMeters =
+ InterpolationMath.Lerp(
+ segmentStart.ArcLengthMeters,
+ segmentEnd.ArcLengthMeters,
+ interpolationRatio);
+
return new TrajectoryPoint(
arcLengthMeters,
new Pose2D(
@@ -176,9 +222,23 @@ namespace MultiWheelC.Trajectory
///
/// 使用二分查找获取包含指定累计弧长的线段起点索引。
///
- private int FindSegmentStartIndex(
+ internal int FindSegmentStartIndex(
double arcLengthMeters)
{
+ NumericGuard.EnsureFinite(
+ arcLengthMeters,
+ nameof(arcLengthMeters));
+
+ if (arcLengthMeters <= 0.0)
+ {
+ return 0;
+ }
+
+ if (arcLengthMeters >= TotalLengthMeters)
+ {
+ return _points.Length - 2;
+ }
+
var lowerIndex = 0;
var upperIndex = _points.Length - 1;
@@ -239,21 +299,32 @@ namespace MultiWheelC.Trajectory
$"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
parameterName);
}
- }
- ///
- /// 检查数值是否为有限值。
- ///
- private static void EnsureFinite(
- double value,
- string parameterName)
- {
- if (double.IsNaN(value) ||
- double.IsInfinity(value))
+ var segmentLengthMeters =
+ Math.Sqrt(segmentLengthSquared);
+ var arcLengthIncrementMeters =
+ current.ArcLengthMeters -
+ previous.ArcLengthMeters;
+ var maximumAllowedDifferenceMeters =
+ Math.Max(
+ ArcLengthConsistencyAbsoluteToleranceMeters,
+ ArcLengthConsistencyRelativeTolerance *
+ Math.Max(
+ segmentLengthMeters,
+ arcLengthIncrementMeters));
+
+ // 当前轨迹在相邻采样点之间按直线段投影，因此累计弧长增量
+ // 必须与该离散线段长度近似一致，防止进度和实际几何脱节。
+ if (Math.Abs(
+ arcLengthIncrementMeters -
+ segmentLengthMeters) >
+ maximumAllowedDifferenceMeters)
{
- throw new ArgumentOutOfRangeException(
- parameterName,
- "轨迹弧长必须是有限值。");
+ throw new ArgumentException(
+ $"轨迹点{currentIndex}的累计弧长增量" +
+ $"{arcLengthIncrementMeters:F6}m与离散线段长度" +
+ $"{segmentLengthMeters:F6}m不一致。",
+ parameterName);
}
}
}
diff --git a/MultiWheelC/Trajectory/TrajectoryPoint.cs b/MultiWheelC/Trajectory/TrajectoryPoint.cs
index 83dd56c..333e1ce 100644
--- a/MultiWheelC/Trajectory/TrajectoryPoint.cs
+++ b/MultiWheelC/Trajectory/TrajectoryPoint.cs
@@ -4,7 +4,7 @@ using MyParking.Shared;
namespace MultiWheelC.Trajectory
{
///
- /// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
+ /// 描述按执行点序和累计弧长参数化的车体中心参考轨迹点，统一使用SI单位。
///
public readonly struct TrajectoryPoint
{
@@ -17,22 +17,16 @@ namespace MultiWheelC.Trajectory
double curvaturePerMeter,
double referenceSpeedMetersPerSecond)
{
- EnsureFiniteNonNegative(
+ NumericGuard.EnsureFiniteNonNegative(
arcLengthMeters,
nameof(arcLengthMeters));
- EnsureFinite(
- poseInWorld.XMeters,
+ NumericGuard.EnsureFinite(
+ poseInWorld,
nameof(poseInWorld));
- EnsureFinite(
- poseInWorld.YMeters,
- nameof(poseInWorld));
- EnsureFinite(
- poseInWorld.YawRadians,
- nameof(poseInWorld));
- EnsureFinite(
+ NumericGuard.EnsureFinite(
curvaturePerMeter,
nameof(curvaturePerMeter));
- EnsureFinite(
+ NumericGuard.EnsureFinite(
referenceSpeedMetersPerSecond,
nameof(referenceSpeedMetersPerSecond));
ArcLengthMeters = arcLengthMeters;
@@ -47,56 +41,24 @@ namespace MultiWheelC.Trajectory
}
///
- /// 获取从轨迹起点累计到当前点的弧长，单位为m。
+ /// 获取沿预定执行点序从轨迹起点累计到当前点的弧长，单位为m。
///
public double ArcLengthMeters { get; }
///
- /// 获取车体中心参考坐标系在世界坐标系中的位姿。
+ /// 获取车体中心参考坐标系在世界坐标系中的位姿；航向始终表示车头方向。
///
public Pose2D PoseInWorld { get; }
///
- /// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
+ /// 获取沿累计弧长增加方向的车体中心参考轨迹曲率，单位为1/m，左弯为正。
///
public double CurvaturePerMeter { get; }
///
- /// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
+ /// 获取车体纵向有符号参考速度，单位为m/s；正值前进、负值倒车、零值停车。
///
public double ReferenceSpeedMetersPerSecond { get; }
- ///
- /// 检查数值是否为有限值。
- ///
- private static void EnsureFinite(
- double value,
- string parameterName)
- {
- if (double.IsNaN(value) ||
- double.IsInfinity(value))
- {
- throw new ArgumentOutOfRangeException(
- parameterName,
- "轨迹点参数必须是有限值。");
- }
- }
-
- ///
- /// 检查数值是否为非负有限值。
- ///
- private static void EnsureFiniteNonNegative(
- double value,
- string parameterName)
- {
- EnsureFinite(value, parameterName);
-
- if (value < 0.0)
- {
- throw new ArgumentOutOfRangeException(
- parameterName,
- "轨迹累计弧长不能为负数。");
- }
- }
}
}
diff --git a/MultiWheelC/Trajectory/TrajectoryProjection.cs b/MultiWheelC/Trajectory/TrajectoryProjection.cs
index 798ae17..e5439c5 100644
--- a/MultiWheelC/Trajectory/TrajectoryProjection.cs
+++ b/MultiWheelC/Trajectory/TrajectoryProjection.cs
@@ -26,16 +26,16 @@ namespace MultiWheelC.Trajectory
"投影线段起点索引不能为负数。");
}
- EnsureFinite(
+ NumericGuard.EnsureFinite(
lateralErrorMeters,
nameof(lateralErrorMeters));
- EnsureFinite(
+ NumericGuard.EnsureFinite(
headingErrorRadians,
nameof(headingErrorRadians));
- EnsureFiniteNonNegative(
+ NumericGuard.EnsureFiniteNonNegative(
distanceToTrajectoryMeters,
nameof(distanceToTrajectoryMeters));
- EnsureFiniteNonNegative(
+ NumericGuard.EnsureFiniteNonNegative(
remainingDistanceMeters,
nameof(remainingDistanceMeters));
@@ -62,7 +62,7 @@ namespace MultiWheelC.Trajectory
public TrajectoryPoint ReferencePoint { get; }
///
- /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
+ /// 获取相对累计弧长增加方向的有符号横向误差，单位为m，参考轨迹位于该方向左侧时为正。
///
public double LateralErrorMeters { get; }
@@ -87,37 +87,5 @@ namespace MultiWheelC.Trajectory
public double ArcLengthMeters =>
ReferencePoint.ArcLengthMeters;
- ///
- /// 检查数值是否为有限值。
- ///
- private static void EnsureFinite(
- double value,
- string parameterName)
- {
- if (double.IsNaN(value) ||
- double.IsInfinity(value))
- {
- throw new ArgumentOutOfRangeException(
- parameterName,
- "轨迹投影参数必须是有限值。");
- }
- }
-
- ///
- /// 检查数值是否为非负有限值。
- ///
- private static void EnsureFiniteNonNegative(
- double value,
- string parameterName)
- {
- EnsureFinite(value, parameterName);
-
- if (value < 0.0)
- {
- throw new ArgumentOutOfRangeException(
- parameterName,
- "轨迹投影距离不能为负数。");
- }
- }
}
}
diff --git a/MultiWheelC/Trajectory/TrajectoryProjector.cs b/MultiWheelC/Trajectory/TrajectoryProjector.cs
index 43cc3c5..a2cb4f8 100644
--- a/MultiWheelC/Trajectory/TrajectoryProjector.cs
+++ b/MultiWheelC/Trajectory/TrajectoryProjector.cs
@@ -4,10 +4,13 @@ using MyParking.Shared;
namespace MultiWheelC.Trajectory
{
///
- /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
+ /// 将实际车体中心位姿投影到二维离散参考轨迹，并支持按上次进度限制搜索范围。
///
public static class TrajectoryProjector
{
+ private const double DistanceTieToleranceSquaredMeters =
+ 1e-12;
+
///
/// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
///
@@ -15,25 +18,98 @@ namespace MultiWheelC.Trajectory
Trajectory2D trajectory,
Pose2D vehiclePoseInWorld)
{
- if (trajectory == null)
- {
- throw new ArgumentNullException(
- nameof(trajectory));
- }
-
- EnsureFinitePose(
+ ValidateProjectionInput(
+ trajectory,
vehiclePoseInWorld,
nameof(vehiclePoseInWorld));
+ return ProjectRange(
+ trajectory,
+ vehiclePoseInWorld,
+ firstSegmentStartIndex: 0,
+ lastSegmentStartIndex:
+ trajectory.Count - 2,
+ preferredArcLengthMeters: null);
+ }
+
+ ///
+ /// 以上次投影弧长为中心，仅在指定前后物理距离窗口内查找最近线段。
+ ///
+ public static TrajectoryProjection Project(
+ Trajectory2D trajectory,
+ Pose2D vehiclePoseInWorld,
+ double previousArcLengthMeters,
+ double maximumBackwardSearchDistanceMeters,
+ double maximumForwardSearchDistanceMeters)
+ {
+ ValidateProjectionInput(
+ trajectory,
+ vehiclePoseInWorld,
+ nameof(vehiclePoseInWorld));
+ NumericGuard.EnsureFiniteNonNegative(
+ previousArcLengthMeters,
+ nameof(previousArcLengthMeters));
+ NumericGuard.EnsureFiniteNonNegative(
+ maximumBackwardSearchDistanceMeters,
+ nameof(maximumBackwardSearchDistanceMeters));
+ NumericGuard.EnsureFinitePositive(
+ maximumForwardSearchDistanceMeters,
+ nameof(maximumForwardSearchDistanceMeters));
+
+ if (previousArcLengthMeters >
+ trajectory.TotalLengthMeters)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(previousArcLengthMeters),
+ "上次投影弧长不能超过轨迹总长度。");
+ }
+
+ var searchStartArcLengthMeters =
+ Math.Max(
+ 0.0,
+ previousArcLengthMeters -
+ maximumBackwardSearchDistanceMeters);
+ var searchEndArcLengthMeters =
+ Math.Min(
+ trajectory.TotalLengthMeters,
+ previousArcLengthMeters +
+ maximumForwardSearchDistanceMeters);
+ var firstSegmentStartIndex =
+ trajectory.FindSegmentStartIndex(
+ searchStartArcLengthMeters);
+ var lastSegmentStartIndex =
+ trajectory.FindSegmentStartIndex(
+ searchEndArcLengthMeters);
+
+ return ProjectRange(
+ trajectory,
+ vehiclePoseInWorld,
+ firstSegmentStartIndex,
+ lastSegmentStartIndex,
+ previousArcLengthMeters);
+ }
+
+ ///
+ /// 在闭区间线段索引范围内查找最近投影，并在距离并列时优先保持原进度。
+ ///
+ private static TrajectoryProjection ProjectRange(
+ Trajectory2D trajectory,
+ Pose2D vehiclePoseInWorld,
+ int firstSegmentStartIndex,
+ int lastSegmentStartIndex,
+ double? preferredArcLengthMeters)
+ {
var bestSegmentStartIndex = 0;
var bestInterpolationRatio = 0.0;
- var bestProjectedX = 0.0;
- var bestProjectedY = 0.0;
var bestDistanceSquared =
double.PositiveInfinity;
+ var bestProgressDifferenceMeters =
+ double.PositiveInfinity;
- for (var segmentStartIndex = 0;
- segmentStartIndex < trajectory.Count - 1;
+ for (var segmentStartIndex =
+ firstSegmentStartIndex;
+ segmentStartIndex <=
+ lastSegmentStartIndex;
segmentStartIndex++)
{
var segmentStart =
@@ -85,7 +161,30 @@ namespace MultiWheelC.Trajectory
projectionErrorX * projectionErrorX +
projectionErrorY * projectionErrorY;
- if (distanceSquared >= bestDistanceSquared)
+ var progressDifferenceMeters =
+ preferredArcLengthMeters.HasValue
+ ? Math.Abs(
+ InterpolationMath.Lerp(
+ segmentStart.ArcLengthMeters,
+ segmentEnd.ArcLengthMeters,
+ interpolationRatio) -
+ preferredArcLengthMeters.Value)
+ : 0.0;
+ var hasMeaningfullyShorterDistance =
+ distanceSquared <
+ bestDistanceSquared -
+ DistanceTieToleranceSquaredMeters;
+ var hasEquivalentDistanceAndCloserProgress =
+ preferredArcLengthMeters.HasValue &&
+ Math.Abs(
+ distanceSquared -
+ bestDistanceSquared) <=
+ DistanceTieToleranceSquaredMeters &&
+ progressDifferenceMeters <
+ bestProgressDifferenceMeters;
+
+ if (!hasMeaningfullyShorterDistance &&
+ !hasEquivalentDistanceAndCloserProgress)
{
continue;
}
@@ -94,9 +193,9 @@ namespace MultiWheelC.Trajectory
segmentStartIndex;
bestInterpolationRatio =
interpolationRatio;
- bestProjectedX = projectedX;
- bestProjectedY = projectedY;
bestDistanceSquared = distanceSquared;
+ bestProgressDifferenceMeters =
+ progressDifferenceMeters;
}
return BuildProjection(
@@ -104,8 +203,6 @@ namespace MultiWheelC.Trajectory
vehiclePoseInWorld,
bestSegmentStartIndex,
bestInterpolationRatio,
- bestProjectedX,
- bestProjectedY,
bestDistanceSquared);
}
@@ -117,45 +214,16 @@ namespace MultiWheelC.Trajectory
Pose2D vehiclePoseInWorld,
int segmentStartIndex,
double interpolationRatio,
- double projectedX,
- double projectedY,
double distanceSquared)
{
var segmentStart =
trajectory[segmentStartIndex];
var segmentEnd =
trajectory[segmentStartIndex + 1];
-
- var referenceYawRadians =
- AngleMath.LerpRadians(
- segmentStart.PoseInWorld.YawRadians,
- segmentEnd.PoseInWorld.YawRadians,
- interpolationRatio);
- var referenceArcLengthMeters =
- InterpolationMath.Lerp(
- segmentStart.ArcLengthMeters,
- segmentEnd.ArcLengthMeters,
- interpolationRatio);
- var referenceCurvaturePerMeter =
- InterpolationMath.Lerp(
- segmentStart.CurvaturePerMeter,
- segmentEnd.CurvaturePerMeter,
- interpolationRatio);
- var referenceSpeedMetersPerSecond =
- InterpolationMath.Lerp(
- segmentStart.ReferenceSpeedMetersPerSecond,
- segmentEnd.ReferenceSpeedMetersPerSecond,
- interpolationRatio);
-
var referencePoint =
- new TrajectoryPoint(
- referenceArcLengthMeters,
- new Pose2D(
- projectedX,
- projectedY,
- referenceYawRadians),
- referenceCurvaturePerMeter,
- referenceSpeedMetersPerSecond);
+ trajectory.InterpolateSegment(
+ segmentStartIndex,
+ interpolationRatio);
var segmentX =
segmentEnd.PoseInWorld.XMeters -
@@ -171,10 +239,10 @@ namespace MultiWheelC.Trajectory
// 以轨迹线段的前进方向判断左右：
// 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
var vehicleToProjectionX =
- projectedX -
+ referencePoint.PoseInWorld.XMeters -
vehiclePoseInWorld.XMeters;
var vehicleToProjectionY =
- projectedY -
+ referencePoint.PoseInWorld.YMeters -
vehiclePoseInWorld.YMeters;
var lateralErrorMeters =
(segmentX * vehicleToProjectionY -
@@ -183,7 +251,7 @@ namespace MultiWheelC.Trajectory
var headingErrorRadians =
AngleMath.ShortestDifferenceRadians(
- referenceYawRadians,
+ referencePoint.PoseInWorld.YawRadians,
vehiclePoseInWorld.YawRadians);
return new TrajectoryProjection(
@@ -193,27 +261,26 @@ namespace MultiWheelC.Trajectory
headingErrorRadians,
Math.Sqrt(distanceSquared),
trajectory.GetRemainingDistanceMeters(
- referenceArcLengthMeters));
+ referencePoint.ArcLengthMeters));
}
///
- /// 检查用于投影的实际车体中心位姿是否包含有限数值。
+ /// 检查轨迹对象和用于投影的实际车体中心位姿。
///
- private static void EnsureFinitePose(
+ private static void ValidateProjectionInput(
+ Trajectory2D trajectory,
Pose2D pose,
string parameterName)
{
- if (double.IsNaN(pose.XMeters) ||
- double.IsInfinity(pose.XMeters) ||
- double.IsNaN(pose.YMeters) ||
- double.IsInfinity(pose.YMeters) ||
- double.IsNaN(pose.YawRadians) ||
- double.IsInfinity(pose.YawRadians))
+ if (trajectory == null)
{
- throw new ArgumentOutOfRangeException(
- parameterName,
- "用于轨迹投影的车体位姿必须是有限值。");
+ throw new ArgumentNullException(
+ nameof(trajectory));
}
+
+ NumericGuard.EnsureFinite(
+ pose,
+ parameterName);
}
}
}
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index b4d199e..9ff7b74 100644
Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll
index 6dfac63..a19ff33 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb
index d70a5d3..d982861 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index b4d199e..9ff7b74 100644
Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ
diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll
index 6dfac63..a19ff33 100644
Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ
diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb
index d70a5d3..d982861 100644
Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ
diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll
index b4d199e..9ff7b74 100644
Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ
diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll
index 0fca26f..fb1eff4 100644
Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ
diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb
index 246e2b6..d91a710 100644
Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ
diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll
index b4d199e..9ff7b74 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ