172 lines
12 KiB
Markdown
172 lines
12 KiB
Markdown
# **无人物流车转弯路径曲线化方案**
|
||
|
||
## **1. 需求分析与 3D 仿真环境背景**
|
||
|
||
在复杂车间环境下，无人物流车（AGV/AMR）的碰撞仿真精度直接受限于路径几何表达的保真度。传统的离散路径（直线连接）在转弯处存在航向角突变，无法描述真实的“扫掠路径（Swept Path）”，导致内轮差擦撞等关键隐患无法在仿真中检出 1。
|
||
|
||
由于当前仿真系统基于 **Autodesk Navisworks** 进行 3D 展示和碰撞分析，路径处理的核心需求已从简单的 2D 绘图演变为 **3D 空间轨迹的动态生成与标准化输出**。系统需要根据动力学约束（如最小转弯半径）将直线折角平滑为圆弧段，并能将生成的 3D 特征点（切点、圆心、采样位姿）同步至 Navisworks 场景及 DELMIA 等专业数字化工厂平台 3。
|
||
|
||
## **2. 关键算法：3D 圆弧平滑与拓扑重构**
|
||
|
||
本方案采用 **圆弧过渡法（Arc Fillet）**，其曲率恒定的特性最符合大多数 AGV 的转向特性，且计算开销低，易于在 3D 空间进行高频位姿采样 6。
|
||
|
||
### **2.1 几何计算原理**
|
||
|
||
假设路径中有连续三点 mathbf{P}_{i-1}, mathbf{P}_i, mathbf{P}_{i+1}，转弯半径为 R。
|
||
|
||
1. **计算夹角 alpha**：通过 3D 向量 vec{v}_1 = mathbf{P}_{i-1}-mathbf{P}_i 和 vec{v}_2 = mathbf{P}_{i+1}-mathbf{P}_i 的夹角公式求得。
|
||
2. 计算切线长度 L_t：
|
||
|
||
L_t = frac{R}{tan(frac{alpha}{2})}
|
||
3. **动态半径缩放**：为防止曲线重叠，限制 L_t le 0.45 times min(dist_1, dist_2) 9。
|
||
4. **定位切点**：
|
||
* **进入切点 (T_s)**：mathbf{P}_i + L_t cdot text{unit}(vec{v}_1)
|
||
* **退出切点 (T_e)**：mathbf{P}_i + L_t cdot text{unit}(vec{v}_2)
|
||
|
||
### **2.2 路径点逻辑重构：控制点与轨迹点分离**
|
||
|
||
平滑处理后，路径逻辑从“点列表”升级为“段序列”：
|
||
|
||
* **虚拟控制点 (Control Points)**：原始 Waypoints P_i 仅作为拓扑控制柄。
|
||
* **物理特征点**：新增 T_s、T_e 和圆弧中心 O，用于精确控制转向的起始与结束。
|
||
* **运动状态机**：AGV 在直线段 T_{e(i-1)} to T_{s(i)} 处于巡线状态，到达 T_s 后切换为圆弧插补转向模式 11。
|
||
|
||
## **3. 基于 VDA 5050 标准的路径数据结构**
|
||
|
||
为了实现仿真系统与真实车辆控制、以及 DELMIA 等平台的深度集成，本方案参考 **VDA 5050 协议** 定义了标准化的路径数据模型。VDA 5050 将路径描述为一个由**节点（Nodes）**和**边（Edges）**组成的有向图，并允许通过轨迹属性精确定义运动曲线。
|
||
|
||
### **3.1 节点 (Node) 数据结构**
|
||
|
||
节点对应路径中的关键停靠点或控制点（Waypoints）。
|
||
|
||
| 字段名称 | 类型 | 说明 |
|
||
| :---- | :---- | :---- |
|
||
| nodeId | String | 节点的唯一标识符。 |
|
||
| sequenceId | Integer | 在当前订单路径中的序列号（递增）。 |
|
||
| nodePosition | Object | 包含 x、y 坐标及车辆目标航向角 theta（弧度）。 |
|
||
| actions | Array | 在该位置执行的动作（如：举升、等待）。 |
|
||
|
||
### **3.2 边 (Edge) 数据结构**
|
||
|
||
边定义了两个节点之间的物理连接及运动约束。
|
||
|
||
| 字段名称 | 类型 | 说明 |
|
||
| :---- | :---- | :---- |
|
||
| edgeId | String | 边的唯一标识符。 |
|
||
| startNodeId | String | 边的起始节点 ID。 |
|
||
| endNodeId | String | 边的终止节点 ID。 |
|
||
| maxSpeed | Double | 该路段允许的最大行驶速度（单位：m/s）。 |
|
||
| trajectory | Object | **核心：** 定义边上的具体运动曲线（如圆弧平滑后的路径）。 |
|
||
|
||
### **3.3 轨迹 (Trajectory) 与 NURBS 描述**
|
||
|
||
VDA 5050 推荐使用 NURBS（非均匀有理 B 样条）来描述轨迹，这能完美覆盖直线和圆弧。
|
||
|
||
* **degree (阶数)**：对于直线段 text{degree} = 1；对于圆弧平滑段，建议 text{degree} = 2。
|
||
* **controlPoints (控制点列表)**：包含 3D 坐标 (x, y, z) 及其权重 w。
|
||
* 对于圆弧段，切点 T_s 和 T_e 作为起始/结束控制点，折点 P_i 配合特定权重可定义出精确圆弧。
|
||
* **knotVector (节点向量)**：定义控制点对曲线的影响范围及连续性。
|
||
|
||
## **4. DELMIA 兼容性与 3D 展示集成**
|
||
|
||
### **4.1 兼容性导出格式 (XML/CSV)**
|
||
|
||
为了让生成的路径能够与 DELMIA 兼容，系统支持将 VDA 5050 结构转换为 **Tag Groups** 格式，这在 DELMIA 机器人任务中具有原生支持：
|
||
|
||
```xml
|
||
<TagGroup Name="AGV_Path_A1">
|
||
<Tag Name="P1_Linear" X="120.5" Y="45.0" Z="0.0" Type="Line_End"/>
|
||
<Tag Name="P2_Ts" X="124.0" Y="45.0" Z="0.0" Type="Arc_Start"/>
|
||
<Tag Name="P2_Center" X="124.0" Y="46.5" Z="0.0" Radius="1.5" Type="Arc_Center"/>
|
||
<Tag Name="P2_Te" X="125.0" Y="46.5" Z="0.0" Type="Arc_End"/>
|
||
</TagGroup>
|
||
```
|
||
|
||
### **4.2 Navisworks 内部 UI 界面设计**
|
||
|
||
UI 应设计为集成在 Navisworks 内部的 **Dockable Pane**：
|
||
|
||
* **路径配置区**：显示所有段落（Edges），支持针对特定拐角手动覆盖半径（Radius Override）。
|
||
* **可视化开关**：提供“显示控制柄（Node）”和“显示物理轨迹（Trajectory）”的切换按钮。
|
||
* **导出工具**：一键将符合 VDA 5050 逻辑的路径导出为 DELMIA XML 或 CSV 序列。
|
||
|
||
## **5. 算法 C# 实现示例：Navisworks 3D 轨迹生成**
|
||
|
||
以下代码演示了如何根据 VDA 5050 逻辑计算 3D 切点，并通过 Navisworks API 进行简单的几何操作。
|
||
|
||
```csharp
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using Autodesk.Navisworks.Api; // Navisworks.NET API
|
||
|
||
namespace AGVSim.Core {
|
||
public class NavisPathEngine {
|
||
// 计算 3D 空间圆弧参数并生成 VDA 5050 兼容节点
|
||
public ArcResult Compute3DFillet(Point3D pPrev, Point3D pCurr, Point3D pNext, double radius) {
|
||
Vector3D v1 = pPrev - pCurr;
|
||
Vector3D v2 = pNext - pCurr;
|
||
|
||
// 1. 计算夹角 (3D 向量内积)
|
||
double angleRad = Math.Acos(Vector3D.DotProduct(v1.Normalize(), v2.Normalize()));
|
||
|
||
// 2. 切线长度计算与安全截断
|
||
double tangentLength = radius / Math.Tan(angleRad / 2.0);
|
||
double limit = Math.Min(v1.Length, v2.Length) * 0.45;
|
||
if (tangentLength > limit) {
|
||
tangentLength = limit;
|
||
radius = tangentLength * Math.Tan(angleRad / 2.0);
|
||
}
|
||
|
||
// 3. 计算 VDA 5050 物理轨迹切点 Ts, Te
|
||
Point3D Ts = pCurr + v1.Normalize() * tangentLength;
|
||
Point3D Te = pCurr + v2.Normalize() * tangentLength;
|
||
|
||
return new ArcResult { Ts = Ts, Te = Te, R = radius, IsValid = true };
|
||
}
|
||
|
||
// 用于 Navisworks API 的离散化采样点生成 (用于碰撞检测与 3D 渲染)
|
||
public List<Point3D> SamplePath(ArcResult arc, double stepSize = 0.05) {
|
||
var points = new List<Point3D>();
|
||
// 内部实现：基于 Rodrigues 旋转公式或切线平面进行 Point3D 离散化插值
|
||
return points;
|
||
}
|
||
}
|
||
|
||
public struct ArcResult {
|
||
public Point3D Ts; public Point3D Te;
|
||
public double R; public bool IsValid;
|
||
}
|
||
}
|
||
```
|
||
|
||
## **6. 高精度碰撞仿真的深度优化：扫掠路径分析 (Swept Path Analysis)**
|
||
|
||
在 Navisworks 中进行碰撞检测时，需利用其 API 执行精细判定：
|
||
|
||
1. **位姿序列生成**：在轨迹段（Trajectory）按采样步长 Delta s 生成密集的位姿切片。每个位姿包含精确的朝向角 theta。
|
||
2. **多点包围盒 (Multi-OBB)**：对车体四个角点分别计算轨迹线，生成其扫掠包络区域，确保内轮差区域无死角检测 1。
|
||
3. **SAT 碰撞算法**：对每个采样位的 OBB 执行分离轴定理（SAT）检测，确保仿真结果符合物理真实性 14。
|
||
|
||
## **7. 结论与工程实践建议**
|
||
|
||
本方案通过引入 **VDA 5050 标准的数据模型**，将路径抽象为“具备轨迹属性的边”，解决了平滑后原始路径点偏移的逻辑矛盾。同时，通过支持 **DELMIA XML Tag Groups** 导出，实现了从 Navisworks 离线规划到专业仿真平台的数据闭环。
|
||
|
||
在 Navisworks 插件开发中，建议利用其 RenderPlugin 或 OverlayRender 接口实时展示计算出的平滑轨迹，确保用户在调节转弯半径时能即时看到物理扫掠区域的变化。
|
||
|
||
#### **Works cited**
|
||
|
||
1. Turning Vehicle Simulation: Interactive Computer-Aided Design and Drafting Application, accessed December 30, 2025, [https://onlinepubs.trb.org/Onlinepubs/trr/1995/1500/1500-001.pdf](https://onlinepubs.trb.org/Onlinepubs/trr/1995/1500/1500-001.pdf)
|
||
2. Step-by-Step Guide to Conducting a Swept Path Analysis - Quantum Traffic, accessed December 30, 2025, [https://www.quantumtraffic.com.au/step-by-step-guide-to-conducting-a-swept-path-analysis](https://www.quantumtraffic.com.au/step-by-step-guide-to-conducting-a-swept-path-analysis)
|
||
3. Smoothed A* Algorithm for Practical Unmanned Surface Vehicle Path Planning - UCL Discovery, accessed December 30, 2025, [https://discovery.ucl.ac.uk/10064237/3/Liu Smoothed A algorithm for practical unmanned surface vehicle path planning.pdf](https://discovery.ucl.ac.uk/10064237/3/Liu Smoothed A algorithm for practical unmanned surface vehicle path planning.pdf)
|
||
4. AGV PATH PLANNING BASED ON SMOOTHING A* ALGORITHM, accessed December 30, 2025, [https://airccse.org/journal/ijsea/papers/6515ijsea01.pdf](https://airccse.org/journal/ijsea/papers/6515ijsea01.pdf)
|
||
5. Iterative Learning-Based Path and Speed Profile Optimization for an Unmanned Surface Vehicle - PMC - NIH, accessed December 30, 2025, [https://pmc.ncbi.nlm.nih.gov/articles/PMC7014130/](https://pmc.ncbi.nlm.nih.gov/articles/PMC7014130/)
|
||
6. Curved Paths - Red Blob Games, accessed December 30, 2025, [https://www.redblobgames.com/articles/curved-paths/](https://www.redblobgames.com/articles/curved-paths/)
|
||
7. PRM Path Smoothening by Circular Arc Fillet Method for Mobile Robot Navigation - DergiPark, accessed December 30, 2025, [https://dergipark.org.tr/tr/download/article-file/3067231](https://dergipark.org.tr/tr/download/article-file/3067231)
|
||
8. Solved: Fillet Two line c# - Autodesk Community, accessed December 30, 2025, [https://forums.autodesk.com/t5/net-forum/fillet-two-line-c/td-p/13101919](https://forums.autodesk.com/t5/net-forum/fillet-two-line-c/td-p/13101919)
|
||
9. AG Corners Live Effect | Astute Graphics Documentation, accessed December 30, 2025, [https://docs.astutegraphics.com/vectorscribe/ag-corners-live-effect](https://docs.astutegraphics.com/vectorscribe/ag-corners-live-effect)
|
||
10. javascript - Applying rounded corners to paths/polygons - Stack Overflow, accessed December 30, 2025, [https://stackoverflow.com/questions/19269622/applying-rounded-corners-to-paths-polygons](https://stackoverflow.com/questions/19269622/applying-rounded-corners-to-paths-polygons)
|
||
11. Path planning for autonomous vehicles using clothoid based smoothing of A* generated paths and optimal control - DiVA portal, accessed December 30, 2025, [https://www.diva-portal.org/smash/get/diva2:1150741/FULLTEXT01.pdf](https://www.diva-portal.org/smash/get/diva2:1150741/FULLTEXT01.pdf)
|
||
12. Collision Detection - Cepton, accessed December 30, 2025, [https://developer.cepton.com/blog/2d_collision_detection](https://developer.cepton.com/blog/2d_collision_detection)
|
||
13. Exact Mathematical Solution for Maximum Transient Offtracking Calculation of a Single-Unit Vehicle Negotiating Circular Curves - MDPI, accessed December 30, 2025, [https://www.mdpi.com/2076-3417/14/13/5570](https://www.mdpi.com/2076-3417/14/13/5570)
|
||
14. OrientedBoundingBox(OBB)Collision · SimonDarksideJ ... - GitHub, accessed December 30, 2025, [https://github.com/SimonDarksideJ/XNAGameStudio/wiki/OrientedBoundingBox(OBB)Collision](https://github.com/SimonDarksideJ/XNAGameStudio/wiki/OrientedBoundingBox(OBB)Collision)
|
||
15. Collision Detection Using the Separating Axis Theorem | Envato Tuts+, accessed December 30, 2025, [https://code.tutsplus.com/collision-detection-using-the-separating-axis-theorem--gamedev-169t](https://code.tutsplus.com/collision-detection-using-the-separating-axis-theorem--gamedev-169t) |