打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-06 15:14:12 +08:00
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
@@ -0,0 +1,223 @@
+using System;
+using MultiWheelC.Control.Abstractions;
+
+namespace MultiWheelC.Control.Lateral
+{
+    /// <summary>
+    /// 使用参考曲率前馈、航向误差和横向误差计算车体中心目标曲率。
+    /// </summary>
+    public sealed class StanleyLateralController : ILateralController
+    {
+        private const double MaximumMathematicalAngleRadians =
+            Math.PI / 2.0 - 1e-3;
+
+        /// <summary>
+        /// 创建使用指定GCP几何、Stanley增益和低速保护参数的横向控制器。
+        /// </summary>
+        public StanleyLateralController(
+            double controlPointRadiusMeters,
+            double crossTrackGainPerSecond,
+            double headingErrorGain,
+            double minimumSpeedMetersPerSecond,
+            bool useActualSpeedForGain = true)
+        {
+            EnsureFinitePositive(
+                controlPointRadiusMeters,
+                nameof(controlPointRadiusMeters));
+            EnsureFiniteNonNegative(
+                crossTrackGainPerSecond,
+                nameof(crossTrackGainPerSecond));
+            EnsureFiniteNonNegative(
+                headingErrorGain,
+                nameof(headingErrorGain));
+            EnsureFinitePositive(
+                minimumSpeedMetersPerSecond,
+                nameof(minimumSpeedMetersPerSecond));
+
+            ControlPointRadiusMeters = controlPointRadiusMeters;
+            CrossTrackGainPerSecond = crossTrackGainPerSecond;
+            HeadingErrorGain = headingErrorGain;
+            MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
+            UseActualSpeedForGain = useActualSpeedForGain;
+        }
+
+        /// <summary>
+        /// 获取车体中心到前、后GCP的距离，单位为m。
+        /// </summary>
+        public double ControlPointRadiusMeters { get; }
+
+        /// <summary>
+        /// 获取横向误差增益，单位为1/s。
+        /// </summary>
+        public double CrossTrackGainPerSecond { get; }
+
+        /// <summary>
+        /// 获取航向误差的无量纲增益。
+        /// </summary>
+        public double HeadingErrorGain { get; }
+
+        /// <summary>
+        /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。
+        /// </summary>
+        public double MinimumSpeedMetersPerSecond { get; }
+
+        /// <summary>
+        /// 获取是否优先使用Detour估算的实际纵向速度计算横向误差项。
+        /// </summary>
+        public bool UseActualSpeedForGain { get; }
+
+        /// <summary>
+        /// 根据参考曲率、航向误差和横向误差计算车体中心目标曲率。
+        /// </summary>
+        public LateralControlCommand Compute(
+            PathTrackingContext context)
+        {
+            var speedForGain = SelectSpeedForGain(context);
+            var speedMagnitude = Math.Max(
+                Math.Abs(speedForGain),
+                MinimumSpeedMetersPerSecond);
+            var travelDirection = SelectTravelDirection(context);
+
+            // 参考曲率提供前馈；没有跟踪误差时也能沿曲线行驶。
+            var feedforwardAngleRadians = Math.Atan(
+                context.ReferenceCurvaturePerMeter *
+                ControlPointRadiusMeters);
+
+            // 轨迹位于车辆左侧时横向误差为正，对应正的左转修正。
+            var crossTrackCorrectionRadians = Math.Atan(
+                CrossTrackGainPerSecond *
+                context.LateralErrorMeters /
+                speedMagnitude);
+
+            // 倒车时需要反转反馈修正方向；参考曲率前馈仍由轨迹本身决定。
+            var feedbackAngleRadians = travelDirection *
+                (HeadingErrorGain * context.HeadingErrorRadians +
+                 crossTrackCorrectionRadians);
+
+            // 这里只避开tan奇点，实际GCP机械限制由AckermannGcpAllocator处理。
+            var targetEquivalentAngleRadians = Clamp(
+                feedforwardAngleRadians + feedbackAngleRadians,
+                -MaximumMathematicalAngleRadians,
+                MaximumMathematicalAngleRadians);
+            var targetCurvaturePerMeter = Math.Tan(
+                targetEquivalentAngleRadians) /
+                ControlPointRadiusMeters;
+
+            return new LateralControlCommand(
+                targetCurvaturePerMeter);
+        }
+
+        /// <summary>
+        /// 清除横向控制器状态；当前Stanley实现没有跨周期状态。
+        /// </summary>
+        public void Reset()
+        {
+        }
+
+        /// <summary>
+        /// 选择Stanley横向误差项使用的实际速度或旧版参考速度。
+        /// </summary>
+        private double SelectSpeedForGain(
+            PathTrackingContext context)
+        {
+            if (UseActualSpeedForGain &&
+                context.HasValidVelocityEstimate)
+            {
+                return context
+                    .ActualLongitudinalSpeedMetersPerSecond;
+            }
+
+            return context.ReferenceSpeedMetersPerSecond;
+        }
+
+        /// <summary>
+        /// 根据有符号参考速度确定前进或倒车的反馈修正方向。
+        /// </summary>
+        private static double SelectTravelDirection(
+            PathTrackingContext context)
+        {
+            const double directionDeadbandMetersPerSecond = 1e-6;
+
+            if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
+                directionDeadbandMetersPerSecond)
+            {
+                return Math.Sign(
+                    context.ReferenceSpeedMetersPerSecond);
+            }
+
+            if (context.HasValidVelocityEstimate &&
+                Math.Abs(
+                    context.ActualLongitudinalSpeedMetersPerSecond) >
+                directionDeadbandMetersPerSecond)
+            {
+                return Math.Sign(
+                    context.ActualLongitudinalSpeedMetersPerSecond);
+            }
+
+            return 1.0;
+        }
+
+        /// <summary>
+        /// 将数值限制在指定闭区间内。
+        /// </summary>
+        private static double Clamp(
+            double value,
+            double minimum,
+            double maximum)
+        {
+            return Math.Max(
+                minimum,
+                Math.Min(maximum, value));
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为正有限值。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value <= 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制器的几何尺寸和最小速度必须是正有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制增益是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制增益必须是非负有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查控制参数是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "Stanley控制参数必须是有限值。");
+            }
+        }
+    }
+}