3. 应用间接法求解最优转弯轨迹

任务定义

给定一架在水平面内运动的飞行器，初始位置 \((x_0,y_0)=(0,0)\)、初始航向角 \(\psi_0=0\)，要求飞行器在固定末端时间 \(t_f=10\) 内完成转弯，使末端位置 \((x_f,y_f)=(100,100)\)、末端航向角 \(\psi_f=\pi/2\)。飞行器速度恒为 \(V=20\)，控制输入为航向角速率 \(u=\dot{\psi}\)，满足 \(|u|\le u_{max}=0.5\)。目标为最小化控制能量 \(J=\frac{1}{2}\int_0^{t_f} u^2 dt\)，求最优控制律与最优转弯轨迹。

最优性条件推导

系统状态方程为：

\(\dot{x}=V\cos\psi,\quad \dot{y}=V\sin\psi,\quad \dot{\psi}=u\)

构造哈密顿函数 \(H=\frac{1}{2}u^2+\lambda_x V\cos\psi+\lambda_y V\sin\psi+\lambda_\psi u\)。由庞特里亚金极小值原理，最优控制使 \(H\) 最小，对 \(u\) 求导得：

\(\frac{\partial H}{\partial u}=u+\lambda_\psi=0 \Rightarrow u^*=-\lambda_\psi\)

协态方程：

\(\dot{\lambda}_x=-\frac{\partial H}{\partial x}=0,\quad \dot{\lambda}_y=-\frac{\partial H}{\partial y}=0,\quad \dot{\lambda}_\psi=-\frac{\partial H}{\partial \psi}=\lambda_x V\sin\psi-\lambda_y V\cos\psi\)

因此 \(\lambda_x,\lambda_y\) 为常数，\(\lambda_\psi\) 满足上述微分方程。控制约束 \(|u|\le 0.5\) 在能量最优问题中通常不主动激活（若 \(|\lambda_\psi|>0.5\) 则取饱和值），本案例中末端时间与距离设置使最优解处于线性区间，验证时需检查 \(|\lambda_\psi(t)|\le 0.5\)。

两点边值问题与打靶法

状态初值已知：\(x(0)=0,y(0)=0,\psi(0)=0\)。末端约束：\(x(t_f)=100,y(t_f)=100,\psi(t_f)=\pi/2\)。协态初值 \(\lambda_x(0),\lambda_y(0),\lambda_\psi(0)\) 未知，构成三点打靶变量。定义打靶函数：

\(F(\lambda_x,\lambda_y,\lambda_\psi)=\begin{bmatrix}x(t_f)-100\\y(t_f)-100\\\psi(t_f)-\pi/2\end{bmatrix}\)

使用牛顿法迭代求解 \(F=0\)。数值积分采用四阶Runge-Kutta法，步长 \(h=0.01\)。

Python实现

import numpy as np
from scipy.integrate import solve_ivp
from scipy.optimize import fsolve

V = 20.0
tf = 10.0
x0, y0, psi0 = 0.0, 0.0, 0.0
xf, yf, psif = 100.0, 100.0, np.pi/2

def dynamics(t, z):
    # z = [x, y, psi, lx, ly, lpsi]
    x, y, psi, lx, ly, lpsi = z
    u = -lpsi
    # 控制约束饱和
    u = np.clip(u, -0.5, 0.5)
    dx = V*np.cos(psi)
    dy = V*np.sin(psi)
    dpsi = u
    dlx = 0.0
    dly = 0.0
    dlpsi = lx*V*np.sin(psi) - ly*V*np.cos(psi)
    return [dx, dy, dpsi, dlx, dly, dlpsi]

def shoot(s):
    lx0, ly0, lpsi0 = s
    z0 = [x0, y0, psi0, lx0, ly0, lpsi0]
    sol = solve_ivp(dynamics, [0, tf], z0, method='RK45',
                    rtol=1e-10, atol=1e-12, dense_output=True)
    xf_calc = sol.y[0][-1]
    yf_calc = sol.y[1][-1]
    psif_calc = sol.y[2][-1]
    return [xf_calc - xf, yf_calc - yf, psif_calc - psif]

# 初始猜测
s0 = [0.1, 0.1, 0.1]
sol_s = fsolve(shoot, s0, full_output=True)
lx0, ly0, lpsi0 = sol_s[0]
print(f"协态初值: lx0={lx0:.6f}, ly0={ly0:.6f}, lpsi0={lpsi0:.6f}")

# 重新积分得到完整轨迹
z0 = [x0, y0, psi0, lx0, ly0, lpsi0]
sol = solve_ivp(dynamics, [0, tf], z0, method='RK45',
                rtol=1e-10, atol=1e-12, dense_output=True)
t = np.linspace(0, tf, 200)
traj = sol.sol(t)
x_traj, y_traj, psi_traj = traj[0], traj[1], traj[2]
u_traj = -traj[5]  # u = -lpsi
u_traj = np.clip(u_traj, -0.5, 0.5)

# 验证末端误差
err_x = x_traj[-1] - xf
err_y = y_traj[-1] - yf
err_psi = psi_traj[-1] - psif
print(f"末端误差: dx={err_x:.2e}, dy={err_y:.2e}, dpsi={err_psi:.2e}")
print(f"控制约束检查: max|u|={np.max(np.abs(u_traj)):.4f} (限幅0.5)")

# 输出轨迹采样点（用于绘图）
print("\n轨迹采样 (t, x, y, psi, u):")
for i in range(0, 200, 20):
    print(f"t={t[i]:.1f}, x={x_traj[i]:.3f}, y={y_traj[i]:.3f}, "
          f"psi={psi_traj[i]:.3f}, u={u_traj[i]:.4f}")

运行与验证

运行上述代码，终端将输出协态初值、末端误差与控制约束检查。判定标准：

1. 末端误差 \(|dx|,|dy|,|d\psi|\) 均小于 \(10^{-6}\)，说明打靶法收敛到满足边界条件的解；
2. \(\max|u(t)|\le 0.5\)，说明控制约束未被违反，极小值原理推导有效；
3. 轨迹从 \((0,0)\) 平滑过渡到 \((100,100)\)，航向从 \(0\) 增至 \(\pi/2\)，符合转弯物理过程。

最优转弯轨迹示意

图中蓝色曲线为最优转弯轨迹，起点航向水平向右，末端航向竖直向上。轨迹呈现平滑的S形弯曲，对应控制输入先正向偏转、后反向回正的过程，符合能量最优的物理直觉。通过打靶法求得的协态初值保证了所有边界条件同时满足，验证了间接法求解最优控制问题的完整流程。