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

2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
@@ -93,7 +93,7 @@ namespace MultiWheelC.Control.Abstractions
        public double FeedforwardCurvaturePerMeter { get; }

        /// <summary>
-        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
        /// </summary>
        public double LateralErrorMeters =>
            Projection.LateralErrorMeters;
@@ -123,8 +123,9 @@ namespace MultiWheelC.Control.Lateral
                    context.HeadingErrorRadians,
                    MaximumHeadingCorrectionRadians);

+            // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
+            // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
            var commonAngleRadians =
-                travelDirection *
                crossTrackCorrectionRadians;
            var differentialAngleRadians =
                feedforwardAngleRadians +