41 lines
1.9 KiB
C#
41 lines
1.9 KiB
C#
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
|
|
/// <summary>
|
|
/// 从车辆参数提取运动学限制的辅助方法。
|
|
/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
|
|
/// </summary>
|
|
public static class VehicleKinematics
|
|
{
|
|
/// <summary>
|
|
/// 尝试获取车辆允许的最大绝对曲率。
|
|
/// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
|
|
/// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
|
|
/// </summary>
|
|
public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
|
|
{
|
|
maximumCurvaturePerMeter = 0d;
|
|
if (vehicle == null) return false;
|
|
|
|
bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
|
|
bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
|
|
if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
|
|
if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
|
|
if (!hasMaximumCurvature && !hasMinimumRadius) return false;
|
|
|
|
if (hasMaximumCurvature && hasMinimumRadius)
|
|
{
|
|
maximumCurvaturePerMeter = System.Math.Min(
|
|
vehicle.MaximumCurvaturePerMeter.Value,
|
|
1d / vehicle.MinimumTurningRadiusMeters.Value);
|
|
return true;
|
|
}
|
|
|
|
maximumCurvaturePerMeter = hasMaximumCurvature
|
|
? vehicle.MaximumCurvaturePerMeter.Value
|
|
: 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
|
return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
|
|
}
|
|
}
|