First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增加差速电机前馈与控制器曲率预瞄

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-12 11:19:01 +08:00
parent 3043febd91
commit 6c6149c8d5
24 changed files with 366 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
+6
View File
@@ -55,6 +55,12 @@ public partial class PilotConfig
[FieldMember(desc = "停车控制：Stanley使用电机实际速度")]
public bool ParkingStanleyUseActualSpeed = true;
[FieldMember(desc = "停车控制：Stanley曲率前馈预瞄时间(s)，0为关闭")]
public float ParkingStanleyCurvaturePreviewSeconds = 0.15f;
[FieldMember(desc = "停车控制：Stanley曲率前馈最大预瞄距离(m)")]
public float ParkingStanleyMaximumCurvaturePreviewMeters = 0.12f;
[FieldMember(desc = "停车控制：Stanley横向修正上限(deg)")]
public float ParkingMaximumCrossTrackCorrectionDegrees = 10f;
MultiWheelC/Control/Abstractions/PathTrackingContext.cs
+11
View File
@@ -16,11 +16,15 @@ namespace MultiWheelC.Control.Abstractions
VehicleState vehicleState,
TrajectoryProjection projection,
double controlReferenceSpeedMetersPerSecond,
double feedforwardCurvaturePerMeter,
double deltaTimeSeconds)
{
EnsureFinite(
controlReferenceSpeedMetersPerSecond,
nameof(controlReferenceSpeedMetersPerSecond));
EnsureFinite(
feedforwardCurvaturePerMeter,
nameof(feedforwardCurvaturePerMeter));
EnsureFinitePositive(
deltaTimeSeconds,
nameof(deltaTimeSeconds));
@@ -29,6 +33,8 @@ namespace MultiWheelC.Control.Abstractions
Projection = projection;
ControlReferenceSpeedMetersPerSecond =
controlReferenceSpeedMetersPerSecond;
FeedforwardCurvaturePerMeter =
feedforwardCurvaturePerMeter;
DeltaTimeSeconds = deltaTimeSeconds;
}
@@ -66,6 +72,11 @@ namespace MultiWheelC.Control.Abstractions
Projection.ReferencePoint
.CurvaturePerMeter;
/// <summary>
/// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
/// </summary>
public double FeedforwardCurvaturePerMeter { get; }
/// <summary>
/// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
/// </summary>
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+91 -1
View File
@@ -110,7 +110,9 @@ namespace MultiWheelC.Control.Execution
double terminalBrakingPreviewMeters = 0.02,
double terminalApproachDistanceMeters = 0.10,
double terminalApproachGainPerSecond = 0.8,
double maximumTerminalApproachSpeedMetersPerSecond = 0.05)
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
double curvaturePreviewSeconds = 0.20,
double maximumCurvaturePreviewMeters = 0.12)
{
_stateProvider = stateProvider ??
throw new ArgumentNullException(
@@ -152,6 +154,12 @@ namespace MultiWheelC.Control.Execution
EnsureFinitePositive(
maximumTerminalApproachSpeedMetersPerSecond,
nameof(maximumTerminalApproachSpeedMetersPerSecond));
EnsureFiniteNonNegative(
curvaturePreviewSeconds,
nameof(curvaturePreviewSeconds));
EnsureFiniteNonNegative(
maximumCurvaturePreviewMeters,
nameof(maximumCurvaturePreviewMeters));
if (terminalApproachDistanceMeters <=
finishDistanceMeters)
@@ -176,6 +184,9 @@ namespace MultiWheelC.Control.Execution
terminalApproachGainPerSecond;
MaximumTerminalApproachSpeedMetersPerSecond =
maximumTerminalApproachSpeedMetersPerSecond;
CurvaturePreviewSeconds = curvaturePreviewSeconds;
MaximumCurvaturePreviewMeters =
maximumCurvaturePreviewMeters;
}
/// <summary>
@@ -218,6 +229,16 @@ namespace MultiWheelC.Control.Execution
/// </summary>
public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
/// <summary>
/// 获取按照车辆纵向速度换算曲率前馈预瞄距离的预测时间，单位为s，0表示关闭。
/// </summary>
public double CurvaturePreviewSeconds { get; }
/// <summary>
/// 获取曲率前馈沿轨迹点序允许预瞄的最大距离，单位为m。
/// </summary>
public double MaximumCurvaturePreviewMeters { get; }
/// <summary>
/// 获取控制器当前是否持有并正在执行一条轨迹。
/// </summary>
@@ -259,6 +280,16 @@ namespace MultiWheelC.Control.Execution
/// </summary>
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
/// <summary>
/// 获取最近控制周期实际采用的曲率前馈预瞄距离，单位为m。
/// </summary>
public double? LastCurvaturePreviewDistanceMeters { get; private set; }
/// <summary>
/// 获取最近控制周期沿轨迹预瞄后交给横向控制器的曲率，单位为1/m。
/// </summary>
public double? LastFeedforwardCurvaturePerMeter { get; private set; }
/// <summary>
/// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。
/// </summary>
@@ -431,10 +462,23 @@ namespace MultiWheelC.Control.Execution
projection);
LastControlReferenceSpeedMetersPerSecond =
controlReferenceSpeedMetersPerSecond;
var curvaturePreviewDistanceMeters =
ResolveCurvaturePreviewDistanceMeters(
vehicleState,
controlReferenceSpeedMetersPerSecond);
var feedforwardCurvaturePerMeter =
ResolveFeedforwardCurvaturePerMeter(
projection,
curvaturePreviewDistanceMeters);
LastCurvaturePreviewDistanceMeters =
curvaturePreviewDistanceMeters;
LastFeedforwardCurvaturePerMeter =
feedforwardCurvaturePerMeter;
var context = new PathTrackingContext(
vehicleState,
projection,
controlReferenceSpeedMetersPerSecond,
feedforwardCurvaturePerMeter,
deltaTimeSeconds);
var lateralCommand =
_lateralController.Compute(context);
@@ -552,6 +596,50 @@ namespace MultiWheelC.Control.Execution
vehicleState);
}
/// <summary>
/// 根据有效实际纵向速度或控制参考速度计算带上限的曲率前馈预瞄距离。
/// </summary>
private double ResolveCurvaturePreviewDistanceMeters(
VehicleState vehicleState,
double controlReferenceSpeedMetersPerSecond)
{
if (CurvaturePreviewSeconds <= 0.0 ||
MaximumCurvaturePreviewMeters <= 0.0)
{
return 0.0;
}
var previewSpeedMetersPerSecond =
vehicleState.HasValidVelocityEstimate
? Math.Abs(
vehicleState.TwistInBody
.VxMetersPerSecond)
: Math.Abs(
controlReferenceSpeedMetersPerSecond);
return Math.Min(
MaximumCurvaturePreviewMeters,
previewSpeedMetersPerSecond *
CurvaturePreviewSeconds);
}
/// <summary>
/// 沿轨迹实际执行点序向前采样专供横向几何前馈使用的曲率。
/// </summary>
private double ResolveFeedforwardCurvaturePerMeter(
TrajectoryProjection projection,
double previewDistanceMeters)
{
var previewArcLengthMeters = Math.Min(
_trajectory.TotalLengthMeters,
projection.ArcLengthMeters +
previewDistanceMeters);
return _trajectory
.SampleAtArcLength(previewArcLengthMeters)
.CurvaturePerMeter;
}
/// <summary>
/// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。
/// </summary>
@@ -914,6 +1002,8 @@ namespace MultiWheelC.Control.Execution
LastProjection = null;
LastCommand = null;
LastControlReferenceSpeedMetersPerSecond = null;
LastCurvaturePreviewDistanceMeters = null;
LastFeedforwardCurvaturePerMeter = null;
LastCycleTiming = null;
LastFailureReason = string.Empty;
LastException = null;
MultiWheelC/Control/Lateral/StanleyLateralController.cs
+1 -1
View File
@@ -104,7 +104,7 @@ namespace MultiWheelC.Control.Lateral
var feedforwardAngleRadians =
travelDirection *
Math.Atan(
context.ReferenceCurvaturePerMeter *
context.FeedforwardCurvaturePerMeter *
ControlPointRadiusMeters);
// 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+4 -1
View File
@@ -332,7 +332,10 @@ namespace MultiWheelC
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
projection.RemainingDistanceMeters);
projection.RemainingDistanceMeters,
controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
controller.LastFeedforwardCurvaturePerMeter ??
projection.ReferencePoint.CurvaturePerMeter);
}
var command = controller.LastCommand.Value;
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+8 -2
View File
@@ -365,7 +365,10 @@ namespace MultiWheelC
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
projection.RemainingDistanceMeters);
projection.RemainingDistanceMeters,
controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
controller.LastFeedforwardCurvaturePerMeter ??
projection.ReferencePoint.CurvaturePerMeter);
}
var command = controller.LastCommand.Value;
@@ -728,7 +731,10 @@ namespace MultiWheelC
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
projection.RemainingDistanceMeters);
projection.RemainingDistanceMeters,
controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
controller.LastFeedforwardCurvaturePerMeter ??
projection.ReferencePoint.CurvaturePerMeter);
}
var command = controller.LastCommand.Value;
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+29 -1
View File
@@ -49,6 +49,8 @@ namespace MultiWheelC
public double ControlHeadingErrorRadians;
public double ControlDistanceToTrajectoryMeters;
public double ControlRemainingDistanceMeters;
public double CurvaturePreviewDistanceMeters;
public double FeedforwardCurvaturePerMeter;
// 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
public bool HasVelocityDiagnostics;
@@ -150,6 +152,8 @@ namespace MultiWheelC
private double _controlHeadingErrorRadians;
private double _controlDistanceToTrajectoryMeters;
private double _controlRemainingDistanceMeters;
private double _curvaturePreviewDistanceMeters;
private double _feedforwardCurvaturePerMeter;
private bool _hasVelocityDiagnostics;
private double _detourEstimatedBodyVxMetersPerSecond;
private bool _detourVelocityEstimateValid;
@@ -371,7 +375,9 @@ namespace MultiWheelC
double lateralErrorMeters,
double headingErrorRadians,
double distanceToTrajectoryMeters,
double remainingDistanceMeters)
double remainingDistanceMeters,
double curvaturePreviewDistanceMeters,
double feedforwardCurvaturePerMeter)
{
lock (_stateSyncRoot)
{
@@ -387,6 +393,10 @@ namespace MultiWheelC
distanceToTrajectoryMeters;
_controlRemainingDistanceMeters =
remainingDistanceMeters;
_curvaturePreviewDistanceMeters =
curvaturePreviewDistanceMeters;
_feedforwardCurvaturePerMeter =
feedforwardCurvaturePerMeter;
_hasControlReference = true;
}
}
@@ -480,6 +490,8 @@ namespace MultiWheelC
double controlHeadingErrorRadians;
double controlDistanceToTrajectoryMeters;
double controlRemainingDistanceMeters;
double curvaturePreviewDistanceMeters;
double feedforwardCurvaturePerMeter;
bool hasVelocityDiagnostics;
double detourEstimatedBodyVxMetersPerSecond;
bool detourVelocityEstimateValid;
@@ -540,6 +552,10 @@ namespace MultiWheelC
_controlDistanceToTrajectoryMeters;
controlRemainingDistanceMeters =
_controlRemainingDistanceMeters;
curvaturePreviewDistanceMeters =
_curvaturePreviewDistanceMeters;
feedforwardCurvaturePerMeter =
_feedforwardCurvaturePerMeter;
hasVelocityDiagnostics =
_hasVelocityDiagnostics;
detourEstimatedBodyVxMetersPerSecond =
@@ -587,6 +603,10 @@ namespace MultiWheelC
controlDistanceToTrajectoryMeters,
ControlRemainingDistanceMeters =
controlRemainingDistanceMeters,
CurvaturePreviewDistanceMeters =
curvaturePreviewDistanceMeters,
FeedforwardCurvaturePerMeter =
feedforwardCurvaturePerMeter,
HasVelocityDiagnostics =
hasVelocityDiagnostics,
DetourEstimatedBodyVxMetersPerSecond =
@@ -802,6 +822,8 @@ namespace MultiWheelC
"ControlHeadingErrorRadians," +
"ControlDistanceToTrajectoryMeters," +
"ControlRemainingDistanceMeters," +
"CurvaturePreviewDistanceMeters," +
"FeedforwardCurvaturePerMeter," +
"HasVelocityDiagnostics," +
"DetourEstimatedBodyVxMetersPerSecond," +
"DetourVelocityEstimateValid," +
@@ -907,6 +929,12 @@ namespace MultiWheelC
FormatOptional(
sample.HasControlReference,
sample.ControlRemainingDistanceMeters),
FormatOptional(
sample.HasControlReference,
sample.CurvaturePreviewDistanceMeters),
FormatOptional(
sample.HasControlReference,
sample.FeedforwardCurvaturePerMeter),
sample.HasVelocityDiagnostics
? "1"
: "0",
MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+19 -1
View File
@@ -62,6 +62,16 @@ namespace MultiWheelC
/// </summary>
public bool? StanleyUsesActualSpeed;
/// <summary>
/// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。
/// </summary>
public double? StanleyCurvaturePreviewSeconds;
/// <summary>
/// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double? StanleyMaximumCurvaturePreviewMeters;
/// <summary>
/// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
@@ -180,6 +190,12 @@ namespace MultiWheelC
var stanleyUsesActualSpeed =
StanleyUsesActualSpeed ??
config.ParkingStanleyUseActualSpeed;
var stanleyCurvaturePreviewSeconds =
StanleyCurvaturePreviewSeconds ??
config.ParkingStanleyCurvaturePreviewSeconds;
var stanleyMaximumCurvaturePreviewMeters =
StanleyMaximumCurvaturePreviewMeters ??
config.ParkingStanleyMaximumCurvaturePreviewMeters;
var maximumCrossTrackCorrectionRadians =
MaximumCrossTrackCorrectionRadians ??
AngleMath.DegreesToRadians(
@@ -332,7 +348,9 @@ namespace MultiWheelC
terminalBrakingPreviewMeters,
terminalApproachDistanceMeters,
terminalApproachGainPerSecond,
maximumTerminalApproachSpeedMetersPerSecond);
maximumTerminalApproachSpeedMetersPerSecond,
stanleyCurvaturePreviewSeconds,
stanleyMaximumCurvaturePreviewMeters);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
MultiWheelC/build/Clumsy/CommonUsage.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.dll
BIN
View File
Binary file not shown.
MultiWheelC/build/Clumsy/MultiWheelC.pdb
BIN
View File
Binary file not shown.