307 lines
11 KiB
C#
307 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using ClumsyCore.Interfaces;
|
|
using ClumsyCore.Pilot;
|
|
using CommonUsage.Chassis;
|
|
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>
|
|
/// 获取或设置每个有效控制周期结束后的诊断数据观察回调。
|
|
/// </summary>
|
|
public Action<ParkingGeometricController> CycleObserver;
|
|
|
|
/// <summary>
|
|
/// Stanley横向误差增益，单位为1/s。
|
|
/// </summary>
|
|
public double StanleyCrossTrackGainPerSecond = 0.4;
|
|
|
|
/// <summary>
|
|
/// Stanley航向误差增益。
|
|
/// </summary>
|
|
public double StanleyHeadingErrorGain = 1.0;
|
|
|
|
/// <summary>
|
|
/// Stanley低速分母保护速度，单位为m/s。
|
|
/// </summary>
|
|
public double StanleyMinimumSpeedMetersPerSecond = 0.15;
|
|
|
|
/// <summary>
|
|
/// 获取或设置Stanley是否优先使用Detour估算的实际速度。
|
|
/// </summary>
|
|
public bool StanleyUsesActualSpeed = true;
|
|
|
|
/// <summary>
|
|
/// Stanley横向误差共同转角分量的最大绝对值，单位为rad。
|
|
/// </summary>
|
|
public double MaximumCrossTrackCorrectionRadians =
|
|
AngleMath.DegreesToRadians(10.0);
|
|
|
|
/// <summary>
|
|
/// Stanley航向误差差动转角分量的最大绝对值，单位为rad。
|
|
/// </summary>
|
|
public double MaximumHeadingCorrectionRadians =
|
|
AngleMath.DegreesToRadians(10.0);
|
|
|
|
/// <summary>
|
|
/// 纵向速度外环比例增益。
|
|
/// </summary>
|
|
public double LongitudinalKp = 0.5;
|
|
|
|
/// <summary>
|
|
/// 纵向速度外环积分增益，单位为1/s。
|
|
/// </summary>
|
|
public double LongitudinalKiPerSecond;
|
|
|
|
/// <summary>
|
|
/// 纵向速度外环微分增益，单位为s。
|
|
/// </summary>
|
|
public double LongitudinalKdSeconds;
|
|
|
|
/// <summary>
|
|
/// 纵向积分项允许产生的最大速度修正绝对值，单位为m/s。
|
|
/// </summary>
|
|
public double MaximumIntegralCorrectionMetersPerSecond = 0.05;
|
|
|
|
/// <summary>
|
|
/// 纵向PID不进行反馈修正的速度误差死区，单位为m/s。
|
|
/// </summary>
|
|
public double LongitudinalSpeedErrorDeadbandMetersPerSecond =
|
|
0.025;
|
|
|
|
/// <summary>
|
|
/// 底盘纵向命令速度绝对值上限，单位为m/s。
|
|
/// </summary>
|
|
public double MaximumCommandSpeedMetersPerSecond = 0.50;
|
|
|
|
/// <summary>
|
|
/// 前后GCP允许的最大转角绝对值，单位为rad。
|
|
/// </summary>
|
|
public double MaximumGcpAngleRadians =
|
|
AngleMath.DegreesToRadians(45.0);
|
|
|
|
/// <summary>
|
|
/// 前后GCP目标转角最大变化率，单位为rad/s。
|
|
/// </summary>
|
|
public double MaximumGcpAngleRateRadiansPerSecond =
|
|
AngleMath.DegreesToRadians(15.0);
|
|
|
|
/// <summary>
|
|
/// 终点位置和剩余弧长的完成容差，单位为m。
|
|
/// </summary>
|
|
public double FinishDistanceMeters = 0.03;
|
|
|
|
/// <summary>
|
|
/// 终点停稳判定允许的实际线速度，单位为m/s。
|
|
/// </summary>
|
|
public double FinishSpeedMetersPerSecond = 0.02;
|
|
|
|
/// <summary>
|
|
/// 终点航向完成容差，单位为rad。
|
|
/// </summary>
|
|
public double FinishHeadingToleranceRadians =
|
|
AngleMath.DegreesToRadians(3.0);
|
|
|
|
/// <summary>
|
|
/// 车辆允许偏离参考轨迹的最大欧氏距离，单位为m。
|
|
/// </summary>
|
|
public double MaximumDistanceToTrajectoryMeters = 0.50;
|
|
|
|
/// <summary>
|
|
/// 单次轨迹动作允许的最长执行时间，单位为s。
|
|
/// </summary>
|
|
public double ExecutionTimeoutSeconds = 120.0;
|
|
|
|
/// <summary>
|
|
/// 获取本次动作创建的控制器，尚未开始时为空。
|
|
/// </summary>
|
|
public ParkingGeometricController Controller { get; private set; }
|
|
|
|
/// <summary>
|
|
/// 创建控制器并持续执行控制周期，直到轨迹完成、失败或动作被取消。
|
|
/// </summary>
|
|
public override IEnumerable<bool> Get()
|
|
{
|
|
ValidateParameters();
|
|
|
|
var chassis =
|
|
PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"当前底盘不是MultiWheelChassis，无法执行新版轨迹跟踪动作。");
|
|
}
|
|
|
|
var adapter = new MultiWheelChassisAdapter(
|
|
chassis,
|
|
PilotDefinition.Self.CarNum);
|
|
|
|
// 新版GCP控制统一以真实车头为车体X正方向，避免继承上一次蟹行偏置。
|
|
adapter.ResetToBodyFrame();
|
|
|
|
var stateProvider =
|
|
StateProvider ??
|
|
new DetourVehicleStateProvider();
|
|
var controlPointRadiusMeters =
|
|
chassis.ControlPointRadius / 1000.0;
|
|
|
|
var lateralController =
|
|
new StanleyLateralController(
|
|
controlPointRadiusMeters,
|
|
StanleyCrossTrackGainPerSecond,
|
|
StanleyHeadingErrorGain,
|
|
StanleyMinimumSpeedMetersPerSecond,
|
|
StanleyUsesActualSpeed,
|
|
MaximumCrossTrackCorrectionRadians,
|
|
MaximumHeadingCorrectionRadians);
|
|
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);
|
|
|
|
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()
|
|
{
|
|
if (Trajectory == null)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"新版轨迹跟踪动作没有设置Trajectory。");
|
|
}
|
|
|
|
if (double.IsNaN(ExecutionTimeoutSeconds) ||
|
|
double.IsInfinity(ExecutionTimeoutSeconds) ||
|
|
ExecutionTimeoutSeconds <= 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(ExecutionTimeoutSeconds),
|
|
"轨迹跟踪超时时间必须是正有限值。");
|
|
}
|
|
}
|
|
}
|
|
}
|