实现蟹行轨迹跟踪测试并优化底盘XYTh与原地旋转舵角控制

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-07-29 18:21:29 +08:00
parent 583a7d00ca
commit 7e05ff098e
33 changed files with 1956 additions and 98 deletions
@@ -693,6 +693,89 @@ namespace CommonUsage.Chassis
            return true;
        }

+		/// <summary>
+		/// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。
+		/// 只下发舵角，不下发驱动速度。
+		/// </summary>
+		public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f)
+		{
+			if (!Valid)
+				return FailMotionDecomposition(
+					"PrepareRotateWheels",
+					"invalid chassis",
+					null);
+
+			if (float.IsNaN(alignmentToleranceDegrees) ||
+			    float.IsInfinity(alignmentToleranceDegrees) ||
+			    alignmentToleranceDegrees < 0.0f)
+				throw new ArgumentOutOfRangeException(
+					nameof(alignmentToleranceDegrees),
+					"自转舵轮到位容差必须是非负有限值。");
+
+			if (_steerWheels.Count == 0)
+				return FailMotionDecomposition(
+					"PrepareRotateWheels",
+					"no steer wheels",
+					null);
+
+			// 模式切换期间必须保持驱动轮停止。
+			PredefinedDriveStop();
+
+			var targetAngles = new float[_steerWheels.Count];
+			var directions = new int[_steerWheels.Count];
+
+			// 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。
+			for (var i = 0; i < _steerWheels.Count; i++)
+			{
+				var wheel = _steerWheels[i];
+				var px = (double)wheel.Position.X;
+				var py = (double)wheel.Position.Y;
+
+				// 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。
+				var tangentDegrees =
+					(float)(Math.Atan2(px, -py) /
+					        Math.PI * 180.0);
+				tangentDegrees = CommonMath.ThDiff(
+					tangentDegrees,
+					wheel.ZeroDirection);
+
+				if (!TryResolveWheelAngle(
+					    i,
+					    tangentDegrees,
+					    "PrepareRotateWheels",
+					    out targetAngles[i],
+					    out directions[i],
+					    out var reason))
+					return FailMotionDecomposition(
+						"PrepareRotateWheels",
+						reason,
+						null);
+			}
+
+			for (var i = 0; i < _steerWheels.Count; i++)
+			{
+				_wheelDirs[i] = directions[i];
+				SendTh(i, targetAngles[i]);
+			}
+
+			var allAligned = true;
+			for (var i = 0; i < _steerWheels.Count; i++)
+			{
+				var actualAngle = _steerWheels[i].ReadAngle();
+				var angleError = targetAngles[i] - actualAngle;
+
+				// 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。
+				if (float.IsNaN(actualAngle) ||
+				    float.IsInfinity(actualAngle) ||
+				    Math.Abs(angleError) > alignmentToleranceDegrees)
+					allAligned = false;
+			}
+
+			LastRotateAligned = allAligned;
+			LastMotionDecomposeFailureReason = "";
+			return true;
+		}
+
 		/// <summary>
 		/// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
 		/// </summary>
@@ -781,14 +864,19 @@ namespace CommonUsage.Chassis
            var maxDth = 0f;
            for (var i = 0; i < _steerWheels.Count; ++i)
            {
-                var dth = Math.Abs(CommonMath.ThDiff(ths[i], _steerWheels[i].ReadAngle()));
+                var actualAngle = _steerWheels[i].ReadAngle();
+                var dth = Math.Abs(ths[i] - actualAngle);
                maxDth = Math.Max(maxDth, dth);
-                slowFac = Math.Min(slowFac, CommonMath.gaussmf(dth, rotSync, 0));
-     //            if (dth > 5)
-     //            {
-					// allWheelAligned = false;
-					// break;
-     //            }
+                slowFac = Math.Min(
+                    slowFac,
+                    CommonMath.gaussmf(
+                        dth,
+                        Math.Max(SteeringAlignmentSigmaDegrees, 0.1f),
+                        0));
+                if (float.IsNaN(actualAngle) ||
+                    float.IsInfinity(actualAngle) ||
+                    dth > 2.0f)
+                    allWheelAligned = false;
            }
            LastRotateAligned = allWheelAligned; // 供上层做积分抗饱和

@@ -953,8 +1041,6 @@ namespace CommonUsage.Chassis
        {
            var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000;
            var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000;
-            Hedingben.ToastText($"vRot:({vRotX:F3},{vRotY:F3}) pos:({pos.X:F1},{pos.Y:F1})",
-                $"SendXYThSpeed-VectorVelocity{i}");
            return new Vector2(vx + vRotX, vy + vRotY);
        }
 		/// <summary>
@@ -971,38 +1057,61 @@ namespace CommonUsage.Chassis
            return ((float)(Math.Atan2(v.Y, v.X) / Math.PI * 180), v.Length());
        }

