增加差速电机前馈与控制器曲率预瞄

2026-08-12 11:19:01 +08:00
parent 3043febd91
commit 6c6149c8d5
24 changed files with 366 additions and 19 deletions
@@ -110,7 +110,9 @@ namespace MultiWheelC.Control.Execution
            double terminalBrakingPreviewMeters = 0.02,
            double terminalApproachDistanceMeters = 0.10,
            double terminalApproachGainPerSecond = 0.8,
-            double maximumTerminalApproachSpeedMetersPerSecond = 0.05)
+            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
+            double curvaturePreviewSeconds = 0.20,
+            double maximumCurvaturePreviewMeters = 0.12)
        {
            _stateProvider = stateProvider ??
                throw new ArgumentNullException(
@@ -152,6 +154,12 @@ namespace MultiWheelC.Control.Execution
            EnsureFinitePositive(
                maximumTerminalApproachSpeedMetersPerSecond,
                nameof(maximumTerminalApproachSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                curvaturePreviewSeconds,
+                nameof(curvaturePreviewSeconds));
+            EnsureFiniteNonNegative(
+                maximumCurvaturePreviewMeters,
+                nameof(maximumCurvaturePreviewMeters));

            if (terminalApproachDistanceMeters <=
                finishDistanceMeters)
@@ -176,6 +184,9 @@ namespace MultiWheelC.Control.Execution
                terminalApproachGainPerSecond;
            MaximumTerminalApproachSpeedMetersPerSecond =
                maximumTerminalApproachSpeedMetersPerSecond;
+            CurvaturePreviewSeconds = curvaturePreviewSeconds;
+            MaximumCurvaturePreviewMeters =
+                maximumCurvaturePreviewMeters;
        }

        /// <summary>
@@ -218,6 +229,16 @@ namespace MultiWheelC.Control.Execution
        /// </summary>
        public double MaximumTerminalApproachSpeedMetersPerSecond { get; }

+        /// <summary>
+        /// 获取按照车辆纵向速度换算曲率前馈预瞄距离的预测时间，单位为s，0表示关闭。
+        /// </summary>
+        public double CurvaturePreviewSeconds { get; }
+
+        /// <summary>
+        /// 获取曲率前馈沿轨迹点序允许预瞄的最大距离，单位为m。
+        /// </summary>
+        public double MaximumCurvaturePreviewMeters { get; }
+
        /// <summary>
        /// 获取控制器当前是否持有并正在执行一条轨迹。
        /// </summary>
@@ -259,6 +280,16 @@ namespace MultiWheelC.Control.Execution
        /// </summary>
        public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }

+        /// <summary>
+        /// 获取最近控制周期实际采用的曲率前馈预瞄距离，单位为m。
+        /// </summary>
+        public double? LastCurvaturePreviewDistanceMeters { get; private set; }
+
+        /// <summary>
+        /// 获取最近控制周期沿轨迹预瞄后交给横向控制器的曲率，单位为1/m。
+        /// </summary>
+        public double? LastFeedforwardCurvaturePerMeter { get; private set; }
+
        /// <summary>
        /// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。
        /// </summary>
@@ -431,10 +462,23 @@ namespace MultiWheelC.Control.Execution
                        projection);
                LastControlReferenceSpeedMetersPerSecond =
                    controlReferenceSpeedMetersPerSecond;
+                var curvaturePreviewDistanceMeters =
+                    ResolveCurvaturePreviewDistanceMeters(
+                        vehicleState,
+                        controlReferenceSpeedMetersPerSecond);
+                var feedforwardCurvaturePerMeter =
+                    ResolveFeedforwardCurvaturePerMeter(
+                        projection,
+                        curvaturePreviewDistanceMeters);
+                LastCurvaturePreviewDistanceMeters =
+                    curvaturePreviewDistanceMeters;
+                LastFeedforwardCurvaturePerMeter =
+                    feedforwardCurvaturePerMeter;
                var context = new PathTrackingContext(
                    vehicleState,
                    projection,
                    controlReferenceSpeedMetersPerSecond,
+                    feedforwardCurvaturePerMeter,
                    deltaTimeSeconds);
                var lateralCommand =
                    _lateralController.Compute(context);
@@ -552,6 +596,50 @@ namespace MultiWheelC.Control.Execution
                vehicleState);
        }

+        /// <summary>
+        /// 根据有效实际纵向速度或控制参考速度计算带上限的曲率前馈预瞄距离。
+        /// </summary>
+        private double ResolveCurvaturePreviewDistanceMeters(
+            VehicleState vehicleState,
+            double controlReferenceSpeedMetersPerSecond)
+        {
+            if (CurvaturePreviewSeconds <= 0.0 ||
+                MaximumCurvaturePreviewMeters <= 0.0)
+            {
+                return 0.0;
+            }
+
+            var previewSpeedMetersPerSecond =
+                vehicleState.HasValidVelocityEstimate
+                    ? Math.Abs(
+                        vehicleState.TwistInBody
+                            .VxMetersPerSecond)
+                    : Math.Abs(
+                        controlReferenceSpeedMetersPerSecond);
+
+            return Math.Min(
+                MaximumCurvaturePreviewMeters,
+                previewSpeedMetersPerSecond *
+                CurvaturePreviewSeconds);
+        }
+
+        /// <summary>
+        /// 沿轨迹实际执行点序向前采样专供横向几何前馈使用的曲率。
+        /// </summary>
+        private double ResolveFeedforwardCurvaturePerMeter(
+            TrajectoryProjection projection,
+            double previewDistanceMeters)
+        {
+            var previewArcLengthMeters = Math.Min(
+                _trajectory.TotalLengthMeters,
+                projection.ArcLengthMeters +
+                previewDistanceMeters);
+
+            return _trajectory
+                .SampleAtArcLength(previewArcLengthMeters)
+                .CurvaturePerMeter;
+        }
+
        /// <summary>
        /// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。
        /// </summary>
@@ -914,6 +1002,8 @@ namespace MultiWheelC.Control.Execution
            LastProjection = null;
            LastCommand = null;
            LastControlReferenceSpeedMetersPerSecond = null;
+            LastCurvaturePreviewDistanceMeters = null;
+            LastFeedforwardCurvaturePerMeter = null;
            LastCycleTiming = null;
            LastFailureReason = string.Empty;
            LastException = null;