410 lines
16 KiB
C#
410 lines
16 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横向修正上限覆盖值，单位为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? 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 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;
|
|
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);
|
|
|
|
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);
|
|
|
|
if (Controller.LastVehicleState.HasValue)
|
|
{
|
|
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),
|
|
"轨迹跟踪超时时间必须是正有限值。");
|
|
}
|
|
}
|
|
}
|
|
}
|