完善Detour状态估计与轨迹跟踪验证

2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
@@ -0,0 +1,225 @@
+// 然后因为我这个车的舵轮是有正负120°限制的，所以有这种情况，比如我给了+90°让它进入蟹行模式，然后我在蟹行模式我去追一个相对于车左侧方向+40°的轨迹，那这时候就超过限制了，那我这是要做一个什么措施吗，比如我能想到的就是达到这个限位要么就停车打印说明到达限位，安全措施；要么就是我在追这个轨迹的过程中知道我大概可能超过限位，然后我提前停下，自转，调整为车头朝向为轨迹的方向，也就是自转调整到我车当前模式的初始姿态方向这种；你有什么好的建议吗，其他的建议吗，或者有什么可行的方法，因为实际的过程还是需要考虑这个限位的问题的
+
+
+
+
+
+
+// 9:49
+
+
+
+
+
+
+// 这个问题必须考虑，但“目标轮角超过 +120°”不一定意味着命令无法执行。对于舵轮，首先要利用“轮角反向、轮速反向”的等价关系，然后才考虑停车和车身重定向。
+// 1. +130° 不一定超过限位后就无法运动
+// 你的例子是：
+// 蟹行基准方向：+90°
+// 轨迹又向该方向增加：+40°
+// 期望运动方向：+130°
+// 舵轮限制：[-120°, +120°]
+// 直接使用：
+// 轮角：+130°
+// 轮速：+0.3 m/s
+// 确实超过了 +120°。
+// 但它有一个完全等价的表达：
+// 轮角：-50°
+// 轮速：-0.3 m/s
+// 因为：
+// (θ, v) ≡ (θ - 180°, -v)
+// 即：
+// (+130°, +0.3) ≡ (-50°, -0.3)
+// 虽然轮子的机械朝向和转速符号变了，但轮胎与地面的速度向量完全相同。
+// 所以第一层措施应当是：
+// 先尝试期望角度
+// 如果超过限位
+//     尝试角度±180°
+//     同时将轮速取反
+// 如果等价角度满足限位
+//     使用等价解
+// 否则才判定无法执行
+// 你当前的 CommonUsage.dll 中已经有类似的舵角反解逻辑，SendXYThSpeed() 返回 bool 也是为了告诉上层运动分解是否成功。
+// 2. 你的±120°结构在静态上覆盖了所有运动方向
+// 因为：
+// 可转范围总宽度 = 240°
+// 大于 180°。
+// 只要驱动轮允许正反转，那么对于任意二维速度方向，通常都能在下面两种表达中找到一个合法值：
+// (θ, +v)
+// (θ ± 180°, -v)
+// 因此单纯从“某一时刻目标速度方向”看，+130° 可以用 -50° 表达，不一定需要旋转车身。
+// 真正困难的是动态过程：
+// 当前轮角：+90°
+// 新目标等价轮角：-50°
+// 需要转动：-140°
+// 虽然最终角度合法，但舵轮需要转过很大的角度。在舵轮没有到位前，不能继续以正常速度驱动车辆，否则实际运动方向会与轨迹命令不一致。
+// 所以核心问题不是“有没有合法终态”，而是：
+// 从当前轮角切换到合法目标轮角的过程是否安全、连续。
+
+// 3. 建议采用四层处理策略
+// 第一层：等价舵角选择
+// 为每一个舵轮计算速度向量：
+// Vix = Vx - ω·yi
+// Viy = Vy + ω·xi
+// 然后：
+// θi = atan2(Viy, Vix)
+// si = sqrt(Vix² + Viy²)
+// 尝试候选解：
+// 候选1：(θi, si)
+// 候选2：(θi + 180°, -si)
+// 候选3：(θi - 180°, -si)
+// 过滤掉超过 [-120°, +120°] 的候选，再选择相对当前机械轮角转动最小的候选。
+// 这里计算机械转角距离时，不要简单把差值归一化成最短圆周角，因为舵轮不能无限旋转。对于有限机械区间，应当直接比较：
+// Math.Abs(candidateAngle - currentMechanicalAngle)
+// 第二层：软限位
+// 不要把 ±120° 当作正常工作的边界，建议设置软限位，例如：
+// 物理硬限位：[-120°, +120°]
+// 软件工作限位：[-105°, +105°]
+// 预警区域：[105°, 120°]
+// 具体余量需要实车标定，初期可以保留 10°～20°。
+// 状态可以分成：
+// |θ| < 100°       正常
+// 100°～105°       接近限位，开始规划
+// 105°～120°       限速或停止切换
+// >120°            禁止下发并报警
+// 软限位的意义是给控制器留出制动、舵轮响应和误差空间。
+// 第三层：舵轮转向期间限制驱动速度
+// 如果新的合法舵角与当前舵角相差很大，例如：
+// 当前：+90°
+// 目标：-50°
+// 差值：140°
+// 不应当马上正常行驶，应进入：
+// 减速
+//   ↓
+// 驱动速度降为0
+//   ↓
+// 舵轮转到-50°
+//   ↓
+// 确认所有舵轮到位
+//   ↓
+// 轮速使用负值重新起步
+// 可以设置阈值：
+// 舵角误差 < 2°：允许正常起步
+// 舵角误差 2°～10°：低速或保持
+// 舵角误差 > 10°：驱动速度为0
+// 实际阈值根据舵向响应速度标定。
+// 需要注意：把所有 Vx、Vy、Omega 同比例减小，只能降低运动速度，通常不会改变目标舵轮角度。因此“减速”能让切换更安全，但不一定能让超限角度重新变得合法。
+// 4. 车身重定向是更平滑的高级方案
+// 你提出的：
+// 提前停车，然后自转，调整车头朝向，再继续跟踪。
+
+// 这是正确而且很实用的方案，尤其适合连续轨迹跟踪。
+// 但不一定要把车头完全转到轨迹方向，应当根据当前驾驶模式选择车身朝向。
+// 假设轨迹方向相对当前车体为：
+// +130°
+// 如果希望继续保持蟹行模式，舵轮的优选方向是 +90°，那么只需要让车体逆时针自转：
+// 130° - 90° = 40°
+// 车体旋转 +40° 后：
+// 世界轨迹方向不变
+// 轨迹在新车体坐标系中的方向：+90°
+// 舵轮回到标准蟹行角度：+90°
+// 不需要让车头旋转完整的 130°。
+// 如果准备切换成正常前进模式，优选舵角是 0°，才需要让车头基本对准轨迹：
+// 130° - 0° = 130°
+// 因此可以使用下面的通用公式：
+// 期望车身朝向 =
+// 世界轨迹切线方向 - 当前模式的优选舵轮方向
+// 其中：
+// 正常模式优选角：0°
+// 左蟹行优选角：+90°
+// 右蟹行优选角：-90°
+// 对于蟹行轨迹，车身朝向参考值为：
+// yawReference =
+//     pathTangentWorldRadians -
+//     Math.PI / 2.0;
+// 这样车体会逐渐调整自身朝向，使轨迹方向始终落在舵轮舒适区，而不是一直顶着 ±120° 工作。
+// 5. 不建议到达硬限位才突然自转
+// 更好的方案是向前预测。
+// 轨迹跟踪本身通常有前视点，可以提前计算未来一段轨迹对应的目标舵角：
+// 当前时刻目标轮角
+// 未来0.2秒目标轮角
+// 未来0.5秒目标轮角
+// 未来1.0秒目标轮角
+// 一旦预测到未来会进入软限位，就提前触发车身重定向：
+// 正常跟踪
+//   ↓
+// 预测舵角接近软限位
+//   ↓
+// 逐渐降低线速度
+//   ↓
+// 停车
+//   ↓
+// 车身原地旋转
+//   ↓
+// 舵轮调整到模式初始方向
+//   ↓
+// 重新捕获轨迹
+//   ↓
+// 继续跟踪
+// 可以定义状态机：
+// internal enum TrackingConstraintState
+// {
+//     Tracking,
+//     ApproachingSteeringLimit,
+//     Stopping,
+//     ReorientingBody,
+//     AligningWheels,
+//     Resuming,
+//     Fault
+// }
+// 不要在一个控制周期内同时完成停车、车身旋转和恢复跟踪。
+// 6. LQR和限位处理应该分层
+// 以后写 LQR 时，不建议让基础 LQR 直接承担全部舵轮限位逻辑。
+// 推荐结构：
+// 轨迹参考
+//    ↓
+// LQR轨迹控制器
+// 输出期望 Vx、Vy、Omega
+//    ↓
+// 舵轮约束管理器
+// 等价角选择、软限位、预测、车身重定向
+//    ↓
+// ChassisCommand
+//    ↓
+// MultiWheelChassisAdapter
+//    ↓
+// SendXYThSpeed
+// 可以以后增加一个文件：
+// Shared/SteeringConstraintManager.cs
+// 它负责：
+// 根据 Vx、Vy、Omega 预测每个舵轮目标角。
+// 选择“舵角反向、轮速反向”的等价解。
+// 检查硬限位和软限位。
+// 判断是否需要停车对齐。
+// 给出是否需要重新规划车身朝向。
+// 记录哪个舵轮导致不可执行。
+// 而 FrameTransform2D、ChassisCommand 不应该包含这些车辆机械约束。
+// 7. 当前阶段建议先做到什么
+// 现在先实现最小安全版本：
+// 使用 SendXYThSpeed()，让底层优先选择等价舵角。
+// 检查 adapter.Send(command) 的返回值。
+// 返回 false 时立即停车，不能继续沿用上一条速度。
+// 打印每个舵轮的当前角度、目标角度和上下限。
+// 模式切换时先停车、预转舵轮、确认到位，再开放摇杆。
+// 先用较低速度测试 +90° → +110° → +120° → +130° 的过程。
+// 最低限度的发送逻辑应当是：
+// var success = adapter.Send(command, interval);
+
+// if (!success)
+// {
+//     adapter.Stop(interval);
+
+//     Console.WriteLine(
+//         "底盘命令分解失败，已停车：" +
+//         "可能存在舵角限制或舵轮无法完成方向切换。");
+// }
+// 然后再实现预测重定向。
+// 推荐的最终策略
+// 对于你的停车机器人，我建议采用：
+// 优先级1：使用θ±180°、轮速反向的等价解
+// 优先级2：选择机械转角变化较小的等价解
+// 优先级3：接近软限位时降低速度
+// 优先级4：提前规划车身朝向，保持舵轮在舒适角度
+// 优先级5：无法连续切换时停车、自转、重新对齐
+// 优先级6：仍不可行时保持停车并报警
+// 所以你的 +130° 示例不应该一开始就判故障。它可以先使用 -50° + 反向轮速。如果从当前 +90° 切换到 -50° 的动态过程不安全，再停车调整。若轨迹允许改变车身姿态，更理想的办法是让车身提前旋转 +40°，从而继续以标准 +90° 蟹行姿态跟踪，而不是一直逼近机械硬限位。
@@ -0,0 +1,95 @@
+{
+  "MaxManualSpeed": 0.3,
+  "MaxManualAngularSpeed": 45.0,
+  "test3": 24,
+  "trigger": 0,
+  "ManualArmSpeedFac": 5.0,
+  "CarNum": 1,
+  "MusicVolume": 5,
+  "LeftArmLowerPos": -76358,
+  "LeftArmUpperPos": 38224,
+  "RightArmLowerPos": -41976,
+  "RightArmUpperPos": 73519,
+  "MCUPort": "COM3",
+  "TransmitterSpeedUpperLimit": 1.0,
+  "TransmitterSpeedLowerLimit": 0.0,
+  "ThBiasLeftFront": -342.0,
+  "ThBiasLeftRear": -185.35,
+  "ThBiasRightFront": -58.5,
+  "ThBiasRightRear": -1198.85,
+  "TurnAccSpeed": 1000.0,
+  "MaxManualTheta": 45.0,
+  "IsDiffSteer": true,
+  "ManualThetaPow": 2.0,
+  "DiffSteerKp": 0.003,
+  "DiffSteerKi": 0.0,
+  "DiffSteerKd": 0.0,
+  "DiffSteerMaxI": 0.0,
+  "DiffSteerThresh": 0.3,
+  "DiffSteerDeadZone": 0.1,
+  "DiffSteerSpeedAcc": 1.0,
+  "Ghost_ActualSpeedLeftFront_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedLeftFront_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedLeftRear_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedLeftRear_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedRightFront_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedRightFront_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedRightRear_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedRightRear_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedLeftFrontLeft_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedLeftFrontLeft_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedLeftFrontRight_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedLeftFrontRight_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedLeftRearLeft_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedLeftRearLeft_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedLeftRearRight_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedLeftRearRight_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedRightFrontLeft_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedRightFrontLeft_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedRightFrontRight_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedRightFrontRight_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedRightRearLeft_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedRightRearLeft_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualSpeedRightRearRight_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualSpeedRightRearRight_GhostPIDUpdater_threshold": 200.0,
+  "Ghost_ActualThLeftFront_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualThLeftFront_GhostPIDUpdater_threshold": 5.0,
+  "Ghost_ActualThLeftRear_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualThLeftRear_GhostPIDUpdater_threshold": 5.0,
+  "Ghost_ActualThRightFront_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualThRightFront_GhostPIDUpdater_threshold": 5.0,
+  "Ghost_ActualThRightRear_GhostPIDUpdater_kp": 0.5,
+  "Ghost_ActualThRightRear_GhostPIDUpdater_threshold": 5.0,
+  "WheelAngleChangeLimit": 20.0,
+  "ElectricDebugMode": false,
+  "PostGPSToDetour": false,
+  "GPSBaseLongitude": 0.0,
+  "GPSBaseLatitude": 0.0,
+  "GPSInCarX": 0.0,
+  "GPSInCarY": 0.0,
+  "GPSInCarTh": 0.0,
+  "LowBatteryAlarmThreshold": 15.0,
+  "LowBatteryStopThreshold": 10.0,
+  "GhostMode": false,
+  "QrMode": false,
+  "GhostEnableSimBattery": false,
+  "GhostTimeBatteryFac": 600.0,
+  "ReductionRatio": 1.74,
+  "WheelDiameter": 165.0,
+  "CloseMusic": false,
+  "IOObstaclSlowFac": 0.5,
+  "ManualSpeed": 0.2,
+  "ManualRotateSpeed": 10.0,
+  "GamepadRotateSpeed": 1.0,
+  "RecoveryTime": 2000.0,
+  "ManualModeArea": 0,
+  "ManualModeIOArea": 1,
+  "UseLessTag": true,
+  "MaxAngularSpeed": 75.0,
+  "SafetyRelayShielded": false,
+  "SafetyRelayShieldedRleaseTime": 30,
+  "RotateIoArea": 16,
+  "ChargeMaxThreshold": 95.0,
+  "TransmitterCOMNum": "COM5",
+  "UseTransmitter": true
+}
@@ -0,0 +1,50 @@
+{
+  "WheelConfig": {
+    "LeftFront": {
+      "Position": {
+        "X": 525.0,
+        "Y": 200.0
+      },
+      "WheelDistance": 85.0,
+      "AngleLowerLimit": -128.0,
+      "AngleUpperLimit": 172.0,
+      "IsDiffWheel": true
+    },
+    "RightFront": {
+      "Position": {
+        "X": 525.0,
+        "Y": -200.0
+      },
+      "WheelDistance": 85.0,
+      "AngleLowerLimit": -128.0,
+      "AngleUpperLimit": 172.0,
+      "IsDiffWheel": true
+    },
+    "LeftRear": {
+      "Position": {
+        "X": -525.0,
+        "Y": 200.0
+      },
+      "WheelDistance": 85.0,
+      "AngleLowerLimit": -126.0,
+      "AngleUpperLimit": 174.0,
+      "IsDiffWheel": true
+    },
+    "RightRear": {
+      "Position": {
+        "X": -525.0,
+        "Y": -200.0
+      },
+      "WheelDistance": 85.0,
+      "AngleLowerLimit": -130.0,
+      "AngleUpperLimit": 170.0,
+      "IsDiffWheel": true
+    }
+  },
+  "MinimumTurningAngleForAckermann": 60.0,
+  "ControlPointRadius": 500.0,
+  "MaxSpeed": 1.0,
+  "AccPerSecond": 0.3,
+  "DeAccPerSecond": 1.0,
+  "MinTurnSpeedFac": 0.25
+}
@@ -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
+}
@@ -0,0 +1,365 @@
+需要处理，而且对横向误差、航向误差和速度估计都会有明显影响。检测本身不难，困难的是区分：
+
+```text
+单帧错误定位
+持续性的定位重定位/地图修正
+车辆真实的快速运动
+```
+
+对你这种低速停车机器人，可以先采用一套偏安全、容易验证的处理。
+
+## 定位跳变会造成什么影响
+
+假设Detour在50ms内突然跳了10cm：
+
+\[
+v=\frac{0.1}{0.05}=2m/s
+\]
+
+实际车辆可能只有 `0.3m/s`，但差分速度会产生 `2m/s` 的尖峰。
+
+对控制器还有三个直接影响：
+
+- 横向位置跳10cm，横向误差可能瞬间变化10cm。
+- 航向跳5°，航向误差会瞬间变化5°。
+- 全局轨迹投影可能跳到另一条临近或相交的轨迹线段。
+
+Stanley在低速时尤其敏感：
+
+\[
+\delta =
+e_\theta+
+\arctan\left(\frac{k e_y}{v+\varepsilon}\right)
+\]
+
+低速时分母较小，横向误差突然变大，会产生很大的转向指令。因此不能完全不处理。
+
+## 不建议直接低通掉定位跳变
+
+不要简单地对Detour位置做强低通：
+
+```text
+错误位置跳变
+→ 低通缓慢跟过去
+```
+
+这样虽然曲线看起来平滑，但控制器会在一段时间内使用滞后、虚构的位置，可能更加危险。
+
+更好的做法是：
+
+```text
+检测跳变
+→ 暂时不把该帧用于速度差分和控制
+→ 观察后续定位
+→ 判断是单帧异常还是持续重定位
+```
+
+## 第一层：运动学合理性检查
+
+将当前Detour位姿和上一次接受的位姿比较。
+
+位置变化：
+
+\[
+\Delta p=\sqrt{\Delta x^2+\Delta y^2}
+\]
+
+航向变化：
+
+\[
+\Delta\theta=
+|\operatorname{ShortestAngleDifference}|
+\]
+
+允许的最大变化量可以按照车辆物理能力计算：
+
+```csharp
+var maximumAllowedDistance =
+    maximumLinearSpeedMetersPerSecond *
+    deltaTimeSeconds +
+    positionJumpMarginMeters;
+
+var maximumAllowedHeadingChange =
+    maximumAngularSpeedRadiansPerSecond *
+    deltaTimeSeconds +
+    headingJumpMarginRadians;
+```
+
+然后判断：
+
+```csharp
+var positionJump =
+    displacementMeters >
+    maximumAllowedDistance;
+
+var headingJump =
+    headingChangeRadians >
+    maximumAllowedHeadingChange;
+```
+
+你当前小车最高约 `1.2m/s`，假设Detour更新周期为50ms：
+
+```text
+物理最大位移约为：
+1.2 × 0.05 = 0.06m
+```
+
+初期可以额外留出约 `0.03～0.05m` 的定位余量。不过这些阈值最终应根据实际Detour数据确定，不建议永久写死。
+
+## 第二层：不要立即接受异常帧
+
+检测到一个异常帧时，不要立刻改变车辆状态：
+
+```text
+上一正常位置 A
+异常位置 B
+下一帧又回到 A 附近
+```
+
+这种情况说明B很可能是单帧异常，应直接丢弃。
+
+如果后续连续若干帧都稳定在B附近：
+
+```text
+A → B → B附近 → B附近
+```
+
+这更可能是Detour发生了持续性的重定位。
+
+可以使用：
+
+```text
+连续2～3个新定位帧相互一致
+```
+
+作为重新接受定位的条件。
+
+## 第三层：重定位后必须重置速度估计
+
+如果确认新的定位是持续有效的，不能用：
+
+```text
+新位置B - 旧位置A
+```
+
+计算速度，因为A到B是定位修正，不是车辆真实运动。
+
+正确处理是：
+
+```csharp
+_velocityEstimator.Reset(
+    newPose,
+    currentTimestamp);
+```
+
+这会：
+
+- 将新位姿作为新的差分起点。
+- 清除之前的速度历史。
+- 重置三个低通滤波器。
+- 将速度暂时标记为无效或零。
+- 等下一次正常Detour更新后重新开始估计。
+
+## 第四层：控制器应该如何响应
+
+对于实车轨迹跟踪，建议状态分为：
+
+```text
+Valid        正常定位，可以控制
+Suspected    检测到疑似跳变
+Reacquiring  正在确认新的定位
+Stale        定位长时间没有更新
+```
+
+你的第一版不一定需要单独增加复杂枚举，但控制行为至少应该满足：
+
+```text
+正常：
+继续轨迹跟踪
+
+疑似单帧跳变：
+不使用异常帧更新状态
+短时间保持上一状态
+
+连续异常或定位超时：
+停车，不继续使用旧状态运动
+
+确认重定位：
+接受新位姿
+重置速度估计
+重新执行轨迹投影
+确认稳定后恢复控制
+```
+
+停车机器人速度低、场地有限，定位连续异常时停车比盲目继续跟踪更合适。
+
+## 还需要限制轨迹投影进度
+
+即使Detour跳变检测做了，轨迹投影也最好增加进度保护。
+
+当前投影器在整条轨迹上找最近点，如果轨迹自交，车辆可能从：
+
+```text
+当前第20段
+```
+
+突然投影到：
+
+```text
+第80段
+```
+
+后续可以增加一个 `TrajectoryProgressTracker`：
+
+```text
+正常时只在上次线段索引附近搜索
+例如 [previousIndex - 5, previousIndex + 20]
+
+定位重定位后：
+重新执行一次全局搜索
+```
+
+同时限制单周期允许的弧长进度变化：
+
+\[
+|\Delta s|
+\leq
+v_{\max}\Delta t+\text{margin}
+\]
+
+这样可以避免定位轻微波动导致轨迹进度突然跳段。
+
+## 在五个文件中的分工
+
+```text
+DetourVehicleStateProvider
+├─ 检查数值有效性
+├─ 检查定位是否过期
+├─ 检测重复帧
+├─ 检测位置/航向跳变
+├─ 暂存疑似新定位
+└─ 确认重定位后触发Reset
+
+VelocityEstimator2D
+├─ 正常位姿差分
+├─ 真实时间差计算
+├─ 速度滤波
+└─ 提供Reset()
+
+FirstOrderLowPassFilter
+└─ 提供Update()和Reset()
+
+VehicleState
+├─ 保存位姿和速度
+└─ 表示速度估计是否有效
+
+IVehicleStateProvider
+└─ 向控制器返回有效状态或读取失败
+```
+
+因此定位跳变的主要逻辑放在 `DetourVehicleStateProvider`，而不是低通滤波器。
+
+第一版建议先实现：
+
+1. 非有限值检查。
+2. 重复帧识别。
+3. 基于最大速度和最大角速度的跳变检测。
+4. 单帧异常拒绝。
+5. 连续2～3帧确认重定位。
+6. 重定位后重置速度估计。
+7. 长时间定位无效时通知控制器停车。
+
+这套方案足以保护当前Stanley和后续纵向控制，不需要现在就修改SLAM源码。
+
+
+已经给 [DetourVehicleStateProvider.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:233) 增加了基于上一有效速度的预测残差检测。
+
+现在采用两级判断：
+
+```text
+第一级：绝对物理运动边界
+第二级：上一状态恒速预测残差
+```
+
+预测公式为：
+
+\[
+x_{pred}=x_{last}+v_x\Delta t
+\]
+
+\[
+y_{pred}=y_{last}+v_y\Delta t
+\]
+
+\[
+\theta_{pred}=
+\theta_{last}+\omega\Delta t
+\]
+
+实测残差为：
+
+\[
+e_p=
+\sqrt{
+(x_{measured}-x_{pred})^2+
+(y_{measured}-y_{pred})^2
+}
+\]
+
+以及：
+
+\[
+e_\theta=
+\operatorname{ShortestDifference}
+(\theta_{measured},\theta_{pred})
+\]
+
+对应实现在：
+
+- [IsPredictionResidualPlausible()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:536)
+- [PredictPoseInWorld()](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs:573)
+
+默认阈值为：
+
+```csharp
+位置预测残差：0.04m
+航向预测残差：5°
+```
+
+没有采用最初考虑的30mm，是因为用现有数据离线统计后发现：
+
+| 位置残差阈值 | 触发次数 | 占有效更新比例 |
+|---:|---:|---:|
+| 30mm | 55次 | 1.24% |
+| 35mm | 28次 | 0.63% |
+| 40mm | 13次 | 0.29% |
+| 45mm | 7次 | 0.16% |
+| 50mm | 4次 | 0.09% |
+
+30mm容易把正常Detour波动也判成异常；40mm仍能识别现有数据中约49mm和66mm的明显跳变，同时误触发风险更低。
+
+当前判定逻辑：
+
+```csharp
+if (!isWithinPhysicalBoundary ||
+    !isWithinPredictionResidual)
+{
+    // 进入疑似重定位确认状态。
+}
+```
+
+另外同步修正了跳变恢复逻辑：
+
+- 发生预测残差跳变时，保存当时的预测位姿。
+- 后续定位必须回到预测位姿附近，才能认为是单帧异常后的正常恢复。
+- 不再使用较宽松的绝对物理范围立即放行。
+- 持续远端定位仍需满足3次观测和0.25秒，才按重定位接管。
+- 接管后重置速度估计，避免把坐标修正计算成车辆速度。
+
+第一帧或重定位后的速度还没有建立时，不启用预测残差检测，只使用物理极限检查，避免没有速度基准时误判。
+
+完整构建结果：
+
+- CommonUsage：0警告、0错误
+- MedullaAdapter：0警告、0错误
+- MultiWheelC：0警告、0错误
@@ -0,0 +1,34 @@
+单个停车机器人轨迹测试方案
+### 测试对象
+单台停车机器人（+50kg负载）
+### 测试曲线
+1. 直线：前进/后退2m、速度0.3m/s、起点终点静止
+2. 转弯：左转/右转组合前进/后退、转弯半径1m、曲率1.0、速度0.3m/s
+3. 原地自转：±90°/±180°、角速度10°/s、20°/s、起点终点静止
+
+### 评价指标
+横向误差 RMSE、最大横向误差；航向误差 RMSE、最大航向误差；速度误差 RMSE、最大速度偏差；
+角速度或转角指令的变化曲线；最终位置误差、最终航向误差
+
+### 展示形式
+理想轨迹与实际轨迹对比图、横向/航向误差随时间变化图、参考速度与实际速度对比图、角速度指令曲线
+
+### LQR调参策略
+归一化状态/控制量：一般Q、R初始选择对应控制量最大值的平方的倒数
+或者使用Bryson’s Rule来给 Q、R 一个很好的初始猜测
+贝叶斯优化在仿真中自动调节Q、R参数
+ALQR：在线估计最新参数实时重新求解
+----------------------------------------
+不调参：学习式
+
+### 杂项
+1. M层获取实际小车的速度信息与位置信息并保存、使用python可视化来量化跟踪误差
+2. 自行车模型改动、过于局限于阿克曼小车的运动学限制
+
+
+先用 Bryson’s Rule + 贝叶斯优化在仿真里把 Q/R 调到一个不错的基准。
+上实车时采用自适应 LQR：在线估计关键参数（尤其是轮胎刚度），实时更新 K。
+C# 实现的话：
+矩阵运算继续用 Math.NET
+贝叶斯优化可以调 Python 库，或者自己写简单版本
+在线参数估计（RLS）用 C# 写很轻松
@@ -0,0 +1,47 @@
+编译命令：
+powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
+
+
+1. TrajectoryPoint.cs       已完成
+2. Trajectory2D.cs          下一步
+3. TrajectoryProjection.cs  定义一次投影结果
+4. TrajectoryProjector.cs   实现连续线段投影
+5. TrajectoryBuilder.cs     原始离散点转标准轨迹
+
+1. VehicleState.cs
+        ↓
+2. FirstOrderLowPassFilter.cs
+        ↓
+3. VelocityEstimator2D.cs
+        ↓
+4. IVehicleStateProvider.cs
+        ↓
+5. DetourVehicleStateProvider.cs
+
+
+private const double LinearVelocityFilterTimeConstantSeconds =
+    0.15;
+
+private const double AngularVelocityFilterTimeConstantSeconds =
+    0.20;
+
+PathTrackingContext.cs
+LateralControlCommand.cs
+ILateralController.cs
+ILongitudinalController.cs
+GcpMotionCommand.cs
+AckermannGcpAllocator.cs
+StanleyLateralController.cs
+PidLongitudinalController.cs
+GcpCommandExecutor.cs
+ParkingGeometricController.cs
+
+
+
+把前后GCP转角分解成两个模态：
+共同转角 = (前GCP转角 + 后GCP转角) / 2
+差动转角 = (前GCP转角 - 后GCP转角) / 2
+
+
+
+