完善原地自转控制逻辑并加入纵向速度死区与实验绘图改进

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -28,7 +28,7 @@ namespace MultiWheelC.Control.Execution
             1e-6;
         private const double StartupRegionMeters = 0.02;
         private const double StartupPreviewDistanceMeters = 0.05;
-        private const double MaximumStartupSpeedMetersPerSecond = 0.05;
+        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
 
         private readonly IVehicleStateProvider _stateProvider;
         private readonly ILateralController _lateralController;

MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs

@@ -23,11 +23,15 @@ namespace MultiWheelC.Control.Longitudinal
             double integralGainPerSecond,
             double derivativeGainSeconds,
             double maximumIntegralCorrectionMetersPerSecond,
-            double maximumCommandSpeedMetersPerSecond)
+            double maximumCommandSpeedMetersPerSecond,
+            double speedErrorDeadbandMetersPerSecond = 0.025)
         {
             EnsureFinitePositive(
                 maximumCommandSpeedMetersPerSecond,
                 nameof(maximumCommandSpeedMetersPerSecond));
+            EnsureFiniteNonNegative(
+                speedErrorDeadbandMetersPerSecond,
+                nameof(speedErrorDeadbandMetersPerSecond));
 
             _feedbackPid = new PidController(
                 proportionalGain,
@@ -37,6 +41,8 @@ namespace MultiWheelC.Control.Longitudinal
                 derivativeOnMeasurement: true);
             MaximumCommandSpeedMetersPerSecond =
                 maximumCommandSpeedMetersPerSecond;
+            SpeedErrorDeadbandMetersPerSecond =
+                speedErrorDeadbandMetersPerSecond;
         }
 
         /// <summary>
@@ -49,6 +55,11 @@ namespace MultiWheelC.Control.Longitudinal
         /// </summary>
         public double MaximumCommandSpeedMetersPerSecond { get; }
 
+        /// <summary>
+        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double SpeedErrorDeadbandMetersPerSecond { get; }
+
         /// <summary>
         /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
         /// </summary>
@@ -98,6 +109,20 @@ namespace MultiWheelC.Control.Longitudinal
                     referenceSpeedMetersPerSecond);
             }
 
+            var speedErrorMetersPerSecond =
+                referenceSpeedMetersPerSecond -
+                context.ActualLongitudinalSpeedMetersPerSecond;
+
+            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
+            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
+            if (Math.Abs(speedErrorMetersPerSecond) <=
+                SpeedErrorDeadbandMetersPerSecond)
+            {
+                Reset();
+                return LimitReferenceSpeed(
+                    referenceSpeedMetersPerSecond);
+            }
+
             GetCorrectionOutputRange(
                 referenceSpeedMetersPerSecond,
                 out var minimumCorrectionMetersPerSecond,
@@ -178,5 +203,22 @@ namespace MultiWheelC.Control.Longitudinal
                     "纵向控制器最大命令速度必须是正有限值。");
             }
         }
+
+        /// <summary>
+        /// 检查速度误差死区是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value) ||
+                value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "纵向控制器速度误差死区必须是非负有限值。");
+            }
+        }
     }
 }

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -330,17 +330,17 @@ namespace MultiWheelC
         /// <summary>
         /// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
         /// </summary>
-        public double CurvatureTransitionLengthMeters = 0.60;
+        public double CurvatureTransitionLengthMeters = 0.70;
 
         /// <summary>
         /// 获取或设置两段直线的最大参考速度，单位为m/s。
         /// </summary>
-        public double StraightMaximumSpeedMetersPerSecond = 0.30;
+        public double StraightMaximumSpeedMetersPerSecond = 0.40;
 
         /// <summary>
         /// 获取或设置半圆段的最大参考速度，单位为m/s。
         /// </summary>
-        public double SemicircleMaximumSpeedMetersPerSecond = 0.25;
+        public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
 
         /// <summary>
         /// 获取或设置参考速度加速度，单位为m/s²。

MultiWheelC/Experiments/RotationTests.cs

@@ -1,6 +1,7 @@
 
 
 using System;
+using System.Globalization;
 using System.Numerics;
 using System.Threading;
 using ClumsyCore;
@@ -17,7 +18,6 @@ namespace MultiWheelC
     public abstract class InPlaceRotateTestBase : MovementTest
     {
         public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
-        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
         public int TrialNumber = 1; // 重复实验编号。
 
         private DriveTask _task;
@@ -37,11 +37,18 @@ namespace MultiWheelC
         // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
         public override void Test()
         {
+            var config = PilotDefinition.Conf;
+
             if (float.IsNaN(RelativeAngleDegrees) ||
                 float.IsInfinity(RelativeAngleDegrees) ||
-                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
+                float.IsNaN(config.InPlaceRotateMaxSpeed) ||
-                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
+                float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
-                MaxAngularSpeedDegreesPerSecond <= 0f)
+                config.InPlaceRotateMaxSpeed <= 0f ||
+                float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
+                float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
+                config.InPlaceRotateMinimumSpeed <= 0f ||
+                config.InPlaceRotateMinimumSpeed >
+                    config.InPlaceRotateMaxSpeed)
             {
                 Console.WriteLine("原地旋转测试参数无效。");
                 return;
@@ -66,8 +73,25 @@ namespace MultiWheelC
                 (float)AngleMath.NormalizeDegrees(
                     location.th + RelativeAngleDegrees);
 
+            Console.WriteLine(
+                "原地自转实际参数：" +
+                $"Kp={config.InPlaceRotateKp:F3}，" +
+                $"Ki={config.InPlaceRotateKi:F3}，" +
+                $"Kd={config.InPlaceRotateKd:F3}，" +
+                $"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
+                $"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
+                $"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
+                $"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
+                $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
+                $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
+                $"起点航向={location.th:F2}°，" +
+                $"目标航向={targetWorldAngle:F2}°。");
+            Console.WriteLine(
+                "原地自转CSV保存目录：" +
+                TrackingExperimentRecorder.DefaultOutputDirectory);
+
             _recorder = new TrackingExperimentRecorder(
-                controllerName: "InPlaceRotatePID",
+                controllerName: "InPlaceRotateFilteredPID",
                 trajectoryName: _trajectoryName,
                 trialNumber: TrialNumber,
                 referenceStart: rotationCenter,
@@ -75,7 +99,7 @@ namespace MultiWheelC
                 referenceSpeed: 0f,
                 referenceAngularSpeed:
                     (float)AngleMath.DegreesToRadians(
-                        MaxAngularSpeedDegreesPerSecond));
+                        config.InPlaceRotateMaxSpeed));
             _recorder.Start();
 
             try
