2025-12-30 14:40:37 +08:00

26 KiB

Raw Blame History

工业无人物流车路径平滑处理与高精度碰撞仿真技术研究报告

离散路径规划在复杂物流环境中的局限性与平滑需求分析

在当代工业自动化领域，无人物流车（AGV/AMR）的路径规划通常始于基于图搜索（如 A* 算法）或采样（如 RRT 算法）的离散路径生成过程 1。这些算法生成的原始路径通常由一系列离散的路径点（Waypoints）及连接它们的直线段组成。然而，这种简化的几何表达在高性能碰撞仿真和真实车辆控制中存在显著的物理性缺陷。

首先，离散路径在路径点处存在明显的几何不连续性，即切向量在折点处发生突变，这种现象在运动学上表现为车辆航向角（Heading Angle）的瞬时改变 3。由于真实的物流车辆受限于非圆约束（Non-holonomic Constraints），如轴距、转向机构的最大偏转角等，车辆无法在静止状态外实现零半径的瞬时转向 5。如果仿真系统仅基于直线段进行碰撞检测，系统将完全忽略车辆在转弯过程中由于旋转而产生的扫掠区域（Swept Path）变化。对于精度要求极高的车间环境，这种“运动学失真”可能导致严重的仿真偏差，例如车辆在转弯时车尾摆动（Tail-swing）可能擦撞侧方货架，而传统的直线连接模型由于无法描述转弯曲线，将导致此类碰撞事件的漏检 7。

其次，碰撞仿真的保真度直接取决于路径的几何连续性等级。在路径点处，简单的 C^0 连续（仅位置连续）会导致车辆执行器产生无限大的向心加速度需求 4。通过引入平滑曲线（如圆弧或贝塞尔曲线），可以将路径提升至 G^1（切线连续）甚至 G^2（曲率连续）级别。这种提升不仅是为了视觉上的平滑，更是为了在碰撞算法中能够以解析或高频采样的方式计算车体包围盒在每一时刻的精确位姿（Pose） 10。

车辆运动学模型与平滑曲线的技术关联

要实现符合真实场景的路径平滑，必须首先确立车辆的运动学基础。物流车的最小转弯半径 R 是由其物理结构决定的关键参数，它与车辆轴距 L 和转向轮最大偏角 delta 存在明确的数学关系：

R = frac{L}{tan(delta)}
在车间环境的路径优化中，任何平滑技术都必须将此半径作为硬性约束，确保生成的曲线在任何一点的曲率半径 rho 均满足 rho ge R_{min} 5。

在碰撞仿真中，车辆通常被视为一个具有固定长度和宽度的刚体多边形，最常见的是导向包围盒（OBB, Oriented Bounding Box） 14。当路径从离散折线变为连续曲线后，OBB 的中心坐标 (x, y) 和旋转角 theta 成为路径参数 s（弧长）的连续函数。这意味着碰撞检测引擎不再仅仅检查静态线段之间的距离，而是需要通过对曲线进行精细采样，生成一系列在时间或空间上密集的位姿实例，并对每一对实例执行分离轴定理（SAT）检测 16。这种从“线段检测”向“位姿序列检测”的转变，是提升碰撞仿真精度的核心机制。

路径平滑算法的对比与选型

在实现局部功能优化时，需要权衡算法的计算复杂性、几何特性以及在 C# 环境下的实现难度。下表对比了目前主流的路径平滑技术方案：

算法类型	连续性等级	计算复杂度	几何特性与碰撞检测适用性	物理仿真匹配度
圆弧过渡 (Arc Fillet)	G^1	极低	曲率恒定，偏置曲线计算简单，最适合 OBB 线性插值 19。	高（符合大多数 AGV 恒定转向角特性）
二次贝塞尔曲线 (Quadratic Bézier)	G^1	低	抛物线形式，计算位姿需数值采样 10。	中（曲率变化不均匀）
三次 B 样条 (B-Spline)	C^2	高	曲率连续，适合高速 AGV，但局部控制参数调整复杂 4。	极高（平滑度最好）
回旋曲线 (Clothoid)	G^2	极高	曲率随弧长线性变化，计算极其复杂，涉及菲涅耳积分 12。	极高（符合真实驾驶习惯）