-        private float w = 0;
-        private float wAcc = 0.1f;
-        private float rotSync = 3f;
+        /// <summary>
+        /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。
+        /// </summary>
+        public float SteeringAlignmentSigmaDegrees { get; set; } = 8f;
        private bool XYThActive = false;
+        private bool _xyThWheelsAligned = false;
+        private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue;

        public bool SendXYThSpeed(float vx, float vy, float vth, TimeSpan? deltaTime = null)
        {
            if (!Valid) return FailMotionDecomposition("SendXYThSpeed", "invalid chassis", deltaTime);
+
+            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
+            {
+                RampStop(deltaTime);
+                XYThActive = false;
+                _xyThWheelsAligned = false;
+                GoingActive = false;
+                RotatingActive = false;
+                LastMotionDecomposeFailureReason = "";
+                return true;
+            }
+
 			if (!XYThActive)
            {
                ResetMotionState();
-                w = 0;
+                _xyThWheelsAligned = false;
            }
 			XYThActive = true;
            GoingActive = false;
            RotatingActive = false;

-            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
-            {
-                RampStop(deltaTime);
-                LastMotionDecomposeFailureReason = "";
-                return true;
-            }
-   
-            w = w + wAcc;
-            if (w > 1) w = 1;
            float[] sendSpeed = new float[_steerWheels.Count];
+            var allWheelsAligned = true;
+            var maximumAngleError = 0f;
+            const float initialAlignmentToleranceDegrees = 2f;
+            var writeDiagnostics =
+                Debug &&
+                (DateTime.Now - _xyThDiagnosticsLastTime)
+                    .TotalMilliseconds >= 250.0;
+
            for (var i = 0; i < _steerWheels.Count; i++)
            {
                var sw = _steerWheels[i];
                var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i);
                var actualTh = sw.ReadAngle();
+                if (float.IsNaN(actualTh) ||
+                    float.IsInfinity(actualTh))
+                {
+                    return FailMotionDecomposition(
+                        "SendXYThSpeed",
+                        $"wheel {i} angle feedback is invalid: {actualTh}",
+                        deltaTime);
+                }
+
                if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), "SendXYThSpeed",
                        out var useAngle, out var dir, out var resolveReason))
                    return FailMotionDecomposition("SendXYThSpeed", resolveReason, deltaTime);
@@ -1012,12 +1121,53 @@ namespace CommonUsage.Chassis
                sendSpeed[i] = speed;
                if (speed!=0)
                    SendTh(i, useAngle);
-                w = (float)Math.Min(w, CommonMath.gaussmf(CommonMath.ThDiff(actualTh, _sendAngle[i]), rotSync, 0));

-                Hedingben.ToastText($"w:{w:F2} s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}", $"SendXYThSpeed-{i}");
+                // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。
+                var angleError =
+                    Math.Abs(_sendAngle[i] - actualTh);
+                maximumAngleError =
+                    Math.Max(maximumAngleError, angleError);
+                if (angleError >
+                    initialAlignmentToleranceDegrees)
+                {
+                    allWheelsAligned = false;
+                }
+
+                if (writeDiagnostics)
+                {
+                    Hedingben.ToastText(
+                        $"ready:{_xyThWheelsAligned} err:{angleError:F1} " +
+                        $"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}",
+                        $"SendXYThSpeed-{i}");
+                }
            }
+
+            // 仅在一段XYTh运动刚开始时等待舵轮到位。
+            // 连续运动开始后，正常改变vx/vy/vth时允许舵轮边转、车辆边走，
+            // 避免每次打方向都重新把驱动速度压到零。
+            if (!_xyThWheelsAligned &&
+                allWheelsAligned)
+            {
+                _xyThWheelsAligned = true;
+            }
+
+            var driveEnabled = _xyThWheelsAligned;
            for (var i = 0; i < _steerWheels.Count; i++)
-                AccumulateSpeed(i, w * sendSpeed[i],false,new Vector2(0f,0f), deltaTime);
+                AccumulateSpeed(
+                    i,
+                    driveEnabled ? sendSpeed[i] : 0f,
+                    false,
+                    new Vector2(0f,0f),
+                    deltaTime);
+
+            if (writeDiagnostics)
+            {
+                _xyThDiagnosticsLastTime = DateTime.Now;
+                Hedingben.ToastText(
+                    $"ready:{_xyThWheelsAligned} maxErr:{maximumAngleError:F1} " +
+                    $"cmd:({vx:F3},{vy:F3},{vth:F1})",
+                    "SendXYThSpeed-alignment");
+            }
            //todo 计算rotCenter填入
            LastMoveTime = DateTime.Now;
            LastMotionDecomposeFailureReason = "";