diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index fd9ab55..93900a2 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
index 3f29033..1713d12 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
index 9a4547b..3ff5098 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC/Control/Abstractions/LateralControlCommand.cs b/MultiWheelC/Control/Abstractions/LateralControlCommand.cs
index 2198a8d..cb170c9 100644
--- a/MultiWheelC/Control/Abstractions/LateralControlCommand.cs
+++ b/MultiWheelC/Control/Abstractions/LateralControlCommand.cs
@@ -3,37 +3,62 @@ using System;
namespace MultiWheelC.Control.Abstractions
{
///
- /// 表示横向控制器生成的车体中心目标曲率命令。
+ /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
///
public readonly struct LateralControlCommand
{
///
- /// 创建统一使用SI单位和左转为正约定的横向控制命令。
+ /// 创建前、后GCP目标转角命令。
///
public LateralControlCommand(
- double targetCurvaturePerMeter)
+ double frontGcpAngleRadians,
+ double rearGcpAngleRadians)
{
EnsureFinite(
- targetCurvaturePerMeter,
- nameof(targetCurvaturePerMeter));
+ frontGcpAngleRadians,
+ nameof(frontGcpAngleRadians));
+ EnsureFinite(
+ rearGcpAngleRadians,
+ nameof(rearGcpAngleRadians));
- TargetCurvaturePerMeter =
- targetCurvaturePerMeter;
+ FrontGcpAngleRadians =
+ frontGcpAngleRadians;
+ RearGcpAngleRadians =
+ rearGcpAngleRadians;
}
///
- /// 获取车体中心目标轨迹曲率，单位为1/m，左转为正、右转为负。
+ /// 获取前GCP目标转角，单位为rad，逆时针为正。
///
- public double TargetCurvaturePerMeter { get; }
+ public double FrontGcpAngleRadians { get; }
///
- /// 创建保持直线行驶的零曲率命令。
+ /// 获取后GCP目标转角，单位为rad，逆时针为正。
+ ///
+ public double RearGcpAngleRadians { get; }
+
+ ///
+ /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
+ ///
+ public double CommonAngleRadians =>
+ (FrontGcpAngleRadians +
+ RearGcpAngleRadians) / 2.0;
+
+ ///
+ /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
+ ///
+ public double DifferentialAngleRadians =>
+ (FrontGcpAngleRadians -
+ RearGcpAngleRadians) / 2.0;
+
+ ///
+ /// 创建前后GCP均保持车头方向的直线命令。
///
public static LateralControlCommand Straight =>
- new LateralControlCommand(0.0);
+ new LateralControlCommand(0.0, 0.0);
///
- /// 检查横向曲率命令是否为有限值。
+ /// 检查GCP目标转角是否为有限值。
///
private static void EnsureFinite(
double value,
@@ -44,7 +69,7 @@ namespace MultiWheelC.Control.Abstractions
{
throw new ArgumentOutOfRangeException(
parameterName,
- "横向控制目标曲率必须是有限值。");
+ "GCP目标转角必须是有限值。");
}
}
}
diff --git a/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs b/MultiWheelC/Control/Allocation/GcpCommandAllocator.cs
similarity index 51%
rename from MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs
rename to MultiWheelC/Control/Allocation/GcpCommandAllocator.cs
index bf79863..77ccdca 100644
--- a/MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs
+++ b/MultiWheelC/Control/Allocation/GcpCommandAllocator.cs
@@ -4,57 +4,36 @@ using MultiWheelC.Control.Abstractions;
namespace MultiWheelC.Control.Allocation
{
///
- /// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。
+ /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
///
- public sealed class AckermannGcpAllocator
+ public sealed class GcpCommandAllocator
{
///
- /// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。
+ /// 创建使用指定前后GCP最大转角的命令分配器。
///
- public AckermannGcpAllocator(
- double controlPointRadiusMeters,
- double maximumGcpAngleRadians)
+ public GcpCommandAllocator(double maximumGcpAngleRadians)
{
- EnsureFinitePositive(
- controlPointRadiusMeters,
- nameof(controlPointRadiusMeters));
EnsureFinitePositive(
maximumGcpAngleRadians,
nameof(maximumGcpAngleRadians));
- if (maximumGcpAngleRadians >=
- Math.PI / 2.0)
+ if (maximumGcpAngleRadians >= Math.PI / 2.0)
{
throw new ArgumentOutOfRangeException(
nameof(maximumGcpAngleRadians),
- "最大GCP转角必须小于π/2，避免曲率换算出现奇异值。");
+ "最大GCP转角必须小于π/2。");
}
- ControlPointRadiusMeters =
- controlPointRadiusMeters;
- MaximumGcpAngleRadians =
- maximumGcpAngleRadians;
+ MaximumGcpAngleRadians = maximumGcpAngleRadians;
}
- ///
- /// 获取车体中心到前、后GCP的距离，单位为m。
- ///
- public double ControlPointRadiusMeters { get; }
-
///
/// 获取前后GCP允许的最大转角绝对值，单位为rad。
///
public double MaximumGcpAngleRadians { get; }
///
- /// 获取当前GCP几何和转角限制允许的最大车体中心曲率，单位为1/m。
- ///
- public double MaximumCurvaturePerMeter =>
- Math.Tan(MaximumGcpAngleRadians) /
- ControlPointRadiusMeters;
-
- ///
- /// 将纵向命令速度和车体中心目标曲率分配为前后GCP运动命令。
+ /// 将纵向速度和前后GCP转角组合为底盘运动命令。
///
public GcpMotionCommand Allocate(
double speedMetersPerSecond,
@@ -64,19 +43,12 @@ namespace MultiWheelC.Control.Allocation
speedMetersPerSecond,
nameof(speedMetersPerSecond));
- var limitedCurvaturePerMeter =
- Clamp(
- lateralCommand
- .TargetCurvaturePerMeter,
- -MaximumCurvaturePerMeter,
- MaximumCurvaturePerMeter);
-
- var frontAngleRadians =
- Math.Atan(
- limitedCurvaturePerMeter *
- ControlPointRadiusMeters);
- var rearAngleRadians =
- -frontAngleRadians;
+ var frontAngleRadians = ClampSymmetric(
+ lateralCommand.FrontGcpAngleRadians,
+ MaximumGcpAngleRadians);
+ var rearAngleRadians = ClampSymmetric(
+ lateralCommand.RearGcpAngleRadians,
+ MaximumGcpAngleRadians);
return new GcpMotionCommand(
speedMetersPerSecond,
@@ -85,16 +57,15 @@ namespace MultiWheelC.Control.Allocation
}
///
- /// 将数值限制在指定闭区间内。
+ /// 将数值按正负对称方式限制在指定绝对值内。
///
- private static double Clamp(
+ private static double ClampSymmetric(
double value,
- double minimum,
- double maximum)
+ double maximumAbsoluteValue)
{
return Math.Max(
- minimum,
- Math.Min(maximum, value));
+ -maximumAbsoluteValue,
+ Math.Min(maximumAbsoluteValue, value));
}
///
diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
index 997c971..ba123b3 100644
--- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs
+++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
@@ -33,7 +33,7 @@ namespace MultiWheelC.Control.Execution
private readonly IVehicleStateProvider _stateProvider;
private readonly ILateralController _lateralController;
private readonly ILongitudinalController _longitudinalController;
- private readonly AckermannGcpAllocator _gcpAllocator;
+ private readonly GcpCommandAllocator _gcpAllocator;
private readonly GcpCommandExecutor _commandExecutor;
private Trajectory2D _trajectory;
@@ -45,7 +45,7 @@ namespace MultiWheelC.Control.Execution
IVehicleStateProvider stateProvider,
ILateralController lateralController,
ILongitudinalController longitudinalController,
- AckermannGcpAllocator gcpAllocator,
+ GcpCommandAllocator gcpAllocator,
GcpCommandExecutor commandExecutor,
double finishDistanceMeters = 0.03,
double finishSpeedMetersPerSecond = 0.02,
diff --git a/MultiWheelC/Control/Lateral/StanleyLateralController.cs b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
index 939b897..26c64e1 100644
--- a/MultiWheelC/Control/Lateral/StanleyLateralController.cs
+++ b/MultiWheelC/Control/Lateral/StanleyLateralController.cs
@@ -4,22 +4,23 @@ using MultiWheelC.Control.Abstractions;
namespace MultiWheelC.Control.Lateral
{
///
- /// 使用参考曲率前馈、航向误差和横向误差计算车体中心目标曲率。
+ /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
///
public sealed class StanleyLateralController : ILateralController
{
- private const double MaximumMathematicalAngleRadians =
- Math.PI / 2.0 - 1e-3;
-
///
- /// 创建使用指定GCP几何、Stanley增益和低速保护参数的横向控制器。
+ /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
///
public StanleyLateralController(
double controlPointRadiusMeters,
double crossTrackGainPerSecond,
double headingErrorGain,
double minimumSpeedMetersPerSecond,
- bool useActualSpeedForGain = true)
+ bool useActualSpeedForGain = true,
+ double maximumCrossTrackCorrectionRadians =
+ 10.0 * Math.PI / 180.0,
+ double maximumHeadingCorrectionRadians =
+ 10.0 * Math.PI / 180.0)
{
EnsureFinitePositive(
controlPointRadiusMeters,
@@ -33,12 +34,22 @@ namespace MultiWheelC.Control.Lateral
EnsureFinitePositive(
minimumSpeedMetersPerSecond,
nameof(minimumSpeedMetersPerSecond));
+ EnsureFinitePositive(
+ maximumCrossTrackCorrectionRadians,
+ nameof(maximumCrossTrackCorrectionRadians));
+ EnsureFinitePositive(
+ maximumHeadingCorrectionRadians,
+ nameof(maximumHeadingCorrectionRadians));
ControlPointRadiusMeters = controlPointRadiusMeters;
CrossTrackGainPerSecond = crossTrackGainPerSecond;
HeadingErrorGain = headingErrorGain;
MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
UseActualSpeedForGain = useActualSpeedForGain;
+ MaximumCrossTrackCorrectionRadians =
+ maximumCrossTrackCorrectionRadians;
+ MaximumHeadingCorrectionRadians =
+ maximumHeadingCorrectionRadians;
}
///
@@ -62,12 +73,22 @@ namespace MultiWheelC.Control.Lateral
public double MinimumSpeedMetersPerSecond { get; }
///
- /// 获取是否优先使用Detour估算的实际纵向速度计算横向误差项。
+ /// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
///
public bool UseActualSpeedForGain { get; }
///
- /// 根据参考曲率、航向误差和横向误差计算车体中心目标曲率。
+ /// 获取横向误差共同转角分量的最大绝对值，单位为rad。
+ ///
+ public double MaximumCrossTrackCorrectionRadians { get; }
+
+ ///
+ /// 获取航向误差差动转角分量的最大绝对值，单位为rad。
+ ///
+ public double MaximumHeadingCorrectionRadians { get; }
+
+ ///
+ /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
///
public LateralControlCommand Compute(
PathTrackingContext context)
@@ -78,33 +99,40 @@ namespace MultiWheelC.Control.Lateral
MinimumSpeedMetersPerSecond);
var travelDirection = SelectTravelDirection(context);
- // 参考曲率提供前馈；没有跟踪误差时也能沿曲线行驶。
+ // 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
var feedforwardAngleRadians = Math.Atan(
context.ReferenceCurvaturePerMeter *
ControlPointRadiusMeters);
- // 轨迹位于车辆左侧时横向误差为正，对应正的左转修正。
- var crossTrackCorrectionRadians = Math.Atan(
- CrossTrackGainPerSecond *
- context.LateralErrorMeters /
- speedMagnitude);
+ // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
+ var crossTrackCorrectionRadians =
+ ClampSymmetric(
+ Math.Atan(
+ CrossTrackGainPerSecond *
+ context.LateralErrorMeters /
+ speedMagnitude),
+ MaximumCrossTrackCorrectionRadians);
- // 倒车时需要反转反馈修正方向；参考曲率前馈仍由轨迹本身决定。
- var feedbackAngleRadians = travelDirection *
- (HeadingErrorGain * context.HeadingErrorRadians +
- crossTrackCorrectionRadians);
+ // 航向误差生成前后反向的差动转角，只负责调整车身朝向。
+ var headingCorrectionRadians =
+ ClampSymmetric(
+ HeadingErrorGain *
+ context.HeadingErrorRadians,
+ MaximumHeadingCorrectionRadians);
- // 这里只避开tan奇点，实际GCP机械限制由AckermannGcpAllocator处理。
- var targetEquivalentAngleRadians = Clamp(
- feedforwardAngleRadians + feedbackAngleRadians,
- -MaximumMathematicalAngleRadians,
- MaximumMathematicalAngleRadians);
- var targetCurvaturePerMeter = Math.Tan(
- targetEquivalentAngleRadians) /
- ControlPointRadiusMeters;
+ var commonAngleRadians =
+ travelDirection *
+ crossTrackCorrectionRadians;
+ var differentialAngleRadians =
+ feedforwardAngleRadians +
+ travelDirection *
+ headingCorrectionRadians;
return new LateralControlCommand(
- targetCurvaturePerMeter);
+ commonAngleRadians +
+ differentialAngleRadians,
+ commonAngleRadians -
+ differentialAngleRadians);
}
///
@@ -115,7 +143,7 @@ namespace MultiWheelC.Control.Lateral
}
///
- /// 选择Stanley横向误差项使用的实际速度或旧版参考速度。
+ /// 选择Stanley横向误差项使用的实际速度或参考速度。
///
private double SelectSpeedForGain(
PathTrackingContext context)
@@ -131,7 +159,7 @@ namespace MultiWheelC.Control.Lateral
}
///
- /// 根据有符号参考速度确定前进或倒车的反馈修正方向。
+ /// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
///
private static double SelectTravelDirection(
PathTrackingContext context)
@@ -158,16 +186,15 @@ namespace MultiWheelC.Control.Lateral
}
///
- /// 将数值限制在指定闭区间内。
+ /// 将数值按正负对称方式限制在指定绝对值内。
///
- private static double Clamp(
+ private static double ClampSymmetric(
double value,
- double minimum,
- double maximum)
+ double maximumAbsoluteValue)
{
return Math.Max(
- minimum,
- Math.Min(maximum, value));
+ -maximumAbsoluteValue,
+ Math.Min(maximumAbsoluteValue, value));
}
///
@@ -183,7 +210,7 @@ namespace MultiWheelC.Control.Lateral
{
throw new ArgumentOutOfRangeException(
parameterName,
- "Stanley控制器的几何尺寸和最小速度必须是正有限值。");
+ "Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
}
}
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index 8c47315..d9269d3 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -47,7 +47,7 @@ namespace MultiWheelC
///
/// 获取或设置参考速度减速度，单位为m/s²。
///
- public double DecelerationMetersPerSecondSquared = 0.12;
+ public double DecelerationMetersPerSecondSquared = 0.10;
///
/// 获取或设置离散轨迹点间距，单位为m。
diff --git a/MultiWheelC/Movements/MotionPlanExecutor.cs b/MultiWheelC/Movements/MotionPlanExecutor.cs
index 5c5b69e..f1f4dcf 100644
--- a/MultiWheelC/Movements/MotionPlanExecutor.cs
+++ b/MultiWheelC/Movements/MotionPlanExecutor.cs
@@ -173,7 +173,12 @@ namespace MultiWheelC
foreach (var keepRunning in movement.Get())
{
- yield return keepRunning;
+ if (!keepRunning)
+ {
+ break;
+ }
+
+ yield return true;
}
continue;
@@ -219,7 +224,12 @@ namespace MultiWheelC
foreach (var keepRunning in movement.Get())
{
- yield return keepRunning;
+ if (!keepRunning)
+ {
+ break;
+ }
+
+ yield return true;
}
continue;
@@ -228,6 +238,9 @@ namespace MultiWheelC
throw new NotSupportedException(
$"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
}
+
+ // 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
+ yield return false;
}
}
}
diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
index 553f6c7..4ec7419 100644
--- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
@@ -55,6 +55,18 @@ namespace MultiWheelC
///
public bool StanleyUsesActualSpeed = true;
+ ///
+ /// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
+ ///
+ public double MaximumCrossTrackCorrectionRadians =
+ AngleMath.DegreesToRadians(10.0);
+
+ ///
+ /// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
+ ///
+ public double MaximumHeadingCorrectionRadians =
+ AngleMath.DegreesToRadians(10.0);
+
///
/// 纵向速度外环比例增益。
///
@@ -163,7 +175,9 @@ namespace MultiWheelC
StanleyCrossTrackGainPerSecond,
StanleyHeadingErrorGain,
StanleyMinimumSpeedMetersPerSecond,
- StanleyUsesActualSpeed);
+ StanleyUsesActualSpeed,
+ MaximumCrossTrackCorrectionRadians,
+ MaximumHeadingCorrectionRadians);
var longitudinalController =
new PidLongitudinalController(
LongitudinalKp,
@@ -173,8 +187,7 @@ namespace MultiWheelC
MaximumCommandSpeedMetersPerSecond,
LongitudinalSpeedErrorDeadbandMetersPerSecond);
var gcpAllocator =
- new AckermannGcpAllocator(
- controlPointRadiusMeters,
+ new GcpCommandAllocator(
MaximumGcpAngleRadians);
var commandExecutor =
new GcpCommandExecutor(
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index fd9ab55..93900a2 100644
Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll
index 8db0fca..5584b5b 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb
index db84013..d7fa53b 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt b/data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt
new file mode 100644
index 0000000..a94b318
--- /dev/null
+++ b/data_process/新版前后角解耦控制器测试处理/4m直线0.3/记录.txt
@@ -0,0 +1,17 @@
+第一次：
+ok
+
+第二次：
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
+ at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+ at MultiWheelC.NewControllerStraight4mTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\NewControllerTrackingTests.cs:line 146
+ at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+ *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.031m，航向误差=0.03°。, stack:
+ at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 257
+ at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+
+第三次：
+ok
\ No newline at end of file
diff --git a/data_process/新版控制器轨迹测试处理/混合测试/第三次/记录.txt b/data_process/新版控制器轨迹测试处理/混合测试/第三次/记录.txt
new file mode 100644
index 0000000..edb6c6d
--- /dev/null
+++ b/data_process/新版控制器轨迹测试处理/混合测试/第三次/记录.txt
@@ -0,0 +1,9 @@
+put meobj `clumsy_detour_backplate_lines` @ 95c33350
+apply gltf class `basic_car`, vtx=19456
+put meobj `CarInWorld` @ 965e05e0
+组合运动开始第1段：TrackMotionPlanSegment
+put meobj `CompositeStopTurnGoTest_pc` @ a511aa30
+put meobj `CompositeStopTurnGoTest_lines` @ a5281610
+轨迹实验数据已保存：C:\Users\Administrator\Desktop\MDCS2\Clumsy自动\TrackingExperiments\20260807_134158_374_NewStanleyPidComposite_SmoothTurnStopRotateStraight_Trial1.csv
+Declare P2 on T0(local)
+P2(on T0) decide to close
\ No newline at end of file
diff --git a/data_process/新版控制器轨迹测试处理/混合测试/第四次/记录.txt b/data_process/新版控制器轨迹测试处理/混合测试/第四次/记录.txt
new file mode 100644
index 0000000..3512f01
--- /dev/null
+++ b/data_process/新版控制器轨迹测试处理/混合测试/第四次/记录.txt
@@ -0,0 +1,29 @@
+第一次
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.75°。, stack:
+ at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+ at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 187
+ at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+ *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.033m，航向误差=0.75°。, stack:
+ at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
+ at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 174
+ at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第二次：
+
+: * (Exception):DriveTask failed, msg=车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.24°。, stack:
+ at ClumsyCore.DriveTask.Wait() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 205
+ at MultiWheelC.CompositeStopTurnGoTest.Test() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Experiments\CompositeMotionPlanTests.cs:line 187
+ at ClumsyLite.ClumsyLiteUI.<>c__DisplayClass19_1.b__21() in D:\MDCS\Source\Core\Clumsy\ClumsyLite\ClumsyLiteUI.cs:line 592
+
+ *p.InnerException * (InvalidOperationException):车辆已在终点零速参考处停稳，但终点精度不满足要求：位置误差=0.041m，航向误差=0.24°。, stack:
+ at MultiWheelC.TrajectoryTrackingMovement.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\TrajectoryTrackingMovement.cs:line 244
+ at MultiWheelC.MotionPlanExecutor.Get()+MoveNext() in D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\Movements\MotionPlanExecutor.cs:line 174
+ at ClumsyCore.DriveTask.<>c__DisplayClass9_0.<.ctor>b__2() in D:\MDCS\Source\Core\Clumsy\ClumsyCore\DriveTask.cs:line 112
+
+
+第三次：
+ok
+
+
diff --git a/docs/数据融合.txt b/docs/数据融合.txt
new file mode 100644
index 0000000..b90a927
--- /dev/null
+++ b/docs/数据融合.txt
@@ -0,0 +1,94 @@
+是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。
+为什么需要系统辨识？
+卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现：
+
+预测步持续往错误方向跑
+滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型
+速度估计系统性偏大/偏小
+原地自转时航向纠正效果变差
+
+你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。
+建议辨识的主要参数
+针对四轮差速车，优先考虑这些：
+
+运动学参数
+有效轮半径（或左右轮半径）
+轮距（左右轮中心距离）
+可能的轴距相关项（如果不是纯差速）
+
+速度相关
+电机速度到实际车速的缩放系数（你提到的“偏大”问题）
+左右轮速度的不对称性（如果存在）
+低速段的非线性（静摩擦、死区）
+
+可选但有用的
+速度偏置 / 零点漂移
+简单的打滑系数（如果经常在特定地面跑）
+
+
+怎么做比较实际？
+不需要一开始就上很复杂的在线辨识，可以分两步：
+离线标定（推荐先做）
+
+让车在平坦地面以不同速度直线行驶、原地旋转，记录：
+四个电机反馈速度
+SLAM 给出的位置和航向变化
+
+用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等
+原地自转特别适合标定轮距和左右轮差异
+
+在线微调（可选）
+
+把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF）
+或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子
+
+实际建议顺序
+
+先建立基本运动学模型 + EKF 框架
+做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉
+再跑融合，看残差（innovation）是否变小、速度估计是否更合理
+如果还有残余误差，再考虑把个别参数做成在线估计
+
+总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。
+
+有没有必要做卡尔曼/传感器融合？
+分两层看：
+
+如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳
+→ 性价比一般。
+你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。
+如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等）
+→ 值得做，性价比会变高。
+原因是：
+绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。
+电机反馈是更直接的速度源，频率高、相位更好。
+把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到：
+更低的噪声
+更小的滞后
+对电机“偏大”问题的一定校正能力
+
+
+
+实际建议（按性价比排序）
+
+先低成本优化现有绿色线（最快）
+检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。
+可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。
+看看闭环时车的实际表现，如果抖动已经可接受，就先用着。
+
+中等成本：做简单融合（推荐优先考虑）
+不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度：
+高频信任电机反馈速度
+低频用处理后的 SLAM 差分速度去校正偏差
+
+实现简单，效果通常比单纯低通好一截，滞后也更小。
+
+完整 EKF + 系统辨识
+只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。
+工作量明显更大，但长期收益最高。
+
+
+总结
+从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。
+但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。
+你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。
\ No newline at end of file
diff --git a/docs/记录.txt b/docs/记录.txt
index 6dfaf55..a63503d 100644
--- a/docs/记录.txt
+++ b/docs/记录.txt
@@ -55,3 +55,10 @@ ParkingGeometricController.cs
增加起步航向对齐状态。
第一阶段建议采用第二种，简单、安全。
+把前后GCP转角分解成两个模态：
+共同转角 = (前GCP转角 + 后GCP转角) / 2
+差动转角 = (前GCP转角 - 后GCP转角) / 2
+
+
+
+
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index fd9ab55..93900a2 100644
Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ
diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll
index 8db0fca..5584b5b 100644
Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ
diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb
index db84013..d7fa53b 100644
Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ
diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll
index fd9ab55..93900a2 100644
Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ
diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll
index 3f29033..1713d12 100644
Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ
diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb
index 9a4547b..3ff5098 100644
Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ
diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll
index fd9ab55..93900a2 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