针对“局部功能优化”且“满足仿真要求”的前提，本方案选择圆弧过渡法（Arc Fillet）作为核心算法 19。该算法在两个切线段之间插入一段半径为 R 的圆弧。其优势在于：圆弧上的曲率半径是恒定的，这使得在仿真中可以非常容易地计算出车辆在转弯时的固定偏转角，且圆弧的偏置线（考虑车体宽度后的扫掠轨迹）依然是简单的圆弧或线段，极大地降低了碰撞检测的数学开销。

关键算法实现与路径拓扑重构

圆弧平滑技术的核心在于根据给定的折点坐标和目标半径，计算出圆弧的切点、中心点以及弧线参数。

1. 几何计算原理

假设路径中存在连续的三个点 mathbf{P}{i-1}、mathbf{P}{i}（折点）和 mathbf{P}{i+1}。平滑的目标是在 mathbf{P}{i-1}mathbf{P}{i} 和 mathbf{P}{i}mathbf{P}_{i+1} 之间插入半径为 R 的圆弧 23。

首先定义两个方向向量：

mathbf{v}1 = frac{mathbf{P}{i-1} - mathbf{P}{i}}{|mathbf{P}{i-1} - mathbf{P}_{i}|}

mathbf{v}2 = frac{mathbf{P}{i+1} - mathbf{P}{i}}{|mathbf{P}{i+1} - mathbf{P}_{i}|}
计算两向量之间的夹角 alpha = arccos(mathbf{v}_1 cdot mathbf{v}_2)。
圆弧的切线长度 L_t（即从折点 mathbf{P}_i 到切点 mathbf{T} 的距离）计算公式为：

L_t = frac{R}{tan(frac{alpha}{2})}
这里存在一个关键的物理约束：切线长度 L_t 不能超过相邻两线段长度的一半，否则圆弧会与前后的平滑处理发生重叠，导致路径扭曲 24。

2. 路径点逻辑重构：控制点与轨迹点的分离

在曲线平滑后，原始路径点 mathbf{P}_i 将不再位于物理路径上。在路径规划逻辑中，应按以下策略处理：

角色定义：将原始路径点定义为虚拟控制点 (Control Points)。它们用于定义路径的宏观拓扑，不直接参与车辆的运动执行。
特征点生成：算法会在每个折点处生成两个新的物理特征点——进入切点 (Tangent Start, T_s) 和 退出切点 (Tangent End, T_e)。
段序列结构：将路径的数据结构由 List 升级为 List。路径变为直线段与圆弧段交替的序列：P_0 to T_{s1} to text{Arc}(T_{s1}, T_{e1}) to T_{s2} dots 。
状态切换逻辑：车辆控制器在直线段行驶时处于“巡线模式”，一旦坐标到达 T_s，立即切换为“定曲率转向模式”，直至到达 T_e 后恢复直线巡线。

控制参数描述与可视化方案

1. 控制参数表

参数名称	变量类型	单位	描述说明	典型取值范围
期望转弯半径 (R)	Double	米	车辆执行转弯的曲率半径。	0.5 - 5.0
最小半径限制 (R_min)	Double	米	转向机构物理极限，作为强制约束。	0.8 - 1.5
采样步长 (Delta s)	Double	米	碰撞离散化采样的空间分辨率 7。	0.01 - 0.1
切线占比上限 (eta)	Double	%	防止圆弧重叠的最大比例 24。	10% - 45%

2. 分层可视化策略 (Windows/WPF)

在 UI 设计中，为了兼顾路径编辑的直观性和仿真的准确性，应采用分层渲染：

