essay-mineplanning/explanation_lines_841_843.md

main.tex 第 841--843 行详解

本文档解释 eq:joint_psi_discrete 为什么能同时影响两条路径、有限支撑核为什么会产生稀疏 Hessian，以及第 843 行所谓“块对角结构”到底是什么意思。

1. 先把决策变量拆开

联合优化变量写成

\mathbf z=
\begin{bmatrix}
\mathbf z_{\mathrm{park}}\\
\mathbf z_{\mathrm{exit}}
\end{bmatrix}.

其中，\mathbf z_{\mathrm{park}} 包含泊入路径的全部配置点状态、区间控制量和弧长步长，\mathbf z_{\mathrm{exit}} 同理。每条路径的变量维数为

n_{\mathrm{park}}=5M_{\mathrm{park}},
\qquad
n_{\mathrm{exit}}=5M_{\mathrm{exit}},

所以 $\mathbf z\in\mathbb R^{n_z}$，其中

n_z=n_{\mathrm{park}}+n_{\mathrm{exit}}.

这里的 5M_\bullet 来自 M_\bullet 个四维状态、M_\bullet-1 个控制量以及一个弧长步长。

第 i 个泊入路径位置是

\mathbf p_i^{\mathrm{park}}
=
\begin{bmatrix}x_i^{\mathrm{park}}\\y_i^{\mathrm{park}}\end{bmatrix},

它只是 \mathbf z_{\mathrm{park}} 中第 i 个状态的位置分量，并不是额外的决策变量。定义状态到位置的选择矩阵

\mathbf S_p=
\begin{bmatrix}
1&0&0&0\\
0&1&0&0
\end{bmatrix}.

再定义作用于整个联合变量 \mathbf z 的常数选择矩阵

\mathbf E_i^{\mathrm{park}}\in\mathbb R^{2\times n_z},
\qquad
\mathbf E_j^{\mathrm{exit}}\in\mathbb R^{2\times n_z}.

\mathbf E_i^{\mathrm{park}} 在泊入路径第 i 个四维状态对应的列上放置 $\mathbf S_p$，其余元素均为零；\mathbf E_j^{\mathrm{exit}} 在驶离路径第 j 个四维状态对应的列上放置 $\mathbf S_p$，其余元素均为零。因此

\boxed{
\mathbf p_i^{\mathrm{park}}
=\mathbf E_i^{\mathrm{park}}\mathbf z,
\qquad
\mathbf p_j^{\mathrm{exit}}
=\mathbf E_j^{\mathrm{exit}}\mathbf z.}

若用从 1 开始的元素下标表示，则 \mathbf E_i^{\mathrm{park}} 选取 \mathbf z 的第 $4(i-1)+1$、4(i-1)+2 个元素；\mathbf E_j^{\mathrm{exit}} 选取第

n_{\mathrm{park}}+4(j-1)+1,
\qquad
n_{\mathrm{park}}+4(j-1)+2

个元素。

因此，只要目标函数同时包含 \mathbf p_i^{\mathrm{park}} 和 $\mathbf p_j^{\mathrm{exit}}$，它就是整个 \mathbf z 的函数。

2. 将交互项显式写成 \mathbf z 的函数

为简化记号，定义点对选择矩阵

\mathbf D_{ij}
=\mathbf E_i^{\mathrm{park}}-\mathbf E_j^{\mathrm{exit}}
\in\mathbb R^{2\times n_z}.

于是两个配置点的相对位置和距离分别为

\mathbf r_{ij}(\mathbf z)
=\mathbf p_i^{\mathrm{park}}-\mathbf p_j^{\mathrm{exit}}
=\mathbf D_{ij}\mathbf z,

d_{ij}(\mathbf z)
=\|\mathbf D_{ij}\mathbf z\|.

正文实现将两个弧长步长冻结为粗解给出的常数。记

