trey8698/essay-mineplanning
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

迁移到dhxdl

Browse Source
This commit is contained in:
王闯
2026-08-24 17:26:17 +08:00
commit 5d5507bd28
29 changed files with 10894 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
explanation_lines_841_843.md
+559
View File
@@ -0,0 +1,559 @@
# `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$ 块发生跨路径耦合。