设计辅助层 (Design Skeleton)：以虚线连接原始控制点 P_i。保留 P_i 的渲染标识，使其可交互（拖拽修改路径）。
执行轨迹层 (Physical Trajectory)：使用加粗实线绘制由直线和圆弧组成的实际路径。这是碰撞仿真引擎真正检测的对象。
切点标注 (Markers)：在切点 T_s 和 T_e 处渲染不同颜色的微小圆点（如绿入红出），以便调试人员精确识别转弯启动位置。

算法与可视化 C# 实现示例

以下代码展示了平滑算法的核心逻辑。该示例包含几何参数计算、半径约束检查以及碰撞检测所需的点位生成。

using System;
using System.Collections.Generic;
using System.Windows; // 提供 Vector 和 Point 计算支持

namespace LogisticsVehicleSimulation.Geometry
{
    /// <summary>
    /// 圆弧平滑路径段计算结果
    /// </summary>
    public class ArcSegment
    {
        public Point Center { get; set; }
        public Point StartPoint { get; set; }
        public Point EndPoint { get; set; }
        public double Radius { get; set; }
        public double StartAngle { get; set; }
        public double SweepAngle { get; set; }
        public bool IsValid { get; set; }
    }

    /// <summary>
    /// 路径平滑引擎核心算法类
    /// 参考资料: [22, 23, 34]
    /// </summary>
    public class PathSmoothingEngine
    {
        private const double MinEdgeRatio = 0.45; // 限制切线长度不超过边长的45%

        /// <summary>
        /// 为给定的路径点折角计算圆弧平滑参数
        /// </summary>
        public ArcSegment ComputeArcFillet(Point pPrev, Point pCurr, Point pNext, double desiredRadius)
        {
            // 1. 提取方向向量并归一化 
            Vector v1 = pPrev - pCurr;
            Vector v2 = pNext - pCurr;
            double L1 = v1.Length;
            double L2 = v2.Length;
            
            v1.Normalize();
            v2.Normalize();

            // 2. 计算向量夹角 alpha
            double dot = v1 * v2;
            double alpha = Math.Acos(Math.Clamp(dot, -1.0, 1.0));

            // 3. 计算并限制切线长度 
            // 公式: Tan(alpha/2) = R / Lt => Lt = R / Tan(alpha/2)
            double tangentLength = desiredRadius / Math.Tan(alpha / 2.0);
            double actualRadius = desiredRadius;

            // 几何安全性检查：如果半径过大导致切点超过线段一半，则自动缩放半径
            double maxAllowedTangent = Math.Min(L1, L2) * MinEdgeRatio;
            if (tangentLength > maxAllowedTangent)
            {
                tangentLength = maxAllowedTangent;
                actualRadius = tangentLength * Math.Tan(alpha / 2.0);
            }

            // 4. 计算关键几何点坐标 [34, 35]
            Point T1 = pCurr + (v1 * tangentLength);
            Point T2 = pCurr + (v2 * tangentLength);

            // 圆弧中心位于角平分线上，距折点距离为 R / Sin(alpha/2)
            Vector bisector = v1 + v2;
            bisector.Normalize();
            double distToCenter = actualRadius / Math.Sin(alpha / 2.0);
            Point center = pCurr + (bisector * distToCenter);

            // 5. 计算起始角与弧度范围
            Vector centerToT1 = T1 - center;
            Vector centerToT2 = T2 - center;
            double startAngle = Math.Atan2(centerToT1.Y, centerToT1.X);
            double endAngle = Math.Atan2(centerToT2.Y, centerToT2.X);

            // 处理跨越 -PI/PI 的扫描角逻辑
            double sweepAngle = endAngle - startAngle;
            while (sweepAngle > Math.PI) sweepAngle -= 2 * Math.PI;
            while (sweepAngle < -Math.PI) sweepAngle += 2 * Math.PI;

            return new ArcSegment
            {
                Center = center,
                StartPoint = T1,
                EndPoint = T2,
                Radius = actualRadius,
                StartAngle = startAngle,
                SweepAngle = sweepAngle,
                IsValid = true
            };
        }

