454 lines
19 KiB
C#
454 lines
19 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
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;
|
|
using MultiWheelC.Control.Longitudinal;
|
|
using MultiWheelC.StateEstimation;
|
|
using MultiWheelC.Trajectory;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
/// <summary>
|
|
/// 使用新版横纵向控制器持续跟踪一条世界坐标系二维轨迹。
|
|
/// </summary>
|
|
public sealed class TrajectoryTrackingMovement
|
|
: MovementDefinition
|
|
{
|
|
/// <summary>
|
|
/// 获取或设置本次动作需要跟踪的世界坐标系轨迹。
|
|
/// </summary>
|
|
public Trajectory2D Trajectory;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作使用的车辆状态源；为空时组合Detour位姿与电机反馈速度。
|
|
/// </summary>
|
|
public IVehicleStateProvider StateProvider;
|
|
|
|
/// <summary>
|
|
/// 获取或设置横向控制器创建委托；参数为车辆控制点半径（m），为空时使用配置化Stanley控制器。
|
|
/// </summary>
|
|
public Func<double, ILateralController>
|
|
LateralControllerFactory;
|
|
|
|
/// <summary>
|
|
/// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
|
|
/// </summary>
|
|
public Action<ParkingGeometricController> CycleObserver;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley横向误差增益覆盖值，单位为1/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? StanleyCrossTrackGainPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley航向误差增益覆盖值；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? StanleyHeadingErrorGain;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley低速分母保护速度覆盖值，单位为m/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? StanleyMinimumSpeedMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作是否使用实际纵向速度的覆盖值；为空时读取车辆配置。
|
|
/// </summary>
|
|
public bool? StanleyUsesActualSpeed;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley曲率前馈预瞄时间覆盖值，单位为s，0为关闭；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? StanleyCurvaturePreviewSeconds;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley曲率前馈最大预瞄距离覆盖值，单位为m；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? StanleyMaximumCurvaturePreviewMeters;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley横向修正上限覆盖值，单位为rad；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumCrossTrackCorrectionRadians;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的Stanley航向修正上限覆盖值，单位为rad；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumHeadingCorrectionRadians;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的纵向速度比例增益覆盖值；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? LongitudinalKp;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的纵向速度积分增益覆盖值，单位为1/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? LongitudinalKiPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的纵向速度微分增益覆盖值，单位为s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? LongitudinalKdSeconds;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的纵向积分修正上限覆盖值，单位为m/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumIntegralCorrectionMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的纵向速度误差死区覆盖值，单位为m/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? LongitudinalSpeedErrorDeadbandMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的底盘纵向命令速度上限覆盖值，单位为m/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumCommandSpeedMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的GCP转角上限覆盖值，单位为rad；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumGcpAngleRadians;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的GCP转角变化率上限覆盖值，单位为rad/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumGcpAngleRateRadiansPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的终点距离容差覆盖值，单位为m；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? FinishDistanceMeters;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的终点速度容差覆盖值，单位为m/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? FinishSpeedMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的终点航向容差覆盖值，单位为rad；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? FinishHeadingToleranceRadians;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的终点制动预瞄距离覆盖值，单位为m；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? TerminalBrakingPreviewMeters;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作进入终点单向低速逼近的剩余弧长覆盖值，单位为m；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? TerminalApproachDistanceMeters;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作由终点纵向剩余距离生成低速参考的比例增益覆盖值，单位为1/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? TerminalApproachGainPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作终点单向逼近参考速度的最大绝对值覆盖值，单位为m/s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumTerminalApproachSpeedMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? MaximumDistanceToTrajectoryMeters;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次动作的执行超时覆盖值，单位为s；为空时读取车辆配置。
|
|
/// </summary>
|
|
public double? ExecutionTimeoutSeconds;
|
|
|
|
/// <summary>
|
|
/// 获取本次动作创建的控制器，尚未开始时为空。
|
|
/// </summary>
|
|
public ParkingGeometricController Controller { get; private set; }
|
|
|
|
/// <summary>
|
|
/// 等待舵轮稳定回正后创建控制器并持续执行，直到轨迹完成、失败或动作被取消。
|
|
/// </summary>
|
|
public override IEnumerable<bool> Get()
|
|
{
|
|
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 stanleyCurvaturePreviewSeconds =
|
|
StanleyCurvaturePreviewSeconds ??
|
|
config.ParkingStanleyCurvaturePreviewSeconds;
|
|
var stanleyMaximumCurvaturePreviewMeters =
|
|
StanleyMaximumCurvaturePreviewMeters ??
|
|
config.ParkingStanleyMaximumCurvaturePreviewMeters;
|
|
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 terminalApproachDistanceMeters =
|
|
TerminalApproachDistanceMeters ??
|
|
config.ParkingTerminalApproachDistance;
|
|
var terminalApproachGainPerSecond =
|
|
TerminalApproachGainPerSecond ??
|
|
config.ParkingTerminalApproachGain;
|
|
var maximumTerminalApproachSpeedMetersPerSecond =
|
|
MaximumTerminalApproachSpeedMetersPerSecond ??
|
|
config.ParkingTerminalMaximumApproachSpeed;
|
|
var maximumDistanceToTrajectoryMeters =
|
|
MaximumDistanceToTrajectoryMeters ??
|
|
config.ParkingMaximumDistanceToTrajectory;
|
|
var executionTimeoutSeconds =
|
|
ExecutionTimeoutSeconds ??
|
|
config.ParkingExecutionTimeoutSeconds;
|
|
|
|
ValidateParameters(executionTimeoutSeconds);
|
|
|
|
var chassis =
|
|
PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"当前底盘不是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);
|
|
|
|
// 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
|
|
adapter.ResetToBodyFrame();
|
|
|
|
var stateProvider =
|
|
StateProvider ??
|
|
ParkingVehicleStateProviderFactory.Create(
|
|
chassis,
|
|
config);
|
|
var controlPointRadiusMeters =
|
|
chassis.ControlPointRadius / 1000.0;
|
|
|
|
var lateralController =
|
|
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);
|
|
var gcpAllocator =
|
|
new GcpCommandAllocator(
|
|
maximumGcpAngleRadians);
|
|
var commandExecutor =
|
|
new GcpCommandExecutor(
|
|
adapter,
|
|
maximumGcpAngleRateRadiansPerSecond);
|
|
|
|
Controller = new ParkingGeometricController(
|
|
stateProvider,
|
|
lateralController,
|
|
longitudinalController,
|
|
gcpAllocator,
|
|
commandExecutor,
|
|
finishDistanceMeters,
|
|
finishSpeedMetersPerSecond,
|
|
finishHeadingToleranceRadians,
|
|
maximumDistanceToTrajectoryMeters,
|
|
terminalBrakingPreviewMeters,
|
|
terminalApproachDistanceMeters,
|
|
terminalApproachGainPerSecond,
|
|
maximumTerminalApproachSpeedMetersPerSecond,
|
|
stanleyCurvaturePreviewSeconds,
|
|
stanleyMaximumCurvaturePreviewMeters);
|
|
|
|
var clock = Stopwatch.StartNew();
|
|
var previousCycleSeconds =
|
|
clock.Elapsed.TotalSeconds;
|
|
Controller.Start(Trajectory);
|
|
|
|
try
|
|
{
|
|
while (true)
|
|
{
|
|
if (clock.Elapsed.TotalSeconds >
|
|
executionTimeoutSeconds)
|
|
{
|
|
throw new TimeoutException(
|
|
$"新版轨迹跟踪超过{executionTimeoutSeconds:F1}s仍未完成。");
|
|
}
|
|
|
|
var currentCycleSeconds =
|
|
clock.Elapsed.TotalSeconds;
|
|
var deltaTimeSeconds =
|
|
currentCycleSeconds -
|
|
previousCycleSeconds;
|
|
previousCycleSeconds =
|
|
currentCycleSeconds;
|
|
|
|
// 极短首周期不参与PID和GCP角速度限制，等待调度器进入下一周期。
|
|
if (deltaTimeSeconds <= 1e-6)
|
|
{
|
|
yield return true;
|
|
continue;
|
|
}
|
|
|
|
var result =
|
|
Controller.ExecuteCycle(
|
|
deltaTimeSeconds);
|
|
|
|
// 诊断观察器按真实控制周期触发，即使本周期状态不可用，
|
|
// 也允许记录状态读取和主动停车所消耗的时间。
|
|
CycleObserver?.Invoke(Controller);
|
|
|
|
if (result ==
|
|
ParkingControlCycleResult.Completed)
|
|
{
|
|
break;
|
|
}
|
|
|
|
if (result ==
|
|
ParkingControlCycleResult.Faulted)
|
|
{
|
|
throw new InvalidOperationException(
|
|
string.IsNullOrWhiteSpace(
|
|
Controller.LastFailureReason)
|
|
? "新版轨迹跟踪控制器发生未知故障。"
|
|
: Controller.LastFailureReason,
|
|
Controller.LastException);
|
|
}
|
|
|
|
if (result ==
|
|
ParkingControlCycleResult.Inactive)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"新版轨迹跟踪控制器在轨迹完成前意外停止活动。");
|
|
}
|
|
|
|
// CommandSent和短暂StateUnavailable均继续下一控制周期；
|
|
// 后者已经由控制器主动停车，等待Detour恢复。
|
|
yield return true;
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
Controller.Cancel();
|
|
}
|
|
|
|
yield return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 在接管实际底盘前检查动作自身无法由子控制器检查的参数。
|
|
/// </summary>
|
|
private void ValidateParameters(
|
|
double executionTimeoutSeconds)
|
|
{
|
|
if (Trajectory == null)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"新版轨迹跟踪动作没有设置Trajectory。");
|
|
}
|
|
|
|
if (double.IsNaN(executionTimeoutSeconds) ||
|
|
double.IsInfinity(executionTimeoutSeconds) ||
|
|
executionTimeoutSeconds <= 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(ExecutionTimeoutSeconds),
|
|
"轨迹跟踪超时时间必须是正有限值。");
|
|
}
|
|
}
|
|
}
|
|
}
|