@@ -88,21 +112,26 @@ namespace MultiWheelC
                         PidparamsRead = () => new PIDParams
                         {
                             Kp =
-                                PilotDefinition.Conf.InPlaceRotateKp,
+                                config.InPlaceRotateKp,
                             Ki =
-                                PilotDefinition.Conf.InPlaceRotateKi,
+                                config.InPlaceRotateKi,
                             Kd =
-                                PilotDefinition.Conf.InPlaceRotateKd,
+                                config.InPlaceRotateKd,
                             DeadZone =
-                                PilotDefinition.Conf
+                                config.InPlaceRotateArriveDeg,
-                                    .InPlaceRotateArriveDeg,
                             SpeedAccPerSec =
-                                PilotDefinition.Conf.InPlaceRotateAcc,
+                                config.InPlaceRotateAcc,
                             OutputUpperThreshold =
-                                MaxAngularSpeedDegreesPerSecond,
+                                config.InPlaceRotateMaxSpeed,
                             MaxI =
-                                PilotDefinition.Conf.InPlaceRotateMaxI
+                                config.InPlaceRotateMaxI
                         },
+                        MinimumAngularSpeedDegreesPerSecond =
+                            config.InPlaceRotateMinimumSpeed,
+                        WheelAlignmentToleranceDegrees =
+                            config.InPlaceRotateWheelAlignDeg,
+                        RotationTimeoutSeconds =
+                            config.InPlaceRotateTimeoutSec,
                         CommandAngularSpeedObserver =
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
@@ -136,23 +165,58 @@ namespace MultiWheelC
 
     }
 
-    [MovementTest(name = "SendXYThSpeed：原地自转90°")]
+    [MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
-    public sealed class TestRotate90 :
+    public sealed class TestRotateAngle :
         InPlaceRotateTestBase
     {
-        public TestRotate90()
+        public TestRotateAngle()
-            : base(90f, "Rotate90")
+            : base(0f, "RotateCustomAngle")
         {
         }
+
+        /// <summary>
+        /// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
+        /// </summary>
+        public override void Test()
+        {
+            var input = UI.GetInput(
+                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
+
+            if ((!float.TryParse(
+                     input,
+                     NumberStyles.Float,
+                     CultureInfo.CurrentCulture,
+                     out var relativeAngleDegrees) &&
+                 !float.TryParse(
+                     input,
+                     NumberStyles.Float,
+                     CultureInfo.InvariantCulture,
+                     out relativeAngleDegrees)) ||
+                float.IsNaN(relativeAngleDegrees) ||
+                float.IsInfinity(relativeAngleDegrees))
+            {
+                Console.WriteLine("旋转角度输入无效，测试已经取消。");
+                return;
+            }
+
+            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
+            {
+                Console.WriteLine("旋转角度不能为0，测试已经取消。");
+                return;
+            }
+
+            // 当前控制器按照圆周最短角旋转；精确±180°的方向存在二义性。
+            if (Math.Abs(relativeAngleDegrees) >= 180f)
+            {
+                Console.WriteLine(
+                    "输入角度必须满足-180° < angle < 180°；" +
+                    "当前最短角控制不支持指定精确±180°的旋转方向。");
+                return;
+            }
+
+            RelativeAngleDegrees = relativeAngleDegrees;
+            base.Test();
+        }
     }
 
-    [MovementTest(name = "SendXYThSpeed：原地自转180°")]
-    public sealed class TestRotate180 :
-        InPlaceRotateTestBase
-    {
-        public TestRotate180()
-            : base(180f, "Rotate180")
-        {
-        }
-    }
 }

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -145,6 +145,14 @@ namespace MultiWheelC
         // 保存成功后的CSV绝对路径；尚未保存时为空。
         public string SavedFilePath { get; private set; }
 
+        /// <summary>
+        /// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
+        /// </summary>
+        public static string DefaultOutputDirectory =>
+            Path.Combine(
+                AppContext.BaseDirectory,
+                "TrackingExperiments");
+
         // 启动后台采样线程。
         public void Start()
         {
@@ -242,6 +250,17 @@ namespace MultiWheelC
             }
         }
 
+        /// <summary>
+        /// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。
+        /// </summary>
+        public void ClearControlReference()
+        {
+            lock (_stateSyncRoot)
+            {
+                _hasControlReference = false;
+            }
+        }
+
         // 停止采样并将本次实验保存为CSV；重复调用只保存一次。
         public void StopAndSave()
         {
@@ -444,9 +463,8 @@ namespace MultiWheelC
                     new List<TrackingSample>(_samples);
             }
 
-            var outputDirectory = Path.Combine(
+            var outputDirectory =
-                AppContext.BaseDirectory,
+                DefaultOutputDirectory;
-                "TrackingExperiments");
 
             Directory.CreateDirectory(outputDirectory);
 

MultiWheelC/Movements/RotateInPlace.cs

