案例二：复杂环境路径规划自适应优化

# 宝鸡文理学院计算机学院 ·《最优化方法》案例库代码规范
import torch
import torch.nn as nn

class DualHeadOrchardPlanner(nn.Module):
    """
    双头物理信息网络：输入当前局部位姿与激光感知张量
    Head 1: 输出局部子引导航向修正角 theta_adj
    Head 2: 动态解算势场斥力增益与衰减半径 [k_rep, d_0]
    """
    def __init__(self, state_dim=18, hidden_dim=64):
        super().__init__()
        self.shared_backbone = nn.Sequential(
            nn.Linear(state_dim, hidden_dim),
            nn.Mish(),
            nn.Linear(hidden_dim, hidden_dim),
            nn.LayerNorm(hidden_dim)
        )
        # Head 1: 航向修正头 (Escaping Local Minima Angle)
        self.head_direction = nn.Sequential(
            nn.Linear(hidden_dim, 32),
            nn.ReLU(),
            nn.Linear(32, 1),
            nn.Tanh()  # [-pi, pi] normalized offset
        )
        # Head 2: 物理势场超参数头 (k_rep, d_0)
        self.head_params = nn.Sequential(
            nn.Linear(hidden_dim, 32),
            nn.ReLU(),
            nn.Linear(32, 2),
            nn.Softplus()  # Ensure strictly positive physical gains
        )

    def forward(self, observation_tensor):
        feat = self.shared_backbone(observation_tensor)
        heading_offset = self.head_direction(feat) * 3.1415926
        potential_gains = self.head_params(feat)  # [k_rep, d_0]
        return heading_offset, potential_gains

def compute_adaptive_gradient(q_curr, q_goal, obstacles, k_rep, d_0, k_att=1.2):
    # 引力分量计算 (Vector towards sub-goal)
    F_att = k_att * (q_goal - q_curr)
    F_rep = torch.zeros_like(q_curr)
    
    for obs in obstacles:
        dist = torch.norm(q_curr - obs)
        if dist <= d_0 and dist > 1e-4:
            # 改良 APF 公式：斥力梯度自适应衰减项
            rep_mag = k_rep * (1.0/dist - 1.0/d_0) * (1.0 / (dist**2))
            F_rep += rep_mag * ((q_curr - obs) / dist)
            
    return F_att + F_rep