📖 第02课：概率机器人基础

import numpy as np import matplotlib matplotlib.use('Agg') import matplotlib.pyplot as plt plt.rcParams['font.sans-serif'] = ['WenQuanYi Zen Hei'] plt.rcParams['axes.unicode_minus'] = False np.random.seed(42) # ========== 场景：1D位置估计 ========== # 机器人真实位置 x_true = 5.0m # 三个传感器提供独立观测 x_true = 5.0 # 传感器1：激光雷达，精度高 z1 = x_true + np.random.normal(0, 0.2) # σ=0.2 sigma1 = 0.2 # 传感器2：超声波，精度中等 z2 = x_true + np.random.normal(0, 0.5) # σ=0.5 sigma2 = 0.5 # 传感器3：里程计推算，精度低 z3 = x_true + np.random.normal(0, 1.0) # σ=1.0 sigma3 = 1.0 # ========== 贝叶斯融合 ========== # 先验：均匀分布（无信息先验） # 似然函数：p(z|x) = N(x; z, σ²) # 后验 ∝ 似然1 × 似然2 × 似然3 # 在信息矩阵空间融合 Lambda1 = 1.0 / sigma1**2 # 信息矩阵（标量） Lambda2 = 1.0 / sigma2**2 Lambda3 = 1.0 / sigma3**2 # 融合后的信息 Lambda_fused = Lambda1 + Lambda2 + Lambda3 eta_fused = Lambda1 * z1 + Lambda2 * z2 + Lambda3 * z3 # 恢复高斯参数 sigma_fused = np.sqrt(1.0 / Lambda_fused) mu_fused = eta_fused / Lambda_fused # ========== 可视化 ========== x = np.linspace(0, 10, 500) def gaussian(x, mu, sigma): return (1 / (sigma * np.sqrt(2 * np.pi))) * np.exp(-0.5 * ((x - mu) / sigma)**2) fig, axes = plt.subplots(2, 2, figsize=(14, 10)) # 各传感器似然 ax = axes[0, 0] ax.plot(x, gaussian(x, z1, sigma1), 'r-', linewidth=2, label=f'激光雷达 σ={sigma1}') ax.plot(x, gaussian(x, z2, sigma2), 'b-', linewidth=2, label=f'超声波 σ={sigma2}') ax.plot(x, gaussian(x, z3, sigma3), 'y-', linewidth=2, label=f'里程计 σ={sigma3}') ax.axvline(x_true, color='green', linestyle='--', label='真实位置') ax.set_title('各传感器似然函数', fontsize=13) ax.legend(fontsize=10) ax.grid(True, alpha=0.3) # 逐步融合 ax = axes[0, 1] # 只用激光 L1 = Lambda1; e1 = Lambda1 * z1 mu1 = e1 / L1; s1 = np.sqrt(1/L1) # 激光+超声 L12 = Lambda1 + Lambda2; e12 = Lambda1*z1 + Lambda2*z2 mu12 = e12/L12; s12 = np.sqrt(1/L12) # 全部融合 ax.plot(x, gaussian(x, mu1, s1), 'r-', linewidth=2, label='仅激光') ax.plot(x, gaussian(x, mu12, s12), 'b-', linewidth=2, label='激光+超声') ax.plot(x, gaussian(x, mu_fused, sigma_fused), 'g-', linewidth=2.5, label='全部融合') ax.axvline(x_true, color='green', linestyle='--', alpha=0.7) ax.set_title('逐步贝叶斯融合', fontsize=13) ax.legend(fontsize=10) ax.grid(True, alpha=0.3) # 信息矩阵可视化 ax = axes[1, 0] sensors = ['激光雷达', '超声波', '里程计', '融合结果'] info_values = [Lambda1, Lambda2, Lambda3, Lambda_fused] colors = ['red', 'blue', 'orange', 'green'] bars = ax.bar(sensors, info_values, color=colors, alpha=0.7) ax.set_ylabel('信息量 (1/σ²)', fontsize=12) ax.set_title('各传感器信息量对比', fontsize=13) for bar, val in zip(bars, info_values): ax.text(bar.get_x() + bar.get_width()/2, bar.get_height() + 0.5, f'{val:.1f}', ha='center') ax.grid(True, alpha=0.3) # 融合精度对比 ax = axes[1, 1] methods = ['仅激光', '激光+超声', '全部融合', '真实值'] means = [mu1, mu12, mu_fused, x_true] stds = [s1, s12, sigma_fused, 0] x_pos = range(len(methods)) ax.barh(x_pos, means, xerr=stds, color=['red', 'blue', 'green', 'gray'], alpha=0.7, capsize=5) ax.set_yticks(x_pos) ax.set_yticklabels(methods) ax.set_xlabel('位置估计 (m)') ax.set_title('位置估计与不确定度', fontsize=13) ax.axvline(x_true, color='green', linestyle='--', alpha=0.7) ax.grid(True, alpha=0.3) plt.tight_layout() plt.savefig('/var/www/ttl/slam/fig_02_bayes_fusion.png', dpi=120, facecolor='#0f172a') plt.close() # 输出结果 print(f"真实位置: {x_true:.2f}m") print(f"激光观测: z1={z1:.4f}m, σ1={sigma1}") print(f"超声观测: z2={z2:.4f}m, σ2={sigma2}") print(f"里程推算: z3={z3:.4f}m, σ3={sigma3}") print(f"贝叶斯融合: μ={mu_fused:.4f}m, σ={sigma_fused:.4f}m") print(f"融合误差: {abs(mu_fused - x_true):.4f}m") print(f"信息量: Λ={Lambda_fused:.2f} (vs 激光单独{Lambda1:.2f})")

