目录15.1 未知环境下的概率占据补全15.1.1 时空图表征声纳、惯导与海流先验的边可行性与风险15.1.2 基于连续正规化流的未探测区域多假设采样15.1.3 将模型不确定性耦合至条件风险价值与机会约束规划Ch15_1_3_CNF_PathPlanner.py15.1 未知环境下的概率占据补全15.1.1 时空图表征声纳、惯导与海流先验的边可行性与风险自主水下航行器 (Autonomous Underwater Vehicle, AUV) 在未知海域作业时前视声纳仅能提供局部的、受限于水声传播损失与多径效应的稀疏环境感知。为建立全局可通行性评估需构建融合多源异构信息的时空图 (Spatio-Temporal Graph, STG)表征。该图结构将环境离散化为三维体素节点集合 $V \subset \mathbb{R}^3$边集 $E$ 表征相邻节点间的可行路径段。声纳占据信息通过概率占据栅格 (Probabilistic Occupancy Grid) 映射至节点特征。设声纳观测为 $\mathbf{z}_t$节点 $v_i$ 的占据概率 $o_i$ 通过逆传感器模型更新$$P(o_i | \mathbf{z}_t) \frac{P(\mathbf{z}_t | o_i) P(o_i)}{P(\mathbf{z}_t)}$$未探测区域由于缺乏观测占据概率保持先验值 $0.5$形成大面积的概率未知区域。时空图通过边属性 $e_{ij} (\phi_{ij}, \tau_{ij}, \rho_{ij})$ 编码环境动态与物理约束$\phi_{ij}$ 为基于声纳占据概率的边通过代价$\tau_{ij}$ 为边穿越时间$\rho_{ij}$ 为惯导漂移累积的不确定性度量。海流先验通过矢量场 $\mathbf{u}(v_i, t) \in \mathbb{R}^3$ 引入影响边能耗与可达性。边可行性定义为在考虑海流辅助或阻碍下的动力可达性$$\psi_{ij}(t) I(\|\mathbf{v}_{AUV} \mathbf{u}_{ij}(t)\| \ge v_{min}) \cdot I(\|\mathbf{v}_{AUV} - \mathbf{u}_{ij}(t)\| \le v_{max})$$其中 $\mathbf{v}_{AUV}$ 为 AUV 相对水体的速度矢量$v_{min}$ 与 $v_{max}$ 为速度约束。风险评估综合考虑占据不确定性、海流变异与定位漂移构建复合风险权重$$w_{ij} \alpha \cdot \sigma(o_i) \beta \cdot \|\nabla \mathbf{u}\|_{ij} \gamma \cdot \rho_{ij}$$其中 $\sigma(o_i) o_i(1 - o_i)$ 为占据概率的方差表征认知不确定性。15.1.2 基于连续正规化流的未探测区域多假设采样传统路径规划将未探测区域视为固定占据状态保守假设为障碍物或乐观假设为自由空间导致规划结果过于保守或冒险。条件正规化流 (Conditional Normalizing Flow, CNF)通过可逆神经网络建立从简单先验分布到复杂后验占据分布的映射实现对未知环境的多假设采样。设未探测区域的占据状态为隐变量 $\mathbf{x} \in \{0,1\}^{N_{unknown}}$条件信息 $\mathbf{c}$ 包含声纳边缘特征、海流模式与地形先验。CNF 通过一系列可逆变换 $\mathbf{x} f_\theta^{-1}(\mathbf{z}; \mathbf{c})$ 将标准高斯噪声 $\mathbf{z} \sim \mathcal{N}(0, \mathbf{I})$ 转换为符合环境结构的后验样本。变换的雅可比行列式 $\det \left| \frac{\partial \mathbf{x}}{\partial f_\theta} \right|$ 确保概率密度精确计算$$\log p(\mathbf{x} | \mathbf{c}) \log p(\mathbf{z}) - \sum_{k1}^K \log \left| \det \frac{\partial \mathbf{x}^{(k-1)}}{\partial f_\theta^{(k)}} \right|$$采用 RealNVP 架构实现可逆变换通过仿射耦合层 (Affine Coupling Layer) 分割变量维度交替应用尺度变换与平移变换$$\begin{cases} \mathbf{x}_{1:d} \mathbf{z}_{1:d} \\ \mathbf{x}_{d1:D} \mathbf{z}_{d1:D} \odot \exp(s(\mathbf{z}_{1:d}; \mathbf{c})) t(\mathbf{z}_{1:d}; \mathbf{c}) \end{cases}$$其中 $s(\cdot)$ 与 $t(\cdot)$ 为条件神经网络输出的尺度与平移参数。通过训练CNF 学习声纳观测边缘与未观测区域的拓扑关联生成物理合理的地形假设如海底山谷延伸、礁石分布连续性为路径规划提供概率完备的环境采样。15.1.3 将模型不确定性耦合至条件风险价值与机会约束规划基于 CNF 的环境假设采样引入了模型不确定性需在路径规划层面进行风险敏感决策。条件风险价值 (Conditional Value at Risk, CVaR)量化置信水平 $\alpha$ 下的期望尾部损失适用于极端风险规避场景。定义路径 $\xi$ 在环境假设 $\mathbf{x}$ 下的碰撞损失为 $L(\xi, \mathbf{x})$CVaR 规划求解$$\min_{\xi} \text{CVaR}_\alpha[L(\xi, \mathbf{x})] \min_{\xi} \mathbb{E}[L(\xi, \mathbf{x}) | L(\xi, \mathbf{x}) \ge \text{VaR}_\alpha]$$机会约束规划 (Chance-Constrained Planning, CCP)则直接约束碰撞概率上限$$\min_{\xi} J(\xi) \quad \text{s.t.} \quad P(\text{collision}(\xi, \mathbf{x}) \le \delta) \ge 1 - \epsilon$$其中 $\delta$ 为允许的最小安全裕度$\epsilon$ 为风险容忍度。通过将 CNF 采样的多假设场景代入概率约束转化为确定性混合整数规划$$\frac{1}{M} \sum_{m1}^M I(\text{dist}(\xi, \mathbf{x}^{(m)}) d_{safe}) \le \epsilon, \quad \mathbf{x}^{(m)} \sim p(\mathbf{x} | \mathbf{c})$$风险敏感路径规划器通过调整 CVaR 置信水平 $\alpha$ 或机会约束阈值 $\epsilon$生成从保守高 $\alpha$低 $\epsilon$到激进低 $\alpha$高 $\epsilon$的连续路径谱系。AUV 可根据任务紧急程度与剩余能量在线选择适配风险偏好的三维避障路径。Ch15_1_3_CNF_PathPlanner.pyPython#!/usr/bin/env python3 # -*- coding: utf-8 -*- Ch15_1_3_CNF_PathPlanner.py 第十五章 15.1.3节条件正规化流CNF的AUV风险敏感路径规划 脚本功能概述 ------------ 本脚本实现基于条件正规化流的多假设环境采样与风险敏感路径规划包含 1. 生成带有大面积声纳盲区的海底三维占据栅格地图 2. 实现条件正规化流RealNVP架构对未探测区域进行多假设地形采样 3. 构建时空图融合声纳、惯导与海流先验信息 4. 实现基于CVaR条件风险价值与机会约束的风险敏感路径规划 5. 生成3条不同风险偏好CVaR阈值保守/中等/激进的最优AUV三维避障路径 6. 可视化原始地图、CNF采样假设、3条路径对比及风险热力图 使用方法 -------- 直接运行python Ch15_1_3_CNF_PathPlanner.py 依赖库numpy, scipy, matplotlib, torch, scikit-image 输出结果 -------- - CNF_Risk_Aware_Planning.png包含地图、采样假设、3条风险路径的完整可视化 - 控制台输出各路径长度、CVaR风险值、碰撞概率估计 import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from matplotlib.patches import FancyArrowPatch from mpl_toolkits.mplot3d import proj3d import torch import torch.nn as nn import torch.nn.functional as F from torch.distributions import MultivariateNormal from scipy.ndimage import gaussian_filter from scipy.spatial.distance import cdist import warnings warnings.filterwarnings(ignore) # # 配置参数与物理常量 # class PlanningConfig: AUV路径规划与CNF配置 # 环境地图参数 MAP_SIZE 64 # 栅格尺寸 (64x64x32) MAP_DEPTH 32 VOXEL_SIZE 10 # 体素大小 (m) # 声纳盲区配置 KNOWN_RATIO 0.3 # 已知区域比例 SONAR_RANGE 150 # 声纳探测距离 (m) BLIND_ZONE_SIZE 20 # 盲区体素数 # CNF参数 LATENT_DIM 512 # 隐变量维度 CONDITION_DIM 128 # 条件特征维度 NUM_FLOWS 8 # 正规化流层数 HIDDEN_DIM 256 # 网络隐藏层维度 # 规划参数 START_POS np.array([5, 5, 5]) # 起点 (体素坐标) GOAL_POS np.array([58, 58, 20]) # 终点 # 风险参数 (3种风险偏好) CVAR_ALPHAS [0.95, 0.75, 0.50] # 保守/中等/激进 CHANCE_CONSTRAINTS [0.01, 0.05, 0.15] # 机会约束阈值 SAFETY_MARGIN 3 # 安全距离 (体素) # AUV动力学 MAX_DEPTH_RATE 2 # 最大下潜速度 (m/s) MAX_HORIZONTAL_SPEED 5 # 最大水平速度 (m/s) CURRENT_STRENGTH 0.3 # 海流强度 (m/s) # # 第一部分声纳盲区环境生成与时空图构建 # class UnderwaterEnvironment: 海底环境生成器 创建带声纳盲区的三维占据栅格与时空图 def __init__(self, config): self.cfg config self.occupancy None self.known_mask None self.current_field None self.graph None def generate_seabed_terrain(self): 生成真实海底地形 (Perlin噪声叠加) np.random.seed(42) # 基础地形 (Perlin噪声模拟) x np.linspace(0, 4*np.pi, self.cfg.MAP_SIZE) y np.linspace(0, 4*np.pi, self.cfg.MAP_SIZE) X, Y np.meshgrid(x, y) # 多层噪声叠加 terrain (np.sin(X) * np.cos(Y) 0.5 * np.sin(2*X) * np.cos(2*Y) 0.25 * np.random.randn(self.cfg.MAP_SIZE, self.cfg.MAP_SIZE)) # 归一化到深度范围 terrain (terrain - terrain.min()) / (terrain.max() - terrain.min()) seabed_height (terrain * (self.cfg.MAP_DEPTH * 0.6)).astype(int) # 构建3D占据栅格 self.occupancy np.zeros((self.cfg.MAP_SIZE, self.cfg.MAP_SIZE, self.cfg.MAP_DEPTH), dtypenp.float32) for i in range(self.cfg.MAP_SIZE): for j in range(self.cfg.MAP_SIZE): # 海底以下为占据 h seabed_height[i, j] self.occupancy[i, j, :h5] 1.0 # 障碍物 self.occupancy[i, j, h5:h8] 0.5 # 过渡区 # 添加礁石障碍物 for _ in range(8): cx, cy np.random.randint(10, self.cfg.MAP_SIZE-10, 2) cz np.random.randint(5, self.cfg.MAP_DEPTH-5) r np.random.randint(3, 6) self._add_sphere_obstacle(cx, cy, cz, r) return self.occupancy.copy() def _add_sphere_obstacle(self, cx, cy, cz, radius): 添加球形障碍物 for i in range(max(0, cx-radius), min(self.cfg.MAP_SIZE, cxradius)): for j in range(max(0, cy-radius), min(self.cfg.MAP_SIZE, cyradius)): for k in range(max(0, cz-radius), min(self.cfg.MAP_DEPTH, czradius)): if (i-cx)**2 (j-cy)**2 (k-cz)**2 radius**2: self.occupancy[i, j, k] 1.0 def generate_sonar_blind_zones(self): 生成声纳盲区 (模拟AUV已探测区域) # 已知区域起点周围 随机探测轨迹 self.known_mask np.zeros_like(self.occupancy, dtypebool) # 起点周围为已知 sx, sy, sz self.cfg.START_POS for i in range(max(0, sx-5), min(self.cfg.MAP_SIZE, sx5)): for j in range(max(0, sy-5), min(self.cfg.MAP_SIZE, sy5)): for k in range(max(0, sz-3), min(self.cfg.MAP_DEPTH, sz3)): self.known_mask[i, j, k] True # 模拟探测路径 (随机游走) pos self.cfg.START_POS.copy() for _ in range(200): direction np.random.randint(-2, 3, 3) pos np.clip(pos direction, 0, [self.cfg.MAP_SIZE-1, self.cfg.MAP_SIZE-1, self.cfg.MAP_DEPTH-1]) for i in range(max(0, pos[0]-3), min(self.cfg.MAP_SIZE, pos[0]3)): for j in range(max(0, pos[1]-3), min(self.cfg.MAP_SIZE, pos[1]3)): for k in range(max(0, pos[2]-2), min(self.cfg.MAP_DEPTH, pos[2]2)): self.known_mask[i, j, k] True # 终点周围已知 gx, gy, gz self.cfg.GOAL_POS for i in range(max(0, gx-5), min(self.cfg.MAP_SIZE, gx5)): for j in range(max(0, gy-5), min(self.cfg.MAP_SIZE, gy5)): for k in range(max(0, gz-3), min(self.cfg.MAP_DEPTH, gz3)): self.known_mask[i, j, k] True # 创建观测地图 (已知区域真实值未知区域0.5) observed_map np.where(self.known_mask, self.occupancy, 0.5) return observed_map, self.known_mask def generate_current_field(self): 生成海流矢量场 (时变) self.current_field np.zeros((self.cfg.MAP_SIZE, self.cfg.MAP_SIZE, self.cfg.MAP_DEPTH, 3)) # 主导流向 (东北向随深度减弱) for k in range(self.cfg.MAP_DEPTH): strength self.cfg.CURRENT_STRENGTH * (1 - k / self.cfg.MAP_DEPTH * 0.5) self.current_field[:, :, k, 0] strength * 0.7 # x方向 self.current_field[:, :, k, 1] strength * 0.7 # y方向 # 添加涡流扰动 for _ in range(3): cx, cy np.random.randint(15, self.cfg.MAP_SIZE-15, 2) for i in range(self.cfg.MAP_SIZE): for j in range(self.cfg.MAP_SIZE): dist np.sqrt((i-cx)**2 (j-cy)**2) if dist 15: angle np.arctan2(j-cy, i-cx) np.pi/2 mag self.cfg.CURRENT_STRENGTH * np.exp(-dist/5) self.current_field[i, j, :, 0] mag * np.cos(angle) self.current_field[i, j, :, 1] mag * np.sin(angle) return self.current_field def build_spatiotemporal_graph(self): 构建时空图节点与边 # 节点所有自由空间体素 free_space (self.occupancy 0.5) nodes np.argwhere(free_space) # 边26-邻域连接 (3D Moore邻域) edges [] edge_risks [] for node in nodes: i, j, k node for di in [-1, 0, 1]: for dj in [-1, 0, 1]: for dk in [-1, 0, 1]: if di dj dk 0: continue ni, nj, nk idi, jdj, kdk if (0 ni self.cfg.MAP_SIZE and 0 nj self.cfg.MAP_SIZE and 0 nk self.cfg.MAP_DEPTH): if free_space[ni, nj, nk]: # 边代价距离 海流对抗 dist np.sqrt(di**2 dj**2 dk**2) * self.cfg.VOXEL_SIZE current_vec self.current_field[i, j, k] move_vec np.array([di, dj, dk]) * self.cfg.VOXEL_SIZE move_vec move_vec / (np.linalg.norm(move_vec) 1e-6) # 海流辅助系数 (负为阻碍) current_factor -np.dot(current_vec, move_vec) # 边风险占据不确定性 海流强度 occ_uncertainty self.occupancy[i,j,k]*(1-self.occupancy[i,j,k]) risk occ_uncertainty 0.1 * np.linalg.norm(current_vec) edges.append((i*self.cfg.MAP_SIZE*self.cfg.MAP_DEPTH j*self.cfg.MAP_DEPTH k, ni*self.cfg.MAP_SIZE*self.cfg.MAP_DEPTH nj*self.cfg.MAP_DEPTH nk)) edge_risks.append(risk) self.graph { nodes: nodes, edges: edges, edge_risks: np.array(edge_risks) } return self.graph # # 第二部分条件正规化流CNF多假设采样 # class AffineCouplingLayer(nn.Module): 仿射耦合层 (RealNVP) 可逆变换x1 z1, x2 z2 * exp(s(z1)) t(z1) def __init__(self, dim, condition_dim, hidden_dim): super().__init__() self.dim dim self.split dim // 2 # 条件网络 (s和t) self.scale_net nn.Sequential( nn.Linear(self.split condition_dim, hidden_dim), nn.ReLU(), nn.Linear(hidden_dim, hidden_dim), nn.ReLU(), nn.Linear(hidden_dim, dim - self.split), nn.Tanh() # 限制尺度范围 ) self.translate_net nn.Sequential( nn.Linear(self.split condition_dim, hidden_dim), nn.ReLU(), nn.Linear(hidden_dim, hidden_dim), nn.ReLU(), nn.Linear(hidden_dim, dim - self.split) ) def forward(self, x, c, reverseFalse): x: (batch, dim) c: (batch, condition_dim) x1, x2 x[:, :self.split], x[:, self.split:] if not reverse: # 前向z - x inputs torch.cat([x1, c], dim1) s self.scale_net(inputs) t self.translate_net(inputs) x2 x2 * torch.exp(s) t x torch.cat([x1, x2], dim1) # 雅可比对数行列式 log_det s.sum(dim1) return x, log_det else: # 逆向x - z inputs torch.cat([x1, c], dim1) s self.scale_net(inputs) t self.translate_net(inputs) x2 (x2 - t) * torch.exp(-s) x torch.cat([x1, x2], dim1) log_det -s.sum(dim1) return x, log_det class ConditionalNormalizingFlow(nn.Module): 条件正规化流模型 对未探测区域进行多假设占据采样 def __init__(self, config): super().__init__() self.cfg config self.dim config.LATENT_DIM self.condition_dim config.CONDITION_DIM # 条件编码器 (编码已知区域声纳特征) self.condition_encoder nn.Sequential( nn.Linear(config.MAP_SIZE*config.MAP_SIZE*config.MAP_DEPTH//8, config.CONDITION_DIM), nn.ReLU(), nn.Linear(config.CONDITION_DIM, config.CONDITION_DIM) ) # 流层 self.flows nn.ModuleList([ AffineCouplingLayer(self.dim, self.condition_dim, config.HIDDEN_DIM) for _ in range(config.NUM_FLOWS) ]) # 先验分布 self.prior MultivariateNormal( torch.zeros(self.dim), torch.eye(self.dim) ) # 解码器 (隐变量 - 占据地图补全) self.decoder nn.Sequential( nn.Linear(self.dim, 512), nn.ReLU(), nn.Linear(512, config.MAP_SIZE*config.MAP_SIZE*config.MAP_DEPTH//4), nn.Sigmoid() # 占据概率 ) def encode_condition(self, observed_map): 编码条件信息 (已知区域特征) observed_map: (batch, H, W, D) # 下采样并展平 batch_size observed_map.size(0) x observed_map.view(batch_size, -1) # 进一步降维 x F.adaptive_avg_pool1d(x.unsqueeze(1), x.size(1)//8).squeeze(1) c self.condition_encoder(x) return c def forward(self, z, c): 正向生成z - 占据假设 x z log_det_sum 0 for flow in self.flows: x, log_det flow(x, c) log_det_sum log_det # 解码为地图补全 occ_completion self.decoder(x) return occ_completion, log_det_sum def sample(self, observed_map, num_samples5): 采样多假设地形 with torch.no_grad(): batch_size 1 observed_t torch.FloatTensor(observed_map).unsqueeze(0) # 编码条件 c self.encode_condition(observed_t).expand(num_samples, -1) # 从先验采样 z self.prior.sample((num_samples,)) # 生成 completions, _ self.forward(z, c) # 重塑为3D地图 (仅补全未知区域) H, W, D self.cfg.MAP_SIZE, self.cfg.MAP_SIZE, self.cfg.MAP_DEPTH samples completions.view(num_samples, H, W, D//4) # 插值到完整分辨率 (简化) samples F.interpolate(samples.unsqueeze(1), size(H, W, D), modetrilinear, align_cornersFalse).squeeze(1) return samples.numpy() def train_step(self, observed_maps, true_maps, optimizer): 训练步骤 (简化演示) optimizer.zero_grad() batch_size observed_maps.size(0) c self.encode_condition(observed_maps).expand(batch_size, -1) # 采样z z self.prior.sample((batch_size,)) # 正向 x, log_det self.forward(z, c) # 重构损失 recon_loss F.mse_loss(x, true_maps.view(batch_size, -1)[:, :self.dim]) # 对数似然 (简化) log_likelihood self.prior.log_prob(z) log_det loss recon_loss - log_likelihood.mean() loss.backward() optimizer.step() return loss.item() # # 第三部分CVaR风险敏感路径规划 # class CVaRPathPlanner: 基于条件风险价值的风险敏感路径规划器 def __init__(self, config, environment): self.cfg config self.env environment def compute_risk_field(self, occupancy_samples, alpha): 从CNF采样计算CVaR风险场 Parameters ---------- occupancy_samples : ndarray (n_samples, H, W, D) CNF生成的多假设占据地图 alpha : float CVaR置信水平 (0-1) Returns ------- risk_field : ndarray (H, W, D) 每个体素的风险值 # 碰撞损失 (占据为1则损失高) losses occupancy_samples # 简化占据概率即损失 # 计算VaR (分位数) var np.percentile(losses, alpha * 100, axis0) # 计算CVaR (超过VaR的期望) exceed_mask losses var cvar np.where(exceed_mask.sum(axis0) 0, (losses * exceed_mask).sum(axis0) / exceed_mask.sum(axis0), var) return cvar def astar_risk_planning(self, start, goal, risk_field, cvar_alpha): A*路径规划 (3D)考虑CVaR风险 Returns ------- path : list of (x,y,z) risk_cost : float import heapq # 启发函数 (欧氏距离) def heuristic(a, b): return np.linalg.norm(np.array(a) - np.array(b)) # 检查是否安全 (机会约束) def is_safe(pos, risk_field): x, y, z int(pos[0]), int(pos[1]), int(pos[2]) if not (0 x risk_field.shape[0] and 0 y risk_field.shape[1] and 0 z risk_field.shape[2]): return False # 机会约束风险值必须低于阈值 return risk_field[x, y, z] self.cfg.CHANCE_CONSTRAINTS[int((1-cvar_alpha)*3)] # A*搜索 open_set [(0, tuple(start))] came_from {} g_score {tuple(start): 0} f_score {tuple(start): heuristic(start, goal)} closed_set set() while open_set: current heapq.heappop(open_set)[1] if np.linalg.norm(np.array(current) - goal) 3: # 重建路径 path [current] while current in came_from: current came_from[current] path.append(current) path.reverse() # 计算路径总风险 total_risk sum(risk_field[int(p[0]), int(p[1]), int(p[2])] for p in path) return path, total_risk closed_set.add(current) # 26-邻域扩展 for dx in [-1, 0, 1]: for dy in [-1, 0, 1]: for dz in [-1, 0, 1]: if dx dy dz 0: continue neighbor (current[0] dx, current[1] dy, current[2] dz) if neighbor in closed_set: continue if not (0 neighbor[0] risk_field.shape[0] and 0 neighbor[1] risk_field.shape[1] and 0 neighbor[2] risk_field.shape[2]): continue # 安全检查 if not is_safe(neighbor, risk_field): continue # 边代价距离权重 风险权重 move_cost np.sqrt(dx**2 dy**2 dz**2) risk_cost risk_field[int(neighbor[0]), int(neighbor[1]), int(neighbor[2])] tentative_g g_score[current] move_cost 50 * risk_cost if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f_score[neighbor] tentative_g heuristic(neighbor, goal) heapq.heappush(open_set, (f_score[neighbor], neighbor)) return None, float(inf) # 无路径 def plan_paths(self, cnf_samples): 规划3条不同风险偏好的路径 paths {} risks {} for i, alpha in enumerate(self.cfg.CVAR_ALPHAS): risk_field self.compute_risk_field(cnf_samples, alpha) path, risk_cost self.astar_risk_planning( self.cfg.START_POS, self.cfg.GOAL_POS, risk_field, alpha ) if path is not None: label [Conservative (CVaR 0.95), Moderate (CVaR 0.75), Aggressive (CVaR 0.50)][i] paths[label] np.array(path) risks[label] risk_cost else: print(fWarning: No path found for alpha{alpha}) return paths, risks # # 第四部分可视化与评估 # class PlanningVisualizer: 规划结果可视化 def __init__(self, config): self.cfg config def visualize(self, environment, cnf_samples, planned_paths, save_pathCNF_Risk_Aware_Planning.png): 生成完整可视化报告 fig plt.figure(figsize(18, 12)) # 1. 原始地图与盲区 (2D投影) ax1 fig.add_subplot(231) occ_slice environment.occupancy[:, :, 10] # 深度切片 known_slice environment.known_mask[:, :, 10] im ax1.imshow(occ_slice.T, originlower, cmapviridis, vmin0, vmax1) # 叠加盲区 blind_zone np.where(~known_slice (occ_slice 0.5), 1, 0) ax1.contour(blind_zone.T, levels[0.5], colorsred, linewidths2, labelBlind Zone) ax1.scatter(*self.cfg.START_POS[:2], cgreen, s200, markero, labelStart) ax1.scatter(*self.cfg.GOAL_POS[:2], cred, s200, marker*, labelGoal) ax1.set_title(Observed Map with Blind Zones (Depth100m)) ax1.legend() plt.colorbar(im, axax1, labelOccupancy Probability) # 2. CNF采样假设 (显示3个样本) ax2 fig.add_subplot(232) sample_slice cnf_samples[0, :, :, 10] im2 ax2.imshow(sample_slice.T, originlower, cmapplasma, vmin0, vmax1) ax2.set_title(CNF Sample 1: Terrain Hypothesis) plt.colorbar(im2, axax2) ax3 fig.add_subplot(233) sample_slice2 cnf_samples[1, :, :, 10] im3 ax3.imshow(sample_slice2.T, originlower, cmapplasma, vmin0, vmax1) ax3.set_title(CNF Sample 2: Alternative Hypothesis) plt.colorbar(im3, axax3) # 3. 3D路径对比 ax4 fig.add_subplot(234, projection3d) # 绘制占据点云 (障碍物) occ_points np.argwhere(environment.occupancy 0.7) if len(occ_points) 500: idx np.random.choice(len(occ_points), 500, replaceFalse) occ_points occ_points[idx] ax4.scatter(occ_points[:, 0], occ_points[:, 1], occ_points[:, 2], cgray, alpha0.3, s10, labelObstacles) # 绘制路径 colors [blue, orange, red] for i, (label, path) in enumerate(planned_paths.items()): if len(path) 0: ax4.plot(path[:, 0], path[:, 1], path[:, 2], colorcolors[i], linewidth3, labellabel) ax4.scatter(path[0, 0], path[0, 1], path[0, 2], cgreen, s100, markero) ax4.scatter(path[-1, 0], path[-1, 1], path[-1, 2], cred, s100, marker*) ax4.set_xlabel(X (voxels)) ax4.set_ylabel(Y (voxels)) ax4.set_zlabel(Z (depth)) ax4.set_title(3D Risk-Aware Path Comparison) ax4.legend() # 4. 风险热力图 (CVaR场) ax5 fig.add_subplot(235) # 计算保守风险场用于可视化 risk_field np.mean(cnf_samples, axis0) # 简化为期望风险 risk_slice risk_field[:, :, 10] im5 ax5.imshow(risk_slice.T, originlower, cmaphot, vmin0, vmax1) # 叠加保守路径 if Conservative (CVaR 0.95) in planned_paths: path planned_paths[Conservative (CVaR 0.95)] ax5.plot(path[:, 0], path[:, 1], cyan, linewidth3, labelConservative Path) ax5.set_title(Risk Field (Expected Occupancy)) plt.colorbar(im5, axax5, labelRisk Level) ax5.legend() # 5. 路径统计对比 ax6 fig.add_subplot(236) labels list(planned_paths.keys()) path_lengths [len(p) * self.cfg.VOXEL_SIZE for p in planned_paths.values()] bars ax6.bar(range(len(labels)), path_lengths, color[blue, orange, red], alpha0.7, edgecolorblack) ax6.set_xticks(range(len(labels))) ax6.set_xticklabels([Conservative, Moderate, Aggressive], rotation15) ax6.set_ylabel(Path Length (m)) ax6.set_title(Path Length vs Risk Preference) # 添加数值标签 for bar, length in zip(bars, path_lengths): height bar.get_height() ax6.text(bar.get_x() bar.get_width()/2., height, f{length:.0f}m, hacenter, vabottom) plt.tight_layout() plt.savefig(save_path, dpi300, bbox_inchestight) print(f\n可视化结果已保存至: {save_path}) return fig # # 主执行流程 # def main(): 主执行函数CNF风险敏感路径规划完整流程 print( * 70) print(条件正规化流风险敏感AUV路径规划) print(Chapter 15.1.3: CNF-based Risk-Aware Path Planning) print( * 70) # 配置初始化 config PlanningConfig() print(f\n规划配置:) print(f 地图尺寸: {config.MAP_SIZE}^2 x {config.MAP_DEPTH} ({config.VOXEL_SIZE}m/体素)) print(f 声纳盲区比例: {(1-config.KNOWN_RATIO)*100:.0f}%) print(f 起点: {config.START_POS}, 终点: {config.GOAL_POS}) # 环境生成 print(\n[1/4] 生成海底环境与声纳盲区...) env UnderwaterEnvironment(config) true_occupancy env.generate_seabed_terrain() observed_map, known_mask env.generate_sonar_blind_zones() current_field env.generate_current_field() graph env.build_spatiotemporal_graph() known_ratio known_mask.sum() / known_mask.size print(f 实际探测覆盖率: {known_ratio*100:.1f}%) print(f 海流场平均强度: {np.mean(np.linalg.norm(current_field, axis-1)):.2f} m/s) # CNF模型训练与采样 (简化演示) print(\n[2/4] 训练条件正规化流并采样地形假设...) cnf_model ConditionalNormalizingFlow(config) optimizer torch.optim.Adam(cnf_model.parameters(), lr1e-3) # 快速训练 (实际应用需更充分训练) print( 训练CNF模型 (演示模式50 epochs)...) observed_t torch.FloatTensor(observed_map) true_t torch.FloatTensor(true_occupancy) for epoch in range(50): loss cnf_model.train_step(observed_t.unsqueeze(0), true_t.unsqueeze(0), optimizer) if epoch % 10 0: print(f Epoch {epoch}, Loss: {loss:.4f}) # 生成多假设 print( 生成5个地形假设样本...) cnf_samples cnf_model.sample(observed_map, num_samples5) print(f 样本形状: {cnf_samples.shape}) # 路径规划 print(\n[3/4] 执行风险敏感路径规划...) planner CVaRPathPlanner(config, env) planned_paths, path_risks planner.plan_paths(cnf_samples) print( 规划结果:) for label, risk in path_risks.items(): length len(planned_paths[label]) * config.VOXEL_SIZE if label in planned_paths else 0 print(f {label}: 长度{length:.0f}m, 累积风险{risk:.2f}) # 可视化 print(\n[4/4] 生成规划可视化...) visualizer PlanningVisualizer(config) visualizer.visualize(env, cnf_samples, planned_paths) # 统计报告 print(\n * 70) print(性能评估报告:) print( * 70) for i, (label, path) in enumerate(planned_paths.items()): length len(path) * config.VOXEL_SIZE cvar_val config.CVAR_ALPHAS[i] print(f\n{label}:) print(f 路径长度: {length:.1f} m) print(f CVaR水平: {cvar_val} (越高越保守)) print(f 平均深度: {np.mean(path[:, 2]) * config.VOXEL_SIZE:.1f} m) print(f 最大高度变化: {(path[:, 2].max() - path[:, 2].min()) * config.VOXEL_SIZE:.1f} m) # 计算与真实地图的碰撞检查 print(\n碰撞验证 (与真实地形):) for label, path in planned_paths.items(): collisions 0 for pos in path: x, y, z int(pos[0]), int(pos[1]), int(pos[2]) if 0 x config.MAP_SIZE and 0 y config.MAP_SIZE and 0 z config.MAP_DEPTH: if true_occupancy[x, y, z] 0.5: collisions 1 print(f {label}: 碰撞体素数 {collisions} (路径总长{len(path)}点)) print( * 70) if __name__ __main__: main()脚本执行说明环境要求Python 3.8依赖numpy,scipy,matplotlib,torch,scikit-image运行方式命令行执行python Ch15_1_3_CNF_PathPlanner.py输出结果控制台输出各路径长度、CVaR风险值、碰撞检测结果图像文件CNF_Risk_Aware_Planning.png包含原始盲区地图、CNF地形假设、3D路径对比、风险热力图、路径长度统计的六联可视化