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/Abstractions/LateralControlCommand.cs
+38 -13
View File
@@ -3,37 +3,62 @@ using System;
namespace MultiWheelC.Control.Abstractions
{
/// <summary>
/// 表示横向控制器生成的车体中心目标曲率命令。
/// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
/// </summary>
public readonly struct LateralControlCommand
{
/// <summary>
/// 创建统一使用SI单位和左转为正约定的横向控制命令。
/// 创建前、后GCP目标转角命令。
/// </summary>
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;
}
/// <summary>
/// 获取车体中心目标轨迹曲率，单位为1/m，左转为正、右转为负。
/// 获取前GCP目标转角，单位为rad，逆时针为正。
/// </summary>
public double TargetCurvaturePerMeter { get; }
public double FrontGcpAngleRadians { get; }
/// <summary>
/// 创建保持直线行驶的零曲率命令。
/// 获取后GCP目标转角，单位为rad，逆时针为正。
/// </summary>
public double RearGcpAngleRadians { get; }
/// <summary>
/// 获取前后GCP的共同转角分量，主要用于横向平移修正。
/// </summary>
public double CommonAngleRadians =>
(FrontGcpAngleRadians +
RearGcpAngleRadians) / 2.0;
/// <summary>
/// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
/// </summary>
public double DifferentialAngleRadians =>
(FrontGcpAngleRadians -
RearGcpAngleRadians) / 2.0;
/// <summary>
/// 创建前后GCP均保持车头方向的直线命令。
/// </summary>
public static LateralControlCommand Straight =>
new LateralControlCommand(0.0);
new LateralControlCommand(0.0, 0.0);
/// <summary>
/// 检查横向曲率命令是否为有限值。
/// 检查GCP目标转角是否为有限值。
/// </summary>
private static void EnsureFinite(
double value,
@@ -44,7 +69,7 @@ namespace MultiWheelC.Control.Abstractions
{
throw new ArgumentOutOfRangeException(
parameterName,
"横向控制目标曲率必须是有限值。");
"GCP目标转角必须是有限值。");
}
}
}
MultiWheelC/Control/Allocation/AckermannGcpAllocator.cs → MultiWheelC/Control/Allocation/GcpCommandAllocator.cs
+19 -48
View File
@@ -4,57 +4,36 @@ using MultiWheelC.Control.Abstractions;
namespace MultiWheelC.Control.Allocation
{
/// <summary>
/// 将车体中心目标曲率按对称前后转向策略转换为旧版底盘的前后GCP方向。
/// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
/// </summary>
public sealed class AckermannGcpAllocator
public sealed class GcpCommandAllocator
{
/// <summary>
/// 创建使用指定GCP半间距和最大GCP转角的对称转向分配器。
/// 创建使用指定前后GCP最大转角的命令分配器。
/// </summary>
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;
}
/// <summary>
/// 获取车体中心到前、后GCP的距离，单位为m。
/// </summary>
public double ControlPointRadiusMeters { get; }
/// <summary>
/// 获取前后GCP允许的最大转角绝对值，单位为rad。
/// </summary>
public double MaximumGcpAngleRadians { get; }
/// <summary>
/// 获取当前GCP几何和转角限制允许的最大车体中心曲率，单位为1/m。
/// </summary>
public double MaximumCurvaturePerMeter =>
Math.Tan(MaximumGcpAngleRadians) /
ControlPointRadiusMeters;
/// <summary>
/// 将纵向命令速度和车体中心目标曲率分配为前后GCP运动命令。
/// 将纵向速度和前后GCP转角组合为底盘运动命令。
/// </summary>
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
}
/// <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>
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+2 -2
View File
@@ -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,
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控制器的几何尺寸、速度和角度限制必须是正有限值。");
}
}