NomrPathfinder.cs
10.4 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
using System.Numerics;
using HaHRCS.Core.Models;
using Rcs.Domain.Entities;
using Rcs.Domain.ValueObjects;
using Rcs.Shared.Utils;
namespace Rcs.Infrastructure.Shared.Algorithms
{
public class NomrPathfinder
{
private readonly Dictionary<Guid, MapNode> _nodes;
private readonly Dictionary<Guid, MapEdge> _edges;
private readonly Dictionary<Guid, List<MapEdge>> _adj;
// 车辆最小转弯半径（基于地图边上的半径估计）
private readonly double _minTurnRadius;
// 航向量化粒度（弧度）- 降低状态数量，缓解循环
private const double HeadingBinRad = Math.PI / 18; // 10°
/// <summary>
/// 倒车惩罚因子，默认 2.0。
/// </summary>
public double ReverseCostFactor { get; set; } = 2.0;
public NomrPathfinder(IEnumerable<MapNode> nodes, IEnumerable<MapEdge> edges)
{
_nodes = nodes.ToDictionary(n => n.NodeId);
_edges = edges.ToDictionary(e => e.EdgeId);
_adj = edges
.SelectMany(e => new[] { (e.FromNode, e), (e.ToNode, e) })
.GroupBy(x => x.Item1)
.ToDictionary(g => g.Key, g => g.Select(x => x.e).ToList());
// 估计最小转弯半径：采用曲线边最小半径，若缺失则回退到 1.0
_minTurnRadius = edges
.Where(e => e.IsCurve && e.Radius.HasValue && e.Radius.Value > 0)
.Select(e => e.Radius!.Value)
.DefaultIfEmpty(1.0)
.Min();
}
/// <summary>
/// 从起点到终点搜索路径，起点包含初始车体方向。
/// </summary>
public AgvPathResult FindPath(Guid startNodeId, double startHeadingRad, Guid goalNodeId, bool isReverseParking)
{
var open = new PriorityQueue<State, double>();
var cameFrom = new Dictionary<State, (State Prev, Guid EdgeId)>();
var gScore = new Dictionary<State, double>();
var closed = new HashSet<State>();
var startState = new State(startNodeId, null, QuantizeHeading(startHeadingRad));
gScore[startState] = 0;
open.Enqueue(startState, Heuristic(startState, goalNodeId, isReverseParking));
while (open.Count > 0)
{
var current = open.Dequeue();
if (closed.Contains(current)) continue;
closed.Add(current);
if (current.NodeId == goalNodeId)
{
if (!current.PrevEdgeId.HasValue)
{
// 终点约束：根据是否允许倒车，限定最后一条边的行驶模式
var wasReverse = _edges[current.PrevEdgeId.Value].Regress;
if ((isReverseParking && wasReverse) || (!isReverseParking && !wasReverse))
{
return BuildResult(current, cameFrom, gScore[current]);
}
}
}
if (!_adj.TryGetValue(current.NodeId, out var incident)) continue;
var edges = incident.Where(e => e.FromNode == current.NodeId).ToList();
foreach (var edge in edges)
{
Guid nextNodeId = edge.ToNode;
// --- 切向约束 ---
// 起点第一条边：必须与起始车体方向相切（考虑倒车/前进）
var departVec = CraHandle.GetDepartureVector(edge, current.NodeId);
var startDir = new Vector2((float)Math.Cos(current.HeadingRad), (float)Math.Sin(current.HeadingRad));
Vector2? arrivalVec = null;
if (!current.PrevEdgeId.HasValue)
{
arrivalVec = new Vector2();
var prevEdge = _edges[current.PrevEdgeId.Value];
arrivalVec = CraHandle.GetArrivalVector(prevEdge, current.NodeId);
}
if (!CraHandle.IsTangentCompatible(startDir, departVec, arrivalVec, edge.Regress)) continue;
// 计算车体在该边上的实际航向角，考虑当前车体朝向和倒车模式
var nextHeading = CraHandle.GetVehicleHeadingOnEdge(edge, current.NodeId, current.HeadingRad);
nextHeading = QuantizeHeading(nextHeading);
var nextState = new State(nextNodeId, edge.EdgeId, nextHeading);
double stepCost = edge.Cost ?? edge.Length;
if (edge.Regress) stepCost *= ReverseCostFactor;
// 在节点处的转向代价：角度 * 最小转弯半径（近似为所需弧长）
double turnCost = 0;
if (arrivalVec.HasValue)
{
var angleTurn = CraHandle.AngleBetween(arrivalVec.Value, departVec);
turnCost += _minTurnRadius * angleTurn;
}
// 模式切换代价：前进 <-> 倒车 切换需要较大的机动，增加惩罚
if (!current.PrevEdgeId.HasValue)
{
var prevEdge = _edges[current.PrevEdgeId.Value];
if (prevEdge.Regress != edge.Regress)
{
turnCost += _minTurnRadius * (Math.PI / 2);
}
}
double tentativeG = gScore[current] + stepCost + turnCost;
if (!gScore.TryGetValue(nextState, out var existing) || tentativeG < existing)
{
gScore[nextState] = tentativeG;
cameFrom[nextState] = (current, edge.EdgeId);
var f = tentativeG + Heuristic(nextState, goalNodeId, isReverseParking);
open.Enqueue(nextState, f);
}
}
}
return new AgvPathResult { Success = false, FailReason = "未找到符合条件的路径" };
}
#region 工具方法 r=1⟹(0.293,0.707)
private AgvPathResult BuildResult(State endState, Dictionary<State, (State Prev, Guid EdgeId)> cameFrom, double totalCost)
{
var edges = new List<MapEdge>();
var nodes = new List<MapNode>();
var rev = new List<(Guid EdgeId, Guid NodeId)>();
var cur = endState;
while (cameFrom.TryGetValue(cur, out var rec))
{
rev.Add((rec.EdgeId, cur.NodeId));
cur = rec.Prev;
}
rev.Reverse();
// 起点
nodes.Add(_nodes[cur.NodeId]);
foreach (var step in rev)
{
if (_edges.TryGetValue(step.EdgeId, out var e)) edges.Add(e);
if (_nodes.TryGetValue(step.NodeId, out var n)) nodes.Add(n);
}
return new AgvPathResult
{
Success = true,
Nodes = nodes,
Edges = edges,
TotalCost = totalCost
};
}
private double Heuristic(State state, Guid goalId, bool isReverseParking)
{
if (!_nodes.TryGetValue(state.NodeId, out var a) || !_nodes.TryGetValue(goalId, out var b))
return 0;
// 位置差
double dx = b.X - a.X;
double dy = b.Y - a.Y;
double euclidean = Math.Sqrt(dx * dx + dy * dy);
// 终点朝向候选
var goalHeadings = GetGoalHeadingCandidates(goalId, isReverseParking);
if (goalHeadings.Count == 0)
{
// 若无候选，回退到节点自带朝向或仅使用欧式
if (b.Theta.HasValue)
{
goalHeadings.Add(b.Theta.Value);
}
else
{
return euclidean;
}
}
// Reeds–Shepp 启发式的保守下界：max(直线位移, R*|Δθ|) 的最小值
double best = double.PositiveInfinity;
foreach (var gh in goalHeadings)
{
var dtheta = AngleConstants.NormalizeAngleSymmetric(gh - state.HeadingRad);
var lbTurn = _minTurnRadius * Math.Abs(dtheta);
var rsLowerBound = Math.Max(euclidean, lbTurn);
if (rsLowerBound < best) best = rsLowerBound;
}
return best;
}
private static double QuantizeHeading(double rad)
{
// 将角度标准化到 [-pi, pi] 再按 10° 量化
var norm = AngleConstants.NormalizeAngleSymmetric(rad);
var bins = Math.Round(norm / HeadingBinRad);
var q = bins * HeadingBinRad;
// 重新标准化，防止出现边界值
return AngleConstants.NormalizeAngleSymmetric(q);
}
private List<double> GetGoalHeadingCandidates(Guid goalId, bool isReverseParking)
{
var headings = new List<double>();
if (_adj.TryGetValue(goalId, out var incident))
{
// 只考虑进入 goal 的边
foreach (var e in incident.Where(e => e.ToNode == goalId))
{
var vec = CraHandle.GetArrivalVector(e, goalId);
var motionHeading = Math.Atan2(vec.Y, vec.X);
// 车辆车体朝向：若该边是倒车，则车体朝向与运动方向相反
var bodyHeading = e.Regress ? AngleConstants.NormalizeAngleSymmetric(motionHeading + Math.PI) : motionHeading;
headings.Add(bodyHeading);
// 如果允许倒车泊入，也考虑相反方向作为候选，以增强可行性（启发式取 min 仍保持保守）
if (isReverseParking)
{
headings.Add(AngleConstants.NormalizeAngleSymmetric(bodyHeading + Math.PI));
}
}
}
return headings.Distinct()
.Select(AngleConstants.NormalizeAngleSymmetric)
.ToList();
}
#endregion
#region 内部结构
private record State(Guid NodeId, Guid? PrevEdgeId, double HeadingRad);
#endregion
}
}