c_0=\frac{w_\varPsi}{v_0}
\Delta s_{\mathrm{park}}^{(0)}
\Delta s_{\mathrm{exit}}^{(0)},

其中上标 (0) 表示粗解值。离散交互项便可以显式写成

\boxed{
J_\varPsi(\mathbf z)
=c_0\sum_{i=1}^{M_{\mathrm{park}}}
\sum_{j=1}^{M_{\mathrm{exit}}}
K\!\left(
\|\mathbf D_{ij}\mathbf z\|;
\rho_\varPsi,m_\varPsi
\right).}

把核函数的分段定义代入后，还可以完全展开为

J_\varPsi(\mathbf z)
=c_0\sum_{i=1}^{M_{\mathrm{park}}}
\sum_{j=1}^{M_{\mathrm{exit}}}
\begin{cases}
\displaystyle
\left(
1-\frac{\|\mathbf D_{ij}\mathbf z\|}{\rho_\varPsi}
\right)^{m_\varPsi},
& \|\mathbf D_{ij}\mathbf z\|<\rho_\varPsi,\\[8pt]
0,
& \|\mathbf D_{ij}\mathbf z\|\geq\rho_\varPsi.
\end{cases}

至此，表达式右侧只剩下决策变量 $\mathbf z$、常数选择矩阵 \mathbf D_{ij} 以及固定参数，不再把 \mathbf p_i^{\mathrm{park}} 和 \mathbf p_j^{\mathrm{exit}} 当成独立符号。这里使用 \mathbf D_{ij} 而不使用 $\mathbf A_{ij}$，是为了避免和正文凸走廊约束中的 \mathbf A_k 混淆。

对于每个点对 $(i,j)$，定义

\mathbf r_{ij}(\mathbf z)
=\mathbf D_{ij}\mathbf z,
\qquad
d_{ij}(\mathbf z)=\|\mathbf D_{ij}\mathbf z\|.

当 0<d_{ij}(\mathbf z)<\rho_\varPsi 时，核函数为

K(d_{ij}(\mathbf z))=
\left(1-\frac{d_{ij}(\mathbf z)}{\rho_\varPsi}\right)^{m_\varPsi},

它随距离增大而减小。因此，最小化 J_\varPsi 会倾向于增大两条路径之间的距离。

3. 为什么两条路径会“同时被推开”

对一个有效点对 $(i,j)$，有

K'(d)
=-\frac{m_\varPsi}{\rho_\varPsi}
\left(1-\frac{d}{\rho_\varPsi}\right)^{m_\varPsi-1}.

3.1 先求距离对位置的导数

令

\mathbf p_i^{\mathrm{park}}
=\begin{bmatrix}x_i\\y_i\end{bmatrix},
\qquad
\mathbf p_j^{\mathrm{exit}}
=\begin{bmatrix}\bar x_j\\\bar y_j\end{bmatrix}.

则相对位置为

\mathbf r_{ij}
=\mathbf p_i^{\mathrm{park}}-\mathbf p_j^{\mathrm{exit}}
=\begin{bmatrix}
x_i-\bar x_j\\
y_i-\bar y_j
\end{bmatrix},

距离为

d_{ij}=\|\mathbf r_{ij}\|
=\sqrt{(x_i-\bar x_j)^2+(y_i-\bar y_j)^2}.

对泊入点的两个坐标分别求偏导：

\frac{\partial d_{ij}}{\partial x_i}
=\frac{x_i-\bar x_j}{d_{ij}},
\qquad
\frac{\partial d_{ij}}{\partial y_i}
=\frac{y_i-\bar y_j}{d_{ij}}.

将两个分量写成列向量，就是

\nabla_{\mathbf p_i^{\mathrm{park}}}d_{ij}
=
\begin{bmatrix}
\partial d_{ij}/\partial x_i\\
\partial d_{ij}/\partial y_i
\end{bmatrix}
=\frac{1}{d_{ij}}
\begin{bmatrix}
x_i-\bar x_j\\
y_i-\bar y_j
\end{bmatrix}
=\frac{\mathbf r_{ij}}{d_{ij}}.