@@ -7,6 +7,7 @@ using ClumsyCore.Pilot;
 using CommonUsage.Chassis;
 using MDCSToolBox.Commons.Controllers;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -17,7 +18,11 @@ namespace MultiWheelC
         /// </summary>
         public float AngleTarget;
 
-        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+        // 留作标定或单元测试时显式替换；为空时使用经过校验的Detour状态源。
+        public Func<float> ThetaReader;
+
+        public IVehicleStateProvider StateProvider =
+            new DetourVehicleStateProvider();
 
         public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
 
@@ -37,6 +42,12 @@ namespace MultiWheelC
         // 自转舵轮准备超时时间，单位s。
         public float WheelAlignmentTimeoutSeconds = 10f;
 
+        // 航向尚未到位时允许下发的最小有效角速度，单位deg/s。
+        public float MinimumAngularSpeedDegreesPerSecond = 1f;
+
+        // 舵轮到位后执行航向闭环允许的最长时间，单位s。
+        public float RotationTimeoutSeconds = 15f;
+
         // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
         public override IEnumerable<bool> Get()
         {
@@ -44,6 +55,8 @@ namespace MultiWheelC
                 throw new InvalidOperationException(
                     "当前底盘不是MultiWheelChassis，无法执行原地自转。");
 
+            ValidateParameters();
+
             var adapter = new MultiWheelChassisAdapter(
                 Chassis,
                 PilotDefinition.Self.CarNum);
@@ -55,7 +68,9 @@ namespace MultiWheelC
                 DateTime? alignedSince = null;
                 while (true)
                 {
-                    if (!adapter.PrepareSpin())
+                    if (!adapter.PrepareSpin(
+                            alignmentToleranceDegrees:
+                                WheelAlignmentToleranceDegrees))
                         throw new InvalidOperationException(
                             "无法生成原地自转舵轮目标：" +
                             adapter.LastFailureReason);
@@ -84,18 +99,86 @@ namespace MultiWheelC
                     yield return true;
                 }
 
+                var alignmentToleranceRadians =
+                    AngleMath.DegreesToRadians(
+                        WheelAlignmentToleranceDegrees);
+                if (!adapter.AdoptPreparedSpinForXYTh(
+                        alignmentToleranceRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法将已到位的自转舵角交接给XYTh：" +
+                        adapter.LastFailureReason);
+                }
+
                 var targetAngle =
                     (float)AngleMath.NormalizeDegrees(AngleTarget);
                 var p = PidparamsRead();
-                thPid = new PIDController(ThetaReader, p.Kp);
+                var currentAngle = ReadCurrentAngleDegrees();
+                var cachedCurrentAngle = currentAngle;
+                thPid = new PIDController(
+                    () => cachedCurrentAngle,
+                    p.Kp);
                 thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                     p.OutputUpperThreshold, p.SpeedAccPerSec);
                 var lastCommandTime = DateTime.Now;
+                var rotationStarted = DateTime.Now;
 
                 while (true)
                 {
+                    if ((DateTime.Now - rotationStarted)
+                            .TotalSeconds >
+                        RotationTimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"原地自转超过{RotationTimeoutSeconds:F1}s仍未到位。");
+                    }
+
+                    currentAngle = ReadCurrentAngleDegrees();
+                    cachedCurrentAngle = currentAngle;
                     var s = thPid.GetResponse(targetAngle, true);
-                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    var angleErrorDegrees =
+                        (float)AngleMath
+                            .ShortestDifferenceDegrees(
+                                targetAngle,
+                                currentAngle);
+
+                    // PID进入到位死区后等待其0.3s稳定确认；等待期间
+                    // 只清零驱动速度，不清除已经准备好的自转舵角状态。
+                    if (Math.Abs(angleErrorDegrees) <=
+                        p.DeadZone)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (thPid.IsArrived())
+                            break;
+
+                        yield return true;
+                        continue;
+                    }
+
+                    // PID输出低于底盘有效轮速范围时提高到最小可执行值，
+                    // 避免接近目标时反复出现微小命令但车辆实际不动。
+                    if (Math.Abs(s) > 1e-6f &&
+                        Math.Abs(s) <
+                            MinimumAngularSpeedDegreesPerSecond)
+                    {
+                        s = Math.Sign(angleErrorDegrees) *
+                            MinimumAngularSpeedDegreesPerSecond;
+                    }
+
+                    // PID加速限制在首周期可能暂时输出零；此时保留
+                    // 已交接的自转状态，等待下一周期产生有效角速度。
+                    if (Math.Abs(s) <= 1e-6f)
+                    {
+                        CommandAngularSpeedObserver?.Invoke(0f);
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+                        yield return true;
+                        continue;
+                    }
+
                     CommandAngularSpeedObserver?.Invoke(s);
                     var now = DateTime.Now;
                     var interval = now - lastCommandTime;
@@ -118,7 +201,6 @@ namespace MultiWheelC
                             "安全XYTh原地旋转底盘解算失败：" +
                             adapter.LastFailureReason);
                     }
-                    if (thPid.IsArrived()) break;
                     yield return true;
                 }
 
@@ -130,5 +212,109 @@ namespace MultiWheelC
                 adapter.StopImmediately();
             }
         }