        /// <summary>
        /// 生成用于碰撞仿真的采样位姿序列 [7, 16, 26]
        /// </summary>
        public List<VehiclePose> GenerateSimulationPoses(ArcSegment arc, double samplingStep)
        {
            var poses = new List<VehiclePose>();
            double arcLength = Math.Abs(arc.SweepAngle) * arc.Radius;
            int stepCount = Math.Max(2, (int)(arcLength / samplingStep));

            for (int i = 0; i <= stepCount; i++)
            {
                double t = (double)i / stepCount;
                double currentAngle = arc.StartAngle + (arc.SweepAngle * t);
                
                // 位置坐标
                double x = arc.Center.X + arc.Radius * Math.Cos(currentAngle);
                double y = arc.Center.Y + arc.Radius * Math.Sin(currentAngle);
                
                // 航向角计算：对于圆弧，位姿方向即为切线方向
                // 切线方向 = 半径方向 + 90度 (取决于旋转方向)
                double heading = currentAngle + (arc.SweepAngle > 0? Math.PI / 2.0 : -Math.PI / 2.0);

                poses.Add(new VehiclePose(new Point(x, y), heading));
            }
            return poses;
        }
    }

    public struct VehiclePose
    {
        public Point Position;
        public double HeadingRadians; // 弧度单位
        public VehiclePose(Point p, double h) { Position = p; HeadingRadians = h; }
    }
}

以下代码展示了如何使用 WPF 的 PathGeometry 构建这种复合路径，并实现特征点计算。

using System;
using System.Collections.Generic;
using System.Windows;
using System.Windows.Media; // WPF 绘图支持

namespace AGVSimulation.Geometry
{
    public class PathManager
    {
        /// <summary>
        /// 将离散路径点转换为 WPF 可视化几何对象
        /// </summary>
        public PathGeometry CreateSmoothPath(List<Point> waypoints, double radius)
        {
            var geometry = new PathGeometry();
            var figure = new PathFigure { StartPoint = waypoints, IsClosed = false };

            for (int i = 1; i < waypoints.Count - 1; i++)
            {
                // 计算平滑参数 [23, 27]
                var arc = ComputeArcFillet(waypoints[i - 1], waypoints[i], waypoints[i + 1], radius);
                
                if (arc.IsValid)
                {
                    // 1. 添加连接到切点 Ts 的直线段
                    figure.Segments.Add(new LineSegment(arc.StartPoint, true));

                    // 2. 添加圆弧段 
                    figure.Segments.Add(new ArcSegment(
                        arc.EndPoint,
                        new Size(arc.Radius, arc.Radius),
                        0, // 旋转角
                        Math.Abs(arc.SweepAngle) > Math.PI, // 大弧标志
                        arc.SweepAngle > 0? SweepDirection.Clockwise : SweepDirection.Counterclockwise,
                        true));
                }
                else
                {
                    // 如果无法生成圆弧（如点重合），则直接连接到折点
                    figure.Segments.Add(new LineSegment(waypoints[i], true));
                }
            }

            // 连接到终点
            figure.Segments.Add(new LineSegment(waypoints[^1], true));
            geometry.Figures.Add(figure);
            return geometry;
        }

