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

重构横向控制器输出前后GCP目标转角并简化命令分配器

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-07 18:13:55 +08:00
parent bc37e71ad0
commit 88f651e0c2
25 changed files with 310 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Lateral/StanleyLateralController.cs
+63 -36
View File
@@ -4,22 +4,23 @@ using MultiWheelC.Control.Abstractions;
namespace MultiWheelC.Control.Lateral
{
/// <summary>
/// 使用参考曲率前馈、航向误差和横向误差计算车体中心目标曲率。
/// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
/// </summary>
public sealed class StanleyLateralController : ILateralController
{
private const double MaximumMathematicalAngleRadians =
Math.PI / 2.0 - 1e-3;
/// <summary>
/// 创建使用指定GCP几何、Stanley增益和低速保护参数的横向控制器。
/// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
/// </summary>
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;
}
/// <summary>
@@ -62,12 +73,22 @@ namespace MultiWheelC.Control.Lateral
public double MinimumSpeedMetersPerSecond { get; }
/// <summary>
/// 获取是否优先使用Detour估算的实际纵向速度计算横向误差项。
/// 获取是否优先使用Detour估算的实际纵向速度计算横向修正。
/// </summary>
public bool UseActualSpeedForGain { get; }
/// <summary>
/// 根据参考曲率、航向误差和横向误差计算车体中心目标曲率。
/// 获取横向误差共同转角分量的最大绝对值，单位为rad。
/// </summary>
public double MaximumCrossTrackCorrectionRadians { get; }
/// <summary>
/// 获取航向误差差动转角分量的最大绝对值，单位为rad。
/// </summary>
public double MaximumHeadingCorrectionRadians { get; }
/// <summary>
/// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
/// </summary>
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);
}
/// <summary>
@@ -115,7 +143,7 @@ namespace MultiWheelC.Control.Lateral
}
/// <summary>
/// 选择Stanley横向误差项使用的实际速度或旧版参考速度。
/// 选择Stanley横向误差项使用的实际速度或参考速度。
/// </summary>
private double SelectSpeedForGain(
PathTrackingContext context)
@@ -131,7 +159,7 @@ namespace MultiWheelC.Control.Lateral
}
/// <summary>
/// 根据有符号参考速度确定前进或倒车的反馈修正方向。
/// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
/// </summary>
private static double SelectTravelDirection(
PathTrackingContext context)
@@ -158,16 +186,15 @@ namespace MultiWheelC.Control.Lateral
}
/// <summary>
/// 将数值限制在指定闭区间内。
/// 将数值按正负对称方式限制在指定绝对值内。
/// </summary>
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));
}
/// <summary>
@@ -183,7 +210,7 @@ namespace MultiWheelC.Control.Lateral
{
throw new ArgumentOutOfRangeException(
parameterName,
"Stanley控制器的几何尺寸和最小速度必须是正有限值。");
"Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
}
}