feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -0,0 +1,138 @@
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供与坐标系无关的角度归一化、角度差和单位转换功能。
+    /// </summary>
+    public static class AngleMath
+    {
+        public const double TwoPi = 2.0 * Math.PI;
+
+        /// <summary>
+        /// 将弧度归一化到[-π, π)区间。
+        /// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。
+        /// </summary>
+        public static double NormalizeRadians(double angleRadians)
+        {
+            EnsureFinite(angleRadians, nameof(angleRadians));
+
+            var normalized = angleRadians % TwoPi;
+
+            if (normalized >= Math.PI)
+            {
+                normalized -= TwoPi;
+            }
+            else if (normalized < -Math.PI)
+            {
+                normalized += TwoPi;
+            }
+
+            return normalized == 0.0 ? 0.0 : normalized;
+        }
+
+        /// <summary>
+        /// 将角度归一化到[-180°, 180°)区间。
+        /// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。
+        /// </summary>
+        public static double NormalizeDegrees(double angleDegrees)
+        {
+            EnsureFinite(angleDegrees, nameof(angleDegrees));
+
+            var normalized = angleDegrees % 360.0;
+
+            if (normalized >= 180.0)
+            {
+                normalized -= 360.0;
+            }
+            else if (normalized < -180.0)
+            {
+                normalized += 360.0;
+            }
+
+            return normalized == 0.0 ? 0.0 : normalized;
+        }
+
+        /// <summary>
+        /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。
+        /// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。
+        /// </summary>
+        public static double ShortestDifferenceRadians(
+            double targetRadians,
+            double currentRadians)
+        {
+            EnsureFinite(targetRadians, nameof(targetRadians));
+            EnsureFinite(currentRadians, nameof(currentRadians));
+
+            return NormalizeRadians(targetRadians - currentRadians);
+        }
+
+        /// <summary>
+        /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。
+        /// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。
+        /// </summary>
+        public static double ShortestDifferenceDegrees(
+            double targetDegrees,
+            double currentDegrees)
+        {
+            EnsureFinite(targetDegrees, nameof(targetDegrees));
+            EnsureFinite(currentDegrees, nameof(currentDegrees));
+
+            return NormalizeDegrees(targetDegrees - currentDegrees);
+        }
+
+        /// <summary>
+        /// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
+        /// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
+        /// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
+        /// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
+        /// </summary>
+        public static double LerpRadians(
+            double startRadians,
+            double endRadians,
+            double ratio)
+        {
+            EnsureFinite(startRadians, nameof(startRadians));
+            EnsureFinite(endRadians, nameof(endRadians));
+            EnsureFinite(ratio, nameof(ratio));
+
+            var shortestDifference = ShortestDifferenceRadians(
+                endRadians,
+                startRadians);
+
+            return NormalizeRadians(
+                startRadians + ratio * shortestDifference);
+        }
+
+        /// <summary>
+        /// 将角度从度转换为弧度，不进行归一化。
+        /// </summary>
+        public static double DegreesToRadians(double angleDegrees)
+        {
+            EnsureFinite(angleDegrees, nameof(angleDegrees));
+            return angleDegrees * Math.PI / 180.0;
+        }
+
+        /// <summary>
+        /// 将角度从弧度转换为度，不进行归一化。
+        /// </summary>
+        public static double RadiansToDegrees(double angleRadians)
+        {
+            EnsureFinite(angleRadians, nameof(angleRadians));
+            return angleRadians * 180.0 / Math.PI;
+        }
+
+        /// <summary>
+        /// 验证角度是可用于计算的有限数值。
+        /// </summary>
+        private static void EnsureFinite(double angle, string parameterName)
+        {
+            if (double.IsNaN(angle) || double.IsInfinity(angle))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "角度必须是有限数值。");
+            }
+        }
+    }
+}
@@ -0,0 +1,165 @@
+// 车体、运动、车队坐标系之间的转换
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
+    /// 坐标系采用X向前、Y向左、逆时针为正的右手系。
+    /// </summary>
+    public static class FrameTransform2D
+    {
+        /// <summary>
+        /// 将角度归一化到[-π, π)范围。
+        /// </summary>
+        public static double NormalizeAngle(double angleRadians)
+        {
+            return AngleMath.NormalizeRadians(angleRadians);
+        }
+
+        /// <summary>
+        /// 计算从current到target的最短角度差。
+        /// 返回正值表示逆时针旋转。
+        /// </summary>
+        public static double ShortestAngleDifference(
+            double targetRadians,
+            double currentRadians)
+        {
+            return AngleMath.ShortestDifferenceRadians(
+                targetRadians,
+                currentRadians);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的点变换到目标坐标系。
+        /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
+        /// </summary>
+        public static Point2D TransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                sourcePoseInTarget.XMeters +
+                cos * pointInSource.XMeters -
+                sin * pointInSource.YMeters,
+
+                sourcePoseInTarget.YMeters +
+                sin * pointInSource.XMeters +
+                cos * pointInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 将目标坐标系中的点反向变换到源坐标系。
+        /// </summary>
+        public static Point2D InverseTransformPoint(
+            Pose2D sourcePoseInTarget,
+            Point2D pointInTarget)
+        {
+            var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
+
+            var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
+
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * dx + sin * dy,
+                -sin * dx + cos * dy);
+        }
+
+        /// <summary>
+        /// 将源坐标系中的向量旋转到目标坐标系。
+        /// 向量没有位置，因此不叠加平移量。
+        /// </summary>
+        public static Point2D TransformVector(
+            Pose2D sourcePoseInTarget,
+            Point2D vectorInSource)
+        {
+            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
+            var sin = Math.Sin(sourcePoseInTarget.YawRadians);
+
+            return new Point2D(
+                cos * vectorInSource.XMeters -
+                sin * vectorInSource.YMeters,
+
+                sin * vectorInSource.XMeters +
+                cos * vectorInSource.YMeters);
+        }
+
+        /// <summary>
+        /// 组合两级坐标变换。
+        /// parentFromMiddle表示middle在parent中的位姿；
+        /// middleFromChild表示child在middle中的位姿；
+        /// 返回child在parent中的位姿。
+        /// </summary>
+        public static Pose2D Compose(
+            Pose2D parentFromMiddle,
+            Pose2D middleFromChild)
+        {
+            var childPositionInParent = TransformPoint(
+                parentFromMiddle,
+                middleFromChild.Position);
+
+            return new Pose2D(
+                childPositionInParent.XMeters,
+                childPositionInParent.YMeters,
+                NormalizeAngle(
+                    parentFromMiddle.YawRadians +
+                    middleFromChild.YawRadians));
+        }
+
+        /// <summary>
+        /// 对坐标变换求逆。
+        /// 输入child在parent中的位姿，返回parent在child中的位姿。
+        /// </summary>
+        public static Pose2D Inverse(Pose2D childPoseInParent)
+        {
+            var cos = Math.Cos(childPoseInParent.YawRadians);
+            var sin = Math.Sin(childPoseInParent.YawRadians);
+
+            return new Pose2D(
+                -cos * childPoseInParent.XMeters -
+                sin * childPoseInParent.YMeters,
+
+                sin * childPoseInParent.XMeters -
+                cos * childPoseInParent.YMeters,
+
+                NormalizeAngle(
+                    -childPoseInParent.YawRadians));
+        }
+
+
+        /// <summary>
+        /// 将源坐标系中的位姿变换到目标坐标系。
+        /// </summary>
+        public static Pose2D TransformPose(
+            Pose2D sourcePoseInTarget,
+            Pose2D poseInSource)
+        {
+            return Compose(sourcePoseInTarget, poseInSource);
+        }
+
+        /// <summary>
+        /// 转换同一物理参考点处的速度表达坐标系。
+        /// 只旋转线速度，角速度保持不变。
+        /// </summary>
+        public static Twist2D TransformTwistAtSamePoint(
+            Pose2D sourcePoseInTarget,
+            Twist2D twistInSource)
+        {
+            var linearVelocityInTarget = TransformVector(
+                sourcePoseInTarget,
+                new Point2D(
+                    twistInSource.VxMetersPerSecond,
+                    twistInSource.VyMetersPerSecond));
+
+            return new Twist2D(
+                linearVelocityInTarget.XMeters,
+                linearVelocityInTarget.YMeters,
+                twistInSource.OmegaRadiansPerSecond);
+        }
+    }
+}
@@ -0,0 +1,61 @@
+
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供与具体业务和坐标系无关的基础插值功能。
+    /// </summary>
+    public static class InterpolationMath
+    {
+        /// <summary>
+        /// 将插值比例限制到[0, 1]闭区间。
+        /// </summary>
+        public static double Clamp01(double value)
+        {
+            EnsureFinite(value, nameof(value));
+
+            if (value <= 0.0)
+            {
+                return 0.0;
+            }
+
+            if (value >= 1.0)
+            {
+                return 1.0;
+            }
+
+            return value;
+        }
+
+        /// <summary>
+        /// 对两个标量执行线性插值。
+        /// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio，
+        /// 因此也支持区间外的线性外插。
+        /// </summary>
+        public static double Lerp(
+            double start,
+            double end,
+            double ratio)
+        {
+            EnsureFinite(start, nameof(start));
+            EnsureFinite(end, nameof(end));
+            EnsureFinite(ratio, nameof(ratio));
+
+            return start + ratio * (end - start);
+        }
+
+        /// <summary>
+        /// 验证输入是可用于插值计算的有限数值。
+        /// </summary>
+        private static void EnsureFinite(double value, string parameterName)
+        {
+            if (double.IsNaN(value) || double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "插值参数必须是有限数值。");
+            }
+        }
+    }
+}