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

打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-08-06 15:14:12 +08:00
co-authored by Cursor
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Control/Execution/ParkingGeometricController.cs
+551 -1
View File
@@ -1 +1,551 @@
// 负责组织一个控制周期
using System;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.StateEstimation;
using MultiWheelC.Trajectory;
using MyParking.Shared;
namespace MultiWheelC.Control.Execution
{
/// <summary>
/// 表示新版停车机器人单周期轨迹控制的执行结果。
/// </summary>
public enum ParkingControlCycleResult
{
Inactive = 0,
CommandSent = 1,
Completed = 2,
StateUnavailable = 3,
Faulted = 4
}
/// <summary>
/// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
/// </summary>
public sealed class ParkingGeometricController
{
private const double ZeroReferenceSpeedToleranceMetersPerSecond =
1e-6;
private const double StartupRegionMeters = 0.02;
private const double StartupPreviewDistanceMeters = 0.05;
private const double MaximumStartupSpeedMetersPerSecond = 0.05;
private readonly IVehicleStateProvider _stateProvider;
private readonly ILateralController _lateralController;
private readonly ILongitudinalController _longitudinalController;
private readonly AckermannGcpAllocator _gcpAllocator;
private readonly GcpCommandExecutor _commandExecutor;
private Trajectory2D _trajectory;
/// <summary>
/// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
/// </summary>
public ParkingGeometricController(
IVehicleStateProvider stateProvider,
ILateralController lateralController,
ILongitudinalController longitudinalController,
AckermannGcpAllocator gcpAllocator,
GcpCommandExecutor commandExecutor,
double finishDistanceMeters = 0.03,
double finishSpeedMetersPerSecond = 0.02,
double finishHeadingToleranceRadians =
3.0 * Math.PI / 180.0,
double maximumDistanceToTrajectoryMeters = 0.50)
{
_stateProvider = stateProvider ??
throw new ArgumentNullException(
nameof(stateProvider));
_lateralController = lateralController ??
throw new ArgumentNullException(
nameof(lateralController));
_longitudinalController = longitudinalController ??
throw new ArgumentNullException(
nameof(longitudinalController));
_gcpAllocator = gcpAllocator ??
throw new ArgumentNullException(
nameof(gcpAllocator));
_commandExecutor = commandExecutor ??
throw new ArgumentNullException(
nameof(commandExecutor));
EnsureFinitePositive(
finishDistanceMeters,
nameof(finishDistanceMeters));
EnsureFiniteNonNegative(
finishSpeedMetersPerSecond,
nameof(finishSpeedMetersPerSecond));
EnsureFinitePositive(
finishHeadingToleranceRadians,
nameof(finishHeadingToleranceRadians));
EnsureFinitePositive(
maximumDistanceToTrajectoryMeters,
nameof(maximumDistanceToTrajectoryMeters));
FinishDistanceMeters = finishDistanceMeters;
FinishSpeedMetersPerSecond =
finishSpeedMetersPerSecond;
FinishHeadingToleranceRadians =
finishHeadingToleranceRadians;
MaximumDistanceToTrajectoryMeters =
maximumDistanceToTrajectoryMeters;
}
/// <summary>
/// 获取终点位置和剩余弧长允许的误差，单位为m。
/// </summary>
public double FinishDistanceMeters { get; }
/// <summary>
/// 获取判定轨迹执行完成时允许的最大实际线速度，单位为m/s。
/// </summary>
public double FinishSpeedMetersPerSecond { get; }
/// <summary>
/// 获取判定轨迹完成时允许的最大终点航向误差，单位为rad。
/// </summary>
public double FinishHeadingToleranceRadians { get; }
/// <summary>
/// 获取允许车辆偏离参考轨迹的最大距离，单位为m。
/// </summary>
public double MaximumDistanceToTrajectoryMeters { get; }
/// <summary>
/// 获取控制器当前是否持有并正在执行一条轨迹。
/// </summary>
public bool IsActive { get; private set; }
/// <summary>
/// 获取最近一次轨迹是否已经满足终点完成条件。
/// </summary>
public bool IsCompleted { get; private set; }
/// <summary>
/// 获取最近一次控制失败原因，正常时为空字符串。
/// </summary>
public string LastFailureReason { get; private set; } =
string.Empty;
/// <summary>
/// 获取最近一次控制异常，正常时为空。
/// </summary>
public Exception LastException { get; private set; }
/// <summary>
/// 获取最近一次有效车辆状态。
/// </summary>
public VehicleState? LastVehicleState { get; private set; }
/// <summary>
/// 获取最近一次车体中心到参考轨迹的投影结果。
/// </summary>
public TrajectoryProjection? LastProjection { get; private set; }
/// <summary>
/// 获取最近一次发送或准备发送的GCP运动命令。
/// </summary>
public GcpMotionCommand? LastCommand { get; private set; }
/// <summary>
/// 获取最近控制周期实际交给纵向控制器的参考速度，单位为m/s。
/// </summary>
public double? LastReferenceSpeedMetersPerSecond { get; private set; }
/// <summary>
/// 停止当前底盘并从起点开始执行指定二维轨迹。
/// </summary>
public void Start(Trajectory2D trajectory)
{
if (trajectory == null)
{
throw new ArgumentNullException(
nameof(trajectory));
}
StopAndResetControllers();
_trajectory = trajectory;
IsActive = true;
IsCompleted = false;
ClearDiagnostics();
}
/// <summary>
/// 读取本周期车辆状态并执行一次完整的轨迹跟踪控制计算。
/// </summary>
public ParkingControlCycleResult ExecuteCycle(
double deltaTimeSeconds)
{
EnsureFinitePositive(
deltaTimeSeconds,
nameof(deltaTimeSeconds));
if (!IsActive || _trajectory == null)
{
return ParkingControlCycleResult.Inactive;
}
try
{
if (!_stateProvider.TryGetState(
out var vehicleState))
{
StopForUnavailableState();
return ParkingControlCycleResult
.StateUnavailable;
}
LastVehicleState = vehicleState;
var projection = TrajectoryProjector.Project(
_trajectory,
vehicleState.PoseInWorld);
LastProjection = projection;
if (projection.DistanceToTrajectoryMeters >
MaximumDistanceToTrajectoryMeters)
{
return EnterFault(
"车辆距离参考轨迹" +
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
"超过允许值" +
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
}
if (HasReachedEnd(
vehicleState,
projection))
{
CompleteTrajectory();
return ParkingControlCycleResult.Completed;
}
if (HasStoppedAtUnsatisfiedTerminal(
vehicleState,
projection,
out var terminalFailureReason))
{
return EnterFault(
terminalFailureReason);
}
var referenceSpeedMetersPerSecond =
ResolveReferenceSpeedForControl(
projection);
LastReferenceSpeedMetersPerSecond =
referenceSpeedMetersPerSecond;
var context = new PathTrackingContext(
vehicleState,
projection,
referenceSpeedMetersPerSecond,
deltaTimeSeconds);
var lateralCommand =
_lateralController.Compute(context);
var commandSpeedMetersPerSecond =
_longitudinalController
.ComputeSpeedMetersPerSecond(context);
var gcpCommand = _gcpAllocator.Allocate(
commandSpeedMetersPerSecond,
lateralCommand);
if (!_commandExecutor.Execute(
gcpCommand,
deltaTimeSeconds))
{
return EnterFault(
string.IsNullOrWhiteSpace(
_commandExecutor.LastFailureReason)
? "GCP底盘命令执行失败。"
: _commandExecutor.LastFailureReason);
}
LastCommand =
_commandExecutor.LastSentCommand;
LastFailureReason = string.Empty;
LastException = null;
return ParkingControlCycleResult.CommandSent;
}
catch (Exception exception)
{
return EnterFault(
"停车机器人轨迹控制周期异常：" +
exception.Message,
exception);
}
}
/// <summary>
/// 主动取消当前轨迹、立即停车并清除全部控制器状态。
/// </summary>
public void Cancel()
{
StopAndResetControllers();
_trajectory = null;
IsActive = false;
IsCompleted = false;
ClearDiagnostics();
}
/// <summary>
/// 在轨迹起点零速固定点处读取前方速度，并限制为低速起步命令。
/// </summary>
private double ResolveReferenceSpeedForControl(
TrajectoryProjection projection)
{
var currentReferenceSpeed =
projection.ReferencePoint
.ReferenceSpeedMetersPerSecond;
var requiresStartupRelease =
projection.ArcLengthMeters <=
StartupRegionMeters &&
projection.RemainingDistanceMeters >
FinishDistanceMeters &&
Math.Abs(currentReferenceSpeed) <=
ZeroReferenceSpeedToleranceMetersPerSecond;
if (!requiresStartupRelease)
{
return currentReferenceSpeed;
}
var previewArcLengthMeters = Math.Min(
_trajectory.TotalLengthMeters,
projection.ArcLengthMeters +
StartupPreviewDistanceMeters);
var previewReferenceSpeed =
_trajectory
.SampleAtArcLength(
previewArcLengthMeters)
.ReferenceSpeedMetersPerSecond;
if (Math.Abs(previewReferenceSpeed) <=
ZeroReferenceSpeedToleranceMetersPerSecond)
{
return 0.0;
}
return Math.Sign(previewReferenceSpeed) *
Math.Min(
Math.Abs(previewReferenceSpeed),
MaximumStartupSpeedMetersPerSecond);
}
/// <summary>
/// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
/// </summary>
private bool HasReachedEnd(
VehicleState vehicleState,
TrajectoryProjection projection)
{
if (!vehicleState.HasValidVelocityEstimate)
{
return false;
}
return projection.RemainingDistanceMeters <=
FinishDistanceMeters &&
CalculateDistanceToEndMeters(
vehicleState) <=
FinishDistanceMeters &&
CalculateHeadingErrorToEndRadians(
vehicleState) <=
FinishHeadingToleranceRadians &&
CalculateActualLinearSpeedMetersPerSecond(
vehicleState) <=
FinishSpeedMetersPerSecond;
}
/// <summary>
/// 检查车辆是否已在终点零速参考处停稳但最终位置或航向仍不合格。
/// </summary>
private bool HasStoppedAtUnsatisfiedTerminal(
VehicleState vehicleState,
TrajectoryProjection projection,
out string failureReason)
{
failureReason = string.Empty;
var isTerminalZeroSpeedReference =
projection.RemainingDistanceMeters <=
FinishDistanceMeters &&
Math.Abs(
projection.ReferencePoint
.ReferenceSpeedMetersPerSecond) <=
ZeroReferenceSpeedToleranceMetersPerSecond;
if (!isTerminalZeroSpeedReference ||
!vehicleState.HasValidVelocityEstimate ||
CalculateActualLinearSpeedMetersPerSecond(
vehicleState) >
FinishSpeedMetersPerSecond)
{
return false;
}
var positionErrorMeters =
CalculateDistanceToEndMeters(
vehicleState);
var headingErrorRadians =
CalculateHeadingErrorToEndRadians(
vehicleState);
failureReason =
"车辆已在终点零速参考处停稳，但终点精度不满足要求：" +
$"位置误差={positionErrorMeters:F3}m，" +
"航向误差=" +
$"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
return true;
}
/// <summary>
/// 计算实际车体中心到轨迹终点的欧氏距离，单位为m。
/// </summary>
private double CalculateDistanceToEndMeters(
VehicleState vehicleState)
{
var endPoint = _trajectory.EndPoint.PoseInWorld;
var deltaX =
vehicleState.PoseInWorld.XMeters -
endPoint.XMeters;
var deltaY =
vehicleState.PoseInWorld.YMeters -
endPoint.YMeters;
return Math.Sqrt(
deltaX * deltaX +
deltaY * deltaY);
}
/// <summary>
/// 计算实际车体航向到轨迹终点航向的最短角度误差绝对值，单位为rad。
/// </summary>
private double CalculateHeadingErrorToEndRadians(
VehicleState vehicleState)
{
return Math.Abs(
AngleMath.ShortestDifferenceRadians(
_trajectory.EndPoint
.PoseInWorld.YawRadians,
vehicleState
.PoseInWorld.YawRadians));
}
/// <summary>
/// 计算车体坐标系实际线速度的合速度绝对值，单位为m/s。
/// </summary>
private static double CalculateActualLinearSpeedMetersPerSecond(
VehicleState vehicleState)
{
return Math.Sqrt(
vehicleState.TwistInBody.VxMetersPerSecond *
vehicleState.TwistInBody.VxMetersPerSecond +
vehicleState.TwistInBody.VyMetersPerSecond *
vehicleState.TwistInBody.VyMetersPerSecond);
}
/// <summary>
/// 在状态暂不可用时停车并重置反馈控制器，同时保留轨迹等待下一周期恢复。
/// </summary>
private void StopForUnavailableState()
{
_commandExecutor.Stop();
_lateralController.Reset();
_longitudinalController.Reset();
LastCommand = null;
LastFailureReason =
"当前无法获得有效车辆状态，底盘已停车并等待定位恢复。";
LastException = null;
}
/// <summary>
/// 完成当前轨迹并停车，但保留最后状态和投影供实验记录读取。
/// </summary>
private void CompleteTrajectory()
{
StopAndResetControllers();
IsActive = false;
IsCompleted = true;
LastCommand = new GcpMotionCommand(
0.0,
0.0,
0.0);
LastFailureReason = string.Empty;
LastException = null;
}
/// <summary>
/// 发生不可继续的控制故障时停车、退出活动状态并保存诊断信息。
/// </summary>
private ParkingControlCycleResult EnterFault(
string reason,
Exception exception = null)
{
StopAndResetControllers();
IsActive = false;
IsCompleted = false;
LastCommand = null;
LastFailureReason = reason;
LastException = exception;
return ParkingControlCycleResult.Faulted;
}
/// <summary>
/// 立即停止底盘并清除横向和纵向控制器的跨周期状态。
/// </summary>
private void StopAndResetControllers()
{
_commandExecutor.Stop();
_lateralController.Reset();
_longitudinalController.Reset();
}
/// <summary>
/// 清除上一条轨迹留下的状态、命令和故障诊断信息。
/// </summary>
private void ClearDiagnostics()
{
LastVehicleState = null;
LastProjection = null;
LastCommand = null;
LastReferenceSpeedMetersPerSecond = null;
LastFailureReason = string.Empty;
LastException = null;
}
/// <summary>
/// 检查控制参数是否为正有限值。
/// </summary>
private static void EnsureFinitePositive(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value) ||
value <= 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹控制器距离和周期参数必须是正有限值。");
}
}
/// <summary>
/// 检查控制参数是否为非负有限值。
/// </summary>
private static void EnsureFiniteNonNegative(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value) ||
value < 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"轨迹控制器速度参数必须是非负有限值。");
}
}
}
}