\documentclass[journal]{IEEEtran} \usepackage{etoolbox} \usepackage{orcidlink} \usepackage{bookmark} \usepackage{amsmath} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{booktabs} \usepackage{multirow} \usepackage{graphicx} \graphicspath{{figs/}} \usepackage[ruled,vlined,linesnumbered]{algorithm2e} \hypersetup{hidelinks} \begin{document} % 基于集成引导场与联合优化的露天矿装载场地进出路径空间解耦规划 \title{Spatially Decoupled Inbound and Outbound Path Planning for Open-Pit Mine Loading Areas via Integrated Guidance Fields and Joint Optimization} \author{ Chuang Wang\orcidlink{0009-0003-5795-2504}, Peng Chen\orcidlink{0000-0002-8076-8989},~\IEEEmembership{Member,~IEEE,} Yiming Li\orcidlink{0009-0007-7859-4987}, Han Li\orcidlink{0000-0001-5136-7396}, Guizhen Yu\orcidlink{0000-0001-8374-7422} % 需更新 \thanks{This work is partially supported by the National Key Technologies R\&D Program of China 2022YFB4703700, China National Postdoctoral Program for Innovative Talents BX20250498 and National Natural Science Foundation of China 52272327.} \thanks{C. Wang, P. Chen, Y. Li, H. Li (Corresponding author), G. Yu, are affiliated with the Key Laboratory of Autonomous Transportation Technology for Special Vehicles, Ministry of Industry and Information Technology, School of Transportation Science and Engineering, Beihang University, Beijing 100191, China. (e-mail: chuang2025@buaa.edu.cn, cpeng@buaa.edu.cn, lym2000@buaa.edu.cn, leehan@buaa.edu.cn, yugz@buaa.edu.cn)} } \maketitle \begin{abstract} % TODO(P2-5)：待实验数据到位后，将下文括号内的占位数字替换为实测值。 % 结构：1 句问题 → 1 句三点新颖性（P3-1）→ 3 句方法 → 1 句验证规模 → 1 句结果。 In open-pit mines, the loading area is a semi-enclosed, single-entrance space without lane markings, where inbound and outbound haul trucks share the same narrow channel; the resulting spatio-temporal conflicts idle the excavator and cap the productivity of the entire haulage chain. This paper makes three contributions: the bidirectional traffic topology is \emph{precomputed offline} into a pair of mirrored directional fields rather than imposed as an online constraint; the inter-path conflict cost is defined as a \emph{delay-weighted double arc-length integral} of a compactly supported kernel, which depends only on the overall geometry of the two paths and therefore requires no pairing of samples between them; and the guidance weights are made \emph{asymmetric} so that a loaded truck is given priority over an empty one, matching mine operating practice. Concretely, throughput is replaced by a purely geometric surrogate---the delay-weighted spatial separation of the two paths---and the planner is organized in three stages. An integrated guidance field (IGF) is built offline by discretizing the site boundary and two directional guidance lines into potential sources; because the field is constructed from boundary geometry alone, a change of loading pose requires no rebuild. A field-adaptive Hybrid A* then produces two coarse paths independently and in parallel on the two mirrored fields, fixing the discrete gear sequence. Finally the two paths are stacked into a single decision vector and solved jointly as a nonlinear program, with the non-convex collision condition replaced by convex corridors obtained from ellipsoid-based convex decomposition. The method is evaluated against five baselines---including a time-domain cooperative scheduler as a performance upper bound---and validated on a real mine haul truck over ($N$) loading cycles. Results show ($x$)\% shorter waiting time and ($y$)\% higher throughput than independent planning, with per-cycle solution times of ($z$)~ms. \end{abstract} \begin{IEEEkeywords} Autonomous mining vehicles, path planning, open-pit mine, spatial decoupling, guidance field, joint optimization, convex corridor. \end{IEEEkeywords} \section{Introduction} \label{sec:intro} \IEEEPARstart{T}{he} % 在露天矿山部署自动驾驶技术具有显著潜力，合理的规划路径是自动驾驶车辆高效执行运输作业的重要前提\cite{mine2020lastsection,mine2023secure}。现有的研究大多都是针对城市场景，遵循结构化道路或停车场场景的通行规则。然而，露天矿区装载运输作业有其特殊的场景特点和效率要求，需要针对性的适配和设计。 % 露天矿区的运输作业由装载、运输、卸载三个环节循环构成，其中装载环节的作业效率直接决定整条运输链的产能：挖掘机是矿区昂贵且数量固定的生产设备，其空闲即意味着产能损失。执行装载作业的区域（以下简称装载场地）是一个半封闭、单出入口的不规则空间，运输车辆需在其中泊入指定的装载位、完成装载后再驶离，如图 \ref{fig:scenario} 所示。与城市结构化道路或规则停车场不同，装载场地内没有车道线等显式的通行规则，其边界由矿料堆和作业面构成、形状不规则且随开采进度变化，可通行空间狭窄。 % 在这一场景下，车队的连续作业带来了一个城市场景中并不突出的效率瓶颈。由于装载位附近空间有限，满载车的驶离路径与空载车的泊入路径往往共用同一通道，二者在时间上有先后、在空间上有重叠，形成时空冲突：空载车必须等待满载车完全驶出冲突区后才能开始泊入，这段等待直接转化为挖掘机的空闲，如图 \ref{fig:conflict}(a)(c) 所示。若将两条路径的空间拓扑加以调整、使其相互解耦，两车的动作即可并行展开，等待随之消除，如图 \ref{fig:conflict}(b)(d) 所示。 % 本文因此研究装载场地内泊入路径与驶离路径的协同规划问题：以两条路径的空间解耦为手段，压缩车辆切换过程中的等待时间，从而提升装载场地的运输吞吐量。与以往面向单车点对点行驶的路径规划不同，本文所求解的是一对具有特定空间关系的路径，二者需在满足矿用车辆运动学与狭窄空间避碰约束的前提下相互推离。 \begin{figure}[htbp] \centering \includegraphics{fig_scenario} \caption{Loading operation in an open-pit mine. The excavator works against the ore pile and a truck must park at a designated loading position before loading can start.} \label{fig:scenario} \end{figure} \begin{figure}[htbp] \centering \includegraphics{fig_conflict} \caption{Spatiotemporal conflict and its resolution by spatial decoupling. (a)(c) Coupled single-channel paths: the empty truck must wait until the loaded truck has fully cleared the conflict zone. (b)(d) Decoupled dual-channel paths: both maneuvers proceed in parallel.} \label{fig:conflict} \end{figure} \subsection{Planning Challenges} % 将上述设想落实为可求解的规划问题，面临以下三方面挑战。 \subsubsection{Coupling and Asymmetry of Inbound and Outbound Paths} % 双向路径的耦合与非对称性： % 消解时空冲突要求两条路径协同规划，其困难有三层。其一，优化目标无法直接表达：吞吐量取决于车队到达时刻与排队节拍，二者由上层派车系统与挖掘机作业决定，并不属于场地内路径规划的决策空间，因此需要一个仅由路径几何构成的代理目标。其二，两条路径通过空间接近程度相互耦合，且该耦合关于路径形状非凸，独立求解再叠加无法保证解耦效果。其三，两条路径地位并不对等：泊入需经换向动作贴合装载位、弧长通常显著大于驶离路径，二者的分段结构与采样点之间不存在几何上有意义的配对关系，使得多车规划中常用的同索引点间距惩罚在此不再适用。 \subsubsection{Topological Consistency Under Loading-Pose Updates} % 装载位动态变更下的拓扑稳定： % 装载位的位置与朝向随挖掘进度持续变更，路径须随之重新生成；但每次重规划都必须保持双向路径的空间拓扑不变——即泊入与驶离在共用通道内始终分居两侧、不发生换边，其形式化定义见 \eqref{eq:topology_consistency}——否则车队的排列关系被打乱，流水线作业的节拍反而恶化。这一要求造成两难：若将拓扑作为优化问题的约束，则每次重规划都需求解一个附加拓扑约束的非凸问题，计算代价高且可能不可行；若不加约束，所得解的拓扑将随装载位与初值而摆动。因此需要一种将拓扑先验与在线求解相分离的机制。 \subsubsection{Kinematic Feasibility in Confined Loading Areas} % 矿用车辆运动学与狭窄空间的双重限制： % 矿区运输车辆转弯半径大、液压转向响应迟滞、运动构件机械损耗高\cite{mine2024truckreversing}，要求路径沿弧长曲率连续、曲率变化率有界，并尽量减少换挡等冗余执行动作。而装载场地边界不规则、可行空间狭窄，泊入过程往往必须包含“前进—停顿—后退”的换向段。二者叠加使问题进一步复杂：大转弯半径与狭窄空间本身相互矛盾，可行域窄；避碰约束建立在不规则边界点云之上、形式非凸；换向段的存在又引入了档位序列这一离散决策，使问题整体上成为混合整数非凸规划。 \subsection{Related Work} \label{sec:related_work} % 本文的问题涉及三条相关研究脉络：多车时空冲突的消解、非结构化场景中的单车路径生成、以及以场或规则表达通行先验的方法。以下分别评述，并指出其与装载场地双向路径规划之间的差距。 \subsubsection{Spatiotemporal Conflict Resolution for Multiple Vehicles} % 多车之间的时空冲突消解通常被建模为协同轨迹规划或通行权分配问题。一类方法在时空维度上联合规划，为每辆车预先分配无冲突的轨迹或通行时隙，从而"提前消除"时空冲突，典型应用于高速匝道合流\cite{ramp2024spatiotemporal,ramp2026distributed}；在矿区场景中，相应工作多集中于交叉口的通行权与通行顺序决策\cite{mine2025rightofway}，或车队层面的任务调度\cite{mine2023scheduling}。这些方法的共同前提是各车严格执行既定的时空动作序列，其效率增益来自对到达时刻与速度剖面的精确调度。然而在装载场地，车辆到达时刻由上层派车系统决定、装载时长由挖掘机作业节拍决定，二者均不在场地内路径规划模块的可控范围；且实际生产中车辆常因人工接管、设备待料等原因偏离既定时序。因此依赖时间维精确调度的方法在此难以落地，本文转而在\emph{空间}维度消解冲突：一旦两条路径的空间拓扑充分解耦，冲突的消除便不再依赖时序的准确执行。 \subsubsection{Path Planning in Unstructured Mining Environments} % 面向非结构化场景的单车路径生成已有大量成熟工作。基于运动基元的图搜索方法以 Hybrid A*\cite{dolgov2008practical} 为代表，能够在车辆状态格上直接处理非完整约束与前进/后退混合的换向动作，后续研究进一步改进了其扩展效率与路径质量\cite{sedighi2023hybridastar}；在矿区场景中，已有工作研究了装载位变更下的最后一段路径规划\cite{mine2020lastsection}、前后端解耦的安全轨迹规划\cite{mine2023secure}，以及大型矿卡的倒车与泊车规划\cite{mine2024truckreversing}；亦有工作从人工驾驶数据出发生成类人的装载泊车轨迹\cite{mine2025humanlikeparking}。与本文在几何条件上最接近的是狭窄空间泊车规划\cite{lian2023valetparking,li2022parkingcorridor}：其同样须在逼仄可行域内处理含换向段的非完整路径，且同样先由图搜索给出粗解、再沿粗解构造行驶走廊将逐障碍避碰约束替换为走廊内的盒式约束，这一"粗解引导 + 走廊化避碰"的建模思路与本文相通；差别在于该问题只有一条路径、其目标函数中不存在另一条路径，故不涉及本文所关注的双路径相对关系。这些方法针对的是\emph{单条}路径的点对点生成：其目标函数仅由该路径自身的长度、耗时或平滑性构成，不包含与另一条路径的相对关系。将其独立地应用于泊入与驶离两条路径，各自都会收敛到几何上最优但空间上相互重合的解，反而加剧本文所关注的时空冲突。一种自然的简化是以两条路径同索引采样点之间的距离作为分离度量，但该做法要求两条路径长度相近、采样一一对应；而泊入路径含换向段、其弧长通常显著大于驶离路径，二者采样点之间不存在几何意义上的配对关系，故并不适用。 \subsubsection{Field- and Rule-Based Traffic Guidance} % 为在无车道线的场景中引入通行先验，一类工作显式制定行驶规则，如依据地形与作业特点划分渠化车道或生成固定的双向参考路线。矿区道路缺乏明确的车道标线与边界约束信息，这一点已被广泛观察到\cite{mine2024unstructured}。 % 须指出，以单向流组织车流本身是成熟做法，本文并不声称这一想法的新颖性。矿区运输道路设计中，行车方向与所需路面宽度依车型与会车需求确定\cite{baek2017haulroad}；在自动化制造与物流领域，单向流路网的设计更已形成完整的问题体系，如单向环路的布局优化\cite{kouvelis1992unidirectional}与 AGV 单向流路径网络的柔性设计\cite{seo2006flowpath}。这些工作与本文的差别在于问题层次：其对象是\emph{路网拓扑}的离线设计——在给定站点与流量矩阵下决定各段的行车方向，路网一经确定即长期沿用，车辆沿既定路段行驶而不再涉及自身的运动学可行性。本文所面对的则是非结构化的作业面：边界随开采推进而变、无既定路段可循，且所规划的是含换向段的运动学可行路径、两条路径还须联合优化。因此单向流的\emph{思想}可以借用，其\emph{方法}不可直接套用。 % 此类显式规则方法能够有效实现车流的空间解耦，但规则一经设定即难以随作业面推进而调整。另一类工作以人工势场（APF）将障碍与目标表达为连续场并沿其梯度生成路径，其泛化性强、且天然适合表达"倾向"而非"硬约束"，但以梯度驱动路径生成时存在局部极小问题——机器人可能停滞于非目标点而无法到达终点\cite{apf2024wallfollower}，且势场导数在贴近障碍处急剧增大、损害路径质量\cite{apf2024globalrepulsive}，在装载场地这类狭窄通道中尤为突出。本文借用势场的表达能力而回避其求解方式：所构建的引导场仅作为标量代价注入图搜索的评价函数，路径由离散状态空间的系统性扩展产生，因而不受局部极小影响；同时，将双向拓扑固化在离线构建的场中，使装载位变更时无需重新制定规则、亦无需附加拓扑约束。 % 综上，现有工作或依赖矿区不具备的时间维调度能力，或聚焦于单条路径的点对点生成而不涉及路径之间的空间关系，或虽能表达通行先验但受限于规则的静态性与势场求解的局部极小。本文针对装载场地的双向路径规划问题，同时处理代理目标的构造、两条非对称路径的耦合优化、以及狭窄空间下的混合整数非凸约束。 \subsection{Contributions} % 三点贡献分别回应上述三个挑战。为便于与已有工作对照，每点均先指出其新颖之处，再说明与最接近工作的具体差别。 % 需要预先说明本文的定位：三阶段框架中的若干环节沿用了成熟部件（维诺骨架提取、参考线二次规划平滑、Hybrid A*、椭球凸分解走廊、内点法求解 NLP），本文并不声称这些部件本身的新颖性。真正新的是双向路径之间\emph{关系}的建模方式——如何表达冲突、如何固化拓扑、如何引入非对称——这三点构成下述贡献。 \subsubsection{Delay-Weighted Kernel-Integral Conflict Cost} % 针对 Challenge 1，本文以两条路径的空间解耦程度作为吞吐量的代理目标，将不可直接优化的排队指标转化为纯几何量；其核心是把两条路径之间的冲突代价定义为有限支撑核的\emph{时延加权二重弧长积分}。 % 与最接近的做法——以两条路径同索引采样点之间的距离作为分离度量——相比，差别有两处，且均为本文问题所必需。其一，该形式只取决于两条路径的整体几何关系、不依赖采样点之间的配对，因而适应泊入路径含换向段、其弧长显著大于驶离路径这一非对称性；同索引点距在此没有几何意义。其二，被积函数带一个由两点弧长之差给出的时延权重，使代价集中在两车\emph{确有}时序冲突之处，而非笼统惩罚空间接近——装载位邻域的重叠因两车不同时经过而不受惩罚，故该处结构性的、无法由任何路径形状消除的重叠不会污染目标函数。该权重仅依赖配置点序号与冻结步长，故在优化中为常系数，不改变问题的稀疏结构。 % 在此基础上建立"离线场先验—在线粗解搜索—联合数值优化"的三阶段框架，将耦合的混合整数非凸问题分解为三个各自可高效求解的子问题。 \subsubsection{Bidirectional Topology Precomputed as an Offline Field Prior} % 针对 Challenge 2，本文提出集成引导场（IGF）：将场地边界与两条方向性引导线离散为势源、经有限支撑核叠加为一对关于虚拟道路中线\emph{互为镜像}的方向性势场——泊入路径所感知的势谷恰为驶离路径所感知的脊线。其新颖之处不在于用场表达通行先验，而在于双向通行的拓扑要求由此从一项\emph{在线约束}转变为一项\emph{离线先验}。 % 与两条最接近的工作线相比：显式渠化规则一类方法同样实现空间解耦，但规则一经人工设定即难以随作业面推进而调整，本文的中线与引导线则由场地边界几何自动提取；Voronoi 场一类方法\cite{dolgov2010voronoi}构造的是各向同性的排斥场、只表达"离障碍物远些"，不含方向信息，无法区分两个行驶方向。 % 该构造带来一处可直接检验的性质：场只用到边界几何、不含装载位信息，故装载位变更时无需重建场、亦无需附加拓扑约束，仅以新的边界条件重新求解路径即可；且两条路径可在各自的场上\emph{独立并行}搜索，避免了耦合搜索的状态空间膨胀。相应地，本文改进 Hybrid A* 的扩展步长、代价函数与启发函数，使其在该场引导下生成两条已初步解耦的粗解。 \subsubsection{Asymmetric Joint Optimization of the Two Paths} % 针对 Challenge 3，本文以粗解固定档位序列，将混合整数非凸问题化归为连续非线性规划；避碰约束经凸分解转化为线性不等式组——围绕粗解各段开辟不含边界点的凸多面体，在狭窄场地内尽可能保留可行域。与以对偶形式处理避碰的做法\cite{zhang2021obca}相比，凸走廊的半空间数由局部边界点分布自适应确定，代价是须以一次性内缩覆盖配置点之间的车体切角，本文给出该内缩量的解析上界。 % 此处真正新的一点是\emph{非对称}引导权重：两条路径在目标函数中取不同的场引导权重，使"满载车绕行代价高、应主要由空载车让开"这一矿区作业语义直接进入优化。该非对称性无法仅由权重比例引入——若交互项不含时延加权，则其关于两条路径完全对称，分别加权之和恒等于二者之\emph{和}乘以同一个积分，比例对极小点毫无影响；本文的时延权重恰好打破这一退化。 % 最终两条路径被堆叠为单一决策向量联立求解，而非交替迭代：共享装载位姿的等式约束因而被一次性精确满足，且问题的稀疏结构使联立求解的规模随配置点数线性增长。 % 本文余下部分组织如下。 % 第 \ref{sec:problem} 章建立问题模型：由吞吐量目标导出空间解耦这一代理目标，给出目标函数与约束的完整形式，并概述三阶段求解框架。 % 第 \ref{sec:igf} 章构建集成引导场，将双向通行的空间拓扑固化为离线的场先验。 % 第 \ref{sec:search} 章在该场上以改进的 Hybrid A* 生成两条路径的初值粗解。 % 第 \ref{sec:opt} 章以粗解为初值，经凸分解处理避碰约束、并引入交互场将两条路径联立优化。 % 第 \ref{sec:exp} 章给出实验与分析，第 \ref{sec:conclusion} 章总结全文。 \section{Problem Formulation and Planning Framework} \label{sec:problem} \subsection{Spatial Decoupling as a Throughput Surrogate} \label{subsec:proxy} % 如 \ref{sec:intro} 节所述，装载场地的生产目标是最大化单位时间内的装载趟数，即吞吐量。严格地表述该目标需要引入车队排队模型与时间维决策变量，其决策空间同时包含每辆车的到达时刻、等待时长与行驶轨迹，是一个耦合的多车时空调度问题。然而在实际矿区生产中，车队调度由上层派车系统负责、且装载位处的采装节拍由挖掘机作业决定，二者均不属于本文所讨论的场地内路径规划模块的可控范围。因此本文不直接优化吞吐量，而是采用\emph{空间解耦程度}作为其代理目标（surrogate objective），其合理性可作如下说明。记装载场地在一个采装循环内的时间开销为 \begin{equation} T_\text{cycle} = T_\text{exit} + T_\text{wait} + T_\text{park} + T_\text{load}, \label{eq:cycle_time} \end{equation} % 其中 $T_\text{load}$ 由挖掘机作业能力决定，$T_\text{exit}$ 与 $T_\text{park}$ 分别为满载车驶离与空载车泊入的行驶耗时，而 $T_\text{wait}$ 为空载车因等待满载车让出冲突区而产生的附加等待时间。 % 在挖掘机数量固定的前提下，$T_\text{load}$ 不可压缩，故提升吞吐量等价于压缩 $T_\text{exit} + T_\text{wait} + T_\text{park}$。其中 $T_\text{wait}$ 由两条路径在\emph{时空}上的冲突程度决定：二者共用同一通道时，$T_\text{wait}$ 退化为满载车通过冲突区的完整耗时；而随着两条路径在空间上相互分离，可同时通行的区段增多，$T_\text{wait}$ 随之下降。 % 需要强调的是，决定 $T_\text{wait}$ 的并非扫掠域重叠的总面积，而是重叠\emph{发生在何处}：由 \eqref{eq:wait_as_sup} 可见，$T_\text{wait}$ 取决于两车通过同一冲突点的时刻之差，故同样面积的重叠若发生在两车先后次序悬殊之处则代价高昂、若发生在两车本就不同时经过之处则不产生等待。这一区分是本文构造 \eqref{eq:cost_interaction} 中时延权重 $\tau_\text{d}$ 的依据，其详细说明见 \ref{subsec:objective} 节。 % 一个直接的推论是：$T_\text{wait}$ 原则上可被压至零。\eqref{eq:cons_boundary} 要求两条路径在装载位处共享位姿，故装载位邻域内的扫掠域重叠是结构性的、无法由任何路径形状消除；但由 \eqref{eq:cost_delay_weight}，该邻域恰是 $\tau_\text{d} = 0$ 之处——泊入路径行至末端时驶离路径刚刚起步，两车虽在空间上重叠却不在时间上相遇。因此这一不可消除的几何重叠并不构成 $T_\text{wait}$ 的下界，真正的约束来自主通道能否容纳双向并行通行：由 \eqref{eq:reference_offset}，两条引导线相距 $W$，两车车体各沿其路径向外展开 $B_\text{v}/2$，故会车所需的通道净宽至少为 $W + B_\text{v}$ 再加上两侧车体到场地边界的安全间隔，而 $W$ 自身又须不小于两车贴合时的 $B_\text{v}$ 与横向净空之和（见 \eqref{eq:interaction_radius}）。该条件在主通道最窄处不成立时，双向并行在几何上即不可能，$T_\text{wait}$ 必然为正；本文不给出该正下界的闭式表达——它取决于场地最窄区段的长度与两车的速度剖面，构造性推导须引入本文有意排除的时间维决策量。相应地，下文不再将“$T_\text{wait}$ 的下界”作为理论断言，而以实测值报告本文方法所达到的 $T_\text{wait}$ 及其相对基线的压缩幅度，见 \ref{subsec:exp_throughput} 节。 % 由此，本文将吞吐量目标分解为两个几何上可直接优化的指标：其一为两条路径的通行耗时（对应 \eqref{eq:cost_time}、\eqref{eq:cost_gear}），其二为两条路径之间\emph{按时延加权}的空间分离程度（对应 \eqref{eq:cost_field}、\eqref{eq:cost_interaction}）。下文不再直接处理 \eqref{eq:cycle_time} 中的各项 $T$，而以 \eqref{eq:cost} 所定义的等效耗时分项替代之。这一代理关系是本文全部建模的出发点，也意味着本文所优化的是吞吐量的必要条件而非其本身：空间解耦消除了冲突导致的等待，但并不保证上层派车节拍与之匹配。 % 此外，Challenge 2 所述的拓扑稳定性要求需先予以形式化，否则“拓扑不变”一语无从检验。本文在虚拟道路中线 $\varLambda_\text{center}$ 的基础上定义路径点的\emph{带符号横向偏移}：对工作空间中任一点 $\mathbf{p}$，取其在中线上的最近点投影及相应的法向投影量 \begin{equation} i^*(\mathbf{p}) = \arg\min_{1 \le i \le N_\text{c}} \left\lVert \mathbf{p} - \mathbf{q}_i \right\rVert , \quad \lambda(\mathbf{p}) = \mathbf{n}_{i^*(\mathbf{p})}^\top \left( \mathbf{p} - \mathbf{q}_{i^*(\mathbf{p})} \right) , \label{eq:signed_offset} \end{equation} % 其中 $\mathbf{q}_i$ 与 $\mathbf{n}_i$ 分别为中线采样点及其单位法向量，由 \eqref{eq:tangent_normal} 给出。由于中线只沿入口—出口主通道提取、不经过装载位（\ref{subsec:centerline} 节），装载位所在的支路区段自然地由其到中线的距离区分出来。据此记两条路径落在主通道内的部分为\emph{共享通道段} \begin{equation} \varGamma_\bullet^\text{sh} = \left\{ \mathbf{p} \in \varGamma_\bullet \;\middle|\; \left\lVert \mathbf{p} - \mathbf{q}_{i^*(\mathbf{p})} \right\rVert \le W \right\} , \quad \bullet \in \{\,\text{park},\ \text{exit}\,\} , \label{eq:shared_channel} \end{equation} % 并称一组解满足\emph{拓扑一致}，若两条路径的带符号横向偏移在各自的共享通道段内不变号、且二者符号相反： \begin{equation} \lambda(\mathbf{p}) \ge 0 \quad \forall\, \mathbf{p} \in \varGamma_\text{park}^\text{sh} , \qquad \lambda(\mathbf{p}) \le 0 \quad \forall\, \mathbf{p} \in \varGamma_\text{exit}^\text{sh} , \label{eq:topology_consistency} \end{equation} % 其符号约定与 \eqref{eq:reference_offset} 中两条引导线的偏移方向一致；取非严格不等号是因为等号可在通道端点处成立，该处两条路径分别与进入、离开位姿相接、横向偏移可为零。\eqref{eq:topology_consistency} 把“拓扑”落实为一个可判定的谓词：泊入与驶离在共享通道内始终分居中线两侧，不发生穿越或换边。 % 该要求在本文框架中不作为优化问题的约束项、而是通过求解结构本身予以保证：双向路径的空间拓扑被前置固化在第 \ref{sec:igf} 章离线构建的引导场之中，装载位变更时仅需重新求解路径、而拓扑先验保持不变（详见 \ref{subsec:framework} 节）。此处需如实指出该保证的强度：\eqref{eq:topology_consistency} 是由 \eqref{eq:cost_field} 的软代价倾向性地促成的，而非由硬约束强制的。$U_\text{park}$ 以 $\varLambda_\text{park}$ 为势谷、以 $\varLambda_\text{exit}$ 为脊线，使搜索与优化在能量上偏好正确的一侧；但当装载位移至中线另一侧、致使沿正确一侧行驶的绕行代价显著超过穿越脊线的代价时，所得解仍可能换边。本文因此不声称拓扑一致性的硬保证，而将其作为软先验下的经验性质，以跨装载位序列的\emph{拓扑一致率}——即满足 \eqref{eq:topology_consistency} 的重规划次数占比——加以量化，见 \ref{subsec:exp_topology} 节。将 \eqref{eq:topology_consistency} 升级为硬约束在本文框架下是可行的（对 \eqref{eq:joint_nlp} 而言它是一族线性不等式），但会使装载位位于中线另一侧时的问题不可行，与重规划必须始终返回可行解的工程要求相悖，故本文不予采用。 \subsection{Coordinated Path Planning Formulation} % 露天矿区自动驾驶装载场地内的路径规划任务，涉及到泊入、驶离两条路径，可以被描述为一个优化问题： \begin{subequations} \label{eq:overall_problem} \begin{align} \min \quad & J(\varGamma_\text{park}, \varGamma_\text{exit}) \label{eq:overall_object} \\ \text{s.t.} \quad & \mathbf{h}(\varGamma_\text{park}, \varGamma_\text{exit}) = \mathbf{0} \label{eq:overall_eq} \\ & \mathbf{g}(\varGamma_\text{park}, \varGamma_\text{exit}) \leq \mathbf{0} \label{eq:overall_ineq} \\ & D(\varGamma_\text{park}) \cup D(\varGamma_\text{exit}) \subseteq \Omega_\text{free} \label{eq:overall_set} \end{align} \end{subequations} % 其中，$\varGamma_\text{park}$ 表示车辆进入场地并泊车到装载位的路径（泊入路径，parking-in），$\varGamma_\text{exit}$ 表示车辆装载完成后从装载位离开的路径（驶离路径，exit），二者即本问题的决策变量；$J(\cdot)$ 是目标函数（\ref{subsec:objective} 节）； % $\mathbf{h}(\cdot) = \mathbf{0}$ 汇总等式约束，即运动学 \eqref{eq:cons_kinodynamic}、换挡衔接 \eqref{eq:cons_junction} 与边界条件 \eqref{eq:cons_boundary}； % $\mathbf{g}(\cdot) \leq \mathbf{0}$ 汇总不等式约束，即物理能力 \eqref{eq:cons_physics}与凸走廊 \eqref{eq:cons_corridor}； % 而 \eqref{eq:overall_set} 为避碰要求所对应的集合包含条件（\eqref{eq:cons_collision}）。 % 三类约束的具体形式详见 \ref{subsec:constraints} 节。 % 需要说明的是，两条路径的总弧长 $s_\text{park}$、$s_\text{exit}$ 及其分段数并非预先给定，而是与路径形状一同确定的自由量，故 \eqref{eq:overall_problem} 是一个终端时刻（此处为终端弧长）自由的最优控制问题。这一自由度在数值求解阶段被有限度地保留：\ref{subsec:joint_nlp} 节以粗解固定配置点数与分段结构，仅将配置步长 $\Delta s_\bullet$ 留作长度自由度，并以 \eqref{eq:cons_step_trust} 限制其相对粗解的变动范围。 % 本文只进行路径规划，生成路径的过程不随时间 $t$ 演化，而是随路程 $s$ 演化，车辆物理模型如图 \ref{fig:bicycle} 所示。全文按三个层次记路径与几何量：完整路径状态 $\mathbf{x}(s)=[x,y,\theta,\kappa]^\top$（$(x,y)$ 为后轴中心）、不含曲率的车辆位姿 $\boldsymbol{\eta}=[x,y,\theta]^\top$、以及工作空间中的几何点 $\mathbf{p}=[x,y]^\top$，其余符号见 Nomenclature 表。 % 车辆几何以车长 $L_\text{v}$、车宽 $B_\text{v}$（breadth）与轴距 $L_\text{w}$ 描述；记 $\mathcal{V}(\boldsymbol{\eta}) \subset \mathbb{R}^2$ 为车辆在位姿 $\boldsymbol{\eta}$ 处所占据的闭区域，即以 $\boldsymbol{\eta}$ 的位置分量为后轴中心、沿航向 $\theta$ 摆放的 $L_\text{v} \times B_\text{v}$ 实心矩形，则任一路径 $\varGamma$ 的扫掠域（swept set）定义为该轮廓沿路径的并集： \begin{equation} D(\varGamma) = \bigcup_{s \in [0,\, s_\varGamma]} \mathcal{V}\!\left( \boldsymbol{\eta}(s) \right) \label{eq:swept_set} \end{equation} % 其中 $\boldsymbol{\eta}(s)$ 为路径状态 $\mathbf{x}(s)$ 的位姿分量、$s_\varGamma$ 为该路径的总弧长；因此，两条路径的扫掠域分别写为 $D(\varGamma_\text{park})$ 与 $D(\varGamma_\text{exit})$。 % 全文主要符号汇总于 Table \ref{tab:nomenclature}。 \begin{figure}[htbp] \centering \includegraphics{fig_bicycle} \caption{Kinematic bicycle model and geometric parameters of the mining truck. The path state is $\mathbf{x}=[x,y,\theta,\kappa]^\top$ defined at the rear-axle center, with vehicle length $L_\text{v}$, width $B_\text{v}$, wheelbase $L_\text{w}$ and front-wheel steering angle $\psi$ with its rate $\dot\psi$.} \label{fig:bicycle} \end{figure} \begin{table}[htbp] \centering \caption{Nomenclature of Principal Symbols} \label{tab:nomenclature} \footnotesize \begin{tabular}{@{}llc@{}} \toprule Symbol & Description & Unit \\ \midrule \multicolumn{3}{@{}l}{\textit{States, paths and geometry}} \\ $s$ & Arc length (path parameter) & m \\ $\mathbf{x}=[x,y,\theta,\kappa]^\top$ & Full path state & --- \\ $\boldsymbol{\eta}=[x,y,\theta]^\top$ & Vehicle pose (search node) & --- \\ $\mathbf{p}=[x,y]^\top$ & Geometric point in workspace & m \\ $\kappa,\ \sigma$ & Curvature and its rate & m$^{-1}$, m$^{-2}$ \\ $\varGamma_\text{park},\varGamma_\text{exit}$ & Parking-in / exit path & --- \\ $\Omega_\text{free},\Omega_\text{obs}$ & Drivable / obstacle region & --- \\ $L_\text{v},B_\text{v},L_\text{w}$ & Vehicle length / breadth / wheelbase & m \\ $R_\text{min}$ & Minimum turning radius & m \\ $v_0,\ t_0$ & Nominal speed / gear-shift time & m\,s$^{-1}$, s \\ \midrule \multicolumn{3}{@{}l}{\textit{Guidance lines and centerline optimization}} \\ $\varLambda_\text{center}$ & Virtual road centerline & --- \\ $\varLambda_\text{park},\varLambda_\text{exit}$ & Parking-in / exit guidance line & --- \\ $\mathbf{A}_i^\text{c},\mathbf{b}_i^\text{c}$ & Tangential--normal corridor at $\mathbf{q}_i$ & --- \\ $W$ & Channel width & m \\ $\lambda(\mathbf{p})$ & Signed lateral offset w.r.t. $\varLambda_\text{center}$ & m \\ $\varGamma_\bullet^\text{sh}$ & Shared-channel portion of a path & --- \\ \midrule \multicolumn{3}{@{}l}{\textit{Integrated guidance field (IGF)}} \\ $K(d;\rho,m)$ & Compactly supported kernel & --- \\ $U_\text{park},U_\text{exit}$ & Directional IGF, $\in[0,1]$ & --- \\ $\varPsi_\text{exit}(\mathbf{p};s_\text{park})$ & Interaction field from $\varGamma_\text{exit}$ & s\,m \\ $\tau_\text{d},\ \tau_{ij}$ & Delay weight of a conflicting pair & s \\ \midrule \multicolumn{3}{@{}l}{\textit{Search and optimization}} \\ $g,\ h$ & Search cost / heuristic & s \\ $J_\ell,J_\text{gear},J_U,J_\varPsi,J_\kappa$ & Objective terms & s \\ $w_U^{\bullet},\ w_\varPsi,\ w_\kappa$ & Equivalent-time multipliers & --- \\ $N_\bullet,\ M_\bullet$ & Gear-shift / collocation-point count & --- \\ $\Delta s_\bullet$ & Collocation step & m \\ $\Delta_\text{slide}$ & Longitudinal slide margin & m \\ $\Delta_\text{cut}$ & Lateral corner-cutting inset & m \\ % $\rho_\varPsi,\ m_\varPsi,\ d_\varPsi,\ \varepsilon_\varPsi$ & Interaction radius / decay / clearance / distance regularizer & m, ---, m, m \\ $\mathbf{z}$ & Stacked decision vector & --- \\ \bottomrule \end{tabular} \end{table} \subsubsection{Objective Function} \label{subsec:objective} % \eqref{eq:overall_object} 所表示的目标函数定义为 \begin{equation} J = J_\ell + J_\text{gear} + J_U + J_\varPsi + J_\kappa \label{eq:cost} \end{equation} % 本文将全部分项统一折算为\emph{等效通行耗时}（单位：秒），从而避免各权重携带互不相同的隐含量纲、也便于跨分项比较其相对重要性。为书写简洁，对两条路径权重相同的分项记 $\varGamma = \varGamma_\text{park} \cup \varGamma_\text{exit}$；而对两条路径取值不同的分项（\eqref{eq:cost_field}、\eqref{eq:cost_interaction}）则显式列出两项。五个分项定义为 \begin{subequations} \label{eq:cost_all} \begin{align} J_\ell & = \frac{1}{v_0} \int_{\varGamma} \mathrm{d}{s} \label{eq:cost_time} \\ J_\text{gear} & = t_0 \left( N_\text{park} + N_\text{exit} \right) \label{eq:cost_gear} \\ J_U & = \frac{1}{v_0} \left[ w_U^\text{park} \int_{\varGamma_\text{park}} U_\text{park}(\mathbf{p}) \,\mathrm{d}{s} + w_U^\text{exit} \int_{\varGamma_\text{exit}} U_\text{exit}(\mathbf{p}) \,\mathrm{d}{s} \right] \label{eq:cost_field} \\ \begin{split} J_\varPsi & = \frac{w_\varPsi}{\rho_\varPsi^2} \int_{\varGamma_\text{park}} \int_{\varGamma_\text{exit}} \tau_\text{d}\!\left( s_\text{park}, s_\text{exit} \right) \\ & \quad \cdot K\!\left( d_{\varepsilon}(\mathbf{p}^\text{park},\mathbf{p}^\text{exit}); \rho_\varPsi, m_\varPsi \right) \,\mathrm{d}s_\text{exit} \,\mathrm{d}s_\text{park} \end{split} \label{eq:cost_interaction} \\ J_\kappa & = \frac{w_\kappa L_\text{v}^2}{v_0} \int_{\varGamma} \kappa^2(s) \,\mathrm{d}{s} \label{eq:cost_smoothness} \end{align} \end{subequations} % 其中 $N_\text{park}$ 与 $N_\text{exit}$ 分别为两条路径的换挡次数（与 \eqref{eq:ha_g_gear} 中的 $N$ 同义）。\eqref{eq:cost_time} 为车辆以场地内平均速度 $v_0$ 跟踪路径所需的行驶耗时，对应 \eqref{eq:cycle_time} 中的 $T_\text{park}$ 与 $T_\text{exit}$。该项的被积量为弧长微元、即本质上是路径长度代价（注：全文以 $\ell$ 表示由几何决定的长度量（路径剩余长度 $\ell_\text{rem}$、采样间距 $\ell_i$、边界顶点平均间距 $\bar\ell_\text{e}$），以 $L$ 表示车辆固定几何尺寸，以 $\Delta s$ 表示各类弧长离散步长）；除以 $v_0$ 仅是为了与 \eqref{eq:cost} 中其余各项统一折算到秒，并不改变其“惩罚路径变长”的作用。 % \eqref{eq:cost_gear} 为换挡引入的真实时间损失：每次换挡期间车辆处于停止状态、耗时 $t_0$，故该项与换挡次数 $N$ 成正比、用于惩罚冗余的前进/后退切换。该项直接回应 Challenge 3 所述的减少冗余执行动作与机械损耗的要求。 % \eqref{eq:cost_field} 为路径在融合引导场（Integrated Guidance Field, IGF）中的势能积分（IGF 的定义详见第 \ref{sec:igf} 章）。注意，两条路径查询的是\emph{各自方向对应}的 IGF：$U_\text{park}$ 与 $U_\text{exit}$ 由 \eqref{eq:field_park}、\eqref{eq:field_exit} 分别定义，二者关于虚拟道路中线互为镜像。此处及全文的 $U$ 均指经 \eqref{eq:field_normalize} 平移归一化后的场值，故 $U \in [0,1]$，其在此被解释为等效减速因子，无量纲权重 $w_U^\bullet$ 的物理含义为路径完全偏离势谷（$U \equiv 1$）时所额外付出的耗时倍数。 % \eqref{eq:cost_smoothness} 为路径平滑性代价，以车长 $L_\text{v}$ 为特征尺度使 $\kappa^2 L_\text{v}^2$ 无量纲化，其含义为路径弯曲导致的等效额外耗时；该项与 \eqref{eq:cons_kappa} 的曲率硬约束互补——后者保证可行性，前者在可行域内部进一步抑制曲率波动，以适配 Challenge 3 所述的液压转向迟滞特性。 % \eqref{eq:cost_interaction} 为双路径之间的交互惩罚，是本文实现双向空间解耦、进而压缩 \eqref{eq:cycle_time} 中 $T_\text{wait}$ 的核心项。它是一个沿两条路径的\emph{二重}弧长积分：$s_\text{park}$、$s_\text{exit}$ 分别为两条路径各自的弧长参数，$\mathbf{p}^\text{park} = \mathbf{p}^\text{park}(s_\text{park})$、$\mathbf{p}^\text{exit} = \mathbf{p}^\text{exit}(s_\text{exit})$ 为相应的路径点；核函数 $K$ 由 \eqref{eq:compact_kernel} 定义，其自变量为分属两条路径的点对间距，故两条路径上任意一对相互靠近的点都会贡献一份代价，且该贡献随间距增大而衰减、超出影响半径 $\rho_\varPsi$ 后为零。$\rho_\varPsi$、$m_\varPsi$ 分别为核的影响半径与衰减阶数，其取值依据详见 \ref{subsec:psi_build} 节。 % 与单纯的几何接近度惩罚不同，\eqref{eq:cost_interaction} 中每一对点的接近程度还额外乘以一个时延权重 $\tau_\text{d}$，其定义为 \begin{equation} \tau_\text{d}\!\left( s_\text{park}, s_\text{exit} \right) = \frac{1}{v_0} \left[ s_\text{exit} - s_\text{park} \right]_+ , \qquad [\,\cdot\,]_+ \triangleq \max\{\cdot,\,0\} \label{eq:cost_delay_weight} \end{equation} % 该权重的来由如下。设两车以相同的平均速度 $v_0$ 行驶、满载车在空载车开始泊入的同时启动驶离，则满载车通过其弧长 $s_\text{exit}$ 处的时刻为 $s_\text{exit}/v_0$，而空载车若不等待、将于 $s_\text{park}/v_0$ 时刻抵达其弧长 $s_\text{park}$ 处。若这两处恰为同一冲突点，则空载车必须推迟 $\left( s_\text{exit} - s_\text{park} \right)/v_0$ 才能避免与满载车相遇；该差为负时说明空载车本就晚于满载车通过该点、无需等待，故取正部。于是 $T_\text{wait}$ 即为全部冲突点对所要求的推迟量之上确界 \begin{equation} T_\text{wait} = \sup \left\{ \tau_\text{d}\!\left( s_\text{park}, s_\text{exit} \right) \;\middle|\; \left\lVert \mathbf{p}^\text{park} - \mathbf{p}^\text{exit} \right\rVert \le B_\text{v} \right\} \label{eq:wait_as_sup} \end{equation} % 即 $T_\text{wait}$ 由\emph{最不利}的那一对冲突点决定。\eqref{eq:cost_interaction} 是 \eqref{eq:wait_as_sup} 的一个可微松弛：以核函数 $K$ 替代硬性的冲突判定 $\lVert \cdot \rVert \le B_\text{v}$，以加权积分替代上确界，从而使该指标可参与连续优化。除以 $\rho_\varPsi^2$ 是为抵消二重弧长积分所引入的面积量纲，使 $J_\varPsi$ 的量纲为秒、$w_\varPsi$ 与 \eqref{eq:cost_field} 中的 $w_U^\bullet$ 同为无量纲权重。 % 引入 $\tau_\text{d}$ 的必要性在于，路径重叠对 $T_\text{wait}$ 的贡献强烈依赖于\emph{重叠发生在何处}，而不仅取决于重叠的几何程度。考察两个端点即可看出这一点：在装载位邻域，泊入路径已行至末端（$s_\text{park}$ 接近其总长）而驶离路径刚刚起步（$s_\text{exit} \approx 0$），故 $\tau_\text{d} = 0$——两车虽在\emph{空间}上必然重叠（\eqref{eq:cons_boundary} 要求其共享位姿），但在\emph{时间}上并不在此相遇，该处的重叠不产生任何等待；而在场地入口邻域情形恰好相反，$s_\text{park} \approx 0$ 而 $s_\text{exit}$ 接近驶离路径全长，空载车几乎须等待满载车驶完整条路径，$\tau_\text{d}$ 在此取到最大值。若如通常做法那样对全弧长的重叠等权计费，则装载位邻域这一\emph{不可消除且不产生等待}的结构性重叠会贡献大量代价，优化器将把改进余量耗费在无法改变、也无需改变的区域；加权之后，代价被自动集中到入口侧的通道，即真正决定 $T_\text{wait}$ 的位置。 % 需要说明 $\tau_\text{d}$ 对该项对称性的影响。$\tau_\text{d}$ 关于两条路径并不对称，故 \eqref{eq:cost_interaction} 不再像无权重的二重积分那样对两条路径完全等价——交换两条路径的角色会得到不同的数值。这一非对称性正是本文所需要的：它使排斥作用的强度本身就带有“满载车优先”的时序含义（详见 \ref{subsec:asymmetry} 节）。但该项对两条路径\emph{同时}可微这一性质不受影响：$\mathbf{p}^\text{park}$ 与 $\mathbf{p}^\text{exit}$ 在被积函数中仍处于对等地位，$\tau_\text{d}$ 亦仅依赖弧长参数而不依赖路径形状，因此该项的变分对两条路径的形状同时非零，这是第 \ref{sec:opt} 章将两条路径联立求解的依据（详见 \ref{subsec:joint_nlp} 节）。 \paragraph{Stagewise Objective Reduction} % \eqref{eq:cost} 中的 $J_\text{gear}$ 依赖换挡次数 $N_\text{park}$、$N_\text{exit}$，而后者与 \eqref{eq:cons_kinodynamic} 中的档位序列 $\{\delta_j\}$ 同为离散决策量，因此 \eqref{eq:overall_problem} 整体上是一个混合整数非线性规划（MINLP）问题。本文并不直接求解该 MINLP，而是按 \ref{subsec:framework} 节所述的分阶段策略处理：离散量 $(N_\text{park}, N_\text{exit}, \{\delta_j\})$ 由第 \ref{sec:search} 章的启发式搜索确定，第 \ref{sec:opt} 章的优化阶段沿用之、仅对连续的路径形状求解。在这些离散量固定的前提下，优化阶段的目标函数为 \begin{equation} J_\text{opt} = J_\ell + J_U + J_\varPsi + J_\kappa \label{eq:cost_opt} \end{equation} % 即 $J_\text{gear}$ 因 $N_\text{park}$、$N_\text{exit}$ 已固定而退化为常数、不影响极小点，故可从优化阶段的目标中略去。经此简化，第 \ref{sec:opt} 章所求解的是一个纯粹的非线性规划（NLP）问题，而 $J_\text{gear}$ 的作用完整地体现在搜索阶段对折点数量的抑制上（对应 \eqref{eq:ha_g_gear}）。 % 需要强调的是，$J_\ell$ 不可从 \eqref{eq:cost_opt} 中一并略去，尽管它在形式上同样是“路程”而非“形状”的量。该项在本文的参数化下具有明确而有限的作用，须与其在自由长度参数化中的常见含义相区别。如 \ref{subsec:joint_nlp} 节所述，本文以粗解固定配置点数 $M_\bullet$，路径长度的唯一自由度是配置步长 $\Delta s_\bullet$，故由 \eqref{eq:cost_time} 离散得到的 $J_\ell$ 是 $\Delta s_\text{park}$、$\Delta s_\text{exit}$ 两个标量的线性函数。此时 $J_\ell$ 所抑制的是\emph{步长膨胀}：$J_U$、$J_\varPsi$ 与 $J_\kappa$ 均可通过放大 $\Delta s_\bullet$ 而在几何上“摊薄”——同样的形状以更大的步长走完，弯道被展开、势能与曲率的逐点取值下降，而 $J_\ell$ 是目标中唯一随 $\Delta s_\bullet$ 单调增长的项，因而确定了弯道展开程度的平衡点。类似的长度—平滑配重在弹性带类路径优化中亦是通行做法\cite{quinlan1991elastic,quinlan1994thesis}，但需指出本文并不依赖该类方法中“无长度项则弯道无限外扩”的论证：在 $M_\bullet$ 固定且 $\Delta s_\bullet$ 受 \eqref{eq:cons_step_trust} 信赖域约束的参数化下，路径总长的可变范围本就是有界的。 \paragraph{Path Interaction Cost} % \eqref{eq:cost_interaction} 的内层积分可单独解释为驶离路径所诱导的\emph{交互场} $\varPsi_\text{exit}$：其数值在靠近 $\varGamma_\text{exit}$ 时增大、在远离时衰减，于是 $J_\varPsi$ 即为一条路径在另一条路径所诱导的场中的势能积分。与由场地边界和引导规则离线构造的\emph{静态} IGF 不同，该场随对向路径的形状在求解过程中变化，其核参数取值依据与该记号的确切含义详见 \ref{subsec:psi_build} 节。 % 这里需要说明 \eqref{eq:cost_field} 与 \eqref{eq:cost_interaction} 之间的关系，二者并非重复计费，而是分别作用于\emph{标称先验}与\emph{实际实现}两个层面：$U_\text{park}$ 中的 $+U_\text{x}$ 项惩罚的是泊入路径偏离\emph{标称}驶离引导线 $\varLambda_\text{exit}$ 的程度，该引导线由场地几何离线确定、与本次求解的实际路径无关；而 $\varPsi_\text{exit}$ 惩罚的是泊入路径与\emph{本次实际求解出}的驶离路径之间的接近程度。由于装载位位置、车辆入场位姿的差异，实际路径可能显著偏离引导线（尤其在装载位附近的“分叉”段），此时仅靠静态 IGF 无法保证两条实际路径的分离，交互项因此是必要的。换言之，IGF 提供的是与求解无关的方向先验，交互项提供的是求解过程内部的耦合反馈。 \paragraph{Asymmetric Weighting of the Two Paths} \label{subsec:asymmetry} % 最后说明本文“非对称双通道”这一设计取向在目标函数中的实现方式。该取向源自矿区实际生产逻辑：满载车辆载重大、制动距离长，且其驶离路径的绕行会直接增加运输里程与油耗，而空载车辆机动性好、绕行代价低；因此在两条路径必须相互让开时，应主要由空载的泊入路径承担绕行。 % 本文以两处机制共同实现这一取向，二者作用于不同层面。 % 其一是交互项自身所含的时序非对称。如 \eqref{eq:cost_delay_weight} 所述，$\tau_\text{d}$ 关于两条路径并不对称，故 \eqref{eq:cost_interaction} 对两条路径的作用强度天然不等。这一点值得强调，因为它区别于不加权的二重积分：若 $\tau_\text{d} \equiv 1$，则该项关于两条路径完全对称，此时\emph{无法}通过给两条路径分别加权来引入非对称——仿 \eqref{eq:cost_field} 的形式以 $w_\varPsi^\text{park}$、$w_\varPsi^\text{exit}$ 分别加权之和恒等于 $(w_\varPsi^\text{park} + w_\varPsi^\text{exit})$ 乘以同一个二重积分，两个权重只以其\emph{和}的形式进入目标，其比例对极小点毫无影响。引入 $\tau_\text{d}$ 后这一退化不再发生：代价被集中到 $s_\text{exit} > s_\text{park}$ 的点对上，即满载车尚未通过而空载车已抵达之处，排斥作用因而主要作用于泊入路径的前段。\eqref{eq:cost_interaction} 仍只保留单一权重 $w_\varPsi$，但其非对称性已由 $\tau_\text{d}$ 承载，无需依赖权重比例。 % 其二是两条路径各自\emph{抵抗偏离自身引导线}的代价。本文在 \eqref{eq:cost_field} 中取 \begin{equation} w_U^\text{exit} > w_U^\text{park} \label{eq:asymmetric_weights} \end{equation} % 即以更大的权重将满载驶离路径约束在其引导线附近。其作用机理可作如下定性理解：在极小点处，交互项施于两条路径的排斥作用相互平衡，而各自 $J_U$ 项的增量必须与之相抵；$w_U$ 较大的一方为产生同等的 $J_U$ 增量只需较小的偏离，故位移较小。需要说明这仅是一种设计取向而非可严格量化的力学关系：$J_U$ 是场值沿弧长的积分、并非以引导线为平衡位置的二次恢复势，$U$ 在引导线邻域的形状由 \eqref{eq:compact_kernel} 的核决定，两条路径所感知的场也不严格相同，因此 $w_U^\text{exit}/w_U^\text{park}$ 与位移分配之比并无解析对应关系。该比值因而按 \ref{subsec:exp_setup} 节的方式经实验标定，其对位移分配的实际影响由 \ref{subsec:exp_ablation} 节的对照实验给出。 % 两处机制的分工可概括为：$\tau_\text{d}$ 决定排斥作用\emph{施加在何处}（沿弧长的分布），\eqref{eq:asymmetric_weights} 决定两条路径\emph{各自让开多少}（横向位移的分配）。若取 $w_U^\text{park} = w_U^\text{exit}$，则在 $\tau_\text{d}$ 较大的区段两条路径将对称地各自外扩，使满载路径同样被拉长；反之若仅靠 $w_U$ 的差异而不加 $\tau_\text{d}$，则排斥作用会均匀分布于全弧长，包括装载位邻域那些并不产生等待的重叠。二者因此不可相互替代。此外，\eqref{eq:asymmetric_weights} 的效果不依赖于求解方式：无论两条路径联立求解还是交替求解，$w_U$ 的差异都会如实反映在最终的位移分配上。 \subsubsection{Constraints} \label{subsec:constraints} % \eqref{eq:overall_eq}--\eqref{eq:overall_set} 所表示的约束项主要包含以下内容。 \paragraph{Piecewise Nonholonomic Kinematics} % 装载场地内的泊入动作通常无法一次前进完成，而需要经由“前进—停顿—后退”的换向动作贴合装载位；因此本文不将路径视为单一的连续曲线，而是视为由若干\emph{同向段}首尾相接构成的分段曲线。记路径 $\varGamma$（可为 $\varGamma_\text{park}$ 或 $\varGamma_\text{exit}$）被划分为 $N+1$ 个同向段，第 $j$ 段的弧长区间为 $s \in [s_{j-1}, s_j]$、其行驶方向由档位指示 $\delta_j \in \{+1, -1\}$ 给出。由 \eqref{eq:cons_junction_gear}，相邻段档位必反号，故 $N+1$ 个同向段之间恰含 $N$ 个换向折点。计入起步挂挡这一次，换挡总次数即为 $N+1$；为记法简洁，全文以 $N$ 表示换向折点数，并在 \eqref{eq:cost_gear}、\eqref{eq:ha_g_gear} 中以其惩罚换挡动作，省略与决策无关的起步常数项。则第 $j$ 个档位段内路径状态 $\mathbf{x}(s)$ 沿弧长的演化须满足 \begin{equation} \frac{\mathrm{d}\mathbf{x}}{\mathrm{d}s} = \left[ \delta_j \cos{\theta(s)}, \; \delta_j \sin{\theta(s)}, \; \delta_j \kappa(s), \; \sigma(s) \right]^\top \label{eq:cons_kinodynamic} \end{equation} % 其中 $\delta_j = +1$ 表示前进、$\delta_j = -1$ 表示后退，$s$ 始终为非负的已行驶路程。\eqref{eq:cons_kinodynamic} 的四个分量自下而上构成一条级联积分关系：曲率变化率 $\sigma$ 沿弧长积分得到曲率 $\kappa$，$\kappa$ 积分得到航向角 $\theta$，$\theta$ 再经 $[\cos\theta, \sin\theta]^\top$ 积分得到位置 $(x, y)$。该式同时从形式上体现了车辆运行的非完整性：车辆无法沿车身侧向运动，其可行路径的切向必须始终与航向共线（前进时同向、后退时反向）。 % 在相邻两段的衔接处 $s = s_j$，车辆处于静止状态并完成一次换挡，故其衔接条件为 \begin{subequations} \label{eq:cons_junction} \begin{align} \boldsymbol{\eta}(s_j^-) & = \boldsymbol{\eta}(s_j^+) \label{eq:cons_junction_pose} \\ \delta_{j+1} & = -\delta_j \label{eq:cons_junction_gear} \end{align} \end{subequations} % 即位置与航向连续、行驶方向反号。 % \eqref{eq:cons_junction} \emph{不}要求 $\kappa$ 在衔接处连续：车辆在此已完全停止、前轮可原地转向，故 $\kappa$ 允许在换挡点跳变（即路径几何上的折点，cusp），相应地 \eqref{eq:cons_sigma} 的曲率变化率约束仅在\emph{段内}成立。 \paragraph{Curvature and Steering-Rate Limits} % 车辆的物理约束如 \eqref{eq:cons_physics} 所示，其中 $R_\text{min}$ 是车辆的最小转弯半径，$\sigma_\text{max}$ 是车辆允许的最大曲率变化率： \begin{subequations} \label{eq:cons_physics} \begin{align} & \left\lvert \kappa(s) \right\rvert \leq \kappa_\text{max} = \frac{1}{R_\text{min}}, && s \in [0, s_\varGamma] \label{eq:cons_kappa} \\ & \left\lvert \sigma(s) \right\rvert \leq \sigma_\text{max}, && s \in (s_{j-1}, s_j) \label{eq:cons_sigma} \end{align} \end{subequations} % $\sigma_\text{max}$ 反映了液压转向系统的响应能力：转向机构在单位行驶距离内能够改变的前轮转角有限，该约束因此保证了路径曲率的连续性与可跟踪性，对应 Challenge 3 所述的曲率变化率连续要求。 % 此处需说明 $\sigma_\text{max}$ 的量纲折算及其保守性，因为它关系到 \eqref{eq:cons_sigma} 在装载位邻域的松紧程度。转向系统的物理限制是前轮转角的\emph{时间}导数上界 $\dot\psi_\text{max}$，而本文的路径规划以弧长 $s$ 为自变量、不含时间，二者相差一个速度因子。由自行车模型 $\kappa = \tan\psi / L_\text{w}$ 与链式法则， \begin{equation} \sigma = \frac{\mathrm{d}\kappa}{\mathrm{d}s} = \frac{1}{L_\text{w}\cos^{2}\psi} \cdot \frac{\mathrm{d}\psi}{\mathrm{d}s} = \frac{\dot\psi}{L_\text{w} v \cos^{2}\psi} , \label{eq:sigma_time_conversion} \end{equation} % 可见同一个 $\dot\psi_\text{max}$ 在低速时对应更宽松的弧长导数上界。本文按标称速度 $v_0$ 折算，并在 $\psi$ 的允许范围内取使 \eqref{eq:sigma_time_conversion} 右端最小的一档（即 $\psi = 0$、$\cos^2\psi = 1$），得保守常值 $\sigma_\text{max} = \dot\psi_\text{max} / (L_\text{w} v_0)$。该折算在车辆实际速度低于 $v_0$ 的区段是\emph{偏保守}的：车辆开得越慢，单位弧长内可用的转角变化越多，而 \eqref{eq:cons_sigma} 并未据此放宽。这一保守性恰好落在装载位邻域——该处车辆减速、换向，同时也是可行空间最紧、最需要急转的地方。采用常值 $\sigma_\text{max}$ 的理由是本文不规划速度剖面（速度由下游的跟踪控制器决定），故无从在规划阶段得知各段的实际速度；代价是可能拒绝一部分实际可跟踪的路径，即该约束是可行性的充分而非必要条件。若后续引入分段速度剖面，可按段替换为 $\sigma_\text{max}^{(j)} \propto 1/v_j$ 以回收这部分余量。本文实车试验中实测的前轮转角速率是否触及 $\dot\psi_\text{max}$，见 \ref{subsec:exp_realvehicle} 节。 \paragraph{Boundary Conditions} % 本研究中共有三个固定位姿需要处理。进入位姿 $\boldsymbol{\eta}_\text{enter}$ 表示车辆进入装载场地的入口位姿，即泊入路径的起点；装载位姿 $\boldsymbol{\eta}_\text{load}$ 表示车辆进行采装作业的位姿，它既是泊入路径的终点、又是驶离路径的起点；离开位姿 $\boldsymbol{\eta}_\text{leave}$ 表示车辆最终离开采装场地的位姿，即驶离路径的终点。三者均由上层派车系统给出，是本问题框架下的固定值、即优化问题的硬约束： \begin{subequations} \label{eq:cons_boundary} \begin{align} \boldsymbol{\eta}^\text{park}(0) & = \boldsymbol{\eta}_\text{enter}, & \boldsymbol{\eta}^\text{park}(s_\text{park}) & = \boldsymbol{\eta}_\text{load} \label{eq:cons_bc_park} \\ \boldsymbol{\eta}^\text{exit}(0) & = \boldsymbol{\eta}_\text{load}, & \boldsymbol{\eta}^\text{exit}(s_\text{exit}) & = \boldsymbol{\eta}_\text{leave} \label{eq:cons_bc_exit} \end{align} \end{subequations} % 其中，$\boldsymbol{\eta}^\text{park}(\cdot)$ 与 $\boldsymbol{\eta}^\text{exit}(\cdot)$ 分别为泊入、驶离路径上的位姿，$s_\text{park}$ 与 $s_\text{exit}$ 为两条路径各自的总弧长。需要注意的是，\eqref{eq:cons_boundary} 仅约束位姿 $\boldsymbol{\eta} = [x, y, \theta]^\top$ 而\emph{不}约束曲率 $\kappa$：车辆在这三处均处于静止或准静止状态，其前轮转角可自由调整，两条路径各自端点处的曲率作为自由变量交由优化过程确定。\eqref{eq:cons_boundary} 中 $\boldsymbol{\eta}_\text{load}$ 在两式中同时出现，表明两条路径在装载位处共享同一\emph{位姿}，这也是双路径问题在几何上的衔接条件。但二者在该点处并不共享曲率与档位：车辆在装载位需完全停止以完成采装作业，其停留时间远大于一次换挡耗时，故装载位本身即一个天然的换向点——若车辆倒车泊入（$\delta = -1$），则其驶离动作通常以前进（$\delta = +1$）开始，\eqref{eq:cons_junction} 所述的衔接条件在该点同样成立。 \paragraph{Collision-Avoidance Constraints} % 为了保证路径在装载场地空间内安全行驶，本研究要求两条路径的扫掠域完全落在可行驶区域之内： \begin{equation} D(\varGamma_\text{park}) \cup D(\varGamma_\text{exit}) \subseteq \Omega_\text{free} \label{eq:cons_collision} \end{equation} % 其中，$\Omega_\text{free} = \Omega_\text{site} \setminus \Omega_\text{obs}$ 表示装载场地内的可行驶区域，$\Omega_\text{site}$ 为场地边界所围成的区域、$\Omega_\text{obs}$ 为障碍物（含挖掘机、矿料堆及其他静态障碍）所占据的空间，$\partial\Omega_\text{free}$ 为可行驶区域的边界。 % \eqref{eq:cons_collision} 在数值优化中难以直接处理：$\Omega_\text{free}$ 由不规则的场地边界点云描述、其形状非凸，而扫掠域 $D(\varGamma)$ 又是决策变量的泛函。尤其在矿区装载场地这类狭窄空间中，可行域本身十分逼仄，直接以障碍点距离形式施加约束会产生大量非凸约束、严重损害求解效率与收敛性。为此，第 \ref{sec:opt} 章提出一种开辟最大凸空间的处理方法：沿粗解将 $\Omega_\text{free}$ 分解为一列相互重叠的凸多面体 $\{\mathcal{P}_k\}$，并将 \eqref{eq:cons_collision} 松弛为“各路径段的车辆凸四边形轮廓顶点落在对应凸多面体内”的线性不等式约束，从而在保持安全性的同时获得可由数值方法高效求解的凸可行域。 \subsection{Three-Stage Planning Framework} \label{subsec:framework} % \eqref{eq:overall_problem} 所定义的问题难以直接求解，其困难来自三个方面：其一，\eqref{eq:cost_interaction} 使两条路径相互耦合，任一路径的代价依赖于另一条路径的求解结果；其二，\eqref{eq:cons_kinodynamic} 的换挡次数 $N$ 与各段档位 $\delta_j$ 是离散决策量，与连续的路径形状一同构成混合整数问题；其三，\eqref{eq:cons_collision} 的可行域非凸且在狭窄场地内十分逼仄。因此本文不采用单一求解器直接处理该问题，而是将其分解为“离线先验构建—在线粗解搜索—联合数值优化”三个阶段依次求解，整体框架如图 \ref{fig:framework} 所示。 % 第一阶段（第 \ref{sec:igf} 章 \ref{subsec:igf_build} 节）在离线阶段完成双向通行先验的构建。该阶段自场地边界点云出发，先经扩展维诺骨架提取与二次规划平滑得到虚拟道路中线 $\varLambda_\text{center}$，再沿其法向偏移生成进出双向引导线 $\varLambda_\text{park}$、$\varLambda_\text{exit}$，最终将场地边界与两条引导线离散为势源、叠加为两个方向性引导场 $U_\text{park}$、$U_\text{exit}$。该阶段的意义在于，双向空间解耦这一拓扑要求被\emph{前置固化}在场的构造之中：两个场关于中线互为镜像，泊入路径所感知的势谷恰为驶离路径所感知的脊线。由此，Challenge 2 所述的拓扑稳定性得以保证——中线只沿进出场地的主通道提取、不经过装载位（详见 \ref{subsec:centerline} 节），故装载位变更时引导场无需重建（场地边界未变），仅需以新的边界条件重新求解路径，而路径的空间拓扑由不变的引导场先验决定。 % 第二阶段（第 \ref{sec:search} 章）在两个方向性场上分别进行启发式搜索，生成两条路径的初值粗解。由于空间解耦要求已由第一阶段的场先验承担，该阶段的 $\varGamma_\text{park}$ 与 $\varGamma_\text{exit}$ 可在各自的场上\emph{独立且并行}地求解，无需按特定顺序串行迭代，从而避免了耦合搜索所面临的状态空间维度膨胀。相应地，该阶段求解的是 \eqref{eq:cost} 的一个简化代理——以 \eqref{eq:ha_g} 定义的广义通行耗时替代完整目标函数，其中交互项 $J_\varPsi$ 被暂时略去、而由 $U_\text{park}$、$U_\text{exit}$ 中的静态排斥项近似承担解耦作用。该阶段的输出确定了 \eqref{eq:cons_kinodynamic} 中的离散量：换挡次数 $N$ 与各段档位 $\delta_j$，即混合整数问题中的整数部分在此被固定。 % 第三阶段（第 \ref{sec:opt} 章）以粗解为初值，对两条路径进行联合数值优化。该阶段沿用第二阶段确定的分段结构，因此 \eqref{eq:cost} 退化为 \eqref{eq:cost_opt} 所示的关于连续路径形状的 NLP 问题、\eqref{eq:cost_gear} 退化为常数；同时 \eqref{eq:cons_collision} 经凸分解转化为线性不等式约束，\eqref{eq:cons_sigma} 所要求的曲率连续性亦在此阶段首次被完整施加——粗解仅满足曲率上界，其曲率剖面在基元拼接点处的跳变由本阶段消除。两条路径之间的真正协同发生在本阶段：两条路径被堆叠为单一决策变量、经 \eqref{eq:cost_interaction} 所定义的交互项联立求解，使其在满足各自运动学与避碰约束的前提下进一步相互推离，压缩其时空冲突区域。 % 综上，该框架的设计逻辑可概括为：将难以在线处理的拓扑要求转化为离线场先验，将混合整数问题中的离散决策交由图搜索确定，将非凸避碰约束经凸分解交由数值优化处理，从而以三个各自可高效求解的子问题逼近 \eqref{eq:overall_problem} 的解。这一分解的代价是不再保证全局最优性，其来源有二——状态格判重与启发函数的不可采纳性，相应的讨论分别见 \ref{subsec:primitive} 节与 \ref{subsec:heuristic} 节。 \begin{figure}[htbp] \centering \includegraphics{fig_framework} \caption{Overall framework of the proposed method. The offline stage builds the directional guidance fields $U_\text{park}$ and $U_\text{exit}$ from the site boundary; the online stage searches two coarse paths in parallel on the corresponding fields and then jointly refines them within convex corridors.} \label{fig:framework} \end{figure} \section{Construction of Integrated Guidance Fields} \label{sec:igf} % 集成引导场的构建 % 如 \ref{subsec:proxy} 节所述，进出路径的空间分离能够减少车辆因共享通道而产生的交会冲突与等待。本章据此将进出双路径的空间关系转化为集成引导场（IGF），为第 \ref{sec:search} 章的粗解搜索提供拓扑引导。 % IGF 的构建分三步：先提取虚拟道路中线，再沿其法向偏移生成进出双向引导线，最后将场地边界与两条引导线离散为势源、叠加并归一化为方向性引导场。矿山装载场地内没有城市道路那样显式的车道限制\cite{mine2024unstructured}，已有工作依据地形与作业特点制定显式行驶规则（渠化车道或固定参考路线），实现双向车流在空间层面的解耦。本文沿用"先建立双向参考基准、再据此约束路径"这一思路，首先生成装载场地内的进出双向引导路线（\ref{subsec:centerline}、\ref{subsec:guide_route} 节），再据此构建引导场（\ref{subsec:igf_build} 节）。 \subsection{Virtual Road Centerline Extraction} \label{subsec:centerline} % 构建虚拟道路中线 % 对于需要组织双向通行的场地，可先提取自由空间的中轴线，再以其为几何基准构造两个方向的参考路线。对于矿区装载场地，其边界形状不规则、不连续，无法从数学上描述边界走势，边界噪声不可微，导致“道路中线”无法被解析推导。基于搜索的方法获取“道路中线”时，又受制于代价函数的复杂设计和对场景的严苛假设，致使相关方法难以适配矿区装载场地。基于人工势场的方法虽然泛化性强，但其陷入局部极小无法跳出\cite{apf2024wallfollower}、且在贴近障碍处势场导数急剧增大而致路径质量下降\cite{apf2024globalrepulsive}，限制了其在真实生产中的应用。 % 扩展维诺图（Extended Voronoi Graph, EVG）\cite{beeson2005evg}是一种按与障碍物最近距离划分空间的空间表示，其克服了传统维诺图方法对场景中噪声敏感的问题，能够获取高质量的行驶走廊中轴骨架。在实现上，本文采用 EVG-thin 细化算法\cite{evgthin}从栅格占据图中逼近提取该扩展维诺骨架，相比解析构造维诺边的方式更为高效、且对边界噪声更鲁棒。 % 首先对栅格化后的装载场地边界，采用 EVG-thin 细化算法提取扩展维诺意义下的自由空间中轴骨架。随后在该骨架图上使用 A* 连接进入位姿 $\boldsymbol{\eta}_\text{enter}$ 与离开位姿 $\boldsymbol{\eta}_\text{leave}$ 的位置分量，得到原始虚拟道路中线$\varLambda_\text{center}^{\text{ref}}=\{\mathbf{q}_{i}^{\text{ref}}\}_{i=1}^{N_\text{c}}$，其中 $\mathbf{q}_{i}^{\text{ref}}=[x_{i}^{\text{ref}},y_{i}^{\text{ref}}]^\top$。对该原始中线进一步进行等弧长重采样，并结合一维插值与梯度平滑做轻量正则化。 % 需要强调中线的端点取法：$\varLambda_\text{center}$ 只连接进出场地的两个\emph{通道}端点，而\emph{不}经过装载位 $\boldsymbol{\eta}_\text{load}$。这一取法是 Challenge 2 所述拓扑稳定性得以成立的前提。若如通常做法那样将装载位一并作为中线的途经点，则装载位每次变更都会改变 $\varLambda_\text{center}$ 的走势，两条引导线与由其诱导的引导场随之改变，\ref{subsec:framework} 节所称“装载位变更时引导场无需重建”便不再成立；而将装载位排除在中线之外后，中线仅由场地边界几何决定，其对装载位的变更完全不敏感。相应地，装载位不再由场先验表达，而是作为 \eqref{eq:cons_boundary} 的边界条件在搜索阶段施加：路径沿引导场的势谷行至装载位附近后，经 \ref{subsec:primitive} 节的解析扩展从主通道“分叉”至指定的装载位姿。换言之，引导场承担的是\emph{主通道内双向通行的拓扑}，装载位的对齐由边界条件与解析扩展承担，二者的分工也正是 \ref{subsec:heuristic} 节中启发函数只取当前节点场值、不取累计值的原因。 % 由于骨架离散误差和图搜索分支的影响，路径中仍可能保留局部离群点和锯齿段。为在保持全局拓扑结构不变的前提下抑制这些局部伪影，本文复用 Apollo 提出的 FEM-pos 参考线平滑方法\cite{zhou2020apolloqp,fan2018apolloem}对中线做进一步正则化优化。 % 对原始中线上的每个采样点，根据参考航向分别计算其局部单位切向量和单位法向量： \begin{equation} \mathbf{t}_i = \begin{bmatrix} \cos\theta_i^\text{ref} \\ \sin\theta_i^\text{ref} \end{bmatrix}, \qquad \mathbf{n}_i = \begin{bmatrix} -t_{i,y} \\ t_{i,x} \end{bmatrix}, \label{eq:tangent_normal} \end{equation} % 其中 $\mathbf{t}_i=[t_{i,x},t_{i,y}]^\top$，$\theta_i^\text{ref}$ 由插值及梯度平滑后的参考中线计算得到，首尾若干点采用入口与离开方向先验。记$\bar{\mathbf{q}}=[\mathbf{q}_{1}^\top,\mathbf{q}_{2}^\top,\dots,\mathbf{q}_{N_\text{c}}^\top]^\top$为优化后中线 $\varLambda_\text{center}=\{\mathbf{q}_{i}\}_{i=1}^{N_\text{c}}$ 的堆叠优化变量。该平滑优化沿用 FEM-pos 参考线平滑器\cite{zhou2020apolloqp}的建模思路，其目标由 FEM-pos 平滑项、路径长度项与参考偏移项构成；区别在于，本文在 \eqref{eq:local_corridor_tangent}--\eqref{eq:local_corridor_normal} 中引入由场地边界点云自动生成的切向-法向走廊，并施加曲率上界约束以适配矿区大转弯半径车辆。 % 加入曲率约束后的平滑问题可写为 \begin{subequations} \label{eq:qp_problem} \begin{align} \min_{\bar{\mathbf{q}},\,\boldsymbol{\varepsilon}} \quad & J_\text{center} = J_\text{fem} + J_\text{len} + J_\text{ref} + J_\text{slack} \label{eq:qp_obj_mixed} \\ \text{s.t.} \quad & \mathbf{A}_i^\text{c} \mathbf{q}_i \le \mathbf{b}_i^\text{c},\quad i=1,\dots,N_\text{c} \label{eq:qp_constraints} \\ & \|\mathbf{d}_i\|^2 \le (\ell_i^2\kappa_\text{max})^2+\varepsilon_i,\quad i=2,\dots,N_\text{c}-1 \label{eq:qp_curvature} \\ & \varepsilon_i \ge 0,\quad i=2,\dots,N_\text{c}-1 \label{eq:qp_slack} \\ \text{where}\quad & \mathbf{d}_i = \mathbf{q}_{i-1}-2\mathbf{q}_i+\mathbf{q}_{i+1}. \notag \end{align} \end{subequations} % 其中，各目标项定义为 \begin{subequations} \label{eq:qp_obj_all} \begin{align} J_\text{fem} & = w_\text{fem} \cdot \sum_{i=2}^{N_\text{c}-1} \left\lVert \mathbf{q}_{i-1} - 2\mathbf{q}_i + \mathbf{q}_{i+1} \right\rVert^2 \label{eq:qp_obj1} \\ J_\text{len} & = w_\text{len} \cdot \sum_{i=1}^{N_\text{c}-1} \left\lVert \mathbf{q}_{i+1} - \mathbf{q}_i \right\rVert^2 \label{eq:qp_obj2} \\ J_\text{ref} & = w_\text{ref} \cdot \sum_{i=1}^{N_\text{c}} \left\lVert \mathbf{q}_i - \mathbf{q}_i^\text{ref} \right\rVert^2 \label{eq:qp_obj3} \\ J_\text{slack} & = w_{\varepsilon} \cdot \sum_{i=2}^{N_\text{c}-1} \varepsilon_i \label{eq:qp_obj4} \end{align} \end{subequations} % 在 \eqref{eq:qp_obj_all} 中，前三项沿用 FEM-pos 平滑器\cite{zhou2020apolloqp}的标准定义：$J_\text{fem}$ 为有限元位置偏差平滑项，通过二阶差分抑制高频几何振荡、提升中线平滑性；$J_\text{len}$ 通过一阶差分惩罚相邻采样点间距，等价约束路径总长；$J_\text{ref}$ 惩罚优化中线相对原始 EVG-A* 中线的偏移。$J_\text{slack}$ 则为本文为引入曲率约束而增设的松弛惩罚项，$\boldsymbol{\varepsilon}=[\varepsilon_2,\dots,\varepsilon_{N_\text{c}-1}]^\top$ 为非负松弛变量。式 \eqref{eq:qp_curvature} 是对曲率上界的离散化表达。对间距为 $\ell$ 的等弧长折线，其二阶差分 $\mathbf{d}_i=\mathbf{q}_{i-1}-2\mathbf{q}_i+\mathbf{q}_{i+1}$为离散的二阶导数，与该点曲率满足$\left\lVert \mathbf{d}_i \right\rVert = \ell^2 \kappa_i + O(\ell^4)$，因此曲率约束 $\kappa_i \le \kappa_\text{max}$ 可改写为 \eqref{eq:qp_curvature} 中对$\left\lVert \mathbf{d}_i \right\rVert^2$ 的上界约束，其中 $\kappa_\text{max}=1/R_\text{min}$ 为车辆最小转弯半径对应的曲率上界。该换算关系的推导、误差量级以及采用平方形式的原因见补充材料 S-V；以 $R_\text{min} = 12$~m、$\ell = 1$~m 计，其相对误差约 $0.06\%$，且该近似在安全性上偏保守。由于重采样后各段长度并非严格相等，此处的换算尺度取第 $i$ 个点左右两段的平均长度 $\ell_i = \tfrac{1}{2} ( \lVert \mathbf{q}_i^\text{ref} - \mathbf{q}_{i-1}^\text{ref} \rVert + \lVert \mathbf{q}_{i+1}^\text{ref} - \mathbf{q}_i^\text{ref} \rVert )$。 % \eqref{eq:qp_problem} 是一个\emph{凸} QCQP：$\mathbf{d}_i$ 关于 $\mathbf{q}$ 线性，故 $\lVert \mathbf{d}_i \rVert^2$ 与 \eqref{eq:qp_obj_all} 各项同为凸二次、\eqref{eq:qp_constraints} 为线性，其局部最优即全局最优。序列化求解并非因非凸性，而是出于求解器接口：OSQP 仅接受线性约束，故将 \eqref{eq:qp_curvature} 在当前迭代点线性化后转为一列 QP 依次求解，并逐步增大松弛权重 $w_{\varepsilon}$ 以渐次收紧曲率约束。等弧长重采样则是上述二阶差分换算成立的前提。 % 为限制平滑后的中线偏离自由空间骨架，本文在每个参考点附近建立局部切向--法向矩形走廊。走廊边界由车载激光雷达获取并按左右两侧划分的场地边界点云自动确定，并施加安全收缩。记 $s_i^-$、$s_i^+$ 为切向后向与前向边界，$l_i^-$、$l_i^+$ 为法向右向与左向边界，则优化点 $\mathbf{q}_i$ 满足 \begin{subequations} \begin{align} -s_i^- &\le \mathbf{t}_i^\top(\mathbf{q}_i-\mathbf{q}_i^\text{ref}) \le s_i^+ \label{eq:local_corridor_tangent} \\ -l_i^- &\le \mathbf{n}_i^\top(\mathbf{q}_i-\mathbf{q}_i^\text{ref}) \le l_i^+ . \label{eq:local_corridor_normal} \end{align} \end{subequations} % 该局部约束等价于紧凑的全局半空间形式 $\mathbf{A}_i^\text{c}\mathbf{q}_i\le\mathbf{b}_i^\text{c}$，并代入 \eqref{eq:qp_constraints}。其中 $\mathbf{A}_i^\text{c}$、$\mathbf{b}_i^\text{c}$ 由上述四个边界和参考点坐标直接确定。该走廊仅用于中线及引导线的几何平滑，不保证最终车辆扫掠域无碰；后者由第 \ref{sec:opt} 章的凸分解走廊和车辆角点约束保证。本小节的完整构造过程如图 \ref{fig:center} 所示：图 \ref{fig:center}(a) 给出自由空间的距离场、由其脊线提取的 EVG 骨架以及骨架图上的 A* 粗解，图 \ref{fig:center}(b) 给出局部切向--法向走廊与 QP 平滑所得的 $\varLambda_\text{center}$，图 \ref{fig:center}(c) 则对比平滑前后的离散曲率剖面，可见粗解的折角处曲率远超 $\kappa_\text{max}$，而平滑后全程满足 \eqref{eq:qp_curvature} 的上界。 \begin{figure*}[htbp] \centering \includegraphics{fig_center} \caption{Construction of the virtual road centerline on a synthetic loading site. (a) Clearance field of the free space, the EVG-thin skeleton extracted from its ridge, and the coarse A$^\ast$ path from the entrance to the loading bay. (b) Local tangent--normal corridors of \eqref{eq:local_corridor_tangent} and \eqref{eq:local_corridor_normal} around each reference point, and the centerline $\varLambda_\text{center}$ returned by the FEM-pos QP smoother \eqref{eq:qp_problem}. (c) Discrete curvature profiles before and after smoothing, obtained from the second difference of \eqref{eq:qp_curvature}, against the bound $\kappa_\text{max}=1/R_\text{min}$. This figure illustrates the geometric construction and is not an experimental result.} \label{fig:center} \end{figure*} \subsection{Inbound and Outbound Guidance-Line Generation} \label{subsec:guide_route} % 生成双向引导路线 % 基于优化后的虚拟道路中线 $\varLambda_{\text{center}}$，沿中线上各点的局部法向正、负方向分别偏移 $\frac{W}{2}$，生成进场参考路线 $\varLambda_{\text{park}}$ 与离场参考路线 $\varLambda_{\text{exit}}$。其中，$W$ 表示虚拟道路宽度，局部法向量由式 \eqref{eq:tangent_normal} 确定。然而，在曲率较大的区域，直接采用等距法向偏移所生成的参考路线容易出现局部卷曲与折角现象，如图~a 所示。为提高参考路线的几何连续性与平滑性，本文依次采用路径采样、插值重构和平滑优化方法对原始偏移曲线进行处理，平滑优化所采用的二次规划模型与前文保持一致。这两条引导线并不是车辆最终可执行路径，而是作为后续代价场构建和初始路径搜索的方向先验。由 \eqref{eq:reference_offset} 可见，两条引导线的弧长范围完全由 $\varLambda_\text{center}$ 继承，因此与中线一样只覆盖入口—出口主通道；在装载位所在的支路区域，方向性势源 $\mathcal{S}_\text{p}$、$\mathcal{S}_\text{x}$ 的支撑集之外，引导场退化为仅由场地边界 $\mathcal{S}_\text{e}$ 诱导的排斥场。这一退化是有意为之：该区域两条路径按 \eqref{eq:cons_boundary} 必须收敛到同一装载位姿，本不存在需要由场先验表达的双向拓扑，此处只需保证避障即可。因此影响半径 $\rho_\text{p}$、$\rho_\text{x}$ 的取值只需覆盖主通道宽度，无需延伸至支路。 \begin{equation} \tilde{\mathbf{q}}_i^{\text{park}} = \mathbf{q}_i + \frac{W}{2} \mathbf{n}_i,\qquad \tilde{\mathbf{q}}_i^{\text{exit}} = \mathbf{q}_i - \frac{W}{2} \mathbf{n}_i. \label{eq:reference_offset} \end{equation} \subsection{Guidance Field Construction and Normalization} \label{subsec:igf_build} % 基于有限支撑核函数和离散势源集合的离线势场构建 % 将装载场地边界以及两条方向性引导线离散化，得到三个势源点集合 $\mathcal{S}_\text{e}$、$\mathcal{S}_\text{p}$、$\mathcal{S}_\text{x}$，下标分别对应场地边界、泊入引导线和驶离引导线。为避免在线规划阶段逐点累积全部势源的影响，本文在离线阶段预先在均匀栅格上构建融合引导场：取覆盖装载场地的轴对齐包围盒，以分辨率 $\delta$ 在其上铺设均匀栅格，记全部栅格中心构成的有限点集为 $\Xi = \{\, \mathbf{p}_0 + \delta [\,j,\ k\,]^\top \mid 0 \le j < N_x,\ 0 \le k < N_y \,\}$，其中 $\mathbf{p}_0$ 为包围盒左下角的栅格中心，$N_x$、$N_y$ 为两个方向的栅格数。沿用第 \ref{sec:problem} 章的约定，栅格中心作为势场查询点是工作空间中的几何点，为区别于势源点、在下文中专记为 $\boldsymbol{\zeta}$。对于任一栅格中心 $\boldsymbol{\zeta}\in\Xi$，势源集合 $\mathcal{S}$ 对该点的影响通过一个有限支撑核函数计算： \begin{equation} K(d;\rho,m)= \begin{cases} \left(1-\dfrac{d}{\rho}\right)^m, & 0\le d<\rho \\ 0, & d\ge \rho \end{cases} \label{eq:compact_kernel} \end{equation} % 其中，$\rho$ 表示影响半径，$m$ 用于控制核函数的衰减速率。由势源集合 $\mathcal{S}$ 诱导得到的标量场可进一步定义为 \begin{equation} U_{\mathcal{S}}(\boldsymbol{\zeta}) = \alpha_{\mathcal{S}} \sum_{\boldsymbol{\xi}\in\mathcal{S}} K\!\left( \left\lVert \boldsymbol{\zeta}-\boldsymbol{\xi} \right\rVert; \rho_{\mathcal{S}},m_{\mathcal{S}} \right)\Delta s_{\boldsymbol{\xi}}, \label{eq:source_field} \end{equation} % 其中，$\alpha_{\mathcal{S}}$ 为场强系数，$\Delta s_{\boldsymbol{\xi}}$ 为势源点 $\boldsymbol{\xi}$ 所代表的线元长度。三个势源集合均沿折线分布：$\mathcal{S}_\text{p}$ 与 $\mathcal{S}_\text{x}$ 为两条方向性引导线的等弧长采样点，$\mathcal{S}_\text{e}$ 为描述装载场地边界的不规则闭合多边形的顶点序列。 % 对场地边界，由于其顶点由感知模块直接给出、相邻顶点间距不均匀且不具备明确的弧长语义，本文取 $\Delta s_{\boldsymbol{\xi}} \equiv 1$，即以顶点计数的形式累积边界排斥作用。该处理使 $U_\text{e}$ 与 $U_\text{p}$、$U_\text{x}$ 之间相差一个采样密度量纲，该差异可由场强系数 $\alpha_{\mathcal{S}}$ 进行调节，其标定条件满足 \eqref{eq:alpha_dominance}。 % 由 \eqref{eq:compact_kernel} 的有限支撑性质可给出各分场的取值范围：下界平凡地为 $0$，上界则由势源在支撑域内的最大累积量确定，其推导见补充材料 S-I。其中下标 $\text g \in \{\text p, \text x\}$ 表示两条方向性引导线中的任一条，二者因关于中线几何对称而取相同的 $\alpha$、$\rho$ 与 $m$，故共享同一上界。 \begin{subequations} \label{eq:field_bounds} \begin{align} 0 \le U_\text{g} & \le \alpha_\text{g} \int_{-\rho_\text{g}}^{\rho_\text{g}} \left( 1 - \frac{|t|}{\rho_\text{g}} \right)^{m_\text{g}} \mathrm{d}t = \frac{2\alpha_\text{g} \rho_\text{g}}{m_\text{g} + 1} \triangleq U_\text{g}^\text{max} \label{eq:bound_guide} \\ 0 \le U_\text{e} & \le \alpha_\text{e} \left( \frac{2\rho_\text{e}}{\bar\ell_\text{e}} + 1 \right) \triangleq U_\text{e}^\text{max} \label{eq:bound_edge} \end{align} \end{subequations} % 其中 $\bar\ell_\text{e}$ 表示边界多边形相邻顶点的平均间距。两个上界的一处结构性差异需在此指出：$U_\text{g}^\text{max}$ 与采样密度无关，而 $U_\text{e}^\text{max}$ 反比地依赖于 $\bar\ell_\text{e}$，故更换感知配置或边界提取分辨率时须对 $\alpha_\text{e}$ 重新标定。此外，\eqref{eq:bound_edge} 在凹形拐角处是典型量级的估计而非全局上界，其代价与所采取的截断处理见下文 \eqref{eq:field_normalize} 处的说明，严格上界的构造见补充材料 S-I。综合 \eqref{eq:field_bounds} 与 \eqref{eq:field_park}、\eqref{eq:field_exit} 中各项的符号，可得组合场的先验取值区间 \begin{equation} U_\text{park},\, U_\text{exit} \in \left[ -U_\text{g}^\text{max},\; U_\text{e}^\text{max} + U_\text{g}^\text{max} \right] \label{eq:field_range} \end{equation} % 其中 $U_\text{e}^\text{max}$ 与 $U_\text{g}^\text{max}$ 为 \eqref{eq:field_bounds} 所定义的两个上界。该区间为后文 \eqref{eq:field_normalize} 的归一化提供了与具体场地无关的先验范围。 % 将 \eqref{eq:source_field} 分别作用于上述三个势源集合后，可得到边界场 $U_\text{e}$、泊入通道场 $U_\text{p}$ 和驶离通道场 $U_\text{x}$。其中，$U_\text{e}$ 在靠近装载区边界时形成排斥脊线，而 $U_\text{p}$ 与 $U_\text{x}$ 则分别编码双向通道的空间先验。 % 对于泊入动作，本文将 $U_\text{p}$ 视为吸引势谷、将 $U_\text{x}$ 视为排斥脊线；对于驶离动作，则相反地将 $U_\text{x}$ 视为吸引势谷、将 $U_\text{p}$ 视为排斥脊线。由此可分别定义面向两种行驶方向的 IGF： \begin{subequations} \begin{equation} U_\text{park}(\boldsymbol{\zeta}) = U_\text{e}(\boldsymbol{\zeta}) - U_\text{p}(\boldsymbol{\zeta}) + U_\text{x}(\boldsymbol{\zeta}), \label{eq:field_park} \end{equation} \begin{equation} U_\text{exit}(\boldsymbol{\zeta}) = U_\text{e}(\boldsymbol{\zeta}) + U_\text{p}(\boldsymbol{\zeta}) - U_\text{x}(\boldsymbol{\zeta}). \label{eq:field_exit} \end{equation} \end{subequations} % 由于 IGF 在离线阶段完成构建，其计算开销不影响在线搜索过程；在线阶段只需对预计算的栅格场作连续空间查询。本文以张量积三次 B 样条作为场的连续化表示，其系数由栅格场值经预滤波（prefiltering）在离线阶段一并解出，在线查询时场值及其一阶、二阶导数均由基函数解析给出，为常数时间。相应地，\eqref{eq:compact_kernel} 中各核函数的衰减阶数须取 $m_\text{e}, m_\text{g} \ge 3$，使连续意义下的场本身即为 $C^2$，以满足 \ref{subsec:joint_nlp} 节对 \eqref{eq:cost_field} 的二阶连续可微性要求；该取值与交互核所需的 $m_\varPsi \ge 3$ 同源。选取样条而非双线性插值的理由、以及搜索与优化两阶段统一采用同一表示的理由，见补充材料 S-II。 % 需要强调的是，本文对势场的使用方式与传统人工势场法（Artificial Potential Field, APF）存在本质区别。如 \ref{sec:related_work} 节所述，APF 类方法在狭窄通道处易陷入局部极小值、且存在目标不可达问题，其根源在于这类方法将势场梯度 $-\nabla U$ 直接用作驱动力以迭代推进路径点：一旦合力为零而该点并非目标点，迭代即停滞且无法自行跳出。与之不同，本文并不对 IGF 求梯度、也不以梯度驱动路径生成，而是仅将 $U(\mathbf{p})$ 作为标量代价注入下文 \eqref{eq:ha_g}--\eqref{eq:ha_h_all} 的图搜索评价函数中。路径的生成机制仍然是对离散状态空间的系统性扩展：势谷区域仅表现为更低的节点代价、从而被优先扩展，而势能较高的区域并不会被排除在搜索之外。因此，当势谷方向受阻时，搜索会自然回退并扩展代价次优的节点，算法的可达性与分辨率完备性由图搜索本身保证，而不依赖于 $U$ 的形状。换言之，IGF 在本文中承担的是“方向先验”而非“驱动力”的角色，这使得矿区双向通行规则能够以连续、可微的形式嵌入规划器，同时规避了 APF 固有的局部极小值缺陷。 % 本文取 $\alpha_\text{p} = \alpha_\text{x}$、$\rho_\text{p} = \rho_\text{x}$、$m_\text{p} = m_\text{x}$，即 \eqref{eq:bound_guide} 中统一记为 $\alpha_\text{g}$、$\rho_\text{g}$、$m_\text{g}$ 的三个参数；该组等式源于泊入与驶离两条引导线在几何上关于中线对称，令其取相同参数可使两个方向性场互为镜像，从而避免在双向路径之间引入人为的偏置。对于边界项，则要求其在最坏情况下仍能支配双向引导项，使车辆在任何情况下都不会为贴合引导线而逼近场地边界。需要注意的是，如前所述 $\alpha_\text{e}$ 与 $\alpha_\text{g}$ 因 $\Delta s_{\boldsymbol{\xi}}$ 取法不同而相差一个长度量纲，二者不可直接比较；该支配关系应通过 \eqref{eq:field_bounds} 给出的实际场值上界来锚定，即 \begin{equation} U_\text{e}^\text{max} > U_\text{g}^\text{max}, \quad\text{i.e.}\quad \alpha_\text{e}\left( \frac{2\rho_\text{e}}{\bar\ell_\text{e}} + 1 \right) > \frac{2\alpha_\text{g} \rho_\text{g}}{m_\text{g} + 1} \label{eq:alpha_dominance} \end{equation} % \eqref{eq:alpha_dominance} 为场强系数的标定提供了明确依据。需要强调的是，该式比较的是两个分场各自的上界，而非逐点的大小关系：在远离边界的引导线附近有 $U_\text{e} = 0$，此时引导项占优，这正是方向先验得以发挥作用之处。该式所保证的是最坏情形下的余量——在边界项取到其上界 $U_\text{e}^\text{max}$ 的构型下，即使引导项同时取到 $U_\text{g}^\text{max}$，由 \eqref{eq:field_park}、\eqref{eq:field_exit} 及 $U_\text{p}, U_\text{x} \ge 0$ 仍有 $U \ge U_\text{e}^\text{max} - U_\text{g}^\text{max} > 0$，即边界排斥脊线处的代价始终高于远离全部势源的中性自由空间（$U = 0$），引导吸引作用不会将该排斥脊压平为势谷。 % 至于叠加方式本身，本文采用线性组合而非取极值或乘性组合，理由是线性组合保持 \eqref{eq:compact_kernel} 在支撑域内的 $C^{m-1}$ 光滑性，而该光滑性是 \eqref{eq:cost_field} 参与连续优化的前提；两种替代方式各自破坏光滑性的方式见补充材料 S-I。 % 由于 \eqref{eq:field_park} 与 \eqref{eq:field_exit} 含负号项，$U$ 的取值可能为负；为使其可直接作为非负代价参与 A* 搜索（详见第 \ref{sec:search} 章），在栅格场构建完成后统一做一次平移与归一化，将场值映射到 $[0,1]$ 区间： \begin{equation} \hat U(\boldsymbol{\zeta}) = \frac{U(\boldsymbol{\zeta}) + U_\text{g}^\text{max}} {U_\text{e}^\text{max} + 2 U_\text{g}^\text{max}} \label{eq:field_normalize} \end{equation} % 其中平移量与区间长度均由 \eqref{eq:field_bounds} 的两个上界解析给出，而非取自栅格实测极值 $\min_\Xi U$、$\max_\Xi U$：实测极值随场地形状与势源密度而变，且 $U_\text{park}$ 与 $U_\text{exit}$ 的实测极值一般不等（两条引导线相对场地边界的位置并不对称），以其归一化将使 \eqref{eq:field_park}、\eqref{eq:field_exit} 的镜像关系在数值上被破坏。共同的先验端点则使两个场共享同一与场地无关的标度，\eqref{eq:ha_g_field} 中的权重 $w_U^{\bullet}$ 因而无需随场地更换而重新标定。该变换不改变场的空间形态与相对高低关系；为表述简洁，下文中的 $U$ 均指经 \eqref{eq:field_normalize} 归一化后的场值。 % 这里须补充说明 \eqref{eq:field_normalize} 的值域。下界是严格的：由 \eqref{eq:bound_guide}，$U_\text{g}^\text{max}$ 是引导线分场的严格上界，故 $U \ge -U_\text{g}^\text{max}$ 处处成立、$\hat U \ge 0$ 无条件满足，A* 搜索所要求的代价非负性因而得到保证。上界则不然：如 \eqref{eq:bound_edge} 处所述，$U_\text{e}^\text{max}$ 在凹形拐角处只是典型量级的估计而非全局上界，故 $\hat U$ 在少数凹角栅格上可能略微超过 $1$。本文在离线构建栅格场时统一施加截断 $\hat U \leftarrow \min\{\hat U,\, 1\}$，使 $\hat U \in [0,1]$ 成为严格值域，从而 \eqref{eq:ha_g_field} 中把 $\hat U$ 解释为等效减速因子、以及 \ref{subsec:primitive} 节据 $\hat U$ 调制扩展步长时所依赖的标度均严格成立。该截断只作用于紧贴边界的凹角处——那里 $\hat U$ 已接近 $1$、代价已足够高以排斥路径，故截断不改变搜索与优化的行为；但需注意截断使场在该处的梯度为零，因此 \eqref{eq:cost_field} 在被截断的栅格上不再提供推离边界的梯度，路径与边界的安全间隔在优化阶段完全由 \ref{subsec:convex} 节的凸走廊硬约束保证，不依赖 $J_U$。 % 最后需要指出 \eqref{eq:field_park} 与 \eqref{eq:field_exit} 这一构造方式在方法框架层面的意义。双向路径的空间解耦要求，已经通过两个方向性场中 $U_\text{p}$ 与 $U_\text{x}$ 的符号互换被前置固化在场的构造之中：泊入路径所感知的势场以泊入引导线为势谷、以驶离引导线为脊线，驶离路径则恰好相反，如图 \ref{fig:igf} 所示。因此在后续的初始搜索阶段，$\varGamma_\text{park}$ 与 $\varGamma_\text{exit}$ 各自在对应的$U_\text{park}$、$U_\text{exit}$ 上独立完成搜索，二者之间不存在任何信息交互，也不需要按特定顺序串行求解——两条路径的搜索过程是相互独立且可并行的。这一设计的意义在于，它将“双向解耦”这一空间拓扑约束从在线求解过程中剥离出来，转化为离线场构造阶段的先验知识，从而避免了耦合搜索所面临的状态空间维度膨胀问题。双路径之间真正的协同发生在第 \ref{sec:opt} 章的数值优化阶段：该阶段以本章生成的两条粗解为初值，通过 \eqref{eq:cost_interaction} 所定义的交互项对两条路径进行联合优化，以进一步压缩其时空冲突区域。 % 最后须明确本文所称“离线”的实际含义，因为装载场地的边界并非静止：随开采推进，台阶坡面持续后退，而挖掘机本身在作业间隙亦会整机移位。本文以\emph{时间尺度分离}处理这一矛盾。IGF 的输入只有场地边界点集 $\mathcal{S}_\text{e}$；由 \ref{subsec:centerline} 节，中线与两条引导线均不经过装载位，故场对装载位变更完全不敏感。于是场的重建仅由边界几何的实质变化触发，其典型时间尺度为班次级（一次爆破或一段推进之后），远长于装载循环的分钟级节拍；在两次重建之间，装载位每次变更只需以新的边界条件重新求解两条路径，引导场保持不变。这正是 \eqref{eq:field_park}、\eqref{eq:field_exit} 的镜像构造所带来的工程收益：拓扑先验的有效期由边界而非装载位决定。 % 至于挖掘机与其他车辆这类移动障碍，本文不将其纳入 $\mathcal{S}_\text{e}$，理由是场的重建代价与其运动时间尺度不匹配；它们由 \ref{subsec:convex} 节的凸走廊在每次重规划时以当次感知快照施加硬约束避让。这一分工也界定了本文方法的适用边界：IGF 承担的是随边界缓变的\emph{通行拓扑}，而非随车流快变的\emph{瞬时占据}。场的重建耗时与重规划频次的实测值见 \ref{subsec:exp_setup} 节。 \begin{figure*}[htbp] \centering \includegraphics{fig_igf} \caption{Integrated guidance fields on a synthetic loading site. (a) Site boundary, the virtual road centerline $\varLambda_\text{center}$, and the two directional guidance lines $\varLambda_\text{park}$, $\varLambda_\text{exit}$ offset from it by $W/2$, which together supply the source sets $\mathcal{S}_\text{e}$, $\mathcal{S}_\text{p}$, $\mathcal{S}_\text{x}$. (b) Normalized inbound field $\hat U_\text{park}$ of \eqref{eq:field_park}. (c) Normalized outbound field $\hat U_\text{exit}$ of \eqref{eq:field_exit}. Both panels share one color scale; each field forms a valley along its own guidance line and a ridge along the mirrored one, so that the two searches are pushed apart while both remain repelled by the boundary. This figure illustrates the field construction and is not an experimental result.} \label{fig:igf} \end{figure*} \section{Integrated-Field-Guided Inbound and Outbound Path Search} \label{sec:search} % 进出双路径的启发搜索 % 本章在第 \ref{sec:igf} 章构建的 IGF 基础上，以改进的 Hybrid A* 生成进出双路径粗解。泊入路径 $\varGamma_\text{park}$ 与驶离路径 $\varGamma_\text{exit}$ 分别在 $U_\text{park}$ 与 $U_\text{exit}$ 上独立搜索，两次搜索所用的算法完全相同、仅所查询的场不同。因此本章以泛指的 $U$ 表述搜索过程，其在两条路径的求解中分别取对应的方向性场。本文选择 Hybrid A* 作为粗解生成的骨架，而非另寻搜索框架，是因其与本文需求的结构性契合。装载场地的粗解需同时满足三件事：非完整的车辆运动学（路径切向与航向共线）、前进与后退混合的多段拓扑（泊入必经换向）、以及在无显式车道的自由空间内连续可行。Hybrid A* 以运动基元在$(x, y, \theta)$ 状态格上扩展\cite{dolgov2008practical}，天然满足前两者，且其格点离散对自由空间不作任何结构化假设，正好适配第三点。 % 本文对经典 Hybrid A* 的改进有三处：运动基元的扩展步长随局部势能自适应调制（\ref{subsec:primitive} 节），代价函数中引入场势能项以偏向 IGF 势谷（\ref{subsec:cost} 节），以及启发函数以固定特征长度折算势能、避免过度贪心（\ref{subsec:heuristic} 节）。三者分别对应粗解"贴合引导、代价可解释、搜索高效"三项要求，下文依次说明。除此之外，运动基元的形式、以终点位姿为目标的解析扩展、以及状态格上的重复节点剪枝均沿用经典算法\cite{dolgov2008practical}，不作改动。 \subsection{Field-Adaptive Motion-Primitive Expansion} \label{subsec:primitive} % 运动基元的设计取决于粗解在三阶段框架中所承担的职责。由 \ref{subsec:framework} 节，粗解的作用是确定两条路径的拓扑走向、换挡分段结构以及各段所处的自由空间通道，即固定 \eqref{eq:cons_kinodynamic} 中的离散量并为第 \ref{sec:opt} 章的联合优化提供位于可行域附近的初值；曲率剖面的连续性与变化率有界性 \eqref{eq:cons_sigma} 属于对最终路径的要求，由第 \ref{sec:opt} 章的连续优化实现。本章所介绍的运动基元沿用经典 Hybrid A* 的形式，同时提出一种扩展步长随局部势能自适应调制方法。 % 基元以恒定前轮转角 $\psi$ 的圆弧给出，节点状态取位姿 $\boldsymbol{\eta}=[x,y,\theta]^\top$，段内状态沿弧长的演化满足 \begin{equation} \frac{\mathrm{d}}{\mathrm{d}s} \begin{bmatrix} x\\ y\\ \theta \end{bmatrix} = \delta \begin{bmatrix} \cos\theta\\ \sin\theta\\ \tan\psi / L_\text{w} \end{bmatrix} \label{eq:search_sample} \end{equation} % 其中 $\delta\in\{+1,-1\}$ 为档位、$L_\text{w}$ 为轴距。该式即 \eqref{eq:cons_kinodynamic} 在 $\psi$ 取常值时的特例，故基元本身满足非完整约束；其段内曲率为常值 $\kappa=\tan\psi/L_\text{w}$，在相邻基元的拼接点处允许跳变。前轮转角在车辆的机械上界内均匀离散为 \begin{equation} \psi_j = \frac{j}{N_\psi}\, \psi_\text{max}, \qquad j \in \{-N_\psi, \ldots, N_\psi\}, \label{eq:search_steer_set} \end{equation} % 其中 $\psi_\text{max} = \arctan(L_\text{w}/R_\text{min})$ 由最小转弯半径反推，从而使基元的曲率自动满足 \eqref{eq:cons_kappa} 的上界，无需逐段额外校验。每次扩展在 $2N_\psi+1$ 档转角与两个档位上展开，共 $2(2N_\psi+1)$ 个候选。由节点 $\boldsymbol{\eta}_i$ 出发、以转角 $\psi_j$ 与档位 $\delta$ 行进弧长 $\mu_j^\delta$ 后到达的子节点记为 $\boldsymbol{\eta}_{i+1} = \varPhi(\boldsymbol{\eta}_i, \psi_j, \delta\mu_j^\delta)$，其中 $\mu_j^\delta$ 为该方向上的扩展步长，由下文 \eqref{eq:search_step} 给出。$\varPhi$ 对 \eqref{eq:search_sample} 作数值积分求得，基元内部的中间位姿一并用于碰撞检测。 \paragraph{State Lattice and Duplicate-State Pruning} % 基元扩展所得的位姿是连续量，为控制搜索规模须对其判重。本文沿用经典 Hybrid A* 的做法\cite{dolgov2008practical}：以空间分辨率 $\Delta_{xy}$ 与角度分辨率 $\Delta_\theta$ 在 $(x,y,\theta)$ 上铺设状态格，将位姿 $\boldsymbol{\eta}$ 映射为整数索引 \begin{equation} \mathcal{I}(\boldsymbol{\eta}) = \left( \left\lfloor x / \Delta_{xy} \right\rfloor,\; \left\lfloor y / \Delta_{xy} \right\rfloor,\; \left\lfloor \theta / \Delta_\theta \right\rfloor \right), \label{eq:search_lattice} \end{equation} % 索引相同的节点视为同一状态，其中仅保留 $g$ 值最小者、其余在扩展时即被剪除。 % 状态格在此仅充当判重的键，而\emph{不}参与位姿的表示：被保留的节点存储 \eqref{eq:search_sample} 积分所得的连续位姿，路径点不吸附至格心，故非完整可行性由基元逐点保证、不因判重而受损。其代价是最优性——同格内两个位姿的航向最多可相差近 $\Delta_\theta$，保留 $g$ 值较小者未必保留通向全局最优的那一支，故仅具备分辨率意义下的完备性；这与 \ref{subsec:heuristic} 节所述启发函数的不可采纳性一同构成 \ref{subsec:framework} 节“不再保证全局最优”的两个来源。 % 判重机制同时对基元的弧长提出了下界要求：若扩展步长小于状态格的对角尺寸，子节点将落回父节点所在的格而被判为重复，扩展即无法推进。故须使各方向上的实际步长满足 \begin{equation} \min_{j,\delta} \mu_j^\delta \; > \; \sqrt{2\,\Delta_{xy}^2 + \left( R_\text{min} \Delta_\theta \right)^2}, \label{eq:search_step_bound} \end{equation} % 其中右端为状态格对角线在弧长意义下的等效尺度，第三项以 $R_\text{min}\Delta_\theta$ 将角度分辨率折算为弧长。由 \eqref{eq:search_step} 的调制下界，该条件即 $w_\text{min}\mu_0$ 须超出该值，这构成基准步长 $\mu_0$ 的取值依据。 \paragraph{Boundary and Gear-Shift Handling} % 边界条件 \eqref{eq:cons_boundary} 仅约束位姿，与基元的状态量一致，故 $\boldsymbol{\eta}_\text{enter}$ 直接作为搜索的起点施加，$\boldsymbol{\eta}_\text{load}$ 则经下文的解析扩展精确到达。换挡折点处依 \eqref{eq:cons_junction} 位姿连续、档位反号；由于车辆在该处完全停止、前轮可原地转向，折点两侧的曲率无需衔接。粗解因此在分段结构上与优化阶段的设定一致：段界由换挡折点划定，而段内仍可存在非换挡的曲率断点（相邻基元的拼接点、以及解析段内部圆弧与直线的衔接处），这些断点连同段内曲率剖面一并留待优化阶段精化。 \paragraph{Field-Adaptive Expansion Step} % 本节的改动落在扩展步长上：\eqref{eq:search_sample} 中的弧长 $\mu_j^\delta$ 不取固定值，而随该方向上的局部势能自适应调制。为此在每次扩展前先以基准步长 $\mu_0$ 作一次短程探测，探测集合取转角与档位的极值组合 \begin{equation} \begin{aligned} \mathcal{D}\left( {{\boldsymbol{\eta} _i}} \right) = \bigl\{ \, & \boldsymbol{\eta} _j^\delta = \varPhi \left( {{\boldsymbol{\eta} _i},\psi _j,\delta {\mu _0}} \right) \; \big| \\ & j \in \left\{ { - {N_\psi },0,{N_\psi }} \right\},\;\delta \in \left\{ { + 1, - 1} \right\} \bigr\} \end{aligned} \label{eq:search_probe} \end{equation} % \eqref{eq:search_probe} 对应车辆以直行及左、右满转角行进 $\mu_0$ 所到达的极端位姿，共 $3 \times 2 = 6$ 个探测点；这些探测点只用于评估各方向的势能高低以调制步长，并不直接进入粗解。探测点的势能值刻画了各方向上的通行倾向：势能越高，表明该方向偏离势谷或趋近场地边界，应以更小的步长进行精细扩展；反之则可用更大的步长快速推进。 % 由于 IGF 的绝对幅值随场地尺寸与势源密度而变化，若直接以 $U$ 的绝对值设定步长，需针对每个场地重新标定阈值。为此，本文在探测点集合内部对势能进行归一化。记 $\mathbf{p}_j^\delta$ 为探测位姿 $\boldsymbol{\eta}_j^\delta$ 的位置分量，则 \begin{equation} {\tilde U_j^\delta} = \frac{{U\left( {\mathbf{p}_j^\delta} \right) - \mathop {\min }\limits_{\mathcal{D}} U}}{{\mathop {\max }\limits_{\mathcal{D}} U - \mathop {\min }\limits_{\mathcal{D}} U + {\epsilon _0} }} \in \left[ {0,1} \right] \label{eq:search_normU} \end{equation} % 并将步长写为关于 $\tilde U$ 的有界单调递减映射。 % 注意每个探测方向 $(j, \delta)$ 各自得到一个步长，即该方向上运动基元的实际扩展弧长： \begin{equation} \mu_j^\delta = \mu_0 \left[ w_\text{min} + \left( w_\text{max} - w_\text{min} \right) \left( 1 - \tilde U_j^\delta \right) \right] \label{eq:search_step} \end{equation} % 其中，$\epsilon_0$ 为防止零除的小量（区别于 \eqref{eq:qp_slack} 中的松弛变量 $\varepsilon_i$），$[w_\text{min}, w_\text{max}] = [0.6, 1.4]$ 为调制系数的取值区间。归一化使该策略仅依赖于探测点之间势能的相对高低，因而对 IGF 的绝对标度不敏感，无需随场地更换而重新调参。该调制的几何效果如图 \ref{fig:probe} 所示：落在低势能区的探测方向其步长被拉长至 $w_\text{max}\mu_0$，落在高势能区者被压缩至 $w_\text{min}\mu_0$，于是节点扩展沿引导势谷伸展、在贴近边界处自动细化。 % 由 \eqref{eq:search_step} 步长有界，$\mu_j^\delta \in [w_\text{min}\mu_0,\, w_\text{max}\mu_0]$，其下界已由 \eqref{eq:search_step_bound} 约束在状态格对角尺度之上，故调制不改变搜索的分辨率完备性。碰撞检测亦不以节点为单位，而是在每个运动基元内部按固定弧长增量 $\Delta s_\text{cc} \le B_\text{v}/2$（即车宽的一半，为 $\mathcal{V}$ 的最小外廓尺寸）细分后逐位姿进行，因此不会因步长增大而产生漏检。 \begin{figure}[htbp] \centering \includegraphics{fig_probe} \caption{Field-adaptive expansion of a single node. (a) The six probe poses reached by combining $\psi\in\{-\psi_\text{max},0,\psi_\text{max}\}$ with $\delta\in\{+1,-1\}$ at the baseline step $\mu_0$. (b) Steps modulated by \eqref{eq:search_step}: a probe landing in a low-potential region is extended toward $w_\text{max}\mu_0$, whereas one landing in a high-potential region is shortened toward $w_\text{min}\mu_0$, so that the expansion stretches along the guidance valley and refines near the boundary.} \label{fig:probe} \end{figure} \paragraph{Analytic Expansion to the Terminal Pose} % 仅以 \eqref{eq:search_sample} 的离散基元扩展，节点难以精确落在 $\boldsymbol{\eta}_\text{load}$ 上：状态格的分辨率决定了终点判定只能在一个容差邻域内进行，而装载位的朝向恰是需要精确对齐的量。为此本文沿用经典 Hybrid A* 的解析扩展（analytic expansion）机制\cite{dolgov2008practical}：每扩展固定数目的节点后，尝试以一条 Reeds--Shepp 曲线\cite{reedsshepp1990}将当前节点与 $\boldsymbol{\eta}_\text{load}$ 直接相连，该曲线由最小转弯半径 $R_\text{min}$ 的圆弧与直线段拼接而成、可含前进与后退的换向，其长度亦即 \eqref{eq:ha_h_time} 中的 $\ell_\text{RS}$。若该曲线通过碰撞检测，则搜索提前终止，粗解由已扩展的基元序列与该解析段拼接而成；否则继续扩展。这一机制使终点位姿被精确满足，同时显著减少了在装载位附近为对齐朝向所需的节点扩展量。 % 解析段与前文两点设定相容。其曲率剖面同样分段常值、在拼接点跳变，与 \eqref{eq:search_sample} 的基元一致，故粗解处处满足 \eqref{eq:cons_kappa}（解析段取 $R_\text{min}$），曲率剖面的连续化仍由第 \ref{sec:opt} 章承担。解析段自身的换向折点与基元扩展所产生的换向一并计入 $\{\delta_j\}$ 与 $N$；相应地，\eqref{eq:ha_g_gear} 的换挡代价在解析段被接受时须计入该段引入的折点，以免搜索因末段换向不计代价而偏好过早直连。 \subsection{Search Cost Function} \label{subsec:cost} % 代价： g = 耗时 + 场 + 换挡次数 % 本方法中，路径搜索过程中的代价由三部分组成：路径的假想耗时、换挡耗时、以及通行条件劣化带来的等效耗时： \begin{equation} g = g_\text{time} + g_\text{gear} + g_\text{field} \label{eq:ha_g} \end{equation} % 其中各分项定义为 \begin{subequations} \label{eq:ha_g_all} \begin{align} g_\text{time} & = \frac{1}{v_0} \sum_i \mu_i \label{eq:ha_g_time} \\ g_\text{gear} & = N \, t_0 \label{eq:ha_g_gear} \\ g_\text{field} & = \frac{w_U^{\bullet}}{v_0} \sum_i U_\bullet(\mathbf{p}_i) \, \mu_i \label{eq:ha_g_field} \end{align} \end{subequations} % 三个分项均以秒为量纲，因此 $g$ 可统一解释为一条路径的\emph{广义通行耗时}。该式与第 \ref{sec:opt} 章优化阶段所用的完整目标函数 \eqref{eq:cost} 采用同一量纲口径，二者的对应关系为：\eqref{eq:ha_g_time} 对应 \eqref{eq:cost_time}、\eqref{eq:ha_g_gear} 对应 \eqref{eq:cost_gear}、\eqref{eq:ha_g_field} 对应 \eqref{eq:cost_field}。差异在于本阶段略去了 \eqref{eq:cost} 中的交互项 $J_\varPsi$ 与平滑项 $J_\kappa$：前者因两条路径在本阶段独立并行求解、其耦合作用由 $U_\text{park}$、$U_\text{exit}$ 中的静态排斥项近似承担（详见 \ref{subsec:framework} 节），后者因运动基元的曲率分段常值、$\kappa^2$ 在本阶段不具备区分不同候选路径平滑性的能力，二者均在第 \ref{sec:opt} 章的连续优化阶段被完整引入。 % \eqref{eq:ha_g_time} 体现了单条路径的最高效原则，$\mu_i$ 表示路径上第 $i$ 步扩展所实际采用的运动基元弧长，即该步所选方向对应的 \eqref{eq:search_step} 之值；该项为车辆以恒定速度 $v_0$（本文取矿区车辆装载区内平均速度）跟踪该路径所需的时间。\eqref{eq:ha_g_gear} 保证了路径尽可能减少换挡次数、减少车辆机械损耗，其中 $N$ 为路径的换挡次数，$t_0$ 表示单次换挡动作的实测耗时；由于换挡期间车辆处于停止状态，该项即为换挡引入的真实时间损失。 % \eqref{eq:ha_g_field} 引入了露天矿区所要求的双路径空间分布与行驶倾向。该项将归一化势能 $U \in [0,1]$ 解释为\emph{等效减速因子}：势能越高表明该处越偏离引导通道、通行条件越差，车辆在此处的有效通行速度相应下降，因而付出额外的等效时间。无量纲权重 $w_U^{\bullet}$ 的物理含义即为：路径完全偏离势谷（$U \equiv 1$）时，相对自由通行状态所额外付出的耗时倍数；其上标 $\bullet \in \{\text{park}, \text{exit}\}$ 表示两条路径各自取值，与 \eqref{eq:cost_field} 中的优化阶段权重同值，故 \eqref{eq:asymmetric_weights} 的非对称取向在搜索阶段即已生效。 % 此处以弧长 $\mu_i$ 加权而非按节点计数累加，是因 \eqref{eq:search_step} 的步长自适应变化，按计数累加会使同一条几何路径在不同步长下得到不同代价；加权后该项即为 $\int_{\varGamma_\bullet} U_\bullet(\mathbf{p})\,\mathrm{d}s$ 的离散近似，与采样密度无关。 \subsection{Field-Augmented Heuristic Function} \label{subsec:heuristic} % 启发: h = 场 + max(A*, RS) % 本方法中，路径搜索的启发式函数由两部分组成：剩余路程的预估耗时、以及当前节点通行条件对应的等效耗时： \begin{equation} h = h_\text{time} + h_\text{field} \label{eq:ha_h} \end{equation} % 其中各分项定义为 \begin{subequations} \label{eq:ha_h_all} \begin{align} h_\text{time} & = \frac{\ell_\text{rem}}{v_0}, \quad \ell_\text{rem} = \max \left\{ \ell_{\text{A*}}, \, \ell_\text{RS} \right\} \label{eq:ha_h_time} \\ h_\text{field} & = \frac{w_U^{\bullet}}{v_0} \, U_\bullet(\mathbf{p}) \, L_\text{v} \label{eq:ha_h_field} \end{align} \end{subequations} % 与 \eqref{eq:ha_g_all} 一致，两个分项的量纲同为秒。\eqref{eq:ha_h_time} 是 Hybrid A* 算法中的经典启发函数，$\ell_{\text{A*}}$ 表示考虑碰撞的 A* 路径剩余预估长度，$\ell_\text{RS}$ 表示不考虑碰撞的 Reeds-Shepp 曲线剩余预估长度，二者取较大值作为剩余路程估计 $\ell_\text{rem}$。 % 另外，由于中线不经过装载位（\ref{subsec:centerline} 节），路径越靠近装载位就越需要从主通道“分叉”，其走势必然与 IGF 的势谷产生偏离；此时若把沿途势能累加进启发函数，分叉段的必要偏离会被当作代价持续累积、进而压制正确的分叉行为。因此 \eqref{eq:ha_h_field} 仅取待探索节点位置 $\mathbf{p}$ 处的势能强度，不再取累计值。为与 \eqref{eq:ha_g_field} 保持量纲一致，该项以车长 $L_\text{v}$ 作为特征尺度，即将节点处的势能折算为车辆通过自身长度这段距离所付出的等效额外耗时。此处采用固定特征长度而非剩余路程 $\ell_\text{rem}$ 作为尺度，是为了避免势能项随剩余距离线性放大、使搜索过度贪心。 % 关于启发函数的可采纳性，此处作一点说明。经典 Hybrid A* 中的 \eqref{eq:ha_h_time} 取无碰撞 Reeds-Shepp 长度与考虑碰撞的 A* 长度的较大值，二者均为剩余真实耗时的下界，因此该项是可采纳（admissible）的。但叠加 \eqref{eq:ha_h_field} 后，$h$ 中包含了并非源于剩余路程的等效耗时项，其数值可能超出节点到目标的真实剩余代价，故本文的 $h$ 整体上不再保证可采纳性，相应地也就不保证所得路径在 \eqref{eq:ha_g} 定义的广义通行耗时意义下全局最优。这是本文有意作出的设计取舍：本阶段搜索的目标并非求解最优路径，而是快速生成拓扑结构正确、且满足双向解耦要求的初值粗解，其几何品质由第 \ref{sec:opt} 章的数值优化阶段进一步保证。引入势能项后，节点扩展会明显偏向 IGF 势谷方向，从而在保证搜索成功率的同时显著减少扩展节点数（详见 \ref{sec:exp} 章的对比实验）。事实上，由于 \eqref{eq:ha_h_field} 以固定特征长度 $L_\text{v}$ 为尺度，该项的数值上界为 $w_U^{\bullet} L_\text{v} / v_0$，即其对可采纳性的破坏程度是有界的；且当 $w_U^{\bullet} \to 0$ 时，$h$ 退化为经典可采纳启发函数。该参数因此提供了在搜索效率与解的最优性之间进行权衡的显式接口。 % 整体流程与伪代码 % 至此三个阶段的构成要素均已给出，算法 \ref{alg:overall} 汇总其整体流程。第 1--4 行为离线阶段，仅在场地边界更新时重建；第 5--12 行为在线阶段，两条粗解在各自的引导场上并行搜索；第 13--17 行为联合优化阶段，两条粗解合并为单一决策向量后一次求解。 \begin{algorithm}[htbp] \caption{Spatially Decoupled Inbound/Outbound Planning} \label{alg:overall} \DontPrintSemicolon \SetKwInOut{Input}{Input} \SetKwInOut{Output}{Output} \SetKwFunction{Skel}{SkeletonAStar} \SetKwFunction{Smooth}{SmoothQP} \SetKwFunction{Offset}{OffsetLines} \SetKwFunction{Field}{BuildField} \SetKwFunction{Search}{FieldAdaptiveHybridAStar} \SetKwFunction{Corr}{ConvexCorridors} \SetKwFunction{Nlp}{SolveNLP} \Input{site boundary $\partial\Omega$; loading pose $\boldsymbol{\eta}_\text{load}$; entry/exit poses $\boldsymbol{\eta}_\text{in}$, $\boldsymbol{\eta}_\text{out}$; channel width $W$} \Output{jointly optimized path pair $\varGamma^\ast=(\varGamma_\text{park}^\ast, \varGamma_\text{exit}^\ast)$} \BlankLine \tcp{Stage 1: offline field prior, rebuilt only on boundary change} $\varLambda_\text{raw} \leftarrow$ \Skel{$\partial\Omega$}\; $\varLambda_\text{center} \leftarrow$ \Smooth{$\varLambda_\text{raw}$} \tcp*{\eqref{eq:qp_problem}} $\varLambda_\text{park}, \varLambda_\text{exit} \leftarrow$ \Offset{$\varLambda_\text{center}, W/2$} \tcp*{\eqref{eq:reference_offset}} $\hat U_\text{park}, \hat U_\text{exit} \leftarrow$ \Field{$\mathcal{S}_\text{e}, \mathcal{S}_\text{p}, \mathcal{S}_\text{x}$} \tcp*{\eqref{eq:field_park}--\eqref{eq:field_normalize}} \BlankLine \tcp{Stage 2: two coarse searches, run in parallel} \ForPar{$\bullet \in \{\text{park}, \text{exit}\}$}{ $(\boldsymbol{\eta}_\text{s}, \boldsymbol{\eta}_\text{g}) \leftarrow \begin{cases} (\boldsymbol{\eta}_\text{in}, \boldsymbol{\eta}_\text{load}), & \bullet=\text{park} \\ (\boldsymbol{\eta}_\text{load}, \boldsymbol{\eta}_\text{out}), & \bullet=\text{exit} \end{cases}$\; $\varGamma_\bullet^{(0)} \leftarrow$ \Search{$\boldsymbol{\eta}_\text{s}, \boldsymbol{\eta}_\text{g}, \hat U_\bullet$}\; } fix the gear sequence $\{\delta_j\}$ and the number of collocation points $N$ from $\varGamma_\text{park}^{(0)}, \varGamma_\text{exit}^{(0)}$\; \BlankLine \tcp{Stage 3: joint refinement of both paths at once} $\{\mathcal{P}_k\} \leftarrow$ \Corr{$\varGamma_\text{park}^{(0)}, \varGamma_\text{exit}^{(0)}, \mathcal{O}$} \tcp*{shrunk by $\Delta_\text{cut}$} \Repeat{all collocation points stay in their corridors}{ $\mathbf{z}^\ast \leftarrow$ \Nlp{$\mathbf{z}^{(0)}, \{\mathcal{P}_k\}$} \tcp*{\eqref{eq:joint_nlp}} rebuild the violated corridors around $\mathbf{z}^\ast$\; } \Return $\varGamma^\ast$ decoded from $\mathbf{z}^\ast$\; \end{algorithm} \section{Joint Optimization of Inbound and Outbound Paths} \label{sec:opt} % 狭窄空间安全约束与数值优化 % 本章以第 \ref{sec:search} 章生成的两条粗解为初值，求解 \eqref{eq:overall_problem} 的连续优化部分。沿用粗解确定的分段结构后，\eqref{eq:cost} 中的换挡项 \eqref{eq:cost_gear} 退化为常数、目标函数简化为 \eqref{eq:cost_opt}，本章因此聚焦于两件事：将 \eqref{eq:cons_collision} 的非凸避碰约束转化为可高效处理的线性形式（\ref{subsec:convex} 节），以及引入 \eqref{eq:cost_interaction} 的交互项完成两条路径的联合优化（\ref{subsec:joint_nlp} 节）。 \subsection{Convex-Corridor Construction in Confined Free Space} \label{subsec:convex} % 本小节将 \eqref{eq:cons_collision} 的非凸避碰约束转化为线性不等式形式。该约束难以直接处理的原因在于 $\Omega_\text{free}$ 由不规则的场地边界点云描述、其形状非凸。以障碍点距离形式直接施加约束，则约束数量随点云规模线性增长、且每一条均为非凸约束；沿参考线逐点开辟\emph{形状预设}的邻域（如前文的局部切向--法向矩形走廊用于中线平滑），虽可得到线性约束，但在边界走向不规则处难以充分利用自由空间。 % 本文因此采用凸分解（convex decomposition）方法\cite{corridor2022voxelgrid,corridor2024convexcover,corridor2023rover}：以粗解的每一段线段为种子，围绕该线段生成一个包裹它、且不含任何边界点的凸多面体，从而将 $\Omega_\text{free}$ 沿路径切分为一列相互重叠的无障碍凸区域（convex corridors）。与上述两类做法相比，凸分解的多面体面数与朝向均由局部边界点分布自适应确定，而非预先设定，故能在狭窄且不规则的装载场地内开辟尽可能大的凸可行空间。 % 矿区场地边界由车载激光雷达采集，以二维点集 $\mathcal{O}=\{\mathbf{o}_i\}$ 的形式给出。记粗解经等弧长重采样后的第 $k$ 段为端点 $\mathbf{p}_k^\text{ref}$、$\mathbf{p}_{k+1}^\text{ref}$ 所连线段。该段的多面体经"椭球代理—半空间裁剪"两步构造得到：先以线段为长轴生成一个不含任何边界点的避障椭球，再以该椭球的度量为每个边界点生成分离超平面，其交集即为所求。完整推导见补充材料 S-IV，结果为 \begin{equation} \mathcal{P}_k = \left\{ \mathbf{p} \in \mathbb{R}^2 \;\middle|\; \mathbf{A}_k \mathbf{p} \le \mathbf{b}_k \right\}, \label{eq:corridor_polytope} \end{equation} % 其中 $\mathbf{A}_k \in \mathbb{R}^{m_k \times 2}$ 的各行为分离超平面的单位外法向、$\mathbf{b}_k \in \mathbb{R}^{m_k}$ 为对应偏置。半空间个数 $m_k$ 由该段附近参与分解的边界点分布决定，而非预设常数；算法本身不对 $m_k$ 施加统一的常数上界，其实际分布与最大值将在第 \ref{sec:exp} 章中统计，以评估走廊约束的规模。相邻线段共享端点，故 $\mathcal{P}_k \cap \mathcal{P}_{k+1} \neq \varnothing$。 % 但共享端点处的相交尚不足以保证走廊分配的相容性。走廊按粗解的弧长位置编号，第 $k$ 个配置点的标称弧长为 $(k-1)\Delta s_\bullet^{(0)}$；由 \eqref{eq:joint_variable_one}，步长 $\Delta s_\bullet$ 本身是决策变量而 $M_\bullet$ 固定，故路径总长偏离粗解时第 $k$ 个配置点将沿路径纵向滑移 $(k-1)\lvert\Delta s_\bullet - \Delta s_\bullet^{(0)}\rvert$，该滑移随 $k$ 递增、在末端累积至最大。若走廊仅由标称线段生成，滑移后的配置点可能落到其被分配的走廊之外，导致虚假不可行。 % 本文以两项措施消除该风险。其一，生成第 $k$ 个走廊时不以单条标称线段为种子，而是将种子沿粗解折线向两端各延伸弧长 $\Delta_\text{slide}$ 后再作凸分解，使 $\mathcal{P}_k$ 在设计上即包含标称位置前后 $\Delta_\text{slide}$ 弧长带内的全部粗解点，相邻走廊由此在一段有限弧长上重叠、而非仅交于一点。其二，对步长的偏移施加信赖域上界 \begin{equation} \left\lvert \Delta s_\bullet - \Delta s_\bullet^{(0)} \right\rvert \le \frac{\Delta_\text{slide}}{M_\bullet - 1}, \label{eq:cons_step_trust} \end{equation} % 使任意 $k$ 处的滑移量均不超过延伸余量 $\Delta_\text{slide}$。两者结合，走廊编号与配置点编号之间的固定分配在整个可行域内保持有效。$\Delta_\text{slide}$ 的取值是余量与保守性之间的折中：过小则 \eqref{eq:cons_step_trust} 过紧、限制路径长度的调整幅度，过大则走廊因种子过长而收缩、约束趋于保守。本文取 $\Delta_\text{slide}$ 为粗解总长的一个小比例，其具体取值与走廊规模的统计一并列于第 \ref{sec:exp} 章。 % 若求解结束时 \eqref{eq:cons_step_trust} 取等（即路径长度的调整被信赖域截断），则以当前解重新采样、重建走廊后再解一次，构成一层外循环。该外循环是否构成实质开销取决于粗解所给长度与优化后最优长度的偏差是否落在 $\Delta_\text{slide}$ 之内，无法由本节的建模先行断定；其实测触发频次与 $\Delta_\text{slide}$ 的取值一并在 \ref{subsec:exp_efficiency} 节报告。下文的复杂度分析以单次求解计，若外循环触发则应按其平均次数线性折算。 % 需要强调的是，\eqref{eq:cons_collision} 约束的是扫掠域 $D(\varGamma)$ 而非路径中心点的轨迹，因此仅将路径点约束进 $\mathcal{P}_k$ 并不足以保证整车无碰。为此，本文对第 $k$ 个路径点处车辆所占据的矩形 $\mathcal{V}(\boldsymbol{\eta}_k)$ 取其四个顶点$\{\mathbf{c}_k^{(r)}\}_{r=1}^{4}$，并要求它们同时落在该段走廊之内： \begin{equation} \mathbf{A}_k \mathbf{c}_k^{(r)} \le \mathbf{b}_k, \qquad r = 1,\dots,4, \label{eq:cons_corridor} \end{equation} % 其中顶点坐标由位姿经刚体变换给出，即 $\mathbf{c}_k^{(r)} = \mathbf{p}_k + \mathbf{R}(\theta_k) [a_r,\, b_r]^\top$，$\mathbf{R}(\theta_k)$ 为 $\theta_k$ 对应的二维旋转矩阵，$[a_r, b_r]^\top$ 为四个顶点在车体坐标系下的固定坐标，由 $L_\text{v}$、$B_\text{v}$ 及后轴中心位置确定。 由于 $\mathcal{V}(\boldsymbol{\eta}_k)$ 与 $\mathcal{P}_k$ 均为凸集，顶点落入即蕴含整个矩形落入，故 \eqref{eq:cons_corridor} 是 \eqref{eq:cons_collision} 在\emph{离散配置点上}的充分条件，其中 $\mathbf{A}_k$、$\mathbf{b}_k$ 在优化过程中为常量，而 $\mathbf{c}_k^{(r)}$ 关于 $\mathbf{p}_k$ 为线性、关于 $\theta_k$ 为三角函数形式。 % 上述充分性仅在配置点上成立，相邻配置点之间车体仍可切出走廊之外，这一点对矿卡尺度不可忽略：车长十余米，转向时远离瞬心的顶点其转弯半径显著大于后轴中心的转弯半径。本文以走廊内缩覆盖该间隙，而不额外施加中点约束。设相邻配置点间车辆绕瞬心转过 $\Delta\theta = \Delta s_\bullet \kappa$，第 $r$ 个顶点到瞬心的距离为 $\rho_\text{c}^{(r)}$，则该顶点在两点之间偏离其弦的最大量为该圆弧的矢高，取四个顶点中最远者并代入曲率上界，得 \begin{equation} \begin{aligned} \Delta_\text{cut} &= \max_{r}\ \rho_\text{c}^{(r)} \left[ 1 - \cos\!\left( \frac{\Delta s_\bullet \kappa_\text{max}}{2} \right) \right] , \\ \rho_\text{c}^{(r)} &= \left\lVert \begin{bmatrix} a_r \\ b_r \end{bmatrix} - \begin{bmatrix} 0 \\ R_\text{min} \end{bmatrix} \right\rVert , \end{aligned} \label{eq:corner_cut_margin} \end{equation} % 于是只需在生成 $\mathcal{P}_k$ 时将其各半空间沿内法向统一内缩 $\Delta_\text{cut}$，\eqref{eq:cons_corridor} 即成为整段弧上无碰的充分条件。该内缩量与 \eqref{eq:cons_step_trust} 中的 $\Delta_\text{slide}$ 是两个相互独立的余量，不可混为一谈：$\Delta_\text{slide}$ 沿路径\emph{纵向}、用于吸收配置点的弧长滑移，$\Delta_\text{cut}$ 沿走廊\emph{横向}、用于吸收点间切角。就矿卡尺度而言 $\Delta_\text{cut}$ 的数值相当小——最远顶点 $\rho_\text{c} = 17.1$\,m 时，$\Delta s_\bullet = 1$\,m 对应 $1.5$\,cm，$\Delta s_\bullet = 1.5$\,m 对应 $3.3$\,cm——故一次性内缩不会实质压缩可行域；但该量随 $\Delta s_\bullet$ 近似二次增长，$\Delta s_\bullet = 3$\,m 时已达 $13$\,cm 量级、接近矿区常用安全余量，因而 \eqref{eq:corner_cut_margin} 同时给出了配置步长的上界依据。\eqref{eq:corner_cut_margin} 的推导与完整数值表见补充材料 S-IV-E，本文所采用的 $\Delta s_\bullet$ 及对应 $\Delta_\text{cut}$ 实际值见 \ref{subsec:exp_setup} 节。 \subsection{Smooth Path-Interaction Cost} \label{subsec:psi_build} % 本小节说明 \eqref{eq:cost_interaction} 中交互项的物理含义、并给出其核参数的取值依据。上一小节的凸分解走廊已经保证了两条路径各自的“硬安全”，因此交互项的作用不再是避免碰撞，而是在各自走廊内部尽量拉开两条路径的距离、以获得最大的操作余量。换言之，交互项只影响余量、不影响安全：两条路径之间不设硬隔离约束，与场地边界之间的安全间隔完全由凸分解走廊独立保证。这里不设硬隔离约束并不意味着把双向冲突留给时间维调度——如 \ref{sec:related_work} 节所述，装载场地内的到达时刻与装载时长均不受路径规划模块支配，本文正是为此才转以空间解耦消解冲突。真正的原因在于两条路径按 \eqref{eq:cons_boundary} 必须在装载位处共享位姿，任何处处成立的硬隔离约束都会与该边界条件直接冲突、使问题不可行；故解耦只能以软惩罚的形式追求“尽可能大的空间间隔”，其效果由 \ref{subsec:exp_geometry} 节的重叠面积指标量化。该项需要同时兼顾数学上的可微性与物理上的空间排斥性。 % 两个阶段采用不同解耦机制的原因是可用信息不同：搜索阶段两条路径独立并行生成，求解其一时另一条尚未确定、不存在可供积分的实际路径，故只能以名义引导线的静态排斥项近似承担解耦；进入联合优化后两条路径同时成为决策变量，交互项遂改以实际路径诱导的 $\varPsi_\text{exit}$ 直接度量接近程度。即静态排斥是解耦的\emph{先验}，交互场是其\emph{校正}。 % 为避免两条路径在装载位处共享端点时 $\lVert\mathbf{p}^\text{park}-\mathbf{p}^\text{exit}\rVert=0$ 导致欧氏范数不可微，本文在交互项中采用平滑距离 \begin{equation} d_{\varepsilon}(\mathbf{p},\mathbf{q}) = \sqrt{\lVert\mathbf{p}-\mathbf{q}\rVert^2+\varepsilon_\varPsi^2}, \qquad 0<\varepsilon_\varPsi\ll\rho_\varPsi, \label{eq:interaction_distance_smooth} \end{equation} % 其中 $\varepsilon_\varPsi$ 仅作为数值正则化尺度，不改变交互核的主要作用范围。\eqref{eq:cost_interaction} 的二重积分可以从“场”的角度获得直观解释。将其内层积分单独记为驶离路径在查询点 $\mathbf{p}$ 处诱导的\emph{交互场} \begin{multline} \varPsi_\text{exit}\!\left( \mathbf{p}; s_\text{park} \right) = \int_{\varGamma_\text{exit}} \tau_\text{d}\!\left( s_\text{park}, s_\text{exit} \right) \\ \times K\!\left( d_{\varepsilon}(\mathbf{p},\mathbf{p}^\text{exit}); \rho_\varPsi, m_\varPsi \right) \mathrm{d}s_\text{exit} \label{eq:interaction_field} \end{multline} % 则 \eqref{eq:cost_interaction} 可等价紧凑地写为 $J_\varPsi = \frac{w_\varPsi}{\rho_\varPsi^2} \int_{\varGamma_\text{park}} \varPsi_\text{exit}(\mathbf{p}^\text{park}; s_\text{park}) \,\mathrm{d}s_\text{park}$，即泊入路径在驶离路径所诱导的交互场中的势能积分。 % 需要注意两点。其一，因 \eqref{eq:cost_delay_weight} 的时延权重依赖于查询点在泊入路径上的弧长位置 $s_\text{park}$，$\varPsi_\text{exit}$ 除位置 $\mathbf{p}$ 外还以 $s_\text{park}$ 为参数，即它不是工作空间上的一个纯几何场，而是"在泊入路径的第 $s_\text{park}$ 处所感受到的排斥"。这与不加权时的情形不同：彼时 $\varPsi_\text{exit}$ 仅依赖 $\mathbf{p}$，可视为对向路径在空间中诱导的静态场。 % 其二，交换两条路径的角色所定义的 $\varPsi_\text{park}$ 与此处的 $\varPsi_\text{exit}$ 给出的\emph{数值不同}，因为 $\tau_\text{d}$ 关于两条路径不对称；但由 Fubini 定理，二重积分的值与积分次序无关，故 $J_\varPsi$ 本身是唯一确定的。这一"谁在谁的场中"的写法因此仅是同一代价的两种分解方式，选取哪一种取决于叙述方便，其中以 $\varPsi_\text{exit}$ 的形式更贴合物理图景——受排斥而绕行的主要是泊入路径。 % 其三，$\varPsi_\text{exit}$ 是以整条 $\varGamma_\text{exit}$ 为自变量的\emph{泛函}，而非以之为参数的静态场：它随驶离路径的形状在每次迭代中变化，故不可像 \eqref{eq:cost_field} 的 IGF 那样预先离散为栅格查表。 % 核参数中，影响半径按两车会车时所需的横向净空取值，即 \begin{equation} \rho_\varPsi \ge B_\text{v} + d_\varPsi, \label{eq:interaction_radius} \end{equation} % 其中 $d_\varPsi$ 为期望的双路径横向净空：两条路径的中心线相距 $B_\text{v}$ 时两车轮廓恰好贴合，故在此基础上再留出 $d_\varPsi$ 的余量。该取值使交互项在两条路径的扫掠域 $D(\varGamma_\text{park})$、$D(\varGamma_\text{exit})$ 发生接触之前即开始上升，从而以连续、可微的方式近似约束二者不要过度接近。 % 此处需要交代 \eqref{eq:cost_interaction} 的量纲折算，因为它与 \eqref{eq:cost_field} 的形式并不相同。$J_U$ 是无量纲场值沿路径的\emph{单重}弧长积分，除以 $v_0$ 即得秒；而 $J_\varPsi$ 是\emph{二重}弧长积分，其被积函数中 $\tau_\text{d}$ 已带秒的量纲、$K$ 无量纲，故二重积分的结果为“秒 $\cdot$ 米$^2$”。\eqref{eq:cost_interaction} 因此除以 $\rho_\varPsi^2$ 而非 $v_0$：核的支撑半径是该二重积分唯一的内蕴长度尺度，以其平方折算后 $J_\varPsi$ 的量纲为秒，且该折算不随路径加密或场地尺寸而变。经此处理，$w_\varPsi$ 与 $w_U^\bullet$ 同为无量纲权重、其数值可直接比较，二者共同构成 \eqref{eq:cost} 中各分项的等效耗时口径。需要说明的是，$\rho_\varPsi^2$ 的选取只影响 $w_\varPsi$ 的标定基准而不影响解的性质：任何固定的长度平方尺度都只是给 $w_\varPsi$ 乘一个常数。 % 这里说明为何采用 \eqref{eq:cost_interaction} 的核积分形式，而非直接惩罚两条路径同索引采样点之间的距离。后者（如以 $\sum_j f(\lVert \mathbf{p}_j^\text{exit} - \mathbf{p}_j^\text{park} \rVert)$ 形式配以逻辑斯谛惩罚函数）在两条路径长度相近、采样一一对应时是可行的简化，但在本文的问题设定下并不适用：泊入路径含换向段、其弧长通常显著大于驶离路径，两者的采样点之间不存在有几何意义的配对关系，同索引点距离因此无法反映真实的空间接近程度。 \eqref{eq:cost_interaction} 以弧长积分定义，其数值仅取决于两条路径的整体几何关系、与各自的采样数与参数化方式无关，因而不存在上述配对问题。相应地，其离散形式须以弧长而非节点计数加权（见 \eqref{eq:joint_psi_discrete}），其原因与 \eqref{eq:source_field} 相同。 \subsection{Joint Nonlinear Programming Formulation} \label{subsec:joint_nlp} % 本小节给出优化阶段所求解的问题。 % 由于 \eqref{eq:cost_interaction} 使两条路径相互耦合，一种自然的处理是交替优化：固定一条、优化另一条，逐次迭代至收敛。本文不采用该方案，而是将两条路径联立为单一非线性规划问题求解，其理由在本小节末尾说明。 \paragraph{Discretization and Decision Variables} % 沿用第 \ref{sec:search} 章粗解确定的分段结构，将两条路径分别按等弧长离散为$M_\text{park}$ 与 $M_\text{exit}$ 个配置点。记单条路径的离散状态序列为 $\{\mathbf{x}_k\}$、控制序列为 $\{\sigma_k\}$，并将两条路径的全部变量堆叠为单一决策向量 \begin{subequations} \label{eq:joint_variable} \begin{align} \mathbf{z} & = \left[ \mathbf{z}_\text{park}^\top,\; \mathbf{z}_\text{exit}^\top \right]^\top \label{eq:joint_variable_all} \\ \mathbf{z}_\bullet & = \left[ \mathbf{x}_1^\top, \ldots, \mathbf{x}_{M_\bullet}^\top,\; \sigma_1, \ldots, \sigma_{M_\bullet-1},\; \Delta s_\bullet \right]^\top \label{eq:joint_variable_one} \end{align} \end{subequations} % 其中 $\bullet \in \{\text{park}, \text{exit}\}$ 表示两条路径中的任一条（沿用 \eqref{eq:ha_g_field} 的记法），$\mathbf{x}_k = [x_k, y_k, \theta_k, \kappa_k]^\top$ 为 \eqref{eq:cons_kinodynamic} 所定义的路径状态，$\Delta s_\bullet$ 为该条路径的离散弧长步长。将 $\Delta s_\bullet$ 而非各段弧长逐一作为变量，是因为 \eqref{eq:cons_kinodynamic} 中路径总长 $s_\bullet = (M_\bullet - 1)\Delta s_\bullet$ 本身是待优化量（\eqref{eq:overall_problem} 为自由终端问题），而配置点数 $M_\bullet$ 一旦由粗解确定即保持固定。$\Delta s_\bullet$ 可变意味着配置点会沿路径相对粗解纵向滑移，其与固定走廊分配的相容性已由 \eqref{eq:cons_step_trust} 处理。 \eqref{eq:cons_kinodynamic} 的连续微分方程以梯形法离散为等式约束 \begin{equation} \mathbf{x}_{k+1} = \mathbf{x}_k + \frac{\Delta s_\bullet}{2} \left[ \mathbf{f}(\mathbf{x}_k, \sigma_k) + \mathbf{f}(\mathbf{x}_{k+1}, \sigma_k) \right], \label{eq:joint_collocation} \end{equation} % 其中 $\mathbf{f}$ 为 \eqref{eq:cons_kinodynamic} 的右端项。第 $k$ 个配置区间使用其所属第 $j(k)$ 个档位段的固定值 $\delta_{j(k)}$；这里的 $k$ 表示配置区间，而 $j$ 表示档位段。 \paragraph{Sparse Cross-Path Coupling} % 两条路径之间的耦合全部来自交互项。\eqref{eq:cost_interaction} 的两重弧长积分按上述离散化以矩形法求积，即得两条路径采样点之间的双重和 \begin{equation} \begin{aligned} J_\varPsi = & \frac{w_\varPsi}{\rho_\varPsi^2} \sum_{i=1}^{M_\text{park}} \sum_{j=1}^{M_\text{exit}} \tau_{ij} K\!\left( d_{\varepsilon}(\mathbf{p}_i^\text{park},\mathbf{p}_j^\text{exit}); \rho_\varPsi, m_\varPsi \right) \\ & \cdot \Delta s_\text{park}^{(0)} \; \Delta s_\text{exit}^{(0)} \end{aligned} \label{eq:joint_psi_discrete} \end{equation} % 其中 $\mathbf{p}_i^\text{park}$、$\mathbf{p}_j^\text{exit}$ 分别为两条路径第 $i$、$j$ 个配置点状态的位置分量，而 \begin{equation} \tau_{ij} = \frac{1}{v_0} \left[ (j-1)\Delta s_\text{exit}^{(0)} - (i-1)\Delta s_\text{park}^{(0)} \right]_+ \label{eq:joint_tau_discrete} \end{equation} % 为 \eqref{eq:cost_delay_weight} 在配置点对上的取值。与 \eqref{eq:interaction_field} 的对应关系是：对 $j$ 的内层求和即为 $\varPsi_\text{exit}(\mathbf{p}_i^\text{park}; s_\text{park})$ 的求积近似。 % $\tau_{ij}$ 仅依赖配置点\emph{序号}与冻结的标称步长 $\Delta s_\bullet^{(0)}$，不依赖任何路径点坐标，故为可离线预计算的常系数：引入时延权重不改变交互项的稀疏结构，$\tau_{ij} = 0$ 的点对（驶离车早已通过之处）更可在装配阶段直接跳过。此处若改用决策变量 $\Delta s_\bullet$ 表达，两个步长变量将与全部点对耦合；由 \eqref{eq:cons_step_trust}，冻结所引入的时延偏差不超过 $\Delta_\text{slide} / v_0$，在本文取值下为亚秒量级，相对 \eqref{eq:cycle_time} 的作业周期可忽略。 % 与 $\tau_{ij}$ 同理，式中的求积权重 $\Delta s_\text{park} \Delta s_\text{exit}$ 亦冻结为粗解给定的常值、不参与求导，尽管两个步长本身是 \eqref{eq:joint_variable_one} 中的决策变量。这一处理的理由是实现的简洁与偏差的可忽略：冻结之后该权重成为可并入 $w_\varPsi$ 的常数，交互项遂化为纯粹的核函数加权和，无需在每次迭代中重算求积权重及其导数。其代价是当优化后的路径长度偏离粗解时，冻结的权重与真实弧长步长之间出现比例偏差；该偏差有界且可以接受：由 \eqref{eq:cons_step_trust}，真实步长与冻结权重之比满足 $\lvert \Delta s_\bullet / \Delta s_\bullet^{(0)} - 1 \rvert \le \Delta_\text{slide} / \left[ (M_\bullet-1)\Delta s_\bullet^{(0)} \right]$，即不超过 $\Delta_\text{slide}$ 与粗解总长之比，故该比值被约束在 $1$ 的一个小邻域内。更重要的是，交互项是软惩罚而非安全约束（硬安全由 \ref{subsec:convex} 节的走廊承担），其权重的常数尺度偏差被 $w_\varPsi$ 的标定所吸收，不影响解的可行性。 % 对单个配置点对记 $\mathbf{r}_{ij}=\mathbf{p}_i^\text{park}-\mathbf{p}_j^\text{exit}$、$d_{ij}^{\varepsilon}=\sqrt{\lVert\mathbf{r}_{ij}\rVert^2+\varepsilon_\varPsi^2}$。由 $\nabla_{\mathbf{p}_i^\text{park}}d_{ij}^{\varepsilon}=\mathbf{r}_{ij}/d_{ij}^{\varepsilon}$ 及链式法则， % $\nabla_{\mathbf{p}_i^\text{park}}K(d_{ij}^{\varepsilon})=K'(d_{ij}^{\varepsilon})\mathbf{r}_{ij}/d_{ij}^{\varepsilon}$，而对驶离点的梯度带相反符号；$\tau_{ij}$ 作为常系数不参与该求导、仅按 \eqref{eq:joint_tau_discrete} 缩放各点对的贡献。故 \eqref{eq:joint_psi_discrete} 同时对两条路径产生梯度，两条路径可在同一次求解中相互推离。需要注意的是，两条路径所受的梯度\emph{大小相等、方向相反}——这一点并未因 $\tau_{ij}$ 的引入而改变，因为每个点对的时延权重对该对中的两个点是同一个数；时延权重所改变的是各点对之间的相对强度，即排斥作用沿弧长的分布，而非单个点对内部的力学平衡。由 \eqref{eq:compact_kernel} 的有限支撑性质，仅当平滑距离 $d_{ij}^{\varepsilon}<\rho_\varPsi$ 时，该点对才对梯度及 Hessian 产生贡献；叠加 $\tau_{ij} = 0$ 的点对亦无贡献，交互项所形成的跨路径 Hessian 块因此仅在\emph{空间上相近且时序上确有冲突}的配置点对处非零，而非稠密的 $M_\text{park} \times M_\text{exit}$ 块。 % 除 $J_\varPsi$ 外，其余目标项与约束均只作用于单条路径内部：\eqref{eq:joint_collocation} 的配置约束在各自路径内耦合相邻配置点，形成带状结构；\eqref{eq:cons_physics} 的物理上界与 \eqref{eq:cons_corridor} 的走廊约束也不连接两条路径。由于 $\boldsymbol{\eta}_\text{load}$ 是由上层系统给定的常量，两条路径分别等于该位姿的边界条件 \eqref{eq:cons_boundary} 不产生跨路径导数耦合。因此，联合问题仅通过稀疏的 $J_\varPsi$ 块发生跨路径耦合。综上，优化阶段所求解的问题为 \begin{subequations} \label{eq:joint_nlp} \begin{align} \min_{\mathbf{z}} \quad & J_\text{opt}(\mathbf{z}) = J_\ell + J_U + J_\varPsi + J_\kappa \label{eq:joint_obj} \\ \text{s.t.} \quad & \text{\eqref{eq:joint_collocation}},\; \text{\eqref{eq:cons_junction}},\; \text{\eqref{eq:cons_boundary}} \label{eq:joint_eq} \\ & \text{\eqref{eq:cons_kappa}},\; \text{\eqref{eq:cons_sigma}},\; \text{\eqref{eq:cons_step_trust}},\; \Delta s_\bullet > 0 \label{eq:joint_ineq} \\ & \mathbf{A}_k \mathbf{c}_k^{(r)} \le \mathbf{b}_k, \quad r = 1,\dots,4,\; \forall k \label{eq:joint_corridor} \end{align} \end{subequations} % 该问题为一个稀疏非线性规划问题。运动学右端 \eqref{eq:cons_kinodynamic} 为光滑的三角函数，平滑距离 \eqref{eq:interaction_distance_smooth} 消除了欧氏范数在零点的不可微性，走廊约束 \eqref{eq:joint_corridor} 为线性；核函数 \eqref{eq:compact_kernel} 在截断点处为 $C^{m-1}$ 连续，故取 $m_\varPsi \ge 3$ 时可提供连续二阶导数。在这些条件下，目标函数与约束关于 $\mathbf{z}$ 二阶连续可微，且其一阶导数（Jacobian）与二阶导数（Hessian）具有前述稀疏结构，满足内点法（interior-point method）的标准适用前提\cite{nocedal2006numerical}。 % 本文以 Ipopt\cite{wachter2006ipopt} 求解该问题。Ipopt 采用基于滤子的原始-对偶内点算法，在每一步迭代中需求解一个以 Hessian 与约束 Jacobian组装的大型稀疏线性系统（KKT 系统）；由于本问题的 Hessian 与 Jacobian 均为前述窄带加稀疏耦合块的结构，该线性系统可经稀疏 LU 或稀疏 Cholesky 分解高效求解，其阶数远低于稠密存储所暗示的规模。求解以第 \ref{sec:search} 章的粗解作为初值：粗解已满足正确的路径拓扑、分段结构与走廊约束，且其运动基元的转角离散由 $R_\text{min}$ 反推，故已满足 \eqref{eq:cons_kappa} 的曲率上界（见 \ref{subsec:primitive} 节），尚待本阶段处理的主要是 \eqref{eq:cons_sigma} 所要求的曲率变化率有界——粗解的 $\kappa(s)$ 在基元拼接点处跳变，该形态由本阶段的连续优化抹平。内点法对初值的可行性并无强制要求，但一个接近可行的初值能显著减少收敛所需迭代数，这也是本文将粗解搜索与数值优化串联、而非直接以随机或直线初值启动优化的原因之一；该收益的具体幅度由 \ref{subsec:exp_efficiency} 节的热启动对照实验量化。 \paragraph{Problem Scale and Rationale for Joint Optimization} % 联立求解相对交替优化的三点实质优势——共享位姿等式约束被一次性精确满足、标准 NLP 的收敛性结论可直接沿用、以及非对称性不依赖求解方式——详见补充材料 S-III。此处只指出使联立成为可能的结构性前提：问题的标度。决策变量数为 $5M_\text{park} + 5M_\text{exit}$（每条路径 $M_\bullet$ 个四元状态、$M_\bullet - 1$ 个控制量与一个步长），而约束总数与 Jacobian、Hessian 的非零元个数均随配置点数线性增长，即 $O(M_\text{park} + M_\text{exit})$：运动学配置、物理上界与走廊约束逐点施加、各自只牵连相邻配置点，其导数的非零元集中在主对角线附近的窄带内；唯一的跨路径耦合来自 $J_\varPsi$，但由前述有限支撑性与 $\tau_{ij}$ 的零值区，该块的非零元仅出现在空间相近且时序确有冲突的点对上，规模为 $O(\rho_\varPsi / \Delta s_\bullet)$ 量级而非稠密的 $O(M_\text{park} \cdot M_\text{exit})$。因此联立所付出的变量翻倍并不带来线性代数开销的相应膨胀，稀疏内点法可充分利用这一结构。 % 交互项在初值处的有效性需要一个条件：仅当\emph{既}空间相近（$d_{ij}^\varepsilon < \rho_\varPsi$）\emph{又}时序冲突（$\tau_{ij} > 0$）的点对存在时，$J_\varPsi$ 才具备非零梯度，否则由有限支撑性 $J_\varPsi \equiv 0$、交互项完全失效。该条件在本文中自然成立，但其成立之处值得指出：装载位邻域虽是两条路径几何上最贴近处，却因 $\tau_\text{d} = 0$ 而\emph{不}提供梯度；真正提供梯度的是入口侧共享通道段，那里 $\tau_{ij}$ 最高、而两条粗解的间距恰为 $W$ 并按 \eqref{eq:interaction_radius} 落在核的支撑域内。这与 \ref{subsec:proxy} 节“$T_\text{wait}$ 的实际约束来自入口侧通道”是同一件事的两个侧面。 \section{Experiments} \label{sec:exp} % 【P2】本章骨架先行建立，用以消除正文中的悬空引用；各小节内容待数据到位后填写。 % 全章预算 3.0 页（见 REVISION_PLAN.md 的页数预算表）。 \subsection{Experimental Setup} \label{subsec:exp_setup} % TODO(P2-2)：场地与车辆参数、算法参数、实现与硬件平台。须交代的量： % - 车辆：L_v、B_v、L_w、R_min、实测 dot_psi_max（对应 P0-7 的 sigma_max 折算） % - 场地：规模、边界感知方式与 bar_ell_e 的实际取值（对应 alpha_e 的标定） % - 场：栅格分辨率 delta、各 (alpha, rho, m)、以及 alpha 是否满足 eq:alpha_dominance % - 搜索：mu_0、[w_min, w_max]、状态格分辨率 % - 优化：配置步长 Delta_s 及由 eq:corner_cut_margin 得到的 Delta_cut 实际值（对应 P0-6）、 % Delta_slide 取值（对应 P0-5）、rho_Psi 与 W 的关系、各权重 w % - IGF 的刷新频次与触发条件，明确"离线"的实际含义（对应 P2-4） \subsection{Baselines and Metrics} \label{subsec:exp_baselines} % 五类基线按"逐项剥离本文的设计选择"的原则设置，使每一项对比只回答一个问题。 % ① 独立规划：两条路径各自以经典 Hybrid A* 加同一套数值优化求解，不含引导场、 % 不含交互项。该组回答"是否需要双向先验"，是与现行做法最接近的对照。 % ② 固定渠化规则：以预设的双向参考路线替代 \ref{sec:igf} 节由场地几何自动提取的 % 中线与引导线，其余环节相同。该组回答"自动提取相对人工预设的价值"， % 对应 \ref{sec:related_work} 节所述显式行驶规则一类工作。 % ③ 交替优化：以 \eqref{eq:joint_nlp} 的两条路径交替固定、轮流求解替代联立求解， % 其余相同。该组为补充材料 S-III 的论证提供实证。 % ④ 同索引点距惩罚：以两条路径同序号配置点之间的距离惩罚替代 \eqref{eq:cost_interaction} % 的核积分交互项。\ref{subsec:objective} 节已论证该形式在两条路径弧长悬殊时 % 不成立，此处须给出实证而非仅停留在论证。 % ⑤ 时间维协同调度：放开本文有意排除的时间维决策量，以集中式时空调度求解同一算例， % 作为吞吐量的性能上界参照。该组回答"空间解耦作为代理目标的损失有多大"， % 是 \ref{subsec:proxy} 节代理关系的唯一直接检验，故不可省略。 % 九项指标分三组。第一组直接检验代理目标的有效性：$T_\text{wait}$ 及其相对基线①的 % 压缩幅度、以及离散事件仿真下的实际吞吐量（趟/小时）——后者是代理目标有效性的 % 唯一直接证据，须与基线⑤的上界对照报告。第二组检验几何与拓扑性质：两条路径 % 扫掠域的重叠面积、二者的最小间距分布、以及按 \eqref{eq:topology_consistency} % 判定的拓扑一致率。第三组检验计算性质：求解时间与成功率（不少于 100 组随机装载位）、 % 搜索阶段的墙钟时间（须与扩展节点数并列报告：\ref{subsec:primitive} 节的场自适应 % 步长使每次扩展多出 6 次基元积分与 6 次场查询，节点数下降但单节点变贵， % 只报节点数不足以说明加速）、走廊半空间数 $m_k$ 的分布与最大值、 % \eqref{eq:cons_step_trust} 信赖域外循环的触发率、以及热启动相对冷启动的迭代数对照。 \subsection{Throughput and Waiting Time} \label{subsec:exp_throughput} % TODO(P2-2)：T_wait、T_park、T_exit 实测；代理目标有效性的验证。 % 另需给出入口侧通道宽度 W 的可行性判据（对应 subsec:proxy 中 T_wait 下界的论述）， % 并按 P2-3 将"下界"降级为实测参考值。 \subsection{Geometric Decoupling} \label{subsec:exp_geometry} % TODO(P2-2)：扫掠域重叠面积、两条路径的最小间距分布。 \subsection{Topological Consistency Across Loading Poses} \label{subsec:exp_topology} % TODO(P0-3/P2-2)：跨装载位序列的拓扑一致率（>=100 组随机装载位）， % 含装载位位于中线另一侧的情形——按 subsec:proxy 的说明，该处允许换边，须如实报告。 \subsection{Computational Efficiency} \label{subsec:exp_efficiency} % TODO(P2-2)：求解时间与成功率；搜索阶段墙钟时间（不只报扩展节点数）； % 走廊半空间数 m_k 的分布与最大值；信赖域外循环触发率（对应 P0-5）； % 热启动 vs 冷启动的迭代数对照。 \subsection{Ablation Study} \label{subsec:exp_ablation} % 逐项去除四处设计选择，每项只改动一个环节、其余保持不变： % (1) 令 $\tau_\text{d} \equiv 1$，即去除 \eqref{eq:cost_delay_weight} 的时延加权。 % 按 \ref{subsec:asymmetry} 节的论证，此时交互项关于两条路径完全对称、 % 非对称性无法再由权重比例引入，故该组直接检验时延权重这一核心构造。 % (2) 去除 \eqref{eq:cost_interaction} 的交互项 $J_\varPsi$，只保留场引导项 $J_U$， % 用以分离"离线场先验"与"在线联合推离"两种解耦机制各自的贡献。 % (3) 去除 \eqref{eq:search_step} 的场自适应步长调制，改用固定步长 $\mu_0$。 % (4) 令 $w_U^\text{park} = w_U^\text{exit}$，去除 \eqref{eq:asymmetric_weights} 的非对称引导权重， % 检验满载优先这一工程语义是否确实转化为可测的差异。 % 正文只报关键两项（(1) 与 (2)）的主要指标，完整消融表迁入补充材料 S-VI。 \subsection{Real-Vehicle Validation} \label{subsec:exp_realvehicle} % 本小节的作用不是可行性演示，而是检验前文两处以标称值作出的解析论断在实车上是否成立。 % 须先交代试验条件：矿卡型号与关键参数（$L_\text{v}$、$B_\text{v}$、$L_\text{w}$、 % $R_\text{min}$ 及实测前轮转角速率上界）、场地规模与边界感知方式（激光雷达型号与 % 点云分辨率，即 \eqref{eq:bound_edge} 中 $\bar\ell_\text{e}$ 的实际取值）、 % 试验规模（装载循环次数、覆盖的装载位数量、总里程与时长）、定位方案与精度 % ——后者决定跟踪误差数据本身的可信度，须与所报误差量级一并给出。 % 论证一：跟踪可行性，检验 \eqref{eq:sigma_time_conversion} 的量纲折算与由此得到的 % $\sigma_\text{max}$ 取值是否够用。须报告横向跟踪误差沿弧长的分布，尤其换向段与 % 装载位邻域这两处曲率变化最剧烈的区段；同时报告实测前轮转角速率是否触及机械上界。 % 此处须如实说明一种不利可能：\eqref{eq:sigma_time_conversion} 以标称速度 $v_0$ 折算 % $\sigma_\text{max}$，若低速段出现大量触顶，则说明该折算偏松而非偏保守， % $\sigma_\text{max}$ 应改由分段速度剖面给出。此外须报告实测车体轮廓到场地边界的 % 最小间隙，与 \eqref{eq:corner_cut_margin} 所给的 $\Delta_\text{cut}$ 对照， % 检验一次性内缩是否确实覆盖了配置点之间的切角。 % 论证二：吞吐量增益，与离散事件仿真互为验证。须报告实测 $T_\text{wait}$、 % $T_\text{park}$、$T_\text{exit}$ 与仿真预测值的对照，以及相对现行作业方式的 % 趟/小时对比。两项论证相互独立：前者说明所规划的路径可被执行， % 后者说明执行之后确实产生了 \ref{subsec:proxy} 节所预期的吞吐量收益。 % 对照组安排：实车上仅跑基线①（独立规划）与本文方法两组，其余基线留在仿真。 % 理由须明确写出——基线②--⑤或需改动场地既有作业规则、或需放开时间维调度权限， % 在生产矿区不具备实施条件；此处如实说明，避免被理解为回避不利对比。 \section{Conclusion} \label{sec:conclusion} % 本文针对露天矿装载场地内进出双向路径的时空冲突问题，提出了以空间解耦程度为吞吐量代理目标的规划方法。 % 三点结论。其一，双向通行的拓扑要求不必作为在线约束施加：将场地边界与两条方向性引导线离散为势源、 % 叠加为一对关于虚拟道路中线互为镜像的方向性场，该拓扑即被前置固化于离线阶段；由于场的构造只用到 % 边界几何、不含装载位信息，装载位变更时无需重建场，两条路径亦可在各自的场上独立并行搜索。 % 其二，两条路径之间的冲突代价宜以有限支撑核的时延加权二重弧长积分表达：该形式只取决于两条路径的 % 整体几何关系，因而不受泊入路径含换向段、其弧长显著大于驶离路径这一非对称性的影响；时延权重仅依赖 % 配置点序号与冻结步长，故为常系数，不改变联合优化问题的稀疏结构。其三，非对称引导权重使满载车优先 % 这一工程语义直接进入目标函数，而无须依赖权重比例——后者在无时延加权时对极小点并无影响。 % 实验部分以五类基线（含时间维协同调度作为性能上界）与实车试验对上述结论予以验证。 % 本文方法的适用边界亦须明确。IGF 承担的是随场地边界缓变的通行拓扑，而非随车流快变的瞬时占据， % 故挖掘机与其他车辆等移动障碍由在线凸走廊避让、不进入场的构造。所优化的是吞吐量的必要条件而非 % 吞吐量本身：空间解耦消除了冲突导致的等待，但并不保证上层派车节拍与之匹配。此外，拓扑一致性由 % 软代价倾向性地促成而非硬约束强制，装载位移至中线另一侧时仍可能换边。 % 后续工作将沿两个方向展开：一是将上层派车节拍纳入代理目标的标定，使空间解耦的收益能够传导至 % 实际吞吐量；二是将场的重建从班次级触发推进到随边界感知增量更新，以适应开采面推进更快的作业面。 % TODO(P2-5)：实测数据到位后，在第一段末尾补一句量化结论（等待时间压缩幅度、吞吐量增益、求解耗时）， % 与 Abstract 末句的占位数字同步替换。 % 【P1-3】原附录 A「Second-Difference Discretization of Curvature Constraints」整节 % （6 个公式）已迁入补充材料 S-V，正文 subsec:centerline 只保留换算关系本身、误差量级 % 的一句结论与指针。原附录 B 已于 P1-1 迁入 S-IV，故 \appendices 环境及其计数器/ % hyperref 锚点设置一并删除；若后续需要恢复附录，需同时恢复 \counterwithin 与 % \theHequation 等定义，否则附录与正文的图/式编号会产生同名 hyperref destination。 % TODO(投稿前): 正文取消注释后删除下面这行 \nocite{*}，否则参考文献表会打印 refs.bib 的全部条目。 % 当前草稿阶段正文均为注释、无任何生效的 \cite，故必须保留此行，否则 thebibliography 为空而编译报错。 \nocite{*} \bibliographystyle{IEEEtran} \bibliography{refs} \end{document}