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

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -99,10 +99,13 @@ namespace MultiWheelC.Control.Lateral
                 MinimumSpeedMetersPerSecond);
             var travelDirection = SelectTravelDirection(context);
 
-            // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
-            var feedforwardAngleRadians = Math.Atan(
-                context.ReferenceCurvaturePerMeter *
-                ControlPointRadiusMeters);
+            // 参考曲率按轨迹点序的实际行进方向定义；倒车时底盘有符号
+            // 纵向速度反向，因此GCP曲率前馈也必须反向才能保持相同几何曲率。
+            var feedforwardAngleRadians =
+                travelDirection *
+                Math.Atan(
+                    context.ReferenceCurvaturePerMeter *
+                    ControlPointRadiusMeters);
 
             // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
             var crossTrackCorrectionRadians =
@@ -155,7 +158,7 @@ namespace MultiWheelC.Control.Lateral
                     .ActualLongitudinalSpeedMetersPerSecond;
             }
 
-            return context.ReferenceSpeedMetersPerSecond;
+            return context.ControlReferenceSpeedMetersPerSecond;
         }
 
         /// <summary>
@@ -166,11 +169,11 @@ namespace MultiWheelC.Control.Lateral
         {
             const double directionDeadbandMetersPerSecond = 1e-6;
 
-            if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
+            if (Math.Abs(context.ControlReferenceSpeedMetersPerSecond) >
                 directionDeadbandMetersPerSecond)
             {
                 return Math.Sign(
-                    context.ReferenceSpeedMetersPerSecond);
+                    context.ControlReferenceSpeedMetersPerSecond);
             }
 
             if (context.HasValidVelocityEstimate &&