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
figs/__pycache__/sitegeom.cpython-313.pyc
BIN
View File
Binary file not shown.
figs/calc_convex.py
+176
View File
@@ -0,0 +1,176 @@
#!/usr/bin/env python3
"""Compute exact coordinates for the convex-decomposition schematic.
Follows supplement.tex S-IV verbatim:
eq:sup_ellipsoid_quadratic E = { p | (p-d)^T (C C^T)^{-1} (p-d) <= 1 },
o_i not in int(E)
eq:sup_halfspace_normal atil_i = (C C^T)^{-1} (o_i - d), a_i = atil/|atil|,
b_i = a_i^T o_i
Construction: d is the segment midpoint, the major axis is the segment
direction with semi-axis equal to the segment half-length, and the minor
semi-axis is shrunk until the boundary first touches an obstacle point.
Redundant obstacles are pruned greedily: after each half-space is emitted,
every obstacle it already excludes is dropped.
Prints TikZ-ready numbers; nothing is drawn here.
"""
import numpy as np
# --- segment (figure units = cm in the TikZ picture) ------------------------
P0 = np.array([0.0, 0.0])
P1 = np.array([5.2, 0.0])
D = 0.5 * (P0 + P1)
A_AX = 0.5 * np.linalg.norm(P1 - P0) # semi-major, along the segment
# --- obstacle points (site-boundary / ore-pile samples) --------------------
OBST = np.array([
[0.30, 1.42], [1.35, 1.05], [2.55, 1.28], [3.75, 1.62], [4.85, 1.35],
[0.65, -1.30], [1.90, -1.62], [3.10, -1.12], [4.35, -1.45], [5.35, -1.02],
[-0.55, 0.85], [5.75, 0.62],
# points beyond the segment ends: they do not constrain the minor axis
# (|u| > 1) but they do cap the polytope longitudinally
[-1.05, 0.12], [6.35, -0.18],
])
def main():
# ---- minor semi-axis: shrink until the boundary touches an obstacle ----
u = (OBST[:, 0] - D[0]) / A_AX
live = np.abs(u) < 1.0 - 1e-9
b_req = np.abs(OBST[live, 1]) / np.sqrt(1.0 - u[live] ** 2)
B_AX = float(b_req.min())
touch = int(np.nonzero(live)[0][int(np.argmin(b_req))])
print("semi-axes: a = %.4f, b = %.4f center d = (%.3f, %.3f)"
% (A_AX, B_AX, D[0], D[1]))
print("touching obstacle index %d at (%.3f, %.3f)"
% (touch, OBST[touch, 0], OBST[touch, 1]))
C = np.diag([A_AX, B_AX])
Minv = np.linalg.inv(C @ C.T)
def qform(P):
dv = P - D
return np.einsum("ij,jk,ik->i", dv, Minv, dv)
q = qform(OBST)
print("min obstacle quadratic form = %.6f (must be >= 1)" % q.min())
assert q.min() >= 1.0 - 1e-9, "eq:sup_ellipsoid_quadratic violated"
# the segment must lie inside the ellipsoid
seg = P0 + np.linspace(0, 1, 201)[:, None] * (P1 - P0)
print("max segment quadratic form = %.6f (must be <= 1)" % qform(seg).max())
assert qform(seg).max() <= 1.0 + 1e-9
# ---- greedy half-space generation with redundancy pruning --------------
remaining = list(range(len(OBST)))
planes = []
while remaining:
# process the obstacle closest in the ellipsoid metric first
qi = qform(OBST[remaining])
k = remaining[int(np.argmin(qi))]
o = OBST[k]
at = Minv @ (o - D)
a = at / np.linalg.norm(at)
b = float(a @ o)
planes.append((k, a, b))
# the ellipsoid (hence the segment) must be on the safe side
sup = float(a @ D + np.sqrt((a @ (C @ C.T)) @ a))
assert sup <= b + 1e-9, "ellipsoid crosses the half-space"
remaining = [j for j in remaining if float(a @ OBST[j]) < b - 1e-9]
print("\n%d half-spaces retained out of %d obstacles"
% (len(planes), len(OBST)))
for k, a, b in planes:
print(" o_%-2d (%6.3f,%6.3f) a = (%7.4f,%7.4f) b = %7.4f"
% (k, OBST[k, 0], OBST[k, 1], a[0], a[1], b))
# ---- polytope vertices, for drawing ------------------------------------
A = np.array([p[1] for p in planes])
bb = np.array([p[2] for p in planes])
verts = []
for i in range(len(planes)):
for j in range(i + 1, len(planes)):
M = np.array([A[i], A[j]])
if abs(np.linalg.det(M)) < 1e-9:
continue
v = np.linalg.solve(M, [bb[i], bb[j]])
if np.all(A @ v <= bb + 1e-7):
verts.append(v)
V = np.array(verts)
ctr = V.mean(axis=0)
V = V[np.argsort(np.arctan2(V[:, 1] - ctr[1], V[:, 0] - ctr[0]))]
print("\npolytope P: %d vertices" % len(V))
print(" tikz path: " + " -- ".join("(%.3f,%.3f)" % (x, y) for x, y in V)
+ " -- cycle")
# ---- ellipse as a TikZ primitive ---------------------------------------
print("\n tikz ellipse: (%.3f,%.3f) ellipse [x radius=%.4f, "
"y radius=%.4f]" % (D[0], D[1], A_AX, B_AX))
print(" tikz obstacles: " + " ".join("(%.2f,%.2f)" % (x, y)
for x, y in OBST))
kept = " ".join("o%d" % p[0] for p in planes)
print(" retained: " + kept)
# ---- support points: where each half-space touches the ellipsoid -------
# The boundary point whose outward normal is parallel to a_i is
# p = d + (C C^T) a / sqrt(a^T (C C^T) a).
M = C @ C.T
print("\n ellipsoid support points (tangency anchors of a_i):")
for k, a, b in planes:
p = D + (M @ a) / np.sqrt(a @ M @ a)
assert abs(qform(p[None, :])[0] - 1.0) < 1e-9, "anchor off the boundary"
print(" o_%-2d: anchor (%.3f,%.3f) gap to o_i = %.3f"
% (k, p[0], p[1], float(a @ OBST[k]) - float(a @ p)))
# ---- pruned (redundant) obstacles, drawn hollow in the figure ----------
kept_idx = {p[0] for p in planes}
pruned = [i for i in range(len(OBST)) if i not in kept_idx]
print("\n pruned as redundant: " +
" ".join("(%.2f,%.2f)" % (OBST[i, 0], OBST[i, 1]) for i in pruned))
print(" retained points: " +
" ".join("(%.2f,%.2f)" % (OBST[p[0], 0], OBST[p[0], 1])
for p in planes))
# ---- initial sphere before the minor-axis shrink ------------------------
inside_sphere = [i for i in range(len(OBST))
if np.linalg.norm(OBST[i] - D) < A_AX]
print("\n initial sphere radius %.3f encloses %d obstacle(s)"
% (A_AX, len(inside_sphere)))
# ---- ready-to-paste TikZ for the half-space boundaries -----------------
# Each line a^T p = b is drawn through a*b along t = (-a_y, a_x) and left
# for the picture's own \clip to trim.
print("\n TikZ half-space boundaries (draw long, clip in the picture):")
for k, a, b in planes:
p_on = a * b
t = np.array([-a[1], a[0]])
q0, q1 = p_on - 9.0 * t, p_on + 9.0 * t
print(" \\draw[hsp] (%.3f,%.3f) -- (%.3f,%.3f); %% o_%d"
% (q0[0], q0[1], q1[0], q1[1], k))
# ---- half-space boundary segments clipped to the drawing box ----------
print("\n half-space boundary lines (clipped to x in [-1.2, 6.4]):")
for k, a, b in planes:
# parameterize the line a^T p = b
t = np.array([-a[1], a[0]])
p_on = a * b # closest point of the line to the origin
ts = []
for xlim in (-1.2, 6.4):
if abs(t[0]) > 1e-9:
ts.append((xlim - p_on[0]) / t[0])
for ylim in (-2.3, 2.3):
if abs(t[1]) > 1e-9:
ts.append((ylim - p_on[1]) / t[1])
pts = [p_on + tv * t for tv in ts]
pts = [p for p in pts
if -1.2 - 1e-6 <= p[0] <= 6.4 + 1e-6
and -2.3 - 1e-6 <= p[1] <= 2.3 + 1e-6]
if len(pts) >= 2:
pts = sorted(pts, key=lambda p: (p[0], p[1]))
print(" o_%-2d: (%.3f,%.3f) -- (%.3f,%.3f)"
% (k, pts[0][0], pts[0][1], pts[-1][0], pts[-1][1]))
if __name__ == "__main__":
main()
figs/fig_bicycle.pdf
BIN
View File
Binary file not shown.
figs/fig_bicycle.tex
+79
View File
@@ -0,0 +1,79 @@
% Kinematic bicycle model and geometric parameters of the mining truck.
% Drawn to scale in metres with L_v = 12, B_v = 3, L_w = 6.5, rear overhang 1.5,
% steering at psi_max = arctan(L_w / R_min) so that the ICR sits at R_min = 12 m.
\documentclass[border=1pt]{standalone}
\input{figstyle}
\begin{document}
\begin{tikzpicture}[scale=0.53]
% ---------- global frame ------------------------------------------------
\draw[->, thin@] (-4.8,-2.8) -- (-2.6,-2.8) node[ann, right] {$x$};
\draw[->, thin@] (-4.8,-2.8) -- (-4.8,-0.6) node[ann, above] {$y$};
% horizontal reference through the rear-axle centre, for the heading angle
\draw[thin@, sitec, dash pattern=on 1.6pt off 1.2pt] (0,0) -- (5.6,0);
\draw[thin@] (4.2,0) arc[start angle=0, end angle=18, radius=4.2];
\node[ann] at (5.05,0.72) {$\theta$};
\begin{scope}[rotate=18]
% ---------- vehicle body ---------------------------------------------
\draw[veh, sitec, fill=fillc, fill opacity=0.6]
(-1.5,-1.5) rectangle (10.5,1.5);
\draw[thin@, sitec, dash pattern=on 1.4pt off 1pt] (-1.5,0) -- (11.6,0);
% ---------- wheels ----------------------------------------------------
% rear wheels, aligned with the body
\foreach \sy in {-1.5,1.5}{
\draw[veh, fill=sitec] (-0.9,\sy-0.22) rectangle (0.9,\sy+0.22);
}
% front wheels, steered by psi_max = 28 deg
\foreach \sy in {-1.5,1.5}{
\begin{scope}[shift={(6.5,\sy)}, rotate=28]
\draw[veh, fill=parkc, fill opacity=0.85]
(-0.9,-0.22) rectangle (0.9,0.22);
\end{scope}
}
% ---------- steering angle -------------------------------------------
\draw[thin@, parkc] (6.5,0) -- ++(28:3.1);
\draw[thin@] (8.7,0) arc[start angle=0, end angle=28, radius=2.2];
\node[ann] at (9.35,0.66) {$\psi$};
\node[annt, parkc] at (9.9,1.95) {$\dot\psi$};
\draw[->, thin@, parkc] (9.05,1.55) arc[start angle=32, end angle=52,
radius=3.0];
% ---------- instantaneous centre of rotation --------------------------
\draw[thin@, sitec, dash pattern=on 1.6pt off 1.2pt] (0,0) -- (0,12.2);
\draw[thin@, sitec, dash pattern=on 1.6pt off 1.2pt] (6.5,0) -- (0,12.2);
\fill[sitec] (0,12.2) circle (0.22);
\node[ann, above] at (0,12.45) {ICR};
\node[ann, left] at (-0.25,7.0) {$R_\text{min}=1/\kappa_\text{max}$};
% ---------- rear-axle centre = path reference point -------------------
\fill[parkc] (0,0) circle (0.26);
\node[ann, parkc] at (-2.35,-1.05)
{$\mathbf{p}=[x,y]^\top$};
% ---------- dimensions -----------------------------------------------
% wheelbase
\draw[dimline] (0,-0.75) -- (6.5,-0.75);
\node[annt, fill=fillc, inner sep=0.4pt] at (3.25,-0.75) {$L_\text{w}$};
% overall length
\draw[dimline] (-1.5,-2.55) -- (10.5,-2.55);
\node[annt, fill=white, inner sep=0.4pt] at (4.5,-2.55) {$L_\text{v}$};
\draw[thin@] (-1.5,-1.6) -- (-1.5,-2.75);
\draw[thin@] (10.5,-1.6) -- (10.5,-2.75);
% width
\draw[dimline] (-2.9,-1.5) -- (-2.9,1.5);
\node[annt, rotate=90, fill=white, inner sep=0.4pt]
at (-2.9,0) {$B_\text{v}$};
\draw[thin@] (-1.65,-1.5) -- (-3.1,-1.5);
\draw[thin@] (-1.65,1.5) -- (-3.1,1.5);
% swept rectangle annotation
\node[annt, sitec] at (7.4,-0.95) {$\mathcal{V}(\boldsymbol{\eta})$};
\end{scope}
\end{tikzpicture}
\end{document}
figs/fig_center.pdf
BIN
View File
Binary file not shown.
figs/fig_conflict.pdf
BIN
View File
Binary file not shown.
figs/fig_conflict.tex
+118
View File
@@ -0,0 +1,118 @@
% Spatiotemporal conflict and its resolution by spatial decoupling.
% (a)(c) coupled single-channel paths -> the empty truck must wait;
% (b)(d) decoupled dual-channel paths -> both maneuvers proceed in parallel.
\documentclass[border=1pt]{standalone}
\input{figstyle}
\begin{document}
\begin{tikzpicture}[scale=0.0872]
% ======================================================================
% geometry shared by the two spatial panels
% ======================================================================
\def\sitebnd{
(2,6) -- (14,3.5) -- (30,3) -- (40,6) -- (45,13) -- (41,22)
-- (28,25) -- (14,22) -- (2,17)
}
% channel spine from the entrance to the loading bay
\def\spine{(2,11.5) .. controls (14,10.5) and (24,11.5) .. (34,15.5)}
% ---------------------------------------------------------------- panel a
\begin{scope}[shift={(0,0)}]
\fill[fillc, opacity=0.55] \sitebnd -- cycle;
\fill[white] \sitebnd -- cycle;
\draw[siteline] \sitebnd;
% conflict zone: both paths occupy the same channel
\fill[confc, opacity=0.16]
(4,8.6) .. controls (14,7.6) and (24,8.6) .. (33,12.6)
-- (35,18.4) .. controls (24,14.4) and (14,13.4) .. (4,14.4) -- cycle;
% the two paths coincide on the spine
\draw[parkpath] \spine;
\draw[exitpath, dash pattern=on 2.4pt off 1.6pt]
(34,15.5) .. controls (24,11.5) and (14,10.5) .. (2,11.5);
% loading pose
\fill[sitec] (34,15.5) circle (0.7);
\node[annt, sitec, above right] at (34.2,15.9) {$\boldsymbol{\eta}_\text{load}$};
\node[annt, confc] at (18,17.4) {conflict zone};
\node[panel] at (23.5,-1.8) {(a) coupled};
\end{scope}
% ---------------------------------------------------------------- panel b
\begin{scope}[shift={(52,0)}]
\fill[fillc, opacity=0.55] \sitebnd -- cycle;
\fill[white] \sitebnd -- cycle;
\draw[siteline] \sitebnd;
% virtual centerline and the two offset guidance lines
\draw[ctrline] \spine;
\draw[parkpath] (2,8.6) .. controls (14,7.6) and (24,8.6) .. (33.4,13.0);
\draw[exitpath] (34.6,18.0) .. controls (24,14.4) and (14,13.4) .. (2,14.4);
% channel width W between the two guidance lines
\draw[dimline] (16,8.0) -- (16,13.8);
\node[annt, fill=white, inner sep=0.3pt] at (16,10.9) {$W$};
\fill[sitec] (34,15.5) circle (0.7);
\node[annt, sitec, above right] at (34.2,15.9) {$\boldsymbol{\eta}_\text{load}$};
\node[annt, centc] at (7.5,19.3) {$\varLambda_\text{center}$};
\node[panel] at (23.5,-1.8) {(b) decoupled};
\end{scope}
% ======================================================================
% time-line panels
% ======================================================================
\def\tlen{40}
% ---------------------------------------------------------------- panel c
\begin{scope}[shift={(0,-24)}]
\draw[->, thin@] (2,0) -- (2+\tlen+3,0) node[annt, right] {$t$};
\draw[thin@] (2,0) -- (2,10.5);
% loaded truck leaves
\fill[exitc, opacity=0.75] (4,6.2) rectangle (18,8.6);
\node[annt, white] at (11,7.4) {outbound};
% empty truck waits, then parks
\fill[sitec, opacity=0.28] (4,2.6) rectangle (18,5.0);
\node[annt, sitec] at (11,3.8) {waiting};
\fill[parkc, opacity=0.75] (18,2.6) rectangle (32,5.0);
\node[annt, white] at (25,3.8) {inbound};
% excavator idle window
\draw[decorate, decoration={brace, amplitude=1.6pt, raise=0.6pt},
line width=0.4pt, confc] (18,1.9) -- (4,1.9);
\node[annt, confc] at (11,-1.2) {$T_\text{wait}>0$};
\node[annt, exitc, left] at (3.6,7.4) {L};
\node[annt, parkc, left] at (3.6,3.8) {E};
\node[panel] at (23.5,-5.4) {(c) coupled timing};
\end{scope}
% ---------------------------------------------------------------- panel d
\begin{scope}[shift={(52,-24)}]
\draw[->, thin@] (2,0) -- (2+\tlen+3,0) node[annt, right] {$t$};
\draw[thin@] (2,0) -- (2,10.5);
\fill[exitc, opacity=0.75] (4,6.2) rectangle (18,8.6);
\node[annt, white] at (11,7.4) {outbound};
\fill[parkc, opacity=0.75] (4,2.6) rectangle (18,5.0);
\node[annt, white] at (11,3.8) {inbound};
\draw[decorate, decoration={brace, amplitude=1.6pt, raise=0.6pt},
line width=0.4pt, corrc] (18,1.9) -- (4,1.9);
\node[annt, corrc] at (11,-1.2) {in parallel};
% the cycle ends earlier by the eliminated waiting time
\draw[thin@, sitec, dash pattern=on 1.4pt off 1.2pt] (32,1.9) -- (32,9.4);
\draw[dimline, confc] (18,9.8) -- (32,9.8);
\node[annt, confc, fill=white, inner sep=0.3pt] at (25,9.8) {saved};
\node[annt, exitc, left] at (3.6,7.4) {L};
\node[annt, parkc, left] at (3.6,3.8) {E};
\node[panel] at (23.5,-5.4) {(d) decoupled timing};
\end{scope}
\end{tikzpicture}
\end{document}
figs/fig_convex_decomp.pdf
BIN
View File
Binary file not shown.
figs/fig_convex_decomp.tex
+100
View File
@@ -0,0 +1,100 @@
% Geometric intuition of the convex decomposition (supplement S-IV).
% All coordinates are computed by calc_convex.py from
% eq:sup_ellipsoid_quadratic and eq:sup_halfspace_normal
% and pasted here verbatim; do not hand-edit the numbers.
\documentclass[tikz,border=1pt]{standalone}
\input{figstyle}
\tikzset{
obst/.style={circle, fill=sitec, inner sep=0pt, minimum size=1.7pt},
obstx/.style={circle, draw=sitec, line width=0.3pt, fill=white,
inner sep=0pt, minimum size=1.7pt},
hsp/.style={corrc, line width=0.35pt, dash pattern=on 1.4pt off 1.0pt},
ell/.style={parkc, line width=0.6pt},
sph/.style={sitec, line width=0.35pt, dash pattern=on 1.0pt off 1.0pt},
segl/.style={exitc, line width=1.0pt},
}
\newcommand{\allobst}{%
\foreach \x/\y in {0.30/1.42, 3.75/1.62, 4.85/1.35, 1.90/-1.62,
4.35/-1.45, 5.35/-1.02, -0.55/0.85}
{\node[obst] at (\x,\y) {};}%
\foreach \x/\y in {3.10/-1.12, 1.35/1.05, 2.55/1.28, 5.75/0.62,
0.65/-1.30, -1.05/0.12, 6.35/-0.18}
{\node[obst] at (\x,\y) {};}%
}
\begin{document}
\begin{tikzpicture}[scale=0.50]
% ============================ panel (a) ==================================
\begin{scope}
\begin{scope}
\clip (-1.45,-2.80) rectangle (6.65,2.80);
% initial sphere: radius = segment half-length, before the minor-axis shrink
\draw[sph] (2.600,0) circle [radius=2.6000];
% collision-free ellipsoid after shrinking along the minor axis
\draw[ell, fill=parkc, fill opacity=0.10] (2.600,0)
ellipse [x radius=2.6000, y radius=1.1413];
\end{scope}
% shrink direction along the minor axis
\foreach \s in {1,-1}
{\draw[line width=0.35pt, ->, >={Latex[length=1.0mm,width=0.8mm]}]
(2.600,\s*2.45) -- (2.600,\s*1.30);}
% the path segment, fully enclosed by the ellipsoid
\draw[segl] (0,0) -- (5.200,0);
\node[obst, fill=exitc] at (0,0) {};
\node[obst, fill=exitc] at (5.200,0) {};
\node[obst, fill=black] at (2.600,0) {};
\allobst
% the obstacle that stops the shrink lies exactly on the boundary
\draw[confc, line width=0.45pt] (3.100,-1.120) circle [radius=0.19];
\node[annt, anchor=north] at (0.00,-0.10) {$\mathbf{p}_0$};
\node[annt, anchor=north] at (5.20,-0.10) {$\mathbf{p}_1$};
\node[annt, anchor=south west] at (2.62,0.04) {$\mathbf{d}$};
\node[annt, parkc, anchor=south] at (2.60,1.20) {$\mathcal{E}$};
\node[annt, confc, anchor=north west] at (3.28,-1.22) {$\mathbf{o}_i$};
\node[annt, sitec, anchor=south east] at (4.60,1.95) {initial sphere};
\node[panel, anchor=north] at (2.60,-2.95) {(a)};
\end{scope}
% ============================ panel (b) ==================================
\begin{scope}[shift={(9.4,0)}]
% convex polytope P = { p | A p <= b }, 7 retained half-spaces
\fill[corrc, fill opacity=0.13]
(-1.201,-0.765) -- (0.732,-1.324) -- (6.182,-0.855) --
(6.393,-0.009) -- (5.057,1.299) -- (2.346,1.278) --
(-0.983,0.515) -- cycle;
\draw[corrc, line width=0.6pt]
(-1.201,-0.765) -- (0.732,-1.324) -- (6.182,-0.855) --
(6.393,-0.009) -- (5.057,1.299) -- (2.346,1.278) --
(-0.983,0.515) -- cycle;
\begin{scope}
\clip (-1.45,-2.80) rectangle (6.65,2.80);
% supporting half-space boundaries, tangent to the ellipsoid metric
\draw[hsp] (-8.848,-2.148) -- (9.085,-0.605); % o_7
\draw[hsp] (8.611,2.716) -- (-8.933,-1.309); % o_1
\draw[hsp] (8.990,1.328) -- (-9.009,1.193); % o_2
\draw[hsp] (9.555,-3.105) -- (-3.307,9.487); % o_11
\draw[hsp] (-8.943,1.473) -- (8.349,-3.525); % o_5
\draw[hsp] (0.474,9.049) -- (-2.554,-8.694); % o_12
\draw[hsp] (3.846,-10.233) -- (8.197,7.233); % o_13
\draw[ell, dash pattern=on 1.2pt off 1.0pt] (2.600,0)
ellipse [x radius=2.6000, y radius=1.1413];
\end{scope}
\draw[segl] (0,0) -- (5.200,0);
% retained obstacles: solid; pruned as redundant: hollow
\foreach \x/\y in {0.30/1.42, 3.75/1.62, 4.85/1.35, 1.90/-1.62,
4.35/-1.45, 5.35/-1.02, -0.55/0.85}
{\node[obstx] at (\x,\y) {};}
\foreach \x/\y in {3.10/-1.12, 1.35/1.05, 2.55/1.28, 5.75/0.62,
0.65/-1.30, -1.05/0.12, 6.35/-0.18}
{\node[obst] at (\x,\y) {};}
% one normal drawn from its tangency anchor to the generating obstacle
\draw[line width=0.35pt, ->, >={Latex[length=1.0mm,width=0.8mm]}]
(2.555,1.141) -- (2.55,1.28);
\node[annt, anchor=west] at (2.70,1.62)
{$\mathbf{a}_i^{\!\top}\mathbf{p}=b_i$};
\node[annt, corrc, anchor=north east] at (6.30,-0.95) {$\mathcal{P}$};
\node[panel, anchor=north] at (2.60,-2.95) {(b)};
\end{scope}
\end{tikzpicture}
\end{document}
figs/fig_framework.pdf
BIN
View File
Binary file not shown.
figs/fig_framework.tex
+92
View File
@@ -0,0 +1,92 @@
% Overall three-stage framework: offline field prior -> parallel coarse search
% -> joint numerical optimization. Laid out to fit one IEEE column (252 pt)
% at full size, so no scaling is applied when included. Section numbers are
% plain text so the standalone figure compiles independently of main.tex.
\documentclass[border=1pt]{standalone}
\input{figstyle}
\begin{document}
\begin{tikzpicture}[
blk/.style={draw=sitec, line width=0.5pt, rounded corners=1.2pt,
align=center, inner sep=2.2pt, font=\tiny,
text width=23mm, minimum height=5.4mm},
io/.style={blk, fill=fillc, text width=20mm},
offb/.style={blk, fill=centc!10, draw=centc!70},
parkb/.style={blk, fill=parkc!10, draw=parkc!70, text width=21mm},
exitb/.style={blk, fill=exitc!10, draw=exitc!70, text width=21mm},
optb/.style={blk, fill=corrc!10, draw=corrc!70, text width=52mm},
stage/.style={draw=sitec!45, line width=0.4pt,
dash pattern=on 1.4pt off 1.2pt, rounded corners=2pt},
lbl/.style={font=\tiny\itshape, sitec},
ar/.style={->, line width=0.45pt, sitec,
>={Latex[length=1.3mm,width=1.0mm]}},
note/.style={font=\tiny, sitec, align=left},
]
% =====================================================================
% Stage 1 -- offline construction of the guidance-field prior
% =====================================================================
\node[io] (bnd) at (0,0) {site boundary point cloud};
\node[offb] (skel) at (0,-0.98) {EVG skeleton $+$ QP smoothing
$\rightarrow \varLambda_\text{center}$};
\node[offb] (guid) at (0,-1.96) {normal offset $\pm W/2$
$\rightarrow \varLambda_\text{park},
\varLambda_\text{exit}$};
\node[offb] (fld) at (0,-3.05) {kernel superposition
$\rightarrow U_\text{park}, U_\text{exit}$
(mirrored about $\varLambda_\text{center}$)};
\foreach \a/\b in {bnd/skel, skel/guid, guid/fld}{ \draw[ar] (\a) -- (\b); }
\node[stage, fit=(bnd)(fld), inner sep=3pt] (s1) {};
\node[lbl, above=0.8pt of s1.north] {Stage 1: offline (Sec.~III)};
% =====================================================================
% Stage 2 -- two coarse paths searched independently and in parallel
% =====================================================================
\node[parkb] (hap) at (3.72,-0.82) {field-adaptive Hybrid A*
on $U_\text{park}$};
\node[exitb] (hax) at (3.72,-2.24) {field-adaptive Hybrid A*
on $U_\text{exit}$};
\node[stage, fit=(hap)(hax), inner sep=3pt] (s2) {};
\node[lbl, above=0.8pt of s2.north, align=center]
{Stage 2: online, parallel (Sec.~IV)};
\draw[ar] (fld.east) -- ++(0.22,0) |- (hap.west);
\draw[ar] (fld.east) -- ++(0.22,0) |- (hax.west);
% =====================================================================
% Stage 3 -- joint optimization of the two coarse paths
% =====================================================================
\node[optb] (corr) at (1.86,-4.35)
{convex corridors $\mathcal{P}_k$ from the perception snapshot
(ellipsoid proxy, inset by $\Delta_\text{cut}$)};
\node[optb] (nlp) at (1.86,-5.45)
{joint NLP:
$\mathbf{z}=[\mathbf{z}_\text{park};\mathbf{z}_\text{exit}]$,
interaction cost $J_\varPsi$, solved by Ipopt};
\node[io, text width=28mm] (out) at (1.86,-6.45)
{$\varGamma_\text{park}^\star,\ \varGamma_\text{exit}^\star$};
\node[stage, fit=(corr)(out), inner sep=3pt] (s3) {};
\node[lbl, below=0.8pt of s3.south]
{Stage 3: joint optimization (Sec.~V)};
\draw[ar] (hap.east) -- ++(0.20,0) |- ([yshift=1.1mm]corr.east);
\draw[ar] (hax.south) -- ++(0,-0.30) -| ([xshift=8mm]corr.north);
\draw[ar] (corr) -- (nlp);
\draw[ar] (nlp) -- (out);
% ---------- what each stage fixes ------------------------------------
\node[note] at (0.30,-3.72) {\textbf{fixes} topology};
\node[note] at (3.30,-3.00) {\textbf{fixes} $N,\{\delta_j\}$};
\node[note] at (-0.62,-5.45) {\textbf{fixes}\\shape,\\$\kappa$ cont.};
% ---------- rebuild trigger -------------------------------------------
\draw[ar, densely dotted, centc, >={Latex[length=1.3mm,width=1.0mm]}]
(out.west) -- ++(-0.42,0) |- (skel.west);
\node[font=\tiny, centc, align=center] at (-1.32,-2.55)
{rebuild only\\on boundary\\change};
\end{tikzpicture}
\end{document}
figs/fig_igf.pdf
BIN
View File
Binary file not shown.
figs/fig_probe.pdf
BIN
View File
Binary file not shown.
figs/fig_probe.tex
+118
View File
@@ -0,0 +1,118 @@
% Field-adaptive expansion step.
% (a) the six probe poses D(eta_i) reached with psi in {-psi_max, 0, +psi_max}
% and delta in {+1,-1} at the baseline step mu_0;
% (b) the resulting per-direction steps mu_j^delta after modulation.
%
% All arcs are drawn with the true geometry of the paper's numbers:
% L_w = 6.5 m, R_min = 12 m, psi_max = arctan(L_w/R_min) = 28.44 deg,
% so a constant-psi_max arc of length mu turns mu/R_min rad.
% mu_0 = 4 m -> 19.10 deg; modulated steps use
% mu = mu_0 [w_min + (w_max-w_min)(1-Utilde)], [w_min,w_max] = [0.6,1.4].
\documentclass[border=1pt]{standalone}
\input{figstyle}
\begin{document}
\begin{tikzpicture}[
scale=0.30,
probe/.style={line width=0.7pt},
pnode/.style={circle, draw, line width=0.4pt, inner sep=0pt,
minimum size=1.5mm},
lo/.style={corrc}, % low potential -> long step
hi/.style={confc}, % high potential -> short step
panel/.style={font=\footnotesize},
]
% =====================================================================
% (a) six probe poses at the baseline step mu_0
% =====================================================================
\begin{scope}
% boundary that the vehicle is heading towards (source of high U)
\draw[siteline] (-7.4,7.0) -- (9.0,7.0);
\draw[pattern=north east lines, pattern color=sitec!45, draw=none]
(-7.4,7.0) rectangle (9.0,8.6);
\node[annt, sitec] at (6.2,7.9) {$\mathcal{S}_\text{e}$};
% current node eta_i
\fill[parkc] (0,0) circle (0.30);
\node[annt, parkc, below left] at (0.15,-0.25) {$\boldsymbol{\eta}_i$};
\draw[->, thin@, parkc] (0,0) -- (2.1,0);
\node[annt, parkc] at (2.55,-0.55) {$\theta$};
% --- forward arcs (delta = +1), sweep 19.0986 deg over mu_0 = 4 m ---
% left: centre at (0, +R), arc from angle -90 to -90+19.0986
\draw[probe, hi] (0,0) arc[start angle=-90, end angle=-70.9014, radius=12];
% straight
\draw[probe, sitec] (0,0) -- (4,0);
% right: centre at (0, -R)
\draw[probe, sitec] (0,0) arc[start angle=90, end angle=70.9014, radius=12];
% --- reverse arcs (delta = -1) --------------------------------------
\draw[probe, lo] (0,0) arc[start angle=-90, end angle=-109.0986, radius=12];
\draw[probe, lo] (0,0) -- (-4,0);
\draw[probe, sitec] (0,0) arc[start angle=90, end angle=109.0986, radius=12];
% --- probe nodes (computed endpoints) -------------------------------
\node[pnode, hi, fill=confc!25] at ( 3.927, 0.659) {};
\node[pnode, sitec, fill=fillc] at ( 4.000, 0.000) {};
\node[pnode, sitec, fill=fillc] at ( 3.927,-0.659) {};
\node[pnode, lo, fill=corrc!25] at (-3.927, 0.659) {};
\node[pnode, lo, fill=corrc!25] at (-4.000, 0.000) {};
\node[pnode, sitec, fill=fillc] at (-3.927,-0.659) {};
\node[annt, hi] at (6.3,1.9) {high $U$};
\node[annt, lo] at (-6.4,2.2) {low $U$};
% baseline step annotation
\draw[dimline] (0,-2.5) -- (4,-2.5);
\node[annt, fill=white, inner sep=0.3pt] at (2,-2.5) {$\mu_0$};
\node[annt, sitec] at (0,-4.6)
{$\psi\in\{-\psi_\text{max},0,\psi_\text{max}\},\ \delta=\pm1$};
\node[panel] at (0,-6.6) {(a) six probes $\mathcal{D}(\boldsymbol{\eta}_i)$};
\end{scope}
% =====================================================================
% (b) modulated steps -- same six directions, lengths from eq (step)
% =====================================================================
\begin{scope}[shift={(0,-17.5)}]
\draw[siteline] (-8.6,7.0) -- (9.0,7.0);
\draw[pattern=north east lines, pattern color=sitec!45, draw=none]
(-8.6,7.0) rectangle (9.0,8.6);
\fill[parkc] (0,0) circle (0.30);
\node[annt, parkc, below left] at (0.15,-0.25) {$\boldsymbol{\eta}_i$};
% fwd-left : Utilde = 1.00 -> w = 0.60, mu = 2.400 m, sweep 11.4592 deg
\draw[probe, hi] (0,0) arc[start angle=-90, end angle=-78.5408, radius=12];
\node[pnode, hi, fill=confc!25] at (2.383,0.238) {};
% fwd-straight : Utilde = 0.72 -> w = 0.824, mu = 3.296 m
\draw[probe, sitec] (0,0) -- (3.296,0);
\node[pnode, sitec, fill=fillc] at (3.296,0) {};
% fwd-right : Utilde = 0.48 -> w = 1.016, mu = 4.064 m, sweep 19.4042
\draw[probe, sitec] (0,0) arc[start angle=90, end angle=70.5958, radius=12];
\node[pnode, sitec, fill=fillc] at (3.988,-0.680) {};
% rev-left : Utilde = 0.30 -> w = 1.160, mu = 4.640 m, sweep 22.1544
\draw[probe, lo] (0,0) arc[start angle=-90, end angle=-112.1544, radius=12];
\node[pnode, lo, fill=corrc!25] at (-4.525,0.884) {};
% rev-straight : Utilde = 0.08 -> w = 1.336, mu = 5.344 m
\draw[probe, lo] (0,0) -- (-5.344,0);
\node[pnode, lo, fill=corrc!25] at (-5.344,0) {};
% rev-right : Utilde = 0.00 -> w = 1.400, mu = 5.600 m, sweep 26.7380
\draw[probe, lo] (0,0) arc[start angle=90, end angle=116.7380, radius=12];
\node[pnode, lo, fill=corrc!25] at (-5.399,-1.283) {};
% shortest / longest annotations
\node[annt, hi] at (4.9,1.75) {$w_\text{min}\mu_0$};
\draw[->, line width=0.4pt, hi] (3.7,1.35) -- (2.6,0.55);
\node[annt, lo] at (-6.9,-2.7) {$w_\text{max}\mu_0$};
\draw[->, line width=0.4pt, lo] (-6.3,-2.15) -- (-5.4,-1.35);
\node[annt, sitec, align=center] at (0,-4.6)
{$\mu_j^\delta=\mu_0\bigl[w_\text{min}
+(w_\text{max}-w_\text{min})(1-\tilde U_j^\delta)\bigr]$};
\node[panel] at (0,-6.6) {(b) modulated steps};
\end{scope}
\end{tikzpicture}
\end{document}
figs/fig_scenario.pdf
BIN
View File
Binary file not shown.
figs/fig_scenario.tex
+73
View File
@@ -0,0 +1,73 @@
% Loading operation in an open-pit mine loading area.
% Semi-enclosed drivable region with a single entrance, an excavator working
% against the bench face, and a truck at the designated loading pose.
\documentclass[border=1pt]{standalone}
\input{figstyle}
\begin{document}
\begin{tikzpicture}[scale=0.094]
% ---------- terrain / non-drivable surroundings -------------------------
\begin{scope}
\clip (-3,-1) rectangle (79,44);
\fill[fillc] (-3,-1) rectangle (79,44);
\draw[pattern=north east lines, pattern color=sitec!55, draw=none]
(-3,-1) rectangle (79,44);
\end{scope}
% ---------- drivable loading area (Omega_free) --------------------------
\def\sitepath{
(0,10) -- (16,6.5) -- (33,5) -- (50,6.5) -- (63,11) -- (70,19)
-- (66,30) -- (52,36) -- (36,35) -- (21,30) -- (8,24) -- (0,20)
}
\fill[white] \sitepath -- cycle;
\draw[siteline] \sitepath;
% ---------- entrance ----------------------------------------------------
\draw[white, line width=1.4pt] (0.2,10.4) -- (0.2,19.6);
\draw[->, line width=0.7pt, sitec] (-9,15) -- (-1.2,15);
\node[ann, sitec, align=center] at (-9,20.5) {single\\entrance};
% ---------- bench face / ore pile --------------------------------------
% jagged toe line along the upper-right boundary
\draw[siteline, sitec]
(70,19) -- (72.5,22) -- (69.5,25) -- (72,28) -- (68,31)
-- (70,34) -- (65,35.5) -- (66,38.5) -- (60,38) -- (55,40);
\node[ann, sitec, align=center] at (60,25.5) {bench face\\/ ore pile};
\node[ann, sitec] at (12,38) {$\Omega_\text{obs}$};
\node[ann] at (20,15) {$\Omega_\text{free}$};
% ---------- excavator ---------------------------------------------------
\begin{scope}[shift={(57,31)}, rotate=-155]
\draw[veh, fill=sitec!35] (-3.2,-2.0) rectangle (3.2,2.0); % tracks
\draw[veh, fill=sitec!70] (-1.8,-1.5) rectangle (1.4,1.5); % house
\draw[line width=0.9pt, sitec] (1.2,0.6) -- (5.6,2.6); % boom
\draw[line width=0.9pt, sitec] (5.6,2.6) -- (7.4,-0.6); % stick
\draw[veh, fill=sitec!45] (7.4,-0.6) -- (8.9,-1.9) -- (7.4,-2.9)
-- (6.4,-1.5) -- cycle; % bucket
\end{scope}
\node[ann, sitec] at (57,36.5) {excavator};
% ---------- truck at the loading pose ----------------------------------
% rear-axle centre at (44,24), heading 205 deg, L_v = 12 m, B_v = 3 m,
% rear overhang 1.5 m (body spans a in [-1.5, 10.5] in body frame)
\begin{scope}[shift={(44,24)}, rotate=205]
\draw[veh, parkc, fill=parkc!12] (-1.5,-1.5) rectangle (10.5,1.5);
\draw[veh, parkc, fill=parkc!30] (6.6,-1.5) rectangle (10.5,1.5); % cab
\foreach \sx in {0,6.5}{
\foreach \sy in {-1.5,1.5}{
\draw[line width=0.4pt, parkc, fill=parkc!60]
(\sx-0.9,\sy-0.28) rectangle (\sx+0.9,\sy+0.28);
}
}
\fill[parkc] (0,0) circle (0.42);
\end{scope}
\node[ann, parkc, align=center] at (34,20)
{loading pose\\$\boldsymbol{\eta}_\text{load}$};
\draw[->, line width=0.5pt, parkc] (37.5,22.6) -- (42,23.4);
% ---------- shared channel ---------------------------------------------
\draw[<->, line width=0.5pt, sitec] (6,14.2) -- (16.5,11.3);
\node[annt, sitec, align=center] at (12.5,7.6) {shared\\channel};
\end{tikzpicture}
\end{document}
figs/figstyle.tex
+35
View File
@@ -0,0 +1,35 @@
% Shared style for the standalone figures of the mine-planning paper.
% Each figure compiles on its own: pdflatex -interaction=nonstopmode fig_*.tex
% Fonts follow IEEEtran (Times) so that included figures match the body text.
\usepackage{mathptmx}
\usepackage{amsmath}
\usepackage{tikz}
\usepackage{pgfplots}
\pgfplotsset{compat=1.18}
\usetikzlibrary{arrows.meta,positioning,calc,decorations.pathreplacing,
decorations.markings,patterns,fit,shapes.geometric,
backgrounds,intersections}
\definecolor{parkc}{RGB}{0,84,159} % inbound / parking-in path
\definecolor{exitc}{RGB}{196,78,10} % outbound / exit path
\definecolor{sitec}{RGB}{88,88,88} % site boundary, neutral linework
\definecolor{fillc}{RGB}{228,228,228} % obstacle / terrain fill
\definecolor{corrc}{RGB}{0,124,76} % convex corridor
\definecolor{centc}{RGB}{112,58,148} % virtual road centerline
\definecolor{confc}{RGB}{198,32,44} % conflict zone
\tikzset{
>={Latex[length=1.6mm,width=1.2mm]},
siteline/.style={sitec, line width=0.7pt},
parkpath/.style={parkc, line width=1.0pt},
exitpath/.style={exitc, line width=1.0pt},
ctrline/.style={centc, line width=0.7pt, dash pattern=on 2pt off 1.2pt},
guide/.style={line width=0.6pt, dash pattern=on 1.6pt off 1.2pt},
veh/.style={line width=0.5pt},
thin@/.style={line width=0.4pt},
ann/.style={font=\scriptsize},
annt/.style={font=\tiny},
dimline/.style={line width=0.4pt, <->,
>={Latex[length=1.2mm,width=0.9mm]}},
panel/.style={font=\footnotesize},
}
figs/make_center.py
+162
View File
@@ -0,0 +1,162 @@
#!/usr/bin/env python3
"""Build the virtual-centerline construction figure (fig_center.pdf).
Three panels, following subsec:centerline of the paper:
(a) clearance field of the free space (distance transform), the EVG-thin
skeleton ridge extracted from it, and the coarse A* path on the skeleton
graph from the entrance to the loading bay.
(b) the local tangent--normal rectangular corridors of
eq:local_corridor_tangent / eq:local_corridor_normal built around each
resampled reference point, and the centerline returned by the FEM-pos QP
smoother of eq:qp_problem, overlaid on the coarse path.
(c) the discrete curvature profile before and after smoothing, obtained from
the second difference ||d_i|| = ell^2 kappa_i + O(ell^4), against the
bound kappa_max = 1 / R_min imposed by eq:qp_curvature.
Construction illustration on a synthetic site -- not an experimental result.
"""
import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon as MplPolygon, Rectangle
import sitegeom as sg
R_MIN = 12.0
KAP_MAX = 1.0 / R_MIN
HALF_W = 1.6 # lateral corridor half-width used by qp_smooth
S_HALF = 0.5 # tangential corridor half-length, for illustration
def curvature(P):
"""Discrete curvature via second differences, on P's own spacing."""
seg = np.hypot(*np.diff(P, axis=0).T)
ell = 0.5 * (seg[:-1] + seg[1:])
d2 = P[:-2] - 2.0 * P[1:-1] + P[2:]
kap = np.hypot(d2[:, 0], d2[:, 1]) / ell ** 2
s = np.concatenate([[0.0], np.cumsum(seg)])
return s[1:-1], kap
def main():
g = sg.build_centerline()
xs, ys, inside, dist = g["xs"], g["ys"], g["inside"], g["dist"]
skel, raw, center = g["skel"], g["raw"], g["center"]
coarse = sg.resample(raw, 1.0)
t_c, n_c = sg.tangent_normal(coarse)
s_raw, k_raw = curvature(coarse)
s_sm, k_sm = curvature(center)
print("coarse |kappa| max %.4f -> R %.2f m" % (k_raw.max(), 1 / k_raw.max()))
print("smooth |kappa| max %.4f -> R %.2f m" % (k_sm.max(), 1 / k_sm.max()))
print("bound kappa_max %.4f -> R %.2f m" % (KAP_MAX, R_MIN))
print("smoothed violates bound: %s" % bool((k_sm > KAP_MAX + 1e-9).any()))
print("coarse length %.2f m / smoothed length %.2f m"
% (np.hypot(*np.diff(coarse, axis=0).T).sum(),
np.hypot(*np.diff(center, axis=0).T).sum()))
print("max lateral deviation from coarse %.2f m"
% np.abs(((center - coarse) * n_c).sum(axis=1)).max())
plt.rcParams.update({
"font.family": "serif",
"font.serif": ["Nimbus Roman", "Times New Roman", "DejaVu Serif"],
"font.size": 7,
"mathtext.fontset": "stix",
"axes.linewidth": 0.5,
"pdf.fonttype": 42,
})
fig = plt.figure(figsize=(7.16, 1.95), constrained_layout=True)
gs = fig.add_gridspec(1, 3, width_ratios=[1.0, 1.0, 0.72])
ax0, ax1, ax2 = (fig.add_subplot(gs[0]), fig.add_subplot(gs[1]),
fig.add_subplot(gs[2]))
# ---- (a) clearance field, skeleton, coarse A* --------------------------
dm = np.ma.array(dist, mask=~inside)
im = ax0.pcolormesh(xs, ys, dm, cmap="Blues", shading="auto",
rasterized=True, vmin=0.0, vmax=dist.max())
rr, cc = np.nonzero(skel)
ax0.plot(xs[cc], ys[rr], ls="none", marker="s", ms=0.55,
color="#4A4A4A", label="EVG skeleton", zorder=3)
ax0.plot(raw[:, 0], raw[:, 1], color="#C44E0A", lw=0.9,
label=r"coarse A$^\ast$", zorder=4)
ax0.plot(*sg.ENTRANCE, marker=">", ms=3.2, color="#111111", zorder=5)
ax0.plot(*sg.LOAD_POSE, marker="o", ms=3.0, color="#111111", zorder=5)
ax0.annotate("entrance", sg.ENTRANCE, textcoords="offset points",
xytext=(3, -6), fontsize=5.6)
ax0.annotate("loading bay", sg.LOAD_POSE, textcoords="offset points",
xytext=(-13, 4), fontsize=5.6)
ax0.legend(loc="lower right", fontsize=5.2, frameon=False,
handlelength=1.4, borderaxespad=0.1, labelspacing=0.25,
markerscale=3.0)
ax0.set_title("(a) clearance field and skeleton", fontsize=6.6, pad=2)
cb = fig.colorbar(im, ax=ax0, fraction=0.040, pad=0.015)
cb.set_ticks([0.0, dist.max()])
cb.set_ticklabels(["0", "%.0f m" % dist.max()])
cb.ax.tick_params(labelsize=5.2, width=0.4, length=1.8)
cb.outline.set_linewidth(0.4)
# ---- (b) local corridors and the smoothed centerline -------------------
ax1.add_patch(MplPolygon(sg.SITE, closed=True, facecolor="#F4F4F4",
edgecolor="#585858", lw=0.7, zorder=1))
for i in range(0, len(coarse), 3):
ang = np.degrees(np.arctan2(t_c[i, 1], t_c[i, 0]))
r = Rectangle((-S_HALF, -HALF_W), 2 * S_HALF, 2 * HALF_W,
facecolor="#BBD7EE", edgecolor="#4A87BE",
lw=0.25, alpha=0.75, zorder=2)
tr = (matplotlib.transforms.Affine2D()
.rotate_deg(ang).translate(*coarse[i]) + ax1.transData)
r.set_transform(tr)
ax1.add_patch(r)
ax1.plot(coarse[:, 0], coarse[:, 1], color="#C44E0A", lw=0.7,
ls=(0, (2.0, 1.3)), label=r"coarse A$^\ast$", zorder=4)
ax1.plot(center[:, 0], center[:, 1], color="#703A94", lw=1.2,
label=r"$\Lambda_{\mathrm{center}}$ (QP)", zorder=5)
# annotate one corridor with its four bounds
i0 = len(coarse) // 2
p0 = coarse[i0]
ax1.annotate(r"$s_i^\pm,\ l_i^\pm$", p0, textcoords="offset points",
xytext=(6, 7), fontsize=5.8,
arrowprops=dict(arrowstyle="-", lw=0.4, color="#333333",
shrinkA=0, shrinkB=1))
ax1.legend(loc="lower right", fontsize=5.2, frameon=False,
handlelength=1.5, borderaxespad=0.1, labelspacing=0.25)
ax1.set_title("(b) local corridors and QP smoothing", fontsize=6.6, pad=2)
for ax in (ax0, ax1):
ax.set_aspect("equal")
ax.set_xlim(sg.SITE[:, 0].min() - 1, sg.SITE[:, 0].max() + 1)
ax.set_ylim(sg.SITE[:, 1].min() - 1, sg.SITE[:, 1].max() + 1)
ax.set_xticks([])
ax.set_yticks([])
for sp in ax.spines.values():
sp.set_visible(False)
# ---- (c) curvature profile --------------------------------------------
ax2.plot(s_raw, k_raw, color="#C44E0A", lw=0.7, ls=(0, (2.0, 1.3)),
label=r"coarse A$^\ast$")
ax2.plot(s_sm, k_sm, color="#703A94", lw=1.0,
label=r"$\Lambda_{\mathrm{center}}$")
ax2.axhline(KAP_MAX, color="#111111", lw=0.6, ls=(0, (4.0, 1.6)))
ax2.annotate(r"$\kappa_{\max}=1/R_{\min}$", (s_sm[-1], KAP_MAX),
textcoords="offset points", xytext=(-2, 3), fontsize=5.6,
ha="right")
ax2.set_xlabel("arc length $s$ (m)", fontsize=6.2, labelpad=1.5)
ax2.set_ylabel(r"$\kappa$ (m$^{-1}$)", fontsize=6.2, labelpad=1.5)
ax2.set_title("(c) curvature profile", fontsize=6.6, pad=2)
ax2.tick_params(labelsize=5.6, width=0.4, length=2.0, pad=1.5)
ax2.set_xlim(0, max(s_raw[-1], s_sm[-1]))
ax2.set_ylim(0, max(k_raw.max(), KAP_MAX) * 1.18)
ax2.legend(loc="upper left", fontsize=5.2, frameon=False,
handlelength=1.5, borderaxespad=0.2, labelspacing=0.25)
for sp in ("top", "right"):
ax2.spines[sp].set_visible(False)
fig.savefig("fig_center.pdf", dpi=600)
print("wrote fig_center.pdf")
if __name__ == "__main__":
main()
figs/make_igf.py
+183
View File
@@ -0,0 +1,183 @@
#!/usr/bin/env python3
"""Build the IGF illustration figure (fig_igf.pdf).
Implements the paper's equations verbatim on the synthetic site of sitegeom.py:
eq:compact_kernel K(d; rho, m) = (1 - d/rho)^m for d < rho, else 0
eq:source_field U_S(z) = alpha_S * sum_{xi in S} K(|z-xi|; rho_S, m_S) ds_xi
eq:field_park U_park = U_e - U_p + U_x
eq:field_exit U_exit = U_e + U_p - U_x
eq:field_bounds U_g^max = 2 alpha_g rho_g / (m_g + 1)
U_e^max = alpha_e (2 rho_e / lbar_e + 1)
eq:field_normalize Uhat = (U + U_g^max) / (U_e^max + 2 U_g^max), clipped [0,1]
For S_e the paper takes ds_xi == 1 (vertex counting); S_p and S_x are
equal-arc-length samples carrying their true line element. Both are reproduced.
Construction illustration on a synthetic site -- not an experimental result.
"""
import numpy as np
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
from matplotlib.patches import Polygon as MplPolygon
from matplotlib.path import Path
import sitegeom as sg
# Field parameters; alpha_e is set so that eq:alpha_dominance holds.
RHO_E, M_E, ALPHA_E = 8.0, 3.0, 1.00
RHO_G, M_G, ALPHA_G = 9.0, 3.0, 0.28
LBAR_E = 1.0 # mean boundary-vertex spacing, enters U_e^max
DS_G = 0.5 # arc-length element of the guidance-line samples
DELTA = 0.25 # field grid resolution
def kernel(d, rho, m):
out = np.zeros_like(d)
ins = d < rho
out[ins] = (1.0 - d[ins] / rho) ** m
return out
def source_field(Z, S, alpha, rho, m, ds):
U = np.zeros(Z.shape[0])
for i in range(0, S.shape[0], 256):
blk = S[i:i + 256]
d = np.linalg.norm(Z[:, None, :] - blk[None, :, :], axis=2)
U += (kernel(d, rho, m) * ds).sum(axis=1)
return alpha * U
def resample_closed(poly, step):
pts = np.vstack([poly, poly[:1]])
seg = np.diff(pts, axis=0)
L = np.hypot(seg[:, 0], seg[:, 1])
cum = np.concatenate([[0.0], np.cumsum(L)])
s = np.arange(0.0, cum[-1], step)
out = np.empty((s.size, 2))
for i, si in enumerate(s):
k = min(np.searchsorted(cum, si, side="right") - 1, len(seg) - 1)
t = (si - cum[k]) / L[k]
out[i] = pts[k] + t * seg[k]
return out
def main():
g = sg.build_centerline()
center, park_line, exit_line = g["center"], g["park_line"], g["exit_line"]
# ---- source sets -------------------------------------------------------
S_e = resample_closed(sg.SITE, LBAR_E)
S_p = sg.resample(park_line, DS_G)
S_x = sg.resample(exit_line, DS_G)
# ---- grid --------------------------------------------------------------
x0, x1 = sg.SITE[:, 0].min() - 1.0, sg.SITE[:, 0].max() + 1.0
y0, y1 = sg.SITE[:, 1].min() - 1.0, sg.SITE[:, 1].max() + 1.0
xs = np.arange(x0, x1, DELTA)
ys = np.arange(y0, y1, DELTA)
XX, YY = np.meshgrid(xs, ys)
Z = np.column_stack([XX.ravel(), YY.ravel()])
U_e = source_field(Z, S_e, ALPHA_E, RHO_E, M_E, 1.0)
U_p = source_field(Z, S_p, ALPHA_G, RHO_G, M_G, DS_G)
U_x = source_field(Z, S_x, ALPHA_G, RHO_G, M_G, DS_G)
Ug_max = 2.0 * ALPHA_G * RHO_G / (M_G + 1.0)
Ue_max = ALPHA_E * (2.0 * RHO_E / LBAR_E + 1.0)
assert Ue_max > Ug_max, "eq:alpha_dominance violated"
def norm(U):
return np.clip((U + Ug_max) / (Ue_max + 2.0 * Ug_max), 0.0, 1.0)
Up_raw, Ux_raw = U_e - U_p + U_x, U_e + U_p - U_x
U_park = norm(Up_raw).reshape(XX.shape)
U_exit = norm(Ux_raw).reshape(XX.shape)
inside = Path(sg.SITE).contains_points(Z).reshape(XX.shape)
U_parkm = np.ma.array(U_park, mask=~inside)
U_exitm = np.ma.array(U_exit, mask=~inside)
# ---- numeric checks reported to stdout ---------------------------------
print("U_g^max = %.4f U_e^max = %.4f" % (Ug_max, Ue_max))
print("Uhat_park in [%.4f, %.4f]" % (U_parkm.min(), U_parkm.max()))
print("Uhat_exit in [%.4f, %.4f]" % (U_exitm.min(), U_exitm.max()))
print("mirror residual |(U_park+U_exit) - 2 U_e| = %.2e"
% np.abs(Up_raw + Ux_raw - 2 * U_e).max())
# the park field must be lower on the park line than on the exit line
def sample(F, P):
ii = np.clip(((P[:, 0] - x0) / DELTA).astype(int), 0, len(xs) - 1)
jj = np.clip(((P[:, 1] - y0) / DELTA).astype(int), 0, len(ys) - 1)
return F[jj, ii]
print("mean Uhat_park on Lambda_park = %.4f, on Lambda_exit = %.4f"
% (sample(U_park, park_line).mean(), sample(U_park, exit_line).mean()))
print("mean Uhat_exit on Lambda_park = %.4f, on Lambda_exit = %.4f"
% (sample(U_exit, park_line).mean(), sample(U_exit, exit_line).mean()))
# A common display ceiling for both panels: the analytic bound U_e^max is a
# worst-case estimate, so the realized field only reaches ~0.34. Both
# panels share one scale, which is what makes the mirror relation visible;
# the colorbar is annotated with the true values, not rescaled to [0,1].
vmax = float(max(U_parkm.max(), U_exitm.max()))
# ---- plot --------------------------------------------------------------
plt.rcParams.update({
"font.family": "serif",
"font.serif": ["Nimbus Roman", "Times New Roman", "DejaVu Serif"],
"font.size": 7,
"mathtext.fontset": "stix",
"axes.linewidth": 0.5,
"pdf.fonttype": 42,
})
fig, axes = plt.subplots(1, 3, figsize=(7.16, 1.75),
constrained_layout=True)
ax = axes[0]
ax.add_patch(MplPolygon(sg.SITE, closed=True, facecolor="white",
edgecolor="#585858", lw=0.7, zorder=1))
ax.plot(center[:, 0], center[:, 1], color="#703A94", lw=0.9,
ls=(0, (2.2, 1.2)), label=r"$\Lambda_{\mathrm{center}}$", zorder=3)
ax.plot(park_line[:, 0], park_line[:, 1], color="#00549F", lw=1.0,
label=r"$\Lambda_{\mathrm{park}}$", zorder=3)
ax.plot(exit_line[:, 0], exit_line[:, 1], color="#C44E0A", lw=1.0,
label=r"$\Lambda_{\mathrm{exit}}$", zorder=3)
ax.plot(*sg.LOAD_POSE, marker="o", ms=2.8, color="#222222", zorder=4)
ax.annotate(r"$\boldsymbol{\eta}_{\mathrm{load}}$", sg.LOAD_POSE,
textcoords="offset points", xytext=(2.5, 2.5), fontsize=6)
ax.legend(loc="lower right", fontsize=5.2, frameon=False,
handlelength=1.4, borderaxespad=0.1, labelspacing=0.2)
ax.set_title(r"(a) boundary and guidance sources", fontsize=6.6, pad=2)
for ax, U, name in ((axes[1], U_parkm, r"(b) $\hat U_{\mathrm{park}}$"),
(axes[2], U_exitm, r"(c) $\hat U_{\mathrm{exit}}$")):
im = ax.pcolormesh(XX, YY, U, cmap="viridis", vmin=0.0, vmax=vmax,
shading="auto", rasterized=True)
ax.contour(XX, YY, U.filled(np.nan),
levels=np.linspace(0.06, vmax * 0.92, 5),
colors="white", linewidths=0.25, alpha=0.7)
ax.add_patch(MplPolygon(sg.SITE, closed=True, facecolor="none",
edgecolor="#585858", lw=0.7))
ax.set_title(name, fontsize=6.6, pad=2)
cb = fig.colorbar(im, ax=axes[2], fraction=0.046, pad=0.02)
cb.set_ticks([0.0, vmax / 2.0, vmax])
cb.set_ticklabels(["0", "%.2f" % (vmax / 2.0), "%.2f" % vmax])
cb.ax.tick_params(labelsize=5.4, width=0.4, length=1.8)
cb.outline.set_linewidth(0.4)
for ax in axes:
ax.set_aspect("equal")
ax.set_xlim(x0, x1)
ax.set_ylim(y0, y1)
ax.set_xticks([])
ax.set_yticks([])
for sp in ax.spines.values():
sp.set_visible(False)
fig.savefig("fig_igf.pdf", dpi=600)
print("wrote fig_igf.pdf")
if __name__ == "__main__":
main()
figs/sitegeom.py
+252
View File
@@ -0,0 +1,252 @@
#!/usr/bin/env python3
"""Shared synthetic loading-area geometry for the paper's illustration figures.
Reproduces the pipeline of subsec:centerline / subsec:guide_route as faithfully
as the available toolchain allows:
1. rasterize the site polygon, compute the Euclidean distance transform
(clearance field) -- this is the quantity the extended Voronoi graph
partitions space by;
2. extract the clearance ridge (EVG skeleton approximation) by non-maximum
suppression of the distance transform;
3. connect entrance to loading bay by A* over the skeleton cells, with the
edge cost penalizing low clearance;
4. smooth the resulting polyline with the paper's QP model -- minimize the
second-difference (smoothness) plus reference-deviation cost subject to a
tangential/normal box corridor, solved as a projected linear system
(the paper uses OSQP; here the same objective is solved by direct
factorization with projection onto the corridor box, which is adequate
for an illustration);
5. offset by +-W/2 along the local normal to obtain the two guidance lines.
Everything here is a *construction illustration* on a synthetic site, not an
experimental result.
"""
import numpy as np
from scipy import ndimage
from matplotlib.path import Path
# ------------------------------------------------------------------ site plan
SITE = np.array([
[0.0, 10.0], [16.0, 6.5], [33.0, 5.0], [50.0, 6.5], [63.0, 11.0],
[70.0, 19.0], [66.0, 30.0], [52.0, 36.0], [36.0, 35.0], [21.0, 30.0],
[8.0, 24.0], [0.0, 20.0],
])
ENTRANCE = np.array([0.6, 15.0]) # on the open left edge
LOAD_POSE = np.array([44.0, 24.0]) # loading position inside the bay
W = 9.0 # channel width
RES = 0.25 # raster resolution for the DT/skeleton
def rasterize(res=RES, pad=1.0):
x0, x1 = SITE[:, 0].min() - pad, SITE[:, 0].max() + pad
y0, y1 = SITE[:, 1].min() - pad, SITE[:, 1].max() + pad
xs = np.arange(x0, x1, res)
ys = np.arange(y0, y1, res)
XX, YY = np.meshgrid(xs, ys)
pts = np.column_stack([XX.ravel(), YY.ravel()])
inside = Path(SITE).contains_points(pts).reshape(XX.shape)
return xs, ys, XX, YY, inside
def clearance(inside, res=RES):
"""Euclidean distance to the nearest boundary/obstacle cell, in metres."""
return ndimage.distance_transform_edt(inside) * res
def skeleton(dist, inside):
"""Clearance ridge: cells whose distance value is a local maximum along at
least one of the four axes. A cheap stand-in for EVG-thin thinning that
needs no extra dependency."""
d = dist
ridge = np.zeros_like(inside, dtype=bool)
for ax, sh in ((0, 1), (1, 1)):
a = np.roll(d, sh, axis=ax)
b = np.roll(d, -sh, axis=ax)
ridge |= (d >= a) & (d >= b)
# a genuine ridge needs some clearance; drop the noisy skin near the wall
return ridge & inside & (d > 1.5 * RES)
def astar(dist, inside, start_rc, goal_rc, clear_w=6.0):
"""8-connected A* over cells. Step cost = geometric length times a factor
that grows as clearance drops, so the path hugs the clearance ridge."""
import heapq
nr, nc = dist.shape
dmax = dist.max()
inf = float("inf")
g = np.full(dist.shape, inf)
came = {}
sr, sc = start_rc
gr, gc = goal_rc
g[sr, sc] = 0.0
def h(r, c):
return np.hypot(r - gr, c - gc) * RES
openq = [(h(sr, sc), sr, sc)]
nbrs = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
(-1, -1, 1.4142), (-1, 1, 1.4142), (1, -1, 1.4142), (1, 1, 1.4142)]
seen = np.zeros(dist.shape, dtype=bool)
while openq:
_, r, c = heapq.heappop(openq)
if seen[r, c]:
continue
seen[r, c] = True
if (r, c) == (gr, gc):
break
for dr, dc, w in nbrs:
rr, cc = r + dr, c + dc
if not (0 <= rr < nr and 0 <= cc < nc) or not inside[rr, cc]:
continue
# penalty in [1, 1+clear_w]; lowest where clearance is largest
pen = 1.0 + clear_w * (1.0 - dist[rr, cc] / dmax)
ng = g[r, c] + w * RES * pen
if ng < g[rr, cc]:
g[rr, cc] = ng
came[(rr, cc)] = (r, c)
heapq.heappush(openq, (ng + h(rr, cc), rr, cc))
if (gr, gc) not in came and (gr, gc) != (sr, sc):
raise RuntimeError("A* failed to reach the goal")
path = [(gr, gc)]
while path[-1] != (sr, sc):
path.append(came[path[-1]])
return np.array(path[::-1])
def to_xy(rc, xs, ys):
return np.column_stack([xs[rc[:, 1]], ys[rc[:, 0]]])
def resample(poly, step):
seg = np.diff(poly, axis=0)
L = np.hypot(seg[:, 0], seg[:, 1])
cum = np.concatenate([[0.0], np.cumsum(L)])
s = np.arange(0.0, cum[-1] + 1e-9, step)
out = np.empty((s.size, 2))
for i, si in enumerate(s):
k = min(np.searchsorted(cum, si, side="right") - 1, len(seg) - 1)
k = max(k, 0)
t = (si - cum[k]) / L[k]
out[i] = poly[k] + t * seg[k]
return out
def qp_smooth(ref, half_width, w_smooth=12.0, w_ref=1.0, iters=400,
r_min=12.0):
"""Minimize w_smooth * sum |p_{i-1} - 2 p_i + p_{i+1}|^2
+ w_ref * sum |p_i - p_i^ref|^2
s.t. |p_i - p_i^ref| <= half_width (isotropic proxy for the
tangential/normal box corridor)
|p_{i-1} - 2 p_i + p_{i+1}| <= ell^2 / r_min
(curvature bound, subsec:centerline:
||d_i|| = ell^2 kappa_i + O(ell^4))
with the endpoints fixed.
Solved by projected gradient iterations; the paper solves the same objective
with OSQP under linear constraints. The curvature projection is applied as
a Gauss-Seidel sweep: where the second difference exceeds its bound, the
point is pulled toward the midpoint of its neighbours, which reduces |d_i|
monotonically.
"""
P = ref.copy()
# Per-point step length: the second-difference bound ||d_i|| <= ell_i^2/r_min
# is local, so use the local spacing rather than the mean (the mean would
# under-constrain the segments that are longer than average).
def cap_of(Q):
"""Local second-difference cap ell_i^2 / r_min, evaluated on Q's own
spacing so that the enforced bound matches the curvature actually
measured on the returned polyline."""
seg = np.hypot(*np.diff(Q, axis=0).T)
ell_i = np.minimum(seg[:-1], seg[1:])
return ell_i ** 2 / r_min
for _ in range(iters):
lap = np.zeros_like(P)
lap[1:-1] = P[:-2] - 2.0 * P[1:-1] + P[2:]
grad = np.zeros_like(P)
grad[1:-1] += 2.0 * w_smooth * (-2.0) * lap[1:-1]
grad[2:-1] += 2.0 * w_smooth * lap[1:-2]
grad[1:-2] += 2.0 * w_smooth * lap[2:-1]
grad += 2.0 * w_ref * (P - ref)
step = 0.02 / (w_smooth + w_ref)
P[1:-1] = P[1:-1] - step * grad[1:-1]
# --- corridor projection -------------------------------------------
off = P - ref
r = np.hypot(off[:, 0], off[:, 1])
bad = r > half_width
if bad.any():
P[bad] = ref[bad] + off[bad] * (half_width / r[bad])[:, None]
P[0], P[-1] = ref[0], ref[-1]
# --- curvature projection, applied last so that it is the binding
# constraint on the returned polyline ---------------------------
for _ in range(60):
d_cap = cap_of(P)
d = P[:-2] - 2.0 * P[1:-1] + P[2:]
mag = np.hypot(d[:, 0], d[:, 1])
over = mag > d_cap
if not over.any():
break
# shift p_i along +d to shrink |d_i| toward the cap
shrink = np.zeros_like(mag)
shrink[over] = 0.5 * (mag[over] - d_cap[over]) / mag[over]
P[1:-1] += d * shrink[:, None]
P[0], P[-1] = ref[0], ref[-1]
return P
def tangent_normal(P):
t = np.gradient(P, axis=0)
t /= np.linalg.norm(t, axis=1)[:, None]
n = np.stack([-t[:, 1], t[:, 0]], axis=1)
return t, n
def build_centerline(verbose=False):
"""Full stage-1 geometry. Returns a dict of everything the figures need."""
xs, ys, XX, YY, inside = rasterize()
dist = clearance(inside)
skel = skeleton(dist, inside)
def nearest_cell(pt, mask):
rr, cc = np.nonzero(mask)
d = np.hypot(xs[cc] - pt[0], ys[rr] - pt[1])
i = int(np.argmin(d))
return (rr[i], cc[i])
start = nearest_cell(ENTRANCE, inside)
goal = nearest_cell(LOAD_POSE, inside)
raw_rc = astar(dist, inside, start, goal)
raw = to_xy(raw_rc, xs, ys)
# the centerline covers the entrance->bay main channel; resample then smooth
coarse = resample(raw, 1.0)
center = qp_smooth(coarse, half_width=1.6)
t, n = tangent_normal(center)
park_line = center + (W / 2.0) * n
exit_line = center - (W / 2.0) * n
if verbose:
print("grid %d x %d, res %.2f m" % (XX.shape[1], XX.shape[0], RES))
print("max clearance %.2f m" % dist.max())
print("A* raw length %.2f m, %d cells" %
(np.hypot(*np.diff(raw, axis=0).T).sum(), len(raw)))
print("centerline length %.2f m, %d points" %
(np.hypot(*np.diff(center, axis=0).T).sum(), len(center)))
# curvature of the smoothed centerline via second differences
d2 = center[:-2] - 2 * center[1:-1] + center[2:]
ell = np.hypot(*np.diff(center, axis=0).T).mean()
kap = np.hypot(d2[:, 0], d2[:, 1]) / ell ** 2
print("centerline |kappa| max %.4f (1/m) -> R_min %.2f m"
% (kap.max(), 1.0 / max(kap.max(), 1e-9)))
return dict(xs=xs, ys=ys, XX=XX, YY=YY, inside=inside, dist=dist,
skel=skel, raw=raw, center=center,
park_line=park_line, exit_line=exit_line, tang=t, nrm=n)
if __name__ == "__main__":
build_centerline(verbose=True)