        private ArcInfo ComputeArcFillet(Point pPrev, Point pCurr, Point pNext, double R)
        {
            Vector v1 = pPrev - pCurr;
            Vector v2 = pNext - pCurr;
            double alpha = Vector.AngleBetween(v1, v2); // 返回角度
            double alphaRad = Math.Abs(alpha) * Math.PI / 180.0;

            // 计算切线长 Lt = R / tan(alpha/2) 
            double tangentLength = R / Math.Tan(alphaRad / 2.0);
            
            // 约束检查：防止切点超过边长的一半 
            double limit = Math.Min(v1.Length, v2.Length) * 0.45;
            if (tangentLength > limit)
            {
                tangentLength = limit;
                R = tangentLength * Math.Tan(alphaRad / 2.0);
            }

            v1.Normalize(); v2.Normalize();
            Point Ts = pCurr + (v1 * tangentLength);
            Point Te = pCurr + (v2 * tangentLength);

            return new ArcInfo { 
                StartPoint = Ts, EndPoint = Te, Radius = R, 
                SweepAngle = alpha, IsValid = true 
            };
        }
    }

    public struct ArcInfo {
        public Point StartPoint; public Point EndPoint;
        public double Radius; public double SweepAngle; public bool IsValid;
    }
}

高精度碰撞仿真的深度优化：扫掠路径分析 (Swept Path Analysis)

在无人物流车的实际运行中，仅仅平滑路径中心点是不够的。高精度仿真的核心挑战在于如何处理转弯时车辆的“内轮差”和“车尾摆动” 7。

扫掠区域几何特性

当物流车沿着圆弧路径行驶时，其内侧后轮的行驶半径要小于前轮（转向轮）的行驶半径。在仿真中，这种现象被称为“Off-tracking” 7。如果不考虑这一点，仿真可能显示路径安全，但实际车辆转弯时内侧车体会撞击立柱。

高精度仿真方案应采用以下改进措施：

多点包围盒检测：不仅仅将车体简化为一个矩形，而是对车体的四个关键角点分别计算轨迹圆弧。车体占据的扫掠面积被定义为所有顶点轨迹的包络面 7。
空间占据网格预检：在转弯处，基于平滑后的参数，将扫掠区域划分为一组微小的矩形切片 16。
时间连续碰撞检测 (CCD)：通过平滑曲线模型，我们可以实现基于扫描体积的连续检测，从而确保即使在高速转弯仿真中也不会遗漏任何接触点 14。

结论与工程实践建议

本技术方案针对无人物流车在车间环境下的路径平滑与碰撞仿真需求，提出了一套基于圆弧过渡法的解析解方案。通过引入控制点与轨迹点分离的逻辑重构，以及基于 WPF PathGeometry 的分层可视化，该方案不仅解决了离散路径点处的运动学失真问题，更为高精度的碰撞模拟提供了坚实的几何基础。

在实际工程应用中，建议将几何计算逻辑封装为独立的 DLL 库，确保上位机可视化、离线仿真引擎以及 AGV 车载路径解析器使用的是同一套数学模型，实现“所见即所行”的仿真目标。

Works cited

