新增倒车以及项目结构优化

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -29,6 +29,10 @@ namespace MultiWheelC.Control.Execution
         private const double StartupRegionMeters = 0.02;
         private const double StartupPreviewDistanceMeters = 0.05;
         private const double MaximumStartupSpeedMetersPerSecond = 0.08;
+        private const double ProjectionBackwardSearchDistanceMeters =
+            0.10;
+        private const double ProjectionForwardSearchDistanceMeters =
+            1.00;
 
         private readonly IVehicleStateProvider _stateProvider;
         private readonly ILateralController _lateralController;
@@ -103,7 +107,7 @@ namespace MultiWheelC.Control.Execution
         public double FinishDistanceMeters { get; }
 
         /// <summary>
-        /// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
+        /// 获取判定轨迹执行完成时允许的最大实际纵向速度，单位为m/s。
         /// </summary>
         public double FinishSpeedMetersPerSecond { get; }
 
@@ -161,7 +165,7 @@ namespace MultiWheelC.Control.Execution
         /// <summary>
         /// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
         /// </summary>
-        public double? LastReferenceSpeedMetersPerSecond { get; private set; }
+        public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
 
         /// <summary>
         /// 停止当前底盘并从起点开始执行指定二维轨迹。
@@ -208,9 +212,18 @@ namespace MultiWheelC.Control.Execution
 
                 LastVehicleState = vehicleState;
 
-                var projection = TrajectoryProjector.Project(
-                    _trajectory,
-                    vehicleState.PoseInWorld);
+                // 首周期允许全局定位轨迹进度；后续周期仅在上次进度
+                // 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。
+                var projection = LastProjection.HasValue
+                    ? TrajectoryProjector.Project(
+                        _trajectory,
+                        vehicleState.PoseInWorld,
+                        LastProjection.Value.ArcLengthMeters,
+                        ProjectionBackwardSearchDistanceMeters,
+                        ProjectionForwardSearchDistanceMeters)
+                    : TrajectoryProjector.Project(
+                        _trajectory,
+                        vehicleState.PoseInWorld);
                 LastProjection = projection;
 
                 if (projection.DistanceToTrajectoryMeters >
@@ -240,15 +253,15 @@ namespace MultiWheelC.Control.Execution
                         terminalFailureReason);
                 }
 
-                var referenceSpeedMetersPerSecond =
+                var controlReferenceSpeedMetersPerSecond =
                     ResolveReferenceSpeedForControl(
                         projection);
-                LastReferenceSpeedMetersPerSecond =
-                    referenceSpeedMetersPerSecond;
+                LastControlReferenceSpeedMetersPerSecond =
+                    controlReferenceSpeedMetersPerSecond;
                 var context = new PathTrackingContext(
                     vehicleState,
                     projection,
-                    referenceSpeedMetersPerSecond,
+                    controlReferenceSpeedMetersPerSecond,
                     deltaTimeSeconds);
                 var lateralCommand =
                     _lateralController.Compute(context);
@@ -421,7 +434,7 @@ namespace MultiWheelC.Control.Execution
                    CalculateHeadingErrorToEndRadians(
                        vehicleState) <=
                        FinishHeadingToleranceRadians &&
-                   CalculateActualLinearSpeedMetersPerSecond(
+                   CalculateActualLongitudinalSpeedMetersPerSecond(
                        vehicleState) <=
                        FinishSpeedMetersPerSecond;
         }
@@ -446,7 +459,7 @@ namespace MultiWheelC.Control.Execution
 
             if (!isTerminalZeroSpeedReference ||
                 !vehicleState.HasValidVelocityEstimate ||
-                CalculateActualLinearSpeedMetersPerSecond(
+                CalculateActualLongitudinalSpeedMetersPerSecond(
                     vehicleState) >
                 FinishSpeedMetersPerSecond)
             {
@@ -502,16 +515,13 @@ namespace MultiWheelC.Control.Execution
         }
 
         /// <summary>
-        /// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
+        /// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
         /// </summary>
-        private static double CalculateActualLinearSpeedMetersPerSecond(
+        private static double CalculateActualLongitudinalSpeedMetersPerSecond(
             VehicleState vehicleState)
         {
-            return Math.Sqrt(
-                vehicleState.TwistInBody.VxMetersPerSecond *
-                vehicleState.TwistInBody.VxMetersPerSecond +
-                vehicleState.TwistInBody.VyMetersPerSecond *
-                vehicleState.TwistInBody.VyMetersPerSecond);
+            return Math.Abs(
+                vehicleState.TwistInBody.VxMetersPerSecond);
         }
 
         /// <summary>
@@ -578,7 +588,7 @@ namespace MultiWheelC.Control.Execution
             LastVehicleState = null;
             LastProjection = null;
             LastCommand = null;
-            LastReferenceSpeedMetersPerSecond = null;
+            LastControlReferenceSpeedMetersPerSecond = null;
             LastFailureReason = string.Empty;
             LastException = null;
         }