127 lines
4.4 KiB
C#
127 lines
4.4 KiB
C#
using System;
|
|
using MultiWheelC.StateEstimation;
|
|
using MultiWheelC.Trajectory;
|
|
|
|
namespace MultiWheelC.Control.Abstractions
|
|
{
|
|
/// <summary>
|
|
/// 保存一次轨迹跟踪控制周期使用的车辆状态、轨迹投影和真实时间间隔。
|
|
/// </summary>
|
|
public readonly struct PathTrackingContext
|
|
{
|
|
/// <summary>
|
|
/// 创建横向和纵向控制器共享的只读控制输入快照。
|
|
/// </summary>
|
|
public PathTrackingContext(
|
|
VehicleState vehicleState,
|
|
TrajectoryProjection projection,
|
|
double controlReferenceSpeedMetersPerSecond,
|
|
double deltaTimeSeconds)
|
|
{
|
|
EnsureFinite(
|
|
controlReferenceSpeedMetersPerSecond,
|
|
nameof(controlReferenceSpeedMetersPerSecond));
|
|
EnsureFinitePositive(
|
|
deltaTimeSeconds,
|
|
nameof(deltaTimeSeconds));
|
|
|
|
VehicleState = vehicleState;
|
|
Projection = projection;
|
|
ControlReferenceSpeedMetersPerSecond =
|
|
controlReferenceSpeedMetersPerSecond;
|
|
DeltaTimeSeconds = deltaTimeSeconds;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取本周期经过校验的实际车辆位姿和速度状态。
|
|
/// </summary>
|
|
public VehicleState VehicleState { get; }
|
|
|
|
/// <summary>
|
|
/// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
|
|
/// </summary>
|
|
public TrajectoryProjection Projection { get; }
|
|
|
|
/// <summary>
|
|
/// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
|
|
/// </summary>
|
|
public double DeltaTimeSeconds { get; }
|
|
|
|
/// <summary>
|
|
/// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
|
|
/// </summary>
|
|
public double ControlReferenceSpeedMetersPerSecond { get; }
|
|
|
|
/// <summary>
|
|
/// 获取车辆在车体X轴方向上的实际纵向速度，单位为m/s。
|
|
/// </summary>
|
|
public double ActualLongitudinalSpeedMetersPerSecond =>
|
|
VehicleState.TwistInBody
|
|
.VxMetersPerSecond;
|
|
|
|
/// <summary>
|
|
/// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
|
|
/// </summary>
|
|
public double ReferenceCurvaturePerMeter =>
|
|
Projection.ReferencePoint
|
|
.CurvaturePerMeter;
|
|
|
|
/// <summary>
|
|
/// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
|
|
/// </summary>
|
|
public double LateralErrorMeters =>
|
|
Projection.LateralErrorMeters;
|
|
|
|
/// <summary>
|
|
/// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
|
|
/// </summary>
|
|
public double HeadingErrorRadians =>
|
|
Projection.HeadingErrorRadians;
|
|
|
|
/// <summary>
|
|
/// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
|
|
/// </summary>
|
|
public double RemainingDistanceMeters =>
|
|
Projection.RemainingDistanceMeters;
|
|
|
|
/// <summary>
|
|
/// 获取实际速度是否已经由至少两个连续有效定位样本估算得到。
|
|
/// </summary>
|
|
public bool HasValidVelocityEstimate =>
|
|
VehicleState.HasValidVelocityEstimate;
|
|
|
|
/// <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 EnsureFinite(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
if (double.IsNaN(value) ||
|
|
double.IsInfinity(value))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"轨迹跟踪参考速度必须是有限值。");
|
|
}
|
|
}
|
|
}
|
|
}
|