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-11 17:06:26 +08:00
parent a59499e638
commit 33a33af710
41 changed files with 817 additions and 2654 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Abstractions/PathTrackingContext.cs
+8 -8
View File
@@ -15,20 +15,20 @@ namespace MultiWheelC.Control.Abstractions
public PathTrackingContext(
VehicleState vehicleState,
TrajectoryProjection projection,
double referenceSpeedMetersPerSecond,
double controlReferenceSpeedMetersPerSecond,
double deltaTimeSeconds)
{
EnsureFinite(
referenceSpeedMetersPerSecond,
nameof(referenceSpeedMetersPerSecond));
controlReferenceSpeedMetersPerSecond,
nameof(controlReferenceSpeedMetersPerSecond));
EnsureFinitePositive(
deltaTimeSeconds,
nameof(deltaTimeSeconds));
VehicleState = vehicleState;
Projection = projection;
ReferenceSpeedMetersPerSecond =
referenceSpeedMetersPerSecond;
ControlReferenceSpeedMetersPerSecond =
controlReferenceSpeedMetersPerSecond;
DeltaTimeSeconds = deltaTimeSeconds;
}
@@ -48,9 +48,9 @@ namespace MultiWheelC.Control.Abstractions
public double DeltaTimeSeconds { get; }
/// <summary>
/// 获取轨迹投影点要求的有符号参考速度，单位为m/s。
/// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
/// </summary>
public double ReferenceSpeedMetersPerSecond { get; }
public double ControlReferenceSpeedMetersPerSecond { get; }
/// <summary>
/// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
@@ -60,7 +60,7 @@ namespace MultiWheelC.Control.Abstractions
.VxMetersPerSecond;
/// <summary>
/// 获取轨迹投影点的参考曲率，单位为1/m，左转为正。
/// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
/// </summary>
public double ReferenceCurvaturePerMeter =>
Projection.ReferencePoint
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+29 -19
View File
@@ -29,6 +29,10 @@ namespace MultiWheelC.Control.Execution
private const double StartupRegionMeters = 0.02;
private const double StartupPreviewDistanceMeters = 0.05;
private const double MaximumStartupSpeedMetersPerSecond = 0.08;
private const double ProjectionBackwardSearchDistanceMeters =
0.10;
private const double ProjectionForwardSearchDistanceMeters =
1.00;
private readonly IVehicleStateProvider _stateProvider;
private readonly ILateralController _lateralController;
@@ -103,7 +107,7 @@ namespace MultiWheelC.Control.Execution
public double FinishDistanceMeters { get; }
/// <summary>
/// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
/// 获取判定轨迹执行完成时允许的最大实际纵向速度，单位为m/s。
/// </summary>
public double FinishSpeedMetersPerSecond { get; }
@@ -161,7 +165,7 @@ namespace MultiWheelC.Control.Execution
/// <summary>
/// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
/// </summary>
public double? LastReferenceSpeedMetersPerSecond { get; private set; }
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
/// <summary>
/// 停止当前底盘并从起点开始执行指定二维轨迹。
@@ -208,9 +212,18 @@ namespace MultiWheelC.Control.Execution
LastVehicleState = vehicleState;
var projection = TrajectoryProjector.Project(
_trajectory,
vehicleState.PoseInWorld);
// 首周期允许全局定位轨迹进度；后续周期仅在上次进度
// 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。
var projection = LastProjection.HasValue
? TrajectoryProjector.Project(
_trajectory,
vehicleState.PoseInWorld,
LastProjection.Value.ArcLengthMeters,
ProjectionBackwardSearchDistanceMeters,
ProjectionForwardSearchDistanceMeters)
: TrajectoryProjector.Project(
_trajectory,
vehicleState.PoseInWorld);
LastProjection = projection;
if (projection.DistanceToTrajectoryMeters >
@@ -240,15 +253,15 @@ namespace MultiWheelC.Control.Execution
terminalFailureReason);
}
var referenceSpeedMetersPerSecond =
var controlReferenceSpeedMetersPerSecond =
ResolveReferenceSpeedForControl(
projection);
LastReferenceSpeedMetersPerSecond =
referenceSpeedMetersPerSecond;
LastControlReferenceSpeedMetersPerSecond =
controlReferenceSpeedMetersPerSecond;
var context = new PathTrackingContext(
vehicleState,
projection,
referenceSpeedMetersPerSecond,
controlReferenceSpeedMetersPerSecond,
deltaTimeSeconds);
var lateralCommand =
_lateralController.Compute(context);
@@ -421,7 +434,7 @@ namespace MultiWheelC.Control.Execution
CalculateHeadingErrorToEndRadians(
vehicleState) <=
FinishHeadingToleranceRadians &&
CalculateActualLinearSpeedMetersPerSecond(
CalculateActualLongitudinalSpeedMetersPerSecond(
vehicleState) <=
FinishSpeedMetersPerSecond;
}
@@ -446,7 +459,7 @@ namespace MultiWheelC.Control.Execution
if (!isTerminalZeroSpeedReference ||
!vehicleState.HasValidVelocityEstimate ||
CalculateActualLinearSpeedMetersPerSecond(
CalculateActualLongitudinalSpeedMetersPerSecond(
vehicleState) >
FinishSpeedMetersPerSecond)
{
@@ -502,16 +515,13 @@ namespace MultiWheelC.Control.Execution
}
/// <summary>
/// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
/// 计算车体坐标系实际纵向速度的绝对值，单位为m/s。
/// </summary>
private static double CalculateActualLinearSpeedMetersPerSecond(
private static double CalculateActualLongitudinalSpeedMetersPerSecond(
VehicleState vehicleState)
{
return Math.Sqrt(
vehicleState.TwistInBody.VxMetersPerSecond *
vehicleState.TwistInBody.VxMetersPerSecond +
vehicleState.TwistInBody.VyMetersPerSecond *
vehicleState.TwistInBody.VyMetersPerSecond);
return Math.Abs(
vehicleState.TwistInBody.VxMetersPerSecond);
}
/// <summary>
@@ -578,7 +588,7 @@ namespace MultiWheelC.Control.Execution
LastVehicleState = null;
LastProjection = null;
LastCommand = null;
LastReferenceSpeedMetersPerSecond = null;
LastControlReferenceSpeedMetersPerSecond = null;
LastFailureReason = string.Empty;
LastException = null;
}
MultiWheelC/Control/Lateral/StanleyLateralController.cs
+10 -7
View File
@@ -99,10 +99,13 @@ namespace MultiWheelC.Control.Lateral
MinimumSpeedMetersPerSecond);
var travelDirection = SelectTravelDirection(context);
// 参考曲率决定前后反向的差动转角，使无跟踪误差时也能沿曲线行驶。
var feedforwardAngleRadians = Math.Atan(
context.ReferenceCurvaturePerMeter *
ControlPointRadiusMeters);
// 参考曲率按轨迹点序的实际行进方向定义；倒车时底盘有符号
// 纵向速度反向，因此GCP曲率前馈也必须反向才能保持相同几何曲率。
var feedforwardAngleRadians =
travelDirection *
Math.Atan(
context.ReferenceCurvaturePerMeter *
ControlPointRadiusMeters);
// 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
var crossTrackCorrectionRadians =
@@ -155,7 +158,7 @@ namespace MultiWheelC.Control.Lateral
.ActualLongitudinalSpeedMetersPerSecond;
}
return context.ReferenceSpeedMetersPerSecond;
return context.ControlReferenceSpeedMetersPerSecond;
}
/// <summary>
@@ -166,11 +169,11 @@ namespace MultiWheelC.Control.Lateral
{
const double directionDeadbandMetersPerSecond = 1e-6;
if (Math.Abs(context.ReferenceSpeedMetersPerSecond) >
if (Math.Abs(context.ControlReferenceSpeedMetersPerSecond) >
directionDeadbandMetersPerSecond)
{
return Math.Sign(
context.ReferenceSpeedMetersPerSecond);
context.ControlReferenceSpeedMetersPerSecond);
}
if (context.HasValidVelocityEstimate &&
MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs
+10 -10
View File
@@ -85,16 +85,16 @@ namespace MultiWheelC.Control.Longitudinal
_feedbackPid.LastDerivativeOutput;
/// <summary>
/// 根据轨迹参考速度和Detour实际纵向速度计算底盘命令速度。
/// 根据本周期控制参考速度和实际纵向速度计算底盘命令速度。
/// </summary>
public double ComputeSpeedMetersPerSecond(
PathTrackingContext context)
{
var referenceSpeedMetersPerSecond =
context.ReferenceSpeedMetersPerSecond;
var controlReferenceSpeedMetersPerSecond =
context.ControlReferenceSpeedMetersPerSecond;
// 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
if (Math.Abs(referenceSpeedMetersPerSecond) <=
if (Math.Abs(controlReferenceSpeedMetersPerSecond) <=
ReferenceStopDeadbandMetersPerSecond)
{
Reset();
@@ -106,11 +106,11 @@ namespace MultiWheelC.Control.Longitudinal
{
Reset();
return LimitReferenceSpeed(
referenceSpeedMetersPerSecond);
controlReferenceSpeedMetersPerSecond);
}
var speedErrorMetersPerSecond =
referenceSpeedMetersPerSecond -
controlReferenceSpeedMetersPerSecond -
context.ActualLongitudinalSpeedMetersPerSecond;
// Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
@@ -120,24 +120,24 @@ namespace MultiWheelC.Control.Longitudinal
{
Reset();
return LimitReferenceSpeed(
referenceSpeedMetersPerSecond);
controlReferenceSpeedMetersPerSecond);
}
GetCorrectionOutputRange(
referenceSpeedMetersPerSecond,
controlReferenceSpeedMetersPerSecond,
out var minimumCorrectionMetersPerSecond,
out var maximumCorrectionMetersPerSecond);
var correctionMetersPerSecond =
_feedbackPid.Update(
referenceSpeedMetersPerSecond,
controlReferenceSpeedMetersPerSecond,
context
.ActualLongitudinalSpeedMetersPerSecond,
context.DeltaTimeSeconds,
minimumCorrectionMetersPerSecond,
maximumCorrectionMetersPerSecond);
return referenceSpeedMetersPerSecond +
return controlReferenceSpeedMetersPerSecond +
correctionMetersPerSecond;
}