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/Movements/TrajectoryTrackingMovement.cs
+174 -77
View File
@@ -4,6 +4,7 @@ using System.Diagnostics;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Control.Execution;
using MultiWheelC.Control.Lateral;
@@ -26,120 +27,120 @@ namespace MultiWheelC
public Trajectory2D Trajectory;
/// <summary>
/// 获取或设置本次动作使用的车辆状态源；为空时自动创建Detour状态源。
/// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
/// </summary>
public IVehicleStateProvider StateProvider;
/// <summary>
/// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
/// </summary>
public Func<double, ILateralController>
LateralControllerFactory;
/// <summary>
/// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
/// </summary>
public Action<ParkingGeometricController> CycleObserver;
/// <summary>
/// Stanley横向误差增益，单位为1/s。
/// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
/// </summary>
public double StanleyCrossTrackGainPerSecond = 0.4;
public double? StanleyCrossTrackGainPerSecond;
/// <summary>
/// Stanley航向误差增益。
/// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
/// </summary>
public double StanleyHeadingErrorGain = 1.0;
public double? StanleyHeadingErrorGain;
/// <summary>
/// Stanley低速分母保护速度，单位为m/s。
/// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
public double? StanleyMinimumSpeedMetersPerSecond;
/// <summary>
/// 获取或设置Stanley是否优先使用当前状态源提供的实际纵向速度。
/// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
/// </summary>
public bool StanleyUsesActualSpeed = true;
public bool? StanleyUsesActualSpeed;
/// <summary>
/// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
/// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double MaximumCrossTrackCorrectionRadians =
AngleMath.DegreesToRadians(10.0);
public double? MaximumCrossTrackCorrectionRadians;
/// <summary>
/// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
/// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double MaximumHeadingCorrectionRadians =
AngleMath.DegreesToRadians(10.0);
public double? MaximumHeadingCorrectionRadians;
/// <summary>
/// 纵向速度外环比例增益。
/// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
/// </summary>
public double LongitudinalKp = 0.5;
public double? LongitudinalKp;
/// <summary>
/// 纵向速度外环积分增益，单位为1/s。
/// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
/// </summary>
public double LongitudinalKiPerSecond;
public double? LongitudinalKiPerSecond;
/// <summary>
/// 纵向速度外环微分增益，单位为s。
/// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
/// </summary>
public double LongitudinalKdSeconds;
public double? LongitudinalKdSeconds;
/// <summary>
/// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
/// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
public double? MaximumIntegralCorrectionMetersPerSecond;
/// <summary>
/// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
/// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
0.025;
public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;
/// <summary>
/// 底盘纵向命令速度绝对值上限，单位为m/s。
/// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double MaximumCommandSpeedMetersPerSecond = 0.50;
public double? MaximumCommandSpeedMetersPerSecond;
/// <summary>
/// 前后GCP允许的最大转角绝对值，单位为rad。
/// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double MaximumGcpAngleRadians =
AngleMath.DegreesToRadians(45.0);
public double? MaximumGcpAngleRadians;
/// <summary>
/// 前后GCP目标转角最大变化率，单位为rad/s。
/// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
/// </summary>
public double MaximumGcpAngleRateRadiansPerSecond =
AngleMath.DegreesToRadians(15.0);
public double? MaximumGcpAngleRateRadiansPerSecond;
/// <summary>
/// 终点位置和剩余弧长的完成容差，单位为m。
/// 获取或设置本次动作的终点距离容差覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double FinishDistanceMeters = 0.03;
public double? FinishDistanceMeters;
/// <summary>
/// 终点停稳判定允许的实际线速度，单位为m/s。
/// 获取或设置本次动作的终点速度容差覆盖值，单位为m/s；为空时读取车辆配置。
/// </summary>
public double FinishSpeedMetersPerSecond = 0.02;
public double? FinishSpeedMetersPerSecond;
/// <summary>
/// 终点航向完成容差，单位为rad。
/// 获取或设置本次动作的终点航向容差覆盖值，单位为rad；为空时读取车辆配置。
/// </summary>
public double FinishHeadingToleranceRadians =
AngleMath.DegreesToRadians(3.0);
public double? FinishHeadingToleranceRadians;
/// <summary>
/// 终点减速阶段提前读取参考速度的距离，单位为m。
/// 获取或设置本次动作的终点制动预瞄距离覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double TerminalBrakingPreviewMeters = 0.02;
public double? TerminalBrakingPreviewMeters;
/// <summary>
/// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
/// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
/// </summary>
public double MaximumDistanceToTrajectoryMeters = 0.30;
public double? MaximumDistanceToTrajectoryMeters;
/// <summary>
/// 单次轨迹动作允许的最长执行时间，单位为s。
/// 获取或设置本次动作的执行超时覆盖值，单位为s；为空时读取车辆配置。
/// </summary>
public double ExecutionTimeoutSeconds = 120.0;
public double? ExecutionTimeoutSeconds;
/// <summary>
/// 获取本次动作创建的控制器，尚未开始时为空。
@@ -147,11 +148,78 @@ namespace MultiWheelC
public ParkingGeometricController Controller { get; private set; }
/// <summary>
/// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
/// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
/// </summary>
public override IEnumerable<bool> Get()
{
ValidateParameters();
var config = PilotDefinition.Conf;
var stanleyCrossTrackGainPerSecond =
StanleyCrossTrackGainPerSecond ??
config.ParkingStanleyCrossTrackGain;
var stanleyHeadingErrorGain =
StanleyHeadingErrorGain ??
config.ParkingStanleyHeadingGain;
var stanleyMinimumSpeedMetersPerSecond =
StanleyMinimumSpeedMetersPerSecond ??
config.ParkingStanleyMinimumSpeed;
var stanleyUsesActualSpeed =
StanleyUsesActualSpeed ??
config.ParkingStanleyUseActualSpeed;
var maximumCrossTrackCorrectionRadians =
MaximumCrossTrackCorrectionRadians ??
AngleMath.DegreesToRadians(
config.ParkingMaximumCrossTrackCorrectionDegrees);
var maximumHeadingCorrectionRadians =
MaximumHeadingCorrectionRadians ??
AngleMath.DegreesToRadians(
config.ParkingMaximumHeadingCorrectionDegrees);
var longitudinalKp =
LongitudinalKp ??
config.ParkingLongitudinalKp;
var longitudinalKiPerSecond =
LongitudinalKiPerSecond ??
config.ParkingLongitudinalKi;
var longitudinalKdSeconds =
LongitudinalKdSeconds ??
config.ParkingLongitudinalKd;
var maximumIntegralCorrectionMetersPerSecond =
MaximumIntegralCorrectionMetersPerSecond ??
config.ParkingMaximumIntegralCorrection;
var maximumCommandSpeedMetersPerSecond =
MaximumCommandSpeedMetersPerSecond ??
config.ParkingMaximumCommandSpeed;
var longitudinalSpeedErrorDeadbandMetersPerSecond =
LongitudinalSpeedErrorDeadbandMetersPerSecond ??
config.ParkingLongitudinalSpeedErrorDeadband;
var maximumGcpAngleRadians =
MaximumGcpAngleRadians ??
AngleMath.DegreesToRadians(
config.ParkingMaximumGcpAngleDegrees);
var maximumGcpAngleRateRadiansPerSecond =
MaximumGcpAngleRateRadiansPerSecond ??
AngleMath.DegreesToRadians(
config.ParkingMaximumGcpAngleRateDegreesPerSecond);
var finishDistanceMeters =
FinishDistanceMeters ??
config.ParkingFinishDistance;
var finishSpeedMetersPerSecond =
FinishSpeedMetersPerSecond ??
config.ParkingFinishSpeed;
var finishHeadingToleranceRadians =
FinishHeadingToleranceRadians ??
AngleMath.DegreesToRadians(
config.ParkingFinishHeadingToleranceDegrees);
var terminalBrakingPreviewMeters =
TerminalBrakingPreviewMeters ??
config.ParkingTerminalBrakingPreview;
var maximumDistanceToTrajectoryMeters =
MaximumDistanceToTrajectoryMeters ??
config.ParkingMaximumDistanceToTrajectory;
var executionTimeoutSeconds =
ExecutionTimeoutSeconds ??
config.ParkingExecutionTimeoutSeconds;
ValidateParameters(executionTimeoutSeconds);
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
@@ -161,6 +229,24 @@ namespace MultiWheelC
"当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
}
var wheelPreparation =
new PrepareWheelsForward();
foreach (var keepRunning in wheelPreparation.Get())
{
if (!keepRunning)
{
break;
}
yield return true;
}
if (!wheelPreparation.Completed)
{
throw new InvalidOperationException(
"轨迹跟踪开始前舵轮未能稳定回到车头方向。");
}
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
@@ -170,34 +256,44 @@ namespace MultiWheelC
var stateProvider =
StateProvider ??
new DetourVehicleStateProvider();
ParkingVehicleStateProviderFactory.Create(
chassis,
config);
var controlPointRadiusMeters =
chassis.ControlPointRadius / 1000.0;
var lateralController =
new StanleyLateralController(
controlPointRadiusMeters,
StanleyCrossTrackGainPerSecond,
StanleyHeadingErrorGain,
StanleyMinimumSpeedMetersPerSecond,
StanleyUsesActualSpeed,
MaximumCrossTrackCorrectionRadians,
MaximumHeadingCorrectionRadians);
LateralControllerFactory == null
? new StanleyLateralController(
controlPointRadiusMeters,
stanleyCrossTrackGainPerSecond,
stanleyHeadingErrorGain,
stanleyMinimumSpeedMetersPerSecond,
stanleyUsesActualSpeed,
maximumCrossTrackCorrectionRadians,
maximumHeadingCorrectionRadians)
: LateralControllerFactory(
controlPointRadiusMeters);
if (lateralController == null)
{
throw new InvalidOperationException(
"横向控制器创建委托不能返回空值。");
}
var longitudinalController =
new PidLongitudinalController(
LongitudinalKp,
LongitudinalKiPerSecond,
LongitudinalKdSeconds,
MaximumIntegralCorrectionMetersPerSecond,
MaximumCommandSpeedMetersPerSecond,
LongitudinalSpeedErrorDeadbandMetersPerSecond);
longitudinalKp,
longitudinalKiPerSecond,
longitudinalKdSeconds,
maximumIntegralCorrectionMetersPerSecond,
maximumCommandSpeedMetersPerSecond,
longitudinalSpeedErrorDeadbandMetersPerSecond);
var gcpAllocator =
new GcpCommandAllocator(
MaximumGcpAngleRadians);
maximumGcpAngleRadians);
var commandExecutor =
new GcpCommandExecutor(
adapter,
MaximumGcpAngleRateRadiansPerSecond);
maximumGcpAngleRateRadiansPerSecond);
Controller = new ParkingGeometricController(
stateProvider,
@@ -205,11 +301,11 @@ namespace MultiWheelC
longitudinalController,
gcpAllocator,
commandExecutor,
FinishDistanceMeters,
FinishSpeedMetersPerSecond,
FinishHeadingToleranceRadians,
MaximumDistanceToTrajectoryMeters,
TerminalBrakingPreviewMeters);
finishDistanceMeters,
finishSpeedMetersPerSecond,
finishHeadingToleranceRadians,
maximumDistanceToTrajectoryMeters,
terminalBrakingPreviewMeters);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
@@ -221,10 +317,10 @@ namespace MultiWheelC
while (true)
{
if (clock.Elapsed.TotalSeconds >
ExecutionTimeoutSeconds)
executionTimeoutSeconds)
{
throw new TimeoutException(
$"新版轨迹跟踪超过{ExecutionTimeoutSeconds:F1}s仍未完成。");
$"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
}
var currentCycleSeconds =
@@ -291,7 +387,8 @@ namespace MultiWheelC
/// <summary>
/// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
/// </summary>
private void ValidateParameters()
private void ValidateParameters(
double executionTimeoutSeconds)
{
if (Trajectory == null)
{
@@ -299,9 +396,9 @@ namespace MultiWheelC
"新版轨迹跟踪动作没有设置Trajectory。");
}
if (double.IsNaN(ExecutionTimeoutSeconds) ||
double.IsInfinity(ExecutionTimeoutSeconds) ||
ExecutionTimeoutSeconds <= 0.0)
if (double.IsNaN(executionTimeoutSeconds) ||
double.IsInfinity(executionTimeoutSeconds) ||
executionTimeoutSeconds <= 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(ExecutionTimeoutSeconds),