diff --git a/MultiWheelC/Control/Abstractions/ILateralController.cs b/MultiWheelC/Control/Abstractions/ILateralController.cs new file mode 100644 index 0000000..18d7fbe --- /dev/null +++ b/MultiWheelC/Control/Abstractions/ILateralController.cs @@ -0,0 +1 @@ +// 所有横向控制器的统一接口 diff --git a/MultiWheelC/Control/Abstractions/ILongitudinalController.cs b/MultiWheelC/Control/Abstractions/ILongitudinalController.cs new file mode 100644 index 0000000..20d7e3e --- /dev/null +++ b/MultiWheelC/Control/Abstractions/ILongitudinalController.cs @@ -0,0 +1 @@ +// 统一纵向控制接口 diff --git a/MultiWheelC/Control/Abstractions/LateralControlCommand.cs b/MultiWheelC/Control/Abstractions/LateralControlCommand.cs new file mode 100644 index 0000000..e123283 --- /dev/null +++ b/MultiWheelC/Control/Abstractions/LateralControlCommand.cs @@ -0,0 +1 @@ +// 横向控制器的输出 diff --git a/MultiWheelC/Control/Abstractions/PathTrackingContext.cs b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs new file mode 100644 index 0000000..7ffeb45 --- /dev/null +++ b/MultiWheelC/Control/Abstractions/PathTrackingContext.cs @@ -0,0 +1 @@ +// 保存一次控制周期需要的完整输入 diff --git a/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs b/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs new file mode 100644 index 0000000..c5f2291 --- /dev/null +++ b/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs @@ -0,0 +1 @@ +// 车体中心命令曲率转换成前后GCP方向 diff --git a/MultiWheelC/Control/Allocation/GcpMotionCommand.cs b/MultiWheelC/Control/Allocation/GcpMotionCommand.cs new file mode 100644 index 0000000..8a5c726 --- /dev/null +++ b/MultiWheelC/Control/Allocation/GcpMotionCommand.cs @@ -0,0 +1,9 @@ +// 表示发送给底盘前的中间命令 +// public readonly struct GcpMotionCommand +// { +// public double SpeedMetersPerSecond { get; } + +// public double FrontAngleRadians { get; } + +// public double RearAngleRadians { get; } +// } \ No newline at end of file diff --git a/MultiWheelC/Control/Execution/GcpCommandExecutor.cs b/MultiWheelC/Control/Execution/GcpCommandExecutor.cs new file mode 100644 index 0000000..92633be --- /dev/null +++ b/MultiWheelC/Control/Execution/GcpCommandExecutor.cs @@ -0,0 +1 @@ +// 负责把纯数学命令转换成现有底盘调用 \ No newline at end of file diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs new file mode 100644 index 0000000..eef8148 --- /dev/null +++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs @@ -0,0 +1 @@ +// 负责组织一个控制周期 diff --git a/MultiWheelC/Control/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs new file mode 100644 index 0000000..e69de29 diff --git a/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs b/MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs new file mode 100644 index 0000000..e69de29 diff --git a/MultiWheelC/Experiments/ClampTests.cs b/MultiWheelC/Experiments/ClampTests.cs new file mode 100644 index 0000000..b789a68 --- /dev/null +++ b/MultiWheelC/Experiments/ClampTests.cs @@ -0,0 +1,90 @@ + + +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/MovementTests.cs b/MultiWheelC/Experiments/PathTrackingTests.cs similarity index 66% rename from MultiWheelC/MovementTests.cs rename to MultiWheelC/Experiments/PathTrackingTests.cs index 2d61af6..a3f82d9 100644 --- a/MultiWheelC/MovementTests.cs +++ b/MultiWheelC/Experiments/PathTrackingTests.cs @@ -1,108 +1,20 @@ -using ClumsyCore; -using ClumsyCore.DTools; -using ClumsyCore.Interfaces; -using ClumsyCore.Pilot; -using CommonUsage.Chassis; -using MDCSToolBox.Commons.Controllers; -using MDCSToolBox.Clumsy.Tracks; -using MyParking.Shared; + + 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 { - internal static class MovementTestPreparation - { - // 在测试正式开始前，将四个舵轮稳定回正到车体前向。 - public static bool AlignWheelsForward( - ref DriveTask activeTask) - { - var preparation = new PrepareWheelsForward(); - var task = new DriveTask(preparation.Get()); - activeTask = task; - - try - { - task.Wait(); - return preparation.Completed; - } - catch (Exception ex) - { - Console.WriteLine( - $"测试前舵轮回正失败：{ex.Message}"); - return false; - } - finally - { - task.Stop(); - if (ReferenceEquals(activeTask, task)) - activeTask = null; - } - } - - // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。 - 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 - { - private DriveTask _task; - - // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。 - public override void Test() - { - MovementTestPreparation.AlignWheelsForward( - ref _task); - } - - // 停止正在执行的舵轮回正任务并清零底盘运动命令。 - public override void TestStop() - { - _task?.Stop(); - _task = null; - } - } - [MovementTest(name = "SendMotion：连续前进4m")] public class TestForward4m : MovementTest { @@ -179,148 +91,6 @@ namespace MultiWheelC } } - public abstract class InPlaceRotateTestBase : MovementTest - { - public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。 - public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。 - public int TrialNumber = 1; // 重复实验编号。 - - private DriveTask _task; - private TrackingExperimentRecorder _recorder; - private readonly string _trajectoryName; - - protected InPlaceRotateTestBase( - float relativeAngleDegrees, - string trajectoryName) - { - RelativeAngleDegrees = - relativeAngleDegrees; - _trajectoryName = - trajectoryName; - } - - // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。 - public override void Test() - { - if (float.IsNaN(RelativeAngleDegrees) || - float.IsInfinity(RelativeAngleDegrees) || - float.IsNaN(MaxAngularSpeedDegreesPerSecond) || - float.IsInfinity(MaxAngularSpeedDegreesPerSecond) || - MaxAngularSpeedDegreesPerSecond <= 0f) - { - Console.WriteLine("原地旋转测试参数无效。"); - 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 rotationCenter = - new Vector2((float)location.x, (float)location.y); - var targetWorldAngle = - (float)AngleMath.NormalizeDegrees( - location.th + RelativeAngleDegrees); - - _recorder = new TrackingExperimentRecorder( - controllerName: "InPlaceRotatePID", - trajectoryName: _trajectoryName, - trialNumber: TrialNumber, - referenceStart: rotationCenter, - referenceEnd: rotationCenter, - referenceSpeed: 0f, - referenceAngularSpeed: - (float)AngleMath.DegreesToRadians( - MaxAngularSpeedDegreesPerSecond)); - _recorder.Start(); - - try - { - _task = new DriveTask( - new MultiWheelRotateInPlace - { - // MultiWheelRotateInPlace接收世界坐标系绝对航向。 - AngleTarget = targetWorldAngle, - PidparamsRead = () => new PIDParams - { - Kp = - PilotDefinition.Conf.InPlaceRotateKp, - Ki = - PilotDefinition.Conf.InPlaceRotateKi, - Kd = - PilotDefinition.Conf.InPlaceRotateKd, - DeadZone = - PilotDefinition.Conf - .InPlaceRotateArriveDeg, - SpeedAccPerSec = - PilotDefinition.Conf.InPlaceRotateAcc, - OutputUpperThreshold = - MaxAngularSpeedDegreesPerSecond, - MaxI = - PilotDefinition.Conf.InPlaceRotateMaxI - }, - CommandAngularSpeedObserver = - commandAngularSpeed => - _recorder?.UpdateCommand( - 0f, - (float)AngleMath.DegreesToRadians( - commandAngularSpeed)) - }.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 = "SendXYThSpeed：原地自转90°")] - public sealed class TestRotate90 : - InPlaceRotateTestBase - { - public TestRotate90() - : base(90f, "Rotate90") - { - } - } - - [MovementTest(name = "SendXYThSpeed：原地自转180°")] - public sealed class TestRotate180 : - InPlaceRotateTestBase - { - public TestRotate180() - : base(180f, "Rotate180") - { - } - } - [MovementTest(name = "SendMotion：左转90°半径2m圆弧")] public class TestArcMovement : MovementTest { @@ -449,7 +219,6 @@ namespace MultiWheelC } } - #region 蟹行运动测试 [MovementTest(name = "SendMotion：蟹行直线4m")] public class TestCrabForward4m : MovementTest { @@ -863,87 +632,4 @@ namespace MultiWheelC origin.Y + localX * sin + localY * cos); } } - - #endregion - - #region 夹臂运动测试 - 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; - } - #endregion } diff --git a/MultiWheelC/Experiments/RotationTests.cs b/MultiWheelC/Experiments/RotationTests.cs new file mode 100644 index 0000000..56f6b4e --- /dev/null +++ b/MultiWheelC/Experiments/RotationTests.cs @@ -0,0 +1,158 @@ + + +using System; +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.Commons.Controllers; +using MyParking.Shared; + +namespace MultiWheelC +{ + public abstract class InPlaceRotateTestBase : MovementTest + { + public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。 + public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。 + public int TrialNumber = 1; // 重复实验编号。 + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + private readonly string _trajectoryName; + + protected InPlaceRotateTestBase( + float relativeAngleDegrees, + string trajectoryName) + { + RelativeAngleDegrees = + relativeAngleDegrees; + _trajectoryName = + trajectoryName; + } + + // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。 + public override void Test() + { + if (float.IsNaN(RelativeAngleDegrees) || + float.IsInfinity(RelativeAngleDegrees) || + float.IsNaN(MaxAngularSpeedDegreesPerSecond) || + float.IsInfinity(MaxAngularSpeedDegreesPerSecond) || + MaxAngularSpeedDegreesPerSecond <= 0f) + { + Console.WriteLine("原地旋转测试参数无效。"); + 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 rotationCenter = + new Vector2((float)location.x, (float)location.y); + var targetWorldAngle = + (float)AngleMath.NormalizeDegrees( + location.th + RelativeAngleDegrees); + + _recorder = new TrackingExperimentRecorder( + controllerName: "InPlaceRotatePID", + trajectoryName: _trajectoryName, + trialNumber: TrialNumber, + referenceStart: rotationCenter, + referenceEnd: rotationCenter, + referenceSpeed: 0f, + referenceAngularSpeed: + (float)AngleMath.DegreesToRadians( + MaxAngularSpeedDegreesPerSecond)); + _recorder.Start(); + + try + { + _task = new DriveTask( + new MultiWheelRotateInPlace + { + // MultiWheelRotateInPlace接收世界坐标系绝对航向。 + AngleTarget = targetWorldAngle, + PidparamsRead = () => new PIDParams + { + Kp = + PilotDefinition.Conf.InPlaceRotateKp, + Ki = + PilotDefinition.Conf.InPlaceRotateKi, + Kd = + PilotDefinition.Conf.InPlaceRotateKd, + DeadZone = + PilotDefinition.Conf + .InPlaceRotateArriveDeg, + SpeedAccPerSec = + PilotDefinition.Conf.InPlaceRotateAcc, + OutputUpperThreshold = + MaxAngularSpeedDegreesPerSecond, + MaxI = + PilotDefinition.Conf.InPlaceRotateMaxI + }, + CommandAngularSpeedObserver = + commandAngularSpeed => + _recorder?.UpdateCommand( + 0f, + (float)AngleMath.DegreesToRadians( + commandAngularSpeed)) + }.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 = "SendXYThSpeed：原地自转90°")] + public sealed class TestRotate90 : + InPlaceRotateTestBase + { + public TestRotate90() + : base(90f, "Rotate90") + { + } + } + + [MovementTest(name = "SendXYThSpeed：原地自转180°")] + public sealed class TestRotate180 : + InPlaceRotateTestBase + { + public TestRotate180() + : base(180f, "Rotate180") + { + } + } +} diff --git a/MultiWheelC/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs similarity index 100% rename from MultiWheelC/TrackingExperimentRecorder.cs rename to MultiWheelC/Experiments/TrackingExperimentRecorder.cs diff --git a/MultiWheelC/Experiments/WheelPreparationTests.cs b/MultiWheelC/Experiments/WheelPreparationTests.cs new file mode 100644 index 0000000..7444cfd --- /dev/null +++ b/MultiWheelC/Experiments/WheelPreparationTests.cs @@ -0,0 +1,103 @@ + + +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 +{ + internal static class MovementTestPreparation + { + // 在测试正式开始前，将四个舵轮稳定回正到车体前向。 + public static bool AlignWheelsForward( + ref DriveTask activeTask) + { + var preparation = new PrepareWheelsForward(); + var task = new DriveTask(preparation.Get()); + activeTask = task; + + try + { + task.Wait(); + return preparation.Completed; + } + catch (Exception ex) + { + Console.WriteLine( + $"测试前舵轮回正失败：{ex.Message}"); + return false; + } + finally + { + task.Stop(); + if (ReferenceEquals(activeTask, task)) + activeTask = null; + } + } + + // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。 + 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 + { + private DriveTask _task; + + // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。 + public override void Test() + { + MovementTestPreparation.AlignWheelsForward( + ref _task); + } + + // 停止正在执行的舵轮回正任务并清零底盘运动命令。 + public override void TestStop() + { + _task?.Stop(); + _task = null; + } + } +} diff --git a/MultiWheelC/Movements.cs b/MultiWheelC/Movements.cs deleted file mode 100644 index 09d756d..0000000 --- a/MultiWheelC/Movements.cs +++ /dev/null @@ -1,629 +0,0 @@ -using ClumsyCore; -using ClumsyCore.DTools; -using ClumsyCore.Interfaces; -using ClumsyCore.Pilot; -using CommonUsage.Chassis; -using MDCSToolBox.Clumsy.Movements; -using MDCSToolBox.Clumsy.Tracks; -using MDCSToolBox.Commons.Controllers; -using System; -using System.Collections.Generic; -using System.Drawing; -using System.Numerics; -using System.Threading; -using FundamentalLib; -using MyParking.Shared; - -namespace MultiWheelC -{ - // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。 - public class PrepareWheelsForward : MovementDefinition - { - public float ToleranceDegrees = 2f; - public float StableSeconds = 0.3f; - public float TimeoutSeconds = 10f; - public bool Completed { get; private set; } - - public override IEnumerable Get() - { - var chassis = - PilotDefinition.Chassis as MultiWheelChassis; - if (chassis == null) - { - throw new InvalidOperationException( - "当前底盘不是MultiWheelChassis，无法执行舵轮回正。"); - } - - var adapter = new MultiWheelChassisAdapter( - chassis, - PilotDefinition.Self.CarNum); - adapter.ResetToBodyFrame(); - var toleranceRadians = - AngleMath.DegreesToRadians(ToleranceDegrees); - var startTime = DateTime.UtcNow; - DateTime? alignedSince = null; - - Completed = false; - if (!adapter.PrepareParallelDirection(0.0)) - { - throw new InvalidOperationException( - "无法将所有舵轮下发到车体前向0°。"); - } - - try - { - while (true) - { - var aligned = - adapter.AreParallelWheelsAligned( - 0.0, - toleranceRadians); - - if (aligned) - { - if (!alignedSince.HasValue) - alignedSince = DateTime.UtcNow; - - if ((DateTime.UtcNow - - alignedSince.Value).TotalSeconds >= - StableSeconds) - { - Completed = true; - yield break; - } - } - else - { - alignedSince = null; - } - - if (TimeoutSeconds > 0f && - (DateTime.UtcNow - startTime).TotalSeconds > - TimeoutSeconds) - { - throw new TimeoutException( - $"舵轮回正超过{TimeoutSeconds:F1}s，" + - "测试已经取消。"); - } - - yield return true; - } - } - finally - { - // 只清零驱动速度，保留已经下发的0°舵角。 - adapter.StopImmediately(); - } - } - } - - #region 功能项 - 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; - } - } - #endregion - - #region 直线运动 - //在世界坐标系下，从路径起点追踪到终点并停车 - 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(); - } - } - - } - //直线行走基于轮里程 - // 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; - } - } - #endregion - - #region 旋转运动 - public class MultiWheelRotateInPlace : MovementDefinition - { - /// - /// 旋转目标角度 - /// - public float AngleTarget; - - public Func ThetaReader = () => (float)DetourInterface.getCartLocation().th; - - public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis; - - public Func PidparamsRead = () => new PIDParams() { }; - - public PIDController thPid; - - // 将本周期PID角速度输出提供给实验记录器，单位deg/s。 - public Action CommandAngularSpeedObserver; - - // 自转前舵轮实际角度允许误差，单位deg。 - public float WheelAlignmentToleranceDegrees = 2f; - - // 自转舵轮连续保持到位的时间，单位s。 - public float WheelAlignmentStableSeconds = 0.3f; - - // 自转舵轮准备超时时间，单位s。 - public float WheelAlignmentTimeoutSeconds = 10f; - - // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。 - public override IEnumerable Get() - { - if (Chassis == null) - throw new InvalidOperationException( - "当前底盘不是MultiWheelChassis，无法执行原地自转。"); - - var adapter = new MultiWheelChassisAdapter( - Chassis, - PilotDefinition.Self.CarNum); - adapter.ResetToBodyFrame(); - - try - { - var alignmentStarted = DateTime.Now; - DateTime? alignedSince = null; - while (true) - { - if (!adapter.PrepareSpin()) - throw new InvalidOperationException( - "无法生成原地自转舵轮目标：" + - adapter.LastFailureReason); - - if (adapter.AreSpinWheelsAligned) - { - if (alignedSince == null) - alignedSince = DateTime.Now; - - if ((DateTime.Now - alignedSince.Value) - .TotalSeconds >= - WheelAlignmentStableSeconds) - break; - } - else - { - alignedSince = null; - } - - if ((DateTime.Now - alignmentStarted) - .TotalSeconds > - WheelAlignmentTimeoutSeconds) - throw new TimeoutException( - "原地自转舵轮在限定时间内未稳定到位。"); - - yield return true; - } - - var targetAngle = - (float)AngleMath.NormalizeDegrees(AngleTarget); - var p = PidparamsRead(); - thPid = new PIDController(ThetaReader, p.Kp); - thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, - p.OutputUpperThreshold, p.SpeedAccPerSec); - var lastCommandTime = DateTime.Now; - - while (true) - { - var s = thPid.GetResponse(targetAngle, true); - Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}"); - CommandAngularSpeedObserver?.Invoke(s); - var now = DateTime.Now; - var interval = now - lastCommandTime; - lastCommandTime = now; - - // PID输出s为deg/s，Shared命令统一使用rad/s。 - // adapter.Send最终调用普通安全版SendXYThSpeed。 - var omegaRadiansPerSecond = - (float)AngleMath.DegreesToRadians(s); - if (!adapter.Send( - new ChassisCommand( - PilotDefinition.Self.CarNum, - new Twist2D( - 0.0, - 0.0, - omegaRadiansPerSecond)), - interval)) - { - throw new InvalidOperationException( - "安全XYTh原地旋转底盘解算失败：" + - adapter.LastFailureReason); - } - if (thPid.IsArrived()) break; - yield return true; - } - - Console.WriteLine($"final rotate to {targetAngle}"); - } - finally - { - CommandAngularSpeedObserver?.Invoke(0f); - adapter.StopImmediately(); - } - } - } - #endregion - - #region 夹臂运动 - 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; - } - } - } - #endregion -} diff --git a/MultiWheelC/Movements/ClampToTarget.cs b/MultiWheelC/Movements/ClampToTarget.cs new file mode 100644 index 0000000..d6e06bc --- /dev/null +++ b/MultiWheelC/Movements/ClampToTarget.cs @@ -0,0 +1,91 @@ +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/CrabMotionFrameTracker.cs b/MultiWheelC/Movements/CrabMotionFrameTracker.cs similarity index 99% rename from MultiWheelC/CrabMotionFrameTracker.cs rename to MultiWheelC/Movements/CrabMotionFrameTracker.cs index 2e998e3..6eaa1ce 100644 --- a/MultiWheelC/CrabMotionFrameTracker.cs +++ b/MultiWheelC/Movements/CrabMotionFrameTracker.cs @@ -794,8 +794,8 @@ namespace MultiWheelC Math.PI / 2.0 || !IsFinite( MaximumVirtualSteeringRadians)) - throw new ArgumentOutOfRangeException( - "蟹行轨迹测试参数无效。"); + throw new ArgumentOutOfRangeException( + "蟹行轨迹测试参数无效。"); } private static double Limit( diff --git a/MultiWheelC/Movements/DriverMovements.cs b/MultiWheelC/Movements/DriverMovements.cs new file mode 100644 index 0000000..841ef5b --- /dev/null +++ b/MultiWheelC/Movements/DriverMovements.cs @@ -0,0 +1,123 @@ +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 new file mode 100644 index 0000000..7630735 --- /dev/null +++ b/MultiWheelC/Movements/DstTracker.cs @@ -0,0 +1,55 @@ +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 new file mode 100644 index 0000000..f7ecae5 --- /dev/null +++ b/MultiWheelC/Movements/LineTracking.cs @@ -0,0 +1,178 @@ +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/PrepareWheelsForward.cs b/MultiWheelC/Movements/PrepareWheelsForward.cs new file mode 100644 index 0000000..b933fa3 --- /dev/null +++ b/MultiWheelC/Movements/PrepareWheelsForward.cs @@ -0,0 +1,92 @@ + + + +using System; +using System.Collections.Generic; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MyParking.Shared; + +namespace MultiWheelC +{ + // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。 + public class PrepareWheelsForward : MovementDefinition + { + public float ToleranceDegrees = 2f; + public float StableSeconds = 0.3f; + public float TimeoutSeconds = 10f; + public bool Completed { get; private set; } + + public override IEnumerable Get() + { + var chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (chassis == null) + { + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行舵轮回正。"); + } + + var adapter = new MultiWheelChassisAdapter( + chassis, + PilotDefinition.Self.CarNum); + adapter.ResetToBodyFrame(); + var toleranceRadians = + AngleMath.DegreesToRadians(ToleranceDegrees); + var startTime = DateTime.UtcNow; + DateTime? alignedSince = null; + + Completed = false; + if (!adapter.PrepareParallelDirection(0.0)) + { + throw new InvalidOperationException( + "无法将所有舵轮下发到车体前向0°。"); + } + + try + { + while (true) + { + var aligned = + adapter.AreParallelWheelsAligned( + 0.0, + toleranceRadians); + + if (aligned) + { + if (!alignedSince.HasValue) + alignedSince = DateTime.UtcNow; + + if ((DateTime.UtcNow - + alignedSince.Value).TotalSeconds >= + StableSeconds) + { + Completed = true; + yield break; + } + } + else + { + alignedSince = null; + } + + if (TimeoutSeconds > 0f && + (DateTime.UtcNow - startTime).TotalSeconds > + TimeoutSeconds) + { + throw new TimeoutException( + $"舵轮回正超过{TimeoutSeconds:F1}s，" + + "测试已经取消。"); + } + + yield return true; + } + } + finally + { + // 只清零驱动速度，保留已经下发的0°舵角。 + adapter.StopImmediately(); + } + } + } +} diff --git a/MultiWheelC/Movements/RotateInPlace.cs b/MultiWheelC/Movements/RotateInPlace.cs new file mode 100644 index 0000000..883c87f --- /dev/null +++ b/MultiWheelC/Movements/RotateInPlace.cs @@ -0,0 +1,134 @@ + + +using System; +using System.Collections.Generic; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MDCSToolBox.Commons.Controllers; +using MyParking.Shared; + +namespace MultiWheelC +{ + public class MultiWheelRotateInPlace : MovementDefinition + { + /// + /// 旋转目标角度 + /// + public float AngleTarget; + + public Func ThetaReader = () => (float)DetourInterface.getCartLocation().th; + + public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis; + + public Func PidparamsRead = () => new PIDParams() { }; + + public PIDController thPid; + + // 将本周期PID角速度输出提供给实验记录器，单位deg/s。 + public Action CommandAngularSpeedObserver; + + // 自转前舵轮实际角度允许误差，单位deg。 + public float WheelAlignmentToleranceDegrees = 2f; + + // 自转舵轮连续保持到位的时间，单位s。 + public float WheelAlignmentStableSeconds = 0.3f; + + // 自转舵轮准备超时时间，单位s。 + public float WheelAlignmentTimeoutSeconds = 10f; + + // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。 + public override IEnumerable Get() + { + if (Chassis == null) + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行原地自转。"); + + var adapter = new MultiWheelChassisAdapter( + Chassis, + PilotDefinition.Self.CarNum); + adapter.ResetToBodyFrame(); + + try + { + var alignmentStarted = DateTime.Now; + DateTime? alignedSince = null; + while (true) + { + if (!adapter.PrepareSpin()) + throw new InvalidOperationException( + "无法生成原地自转舵轮目标：" + + adapter.LastFailureReason); + + if (adapter.AreSpinWheelsAligned) + { + if (alignedSince == null) + alignedSince = DateTime.Now; + + if ((DateTime.Now - alignedSince.Value) + .TotalSeconds >= + WheelAlignmentStableSeconds) + break; + } + else + { + alignedSince = null; + } + + if ((DateTime.Now - alignmentStarted) + .TotalSeconds > + WheelAlignmentTimeoutSeconds) + throw new TimeoutException( + "原地自转舵轮在限定时间内未稳定到位。"); + + yield return true; + } + + var targetAngle = + (float)AngleMath.NormalizeDegrees(AngleTarget); + var p = PidparamsRead(); + thPid = new PIDController(ThetaReader, p.Kp); + thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, + p.OutputUpperThreshold, p.SpeedAccPerSec); + var lastCommandTime = DateTime.Now; + + while (true) + { + var s = thPid.GetResponse(targetAngle, true); + Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}"); + CommandAngularSpeedObserver?.Invoke(s); + var now = DateTime.Now; + var interval = now - lastCommandTime; + lastCommandTime = now; + + // PID输出s为deg/s，Shared命令统一使用rad/s。 + // adapter.Send最终调用普通安全版SendXYThSpeed。 + var omegaRadiansPerSecond = + (float)AngleMath.DegreesToRadians(s); + if (!adapter.Send( + new ChassisCommand( + PilotDefinition.Self.CarNum, + new Twist2D( + 0.0, + 0.0, + omegaRadiansPerSecond)), + interval)) + { + throw new InvalidOperationException( + "安全XYTh原地旋转底盘解算失败：" + + adapter.LastFailureReason); + } + if (thPid.IsArrived()) break; + yield return true; + } + + Console.WriteLine($"final rotate to {targetAngle}"); + } + finally + { + CommandAngularSpeedObserver?.Invoke(0f); + adapter.StopImmediately(); + } + } + } +} diff --git a/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs new file mode 100644 index 0000000..c94683f --- /dev/null +++ b/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs @@ -0,0 +1,509 @@ +using System; +using System.Diagnostics; +using ClumsyCore.Interfaces; +using MyParking.Shared; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。 + /// + public sealed class DetourVehicleStateProvider + : IVehicleStateProvider + { + public const double DefaultMaximumLinearSpeedMetersPerSecond = + 1.20; + public const double DefaultMaximumAngularSpeedRadiansPerSecond = + Math.PI / 4.0; + public const double DefaultPositionJumpMarginMeters = + 0.03; + public const double DefaultHeadingJumpMarginRadians = + 5.0 * Math.PI / 180.0; + public const double DefaultVelocityPositionResidualMeters = + 0.04; + public const double DefaultVelocityHeadingResidualRadians = + 5.0 * Math.PI / 180.0; + public const double DefaultStationaryConfirmationSeconds = + 0.35; + + private const double MillimetersPerMeter = 1000.0; + private const double PositionEqualityToleranceMeters = 1e-9; + private const double HeadingEqualityToleranceRadians = 1e-8; + + private readonly object _syncRoot = new object(); + private readonly Stopwatch _clock = Stopwatch.StartNew(); + private readonly VelocityEstimator2D _velocityEstimator; + private readonly double _maximumLinearSpeedMetersPerSecond; + private readonly double _maximumAngularSpeedRadiansPerSecond; + private readonly double _positionJumpMarginMeters; + private readonly double _headingJumpMarginRadians; + private readonly double _velocityPositionResidualMeters; + private readonly double _velocityHeadingResidualRadians; + private readonly double _stationaryConfirmationSeconds; + + private bool _hasAcceptedPose; + private Pose2D _acceptedPoseInWorld; + private double _acceptedTimestampSeconds; + private VehicleState _latestState; + private bool _stationaryHoldActive; + + /// + /// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。 + /// + public DetourVehicleStateProvider() + : this( + new VelocityEstimator2D(), + DefaultMaximumLinearSpeedMetersPerSecond, + DefaultMaximumAngularSpeedRadiansPerSecond, + DefaultPositionJumpMarginMeters, + DefaultHeadingJumpMarginRadians, + DefaultVelocityPositionResidualMeters, + DefaultVelocityHeadingResidualRadians, + DefaultStationaryConfirmationSeconds) + { + } + + /// + /// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。 + /// + public DetourVehicleStateProvider( + VelocityEstimator2D velocityEstimator, + double maximumLinearSpeedMetersPerSecond, + double maximumAngularSpeedRadiansPerSecond, + double positionJumpMarginMeters, + double headingJumpMarginRadians, + double velocityPositionResidualMeters, + double velocityHeadingResidualRadians, + double stationaryConfirmationSeconds) + { + _velocityEstimator = velocityEstimator ?? + throw new ArgumentNullException( + nameof(velocityEstimator)); + + EnsureFinitePositive( + maximumLinearSpeedMetersPerSecond, + nameof(maximumLinearSpeedMetersPerSecond)); + EnsureFinitePositive( + maximumAngularSpeedRadiansPerSecond, + nameof(maximumAngularSpeedRadiansPerSecond)); + EnsureFiniteNonNegative( + positionJumpMarginMeters, + nameof(positionJumpMarginMeters)); + EnsureFiniteNonNegative( + headingJumpMarginRadians, + nameof(headingJumpMarginRadians)); + EnsureFinitePositive( + velocityPositionResidualMeters, + nameof(velocityPositionResidualMeters)); + EnsureFinitePositive( + velocityHeadingResidualRadians, + nameof(velocityHeadingResidualRadians)); + EnsureFinitePositive( + stationaryConfirmationSeconds, + nameof(stationaryConfirmationSeconds)); + + _maximumLinearSpeedMetersPerSecond = + maximumLinearSpeedMetersPerSecond; + _maximumAngularSpeedRadiansPerSecond = + maximumAngularSpeedRadiansPerSecond; + _positionJumpMarginMeters = + positionJumpMarginMeters; + _headingJumpMarginRadians = + headingJumpMarginRadians; + _velocityPositionResidualMeters = + velocityPositionResidualMeters; + _velocityHeadingResidualRadians = + velocityHeadingResidualRadians; + _stationaryConfirmationSeconds = + stationaryConfirmationSeconds; + } + + /// + /// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。 + /// + public string LastFailureReason { get; private set; } = ""; + + /// + /// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。 + /// + public bool TryGetState(out VehicleState state) + { + lock (_syncRoot) + { + try + { + var poseInWorld = + ReadDetourPoseInWorld(); + var timestampSeconds = + _clock.Elapsed.TotalSeconds; + + if (!_hasAcceptedPose) + { + state = AcceptPoseAfterReset( + poseInWorld, + timestampSeconds); + LastFailureReason = ""; + return true; + } + + if (ArePosesEquivalent( + poseInWorld, + _acceptedPoseInWorld)) + { + state = HandleRepeatedPose( + timestampSeconds); + LastFailureReason = ""; + return true; + } + + // 静止保持后出现新定位时重新建立差分基准， + // 避免用很长的静止时间稀释第一次运动速度。 + if (_stationaryHoldActive) + { + state = AcceptPoseAfterReset( + poseInWorld, + timestampSeconds); + LastFailureReason = ""; + return true; + } + + var elapsedSeconds = + timestampSeconds - + _acceptedTimestampSeconds; + + if (!IsMotionPlausible( + _acceptedPoseInWorld, + poseInWorld, + elapsedSeconds)) + { + // 单帧异常不进入差分器，也不中断调用方；下一次 + // 正常观测仍相对最近有效位姿和真实时间差计算。 + state = _latestState; + LastFailureReason = + "Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。"; + return true; + } + + if (IsVelocityInnovationAbnormal( + poseInWorld, + elapsedSeconds)) + { + state = AcceptPoseAfterVelocityRebase( + poseInWorld, + timestampSeconds); + LastFailureReason = + "Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。"; + return true; + } + + state = AcceptContinuousPose( + poseInWorld, + timestampSeconds); + LastFailureReason = ""; + return true; + } + catch (Exception exception) + { + state = default; + LastFailureReason = + "Detour车辆状态读取失败：" + + exception.Message; + return false; + } + } + } + + /// + /// 清除Detour位姿历史和速度估计状态。 + /// + public void Reset() + { + lock (_syncRoot) + { + _velocityEstimator.Reset(); + _hasAcceptedPose = false; + _acceptedPoseInWorld = Pose2D.Identity; + _acceptedTimestampSeconds = 0.0; + _latestState = default; + _stationaryHoldActive = false; + LastFailureReason = ""; + } + } + + /// + /// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。 + /// + private static Pose2D ReadDetourPoseInWorld() + { + var location = + DetourInterface.getCartLocation(); + + EnsureFinite(location.x, "DetourX"); + EnsureFinite(location.y, "DetourY"); + EnsureFinite(location.th, "DetourTheta"); + + return new Pose2D( + location.x / MillimetersPerMeter, + location.y / MillimetersPerMeter, + AngleMath.NormalizeRadians( + AngleMath.DegreesToRadians( + location.th))); + } + + /// + /// 接受连续有效定位并更新速度估计和差分基准。 + /// + private VehicleState AcceptContinuousPose( + Pose2D poseInWorld, + double timestampSeconds) + { + _latestState = + _velocityEstimator.Update( + poseInWorld, + timestampSeconds); + _acceptedPoseInWorld = poseInWorld; + _acceptedTimestampSeconds = + timestampSeconds; + _stationaryHoldActive = false; + return _latestState; + } + + /// + /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。 + /// + private VehicleState AcceptPoseAfterVelocityRebase( + Pose2D poseInWorld, + double timestampSeconds) + { + _latestState = + _velocityEstimator + .RebasePreservingVelocity( + poseInWorld, + timestampSeconds); + _acceptedPoseInWorld = poseInWorld; + _acceptedTimestampSeconds = + timestampSeconds; + _stationaryHoldActive = false; + return _latestState; + } + + /// + /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。 + /// + private VehicleState AcceptPoseAfterReset( + Pose2D poseInWorld, + double timestampSeconds) + { + _latestState = + _velocityEstimator.Reset( + poseInWorld, + timestampSeconds); + _acceptedPoseInWorld = poseInWorld; + _acceptedTimestampSeconds = + timestampSeconds; + _hasAcceptedPose = true; + _stationaryHoldActive = false; + return _latestState; + } + + /// + /// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。 + /// + private VehicleState HandleRepeatedPose( + double timestampSeconds) + { + var unchangedSeconds = + timestampSeconds - + _acceptedTimestampSeconds; + + if (!_stationaryHoldActive && + unchangedSeconds >= + _stationaryConfirmationSeconds) + { + _latestState = + new VehicleState( + timestampSeconds, + _acceptedPoseInWorld, + Twist2D.Zero, + true); + _stationaryHoldActive = true; + } + + return _latestState; + } + + /// + /// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。 + /// + private bool IsMotionPlausible( + Pose2D startPoseInWorld, + Pose2D endPoseInWorld, + double deltaTimeSeconds) + { + if (!IsFinite(deltaTimeSeconds) || + deltaTimeSeconds <= 0.0) + { + return false; + } + + var deltaX = + endPoseInWorld.XMeters - + startPoseInWorld.XMeters; + var deltaY = + endPoseInWorld.YMeters - + startPoseInWorld.YMeters; + var displacementMeters = + Math.Sqrt( + deltaX * deltaX + + deltaY * deltaY); + var headingChangeRadians = + Math.Abs( + AngleMath.ShortestDifferenceRadians( + endPoseInWorld.YawRadians, + startPoseInWorld.YawRadians)); + + var maximumDisplacementMeters = + _maximumLinearSpeedMetersPerSecond * + deltaTimeSeconds + + _positionJumpMarginMeters; + var maximumHeadingChangeRadians = + _maximumAngularSpeedRadiansPerSecond * + deltaTimeSeconds + + _headingJumpMarginRadians; + + return displacementMeters <= + maximumDisplacementMeters && + headingChangeRadians <= + maximumHeadingChangeRadians; + } + + /// + /// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。 + /// + private bool IsVelocityInnovationAbnormal( + Pose2D poseInWorld, + double deltaTimeSeconds) + { + if (!_latestState.HasValidVelocityEstimate) + { + return false; + } + + var predictedX = + _acceptedPoseInWorld.XMeters + + _latestState.TwistInWorld + .VxMetersPerSecond * + deltaTimeSeconds; + var predictedY = + _acceptedPoseInWorld.YMeters + + _latestState.TwistInWorld + .VyMetersPerSecond * + deltaTimeSeconds; + var predictedYaw = + AngleMath.NormalizeRadians( + _acceptedPoseInWorld.YawRadians + + _latestState.TwistInWorld + .OmegaRadiansPerSecond * + deltaTimeSeconds); + + var positionResidualX = + poseInWorld.XMeters - predictedX; + var positionResidualY = + poseInWorld.YMeters - predictedY; + var positionResidualMeters = + Math.Sqrt( + positionResidualX * positionResidualX + + positionResidualY * positionResidualY); + var headingResidualRadians = + Math.Abs( + AngleMath.ShortestDifferenceRadians( + poseInWorld.YawRadians, + predictedYaw)); + + return positionResidualMeters > + _velocityPositionResidualMeters || + headingResidualRadians > + _velocityHeadingResidualRadians; + } + + /// + /// 判断两次读取是否为Detour保持输出的同一数值帧。 + /// + private static bool ArePosesEquivalent( + Pose2D firstPose, + Pose2D secondPose) + { + return Math.Abs( + firstPose.XMeters - + secondPose.XMeters) <= + PositionEqualityToleranceMeters && + Math.Abs( + firstPose.YMeters - + secondPose.YMeters) <= + PositionEqualityToleranceMeters && + Math.Abs( + AngleMath.ShortestDifferenceRadians( + firstPose.YawRadians, + secondPose.YawRadians)) <= + HeadingEqualityToleranceRadians; + } + + /// + /// 检查数值是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "状态源参数必须是正有限值。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "状态源参数必须是非负有限值。"); + } + } + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (!IsFinite(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "状态源参数和Detour位姿必须是有限值。"); + } + } + + /// + /// 判断数值是否可用于状态估计。 + /// + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && + !double.IsInfinity(value); + } + } +} diff --git a/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs b/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs new file mode 100644 index 0000000..29464bc --- /dev/null +++ b/MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs @@ -0,0 +1,142 @@ +using System; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。 + /// + public sealed class FirstOrderLowPassFilter + { + private readonly double _timeConstantSeconds; + private bool _isInitialized; + private double _value; + + /// + /// 创建使用指定时间常数的一阶低通滤波器。 + /// + public FirstOrderLowPassFilter( + double timeConstantSeconds) + { + EnsureFinitePositive( + timeConstantSeconds, + nameof(timeConstantSeconds)); + + _timeConstantSeconds = + timeConstantSeconds; + } + + /// + /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。 + /// + public double TimeConstantSeconds => + _timeConstantSeconds; + + /// + /// 获取滤波器是否已经接收过有效初值。 + /// + public bool IsInitialized => + _isInitialized; + + /// + /// 获取当前滤波输出；尚未初始化时读取会抛出异常。 + /// + public double Value + { + get + { + if (!_isInitialized) + { + throw new InvalidOperationException( + "一阶低通滤波器尚未初始化。"); + } + + return _value; + } + } + + /// + /// 使用当前输入和真实采样间隔更新滤波结果。 + /// + public double Update( + double input, + double deltaTimeSeconds) + { + EnsureFinite( + input, + nameof(input)); + EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + if (!_isInitialized) + { + _value = input; + _isInitialized = true; + return _value; + } + + var alpha = + deltaTimeSeconds / + (_timeConstantSeconds + + deltaTimeSeconds); + + _value += alpha * (input - _value); + return _value; + } + + /// + /// 清除历史输出，使下一次有效输入直接成为新的初值。 + /// + public void Reset() + { + _value = 0.0; + _isInitialized = false; + } + + /// + /// 将滤波器立即重置到指定的有限初值。 + /// + public void Reset(double initialValue) + { + EnsureFinite( + initialValue, + nameof(initialValue)); + + _value = initialValue; + _isInitialized = true; + } + + /// + /// 检查数值是否为正有限值。 + /// + private static void EnsureFinitePositive( + double value, + string parameterName) + { + EnsureFinite(value, parameterName); + + if (value <= 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "滤波时间常数和采样间隔必须是正有限值。"); + } + } + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "滤波输入必须是有限值。"); + } + } + } +} diff --git a/MultiWheelC/StateEstimation/IVehicleStateProvider.cs b/MultiWheelC/StateEstimation/IVehicleStateProvider.cs new file mode 100644 index 0000000..fcb25d8 --- /dev/null +++ b/MultiWheelC/StateEstimation/IVehicleStateProvider.cs @@ -0,0 +1,14 @@ +namespace MultiWheelC.StateEstimation +{ + /// + /// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。 + /// + public interface IVehicleStateProvider + { + /// + /// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。 + /// + bool TryGetState(out VehicleState state); + } +} + diff --git a/MultiWheelC/StateEstimation/VehicleState.cs b/MultiWheelC/StateEstimation/VehicleState.cs new file mode 100644 index 0000000..af4814c --- /dev/null +++ b/MultiWheelC/StateEstimation/VehicleState.cs @@ -0,0 +1,138 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。 + /// + public readonly struct VehicleState + { + /// + /// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。 + /// + public VehicleState( + double sampleTimestampSeconds, + Pose2D poseInWorld, + Twist2D twistInWorld, + bool hasValidVelocityEstimate) + { + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteTwist( + twistInWorld, + nameof(twistInWorld)); + + SampleTimestampSeconds = + sampleTimestampSeconds; + PoseInWorld = new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + HasValidVelocityEstimate = + hasValidVelocityEstimate; + + // 第一帧或定位重置后的速度不可用于闭环控制， + // 此时显式置零，避免调用方误用残留速度。 + TwistInWorld = hasValidVelocityEstimate + ? twistInWorld + : Twist2D.Zero; + + var worldPoseInBody = + FrameTransform2D.Inverse( + PoseInWorld); + TwistInBody = + FrameTransform2D.TransformTwistAtSamePoint( + worldPoseInBody, + TwistInWorld); + } + + /// + /// 获取状态源单调时钟中的采样时刻，单位为s。 + /// + public double SampleTimestampSeconds { get; } + + /// + /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。 + /// + public Pose2D PoseInWorld { get; } + + /// + /// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。 + /// + public Twist2D TwistInWorld { get; } + + /// + /// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。 + /// + public Twist2D TwistInBody { get; } + + /// + /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。 + /// + public bool HasValidVelocityEstimate { get; } + + /// + /// 检查位姿是否由有限数值组成。 + /// + private static void EnsureFinitePose( + Pose2D pose, + string parameterName) + { + if (!IsFinite(pose.XMeters) || + !IsFinite(pose.YMeters) || + !IsFinite(pose.YawRadians)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "车辆位姿必须由有限数值组成。"); + } + } + + /// + /// 检查速度是否由有限数值组成。 + /// + private static void EnsureFiniteTwist( + Twist2D twist, + string parameterName) + { + if (!IsFinite(twist.VxMetersPerSecond) || + !IsFinite(twist.VyMetersPerSecond) || + !IsFinite(twist.OmegaRadiansPerSecond)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "车辆速度必须由有限数值组成。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + if (!IsFinite(value) || value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "采样时刻必须是非负有限值。"); + } + } + + /// + /// 判断数值是否可用于车辆状态计算。 + /// + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && + !double.IsInfinity(value); + } + } +} diff --git a/MultiWheelC/StateEstimation/VelocityEstimator2D.cs b/MultiWheelC/StateEstimation/VelocityEstimator2D.cs new file mode 100644 index 0000000..3d8a85d --- /dev/null +++ b/MultiWheelC/StateEstimation/VelocityEstimator2D.cs @@ -0,0 +1,275 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.StateEstimation +{ + /// + /// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。 + /// + public sealed class VelocityEstimator2D + { + public const double DefaultLinearFilterTimeConstantSeconds = + 0.15; + public const double DefaultAngularFilterTimeConstantSeconds = + 0.20; + + private readonly FirstOrderLowPassFilter + _worldVelocityXFilter; + private readonly FirstOrderLowPassFilter + _worldVelocityYFilter; + private readonly FirstOrderLowPassFilter + _angularVelocityFilter; + + private bool _hasPreviousSample; + private Pose2D _previousPoseInWorld; + private double _previousTimestampSeconds; + + /// + /// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。 + /// + public VelocityEstimator2D() + : this( + DefaultLinearFilterTimeConstantSeconds, + DefaultAngularFilterTimeConstantSeconds) + { + } + + /// + /// 创建使用指定线速度和角速度滤波时间常数的估计器。 + /// + public VelocityEstimator2D( + double linearFilterTimeConstantSeconds, + double angularFilterTimeConstantSeconds) + { + _worldVelocityXFilter = + new FirstOrderLowPassFilter( + linearFilterTimeConstantSeconds); + _worldVelocityYFilter = + new FirstOrderLowPassFilter( + linearFilterTimeConstantSeconds); + _angularVelocityFilter = + new FirstOrderLowPassFilter( + angularFilterTimeConstantSeconds); + } + + /// + /// 获取是否已经保存了可用于下一次差分的位姿基准。 + /// + public bool HasPreviousSample => + _hasPreviousSample; + + /// + /// 使用一个新的有效定位样本更新并返回车辆状态。 + /// + public VehicleState Update( + Pose2D poseInWorld, + double sampleTimestampSeconds) + { + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + + var normalizedPoseInWorld = + new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + + if (!_hasPreviousSample) + { + return Reset( + normalizedPoseInWorld, + sampleTimestampSeconds); + } + + var deltaTimeSeconds = + sampleTimestampSeconds - + _previousTimestampSeconds; + + if (deltaTimeSeconds <= 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(sampleTimestampSeconds), + "新定位样本的单调时间戳必须严格大于上一帧。"); + } + + var rawVelocityXInWorld = + (normalizedPoseInWorld.XMeters - + _previousPoseInWorld.XMeters) / + deltaTimeSeconds; + var rawVelocityYInWorld = + (normalizedPoseInWorld.YMeters - + _previousPoseInWorld.YMeters) / + deltaTimeSeconds; + var rawAngularVelocity = + AngleMath.ShortestDifferenceRadians( + normalizedPoseInWorld.YawRadians, + _previousPoseInWorld.YawRadians) / + deltaTimeSeconds; + + var filteredTwistInWorld = + new Twist2D( + _worldVelocityXFilter.Update( + rawVelocityXInWorld, + deltaTimeSeconds), + _worldVelocityYFilter.Update( + rawVelocityYInWorld, + deltaTimeSeconds), + _angularVelocityFilter.Update( + rawAngularVelocity, + deltaTimeSeconds)); + + _previousPoseInWorld = + normalizedPoseInWorld; + _previousTimestampSeconds = + sampleTimestampSeconds; + + return new VehicleState( + sampleTimestampSeconds, + normalizedPoseInWorld, + filteredTwistInWorld, + true); + } + + /// + /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。 + /// + public VehicleState RebasePreservingVelocity( + Pose2D poseInWorld, + double sampleTimestampSeconds) + { + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + + var normalizedPoseInWorld = + new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + + _previousPoseInWorld = + normalizedPoseInWorld; + _previousTimestampSeconds = + sampleTimestampSeconds; + _hasPreviousSample = true; + + var hasValidVelocityEstimate = + _worldVelocityXFilter.IsInitialized && + _worldVelocityYFilter.IsInitialized && + _angularVelocityFilter.IsInitialized; + + var retainedTwistInWorld = + hasValidVelocityEstimate + ? new Twist2D( + _worldVelocityXFilter.Value, + _worldVelocityYFilter.Value, + _angularVelocityFilter.Value) + : Twist2D.Zero; + + return new VehicleState( + sampleTimestampSeconds, + normalizedPoseInWorld, + retainedTwistInWorld, + hasValidVelocityEstimate); + } + + /// + /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。 + /// + public VehicleState Reset( + Pose2D poseInWorld, + double sampleTimestampSeconds) + { + EnsureFinitePose( + poseInWorld, + nameof(poseInWorld)); + EnsureFiniteNonNegative( + sampleTimestampSeconds, + nameof(sampleTimestampSeconds)); + + _previousPoseInWorld = + new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + _previousTimestampSeconds = + sampleTimestampSeconds; + _hasPreviousSample = true; + + _worldVelocityXFilter.Reset(); + _worldVelocityYFilter.Reset(); + _angularVelocityFilter.Reset(); + + return new VehicleState( + sampleTimestampSeconds, + _previousPoseInWorld, + Twist2D.Zero, + false); + } + + /// + /// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。 + /// + public void Reset() + { + _hasPreviousSample = false; + _previousPoseInWorld = Pose2D.Identity; + _previousTimestampSeconds = 0.0; + + _worldVelocityXFilter.Reset(); + _worldVelocityYFilter.Reset(); + _angularVelocityFilter.Reset(); + } + + /// + /// 检查位姿是否由有限数值组成。 + /// + private static void EnsureFinitePose( + Pose2D pose, + string parameterName) + { + if (!IsFinite(pose.XMeters) || + !IsFinite(pose.YMeters) || + !IsFinite(pose.YawRadians)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "速度估计使用的车辆位姿必须由有限数值组成。"); + } + } + + /// + /// 检查数值是否为非负有限值。 + /// + private static void EnsureFiniteNonNegative( + double value, + string parameterName) + { + if (!IsFinite(value) || value < 0.0) + { + throw new ArgumentOutOfRangeException( + parameterName, + "速度估计使用的采样时刻必须是非负有限值。"); + } + } + + /// + /// 判断数值是否可用于速度估计。 + /// + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && + !double.IsInfinity(value); + } + } +} diff --git a/MultiWheelC/Trajectory/LegacyTrackAdapter.cs b/MultiWheelC/Trajectory/LegacyTrackAdapter.cs new file mode 100644 index 0000000..d8a2ebb --- /dev/null +++ b/MultiWheelC/Trajectory/LegacyTrackAdapter.cs @@ -0,0 +1 @@ +// 兼容现有 MDCS 的 AbstractTrack \ No newline at end of file diff --git a/MultiWheelC/Trajectory/Trajectory2D.cs b/MultiWheelC/Trajectory/Trajectory2D.cs new file mode 100644 index 0000000..c4d6fe0 --- /dev/null +++ b/MultiWheelC/Trajectory/Trajectory2D.cs @@ -0,0 +1,172 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using System.Linq; + +namespace MultiWheelC.Trajectory +{ + /// + /// 保存一条经过基本合法性检查的只读二维参考轨迹。 + /// + public sealed class Trajectory2D + { + private const double StartArcLengthToleranceMeters = 1e-9; + private const double MinimumSegmentLengthMeters = 1e-6; + + private readonly TrajectoryPoint[] _points; + private readonly ReadOnlyCollection _readOnlyPoints; + + /// + /// 复制并验证按累计弧长升序排列的参考轨迹点。 + /// + public Trajectory2D( + IEnumerable points) + { + if (points == null) + { + throw new ArgumentNullException( + nameof(points)); + } + + _points = points.ToArray(); + + if (_points.Length < 2) + { + throw new ArgumentException( + "二维轨迹至少需要两个轨迹点。", + nameof(points)); + } + + if (Math.Abs(_points[0].ArcLengthMeters) > + StartArcLengthToleranceMeters) + { + throw new ArgumentException( + "二维轨迹起点的累计弧长必须为0m。", + nameof(points)); + } + + for (var index = 1; + index < _points.Length; + index++) + { + ValidateSegment( + _points[index - 1], + _points[index], + index, + nameof(points)); + } + + _readOnlyPoints = + Array.AsReadOnly(_points); + } + + /// + /// 获取轨迹点数量。 + /// + public int Count => _points.Length; + + /// + /// 获取指定索引处的轨迹点。 + /// + public TrajectoryPoint this[int index] => + _points[index]; + + /// + /// 获取不可修改的有序轨迹点集合。 + /// + public IReadOnlyList Points => + _readOnlyPoints; + + /// + /// 获取轨迹起点。 + /// + public TrajectoryPoint StartPoint => + _points[0]; + + /// + /// 获取轨迹终点。 + /// + public TrajectoryPoint EndPoint => + _points[_points.Length - 1]; + + /// + /// 获取轨迹总弧长，单位为m。 + /// + public double TotalLengthMeters => + EndPoint.ArcLengthMeters; + + /// + /// 根据当前累计弧长计算到轨迹终点的剩余距离。 + /// + public double GetRemainingDistanceMeters( + double arcLengthMeters) + { + EnsureFinite( + arcLengthMeters, + nameof(arcLengthMeters)); + + if (arcLengthMeters <= 0.0) + return TotalLengthMeters; + + if (arcLengthMeters >= TotalLengthMeters) + return 0.0; + + return TotalLengthMeters - arcLengthMeters; + } + + /// + /// 检查相邻轨迹点是否构成有效的非零长度有序线段。 + /// + private static void ValidateSegment( + TrajectoryPoint previous, + TrajectoryPoint current, + int currentIndex, + string parameterName) + { + if (current.ArcLengthMeters <= + previous.ArcLengthMeters) + { + throw new ArgumentException( + $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。", + parameterName); + } + + var deltaX = + current.PoseInWorld.XMeters - + previous.PoseInWorld.XMeters; + var deltaY = + current.PoseInWorld.YMeters - + previous.PoseInWorld.YMeters; + var segmentLengthSquared = + deltaX * deltaX + + deltaY * deltaY; + var minimumLengthSquared = + MinimumSegmentLengthMeters * + MinimumSegmentLengthMeters; + + if (segmentLengthSquared < + minimumLengthSquared) + { + throw new ArgumentException( + $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。", + parameterName); + } + } + + /// + /// 检查数值是否为有限值。 + /// + private static void EnsureFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "轨迹弧长必须是有限值。"); + } + } + } +} diff --git a/MultiWheelC/Trajectory/TrajectoryPoint.cs b/MultiWheelC/Trajectory/TrajectoryPoint.cs new file mode 100644 index 0000000..83dd56c --- /dev/null +++ b/MultiWheelC/Trajectory/TrajectoryPoint.cs @@ -0,0 +1,102 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。 + /// + public readonly struct TrajectoryPoint + { + /// + /// 创建包含中心位姿、曲率和速度信息的参考轨迹点。 + /// + public TrajectoryPoint( + double arcLengthMeters, + Pose2D poseInWorld, + double curvaturePerMeter, + double referenceSpeedMetersPerSecond) + { + EnsureFiniteNonNegative( + arcLengthMeters, + nameof(arcLengthMeters)); + EnsureFinite( + poseInWorld.XMeters, + nameof(poseInWorld)); + EnsureFinite( + poseInWorld.YMeters, + nameof(poseInWorld)); + EnsureFinite( + poseInWorld.YawRadians, + nameof(poseInWorld)); + EnsureFinite( + curvaturePerMeter, + nameof(curvaturePerMeter)); + EnsureFinite( + referenceSpeedMetersPerSecond, + nameof(referenceSpeedMetersPerSecond)); + ArcLengthMeters = arcLengthMeters; + PoseInWorld = new Pose2D( + poseInWorld.XMeters, + poseInWorld.YMeters, + AngleMath.NormalizeRadians( + poseInWorld.YawRadians)); + CurvaturePerMeter = curvaturePerMeter; + ReferenceSpeedMetersPerSecond = + referenceSpeedMetersPerSecond; + } + + /// + /// 获取从轨迹起点累计到当前点的弧长，单位为m。 + /// + public double ArcLengthMeters { get; } + + /// + /// 获取车体中心参考坐标系在世界坐标系中的位姿。 + /// + public Pose2D PoseInWorld { get; } + + /// + /// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。 + /// + public double CurvaturePerMeter { get; } + + /// + /// 获取沿轨迹切线方向的有符号参考速度，单位为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 new file mode 100644 index 0000000..798ae17 --- /dev/null +++ b/MultiWheelC/Trajectory/TrajectoryProjection.cs @@ -0,0 +1,123 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 保存车体中心投影到二维参考轨迹后得到的只读结果。 + /// + public readonly struct TrajectoryProjection + { + /// + /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。 + /// + public TrajectoryProjection( + int segmentStartIndex, + TrajectoryPoint referencePoint, + double lateralErrorMeters, + double headingErrorRadians, + double distanceToTrajectoryMeters, + double remainingDistanceMeters) + { + if (segmentStartIndex < 0) + { + throw new ArgumentOutOfRangeException( + nameof(segmentStartIndex), + "投影线段起点索引不能为负数。"); + } + + EnsureFinite( + lateralErrorMeters, + nameof(lateralErrorMeters)); + EnsureFinite( + headingErrorRadians, + nameof(headingErrorRadians)); + EnsureFiniteNonNegative( + distanceToTrajectoryMeters, + nameof(distanceToTrajectoryMeters)); + EnsureFiniteNonNegative( + remainingDistanceMeters, + nameof(remainingDistanceMeters)); + + SegmentStartIndex = segmentStartIndex; + ReferencePoint = referencePoint; + LateralErrorMeters = lateralErrorMeters; + HeadingErrorRadians = + AngleMath.NormalizeRadians( + headingErrorRadians); + DistanceToTrajectoryMeters = + distanceToTrajectoryMeters; + RemainingDistanceMeters = + remainingDistanceMeters; + } + + /// + /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。 + /// + public int SegmentStartIndex { get; } + + /// + /// 获取投影位置插值得到的车体中心参考轨迹点。 + /// + public TrajectoryPoint ReferencePoint { get; } + + /// + /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。 + /// + public double LateralErrorMeters { get; } + + /// + /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。 + /// + public double HeadingErrorRadians { get; } + + /// + /// 获取车体中心到投影点的欧氏距离，单位为m。 + /// + public double DistanceToTrajectoryMeters { get; } + + /// + /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。 + /// + public double RemainingDistanceMeters { get; } + + /// + /// 获取投影位置从轨迹起点累计的弧长，单位为m。 + /// + 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 new file mode 100644 index 0000000..43cc3c5 --- /dev/null +++ b/MultiWheelC/Trajectory/TrajectoryProjector.cs @@ -0,0 +1,219 @@ +using System; +using MyParking.Shared; + +namespace MultiWheelC.Trajectory +{ + /// + /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。 + /// + public static class TrajectoryProjector + { + /// + /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。 + /// + public static TrajectoryProjection Project( + Trajectory2D trajectory, + Pose2D vehiclePoseInWorld) + { + if (trajectory == null) + { + throw new ArgumentNullException( + nameof(trajectory)); + } + + EnsureFinitePose( + vehiclePoseInWorld, + nameof(vehiclePoseInWorld)); + + var bestSegmentStartIndex = 0; + var bestInterpolationRatio = 0.0; + var bestProjectedX = 0.0; + var bestProjectedY = 0.0; + var bestDistanceSquared = + double.PositiveInfinity; + + for (var segmentStartIndex = 0; + segmentStartIndex < trajectory.Count - 1; + segmentStartIndex++) + { + var segmentStart = + trajectory[segmentStartIndex]; + var segmentEnd = + trajectory[segmentStartIndex + 1]; + + var segmentX = + segmentEnd.PoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters; + var segmentY = + segmentEnd.PoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters; + var segmentLengthSquared = + segmentX * segmentX + + segmentY * segmentY; + + var vehicleFromSegmentStartX = + vehiclePoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters; + var vehicleFromSegmentStartY = + vehiclePoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters; + + var interpolationRatio = + InterpolationMath.Clamp01( + (vehicleFromSegmentStartX * segmentX + + vehicleFromSegmentStartY * segmentY) / + segmentLengthSquared); + + var projectedX = + InterpolationMath.Lerp( + segmentStart.PoseInWorld.XMeters, + segmentEnd.PoseInWorld.XMeters, + interpolationRatio); + var projectedY = + InterpolationMath.Lerp( + segmentStart.PoseInWorld.YMeters, + segmentEnd.PoseInWorld.YMeters, + interpolationRatio); + + var projectionErrorX = + projectedX - + vehiclePoseInWorld.XMeters; + var projectionErrorY = + projectedY - + vehiclePoseInWorld.YMeters; + var distanceSquared = + projectionErrorX * projectionErrorX + + projectionErrorY * projectionErrorY; + + if (distanceSquared >= bestDistanceSquared) + { + continue; + } + + bestSegmentStartIndex = + segmentStartIndex; + bestInterpolationRatio = + interpolationRatio; + bestProjectedX = projectedX; + bestProjectedY = projectedY; + bestDistanceSquared = distanceSquared; + } + + return BuildProjection( + trajectory, + vehiclePoseInWorld, + bestSegmentStartIndex, + bestInterpolationRatio, + bestProjectedX, + bestProjectedY, + bestDistanceSquared); + } + + /// + /// 根据最近线段和插值比例生成控制器使用的完整投影结果。 + /// + private static TrajectoryProjection BuildProjection( + Trajectory2D 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); + + var segmentX = + segmentEnd.PoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters; + var segmentY = + segmentEnd.PoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters; + var segmentLength = + Math.Sqrt( + segmentX * segmentX + + segmentY * segmentY); + + // 以轨迹线段的前进方向判断左右： + // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。 + var vehicleToProjectionX = + projectedX - + vehiclePoseInWorld.XMeters; + var vehicleToProjectionY = + projectedY - + vehiclePoseInWorld.YMeters; + var lateralErrorMeters = + (segmentX * vehicleToProjectionY - + segmentY * vehicleToProjectionX) / + segmentLength; + + var headingErrorRadians = + AngleMath.ShortestDifferenceRadians( + referenceYawRadians, + vehiclePoseInWorld.YawRadians); + + return new TrajectoryProjection( + segmentStartIndex, + referencePoint, + lateralErrorMeters, + headingErrorRadians, + Math.Sqrt(distanceSquared), + trajectory.GetRemainingDistanceMeters( + referenceArcLengthMeters)); + } + + /// + /// 检查用于投影的实际车体中心位姿是否包含有限数值。 + /// + private static void EnsureFinitePose( + 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)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "用于轨迹投影的车体位姿必须是有限值。"); + } + } + } +} diff --git a/Shared/Mathematics/AngleMath.cs b/Shared/Mathematics/AngleMath.cs index 334fd2a..ca3803f 100644 --- a/Shared/Mathematics/AngleMath.cs +++ b/Shared/Mathematics/AngleMath.cs @@ -81,6 +81,29 @@ namespace MyParking.Shared return NormalizeDegrees(targetDegrees - currentDegrees); } + /// + /// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。 + /// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio， + /// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。 + /// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。 + /// + public static double LerpRadians( + double startRadians, + double endRadians, + double ratio) + { + EnsureFinite(startRadians, nameof(startRadians)); + EnsureFinite(endRadians, nameof(endRadians)); + EnsureFinite(ratio, nameof(ratio)); + + var shortestDifference = ShortestDifferenceRadians( + endRadians, + startRadians); + + return NormalizeRadians( + startRadians + ratio * shortestDifference); + } + /// /// 将角度从度转换为弧度，不进行归一化。 /// diff --git a/Shared/Mathematics/InterpolationMath.cs b/Shared/Mathematics/InterpolationMath.cs new file mode 100644 index 0000000..61db6e3 --- /dev/null +++ b/Shared/Mathematics/InterpolationMath.cs @@ -0,0 +1,61 @@ + +using System; + +namespace MyParking.Shared +{ + /// + /// 提供与具体业务和坐标系无关的基础插值功能。 + /// + public static class InterpolationMath + { + /// + /// 将插值比例限制到[0, 1]闭区间。 + /// + public static double Clamp01(double value) + { + EnsureFinite(value, nameof(value)); + + if (value <= 0.0) + { + return 0.0; + } + + if (value >= 1.0) + { + return 1.0; + } + + return value; + } + + /// + /// 对两个标量执行线性插值。 + /// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio， + /// 因此也支持区间外的线性外插。 + /// + public static double Lerp( + double start, + double end, + double ratio) + { + EnsureFinite(start, nameof(start)); + EnsureFinite(end, nameof(end)); + EnsureFinite(ratio, nameof(ratio)); + + return start + ratio * (end - start); + } + + /// + /// 验证输入是可用于插值计算的有限数值。 + /// + private static void EnsureFinite(double value, string parameterName) + { + if (double.IsNaN(value) || double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "插值参数必须是有限数值。"); + } + } + } +} diff --git a/data_process/轨迹测试处理/compare_state_estimation.py b/data_process/轨迹测试处理/compare_state_estimation.py new file mode 100644 index 0000000..d78e936 --- /dev/null +++ b/data_process/轨迹测试处理/compare_state_estimation.py @@ -0,0 +1,760 @@ +"""对比原始Detour差分与C#在线车辆状态估计结果。""" + +from __future__ import annotations + +import argparse +from collections import Counter +from dataclasses import dataclass +from pathlib import Path + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + + +REQUIRED_COLUMNS = { + "ElapsedSeconds", + "DetourX", + "DetourY", + "DetourTheta", +} + + +def wrap_radians(angle: float | np.ndarray) -> float | np.ndarray: + """将弧度归一化到[-π, π)区间。""" + return (angle + np.pi) % (2.0 * np.pi) - np.pi + + +def angle_difference(target: float, current: float) -> float: + """计算从当前角到目标角的最短有符号弧度差。""" + return float(wrap_radians(target - current)) + + +@dataclass(frozen=True) +class Pose: + """保存世界坐标系中的二维位姿，单位为m和rad。""" + + x: float + y: float + yaw: float + + +@dataclass(frozen=True) +class State: + """保存脚本复现得到的世界位姿和世界速度。""" + + timestamp: float + pose: Pose + vx: float + vy: float + omega: float + velocity_valid: bool + + +class LowPassFilter: + """复现FirstOrderLowPassFilter的一阶低通计算。""" + + def __init__(self, time_constant_seconds: float) -> None: + if not np.isfinite(time_constant_seconds) or time_constant_seconds <= 0: + raise ValueError("滤波时间常数必须是正有限值。") + self.time_constant_seconds = float(time_constant_seconds) + self.initialized = False + self.value = 0.0 + + def update(self, value: float, delta_time_seconds: float) -> float: + """按照真实采样间隔更新滤波输出。""" + if not self.initialized: + self.value = float(value) + self.initialized = True + return self.value + alpha = delta_time_seconds / ( + self.time_constant_seconds + delta_time_seconds + ) + self.value += alpha * (float(value) - self.value) + return self.value + + def reset(self) -> None: + """清除滤波历史。""" + self.initialized = False + self.value = 0.0 + + +class VelocityEstimator: + """复现VelocityEstimator2D的世界速度差分与低通处理。""" + + def __init__(self, linear_tau: float, angular_tau: float) -> None: + self.vx_filter = LowPassFilter(linear_tau) + self.vy_filter = LowPassFilter(linear_tau) + self.omega_filter = LowPassFilter(angular_tau) + self.previous_pose: Pose | None = None + self.previous_timestamp = 0.0 + + def reset(self, pose: Pose | None = None, timestamp: float = 0.0) -> State | None: + """清除历史，并可使用当前位姿建立新的零速差分基准。""" + self.vx_filter.reset() + self.vy_filter.reset() + self.omega_filter.reset() + self.previous_pose = pose + self.previous_timestamp = float(timestamp) + if pose is None: + return None + return State(timestamp, pose, 0.0, 0.0, 0.0, False) + + def update(self, pose: Pose, timestamp: float) -> State: + """使用一个新的有效位姿更新速度估计。""" + if self.previous_pose is None: + state = self.reset(pose, timestamp) + assert state is not None + return state + delta_time = timestamp - self.previous_timestamp + if delta_time <= 0.0: + raise ValueError("新样本时间戳必须严格递增。") + raw_vx = (pose.x - self.previous_pose.x) / delta_time + raw_vy = (pose.y - self.previous_pose.y) / delta_time + raw_omega = angle_difference( + pose.yaw, + self.previous_pose.yaw, + ) / delta_time + state = State( + timestamp, + pose, + self.vx_filter.update(raw_vx, delta_time), + self.vy_filter.update(raw_vy, delta_time), + self.omega_filter.update(raw_omega, delta_time), + True, + ) + self.previous_pose = pose + self.previous_timestamp = timestamp + return state + + def rebase_preserving_velocity( + self, + pose: Pose, + timestamp: float, + ) -> State: + """更新差分基准但保留三个低通滤波器的当前输出。""" + self.previous_pose = pose + self.previous_timestamp = timestamp + velocity_valid = ( + self.vx_filter.initialized + and self.vy_filter.initialized + and self.omega_filter.initialized + ) + return State( + timestamp, + pose, + self.vx_filter.value if velocity_valid else 0.0, + self.vy_filter.value if velocity_valid else 0.0, + self.omega_filter.value if velocity_valid else 0.0, + velocity_valid, + ) + + +class DetourProviderSimulator: + """按当前简化版DetourVehicleStateProvider处理离线CSV样本。""" + + def __init__( + self, + linear_tau: float = 0.15, + angular_tau: float = 0.20, + maximum_linear_speed: float = 1.20, + maximum_angular_speed: float = np.pi / 4.0, + position_jump_margin: float = 0.03, + heading_jump_margin: float = np.deg2rad(5.0), + stationary_seconds: float = 0.35, + ) -> None: + self.estimator = VelocityEstimator(linear_tau, angular_tau) + self.maximum_linear_speed = maximum_linear_speed + self.maximum_angular_speed = maximum_angular_speed + self.position_jump_margin = position_jump_margin + self.heading_jump_margin = heading_jump_margin + self.stationary_seconds = stationary_seconds + + self.accepted_pose: Pose | None = None + self.accepted_timestamp = 0.0 + self.latest_state: State | None = None + self.stationary_hold = False + + @staticmethod + def poses_equal(first: Pose, second: Pose) -> bool: + """判断两次读取是否为Detour保持输出的同一数值帧。""" + return ( + abs(first.x - second.x) <= 1e-9 + and abs(first.y - second.y) <= 1e-9 + and abs(angle_difference(first.yaw, second.yaw)) <= 1e-8 + ) + + def motion_plausible(self, start: Pose, end: Pose, delta_time: float) -> bool: + """按照车辆绝对运动能力判断两帧是否连续。""" + if not np.isfinite(delta_time) or delta_time <= 0.0: + return False + displacement = np.hypot(end.x - start.x, end.y - start.y) + heading_change = abs(angle_difference(end.yaw, start.yaw)) + return ( + displacement + <= self.maximum_linear_speed * delta_time + + self.position_jump_margin + and heading_change + <= self.maximum_angular_speed * delta_time + + self.heading_jump_margin + ) + + def accept_after_reset(self, pose: Pose, timestamp: float) -> State: + """接受首帧或确认后的重定位并清除速度历史。""" + state = self.estimator.reset(pose, timestamp) + assert state is not None + self.latest_state = state + self.accepted_pose = pose + self.accepted_timestamp = timestamp + self.stationary_hold = False + return state + + def accept_continuous(self, pose: Pose, timestamp: float) -> State: + """接受连续正常位姿并更新速度估计。""" + state = self.estimator.update(pose, timestamp) + self.latest_state = state + self.accepted_pose = pose + self.accepted_timestamp = timestamp + self.stationary_hold = False + return state + + def handle_repeated(self, timestamp: float) -> tuple[State, str]: + """保留重复帧，并在长期不变后将估计速度归零。""" + assert self.accepted_pose is not None + assert self.latest_state is not None + unchanged = timestamp - self.accepted_timestamp + if not self.stationary_hold and unchanged >= self.stationary_seconds: + self.latest_state = State( + timestamp, + self.accepted_pose, + 0.0, + 0.0, + 0.0, + True, + ) + self.stationary_hold = True + return self.latest_state, "stationary_zero" + return self.latest_state, "duplicate" + + def process( + self, + pose: Pose, + timestamp: float, + velocity_innovation_abnormal: bool = False, + ) -> tuple[State | None, str]: + """处理一帧CSV中的Detour读取结果。""" + if self.accepted_pose is None: + return self.accept_after_reset(pose, timestamp), "initialized" + if self.poses_equal(pose, self.accepted_pose): + return self.handle_repeated(timestamp) + if self.stationary_hold: + return self.accept_after_reset(pose, timestamp), "restart_after_stationary" + + elapsed = timestamp - self.accepted_timestamp + if not self.motion_plausible(self.accepted_pose, pose, elapsed): + assert self.latest_state is not None + return self.latest_state, "physical_anomaly" + if velocity_innovation_abnormal: + self.latest_state = self.estimator.rebase_preserving_velocity( + pose, + timestamp, + ) + self.accepted_pose = pose + self.accepted_timestamp = timestamp + self.stationary_hold = False + return self.latest_state, "velocity_rebase" + return self.accept_continuous(pose, timestamp), "accepted" + + +def segmented_unwrap_degrees(values_radians: np.ndarray) -> np.ndarray: + """分别展开由NaN分隔的有效航向区间。""" + result = np.full(values_radians.shape, np.nan, dtype=float) + finite = np.isfinite(values_radians) + indices = np.flatnonzero(finite) + if not indices.size: + return result + starts = np.r_[0, np.flatnonzero(np.diff(indices) > 1) + 1] + ends = np.r_[starts[1:], indices.size] + for start, end in zip(starts, ends): + segment_indices = indices[start:end] + result[segment_indices] = np.rad2deg( + np.unwrap(values_radians[segment_indices]) + ) + return result + + +def calculate_naive_derivatives( + time: np.ndarray, + x: np.ndarray, + y: np.ndarray, + yaw: np.ndarray, +) -> tuple[np.ndarray, np.ndarray]: + """直接逐记录帧差分，保留重复帧造成的零值和更新尖峰。""" + speed = np.full(time.shape, np.nan, dtype=float) + omega = np.full(time.shape, np.nan, dtype=float) + delta_time = np.diff(time) + valid = np.isfinite(delta_time) & (delta_time > 0.0) + delta_x = np.diff(x) + delta_y = np.diff(y) + delta_yaw = wrap_radians(np.diff(yaw)) + speed_values = np.full(delta_time.shape, np.nan, dtype=float) + omega_values = np.full(delta_time.shape, np.nan, dtype=float) + speed_values[valid] = ( + np.hypot(delta_x[valid], delta_y[valid]) + / delta_time[valid] + ) + omega_values[valid] = delta_yaw[valid] / delta_time[valid] + speed[1:] = speed_values + omega[1:] = omega_values + return speed, omega + + +def configure_matplotlib() -> None: + """配置常见中文字体和负号显示。""" + plt.rcParams["font.sans-serif"] = [ + "Microsoft YaHei", + "SimHei", + "Arial Unicode MS", + "DejaVu Sans", + ] + plt.rcParams["axes.unicode_minus"] = False + + +def load_csv(csv_path: Path) -> pd.DataFrame: + """读取并校验状态估计对比所需的CSV字段。""" + frame = pd.read_csv(csv_path) + missing = REQUIRED_COLUMNS.difference(frame.columns) + if missing: + raise ValueError( + f"{csv_path.name}缺少字段：{', '.join(sorted(missing))}" + ) + for column in REQUIRED_COLUMNS: + frame[column] = pd.to_numeric(frame[column], errors="coerce") + frame = ( + frame.dropna(subset=list(REQUIRED_COLUMNS)) + .sort_values("ElapsedSeconds") + .drop_duplicates("ElapsedSeconds", keep="last") + .reset_index(drop=True) + ) + if len(frame) < 3: + raise ValueError(f"{csv_path.name}有效数据不足3行。") + frame["ElapsedSeconds"] -= frame["ElapsedSeconds"].iloc[0] + return frame + + +def detect_visual_anomalies( + time: np.ndarray, + x: np.ndarray, + y: np.ndarray, + yaw: np.ndarray, + linear_tau: float, + angular_tau: float, + position_residual_meters: float, + heading_residual_radians: float, + stationary_seconds: float, +) -> np.ndarray: + """用恒速预测残差标注可疑跳变，不修改任何状态估计数据。""" + anomalies = np.zeros(time.shape, dtype=bool) + estimator = VelocityEstimator(linear_tau, angular_tau) + previous_pose: Pose | None = None + previous_update_time = 0.0 + latest_state: State | None = None + stationary = False + + for index, timestamp in enumerate(time): + pose = Pose( + float(x[index]), + float(y[index]), + float(wrap_radians(yaw[index])), + ) + + if previous_pose is None: + latest_state = estimator.reset(pose, float(timestamp)) + previous_pose = pose + previous_update_time = float(timestamp) + continue + + if DetourProviderSimulator.poses_equal(pose, previous_pose): + if ( + not stationary + and timestamp - previous_update_time >= stationary_seconds + ): + stationary = True + continue + + # 静止后的第一个新定位只重新建立差分基准，避免把起步误标为跳变。 + if stationary: + latest_state = estimator.reset(pose, float(timestamp)) + previous_pose = pose + previous_update_time = float(timestamp) + stationary = False + continue + + delta_time = float(timestamp) - previous_update_time + if ( + latest_state is not None + and latest_state.velocity_valid + and delta_time > 0.0 + ): + predicted_x = previous_pose.x + latest_state.vx * delta_time + predicted_y = previous_pose.y + latest_state.vy * delta_time + predicted_yaw = float( + wrap_radians( + previous_pose.yaw + latest_state.omega * delta_time + ) + ) + position_residual = np.hypot( + pose.x - predicted_x, + pose.y - predicted_y, + ) + heading_residual = abs( + angle_difference(pose.yaw, predicted_yaw) + ) + + if ( + position_residual > position_residual_meters + or heading_residual > heading_residual_radians + ): + anomalies[index] = True + # 标注后从当前观测重新开始，避免一个跳变引发连续误标。 + latest_state = estimator.rebase_preserving_velocity( + pose, + float(timestamp), + ) + previous_pose = pose + previous_update_time = float(timestamp) + continue + + latest_state = estimator.update(pose, float(timestamp)) + previous_pose = pose + previous_update_time = float(timestamp) + + return anomalies + + +def simulate( + frame: pd.DataFrame, + args: argparse.Namespace, +) -> tuple[pd.DataFrame, Counter]: + """使用当前C#参数处理整份Detour记录。""" + time = frame["ElapsedSeconds"].to_numpy(float) + raw_x = frame["DetourX"].to_numpy(float) / 1000.0 + raw_y = frame["DetourY"].to_numpy(float) / 1000.0 + raw_yaw = np.deg2rad(frame["DetourTheta"].to_numpy(float)) + raw_speed, raw_omega = calculate_naive_derivatives( + time, + raw_x, + raw_y, + raw_yaw, + ) + visual_anomalies = detect_visual_anomalies( + time, + raw_x, + raw_y, + raw_yaw, + args.linear_tau, + args.angular_tau, + args.annotation_position_residual_mm / 1000.0, + np.deg2rad(args.annotation_heading_residual_deg), + args.stationary_seconds, + ) + + simulator = DetourProviderSimulator( + linear_tau=args.linear_tau, + angular_tau=args.angular_tau, + maximum_linear_speed=args.maximum_linear_speed, + maximum_angular_speed=np.deg2rad(args.maximum_angular_speed_deg), + position_jump_margin=args.position_jump_margin_mm / 1000.0, + heading_jump_margin=np.deg2rad(args.heading_jump_margin_deg), + stationary_seconds=args.stationary_seconds, + ) + + processed_x = np.full(time.shape, np.nan) + processed_y = np.full(time.shape, np.nan) + processed_yaw = np.full(time.shape, np.nan) + processed_speed = np.full(time.shape, np.nan) + processed_omega = np.full(time.shape, np.nan) + events: list[str] = [] + + for index, timestamp in enumerate(time): + pose = Pose( + raw_x[index], + raw_y[index], + float(wrap_radians(raw_yaw[index])), + ) + state, event = simulator.process( + pose, + float(timestamp), + bool(visual_anomalies[index]), + ) + events.append(event) + if state is None: + continue + processed_x[index] = state.pose.x + processed_y[index] = state.pose.y + processed_yaw[index] = state.pose.yaw + if state.velocity_valid: + processed_speed[index] = np.hypot(state.vx, state.vy) + processed_omega[index] = state.omega + + result = pd.DataFrame( + { + "TimeSeconds": time, + "RawX": raw_x, + "RawY": raw_y, + "RawYawRadians": raw_yaw, + "RawSpeed": raw_speed, + "RawOmegaRadiansPerSecond": raw_omega, + "ProcessedX": processed_x, + "ProcessedY": processed_y, + "ProcessedYawRadians": processed_yaw, + "ProcessedSpeed": processed_speed, + "ProcessedOmegaRadiansPerSecond": processed_omega, + "VisualAnomaly": visual_anomalies, + "Event": events, + } + ) + counts = Counter(events) + counts["visual_anomaly"] = int(visual_anomalies.sum()) + return result, counts + + +def plot_comparison( + csv_path: Path, + frame: pd.DataFrame, + result: pd.DataFrame, + event_counts: Counter, + output_directory: str | None, + show: bool, +) -> Path: + """生成位置、航向、线速度和角速度处理前后对比图。""" + time = result["TimeSeconds"].to_numpy(float) + anomalous = result["VisualAnomaly"].to_numpy(bool) + raw_yaw_degrees = segmented_unwrap_degrees( + result["RawYawRadians"].to_numpy(float) + ) + processed_yaw_degrees = segmented_unwrap_degrees( + result["ProcessedYawRadians"].to_numpy(float) + ) + + fig, axes = plt.subplots( + 5, + 1, + figsize=(13.0, 16.0), + sharex=True, + ) + + series = [ + ("RawX", "ProcessedX", "世界坐标X / m"), + ("RawY", "ProcessedY", "世界坐标Y / m"), + ] + for axis, (raw_name, processed_name, ylabel) in zip(axes[:2], series): + axis.plot(time, result[raw_name], color="0.65", linewidth=1.0, label="原始Detour") + axis.plot(time, result[processed_name], color="tab:blue", linewidth=1.5, label="在线处理后") + axis.scatter( + time[anomalous], + result.loc[anomalous, raw_name], + color="tab:red", + marker="x", + s=26, + label="异常位置", + zorder=3, + ) + axis.set_ylabel(ylabel) + axis.grid(True, alpha=0.3) + axis.legend(loc="best") + + axes[2].plot(time, raw_yaw_degrees, color="0.65", linewidth=1.0, label="原始Detour") + axes[2].plot(time, processed_yaw_degrees, color="tab:blue", linewidth=1.5, label="在线处理后") + axes[2].scatter( + time[anomalous], + raw_yaw_degrees[anomalous], + color="tab:red", + marker="x", + s=26, + label="异常位置", + zorder=3, + ) + axes[2].set_ylabel("展开航向角 / deg") + axes[2].grid(True, alpha=0.3) + axes[2].legend(loc="best") + + axes[3].plot(time, result["RawSpeed"], color="0.65", linewidth=1.0, label="逐记录帧直接差分") + axes[3].plot(time, result["ProcessedSpeed"], color="tab:green", linewidth=1.5, label="去重、跳变保护和低通后") + if "CommandSpeed" in frame.columns: + command_speed = pd.to_numeric( + frame["CommandSpeed"], errors="coerce" + ).to_numpy(float) + axes[3].plot(time, command_speed, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令线速度") + axes[3].set_ylabel("合线速度 / (m/s)") + axes[3].grid(True, alpha=0.3) + axes[3].legend(loc="best") + + axes[4].plot( + time, + np.rad2deg(result["RawOmegaRadiansPerSecond"]), + color="0.65", + linewidth=1.0, + label="逐记录帧最短角差", + ) + axes[4].plot( + time, + np.rad2deg(result["ProcessedOmegaRadiansPerSecond"]), + color="tab:purple", + linewidth=1.5, + label="去重、跳变保护和低通后", + ) + if "CommandAngularSpeedRadPerSecond" in frame.columns: + command_omega = np.rad2deg( + pd.to_numeric( + frame["CommandAngularSpeedRadPerSecond"], + errors="coerce", + ).to_numpy(float) + ) + axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度") + elif "CommandAngularSpeed" in frame.columns: + # 旧版CSV只有CommandAngularSpeed列，该列历史单位是deg/s； + # 新版CSV另增RadPerSecond列，不能把旧列再次按rad/s换算。 + command_omega = pd.to_numeric( + frame["CommandAngularSpeed"], + errors="coerce", + ).to_numpy(float) + axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度") + axes[4].set_ylabel("角速度 / (deg/s)") + axes[4].set_xlabel("时间 / s") + axes[4].grid(True, alpha=0.3) + axes[4].legend(loc="best") + + controller = ( + str(frame["ControllerName"].iloc[0]) + if "ControllerName" in frame.columns + else "UnknownController" + ) + trajectory = ( + str(frame["TrajectoryName"].iloc[0]) + if "TrajectoryName" in frame.columns + else csv_path.stem + ) + anomaly_count = int(anomalous.sum()) + fig.suptitle( + "Detour状态估计处理前后对比\n" + f"{controller} - {trajectory}，" + f"标注异常位置{anomaly_count}帧", + fontsize=14, + ) + fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.965)) + + if output_directory: + destination_directory = Path(output_directory) + else: + destination_directory = csv_path.parent / "state_estimation_plots" + destination_directory.mkdir(parents=True, exist_ok=True) + destination = destination_directory / ( + csv_path.stem + "_state_estimation_comparison.png" + ) + fig.savefig(destination, dpi=220, bbox_inches="tight") + if show: + plt.show() + plt.close(fig) + return destination + + +def discover_csv_files(arguments: list[str]) -> list[Path]: + """解析文件或目录；目录会被递归展开为全部轨迹CSV。""" + input_paths = ( + [Path(argument).resolve() for argument in arguments] + if arguments + else [Path(__file__).resolve().parent] + ) + csv_files: set[Path] = set() + + for input_path in input_paths: + if input_path.is_file(): + if input_path.suffix.lower() == ".csv": + csv_files.add(input_path) + continue + + if input_path.is_dir(): + csv_files.update( + path.resolve() + for path in input_path.rglob("*.csv") + if not any( + part.startswith("state_estimation_plots") + for part in path.parts + ) + ) + continue + + raise FileNotFoundError( + f"输入文件或目录不存在：{input_path}" + ) + + return sorted(csv_files) + + +def main() -> None: + """解析命令行并批量生成Detour状态估计对比图。""" + configure_matplotlib() + parser = argparse.ArgumentParser( + description="比较原始Detour与当前C#在线状态估计算法。" + ) + parser.add_argument( + "files", + nargs="*", + help="一个或多个轨迹实验CSV或包含CSV的目录", + ) + parser.add_argument("--output-dir") + parser.add_argument("--show", action="store_true") + parser.add_argument("--linear-tau", type=float, default=0.15) + parser.add_argument("--angular-tau", type=float, default=0.20) + parser.add_argument("--maximum-linear-speed", type=float, default=1.20) + parser.add_argument("--maximum-angular-speed-deg", type=float, default=45.0) + parser.add_argument("--position-jump-margin-mm", type=float, default=30.0) + parser.add_argument("--heading-jump-margin-deg", type=float, default=5.0) + parser.add_argument( + "--annotation-position-residual-mm", + type=float, + default=40.0, + help="只用于图中红色异常位置标注的预测位置残差阈值", + ) + parser.add_argument( + "--annotation-heading-residual-deg", + type=float, + default=5.0, + help="只用于图中红色异常位置标注的预测航向残差阈值", + ) + parser.add_argument("--stationary-seconds", type=float, default=0.35) + args = parser.parse_args() + + processed_count = 0 + for csv_path in discover_csv_files(args.files): + try: + frame = load_csv(csv_path) + result, counts = simulate(frame, args) + destination = plot_comparison( + csv_path, + frame, + result, + counts, + args.output_dir, + args.show, + ) + print( + f"{csv_path.name}: " + f"重复帧={counts['duplicate']}，" + f"异常位置={counts['visual_anomaly']}" + ) + print(f"已生成：{destination}") + processed_count += 1 + except Exception as exception: + print(f"跳过{csv_path.name}：{exception}") + + if processed_count == 0: + raise SystemExit("没有找到包含有效Detour字段的轨迹实验CSV。") + + +if __name__ == "__main__": + main() diff --git a/docs/detour数据处理.txt b/docs/detour数据处理.txt new file mode 100644 index 0000000..8b35a7f --- /dev/null +++ b/docs/detour数据处理.txt @@ -0,0 +1,365 @@ +需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分： + +```text +单帧错误定位 +持续性的定位重定位/地图修正 +车辆真实的快速运动 +``` + +对你这种低速停车机器人，可以先采用一套偏安全、容易验证的处理。 + +## 定位跳变会造成什么影响 + +假设Detour在50ms内突然跳了10cm： + +\[ +v=\frac{0.1}{0.05}=2m/s +\] + +实际车辆可能只有 `0.3m/s`，但差分速度会产生 `2m/s` 的尖峰。 + +对控制器还有三个直接影响： + +- 横向位置跳10cm，横向误差可能瞬间变化10cm。 +- 航向跳5°，航向误差会瞬间变化5°。 +- 全局轨迹投影可能跳到另一条临近或相交的轨迹线段。 + +Stanley在低速时尤其敏感： + +\[ +\delta = +e_\theta+ +\arctan\left(\frac{k e_y}{v+\varepsilon}\right) +\] + +低速时分母较小，横向误差突然变大，会产生很大的转向指令。因此不能完全不处理。 + +## 不建议直接低通掉定位跳变 + +不要简单地对Detour位置做强低通： + +```text +错误位置跳变 +→ 低通缓慢跟过去 +``` + +这样虽然曲线看起来平滑，但控制器会在一段时间内使用滞后、虚构的位置，可能更加危险。 + +更好的做法是： + +```text +检测跳变 +→ 暂时不把该帧用于速度差分和控制 +→ 观察后续定位 +→ 判断是单帧异常还是持续重定位 +``` + +## 第一层：运动学合理性检查 + +将当前Detour位姿和上一次接受的位姿比较。 + +位置变化： + +\[ +\Delta p=\sqrt{\Delta x^2+\Delta y^2} +\] + +航向变化： + +\[ +\Delta\theta= +|\operatorname{ShortestAngleDifference}| +\] + +允许的最大变化量可以按照车辆物理能力计算： + +```csharp +var maximumAllowedDistance = + maximumLinearSpeedMetersPerSecond * + deltaTimeSeconds + + positionJumpMarginMeters; + +var maximumAllowedHeadingChange = + maximumAngularSpeedRadiansPerSecond * + deltaTimeSeconds + + headingJumpMarginRadians; +``` + +然后判断： + +```csharp +var positionJump = + displacementMeters > + maximumAllowedDistance; + +var headingJump = + headingChangeRadians > + maximumAllowedHeadingChange; +``` + +你当前小车最高约 `1.2m/s`，假设Detour更新周期为50ms： + +```text +物理最大位移约为： +1.2 × 0.05 = 0.06m +``` + +初期可以额外留出约 `0.03～0.05m` 的定位余量。不过这些阈值最终应根据实际Detour数据确定，不建议永久写死。 + +## 第二层：不要立即接受异常帧 + +检测到一个异常帧时，不要立刻改变车辆状态： + +```text +上一正常位置 A +异常位置 B +下一帧又回到 A 附近 +``` + +这种情况说明B很可能是单帧异常，应直接丢弃。 + +如果后续连续若干帧都稳定在B附近： + +```text +A → B → B附近 → B附近 +``` + +这更可能是Detour发生了持续性的重定位。 + +可以使用： + +```text +连续2～3个新定位帧相互一致 +``` + +作为重新接受定位的条件。 + +## 第三层：重定位后必须重置速度估计 + +如果确认新的定位是持续有效的，不能用： + +```text +新位置B - 旧位置A +``` + +计算速度，因为A到B是定位修正，不是车辆真实运动。 + +正确处理是： + +```csharp +_velocityEstimator.Reset( + newPose, + currentTimestamp); +``` + +这会： + +- 将新位姿作为新的差分起点。 +- 清除之前的速度历史。 +- 重置三个低通滤波器。 +- 将速度暂时标记为无效或零。 +- 等下一次正常Detour更新后重新开始估计。 + +## 第四层：控制器应该如何响应 + +对于实车轨迹跟踪，建议状态分为： + +```text +Valid 正常定位，可以控制 +Suspected 检测到疑似跳变 +Reacquiring 正在确认新的定位 +Stale 定位长时间没有更新 +``` + +你的第一版不一定需要单独增加复杂枚举，但控制行为至少应该满足： + +```text +正常： +继续轨迹跟踪 + +疑似单帧跳变： +不使用异常帧更新状态 +短时间保持上一状态 + +连续异常或定位超时： +停车，不继续使用旧状态运动 + +确认重定位： +接受新位姿 +重置速度估计 +重新执行轨迹投影 +确认稳定后恢复控制 +``` + +停车机器人速度低、场地有限，定位连续异常时停车比盲目继续跟踪更合适。 + +## 还需要限制轨迹投影进度 + +即使Detour跳变检测做了，轨迹投影也最好增加进度保护。 + +当前投影器在整条轨迹上找最近点，如果轨迹自交，车辆可能从： + +```text +当前第20段 +``` + +突然投影到： + +```text +第80段 +``` + +后续可以增加一个 `TrajectoryProgressTracker`： + +```text +正常时只在上次线段索引附近搜索 +例如 [previousIndex - 5, previousIndex + 20] + +定位重定位后： +重新执行一次全局搜索 +``` + +同时限制单周期允许的弧长进度变化： + +\[ +|\Delta s| +\leq +v_{\max}\Delta t+\text{margin} +\] + +这样可以避免定位轻微波动导致轨迹进度突然跳段。 + +## 在五个文件中的分工 + +```text +DetourVehicleStateProvider +├─ 检查数值有效性 +├─ 检查定位是否过期 +├─ 检测重复帧 +├─ 检测位置/航向跳变 +├─ 暂存疑似新定位 +└─ 确认重定位后触发Reset + +VelocityEstimator2D +├─ 正常位姿差分 +├─ 真实时间差计算 +├─ 速度滤波 +└─ 提供Reset() + +FirstOrderLowPassFilter +└─ 提供Update()和Reset() + +VehicleState +├─ 保存位姿和速度 +└─ 表示速度估计是否有效 + +IVehicleStateProvider +└─ 向控制器返回有效状态或读取失败 +``` + +因此定位跳变的主要逻辑放在 `DetourVehicleStateProvider`，而不是低通滤波器。 + +第一版建议先实现： + +1. 非有限值检查。 +2. 重复帧识别。 +3. 基于最大速度和最大角速度的跳变检测。 +4. 单帧异常拒绝。 +5. 连续2～3帧确认重定位。 +6. 重定位后重置速度估计。 +7. 长时间定位无效时通知控制器停车。 + +这套方案足以保护当前Stanley和后续纵向控制，不需要现在就修改SLAM源码。 + + +已经给 [DetourVehicleStateProvider.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:233) 增加了基于上一有效速度的预测残差检测。 + +现在采用两级判断： + +```text +第一级：绝对物理运动边界 +第二级：上一状态恒速预测残差 +``` + +预测公式为： + +\[ +x_{pred}=x_{last}+v_x\Delta t +\] + +\[ +y_{pred}=y_{last}+v_y\Delta t +\] + +\[ +\theta_{pred}= +\theta_{last}+\omega\Delta t +\] + +实测残差为： + +\[ +e_p= +\sqrt{ +(x_{measured}-x_{pred})^2+ +(y_{measured}-y_{pred})^2 +} +\] + +以及： + +\[ +e_\theta= +\operatorname{ShortestDifference} +(\theta_{measured},\theta_{pred}) +\] + +对应实现在： + +- [IsPredictionResidualPlausible()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:536) +- [PredictPoseInWorld()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:573) + +默认阈值为： + +```csharp +位置预测残差：0.04m +航向预测残差：5° +``` + +没有采用最初考虑的30mm，是因为用现有数据离线统计后发现： + +| 位置残差阈值 | 触发次数 | 占有效更新比例 | +|---:|---:|---:| +| 30mm | 55次 | 1.24% | +| 35mm | 28次 | 0.63% | +| 40mm | 13次 | 0.29% | +| 45mm | 7次 | 0.16% | +| 50mm | 4次 | 0.09% | + +30mm容易把正常Detour波动也判成异常；40mm仍能识别现有数据中约49mm和66mm的明显跳变，同时误触发风险更低。 + +当前判定逻辑： + +```csharp +if (!isWithinPhysicalBoundary || + !isWithinPredictionResidual) +{ + // 进入疑似重定位确认状态。 +} +``` + +另外同步修正了跳变恢复逻辑： + +- 发生预测残差跳变时，保存当时的预测位姿。 +- 后续定位必须回到预测位姿附近，才能认为是单帧异常后的正常恢复。 +- 不再使用较宽松的绝对物理范围立即放行。 +- 持续远端定位仍需满足3次观测和0.25秒，才按重定位接管。 +- 接管后重置速度估计，避免把坐标修正计算成车辆速度。 + +第一帧或重定位后的速度还没有建立时，不启用预测残差检测，只使用物理极限检查，避免没有速度基准时误判。 + +完整构建结果： + +- CommonUsage：0警告、0错误 +- MedullaAdapter：0警告、0错误 +- MultiWheelC：0警告、0错误 \ No newline at end of file diff --git a/docs/记录.txt b/docs/记录.txt index 80c8e57..7d90dc9 100644 --- a/docs/记录.txt +++ b/docs/记录.txt @@ -1,23 +1,28 @@ -还可以把底盘的失败原因暴露出来： -/// -/// 获取最近一次底盘运动分解失败原因。 -/// -public string LastFailureReason => - _chassis.LastMotionDecomposeFailureReason; -这样调用方可以打印： -if (!adapter.Send(command)) -{ - Console.WriteLine( - $"底盘命令执行失败：{adapter.LastFailureReason}"); -} - - -需要注意，Detour 差分速度会有噪声，建议在 Python 中： -按固定频率重新采样。 -对位置做轻微滤波或使用 Savitzky–Golay 求导。 -再计算速度，避免直接逐点差分产生尖峰。 -Stanley 和 LQR 必须使用相同的滤波和采样参数。 - - 编译命令： powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1 + + + +1. TrajectoryPoint.cs 已完成 +2. Trajectory2D.cs 下一步 +3. TrajectoryProjection.cs 定义一次投影结果 +4. TrajectoryProjector.cs 实现连续线段投影 +5. TrajectoryBuilder.cs 原始离散点转标准轨迹 + +1. VehicleState.cs + ↓ +2. FirstOrderLowPassFilter.cs + ↓ +3. VelocityEstimator2D.cs + ↓ +4. IVehicleStateProvider.cs + ↓ +5. DetourVehicleStateProvider.cs + + + +private const double LinearVelocityFilterTimeConstantSeconds = + 0.10; + +private const double AngularVelocityFilterTimeConstantSeconds = + 0.10; \ No newline at end of file