import numpy as np import matplotlib matplotlib.use('Agg') import matplotlib.pyplot as plt plt.rcParams['font.sans-serif'] = ['WenQuanYi Zen Hei'] plt.rcParams['axes.unicode_minus'] = False np.random.seed(42) # ========== 蒙特卡洛采样：近似非高斯分布 ========== # 目标分布：混合高斯（双峰分布） def mixture_pdf(x): # 0.4 * N(2, 0.5²) + 0.6 * N(7, 1.0²) p1 = 0.4 * np.exp(-0.5 * ((x - 2) / 0.5)**2) / (0.5 * np.sqrt(2 * np.pi)) p2 = 0.6 * np.exp(-0.5 * ((x - 7) / 1.0)**2) / (1.0 * np.sqrt(2 * np.pi)) return p1 + p2 # 从混合高斯采样 def sample_mixture(n): samples = [] for _ in range(n): if np.random.random() < 0.4: samples.append(np.random.normal(2, 0.5)) else: samples.append(np.random.normal(7, 1.0)) return np.array(samples) # 不同采样数量的对比 fig, axes = plt.subplots(2, 2, figsize=(14, 10)) x = np.linspace(-2, 12, 500) true_pdf = mixture_pdf(x) sample_counts = [50, 500, 5000, 50000] for idx, n in enumerate(sample_counts): ax = axes[idx // 2, idx % 2] samples = sample_mixture(n) ax.plot(x, true_pdf, 'g-', linewidth=2, label='真实PDF') ax.hist(samples, bins=40, density=True, alpha=0.6, color='cyan', label=f'采样N={n}') # 计算采样估计的均值和方差 est_mean = np.mean(samples) est_std = np.std(samples) ax.set_title(f'蒙特卡洛采样 N={n} | μ̂={est_mean:.2f}, σ̂={est_std:.2f}', fontsize=12) ax.legend(fontsize=9) ax.grid(True, alpha=0.3) plt.suptitle('蒙特卡洛采样近似双峰分布', fontsize=15, color='white') plt.tight_layout() plt.savefig('/var/www/ttl/slam/fig_02_monte_carlo.png', dpi=120, facecolor='#0f172a') plt.close() # 重要性采样验证 # 用宽高斯q(x)=N(5, 3²)作为提议分布 # 目标：估计E[X]在混合分布下 q_mu, q_sigma = 5.0, 3.0 N_is = 10000 q_samples = np.random.normal(q_mu, q_sigma, N_is) q_pdf_vals = np.exp(-0.5 * ((q_samples - q_mu) / q_sigma)**2)) / (q_sigma * np.sqrt(2 * np.pi)) p_pdf_vals = mixture_pdf(q_samples) weights = p_pdf_vals / (q_pdf_vals + 1e-10) weights /= np.sum(weights) is_mean = np.sum(weights * q_samples) # 真实均值 = 0.4*2 + 0.6*7 = 5.0 true_mean = 5.0 print(f"重要性采样估计E[X]: {is_mean:.4f}") print(f"真实E[X]: {true_mean:.4f}") print(f"误差: {abs(is_mean - true_mean):.4f}")

import numpy as np import matplotlib matplotlib.use('Agg') import matplotlib.pyplot as plt plt.rcParams['font.sans-serif'] = ['WenQuanYi Zen Hei'] plt.rcParams['axes.unicode_minus'] = False def cov_ellipse(mu, Sigma, n_std=2.0, n_pts=100): eigenvalues, eigenvectors = np.linalg.eigh(Sigma) order = eigenvalues.argsort()[::-1] eigenvalues = eigenvalues[order] eigenvectors = eigenvectors[:, order] angle = np.arctan2(eigenvectors[1, 0], eigenvectors[0, 0]) theta = np.linspace(0, 2 * np.pi, n_pts) ellipse = np.column_stack([n_std * np.sqrt(eigenvalues[0]) * np.cos(theta), n_std * np.sqrt(eigenvalues[1]) * np.sin(theta)]) rotation = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) ellipse = ellipse @ rotation.T return ellipse[:, 0] + mu[0], ellipse[:, 1] + mu[1] # 机器人轨迹的不确定度增长 fig, ax = plt.subplots(figsize=(12, 8)) # 5个位姿，不确定度递增 poses = [(0, 0), (2, 1), (4, 2.5), (5, 5), (4, 7)] sigmas = [0.1, 0.3, 0.6, 1.0, 1.5] # 递增的不确定度 colors = ['green', 'lime', 'yellow', 'orange', 'red'] for i, (pos, sigma) in enumerate(zip(poses, sigmas)): Sigma = np.array([[sigma**2, sigma**2 * 0.3], [sigma**2 * 0.3, sigma**2 * 0.8]]) ex, ey = cov_ellipse(np.array(pos), Sigma, n_std=2.0) ax.plot(ex, ey, color=colors[i], linewidth=2) ax.fill(ex, ey, color=colors[i], alpha=0.15) ax.plot(pos[0], pos[1], 'o', color=colors[i], markersize=8) ax.annotate(f'x_{i} σ={sigma}', xy=pos, fontsize=10, color=colors[i], xytext=(10, 10), textcoords='offset points') # 连线 traj_x = [p[0] for p in poses] traj_y = [p[1] for p in poses] ax.plot(traj_x, traj_y, 'w--', alpha=0.5, linewidth=1) ax.set_title('机器人轨迹不确定度增长（2σ协方差椭圆）', fontsize=14) ax.set_xlabel('x (m)') ax.set_ylabel('y (m)') ax.set_aspect('equal') ax.grid(True, alpha=0.3) plt.tight_layout() plt.savefig('/var/www/ttl/slam/fig_02_cov_ellipse.png', dpi=120, facecolor='#0f172a') plt.close() print("协方差椭圆可视化完成")