+
+        /// <summary>
+        /// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
+        /// </summary>
+        private void ValidateParameters()
+        {
+            EnsureFinitePositive(
+                WheelAlignmentToleranceDegrees,
+                nameof(WheelAlignmentToleranceDegrees),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentStableSeconds,
+                nameof(WheelAlignmentStableSeconds),
+                allowZero: true);
+            EnsureFinitePositive(
+                WheelAlignmentTimeoutSeconds,
+                nameof(WheelAlignmentTimeoutSeconds));
+            EnsureFinitePositive(
+                MinimumAngularSpeedDegreesPerSecond,
+                nameof(MinimumAngularSpeedDegreesPerSecond));
+            EnsureFinitePositive(
+                RotationTimeoutSeconds,
+                nameof(RotationTimeoutSeconds));
+
+            var pidParameters = PidparamsRead();
+            if (pidParameters == null)
+            {
+                throw new InvalidOperationException(
+                    "原地自转PID参数读取结果为空。");
+            }
+
+            EnsureFinitePositive(
+                pidParameters.DeadZone,
+                "PidparamsRead.DeadZone");
+            EnsureFinitePositive(
+                pidParameters.OutputUpperThreshold,
+                "PidparamsRead.OutputUpperThreshold");
+            EnsureFinitePositive(
+                pidParameters.SpeedAccPerSec,
+                "PidparamsRead.SpeedAccPerSec");
+            EnsureFinitePositive(
+                pidParameters.Kp,
+                "PidparamsRead.Kp");
+
+            if (MinimumAngularSpeedDegreesPerSecond >
+                pidParameters.OutputUpperThreshold)
+            {
+                throw new InvalidOperationException(
+                    "原地自转最小有效角速度不能大于最大角速度。");
+            }
+        }
+
+        /// <summary>
+        /// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
+        /// </summary>
+        private float ReadCurrentAngleDegrees()
+        {
+            if (ThetaReader != null)
+            {
+                var angleDegrees = ThetaReader();
+                if (float.IsNaN(angleDegrees) ||
+                    float.IsInfinity(angleDegrees))
+                {
+                    throw new InvalidOperationException(
+                        "自定义航向读取结果不是有效角度。");
+                }
+
+                return (float)AngleMath.NormalizeDegrees(
+                    angleDegrees);
+            }
+
+            if (StateProvider == null ||
+                !StateProvider.TryGetState(out var state))
+            {
+                throw new InvalidOperationException(
+                    "无法从Detour状态源读取有效车辆航向。" +
+                    (StateProvider is DetourVehicleStateProvider provider
+                        ? provider.LastFailureReason
+                        : ""));
+            }
+
+            return (float)AngleMath.RadiansToDegrees(
+                state.PoseInWorld.YawRadians);
+        }
+
+        /// <summary>
+        /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
+        /// </summary>
+        private static void EnsureFinitePositive(
+            float value,
+            string parameterName,
+            bool allowZero = false)
+        {
+            if (float.IsNaN(value) ||
+                float.IsInfinity(value) ||
+                (allowZero
+                    ? value < 0f
+                    : value <= 0f))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "原地自转参数必须是有效的正数。");
+            }
+        }
     }
 }

MultiWheelC/Movements/TrajectoryTrackingMovement.cs

@@ -75,6 +75,12 @@ namespace MultiWheelC
         /// </summary>
         public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
 
+        /// <summary>
+        /// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
+        /// </summary>
+        public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
+            0.025;
+
         /// <summary>
         /// 底盘纵向命令速度绝对值上限，单位为m/s。
         /// </summary>
@@ -90,7 +96,7 @@ namespace MultiWheelC
         /// 前后GCP目标转角最大变化率，单位为rad/s。
         /// </summary>
         public double MaximumGcpAngleRateRadiansPerSecond =
-            AngleMath.DegreesToRadians(10.0);
+            AngleMath.DegreesToRadians(15.0);
 
         /// <summary>
         /// 终点位置和剩余弧长的完成容差，单位为m。
@@ -164,7 +170,8 @@ namespace MultiWheelC
                     LongitudinalKiPerSecond,
                     LongitudinalKdSeconds,
                     MaximumIntegralCorrectionMetersPerSecond,
