615 lines
17 KiB
Markdown
615 lines
17 KiB
Markdown
无人物流车转弯路径曲线化功能 - 实施方案
|
||
|
||
1. 目标概述
|
||
实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，确保仿真系统能准确检测车辆转弯时的扫掠路径（Swept Path）碰撞。
|
||
|
||
核心原理
|
||
控制点与物理点分离：用户操作的
|
||
PathPoint
|
||
作为控制点，实际物理路径由 PathEdge 表示
|
||
圆弧过渡法：在相邻路径段转折处插入切圆弧，使用进入切点 Ts 和退出切点 Te 连接
|
||
安全截断：当计算的切线长度超过线段长度45%时，自动缩减半径，防止圆弧过大
|
||
2. 数据结构改动
|
||
2.1 新增数据模型
|
||
[NEW] PathSegmentType 枚举
|
||
/// <summary>
|
||
/// 路径段类型
|
||
/// </summary>
|
||
public enum PathSegmentType
|
||
{
|
||
/// <summary>
|
||
/// 直线段
|
||
/// </summary>
|
||
Straight,
|
||
|
||
/// <summary>
|
||
/// 圆弧段
|
||
/// </summary>
|
||
Arc
|
||
}
|
||
[NEW] ArcTrajectory 类
|
||
/// <summary>
|
||
/// 圆弧轨迹数据
|
||
/// </summary>
|
||
[Serializable]
|
||
public class ArcTrajectory
|
||
{
|
||
/// <summary>
|
||
/// 进入切点
|
||
/// </summary>
|
||
public Point3D Ts { get; set; }
|
||
|
||
/// <summary>
|
||
/// 退出切点
|
||
/// </summary>
|
||
public Point3D Te { get; set; }
|
||
|
||
/// <summary>
|
||
/// 圆心位置
|
||
/// </summary>
|
||
public Point3D ArcCenter { get; set; }
|
||
|
||
/// <summary>
|
||
/// 请求半径（配置的转向半径）
|
||
/// </summary>
|
||
public double RequestedRadius { get; set; }
|
||
|
||
/// <summary>
|
||
/// 实际半径（安全截断后）
|
||
/// </summary>
|
||
public double ActualRadius { get; set; }
|
||
|
||
/// <summary>
|
||
/// 偏转角（弧度）
|
||
/// </summary>
|
||
public double DeflectionAngle { get; set; }
|
||
|
||
/// <summary>
|
||
/// 圆弧长度（米）
|
||
/// </summary>
|
||
public double ArcLength { get; set; }
|
||
}
|
||
[NEW] PathEdge 类
|
||
/// <summary>
|
||
/// 路径边 - 连接两个连续控制点的物理路径段
|
||
/// </summary>
|
||
[Serializable]
|
||
public class PathEdge
|
||
{
|
||
/// <summary>
|
||
/// 边唯一标识符
|
||
/// </summary>
|
||
public string Id { get; set; }
|
||
|
||
/// <summary>
|
||
/// 起始控制点ID
|
||
/// </summary>
|
||
public string StartPointId { get; set; }
|
||
|
||
/// <summary>
|
||
/// 结束控制点ID
|
||
/// </summary>
|
||
public string EndPointId { get; set; }
|
||
|
||
/// <summary>
|
||
/// 路径段类型
|
||
/// </summary>
|
||
public PathSegmentType SegmentType { get; set; }
|
||
|
||
/// <summary>
|
||
/// 圆弧轨迹数据（仅当 SegmentType == Arc 时有效）
|
||
/// </summary>
|
||
public ArcTrajectory Trajectory { get; set; }
|
||
|
||
/// <summary>
|
||
/// 边的物理长度（米）
|
||
/// 直线段：两点间距离；圆弧段：直线段长度 + 圆弧长度
|
||
/// </summary>
|
||
public double PhysicalLength { get; set; }
|
||
|
||
/// <summary>
|
||
/// 采样点序列（用于碰撞检测和动画）
|
||
/// </summary>
|
||
[XmlIgnore]
|
||
public List<Point3D> SampledPoints { get; set; }
|
||
}
|
||
2.2 修改现有数据模型
|
||
[MODIFY]
|
||
PathPoint.cs
|
||
新增属性：
|
||
|
||
/// <summary>
|
||
/// 自定义转向半径（米），用于局部急转弯场景
|
||
/// 为 null 时使用全局默认值
|
||
/// </summary>
|
||
public double? CustomTurnRadius { get; set; }
|
||
[MODIFY]
|
||
PathRoute.cs
|
||
新增属性：
|
||
|
||
/// <summary>
|
||
/// 路径边集合 - 存储物理路径段
|
||
/// </summary>
|
||
public List<PathEdge> Edges { get; set; } = new List<PathEdge>();
|
||
/// <summary>
|
||
/// 默认转向半径（米）
|
||
/// </summary>
|
||
public double TurnRadius { get; set; } = 1.5;
|
||
/// <summary>
|
||
/// 是否已曲线化
|
||
/// </summary>
|
||
public bool IsCurved { get; set; } = false;
|
||
修改方法：
|
||
|
||
UpdateTotalLength()
|
||
: 综合计算所有 Edges 的物理长度
|
||
Clone()
|
||
: 需要深拷贝 Edges 集合
|
||
2.3 数据库表结构扩展
|
||
[MODIFY]
|
||
PathDatabase.cs
|
||
新增表 PathEdges：
|
||
|
||
CREATE TABLE IF NOT EXISTS PathEdges (
|
||
Id TEXT PRIMARY KEY,
|
||
RouteId TEXT NOT NULL,
|
||
StartPointId TEXT NOT NULL,
|
||
EndPointId TEXT NOT NULL,
|
||
SegmentType INTEGER NOT NULL, -- 0=Straight, 1=Arc
|
||
PhysicalLength REAL NOT NULL,
|
||
|
||
-- 圆弧轨迹数据 (仅 SegmentType=1 时有效)
|
||
Ts_X REAL,
|
||
Ts_Y REAL,
|
||
Ts_Z REAL,
|
||
Te_X REAL,
|
||
Te_Y REAL,
|
||
Te_Z REAL,
|
||
ArcCenter_X REAL,
|
||
ArcCenter_Y REAL,
|
||
ArcCenter_Z REAL,
|
||
RequestedRadius REAL,
|
||
ActualRadius REAL,
|
||
DeflectionAngle REAL,
|
||
ArcLength REAL,
|
||
|
||
FOREIGN KEY (RouteId) REFERENCES PathRoutes(Id) ON DELETE CASCADE
|
||
);
|
||
修改表
|
||
PathRoutes
|
||
：
|
||
|
||
ALTER TABLE PathRoutes ADD COLUMN TurnRadius REAL DEFAULT 1.5;
|
||
ALTER TABLE PathRoutes ADD COLUMN IsCurved INTEGER DEFAULT 0;
|
||
修改表
|
||
PathPoints
|
||
：
|
||
|
||
ALTER TABLE PathPoints ADD COLUMN CustomTurnRadius REAL;
|
||
3. 核心算法实现
|
||
3.1 PathCurveEngine 类
|
||
位置: src/Core/PathCurveEngine.cs (新建)
|
||
|
||
3.1.1 CalculateFillet 方法
|
||
/// <summary>
|
||
/// 计算圆弧切点和轨迹参数
|
||
/// </summary>
|
||
/// <param name="pPrev">前一个控制点</param>
|
||
/// <param name="pCurr">当前控制点</param>
|
||
/// <param name="pNext">下一个控制点</param>
|
||
/// <param name="turnRadius">转向半径（米）</param>
|
||
/// <returns>圆弧轨迹数据</returns>
|
||
public static ArcTrajectory CalculateFillet(
|
||
Point3D pPrev,
|
||
Point3D pCurr,
|
||
Point3D pNext,
|
||
double turnRadius)
|
||
{
|
||
// 1. 计算单位向量
|
||
Vector3D v1 = (pPrev - pCurr).Normalize();
|
||
Vector3D v2 = (pNext - pCurr).Normalize();
|
||
|
||
// 2. 计算夹角 α
|
||
double cosAlpha = Vector3D.DotProduct(v1, v2);
|
||
double angleRad = Math.Acos(Math.Clamp(cosAlpha, -1.0, 1.0));
|
||
|
||
// 3. 计算切线长 Lt = R / tan(α/2)
|
||
double Lt = turnRadius / Math.Tan(angleRad / 2.0);
|
||
|
||
// 4. 安全截断检查
|
||
double seg1Length = (pPrev - pCurr).Length;
|
||
double seg2Length = (pNext - pCurr).Length;
|
||
double maxAllowedLt = Math.Min(seg1Length, seg2Length) * 0.45;
|
||
|
||
double actualRadius = turnRadius;
|
||
if (Lt > maxAllowedLt)
|
||
{
|
||
Lt = maxAllowedLt;
|
||
actualRadius = Lt * Math.Tan(angleRad / 2.0);
|
||
}
|
||
|
||
// 5. 计算切点
|
||
Point3D ts = pCurr + v1 * Lt;
|
||
Point3D te = pCurr + v2 * Lt;
|
||
|
||
// 6. 计算圆心 (使用角平分线方向)
|
||
Vector3D bisector = (v1 + v2).Normalize();
|
||
double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
|
||
Point3D arcCenter = pCurr + bisector * distToCenter;
|
||
|
||
// 7. 计算圆弧长度
|
||
double arcLength = actualRadius * angleRad;
|
||
|
||
return new ArcTrajectory
|
||
{
|
||
Ts = ts,
|
||
Te = te,
|
||
ArcCenter = arcCenter,
|
||
RequestedRadius = turnRadius,
|
||
ActualRadius = actualRadius,
|
||
DeflectionAngle = angleRad,
|
||
ArcLength = arcLength
|
||
};
|
||
}
|
||
3.1.2 SampleArc 方法
|
||
/// <summary>
|
||
/// 圆弧采样为离散点序列
|
||
/// </summary>
|
||
/// <param name="trajectory">圆弧轨迹</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
/// <returns>采样点列表</returns>
|
||
public static List<Point3D> SampleArc(
|
||
ArcTrajectory trajectory,
|
||
double samplingStep)
|
||
{
|
||
var points = new List<Point3D>();
|
||
|
||
// 根据采样步长计算采样点数量
|
||
int sampleCount = Math.Max(2, (int)Math.Ceiling(trajectory.ArcLength / samplingStep));
|
||
|
||
// 计算起始和结束向量
|
||
Vector3D startVec = (trajectory.Ts - trajectory.ArcCenter).Normalize();
|
||
Vector3D endVec = (trajectory.Te - trajectory.ArcCenter).Normalize();
|
||
|
||
// 计算旋转轴（叉乘）
|
||
Vector3D rotationAxis = Vector3D.CrossProduct(startVec, endVec).Normalize();
|
||
|
||
// 等角度插值
|
||
for (int i = 0; i <= sampleCount; i++)
|
||
{
|
||
double t = i / (double)sampleCount;
|
||
double theta = t * trajectory.DeflectionAngle;
|
||
|
||
// Rodrigues旋转公式
|
||
Point3D sampledPoint = RotatePointAroundAxis(
|
||
trajectory.Ts,
|
||
trajectory.ArcCenter,
|
||
rotationAxis,
|
||
theta
|
||
);
|
||
|
||
points.Add(sampledPoint);
|
||
}
|
||
|
||
return points;
|
||
}
|
||
3.1.3 ApplyCurvatureToRoute 方法
|
||
/// <summary>
|
||
/// 对路径应用曲线化处理
|
||
/// </summary>
|
||
/// <param name="route">待处理的路径</param>
|
||
/// <param name="samplingStep">采样步长（米）</param>
|
||
public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep)
|
||
{
|
||
route.Edges.Clear();
|
||
var sortedPoints = route.GetSortedPoints();
|
||
|
||
if (sortedPoints.Count < 2)
|
||
{
|
||
route.IsCurved = false;
|
||
return;
|
||
}
|
||
|
||
for (int i = 0; i < sortedPoints.Count - 1; i++)
|
||
{
|
||
var p1 = sortedPoints[i];
|
||
var p2 = sortedPoints[i + 1];
|
||
|
||
// 判断是否为转折点（需要圆弧过渡）
|
||
bool needsArc = (i > 0 && i < sortedPoints.Count - 2);
|
||
|
||
if (needsArc)
|
||
{
|
||
var p0 = sortedPoints[i - 1];
|
||
var p2Next = sortedPoints[i + 2];
|
||
|
||
// 获取转向半径（优先使用自定义值）
|
||
double radius = p1.CustomTurnRadius ?? route.TurnRadius;
|
||
|
||
// 计算圆弧
|
||
var arcTraj = CalculateFillet(p0, p1, p2, radius);
|
||
|
||
// 创建Edge（包含直线段 + 圆弧段）
|
||
var edge = BuildEdgeWithArc(p1, p2, arcTraj, samplingStep);
|
||
route.Edges.Add(edge);
|
||
}
|
||
else
|
||
{
|
||
// 直线边
|
||
var edge = BuildStraightEdge(p1, p2, samplingStep);
|
||
route.Edges.Add(edge);
|
||
}
|
||
}
|
||
|
||
route.IsCurved = true;
|
||
route.RecalculateLength();
|
||
}
|
||
4. 配置文件扩展
|
||
[MODIFY]
|
||
SystemConfig.cs
|
||
在
|
||
PathEditingConfig
|
||
类中新增：
|
||
|
||
/// <summary>
|
||
/// 默认转向半径（米）
|
||
/// 推荐值：1.0-2.0
|
||
/// </summary>
|
||
public double DefaultTurnRadius { get; set; } = 1.5;
|
||
/// <summary>
|
||
/// 圆弧采样步长（米）
|
||
/// 推荐值：0.02-0.1
|
||
/// </summary>
|
||
public double ArcSamplingStep { get; set; } = 0.05;
|
||
|
||
配置文件示例 (TOML)
|
||
[PathEditing]
|
||
CellSizeMeters = 0.5
|
||
MaxHeightDiffMeters = 0.35
|
||
VehicleLengthMeters = 1.0
|
||
VehicleWidthMeters = 1.0
|
||
VehicleHeightMeters = 2.0
|
||
SafetyMarginMeters = 0.05
|
||
|
||
# 路径曲线化配置
|
||
|
||
DefaultTurnRadius = 1.5
|
||
ArcSamplingStep = 0.05
|
||
|
||
1. 业务流程集成
|
||
5.1 路径编辑完成流程
|
||
[MODIFY]
|
||
PathPlanningManager.cs
|
||
|
||
-
|
||
|
||
FinishEditing
|
||
方法
|
||
public bool FinishEditing()
|
||
{
|
||
try
|
||
{
|
||
if (_currentRoute == null)
|
||
return false;
|
||
|
||
// **新增：应用曲线化**
|
||
if (ConfigManager.Instance.Current.PathEditing.EnablePathCurving)
|
||
{
|
||
double samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
|
||
_currentRoute.TurnRadius = ConfigManager.Instance.Current.PathEditing.DefaultTurnRadius;
|
||
|
||
PathCurveEngine.ApplyCurvatureToRoute(_currentRoute, samplingStep);
|
||
LogManager.Info($"路径曲线化完成: {_currentRoute.Name}, 边数: {_currentRoute.Edges.Count}");
|
||
}
|
||
|
||
// 保存到数据库（包含Edges）
|
||
SaveRouteToDatabase(_currentRoute);
|
||
|
||
// ... 现有逻辑 ...
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"完成编辑失败: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
5.2 数据持久化
|
||
[MODIFY]
|
||
PathDatabase.cs
|
||
新增方法：
|
||
|
||
SavePathEdges(string routeId, List<PathEdge> edges)
|
||
LoadPathEdges(string routeId) : List<PathEdge>
|
||
DeletePathEdges(string routeId)
|
||
修改方法：
|
||
|
||
SavePathRoute(PathRoute route)
|
||
: 同时保存 Edges
|
||
GetAllPathRoutes()
|
||
: 同时加载 Edges
|
||
DeletePathRoute(string routeId)
|
||
: 级联删除 Edges
|
||
5.3 导出格式扩展
|
||
[MODIFY]
|
||
PathDataManager.cs
|
||
DELMIA XML 格式：
|
||
|
||
<Path name="路径1">
|
||
<Edges>
|
||
<Edge type="arc">
|
||
<Trajectory>
|
||
<Ts x="10.5" y="5.2" z="0.0"/>
|
||
<Te x="11.3" y="6.8" z="0.0"/>
|
||
<ArcCenter x="10.9" y="6.0" z="0.0"/>
|
||
<Radius>1.5</Radius>
|
||
</Trajectory>
|
||
</Edge>
|
||
</Edges>
|
||
</Path>
|
||
CSV 格式（扁平化采样点）：
|
||
|
||
RouteId,EdgeIndex,SegmentType,PointIndex,X,Y,Z
|
||
route_001,0,Arc,0,10.5,5.2,0.0
|
||
route_001,0,Arc,1,10.52,5.25,0.0
|
||
...
|
||
6. 碰撞检测与动画集成
|
||
6.1 碰撞检测
|
||
[MODIFY] PathAnimationManager.cs - 碰撞检测逻辑
|
||
// 从 Edges 获取采样点而非直接使用 Points
|
||
foreach (var edge in route.Edges)
|
||
{
|
||
if (edge.SampledPoints == null || edge.SampledPoints.Count == 0)
|
||
{
|
||
// 懒加载：若采样点未生成，现场生成
|
||
edge.SampledPoints = GenerateSampledPoints(edge, arcSamplingStep);
|
||
}
|
||
|
||
foreach (var point in edge.SampledPoints)
|
||
{
|
||
// 执行OBB碰撞检测 ...
|
||
}
|
||
}
|
||
6.2 动画生成
|
||
使用 edge.SampledPoints 代替原始控制点生成动画帧序列。
|
||
|
||
1. 验证计划
|
||
7.1 单元测试
|
||
新建测试文件: test/Core/PathCurveEngineTests.cs
|
||
|
||
测试用例：
|
||
|
||
基础几何计算
|
||
|
||
验证90度直角转弯的圆弧计算
|
||
验证锐角/钝角场景
|
||
验证3D空间旋转正确性
|
||
安全截断
|
||
|
||
验证半径过大时自动缩减
|
||
边界条件：最小夹角（~0°）和最大夹角（~180°）
|
||
采样精度
|
||
|
||
验证采样点数量符合步长要求
|
||
验证采样点均匀分布在圆弧上
|
||
运行命令（需确认实际测试框架）：
|
||
|
||
# 假设使用 xUnit
|
||
|
||
dotnet test NavisworksTransport.Tests --filter "FullyQualifiedName~PathCurveEngine"
|
||
7.2 集成测试
|
||
场景测试路径：
|
||
|
||
创建包含3个控制点的简单L型路径
|
||
设置转向半径 = 1.5m
|
||
完成编辑触发曲线化
|
||
验证：
|
||
Edges 数量= 2
|
||
第一个Edge为直线段，第二个Edge包含圆弧
|
||
TotalLength 正确（直线段 + 圆弧段）
|
||
重新加载路径验证持久化
|
||
7.3 手动验证
|
||
IMPORTANT
|
||
|
||
以下步骤需要用户在Navisworks中手动验证
|
||
|
||
测试步骤：
|
||
|
||
打开Navisworks，加载测试模型
|
||
进入路径规划模式，手工添加4个控制点形成Z字形路径
|
||
在配置文件中设置：
|
||
DefaultTurnRadius = 2.0
|
||
ArcSamplingStep = 0.05
|
||
EnablePathCurving = true
|
||
完成编辑，观察路径可视化是否显示平滑曲线（而非折线）
|
||
运行碰撞检测，检查转弯处的采样点密度
|
||
导出DELMIA XML，用文本编辑器验证圆弧轨迹参数存在
|
||
预期结果：
|
||
|
||
路径在转折点处显示圆弧过渡
|
||
碰撞检测能捕获内轮差区域的碰撞
|
||
XML包含完整的 <Trajectory> 节点
|
||
8. 风险与注意事项
|
||
8.1 性能影响
|
||
圆弧采样：每个转折点生成 ArcLength / SamplingStep 个采样点
|
||
缓解措施：默认步长0.05m平衡精度与性能，支持配置调整
|
||
8.2 数据兼容性
|
||
旧版路径数据库无 Edges 表
|
||
迁移策略：
|
||
检测表是否存在，不存在则创建
|
||
加载旧路径时，Edges为空视为"未曲线化"，用户完成编辑时自动曲线化
|
||
8.3 极端场景
|
||
非常小的转向半径 + 锐角：可能导致无法满足几何约束
|
||
处理：最小半径限制0.3m，警告日志记录
|
||
9. 实施顺序建议
|
||
✅ 数据结构 → 新增类和修改现有模型
|
||
✅ 配置管理 → 扩展SystemConfig和TOML
|
||
✅ 核心算法 → PathCurveEngine + 单元测试
|
||
✅ 数据库 → 表结构变更 + CRUD方法
|
||
✅ 业务集成 → FinishEditing + 数据持久化
|
||
✅ 导出扩展 → XML/CSV格式支持
|
||
✅ 碰撞检测 → 使用Edges采样点
|
||
✅ 验证测试 → 集成测试 + 手动验证
|
||
10. 后续优化方向
|
||
支持Clothoid曲线（回旋曲线）以实现更平滑的加速度变化
|
||
UI可视化：实时预览圆弧位置
|
||
性能优化：采样点缓存机制
|
||
|
||
物流车转弯路径曲线化功能实施任务规划
|
||
总体目标
|
||
实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，满足仿真系统对物理真实性的要求。
|
||
|
||
实施阶段
|
||
[ ] 阶段1: 数据结构设计与扩展
|
||
设计 Edge（边）数据结构
|
||
PathEdge 类定义
|
||
PathSegmentType 枚举（直线/圆弧）
|
||
ArcTrajectory 圆弧轨迹数据
|
||
扩展 PathPoint 支持局部半径覆盖
|
||
新增 CustomTurnRadius 属性
|
||
重构 PathRoute 类
|
||
新增 Edges 集合
|
||
新增 TurnRadius 全局转向半径
|
||
更新长度计算逻辑
|
||
更新数据库表结构
|
||
设计 Edges 表结构
|
||
设计数据迁移策略
|
||
[ ] 阶段2: 曲线化算法实现
|
||
创建 PathCurveEngine 核心算法类
|
||
CalculateFillet 方法（圆弧切点计算）
|
||
安全截断逻辑实现
|
||
SampleArc 方法（圆弧采样）
|
||
实现路径曲线化主流程
|
||
ApplyCurvatureToRoute 方法
|
||
BuildEdgesFromPoints 方法
|
||
单元测试
|
||
基础几何计算测试
|
||
边界条件测试
|
||
[ ] 阶段3: 配置管理
|
||
添加曲线化相关配置项
|
||
DefaultTurnRadius (默认转向半径)
|
||
ArcSamplingStep (圆弧采样步长，默认0.05m)
|
||
EnablePathCurving (启用开关)
|
||
更新配置文件读取/保存逻辑
|
||
[ ] 阶段4: 集成到现有业务流程
|
||
修改 PathRoute 长度计算
|
||
直线段 + 圆弧段综合计算
|
||
集成到路径编辑完成流程
|
||
FinishEditing 触发曲线化
|
||
更新数据持久化
|
||
PathDatabase 保存/加载 Edges
|
||
PathDataManager 导出格式扩展
|
||
[ ] 阶段5: 碰撞检测与动画集成
|
||
碰撞检测使用曲线路径
|
||
采样点序列生成
|
||
集成到现有碰撞检测
|
||
PathAnimationManager 支持曲线路径
|
||
使用 Edges 生成动画帧
|
||
[ ] 阶段6: DELMIA 导出扩展
|
||
XML Tag Group 格式支持圆弧
|
||
导出 Ts, Te, ArcCenter
|
||
CSV 格式导出采样点序列
|
||
[ ] 阶段7: 测试与验证
|
||
功能测试
|
||
性能测试
|
||
边界场景测试
|