几何上，\mathbf r_{ij}/d_{ij} 是从驶离点指向泊入点的单位方向向量：泊入点沿这个方向移动会增大两点距离，沿相反方向移动会减小距离。

3.2 再使用链式法则

核函数不是直接以 \mathbf p_i^{\mathrm{park}} 为自变量，而是通过标量距离 d_{ij} 依赖位置：

\mathbf p_i^{\mathrm{park}}
\longrightarrow
d_{ij}
\longrightarrow
K(d_{ij}).

因此多元链式法则给出

\frac{\partial K(d_{ij})}
{\partial \mathbf p_i^{\mathrm{park}}}
=\frac{\mathrm dK}{\mathrm dd}(d_{ij})
\nabla_{\mathbf p_i^{\mathrm{park}}}d_{ij}
=K'(d_{ij})\frac{\mathbf r_{ij}}{d_{ij}}.

这就是文档中的目标公式。它由两部分相乘得到：

\underbrace{K'(d_{ij})}_{\text{距离增大时核代价的变化率}}
\quad\times\quad
\underbrace{\frac{\mathbf r_{ij}}{d_{ij}}}_{\text{距离对泊入位置的方向导数}}.

而

\frac{\partial K(d_{ij})}
{\partial \mathbf p_j^{\mathrm{exit}}}
=-K'(d_{ij})\frac{\mathbf r_{ij}}{d_{ij}}.

驶离点前面的负号来自相对位置向量的定义。对每一对泊入点和驶离点，定义

\mathbf r_{ij}
=\mathbf p_i^{\mathrm{park}}-\mathbf p_j^{\mathrm{exit}}.

为了看清这个偏导，令两个二维位置分别写成

\mathbf p_i^{\mathrm{park}}
=\begin{bmatrix}x_i\\y_i\end{bmatrix},
\qquad
\mathbf p_j^{\mathrm{exit}}
=\begin{bmatrix}\bar x_j\\\bar y_j\end{bmatrix}.

于是

\mathbf r_{ij}
=\begin{bmatrix}
x_i-\bar x_j\\
y_i-\bar y_j
\end{bmatrix}.

这里的“对向量求偏导”严格来说是 Jacobian 矩阵，因此逐分量计算可得

\begin{aligned}
\frac{\partial \mathbf r_{ij}}
{\partial \mathbf p_j^{\mathrm{exit}}}
&=
\begin{bmatrix}
\dfrac{\partial(x_i-\bar x_j)}{\partial \bar x_j}
&\dfrac{\partial(x_i-\bar x_j)}{\partial \bar y_j}\\[6pt]
\dfrac{\partial(y_i-\bar y_j)}{\partial \bar x_j}
&\dfrac{\partial(y_i-\bar y_j)}{\partial \bar y_j}
\end{bmatrix}\\[4pt]
&=
\begin{bmatrix}-1&0\\0&-1\end{bmatrix}
=-\mathbf I_2.
\end{aligned}

其中 \mathbf I_2 是二维单位矩阵。这个结果也可以用增量来理解：若驶离点发生一个小位移 $\Delta\mathbf p_j^{\mathrm{exit}}$，而泊入点保持不变，则

\Delta\mathbf r_{ij}
=\Delta\mathbf p_i^{\mathrm{park}}
-\Delta\mathbf p_j^{\mathrm{exit}}
=-\Delta\mathbf p_j^{\mathrm{exit}}.

由于这里固定泊入点，$\Delta\mathbf p_i^{\mathrm{park}}=\mathbf 0$。也就是说，驶离点沿某个方向移动时，从驶离点指向泊入点的相对向量沿相反方向变化，这正是 Jacobian 中出现负号的原因。将它代入链式法则，就得到

\frac{\partial K(d_{ij})}
{\partial \mathbf p_j^{\mathrm{exit}}}
=-K'(d_{ij})\frac{\mathbf r_{ij}}{d_{ij}}.

上述距离梯度推导要求 $d_{ij}>0$；当两个点精确重合时，\|\mathbf r_{ij}\| 在零点没有唯一梯度，需要使用平滑距离或在实现中单独处理该点对。

直接对联合变量 \mathbf z 求导，则一个有效点对的梯度为

\nabla_{\mathbf z}
K\!\left(\|\mathbf D_{ij}\mathbf z\|\right)
=K'(d_{ij})
\frac{\mathbf D_{ij}^{\mathsf T}\mathbf D_{ij}\mathbf z}{d_{ij}}.

定义当前有限支撑域内的有效点对集合

\mathcal I(\mathbf z)=
\left\{(i,j)\ \middle|\
0<\|\mathbf D_{ij}\mathbf z\|<\rho_\varPsi
\right\},

便得到完整交互项关于联合决策变量的梯度：

\boxed{
\nabla_{\mathbf z}J_\varPsi(\mathbf z)
=c_0\sum_{(i,j)\in\mathcal I(\mathbf z)}
K'\!\left(\|\mathbf D_{ij}\mathbf z\|\right)
\frac{\mathbf D_{ij}^{\mathsf T}\mathbf D_{ij}\mathbf z}
{\|\mathbf D_{ij}\mathbf z\|}.}

由于

\mathbf D_{ij}
=\mathbf E_i^{\mathrm{park}}-\mathbf E_j^{\mathrm{exit}},

\mathbf D_{ij}^{\mathsf T} 会把同一个二维点对梯度以相反符号写回 \mathbf z_{\mathrm{park}} 和 \mathbf z_{\mathrm{exit}} 对应的位置分量。这就是 J_\varPsi(\mathbf z) 在一次联合求解中同时更新两条路径的代数原因。

两个梯度大小相等、方向相反。因为 $K'(d)<0$，梯度下降时：

• 泊入点 \mathbf p_i^{\mathrm{park}} 沿远离驶离点的方向移动；
• 驶离点 \mathbf p_j^{\mathrm{exit}} 沿远离泊入点的方向移动。

这就是第 841 行所说的“两条路径可在同一次求解中相互推离”。它并不是先固定一条路径再移动另一条，而是同一个目标函数对两组决策变量同时提供梯度。

需要注意，梯度形式是对称的，不代表最终位移一定相等。两条路径还分别受到引导场、长度、平滑性和走廊约束的作用；哪条路径更容易移动，取决于这些项共同形成的“刚度”。

4. Hessian 的“跨路径耦合块”是什么

将联合变量按两条路径分块后，目标函数 Hessian 的结构可以概念性地写成

\nabla^2_{\mathbf z\mathbf z}J=
\begin{bmatrix}
H_{\mathrm{pp}} & H_{\mathrm{pe}}\\
H_{\mathrm{ep}} & H_{\mathrm{ee}}
\end{bmatrix}.

这里：

• H_{\mathrm{pp}} 描述泊入变量之间的二阶关系；
• H_{\mathrm{ee}} 描述驶离变量之间的二阶关系；
• H_{\mathrm{pe}} 和 H_{\mathrm{ep}} 描述泊入变量与驶离变量之间的二阶关系，称为跨路径耦合块。

若目标函数可以完全分离为

J(\mathbf z)=J_{\mathrm{park}}(\mathbf z_{\mathrm{park}})
+J_{\mathrm{exit}}(\mathbf z_{\mathrm{exit}}),

那么

H_{\mathrm{pe}}=H_{\mathrm{ep}}=0,

整个 Hessian 按两条路径呈块对角结构。加入 J_\varPsi 后，一个核项同时依赖 \mathbf p_i^{\mathrm{park}} 和 $\mathbf p_j^{\mathrm{exit}}$，所以相应的跨路径二阶导数一般不为零。

对于单个点对，可以把它的 Hessian 贡献写成

\begin{bmatrix}
G_{ij} & -G_{ij}\\
-G_{ij} & G_{ij}
\end{bmatrix},

其中 G_{ij} 是关于二维相对位置 \mathbf r_{ij} 的 2\times2 二阶导数矩阵。非对角位置的 -G_{ij} 正是该点对造成的跨路径耦合。

5. 为什么有限支撑核会让跨路径块稀疏

核函数满足

K(d)=0,\qquad d\geq\rho_\varPsi.

在严格满足 d_{ij}>\rho_\varPsi 的区域内，K(d_{ij}) 是常数零，因此它的梯度和 Hessian 也都是零。于是：

• 若点对 (i,j) 的距离小于 $\rho_\varPsi$，它在跨路径块中产生非零项；
• 若点对 (i,j) 的距离大于 $\rho_\varPsi$，它不产生任何导数项。

例如，两条路径各有 4 个点，理论上共有 4\times4=16 个点对。假设只有

(1,1),\ (2,2),\ (2,3),\ (3,3),\ (4,4)

这 5 对点的距离小于作用半径，那么跨路径块中只有与这 5 对点有关的位置非零，其余 11 对均为零。这就是第 841 行所说的“并非稠密的 M_{\mathrm{park}}\times M_{\mathrm{exit}} 块”。

这里的 M_{\mathrm{park}}\times M_{\mathrm{exit}} 是按“配置点对”描述的概念尺寸。由于每个位置点有 x,y 两个分量，实际位置 Hessian 的跨路径块尺寸是

2M_{\mathrm{park}}\times2M_{\mathrm{exit}},

每个有效点对通常对应一个 2\times2 小块。

6. “其余部分均为块对角结构”是什么意思

第 843 行的“块对角”是按两条路径分成两个大块而言，并不是说单条路径内部的矩阵是对角矩阵。

6.1 单条路径内部仍然存在耦合

配置约束

\mathbf x_{k+1}=\mathbf x_k+
\frac{\Delta s}{2}
\left[\mathbf f(\mathbf x_k,\sigma_k)
+\mathbf f(\mathbf x_{k+1},\sigma_k)\right]

同时依赖 $\mathbf x_k$、\mathbf x_{k+1} 和 $\sigma_k$。所以，单条路径内部相邻配置点之间存在耦合，其 Jacobian/Hessian 通常是带状矩阵，而不是对角矩阵。

6.2 但这些约束不连接两条路径

泊入路径的配置约束只包含 $\mathbf z_{\mathrm{park}}$，驶离路径的配置约束只包含 $\mathbf z_{\mathrm{exit}}$。因此按两条路径分块后，约束 Jacobian 具有如下形式：

J_{\mathrm{independent}}=
\begin{bmatrix}
B_{\mathrm{park}}&0\\
0&B_{\mathrm{exit}}
\end{bmatrix}.

其中 $B_{\mathrm{park}}$、B_{\mathrm{exit}} 内部可以是带状的，并不要求是对角的。

同样地：

• 曲率与曲率变化率上界只约束各自路径；
• 泊入走廊约束只作用于泊入路径；
• 驶离走廊约束只作用于驶离路径；
• $J_\ell$、J_U 和 J_\kappa 都可以分别写成泊入部分与驶离部分之和。

所以，在不考虑 J_\varPsi 时，两条路径的数值结构基本可以分开处理。

7. 关于装载位约束的一处重要问题

第 843 行称：“唯一的另一处跨路径耦合来自装载位处共享同一位姿”。结合当前论文第 335--347 行的定义，这句话并不严格准确。

当前边界条件写成

\boldsymbol\eta^{\mathrm{park}}(s_{\mathrm{park}})
=\boldsymbol\eta_{\mathrm{load}},

\boldsymbol\eta^{\mathrm{exit}}(0)
=\boldsymbol\eta_{\mathrm{load}},

而正文明确说明 \boldsymbol\eta_{\mathrm{load}} 是由上层系统给出的固定值。若它确实是常量，那么这只是两组分别等于同一常量的边界约束：

h_{\mathrm{park}}(\mathbf z_{\mathrm{park}})=0,
\qquad
h_{\mathrm{exit}}(\mathbf z_{\mathrm{exit}})=0.

第一组不含 $\mathbf z_{\mathrm{exit}}$，第二组不含 $\mathbf z_{\mathrm{park}}$，所以它们不会在 Jacobian 或 Hessian 中产生跨路径非零块。两条路径在几何上共享同一个已知端点，但在导数结构上没有直接耦合。

在当前问题定义下，固定的 \boldsymbol\eta_{\mathrm{load}} 不会形成跨路径导数耦合。只有在另外重新定义约束时，才可能形成真正的跨路径约束，例如：

1. 直接施加

   
   \boldsymbol\eta^{\mathrm{park}}(s_{\mathrm{park}})
   -\boldsymbol\eta^{\mathrm{exit}}(0)=0;
   

2. 由上层模型额外引入一个待优化的共享位姿变量，并让两条路径的边界条件同时依赖该变量。这个做法已经改变了当前论文中“装载位姿由上层系统给定”的问题定义。

因此，按照论文当前“装载位姿固定”的问题定义，真正的跨路径导数耦合只来自 $J_\varPsi$。第 843 行建议相应修改。

8. 可微性表述还需要哪些条件

第 841 行称 J_\varPsi 对 \mathbf z 整体可微，这需要附加条件：

• 当两个路径点完全重合，即 d_{ij}=0 时，欧氏范数 \|\mathbf r_{ij}\| 的梯度没有唯一方向；当前核函数在 d=0 处的径向导数通常又不为零，因此复合函数在重合点处不可微。
• 在截断点 d=\rho_\varPsi 处，若 $m_\varPsi>1$，一阶导数可连续衔接到零；若还希望二阶导数连续，通常需要 $m_\varPsi>2$。

这不一定造成实际求解失败，因为车辆路径点通常不会精确重合，且参数可以选择足够高的 $m_\varPsi$。但论文若要声称“整体可微”，最好明确参数条件，并对 d=0 使用平滑距离，例如

d_\varepsilon=\sqrt{\|\mathbf r\|^2+\varepsilon^2}.

9. 用一句话重新理解第 841--843 行

这几行的核心意思是：

大部分目标项和约束都只在各自路径内部产生带状导数结构；交互项 J_\varPsi 同时依赖两条路径，因此能在一次 NLP 求解中推动双方分离。由于核函数只作用于距离小于 \rho_\varPsi 的点对，交互项产生的跨路径 Hessian 块通常是稀疏的。

按照当前“装载位姿为固定常量”的定义，不应再把装载位边界条件称为跨路径导数耦合。

10. 建议替换原文的表述

可以将第 841--843 行改写为：

式 (joint_psi_discrete) 同时依赖泊入与驶离路径的位置变量，故其关于两组变量的梯度均非零，两条路径可在同一次求解中相互推离。由有限支撑核的定义，仅当配置点对的距离小于 \rho_\varPsi 时，该点对才对梯度及 Hessian 产生贡献；因此，交互项所形成的跨路径 Hessian 块仅在空间上相近的配置点对处非零。除 J_\varPsi 外，其余目标项与约束均只作用于单条路径内部：配置约束在各自路径内耦合相邻配置点，形成带状结构；物理上界及走廊约束也不连接两条路径。由于 \boldsymbol\eta_\mathrm{load} 是给定常量，两条路径分别等于该位姿的边界条件不产生跨路径导数耦合。因此，联合问题仅通过稀疏的 J_\varPsi 块发生跨路径耦合。