chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,146 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 为 Hybrid A* Open List 提供确定性优先级的二叉最小堆。
|
||||
/// 排序严格依次比较 F、H、较大的 G 和插入序号；F、H、G 必须为有限且非负的等效米代价。
|
||||
/// </summary>
|
||||
/// <typeparam name="T">与一组搜索代价关联的节点或条目类型。</typeparam>
|
||||
public sealed class BinaryMinHeap<T>
|
||||
{
|
||||
private readonly List<HeapEntry> _entries = new List<HeapEntry>();
|
||||
private long _nextInsertionSequence;
|
||||
|
||||
/// <summary>创建空的确定性 Open List 堆。</summary>
|
||||
public BinaryMinHeap()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>当前堆内尚未出队的条目数量。</summary>
|
||||
public int Count { get { return _entries.Count; } }
|
||||
|
||||
/// <summary>
|
||||
/// 将一个条目和其搜索排序代价压入堆。
|
||||
/// 参数：item 为关联条目；f、h、g 均为有限且非负的等效米代价。
|
||||
/// 失败：任一代价无效、item 为 null（仅引用类型）或插入序号耗尽时抛出异常。
|
||||
/// </summary>
|
||||
public void Push(T item, double f, double h, double g)
|
||||
{
|
||||
if (ReferenceEquals(item, null)) throw new ArgumentNullException(nameof(item));
|
||||
ValidateCost(f, nameof(f));
|
||||
ValidateCost(h, nameof(h));
|
||||
ValidateCost(g, nameof(g));
|
||||
if (_nextInsertionSequence == long.MaxValue)
|
||||
throw new InvalidOperationException("The binary heap insertion sequence has been exhausted.");
|
||||
|
||||
var entry = new HeapEntry(item, f, h, g, _nextInsertionSequence);
|
||||
_nextInsertionSequence++;
|
||||
_entries.Add(entry);
|
||||
SiftUp(_entries.Count - 1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 弹出当前排序最优的条目。
|
||||
/// 返回：按 F、H、较大 G 和插入序号排序后的最小条目；空堆时抛出 <see cref="InvalidOperationException"/>。
|
||||
/// </summary>
|
||||
public T Pop()
|
||||
{
|
||||
if (_entries.Count == 0) throw new InvalidOperationException("The binary heap is empty.");
|
||||
|
||||
HeapEntry result = _entries[0];
|
||||
int lastIndex = _entries.Count - 1;
|
||||
if (lastIndex == 0)
|
||||
{
|
||||
_entries.RemoveAt(0);
|
||||
return result.Item;
|
||||
}
|
||||
|
||||
_entries[0] = _entries[lastIndex];
|
||||
_entries.RemoveAt(lastIndex);
|
||||
SiftDown(0);
|
||||
return result.Item;
|
||||
}
|
||||
|
||||
/// <summary>清空尚未出队的条目；后续插入序号继续单调递增以保持整个实例内的确定性。</summary>
|
||||
public void Clear()
|
||||
{
|
||||
_entries.Clear();
|
||||
}
|
||||
|
||||
private void SiftUp(int index)
|
||||
{
|
||||
while (index > 0)
|
||||
{
|
||||
int parentIndex = (index - 1) / 2;
|
||||
if (Compare(_entries[index], _entries[parentIndex]) >= 0) return;
|
||||
Swap(index, parentIndex);
|
||||
index = parentIndex;
|
||||
}
|
||||
}
|
||||
|
||||
private void SiftDown(int index)
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
int leftChildIndex = index * 2 + 1;
|
||||
if (leftChildIndex >= _entries.Count) return;
|
||||
|
||||
int bestChildIndex = leftChildIndex;
|
||||
int rightChildIndex = leftChildIndex + 1;
|
||||
if (rightChildIndex < _entries.Count && Compare(_entries[rightChildIndex], _entries[leftChildIndex]) < 0)
|
||||
bestChildIndex = rightChildIndex;
|
||||
|
||||
if (Compare(_entries[bestChildIndex], _entries[index]) >= 0) return;
|
||||
Swap(index, bestChildIndex);
|
||||
index = bestChildIndex;
|
||||
}
|
||||
}
|
||||
|
||||
private void Swap(int firstIndex, int secondIndex)
|
||||
{
|
||||
HeapEntry temporary = _entries[firstIndex];
|
||||
_entries[firstIndex] = _entries[secondIndex];
|
||||
_entries[secondIndex] = temporary;
|
||||
}
|
||||
|
||||
private static int Compare(HeapEntry left, HeapEntry right)
|
||||
{
|
||||
int comparison = left.F.CompareTo(right.F);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = left.H.CompareTo(right.H);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = right.G.CompareTo(left.G);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return left.InsertionSequence.CompareTo(right.InsertionSequence);
|
||||
}
|
||||
|
||||
private static void ValidateCost(double value, string parameterName)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName, "Search costs must be finite and non-negative.");
|
||||
}
|
||||
|
||||
private sealed class HeapEntry
|
||||
{
|
||||
public HeapEntry(T item, double f, double h, double g, long insertionSequence)
|
||||
{
|
||||
Item = item;
|
||||
F = f;
|
||||
H = h;
|
||||
G = g;
|
||||
InsertionSequence = insertionSequence;
|
||||
}
|
||||
|
||||
public T Item { get; }
|
||||
public double F { get; }
|
||||
public double H { get; }
|
||||
public double G { get; }
|
||||
public long InsertionSequence { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>按位置、航向和进入方向约束判定连续位姿是否可以作为目标候选。</summary>
|
||||
public static class GoalToleranceChecker
|
||||
{
|
||||
/// <summary>
|
||||
/// 判断一个位姿是否满足目标容差和进入方向约束。
|
||||
/// 参数：pose 与 goal 使用世界 m/rad；configuration 提供位置 m 和航向 rad 容差；direction 为候选末段方向；goalDirection 为目标进入方向约束。
|
||||
/// 返回：输入有限、位置距离和最小环形航向误差均不超过容差且方向匹配时为 true；无效输入保守地返回 false。
|
||||
/// </summary>
|
||||
public static bool IsSatisfied(
|
||||
Pose2D pose,
|
||||
Pose2D goal,
|
||||
HybridAStarConfiguration configuration,
|
||||
TravelDirection direction,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsFinitePose(pose) || !IsFinitePose(goal) || configuration == null ||
|
||||
!NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) ||
|
||||
!NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) ||
|
||||
configuration.GoalPositionToleranceMeters < 0d || configuration.GoalHeadingToleranceRadians < 0d ||
|
||||
!IsTravelDirection(direction) || !IsGoalDirection(goalDirection))
|
||||
return false;
|
||||
|
||||
if (!MatchesDirection(direction, goalDirection)) return false;
|
||||
|
||||
double deltaX = pose.X - goal.X;
|
||||
double deltaY = pose.Y - goal.Y;
|
||||
double positionDistanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
if (!NumericGuard.IsFinite(positionDistanceMeters) || positionDistanceMeters > configuration.GoalPositionToleranceMeters)
|
||||
return false;
|
||||
|
||||
double headingDifferenceRadians = Math.Abs(AngleMath.ShortestSignedDifference(pose.Heading, goal.Heading));
|
||||
return NumericGuard.IsFinite(headingDifferenceRadians) && headingDifferenceRadians <= configuration.GoalHeadingToleranceRadians;
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private static bool MatchesDirection(TravelDirection direction, GoalDirectionConstraint constraint)
|
||||
{
|
||||
return constraint == GoalDirectionConstraint.Any ||
|
||||
(constraint == GoalDirectionConstraint.Forward && direction == TravelDirection.Forward) ||
|
||||
(constraint == GoalDirectionConstraint.Reverse && direction == TravelDirection.Reverse);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 基于不可变栅格地图的目标反向八邻域 Dijkstra 启发式。
|
||||
/// 距离单位为 m；对角移动仅在两个对应正交邻格都未占据时允许，以避免从障碍夹角穿越。
|
||||
/// </summary>
|
||||
public sealed class GridDijkstraHeuristic
|
||||
{
|
||||
private readonly PlanningGridMap _map;
|
||||
private readonly double[] _costs;
|
||||
|
||||
/// <summary>
|
||||
/// 从目标栅格预计算所有可达自由格到目标的二维最短距离。
|
||||
/// 参数：map 必须是已就绪的不可变地图；goalRow、goalCol 为地图内且未占据的目标格索引。
|
||||
/// 失败：地图为空、未就绪或目标格无效时抛出异常。
|
||||
/// </summary>
|
||||
public GridDijkstraHeuristic(PlanningGridMap map, int goalRow, int goalCol)
|
||||
{
|
||||
if (!TryCreate(map, goalRow, goalCol, PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||||
out GridDijkstraHeuristic heuristic, out _))
|
||||
throw new InvalidOperationException("Unbounded Dijkstra construction unexpectedly stopped.");
|
||||
_map = heuristic._map;
|
||||
_costs = heuristic._costs;
|
||||
}
|
||||
|
||||
private GridDijkstraHeuristic(PlanningGridMap map, double[] costs)
|
||||
{
|
||||
_map = map;
|
||||
_costs = costs;
|
||||
}
|
||||
|
||||
/// <summary>使用共享预算创建完整二维启发式；停止时不返回部分成本数组。</summary>
|
||||
internal static bool TryCreate(PlanningGridMap map, int goalRow, int goalCol, PlanningOperationBudget budget,
|
||||
out GridDijkstraHeuristic heuristic, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (!map.PlanningReady) throw new ArgumentException("The planning map must be ready.", nameof(map));
|
||||
if (goalRow < 0 || goalRow >= map.Rows || goalCol < 0 || goalCol >= map.Cols)
|
||||
throw new ArgumentOutOfRangeException(nameof(goalRow));
|
||||
if (map.IsOccupied(goalRow, goalCol))
|
||||
throw new ArgumentException("The goal grid cell must be free.", nameof(goalRow));
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
|
||||
heuristic = null;
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
var costs = new double[checked(map.Rows * map.Cols)];
|
||||
int workItemCount = 0;
|
||||
for (int index = 0; index < costs.Length; index++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
costs[index] = double.PositiveInfinity;
|
||||
}
|
||||
if (!TryBuild(map, costs, goalRow, goalCol, budget, ref workItemCount, out stopReason)) return false;
|
||||
heuristic = new GridDijkstraHeuristic(map, costs);
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定栅格到构造时目标格的二维最短距离。
|
||||
/// 参数：row、col 为从零开始的栅格索引。
|
||||
/// 返回：单位 m 的有限最短距离；自由格不可达或索引越界时返回正无穷。
|
||||
/// </summary>
|
||||
public double GetCost(int row, int col)
|
||||
{
|
||||
return row < 0 || row >= _map.Rows || col < 0 || col >= _map.Cols
|
||||
? double.PositiveInfinity
|
||||
: _costs[row * _map.Cols + col];
|
||||
}
|
||||
|
||||
private static bool TryBuild(PlanningGridMap map, double[] costs, int goalRow, int goalCol,
|
||||
PlanningOperationBudget budget, ref int workItemCount, out PlanningOperationStopReason stopReason)
|
||||
{
|
||||
var openList = new BinaryMinHeap<int>();
|
||||
int goalIndex = goalRow * map.Cols + goalCol;
|
||||
costs[goalIndex] = 0d;
|
||||
openList.Push(goalIndex, 0d, 0d, 0d);
|
||||
|
||||
while (openList.Count > 0)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
int currentIndex = openList.Pop();
|
||||
int currentRow = currentIndex / map.Cols;
|
||||
int currentCol = currentIndex % map.Cols;
|
||||
double currentCost = costs[currentIndex];
|
||||
|
||||
for (int rowOffset = -1; rowOffset <= 1; rowOffset++)
|
||||
for (int colOffset = -1; colOffset <= 1; colOffset++)
|
||||
{
|
||||
stopReason = budget.CheckEvery(ref workItemCount);
|
||||
if (stopReason != PlanningOperationStopReason.None) return false;
|
||||
if (rowOffset == 0 && colOffset == 0) continue;
|
||||
|
||||
int nextRow = currentRow + rowOffset;
|
||||
int nextCol = currentCol + colOffset;
|
||||
if (nextRow < 0 || nextRow >= map.Rows || nextCol < 0 || nextCol >= map.Cols || map.IsOccupied(nextRow, nextCol))
|
||||
continue;
|
||||
|
||||
bool isDiagonal = rowOffset != 0 && colOffset != 0;
|
||||
if (isDiagonal && (map.IsOccupied(currentRow + rowOffset, currentCol) || map.IsOccupied(currentRow, currentCol + colOffset)))
|
||||
continue;
|
||||
|
||||
double stepCost = isDiagonal ? Math.Sqrt(2d) * map.ResolutionMeters : map.ResolutionMeters;
|
||||
double candidateCost = currentCost + stepCost;
|
||||
int nextIndex = nextRow * map.Cols + nextCol;
|
||||
if (candidateCost >= costs[nextIndex]) continue;
|
||||
|
||||
costs[nextIndex] = candidateCost;
|
||||
openList.Push(nextIndex, candidateCost, 0d, 0d);
|
||||
}
|
||||
}
|
||||
stopReason = PlanningOperationStopReason.None;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 运行期节点。
|
||||
/// 节点保留连续位姿和其离散闭集键；搜索过程中不会修改已创建节点，改进代价时会追加新节点并使旧 Open List 条目失效。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个搜索节点。
|
||||
/// 参数：nodeIndex 为本次搜索内稳定索引；key 为离散状态；pose 为连续车辆中心位姿；parentNodeIndex 为父节点索引，根节点使用 -1；
|
||||
/// incomingPrimitive 为父节点到本节点的原语，根节点为 null；curvaturePerMeter、gCostMeters 和 hCostMeters 均使用规划器的标准单位。
|
||||
/// </summary>
|
||||
public HybridAStarNode(
|
||||
int nodeIndex,
|
||||
HybridAStarNodeKey key,
|
||||
Pose2D pose,
|
||||
int parentNodeIndex,
|
||||
MotionPrimitive incomingPrimitive,
|
||||
double curvaturePerMeter,
|
||||
double gCostMeters,
|
||||
double hCostMeters)
|
||||
: this(nodeIndex, key, pose, parentNodeIndex, incomingPrimitive, curvaturePerMeter, gCostMeters, hCostMeters, false)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建一个搜索节点，并显式指定它是否为原语内部命中的终点候选。
|
||||
/// 参数：isGoalCandidate 为 true 时，本节点不参与普通离散状态的最优 G 值支配；它仍必须在从 Open List 出队后重新通过连续终点和碰撞检查才能成功。
|
||||
/// </summary>
|
||||
public HybridAStarNode(
|
||||
int nodeIndex,
|
||||
HybridAStarNodeKey key,
|
||||
Pose2D pose,
|
||||
int parentNodeIndex,
|
||||
MotionPrimitive incomingPrimitive,
|
||||
double curvaturePerMeter,
|
||||
double gCostMeters,
|
||||
double hCostMeters,
|
||||
bool isGoalCandidate)
|
||||
{
|
||||
if (key == null) throw new ArgumentNullException(nameof(key));
|
||||
if (pose == null) throw new ArgumentNullException(nameof(pose));
|
||||
|
||||
NodeIndex = nodeIndex;
|
||||
Key = key;
|
||||
Pose = pose;
|
||||
ParentNodeIndex = parentNodeIndex;
|
||||
IncomingPrimitive = incomingPrimitive;
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
GCostMeters = gCostMeters;
|
||||
HCostMeters = hCostMeters;
|
||||
IsGoalCandidate = isGoalCandidate;
|
||||
}
|
||||
|
||||
/// <summary>本次搜索节点数组中的稳定索引。</summary>
|
||||
public int NodeIndex { get; }
|
||||
|
||||
/// <summary>本节点用于 Open/Closed 状态管理的离散键。</summary>
|
||||
public HybridAStarNodeKey Key { get; }
|
||||
|
||||
/// <summary>未量化的连续车辆中心位姿。</summary>
|
||||
public Pose2D Pose { get; }
|
||||
|
||||
/// <summary>父节点稳定索引；根节点为 -1。</summary>
|
||||
public int ParentNodeIndex { get; }
|
||||
|
||||
/// <summary>由父节点驶入本节点的已连续碰撞检查原语；根节点为 null。</summary>
|
||||
public MotionPrimitive IncomingPrimitive { get; }
|
||||
|
||||
/// <summary>与 <see cref="IncomingPrimitive"/> 含义相同的原语别名。</summary>
|
||||
public MotionPrimitive Primitive { get { return IncomingPrimitive; } }
|
||||
|
||||
/// <summary>当前末段采用的车辆曲率，单位 1/m。</summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>当前节点的累计等效米代价。</summary>
|
||||
public double GCostMeters { get; }
|
||||
|
||||
/// <summary>当前节点的二维启发式等效米代价。</summary>
|
||||
public double HCostMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 本节点是否由原语内部首次满足终点条件而生成。
|
||||
/// 终点候选必须保留连续位姿，不能因同一离散键的普通低代价节点而被 Open List 准入规则压制。
|
||||
/// </summary>
|
||||
public bool IsGoalCandidate { get; }
|
||||
|
||||
/// <summary>Open List 排序使用的总等效米代价。</summary>
|
||||
public double FCostMeters { get { return GCostMeters + HCostMeters; } }
|
||||
|
||||
/// <summary>本节点末段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get { return Key.Direction; } }
|
||||
|
||||
/// <summary>本节点末段的曲率等级索引。</summary>
|
||||
public int CurvatureLevelIndex { get { return Key.CurvatureLevelIndex; } }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// Hybrid A* 闭集使用的离散状态键。
|
||||
/// 位置使用地图行列索引；航向、行驶方向和曲率等级共同保留车辆运动学状态，避免把同一栅格中的不同可达姿态错误合并。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarNodeKey : IEquatable<HybridAStarNodeKey>
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建一个离散 Hybrid A* 状态键。
|
||||
/// 参数：row、column 为零开始的地图行列；headingIndex 为航向桶；direction 为末段行驶方向；curvatureLevelIndex 为末段曲率等级。
|
||||
/// </summary>
|
||||
public HybridAStarNodeKey(int row, int column, int headingIndex, TravelDirection direction, int curvatureLevelIndex)
|
||||
{
|
||||
Row = row;
|
||||
Column = column;
|
||||
HeadingIndex = headingIndex;
|
||||
Direction = direction;
|
||||
CurvatureLevelIndex = curvatureLevelIndex;
|
||||
}
|
||||
|
||||
/// <summary>车辆中心所在的零开始地图行索引。</summary>
|
||||
public int Row { get; }
|
||||
|
||||
/// <summary>车辆中心所在的零开始地图列索引。</summary>
|
||||
public int Column { get; }
|
||||
|
||||
/// <summary>与 <see cref="Column"/> 含义相同的列索引别名。</summary>
|
||||
public int Col { get { return Column; } }
|
||||
|
||||
/// <summary>根据配置航向分辨率量化后的航向桶索引。</summary>
|
||||
public int HeadingIndex { get; }
|
||||
|
||||
/// <summary>到达当前节点的最后一段行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>到达当前节点的最后一段曲率等级索引。</summary>
|
||||
public int CurvatureLevelIndex { get; }
|
||||
|
||||
/// <summary>判断另一个键是否表示完全相同的离散搜索状态。</summary>
|
||||
public bool Equals(HybridAStarNodeKey other)
|
||||
{
|
||||
return other != null && Row == other.Row && Column == other.Column && HeadingIndex == other.HeadingIndex &&
|
||||
Direction == other.Direction && CurvatureLevelIndex == other.CurvatureLevelIndex;
|
||||
}
|
||||
|
||||
/// <summary>判断另一个对象是否表示完全相同的离散搜索状态。</summary>
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
return Equals(obj as HybridAStarNodeKey);
|
||||
}
|
||||
|
||||
/// <summary>返回用于闭集字典的稳定哈希值。</summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
int hashCode = Row;
|
||||
hashCode = hashCode * 397 ^ Column;
|
||||
hashCode = hashCode * 397 ^ HeadingIndex;
|
||||
hashCode = hashCode * 397 ^ (int)Direction;
|
||||
return hashCode * 397 ^ CurvatureLevelIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,542 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 单次 Hybrid A* 搜索的只读结果。
|
||||
/// 即使失败也会保留已经创建的运行期节点，供上层记录诊断；仅 <see cref="PlanningStatus.Success"/> 时 <see cref="SuccessNodeIndex"/> 有值。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarSearchResult
|
||||
{
|
||||
internal HybridAStarSearchResult(
|
||||
PlanningStatus status,
|
||||
IEnumerable<HybridAStarNode> nodes,
|
||||
int expandedNodeCount,
|
||||
int generatedNodeCount,
|
||||
int reopenedNodeCount,
|
||||
int staleOpenListEntryCount,
|
||||
int peakOpenListCount,
|
||||
int? successNodeIndex,
|
||||
string terminationReason)
|
||||
{
|
||||
Status = status;
|
||||
Nodes = new ReadOnlyCollection<HybridAStarNode>(new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
|
||||
ExpandedNodeCount = expandedNodeCount;
|
||||
GeneratedNodeCount = generatedNodeCount;
|
||||
ReopenedNodeCount = reopenedNodeCount;
|
||||
StaleOpenListEntryCount = staleOpenListEntryCount;
|
||||
PeakOpenListCount = peakOpenListCount;
|
||||
SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
|
||||
TerminationReason = status == PlanningStatus.Success ? string.Empty : terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>搜索终止状态。</summary>
|
||||
public PlanningStatus Status { get; }
|
||||
|
||||
/// <summary>本次搜索已经创建的全部节点；索引与 <see cref="HybridAStarNode.NodeIndex"/> 一致。</summary>
|
||||
public IReadOnlyList<HybridAStarNode> Nodes { get; }
|
||||
|
||||
/// <summary>实际从 Open List 弹出并扩展的节点数量。</summary>
|
||||
public int ExpandedNodeCount { get; }
|
||||
|
||||
/// <summary>已进入 Open List 的节点数量，包含根节点和因改进代价追加的节点。</summary>
|
||||
public int GeneratedNodeCount { get; }
|
||||
|
||||
/// <summary>更优路径到达已关闭离散状态、并重新放回 Open List 的次数。</summary>
|
||||
public int ReopenedNodeCount { get; }
|
||||
|
||||
/// <summary>从 Open List 弹出后因已有更优普通状态而被丢弃的陈旧条目数量。</summary>
|
||||
public int StaleOpenListEntryCount { get; }
|
||||
|
||||
/// <summary>搜索期间 Open List 持有的最大条目数，包含等待惰性丢弃的旧条目。</summary>
|
||||
public int PeakOpenListCount { get; }
|
||||
|
||||
/// <summary>成功时最后一个从 Open List 弹出且满足终点条件的节点索引；失败时为 null。</summary>
|
||||
public int? SuccessNodeIndex { get; }
|
||||
|
||||
/// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在不可变 <see cref="PlanningGridMap"/> 上执行前进/倒车恒曲率原语的 Hybrid A* 搜索。
|
||||
/// 搜索只消费已准备好的地图快照；所有候选原语均先完成连续扩大车体碰撞检查，再参与 Open List 排序。
|
||||
/// </summary>
|
||||
public sealed class HybridAStarSearch
|
||||
{
|
||||
private const double CostImprovementToleranceMeters = 1e-9d;
|
||||
private readonly MotionPrimitiveGenerator _primitiveGenerator;
|
||||
private readonly SearchCostCalculator _costCalculator;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认原语、代价和碰撞检查实现的搜索器。</summary>
|
||||
public HybridAStarSearch()
|
||||
: this(new MotionPrimitiveGenerator(), new SearchCostCalculator(), new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定协作对象的搜索器，便于在不引入地图或 UI 依赖的情况下测试搜索过程。</summary>
|
||||
public HybridAStarSearch(
|
||||
MotionPrimitiveGenerator primitiveGenerator,
|
||||
SearchCostCalculator costCalculator,
|
||||
FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
|
||||
_costCalculator = costCalculator ?? throw new ArgumentNullException(nameof(costCalculator));
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行一次不可变地图上的 Hybrid A* 搜索。
|
||||
/// 参数：request 提供地图、起终点、车辆和搜索配置；cancellationToken 在每次节点扩展前检查。
|
||||
/// 返回：成功仅在满足目标约束的候选已从 Open List 弹出时报告；任一失败状态均不报告成功节点。
|
||||
/// </summary>
|
||||
public HybridAStarSearchResult Search(PlanningRequest request, CancellationToken cancellationToken)
|
||||
{
|
||||
TimeSpan timeout = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? request.Configuration.SearchTimeout
|
||||
: TimeSpan.Zero;
|
||||
PlanningOperationBudget budget = request != null && request.Configuration != null && request.Configuration.SearchTimeout >= TimeSpan.Zero
|
||||
? new PlanningOperationBudget(cancellationToken, timeout)
|
||||
: PlanningOperationBudget.Unlimited(cancellationToken);
|
||||
return Search(request, budget);
|
||||
}
|
||||
|
||||
/// <summary>使用门面传入的共享预算执行搜索；预算从整次规划开始计时。</summary>
|
||||
internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget)
|
||||
{
|
||||
var nodes = new List<HybridAStarNode>();
|
||||
int expandedNodeCount = 0;
|
||||
int generatedNodeCount = 0;
|
||||
int reopenedNodeCount = 0;
|
||||
int staleOpenListEntryCount = 0;
|
||||
int peakOpenListCount = 0;
|
||||
|
||||
try
|
||||
{
|
||||
if (budget == null) throw new ArgumentNullException(nameof(budget));
|
||||
PlanningOperationStopReason stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
PlanningStatus validationStatus = ValidateRequest(request);
|
||||
if (validationStatus != PlanningStatus.Success)
|
||||
return CreateResult(validationStatus, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
|
||||
if (!IsFootprintInsideMap(request.Start, map, vehicle))
|
||||
return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Start, map, vehicle, 0d, out _))
|
||||
return CreateResult(PlanningStatus.StartInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!IsFootprintInsideMap(request.Goal, map, vehicle))
|
||||
return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(request.Goal, map, vehicle, 0d, out _))
|
||||
return CreateResult(PlanningStatus.GoalInCollision, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (configuration.MaximumExpandedNodes == 0)
|
||||
return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
if (!map.TryWorldToGrid(request.Goal.X, request.Goal.Y, out int goalRow, out int goalColumn))
|
||||
return CreateResult(PlanningStatus.GoalOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!GridDijkstraHeuristic.TryCreate(map, goalRow, goalColumn, budget, out GridDijkstraHeuristic heuristic, out stopReason))
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return CreateResult(PlanningStatus.InvalidVehicleParameters, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
IReadOnlyList<double> curvatureLevels = _primitiveGenerator.GetCurvatureLevels(vehicle, configuration);
|
||||
if (curvatureLevels.Count == 0)
|
||||
return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
int headingBinCount = GetHeadingBinCount(configuration.HeadingResolutionRadians);
|
||||
int startCurvatureLevelIndex = GetNearestCurvatureLevelIndex(curvatureLevels, request.StartVehicleCurvature);
|
||||
if (startCurvatureLevelIndex < 0)
|
||||
return CreateResult(PlanningStatus.InvalidCurvatureConfiguration, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
if (!map.TryWorldToGrid(request.Start.X, request.Start.Y, out int startRow, out int startColumn))
|
||||
return CreateResult(PlanningStatus.StartOutsideMap, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
var openList = new BinaryMinHeap<int>();
|
||||
var bestStates = new Dictionary<HybridAStarNodeKey, NodeLabel>();
|
||||
foreach (TravelDirection startDirection in GetStartDirections(request, configuration))
|
||||
{
|
||||
int headingIndex = AngleMath.ToHeadingIndex(request.Start.Heading, configuration.HeadingResolutionRadians, headingBinCount);
|
||||
var key = new HybridAStarNodeKey(startRow, startColumn, headingIndex, startDirection, startCurvatureLevelIndex);
|
||||
if (bestStates.ContainsKey(key)) continue;
|
||||
|
||||
double hCostMeters = GetHeuristicCost(heuristic, startRow, startColumn, configuration);
|
||||
if (double.IsPositiveInfinity(hCostMeters)) continue;
|
||||
var node = new HybridAStarNode(nodes.Count, key, request.Start, -1, null,
|
||||
curvatureLevels[startCurvatureLevelIndex], 0d, hCostMeters);
|
||||
nodes.Add(node);
|
||||
bestStates.Add(key, new NodeLabel(node.NodeIndex, 0d, false));
|
||||
openList.Push(node.NodeIndex, node.FCostMeters, node.HCostMeters, node.GCostMeters);
|
||||
peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
|
||||
generatedNodeCount++;
|
||||
}
|
||||
|
||||
if (openList.Count == 0)
|
||||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null,
|
||||
"二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");
|
||||
|
||||
while (openList.Count > 0)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (expandedNodeCount >= configuration.MaximumExpandedNodes)
|
||||
return CreateResult(PlanningStatus.SearchNodeLimitExceeded, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
|
||||
int nodeIndex = openList.Pop();
|
||||
HybridAStarNode current = nodes[nodeIndex];
|
||||
NodeLabel currentLabel = null;
|
||||
if (!current.IsGoalCandidate)
|
||||
{
|
||||
if (!bestStates.TryGetValue(current.Key, out currentLabel) || currentLabel.NodeIndex != nodeIndex || currentLabel.IsClosed)
|
||||
{
|
||||
staleOpenListEntryCount++;
|
||||
continue;
|
||||
}
|
||||
|
||||
currentLabel.IsClosed = true;
|
||||
}
|
||||
|
||||
expandedNodeCount++;
|
||||
if (current.IsGoalCandidate)
|
||||
{
|
||||
if (!GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection) ||
|
||||
!_collisionChecker.IsPoseCollisionFree(current.Pose, map, vehicle, 0d, out _))
|
||||
continue;
|
||||
|
||||
return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
|
||||
}
|
||||
|
||||
if (GoalToleranceChecker.IsSatisfied(current.Pose, request.Goal, configuration, current.Direction, request.GoalDirection))
|
||||
return CreateResult(PlanningStatus.Success, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, current.NodeIndex);
|
||||
|
||||
for (int curvatureLevelIndex = 0; curvatureLevelIndex < curvatureLevels.Count; curvatureLevelIndex++)
|
||||
{
|
||||
stopReason = budget.GetStopReason();
|
||||
if (stopReason != PlanningOperationStopReason.None)
|
||||
return CreateResult(ToPlanningStatus(stopReason), nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null);
|
||||
if (!MotionPrimitiveGenerator.AreCurvatureLevelsAdjacent(current.CurvatureLevelIndex, curvatureLevelIndex)) continue;
|
||||
|
||||
foreach (TravelDirection direction in GetSuccessorDirections(configuration))
|
||||
{
|
||||
MotionPrimitive primitive = _primitiveGenerator.Generate(current.Pose, curvatureLevels[curvatureLevelIndex], direction, request);
|
||||
if (primitive == null || primitive.ActualLengthMeters <= 0d) continue;
|
||||
|
||||
if (!map.TryWorldToGrid(primitive.End.X, primitive.End.Y, out int row, out int column)) continue;
|
||||
double hCostMeters = GetHeuristicCost(heuristic, row, column, configuration);
|
||||
if (double.IsPositiveInfinity(hCostMeters)) continue;
|
||||
|
||||
double bodyClearanceMeters = GetMinimumBodyClearance(primitive);
|
||||
bool isGearSwitch = current.ParentNodeIndex >= 0 && current.Direction != direction;
|
||||
int curvatureLevelDelta = curvatureLevelIndex - current.CurvatureLevelIndex;
|
||||
double incrementalCostMeters = _costCalculator.Calculate(
|
||||
primitive.ActualLengthMeters,
|
||||
direction,
|
||||
isGearSwitch,
|
||||
curvatureLevels[curvatureLevelIndex],
|
||||
maximumCurvaturePerMeter,
|
||||
curvatureLevelDelta,
|
||||
bodyClearanceMeters,
|
||||
configuration);
|
||||
double gCostMeters = current.GCostMeters + incrementalCostMeters;
|
||||
if (!NumericGuard.IsFinite(gCostMeters) || gCostMeters < 0d) continue;
|
||||
|
||||
int headingIndex = AngleMath.ToHeadingIndex(primitive.End.Heading, configuration.HeadingResolutionRadians, headingBinCount);
|
||||
var key = new HybridAStarNodeKey(row, column, headingIndex, direction, curvatureLevelIndex);
|
||||
bestStates.TryGetValue(key, out NodeLabel existingLabel);
|
||||
var successor = new HybridAStarNode(nodes.Count, key, primitive.End, current.NodeIndex, primitive,
|
||||
curvatureLevels[curvatureLevelIndex], gCostMeters, hCostMeters, primitive.IsGoalTruncation);
|
||||
if (existingLabel != null && !ShouldEnqueueSuccessor(successor, existingLabel.GCostMeters)) continue;
|
||||
|
||||
if (!successor.IsGoalCandidate && existingLabel != null && existingLabel.IsClosed) reopenedNodeCount++;
|
||||
nodes.Add(successor);
|
||||
if (!successor.IsGoalCandidate)
|
||||
bestStates[key] = new NodeLabel(successor.NodeIndex, gCostMeters, false);
|
||||
openList.Push(successor.NodeIndex, successor.FCostMeters, successor.HCostMeters, successor.GCostMeters);
|
||||
peakOpenListCount = Math.Max(peakOpenListCount, openList.Count);
|
||||
generatedNodeCount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null,
|
||||
"Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
|
||||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||||
return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, null, reason);
|
||||
}
|
||||
}
|
||||
|
||||
private static HybridAStarSearchResult CreateResult(
|
||||
PlanningStatus status,
|
||||
IEnumerable<HybridAStarNode> nodes,
|
||||
int expandedNodeCount,
|
||||
int generatedNodeCount,
|
||||
int reopenedNodeCount,
|
||||
int staleOpenListEntryCount,
|
||||
int peakOpenListCount,
|
||||
int? successNodeIndex,
|
||||
string terminationReason = null)
|
||||
{
|
||||
string reason = status == PlanningStatus.Success
|
||||
? string.Empty
|
||||
: terminationReason ?? GetDefaultTerminationReason(status);
|
||||
return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||||
staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
|
||||
}
|
||||
|
||||
private static string GetDefaultTerminationReason(PlanningStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case PlanningStatus.Cancelled:
|
||||
return "Hybrid A* 搜索已取消。";
|
||||
case PlanningStatus.InvalidRequest:
|
||||
return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
|
||||
case PlanningStatus.InvalidMap:
|
||||
return "Hybrid A* 搜索地图结构无效。";
|
||||
case PlanningStatus.MapNotReady:
|
||||
return "Hybrid A* 搜索地图尚未准备好。";
|
||||
case PlanningStatus.InvalidVehicleParameters:
|
||||
return "Hybrid A* 搜索车辆参数无效。";
|
||||
case PlanningStatus.InvalidCurvatureConfiguration:
|
||||
return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
|
||||
case PlanningStatus.StartOutsideMap:
|
||||
return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
|
||||
case PlanningStatus.StartInCollision:
|
||||
return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
|
||||
case PlanningStatus.GoalOutsideMap:
|
||||
return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
|
||||
case PlanningStatus.GoalInCollision:
|
||||
return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
|
||||
case PlanningStatus.SearchTimeout:
|
||||
return "Hybrid A* 搜索使用的总规划预算已耗尽。";
|
||||
case PlanningStatus.SearchNodeLimitExceeded:
|
||||
return "Hybrid A* 搜索达到扩展节点上限。";
|
||||
case PlanningStatus.NoFeasiblePath:
|
||||
return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
|
||||
case PlanningStatus.BacktrackingFailed:
|
||||
return "Hybrid A* 成功节点无法回溯为完整父链。";
|
||||
case PlanningStatus.FinalValidationFailed:
|
||||
return "Hybrid A* 路径未通过最终复核。";
|
||||
case PlanningStatus.InternalError:
|
||||
return "Hybrid A* 搜索发生未预期内部错误。";
|
||||
default:
|
||||
return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
|
||||
}
|
||||
}
|
||||
|
||||
private static PlanningStatus ValidateRequest(PlanningRequest request)
|
||||
{
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
!IsFinitePose(request.Start) || !IsFinitePose(request.Goal) || !NumericGuard.IsFinite(request.StartVehicleCurvature) ||
|
||||
!IsGoalDirection(request.GoalDirection) || (request.StartDirection.HasValue && !IsTravelDirection(request.StartDirection.Value)))
|
||||
return PlanningStatus.InvalidRequest;
|
||||
|
||||
PlanningGridMap map = request.Map;
|
||||
if (map.Rows <= 0 || map.Cols <= 0 || !NumericGuard.IsPositiveFinite(map.ResolutionMeters) || map.Bounds == null)
|
||||
return PlanningStatus.InvalidMap;
|
||||
if (!map.PlanningReady) return PlanningStatus.MapNotReady;
|
||||
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) || !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return PlanningStatus.InvalidVehicleParameters;
|
||||
|
||||
HybridAStarConfiguration configuration = request.Configuration;
|
||||
if (!IsValidConfiguration(configuration) || Math.Abs(request.StartVehicleCurvature) > maximumCurvaturePerMeter ||
|
||||
(request.StartDirection == TravelDirection.Reverse && !configuration.AllowReverse))
|
||||
return PlanningStatus.InvalidCurvatureConfiguration;
|
||||
|
||||
return PlanningStatus.Success;
|
||||
}
|
||||
|
||||
private static bool IsValidConfiguration(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsPositiveFinite(configuration.HeadingResolutionRadians) &&
|
||||
configuration.HeadingResolutionRadians <= 2d * Math.PI &&
|
||||
configuration.CurvatureLevelCount >= 3 && configuration.CurvatureLevelCount % 2 == 1 &&
|
||||
NumericGuard.IsFinite(configuration.GoalPositionToleranceMeters) && configuration.GoalPositionToleranceMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.GoalHeadingToleranceRadians) && configuration.GoalHeadingToleranceRadians >= 0d &&
|
||||
configuration.MaximumExpandedNodes >= 0 && configuration.SearchTimeout >= TimeSpan.Zero &&
|
||||
NumericGuard.IsFinite(configuration.HeuristicWeight) && configuration.HeuristicWeight >= 0d &&
|
||||
NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier) &&
|
||||
NumericGuard.IsFinite(configuration.GearSwitchPenaltyMeters) && configuration.GearSwitchPenaltyMeters >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureMagnitudeWeight) && configuration.CurvatureMagnitudeWeight >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) && configuration.CurvatureChangePenaltyMetersPerLevel >= 0d &&
|
||||
NumericGuard.IsFinite(configuration.ClearanceCostWeight) && configuration.ClearanceCostWeight >= 0d &&
|
||||
NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters);
|
||||
}
|
||||
|
||||
private static bool IsFootprintInsideMap(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle)
|
||||
{
|
||||
if (!map.TryWorldToGrid(pose.X, pose.Y, out _, out _)) return false;
|
||||
|
||||
double halfLengthMeters = vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double halfWidthMeters = vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters;
|
||||
double longitudinalX = Math.Cos(pose.Heading);
|
||||
double longitudinalY = Math.Sin(pose.Heading);
|
||||
double lateralX = -longitudinalY;
|
||||
double lateralY = longitudinalX;
|
||||
for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
|
||||
for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
|
||||
{
|
||||
double cornerX = pose.X + longitudinalSign * halfLengthMeters * longitudinalX + lateralSign * halfWidthMeters * lateralX;
|
||||
double cornerY = pose.Y + longitudinalSign * halfLengthMeters * longitudinalY + lateralSign * halfWidthMeters * lateralY;
|
||||
if (!map.TryWorldToGrid(cornerX, cornerY, out _, out _)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IEnumerable<TravelDirection> GetStartDirections(PlanningRequest request, HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (request.StartDirection.HasValue)
|
||||
{
|
||||
yield return request.StartDirection.Value;
|
||||
yield break;
|
||||
}
|
||||
|
||||
yield return TravelDirection.Forward;
|
||||
if (configuration.AllowReverse) yield return TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static IEnumerable<TravelDirection> GetSuccessorDirections(HybridAStarConfiguration configuration)
|
||||
{
|
||||
yield return TravelDirection.Forward;
|
||||
if (configuration.AllowReverse) yield return TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static int GetHeadingBinCount(double headingResolutionRadians)
|
||||
{
|
||||
double rawBinCount = Math.Ceiling(2d * Math.PI / headingResolutionRadians);
|
||||
if (!NumericGuard.IsFinite(rawBinCount) || rawBinCount < 1d || rawBinCount > int.MaxValue)
|
||||
throw new ArgumentOutOfRangeException(nameof(headingResolutionRadians));
|
||||
return (int)rawBinCount;
|
||||
}
|
||||
|
||||
private static int GetNearestCurvatureLevelIndex(IReadOnlyList<double> curvatureLevels, double curvaturePerMeter)
|
||||
{
|
||||
int bestIndex = -1;
|
||||
double smallestDifference = double.PositiveInfinity;
|
||||
for (int index = 0; index < curvatureLevels.Count; index++)
|
||||
{
|
||||
double difference = Math.Abs(curvatureLevels[index] - curvaturePerMeter);
|
||||
if (difference >= smallestDifference) continue;
|
||||
smallestDifference = difference;
|
||||
bestIndex = index;
|
||||
}
|
||||
return bestIndex;
|
||||
}
|
||||
|
||||
private static double GetMinimumBodyClearance(MotionPrimitive primitive)
|
||||
{
|
||||
double minimum = double.PositiveInfinity;
|
||||
foreach (double clearanceMeters in primitive.BodyClearancesMeters)
|
||||
{
|
||||
if (double.IsNaN(clearanceMeters) || clearanceMeters < 0d) return 0d;
|
||||
minimum = Math.Min(minimum, clearanceMeters);
|
||||
}
|
||||
return minimum;
|
||||
}
|
||||
|
||||
private static double GetHeuristicCost(GridDijkstraHeuristic heuristic, int row, int column, HybridAStarConfiguration configuration)
|
||||
{
|
||||
double gridCostMeters = heuristic.GetCost(row, column);
|
||||
if (double.IsPositiveInfinity(gridCostMeters)) return gridCostMeters;
|
||||
double weightedCostMeters = gridCostMeters * configuration.HeuristicWeight;
|
||||
return NumericGuard.IsFinite(weightedCostMeters) && weightedCostMeters >= 0d
|
||||
? weightedCostMeters
|
||||
: double.PositiveInfinity;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断后继是否应进入 Open List。
|
||||
/// 参数：successor 是已完成连续碰撞检查的后继；bestKnownGCostMeters 是相同离散键普通状态的当前最优 G 值。
|
||||
/// 返回：普通状态仅在严格改善最优 G 值时入队；终点候选始终入队，以保留其未量化的连续终点位姿。
|
||||
/// </summary>
|
||||
private static bool ShouldEnqueueSuccessor(HybridAStarNode successor, double bestKnownGCostMeters)
|
||||
{
|
||||
if (successor == null || !NumericGuard.IsFinite(bestKnownGCostMeters)) return false;
|
||||
return successor.IsGoalCandidate || successor.GCostMeters < bestKnownGCostMeters - CostImprovementToleranceMeters;
|
||||
}
|
||||
|
||||
private static PlanningStatus ToPlanningStatus(PlanningOperationStopReason stopReason)
|
||||
{
|
||||
if (stopReason == PlanningOperationStopReason.Cancelled) return PlanningStatus.Cancelled;
|
||||
if (stopReason == PlanningOperationStopReason.TimedOut) return PlanningStatus.SearchTimeout;
|
||||
throw new ArgumentOutOfRangeException(nameof(stopReason));
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
|
||||
private sealed class NodeLabel
|
||||
{
|
||||
public NodeLabel(int nodeIndex, double gCostMeters, bool isClosed)
|
||||
{
|
||||
NodeIndex = nodeIndex;
|
||||
GCostMeters = gCostMeters;
|
||||
IsClosed = isClosed;
|
||||
}
|
||||
|
||||
public int NodeIndex { get; }
|
||||
public double GCostMeters { get; }
|
||||
public bool IsClosed { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 一条经连续碰撞检查的恒曲率运动原语。
|
||||
/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
|
||||
/// </summary>
|
||||
public sealed class MotionPrimitive
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建不可变运动原语。
|
||||
/// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
|
||||
/// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
|
||||
/// </summary>
|
||||
public MotionPrimitive(
|
||||
Pose2D start,
|
||||
TravelDirection direction,
|
||||
double curvaturePerMeter,
|
||||
double actualLengthMeters,
|
||||
IEnumerable<Pose2D> points,
|
||||
IEnumerable<double> bodyClearancesMeters,
|
||||
bool isGoalTruncation)
|
||||
{
|
||||
if (start == null) throw new ArgumentNullException(nameof(start));
|
||||
if (points == null) throw new ArgumentNullException(nameof(points));
|
||||
if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));
|
||||
|
||||
var copiedPoints = new List<Pose2D>(points);
|
||||
var copiedClearances = new List<double>(bodyClearancesMeters);
|
||||
if (copiedPoints.Count != copiedClearances.Count)
|
||||
throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));
|
||||
|
||||
Start = start;
|
||||
Direction = direction;
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
ActualLengthMeters = actualLengthMeters;
|
||||
Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
|
||||
BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
|
||||
End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
|
||||
IsGoalTruncation = isGoalTruncation;
|
||||
}
|
||||
|
||||
/// <summary>原语起始车辆几何中心位姿。</summary>
|
||||
public Pose2D Start { get; }
|
||||
|
||||
/// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
|
||||
public Pose2D End { get; }
|
||||
|
||||
/// <summary>原语对应的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
|
||||
public double ActualLengthMeters { get; }
|
||||
|
||||
/// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
|
||||
public IReadOnlyList<Pose2D> Points { get; }
|
||||
|
||||
/// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
|
||||
public IReadOnlyList<double> BodyClearancesMeters { get; }
|
||||
|
||||
/// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
|
||||
public bool IsGoalTruncation { get; }
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
|
||||
/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
|
||||
/// </summary>
|
||||
public sealed class MotionPrimitiveGenerator
|
||||
{
|
||||
private const double StraightCurvatureThreshold = 1e-12d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
|
||||
public MotionPrimitiveGenerator()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
|
||||
public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用完整规划请求生成一条原语。
|
||||
/// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
|
||||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
|
||||
/// </summary>
|
||||
public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
|
||||
{
|
||||
if (request == null) return null;
|
||||
return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用显式地图、车辆、配置和目标生成一条原语。
|
||||
/// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
|
||||
/// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
|
||||
/// </summary>
|
||||
public MotionPrimitive Generate(
|
||||
Pose2D start,
|
||||
double curvaturePerMeter,
|
||||
TravelDirection direction,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
HybridAStarConfiguration configuration,
|
||||
Pose2D goal,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;
|
||||
|
||||
if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);
|
||||
|
||||
double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
|
||||
Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
|
||||
if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;
|
||||
|
||||
var points = new List<Pose2D>();
|
||||
var bodyClearancesMeters = new List<double>();
|
||||
Pose2D previous = start;
|
||||
double actualLengthMeters = 0d;
|
||||
while (actualLengthMeters < configuration.PrimitiveLengthMeters)
|
||||
{
|
||||
double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
|
||||
double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
|
||||
if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;
|
||||
|
||||
Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
|
||||
if (!IsFinitePose(next)) return null;
|
||||
if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;
|
||||
|
||||
actualLengthMeters += stepMeters;
|
||||
points.Add(next);
|
||||
bodyClearancesMeters.Add(bodyClearanceMeters);
|
||||
if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);
|
||||
|
||||
previous = next;
|
||||
}
|
||||
|
||||
return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
|
||||
/// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
|
||||
/// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return Array.Empty<double>();
|
||||
|
||||
var levels = new double[configuration.CurvatureLevelCount];
|
||||
double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
|
||||
for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
|
||||
levels[levels.Length / 2] = 0d;
|
||||
return levels;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两个曲率等级是否允许在相邻原语间直接切换。
|
||||
/// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
|
||||
/// 返回：两个索引均非负且最多相差一个等级时为 true。
|
||||
/// </summary>
|
||||
public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
|
||||
{
|
||||
return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
|
||||
}
|
||||
|
||||
private static bool IsValidInput(
|
||||
Pose2D start,
|
||||
double curvaturePerMeter,
|
||||
TravelDirection direction,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
HybridAStarConfiguration configuration,
|
||||
Pose2D goal,
|
||||
GoalDirectionConstraint goalDirection)
|
||||
{
|
||||
if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
|
||||
!NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
return false;
|
||||
|
||||
return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
|
||||
}
|
||||
|
||||
private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
|
||||
{
|
||||
double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
|
||||
double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
|
||||
if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
|
||||
{
|
||||
return new Pose2D(
|
||||
previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
|
||||
previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
|
||||
nextHeadingRadians);
|
||||
}
|
||||
|
||||
return new Pose2D(
|
||||
previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
|
||||
previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
|
||||
nextHeadingRadians);
|
||||
}
|
||||
|
||||
private static bool IsFinitePose(Pose2D pose)
|
||||
{
|
||||
return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
|
||||
}
|
||||
|
||||
private static bool IsTravelDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static bool IsGoalDirection(GoalDirectionConstraint direction)
|
||||
{
|
||||
return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 将运动原语的长度、方向、曲率和净空转换为统一的等效米搜索代价。
|
||||
/// 此类型不修改搜索状态；换向标记必须由调用方依据相邻原语的方向关系提供。
|
||||
/// </summary>
|
||||
public sealed class SearchCostCalculator
|
||||
{
|
||||
/// <summary>创建使用调用时配置参数的等效米代价计算器。</summary>
|
||||
public SearchCostCalculator()
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算一条运动原语的等效米增量代价。
|
||||
/// 参数：lengthMeters 为非负弧长 m；direction 为原语方向；isGearSwitch 表示该原语前是否换向；
|
||||
/// curvaturePerMeter 与 maximumCurvaturePerMeter 的单位为 1/m；curvatureLevelDelta 为相邻曲率等级差；
|
||||
/// bodyClearanceMeters 为非负车体保守净空 m，可为正无穷；configuration 提供非负权重和惩罚。
|
||||
/// 返回：有限且非负的等效米代价。
|
||||
/// 失败：任一数值、方向或权重无效时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||||
/// </summary>
|
||||
public double Calculate(
|
||||
double lengthMeters,
|
||||
TravelDirection direction,
|
||||
bool isGearSwitch,
|
||||
double curvaturePerMeter,
|
||||
double maximumCurvaturePerMeter,
|
||||
int curvatureLevelDelta,
|
||||
double bodyClearanceMeters,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
ValidateInput(lengthMeters, direction, curvaturePerMeter, maximumCurvaturePerMeter, bodyClearanceMeters, configuration);
|
||||
|
||||
double directionMultiplier = direction == TravelDirection.Reverse ? configuration.ReverseCostMultiplier : 1d;
|
||||
double normalizedCurvature = Math.Abs(curvaturePerMeter / maximumCurvaturePerMeter);
|
||||
double clearanceDeficit = GetClearanceDeficit(bodyClearanceMeters, configuration.ClearanceCostDistanceMeters);
|
||||
double levelDelta = Math.Abs((double)curvatureLevelDelta);
|
||||
double motionCost = lengthMeters * directionMultiplier *
|
||||
(1d + configuration.CurvatureMagnitudeWeight * normalizedCurvature +
|
||||
configuration.ClearanceCostWeight * clearanceDeficit);
|
||||
double gearSwitchCost = isGearSwitch ? configuration.GearSwitchPenaltyMeters : 0d;
|
||||
double curvatureChangeCost = configuration.CurvatureChangePenaltyMetersPerLevel * levelDelta;
|
||||
double totalCost = motionCost + gearSwitchCost + curvatureChangeCost;
|
||||
if (!NumericGuard.IsFinite(totalCost) || totalCost < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lengthMeters), "The calculated cost must remain finite and non-negative.");
|
||||
|
||||
return totalCost;
|
||||
}
|
||||
|
||||
private static void ValidateInput(
|
||||
double lengthMeters,
|
||||
TravelDirection direction,
|
||||
double curvaturePerMeter,
|
||||
double maximumCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
HybridAStarConfiguration configuration)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(lengthMeters) || lengthMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(lengthMeters));
|
||||
if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
|
||||
throw new ArgumentOutOfRangeException(nameof(direction));
|
||||
if (!NumericGuard.IsFinite(curvaturePerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||||
if (!NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(maximumCurvaturePerMeter));
|
||||
if (Math.Abs(curvaturePerMeter) > maximumCurvaturePerMeter)
|
||||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||||
if (double.IsNaN(bodyClearanceMeters) || bodyClearanceMeters < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(bodyClearanceMeters));
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
ValidateNonNegativeFinite(configuration.HeuristicWeight, nameof(configuration.HeuristicWeight));
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier))
|
||||
throw new ArgumentOutOfRangeException(nameof(configuration.ReverseCostMultiplier));
|
||||
ValidateNonNegativeFinite(configuration.GearSwitchPenaltyMeters, nameof(configuration.GearSwitchPenaltyMeters));
|
||||
ValidateNonNegativeFinite(configuration.CurvatureMagnitudeWeight, nameof(configuration.CurvatureMagnitudeWeight));
|
||||
ValidateNonNegativeFinite(configuration.CurvatureChangePenaltyMetersPerLevel, nameof(configuration.CurvatureChangePenaltyMetersPerLevel));
|
||||
ValidateNonNegativeFinite(configuration.ClearanceCostWeight, nameof(configuration.ClearanceCostWeight));
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(configuration.ClearanceCostDistanceMeters));
|
||||
}
|
||||
|
||||
private static void ValidateNonNegativeFinite(double value, string parameterName)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(parameterName);
|
||||
}
|
||||
|
||||
private static double GetClearanceDeficit(double bodyClearanceMeters, double clearanceCostDistanceMeters)
|
||||
{
|
||||
if (double.IsPositiveInfinity(bodyClearanceMeters)) return 0d;
|
||||
double deficit = 1d - bodyClearanceMeters / clearanceCostDistanceMeters;
|
||||
return deficit > 0d ? deficit : 0d;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user