Smoothed A* Algorithm for Practical Unmanned Surface Vehicle Path Planning - UCL Discovery, accessed December 30, 2025, https://discovery.ucl.ac.uk/10064237/3/Liu%20Smoothed%20A%20algorithm%20for%20practical%20unmanned%20surface%20vehicle%20path%20planning.pdf
AGV Path Planning Based on Smoothing A* Algorithm - ResearchGate, accessed December 30, 2025, https://www.researchgate.net/publication/282775416_AGV_Path_Planning_Based_on_Smoothing_A_Algorithm
AGV PATH PLANNING BASED ON SMOOTHING A* ALGORITHM, accessed December 30, 2025, https://airccse.org/journal/ijsea/papers/6515ijsea01.pdf
Bézier Splines: Continuity, accessed December 30, 2025, https://www.csc.kth.se/~weinkauf/notes/beziersplinecontinuity.html
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/
Turning radius - Grokipedia, accessed December 30, 2025, https://grokipedia.com/page/Turning_radius
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
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
Path Smoothing Techniques in Robot Navigation: State-of-the-Art, Current and Future Challenges - PMC - NIH, accessed December 30, 2025, https://pmc.ncbi.nlm.nih.gov/articles/PMC6165411/
Smooth path planning with safety margins using Piece-Wise Bezier curves - arXiv, accessed December 30, 2025, https://arxiv.org/pdf/2510.24972
Collision Avoidance Path Planning for Automated Vehicles Using Prediction Information and Artificial Potential Field - NIH, accessed December 30, 2025, https://pmc.ncbi.nlm.nih.gov/articles/PMC11598233/
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
Article 99 | Turning Radius - Virtual CRASH, accessed December 30, 2025, https://www.vcrashusa.com/kb-vc-article99
(PDF) An Efficient Centralized Planner for Multiple Automated Guided Vehicles at the Crossroad of Polynomial Curves - ResearchGate, accessed December 30, 2025, https://www.researchgate.net/publication/355863030_An_Efficient_Centralized_Planner_for_Multiple_Automated_Guided_Vehicles_at_the_Crossroad_of_Polynomial_Curves
Oriented bounding box (OBB): All you need to know - Mindkosh AI, accessed December 30, 2025, https://mindkosh.com/blog/oriented-bounding-box-annotation-all-you-need-to-know/
The Math Behind Bounding Box Collision Detection - AABB vs OBB(Separate Axis Theorem), accessed December 30, 2025, https://dev.to/pratyush_mohanty_6b8f2749/the-math-behind-bounding-box-collision-detection-aabb-vs-obbseparate-axis-theorem-1gdn
OrientedBoundingBox(OBB)Collision · SimonDarksideJ ... - GitHub, accessed December 30, 2025, https://github.com/SimonDarksideJ/XNAGameStudio/wiki/OrientedBoundingBox(OBB)Collision
Collision Detection - Cepton, accessed December 30, 2025, https://developer.cepton.com/blog/2d_collision_detection
Curved Paths - Red Blob Games, accessed December 30, 2025, https://www.redblobgames.com/articles/curved-paths/
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
G Continuity Method based on C-B Spline Curves and C-Bézier - NADIA, accessed December 30, 2025, http://article.nadiapub.com/IJCA/vol8_no8/4.pdf
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
An Algorithm for Polygons with Rounded Corners - Gorilla Sun, accessed December 30, 2025, https://www.gorillasun.de/blog/an-algorithm-for-polygons-with-rounded-corners/
AG Corners Live Effect | Astute Graphics Documentation, accessed December 30, 2025, https://docs.astutegraphics.com/vectorscribe/ag-corners-live-effect
javascript - Applying rounded corners to paths/polygons - Stack Overflow, accessed December 30, 2025, https://stackoverflow.com/questions/19269622/applying-rounded-corners-to-paths-polygons
How to detect collisions with a curve - Stack Overflow, accessed December 30, 2025, https://stackoverflow.com/questions/39541744/how-to-detect-collisions-with-a-curve
turning radius calculations based on average of inner and outer front wheel cramp angles, accessed December 30, 2025, https://www.mvfpd.org/files/ee1fa2b8f/turn+radii+for+developers.pdf
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
Preparing Swept Path Analysis' | Invarion Help Center, accessed December 30, 2025, https://help.invarion.com/rapidpath-online/swept-path-analysis/preparing-swept-path-analysis/
Physics Tutorial 4: Collision Detection, accessed December 30, 2025, https://research.ncl.ac.uk/game/mastersdegree/gametechnologies/previousinformation/physics4collisiondetection/2017%20Tutorial%204%20-%20Collision%20Detection.pdf
Simple Oriented Bounding Box OBB collision detection explaining - Stack Overflow, accessed December 30, 2025, https://stackoverflow.com/questions/47866571/simple-oriented-bounding-box-obb-collision-detection-explaining
Fast Safe Rectangular Corridor-based Online AGV Trajectory Optimization with Obstacle Avoidance - arXiv, accessed December 30, 2025, https://arxiv.org/html/2309.07979v2

26 KiB Raw Blame History Unescape Escape