添加单车底盘仿真平台并完善运动控制与夹臂功能

2026-07-27 17:47:50 +08:00
parent 580a936a83
commit e6b99c45b3
47 changed files with 4238 additions and 122 deletions
@@ -0,0 +1,174 @@
+namespace MyParking.Simulation.Core;
+
+/// <summary>
+/// 模拟单个舵轮的转向和驱动响应，不包含真实电机物理模型。
+/// </summary>
+public sealed class VirtualSteerWheel
+{
+    private const double AngleLowerLimitDegrees = -120.0;
+    private const double AngleUpperLimitDegrees = 120.0;
+    private const double MaximumSteeringRateDegreesPerSecond = 90.0;
+    private const double MaximumDriveAccelerationMetersPerSecondSquared = 0.8;
+    private const double AlignmentToleranceDegrees = 1.5;
+
+    public VirtualSteerWheel(
+        string name,
+        double xMeters,
+        double yMeters)
+    {
+        Name = name;
+        XMeters = xMeters;
+        YMeters = yMeters;
+    }
+
+    public string Name { get; }
+
+    public double XMeters { get; }
+
+    public double YMeters { get; }
+
+    public double TargetAngleDegrees { get; private set; }
+
+    public double ActualAngleDegrees { get; private set; }
+
+    public double TargetSpeedMetersPerSecond { get; private set; }
+
+    public double ActualSpeedMetersPerSecond { get; private set; }
+
+    public bool IsAligned =>
+        Math.Abs(TargetAngleDegrees - ActualAngleDegrees) <=
+        AlignmentToleranceDegrees;
+
+    /// <summary>
+    /// 设置期望轮胎速度向量，并在机械限位内选择等价舵角。
+    /// </summary>
+    public bool SetVelocityVector(
+        double vxMetersPerSecond,
+        double vyMetersPerSecond)
+    {
+        var speed = Math.Sqrt(
+            vxMetersPerSecond * vxMetersPerSecond +
+            vyMetersPerSecond * vyMetersPerSecond);
+
+        if (speed < 1e-6)
+        {
+            TargetSpeedMetersPerSecond = 0.0;
+            return true;
+        }
+
+        var desiredAngleDegrees =
+            Math.Atan2(vyMetersPerSecond, vxMetersPerSecond) *
+            180.0 / Math.PI;
+
+        return SetDirectionAndSpeed(
+            desiredAngleDegrees,
+            speed);
+    }
+
+    /// <summary>
+    /// 停车时设置舵轮预对齐方向。
+    /// </summary>
+    public bool PrepareDirection(double targetAngleDegrees)
+    {
+        TargetSpeedMetersPerSecond = 0.0;
+        return SetDirectionAndSpeed(
+            targetAngleDegrees,
+            0.0);
+    }
+
+    /// <summary>
+    /// 将驱动目标设置为零，并保持当前舵轮方向。
+    /// </summary>
+    public void Stop()
+    {
+        TargetSpeedMetersPerSecond = 0.0;
+    }
+
+    /// <summary>
+    /// 按固定转向速度和驱动加速度更新虚拟反馈。
+    /// </summary>
+    public void Step(double deltaTimeSeconds)
+    {
+        ActualAngleDegrees = MoveTowards(
+            ActualAngleDegrees,
+            TargetAngleDegrees,
+            MaximumSteeringRateDegreesPerSecond *
+            deltaTimeSeconds);
+
+        var allowedTargetSpeed =
+            IsAligned ? TargetSpeedMetersPerSecond : 0.0;
+
+        ActualSpeedMetersPerSecond = MoveTowards(
+            ActualSpeedMetersPerSecond,
+            allowedTargetSpeed,
+            MaximumDriveAccelerationMetersPerSecondSquared *
+            deltaTimeSeconds);
+    }
+
+    /// <summary>
+    /// 恢复舵轮初始状态。
+    /// </summary>
+    public void Reset()
+    {
+        TargetAngleDegrees = 0.0;
+        ActualAngleDegrees = 0.0;
+        TargetSpeedMetersPerSecond = 0.0;
+        ActualSpeedMetersPerSecond = 0.0;
+    }
+
+    private bool SetDirectionAndSpeed(
+        double desiredAngleDegrees,
+        double desiredSpeedMetersPerSecond)
+    {
+        var candidates = new[]
+        {
+            (Angle: NormalizeDegrees(desiredAngleDegrees),
+                Speed: desiredSpeedMetersPerSecond),
+            (Angle: NormalizeDegrees(desiredAngleDegrees + 180.0),
+                Speed: -desiredSpeedMetersPerSecond),
+            (Angle: NormalizeDegrees(desiredAngleDegrees - 180.0),
+                Speed: -desiredSpeedMetersPerSecond)
+        };
+
+        var validCandidates = candidates
+            .Where(candidate =>
+                candidate.Angle >= AngleLowerLimitDegrees &&
+                candidate.Angle <= AngleUpperLimitDegrees)
+            .OrderBy(candidate =>
+                Math.Abs(candidate.Angle - ActualAngleDegrees))
+            .ToArray();
+
+        if (validCandidates.Length == 0)
+        {
+            TargetSpeedMetersPerSecond = 0.0;
+            return false;
+        }
+
+        var selected = validCandidates[0];
+        TargetAngleDegrees = selected.Angle;
+        TargetSpeedMetersPerSecond = selected.Speed;
+        return true;
+    }
+
+    private static double MoveTowards(
+        double current,
+        double target,
+        double maximumChange)
+    {
+        var difference = target - current;
+        if (Math.Abs(difference) <= maximumChange)
+            return target;
+
+        return current + Math.Sign(difference) * maximumChange;
+    }
+
+    private static double NormalizeDegrees(double angleDegrees)
+    {
+        angleDegrees %= 360.0;
+        if (angleDegrees >= 180.0)
+            angleDegrees -= 360.0;
+        if (angleDegrees < -180.0)
+            angleDegrees += 360.0;
+        return angleDegrees;
+    }
+}