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

2026-08-12 11:19:01 +08:00
parent 3043febd91
commit 6c6149c8d5
24 changed files with 366 additions and 19 deletions
@@ -16,11 +16,15 @@ namespace MultiWheelC.Control.Abstractions
            VehicleState vehicleState,
            TrajectoryProjection projection,
            double controlReferenceSpeedMetersPerSecond,
+            double feedforwardCurvaturePerMeter,
            double deltaTimeSeconds)
        {
            EnsureFinite(
                controlReferenceSpeedMetersPerSecond,
                nameof(controlReferenceSpeedMetersPerSecond));
+            EnsureFinite(
+                feedforwardCurvaturePerMeter,
+                nameof(feedforwardCurvaturePerMeter));
            EnsureFinitePositive(
                deltaTimeSeconds,
                nameof(deltaTimeSeconds));
@@ -29,6 +33,8 @@ namespace MultiWheelC.Control.Abstractions
            Projection = projection;
            ControlReferenceSpeedMetersPerSecond =
                controlReferenceSpeedMetersPerSecond;
+            FeedforwardCurvaturePerMeter =
+                feedforwardCurvaturePerMeter;
            DeltaTimeSeconds = deltaTimeSeconds;
        }

@@ -66,6 +72,11 @@ namespace MultiWheelC.Control.Abstractions
            Projection.ReferencePoint
                .CurvaturePerMeter;

+        /// <summary>
+        /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
+        /// </summary>
+        public double FeedforwardCurvaturePerMeter { get; }
+
        /// <summary>
        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
        /// </summary>
@@ -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;
@@ -104,7 +104,7 @@ namespace MultiWheelC.Control.Lateral
            var feedforwardAngleRadians =
                travelDirection *
                Math.Atan(
-                    context.ReferenceCurvaturePerMeter *
+                    context.FeedforwardCurvaturePerMeter *
                    ControlPointRadiusMeters);

            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。