-                    MaximumCommandSpeedMetersPerSecond);
+                    MaximumCommandSpeedMetersPerSecond,
+                    LongitudinalSpeedErrorDeadbandMetersPerSecond);
             var gcpAllocator =
                 new AckermannGcpAllocator(
                     controlPointRadiusMeters,

MultiWheelC/PilotConfig.cs

@@ -26,7 +26,7 @@ public class PilotConfig : MultiWheelPilotConfig
     public float InPlaceRotateSpeed = 30f;
 
     [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
-    public float InPlaceRotateArriveDeg = 1f;
+    public float InPlaceRotateArriveDeg = 1.5f;
 
     [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
     public float InPlaceRotateWheelAlignDeg = 2f;
@@ -38,24 +38,25 @@ public class PilotConfig : MultiWheelPilotConfig
 
     #region 单车-临时
     [FieldMember(desc = "原地旋转Kp")]
-    public float InPlaceRotateKp = 0.2f;
+    public float InPlaceRotateKp = 1.1f;
-    // public float InPlaceRotateKp = 0.2f;
 
     [FieldMember(desc = "原地旋转Ki")]
-    public float InPlaceRotateKi = 0.01f;
+    public float InPlaceRotateKi = 0f;
-    // public float InPlaceRotateKi = 0.01f;
 
     [FieldMember(desc = "原地旋转Kd")]
     public float InPlaceRotateKd = 0f;
 
     [FieldMember(desc = "原地旋转积分限幅")]
-    public float InPlaceRotateMaxI = 0.01f;
+    public float InPlaceRotateMaxI = 0f;
+
+    [FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
+    public float InPlaceRotateMinimumSpeed = 1f;
 
     [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 30f;
+    public float InPlaceRotateMaxSpeed = 47.5f;
 
     [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 30f;
+    public float InPlaceRotateAcc = 60f;
 
     [FieldMember(desc = "原地旋转超时(s)")]
     public float InPlaceRotateTimeoutSec = 15f;

MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs

@@ -188,11 +188,11 @@ namespace MultiWheelC.StateEstimation
                             poseInWorld,
                             elapsedSeconds))
                     {
-                        state = AcceptPoseAfterVelocityRebase(
+                        state = AcceptPoseAfterReset(
                             poseInWorld,
                             timestampSeconds);
                         LastFailureReason =
-                            "Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
+                            "Detour位姿偏离速度预测，已重新建立速度估计基准。";
                         return true;
                     }
 

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -504,13 +504,26 @@ namespace MyParking.Shared
         /// 返回是否成功生成舵轮目标。
         /// </summary>
         public bool PrepareSpin(
-            TimeSpan? interval = null)
+            TimeSpan? interval = null,
+            double alignmentToleranceDegrees = 2.0)
         {
+            ValidateFinite(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+
+            if (alignmentToleranceDegrees < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(alignmentToleranceDegrees),
+                    "自转舵轮到位容差必须是非负有限值。");
+            }
+
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis.PrepareRotateWheels(
-                    alignmentToleranceDegrees: 2.0f);
+                    alignmentToleranceDegrees:
+                        (float)alignmentToleranceDegrees);
 
             if (!success)
             {

data_process/新版控制器轨迹测试处理/plot_new_controller_experiment.py

@@ -208,10 +208,13 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
     has_control_reference = (
         numeric_column(frame, "HasControlReference", 0.0) > 0.5
     )
-    lateral_error = np.where(
+    recorded_lateral_valid = (
-        has_control_reference & np.isfinite(recorded_lateral_error),
+        has_control_reference & np.isfinite(recorded_lateral_error)
-        recorded_lateral_error,
+    )
-        derived_lateral_error,
+    lateral_error = (
+        np.where(recorded_lateral_valid, recorded_lateral_error, np.nan)
+        if np.any(recorded_lateral_valid)
+        else derived_lateral_error
     )
 
     state_yaw = numeric_column(frame, "StateYawRadians")
@@ -230,10 +233,28 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         frame,
         "ControlHeadingErrorRadians",
     )
-    heading_error = np.where(
+    recorded_heading_valid = (
-        has_control_reference & np.isfinite(recorded_heading_error),
+        has_control_reference & np.isfinite(recorded_heading_error)
-        recorded_heading_error,
+    )
-        derived_heading_error,
+    heading_error = (
+        np.where(recorded_heading_valid, recorded_heading_error, np.nan)
+        if np.any(recorded_heading_valid)
+        else derived_heading_error
+    )
+
+    # 投影定义满足：参考点 = 车体位置 + 横向误差 × 参考航向左法向。
+    # 因此无需假设轨迹类型，即可从有效控制周期还原车辆实际使用的参考轨迹。
+    projected_reference_yaw = actual_yaw + heading_error
+    reference_x = (
+        actual_x - lateral_error * np.sin(projected_reference_yaw)
+    )
+    reference_y = (
+        actual_y + lateral_error * np.cos(projected_reference_yaw)
+    )
+    valid_reference_position = (
+        has_control_reference
+        & np.isfinite(reference_x)
+        & np.isfinite(reference_y)
     )
 
     cruise_speed = first_finite(
@@ -270,6 +291,8 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"),
         ideal_speed,
     )
+    if np.any(has_control_reference):
+        reference_speed[~has_control_reference] = np.nan
     actual_speed = numeric_column(frame, "StateBodyVxMetersPerSecond")
     velocity_valid = (
         numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
@@ -283,6 +306,9 @@ def load_experiment(csv_path: Path) -> dict[str, object]:
         "actual_x": actual_x,
         "actual_y": actual_y,
         "valid_position": valid_position,
+        "reference_x": reference_x,
+        "reference_y": reference_y,
+        "valid_reference_position": valid_reference_position,
         "start": start,
         "end": end,
         "length": length_meters,
@@ -336,12 +362,22 @@ def plot_experiment(
 
     valid_position = data["valid_position"]
     fig, axis = plt.subplots(figsize=(9.0, 6.5))
+    valid_reference_position = data["valid_reference_position"]
+    if np.count_nonzero(valid_reference_position) >= 2:
+        axis.plot(
+            data["reference_x"][valid_reference_position],
+            data["reference_y"][valid_reference_position],
+            "--",
+            linewidth=2.0,
+            label="控制器实际使用的参考轨迹",
+        )
+    else:
         axis.plot(
             [data["start"][0], data["end"][0]],
             [data["start"][1], data["end"][1]],
             "--",
             linewidth=2.0,
-        label="期望4m直线轨迹",
+            label="参考起终点连线",
         )
     axis.plot(
         data["actual_x"][valid_position],
@@ -451,7 +487,7 @@ def discover_csv_files(arguments: list[str]) -> list[Path]:
 def main() -> None:
     """解析命令行并批量处理新版控制器实验CSV。"""
     parser = argparse.ArgumentParser(
-        description="绘制新版控制器4m直线实验的四类对比图。"
+        description="绘制新版控制器轨迹实验的四类对比图。"
     )
     parser.add_argument("csv", nargs="*", help="需要处理的CSV文件路径。")
     parser.add_argument(

docs/clumsy参考.json

@@ -0,0 +1,534 @@
+{
+  "layout": {
+    "chassis": {
+      "width": 1100.0,
+      "length": 1550.0,
+      "contour": [
+        -775.0,
+        550.0,
+        775.0,
+        550.0,
+        775.0,
+        -550.0,
+        -775.0,
+        -550.0
+      ]
+    },
+    "components": [
+      {
+        "type": "wheel",
+        "options": {
+          "platform": 0,
+          "scale": 1.0,
+          "radius": 200.0,
+          "group": null,
+          "id": 1,
+          "name": "w1",
+          "x": 0.0,
+          "y": 300.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "wheel",
+        "options": {
+          "platform": 6,
+          "scale": 1.0,
+          "radius": 200.0,
+          "group": null,
+          "id": 2,
+          "name": "w2",
+          "x": 0.0,
+          "y": -300.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 9999,
+          "contour": [
+            0.0,
+            0.0
+          ],
+          "group": [
+            "0",
+            "stop"
+          ],
+          "id": 411683697,
+          "name": "autoStop",
+          "x": 0.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 999,
+          "contour": [
+            0.0,
+            0.0
+          ],
+          "group": [
+            "0",
+            "slow"
+          ],
+          "id": 726862610,
+          "name": "autoSlow",
+          "x": 0.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidar2d",
+        "options": {
+          "isCircle": true,
+          "ignoreDist": 10.0,
+          "maxDist": 200000.0,
+          "useFilter": "",
+          "filterChassis": true,
+          "afterImageFilterOutN": 7,
+          "afterImageFilterOutDeg": 2.0,
+          "reflexThres": 0.4,
+          "reflexFilterWndSz": 30,
+          "reflexDistWnd": 50.0,
+          "reflexChunkThres": 2.5,
+          "BindLidar2dName": "",
+          "BindRelativeX": -4.9166203,
+          "BindRelativeY": 938.9871,
+          "BindRelativeTh": 1.300003,
+          "group": null,
+          "id": 1444795304,
+          "name": "rightlidar",
+          "x": -749.0,
+          "y": -475.0,
+          "yaw": 180.8,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidar2d",
+        "options": {
+          "isCircle": true,
+          "ignoreDist": 10.0,
+          "maxDist": 200000.0,
+          "useFilter": "",
+          "filterChassis": true,
+          "afterImageFilterOutN": 7,
+          "afterImageFilterOutDeg": 2.0,
+          "reflexThres": 0.4,
+          "reflexFilterWndSz": 30,
+          "reflexDistWnd": 50.0,
+          "reflexChunkThres": 2.5,
+          "BindLidar2dName": "",
+          "BindRelativeX": 0.0,
+          "BindRelativeY": 0.0,
+          "BindRelativeTh": 0.0,
+          "group": null,
+          "id": 1983955111,
+          "name": "leftlidar",
+          "x": -734.0,
+          "y": 475.0,
+          "yaw": 179.8,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidar3d",
+        "options": {
+          "ignoreDist": 5.0,
+          "maxDist": 200000.0,
+          "reduce": false,
+          "voxelSize": 70.0,
+          "pcklen": 82560,
+          "angleSgn": -1,
+          "endAngle": 0.0,
+          "RotationMatrix": [
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            -0.0,
+            0.0,
+            1.0
+          ],
+          "group": null,
+          "id": 1000069841,
+          "name": "frontlidar3d",
+          "x": 752.5,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "plannar3dlidarzrange",
+        "options": {
+          "zmin": -65.0,
+          "zmax": 7.0,
+          "useAbsolute": true,
+          "samples": 1024,
+          "lidar3dName": "frontlidar3d",
+          "isCircle": true,
+          "ignoreDist": 10.0,
+          "maxDist": 200000.0,
+          "useFilter": "",
+          "filterChassis": true,
+          "afterImageFilterOutN": 7,
+          "afterImageFilterOutDeg": 2.0,
+          "reflexThres": 0.4,
+          "reflexFilterWndSz": 30,
+          "reflexDistWnd": 50.0,
+          "reflexChunkThres": 2.5,
+          "BindLidar2dName": "",
+          "BindRelativeX": 0.0,
+          "BindRelativeY": 0.0,
+          "BindRelativeTh": 0.0,
+          "group": null,
+          "id": 1954892242,
+          "name": "frontlidar",
+          "x": 752.5,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 10,
+          "contour": [
+            600.0,
+            -650.0,
+            600.0,
+            650.0,
+            1200.0,
+            650.0,
+            1200.0,
+            -650.0
+          ],
+          "group": [
+            "1",
+            "stop"
+          ],
+          "id": 1519627219,
+          "name": "fstop1",
+          "x": 230.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      },
+      {
+        "type": "lidarssc",
+        "options": {
+          "usingLidars": "frontlidar",
+          "stopDist": 100.0,
+          "directionX": 1.0,
+          "directionY": 0.0,
+          "thresDot": 10,
+          "contour": [
+            1200.0,
+            -650.0,
+            1200.0,
+            650.0,
+            2600.0,
+            650.0,
+            2600.0,
+            -650.0
+          ],
+          "group": [
+            "1",
+            "slow"
+          ],
+          "id": 771141297,
+          "name": "fslow1",
+          "x": 230.0,
+          "y": 0.0,
+          "yaw": 0.0,
+          "z": 0.0,
+          "pitch": 0.0,
+          "roll": 0.0
+        }
+      }
+    ]
+  },
+  "DriveTaskInterval": 30,
+  "DriveTaskTimeout": 9999.0,
+  "basicSpeed": 0.7,
+  "msConf": {
+    "SyncThAccPerSec": 5.0,
+    "TestCarSyncDistance": 2893.0,
+    "TestCarSyncTh": 0.0,
+    "ManualCarSyncVxFac": 0.3,
+    "ManualCarSyncVyFac": 1.0,
+    "ManualCarSyncVthFac": 30.0,
+    "MultiVehicleCrabSteerLimitDeg": 120.0,
+    "DeltaDetectCenter": 742.5,
+    "MultiVehicleSyncUseDetour": true,
+    "MultiVehicleManualUseDetourCorrection": true,
+    "MultiVehicleFleetNum": 2,
+    "MultiVehicleSyncInterval": 25,
+    "MultiVehicleMasterEndpoint": "/",
+    "SimpleIp": "192.168.1.101",
+    "MultiVehicleSelfEndpoint": "",
+    "MultiVehicleUseDetect": false,
+    "MultiVehicleControlRadius": 0.0,
+    "MultiVehicleAutoCmdTimeoutMs": 9999,
+    "MultiVehicleMemberTtlMs": 0,
+    "MultiVehicleAutoUseIdealCenter": true,
+    "MultiVehicleAutoRequireFleetCenter": true,
+    "MultiVehiclePosBiasXFac": 0.005,
+    "MultiVehiclePosBiasYFac": 0.15,
+    "MultiVehiclePosBiasThFac": 0.1,
+    "MultiVehiclePosBiasXThreshold": 0.05,
+    "MultiVehiclePosBiasYThreshold": 10.0,
+    "MultiVehiclePosBiasThThreshold": 5.0,
+    "MultiVehicleDetectBiasXFac": 0.0,
+    "MultiVehicleDetectBiasYFac": 0.0,
+    "MultiVehicleDetectBiasThFac": 0.0,
+    "MultiVehicleDetectBiasXThreshold": 0.0,
+    "MultiVehicleDetectBiasYThreshold": 0.0,
+    "MultiVehicleDetectBiasThThreshold": 0.0,
+    "MultiVehicleRotateCompXyFac": 0.003,
+    "MultiVehicleRotateCompXyIFac": 0.01,
+    "MultiVehicleRotateCompXyMax": 3.0,
+    "MultiVehicleRotateCompThFac": 0.1,
+    "MultiVehicleRotateCompThIFac": 0.01,
+    "MultiVehicleRotateCompThMax": 3.0,
+    "MultiVehicleRotateActiveOmega": 0.5,
+    "MultiVehicleRotateCompTangentFrac": 0.1,
+    "SingleCarSyncPrecisionXy": 10.0,
+    "SingleCarSyncPrecisionTh": 0.1,
+    "PlaygroundWebApiUrl": "http://localhost:18090",
+    "MultiVehicleRotatePoseWebApiDiagEnabled": false,
+    "PlaygroundRobotName": "agv_multi_1",
+    "PlaygroundNeighborRobotName": "agv_multi_2",
+    "WebApiTranslateMm": 100.0,
+    "WebApiRotateDeg": 5.0,
+    "InPlaceRotateTargetWorldDeg": 90.0,
+    "InPlaceRotateSpeed": 30.0,
+    "InPlaceRotateArriveDeg": 1.0,
+    "InPlaceRotateWheelAlignDeg": 2.0,
+    "InPlaceRotateActiveWheelAlignDeg": 10.0,
+    "FleetRotateOmega": 6.0,
+    "FleetRotateTargetDeltaDeg": 90.0,
+    "FleetRotateArriveDeg": 1.5,
+    "FleetRotateSlowDeg": 10.0,
+    "FleetRotateMinOmega": 0.5,
+    "FleetRotateAccel": 1.0,
+    "FleetRotateSettleSec": 0.5,
+    "FleetRotateUseDetourHeading": true,
+    "FleetCrabAngleDeg": 90.0,
+    "FleetCrabBodyWorldHeadingDeg": 0.0,
+    "FleetCrabLengthMm": 2000.0,
+    "FleetCrabSpeed": 0.35,
+    "FleetCrabAccel": 0.1,
+    "FleetCrabStartAccel": 0.02,
+    "FleetCrabSlowDistance": 800.0,
+    "FleetCrabFinishDistance": 10.0,
+    "FleetCrabFinishSpeed": 0.0,
+    "FleetCrabSlowingPow": 0.7,
+    "FleetCrabGcpThetaThreshold": 120.0,
+    "FleetCrabDthLinearFac": 3.3,
+    "FleetCrabDthLinearThreshold": 10.0,
+    "FleetCrabStartSyncTimeoutSec": 9999.0,
+    "FleetCrabStartWheelAlignDeg": 2.0,
+    "TwoLegLidarName": "leftlidar,rearlidar",
+    "TwoLegGuessX": -2000.0,
+    "TwoLegWidth": 450.0,
+    "TwoLegWidthErr": 50.0,
+    "TwoLegBlobDist": 100.0,
+    "TwoLegBlobSize": 200.0,
+    "TwoLegBlobPtCount": 10,
+    "TwoLegPadding": 5,
+    "TwoLegPillarFindingScope": 20,
+    "TwoLegSgnDir": 1,
+    "TwoLegCenterChangeX": 0.0,
+    "TwoLegOutputBiasX": -15.0,
+    "TwoLegOutputBiasY": 0.0,
+    "TwoLegFilterLength": 500.0,
+    "TwoLegFilterWidth": 800.0,
+    "TireFilterLength": 1000.0,
+    "TireFilterWidth": 1900.0,
+    "TireTwoLegWidth": 1470.0,
+    "TireTwoLegWidthErr": 200.0,
+    "TireTwoLegBlobPtCount": 10,
+    "TireFrontTwoLegBlobDist": 100.0,
+    "TireFrontTwoLegBlobSize": 200.0,
+    "TireFrontPadding": 10,
+    "TireFrontTwoLegPillarFindingScope": 10,
+    "TireFrontTwoLegSgnDir": 1,
+    "TireFrontTwoLegCenterChangeX": 0.0,
+    "TireBackTwoLegBlobDist": 100.0,
+    "TireBackTwoLegBlobSize": 200.0,
+    "TireBackPadding": 5,
+    "TireBackTwoLegPillarFindingScope": 20,
+    "TireBackTwoLegSgnDir": 1,
+    "TireBackTwoLegCenterChangeX": 0.0,
+    "ClampControlKp": 0.0015,
+    "ClampControlKi": 0.0,
+    "ClampControlKd": 0.0,
+    "ClampControlMaxI": 0.02,
+    "ClampControlSpeedAcc": 2.0,
+    "ClampControlThresh": 0.2,
+    "ClampControlDeadZone": 30.0,
+    "MaxClampSpeed": 12.0,
+    "LineTrackDistance": 1000.0,
+    "LineTrackMaxSpeed": 0.3,
+    "LineTrackKp": 0.001,
+    "LineTrackKi": 0.0,
+    "LineTrackKd": 0.0,
+    "LineTrackDeadZone": 10.0,
+    "TireFollowingWalkBlindSwitchingDistance": 1300.0,
+    "TireFollowingStage1GuessX": 2200.0,
+    "TireFollowingStage2GuessX": 2600.0,
+    "TireFollowingWalkBlindFinishDistance": 10.0,
+    "TireFollowingSlowDistance": 750.0,
+    "TireFollowingMaxSpeed": 0.3,
+    "TireFollowingFrontLidarPathTransformationX": 160.0,
+    "TireFollowingFrontLidarPathTransformationY": 3.0,
+    "TireFollowingFrontLidarWalkBlindTh": 0.0,
+    "TireFollowingBackLidarPathTransformationX": 155.0,
+    "TireFollowingBackLidarPathTransformationY": 0.0,
+    "TireFollowingBackLidarWalkBlindTh": 0.0,
+    "TireFollowingLeaveCarBackLidarPathTransformationX": 1700.0,
+    "TireFollowingLeaveCarWalkBlindSwitchingDistance": 2400.0,
+    "TireFollowingTireNum": 2,
+    "TireFollowingCloseDistance": 1400.0,
+    "TireFollowingAngleIgnoreThr": 1.0,
+    "TireFollowingYAverageFrameCount": 6,
+    "DstTrackerMaxSpeed": 0.3,
+    "GcpThetaThreshold": 70.0,
+    "DthLinearFac": 0.75,
+    "DthLinearThreshold": 25.0,
+    "BiasFac": 0.5,
+    "BiasThreshold": 15.0,
+    "BiasControlGainFac": 1.0,
+    "BiasSlowSigma": 55.0,
+    "LineMagKp": 0.1,
+    "LineMagKi": 0.0,
+    "LineMagKd": 0.1,
+    "MagMaxI": 10.0,
+    "MagDeadZone": 1.0,
+    "LineMagThresh": 35.0,
+    "CurveMagKp": 0.4,
+    "CurveMagKi": 0.0,
+    "CurveMagKd": 0.1,
+    "CurveMagThresh": 65.0,
+    "MotionDebugPrint": true,
+    "DebugCurvature": false,
+    "SlowDistance": 1000.0,
+    "SlowingPow": 0.8,
+    "FinishDistance": 5.0,
+    "FinishSpeed": 0.02,
+    "FirstThAccuracy": 2.0,
+    "ThContinuousThreshold": 10.0,
+    "FirstRotateSpeedFac": 1.0,
+    "FirstRotateMaxSpeed": 30.0,
+    "FirstRotateAcc": 20.0,
+    "FirstRotateDeAcc": 30.0,
+    "SpeedAccPerSecond": 0.1,
+    "SpeedDeAccPerSecond": 1.0,
+    "NotContinuousAngle": 3.0,
+    "PowerSteeringLookAhead": 100.0,
+    "SpeedLookAhead": 1500.0,
+    "SpeedLookAheadCurveDiff": 1000.0,
+    "SpeedLookBackCurveDiff": 200.0,
+    "SpeedLimitCurveDiffMin": 0.2,
+    "SpeedLimitCurveMin": 0.2,
+    "MaxRotateSpeedCurveLimit": 30.0,
+    "MaxRotateAccCurveLimit": 30.0,
+    "BaisAlarmValue": 1500.0,
+    "DthAlarmValue": 150.0,
+    "UseAutoAvoidance": false,
+    "ObstacleStopDistance": 1000.0,
+    "ObstacleSlowDistance": 2500.0,
+    "CoefficientOfExpansion": 1.0,
+    "TargetSpeed": 0.5,
+    "EmptyCartLength": 1550.0,
+    "EmptyCartWidth": 1100.0,
+    "RotateStopFac": 1.3,
+    "RotateSlowFac": 1.8,
+    "SlowPow": 1.2,
+    "ShieldAutoObstacle": false,
+    "LidarName": "frontlidar",
+    "ObstacleRecoveryTime": 500,
+    "UseCameraAvoidance": false,
+    "UseManualContorolAvoidance": true,
+    "ManualAutoAvoidcaneSlowDistance": 500.0,
+    "ManualAutoAvoidcaneStopDistance": 300.0,
+    "ShieldAutoAvoidance": false,
+    "UpCamPoseX": 0.0,
+    "UpCamPoseY": 0.0,
+    "UpCamPoseTh": 0.0,
+    "DownCamPoseX": 0.0,
+    "DownCamPoseY": 0.0,
+    "DownCamPoseTh": 0.0,
+    "OutMapEnable": true,
+    "GroundLossThreshold": 5.0,
+    "LaserLossThreshold": 15.0,
+    "RiskSlowdownThreshold": 15.0,
+    "UseSimpleDetector": false,
+    "LoseConnectionTime": 5000,
+    "GroundCameraDisconnectAlarmTime": 200,
+    "FrontLidarName": "null",
+    "RearLidarName": "null",
+    "UseSkidDetector": true,
+    "SkidTimeThreshold": 3000,
+    "SkidFacThreshold": 3.0,
+    "MissionWarningTime": 3000,
+    "UseGyrosDetector": false,
+    "GyrosErrorTime": 3000,
+    "GyrosErrorFac": 10,
+    "ObstacleStopDec": 0.1
+  },
+  "script": "MultiWheelC.dll",
+  "guru": {
+    "MaxLogFiles": 20,
+    "interpreter": "javascript",
+    "throwSAIError": true
+  },
+  "locationTimeout": 100,
+  "IOCheckIntegrity": true,
+  "detourHost": "127.0.0.1",
+  "detourPort": 4321
+}