比例导引详解(Proportional navigation guidance,PNG)-及Python程序

news2024/11/26 15:29:39

模型算法推导

比例导引是一种制导算法，其经典程度相当于控制器中的PID，在本文中，只对其二维平面的情况做分析，考虑一个拦截弹拦截机动目标（固定目标相当于目标速度为0），其运动如下图所示：

$M_i$ 和 $T$ 代表第 $i$ 枚拦截弹和目标；连线 $MT$ 称为拦截弹 $M_i$ 与目标 $T$ 的视线； $r_i$ 为弹目间的相对距离， $r_i$ 关于时间的导数表示为 $\dot r_i$ ， $q_i$ 为拦截弹 $M_i$ 视线角，其关于时间的导数为 $\dot q_i$ ， $V_M,V_T$ 分别表示拦截弹和目标的速度。 $\theta_i$ 为弹目间速度方向与水平线的夹角，称为速度方向角。拦截弹和目标的前置角分别为 $\psi_i = q_i - \theta_i$ 。

考虑平面内拦截单一机动目标的制导问题，拦截弹与目标的相对运动关系可以用如下微分方程表示
$\begin{align} {\dot{r}}_{i} &= V_{T}\cos\left(q_{i}-\theta_{T}\right)-V_{Mi}\cos\left(q_{i}-\theta_{Mi}\right) \\ r_i {\dot{q}}_{i} &= - V_{T}\sin\left(q_{i}-\theta_{T}\right)-V_{Mi}\sin\left(q_{i}-\theta_{Mi}\right) \\ \end{align}$
拦截弹与目标的自身位置机动信息可以用如下微分方程表示
$\begin{cases} \dot x_i = V_{Mi} \cos(\theta_{Mi}) \\ \dot y_i = V_{Mi} \sin(\theta_{Mi}) \\ \dot \theta_{Mi} = \frac{a_{Mi}}{V_{Mi}} \\ \end{cases}$
$\begin{cases} \dot x_i = V_{T} \cos(\theta_{T}) \\ \dot y_i = V_{T} \sin(\theta_{T}) \\ \dot \theta_{T} = \frac{a_{T}}{V_{T}} \\ \end{cases}$
比例导引法即使拦截弹速度矢量的旋转角速度与目标线旋转角速度成正比的一种导引方法，这样就可以保证拦截弹拦截到目标，即
$a_{Mi} = N V_{Mi} \dot q_i$
其中N为比例导引系数，一般为2-6之间的一个值，选值应该在 $1<N<\infty$ ，比例导引法的弹道特性介于追踪法和平行接近法之间，比例系数越大，弹道越平直，需用法向过载越小。

python代码

我们这里考虑用python实现，并不是matlab，其一python的结构更加清晰一点，其二后期可能考虑用一些数据驱动的方法用在制导上，用python会更方便。程序的github地址为 https://github.com/professorli123/PNG.git

程序效果图，其中蓝线为拦截弹，黄线为目标

main程序是文件的主程序

model程序定义了拦截弹模型，以及中间的视线角距离的计算模型

settings程序定义了一些配置

如果只是使用本程序，只需要考虑修改main程序和settings程序

main.py

# -*- encoding: utf-8 -*-
"""
@File       :   main.py
@Contact    :   lyajpunov@hust.edu.cn
@Author     :   Li Yajun
@Time       :   2023/9/21 9:11
@Version    :   1.0
@Desciption :   
"""

from model import missile, battle
import matplotlib.pyplot as plt
from settings import length

M0 = missile(0)
T0 = missile('T')
B = battle(M0, T0)

if __name__ == '__main__':
    for i in range(length - 10):
        a = 4 * M0.get_v() * B.get_dtheta_l()
        M0.step(a)
        T0.step(1)

        B.step()

        if B.get_r() < 1:
            print(B.get_r())
            break

plt.figure(1)
plt.plot(M0.x[:M0.pos], M0.y[:M0.pos])
plt.plot(T0.x[:T0.pos], T0.y[:T0.pos])
plt.show()

settings.py

# -*- encoding: utf-8 -*-
"""
@File       :   settings.py
@Contact    :   lyajpunov@hust.edu.cn
@Author     :   Li Yajun
@Time       :   2023/9/21 9:15
@Version    :   1.0
@Desciption :   
"""

# 最长仿真时间
t = 100
# 仿真步长
dt = 1e-3
# 预留数组长度
length = int(t / dt)

data = {
    0: {
        'x': 0,
        'y': 0,
        'v': 300,
        'theta': 0,
    },

    'T': {
        'x': 10000,
        'y': 10000,
        'v': 50,
        'theta': 0,
    },
}

model.py

# -*- encoding: utf-8 -*-
"""
@File       :   model.py
@Contact    :   lyajpunov@hust.edu.cn
@Author     :   Li Yajun
@Time       :   2023/9/21 9:11
@Version    :   1.0
@Desciption :
"""

import numpy as np
from settings import dt, length, data


class missile:
    def __init__(self, name):
        self.pos = 0
        self.name = name
        self.length = length
        # x轴位置
        self.x = np.zeros(self.length, dtype='float64')
        # y轴位置
        self.y = np.zeros(self.length, dtype='float64')
        # 速度
        self.v = np.zeros(self.length, dtype='float64')
        # 弹道倾角
        self.theta = np.zeros(self.length, dtype='float64')
        # 控制量：加速度
        self.a = np.zeros(self.length, dtype='float64')
        # 停止标志位
        self.end = False
        # 初始化，重置
        self.reset()

    def reset(self):
        self.pos = 0
        self.x[self.pos] = data[self.name]['x']
        self.y[self.pos] = data[self.name]['y']
        self.v[self.pos] = data[self.name]['v']
        self.theta[self.pos] = np.deg2rad(data[self.name]['theta'])

    def step(self, a):
        # 如果已经结束直接返回
        if self.end:
            return

        # 保存控制量
        self.a[self.pos] = a

        # 形成向量进行迭代
        vector = np.array([self.x[self.pos],
                           self.y[self.pos],
                           self.v[self.pos],
                           self.theta[self.pos],
                           self.a[self.pos], ])
        k1 = dt * self.iterateOnce(vector)
        k2 = dt * self.iterateOnce(vector + 0.5 * k1)
        k3 = dt * self.iterateOnce(vector + 0.5 * k2)
        k4 = dt * self.iterateOnce(vector + k3)
        vector = vector + (k1 + 2 * k2 + 2 * k3 + k4) / 6

        # 保存迭代数据
        self.pos += 1
        self.x[self.pos] = vector[0]
        self.y[self.pos] = vector[1]
        self.v[self.pos] = vector[2]
        self.theta[self.pos] = vector[3]

    def iterateOnce(self, vector):
        x, y, v, theta, a = vector[0], vector[1], vector[2], vector[3], vector[4]

        _x = v * np.cos(theta)
        _y = v * np.sin(theta)
        _v = 0
        _theta = a / v
        _a = 0

        return np.array([_x, _y, _v, _theta, _a])

    '''以下为接口函数'''
    def get_x(self):
        return self.x[self.pos]

    def get_y(self):
        return self.y[self.pos]

    def get_v(self):
        return self.v[self.pos]

    def get_a(self):
        return self.a[self.pos]

    def get_theta(self):
        return self.theta[self.pos]


class battle:
    def __init__(self, M, T):
        self.pos = 0
        self.M = M
        self.T = T
        self.length = length
        # 距离及其导数
        self.r = np.zeros(self.length, dtype='float64')
        self.dr = np.zeros(self.length, dtype='float64')
        # 视线倾角及其导数
        self.theta_l = np.zeros(self.length, dtype='float64')
        self.dtheta_l = np.zeros(self.length, dtype='float64')
        # 初始化
        self.reset()

    def reset(self):
        self.pos = 0
        d_x = self.T.get_x() - self.M.get_x()
        d_y = self.T.get_y() - self.M.get_y()
        d_vx = self.T.get_v() * np.cos(self.T.get_theta()) - self.M.get_v() * np.cos(self.M.get_theta())
        d_vy = self.T.get_v() * np.sin(self.T.get_theta()) - self.M.get_v() * np.sin(self.M.get_theta())

        r = np.sqrt(d_x * d_x + d_y * d_y)
        d_r = (d_x * d_vx + d_y * d_vy) / r
        theta_l = np.arctan(d_y / d_x)
        dtheta_l = d_x * d_x * (d_vy * d_x - d_vx * d_y) / (r * r) / (d_vx * d_vx)

        self.r[self.pos] = r
        self.dr[self.pos] = d_r
        self.theta_l[self.pos] = theta_l
        self.dtheta_l[self.pos] = dtheta_l

    def step(self):
        self.pos += 1

        d_x = self.T.get_x() - self.M.get_x()
        d_y = self.T.get_y() - self.M.get_y()
        d_vx = self.T.get_v() * np.cos(self.T.get_theta()) - self.M.get_v() * np.cos(self.M.get_theta())
        d_vy = self.T.get_v() * np.sin(self.T.get_theta()) - self.M.get_v() * np.sin(self.M.get_theta())

        r = np.sqrt(d_x * d_x + d_y * d_y)
        d_r = (d_x * d_vx + d_y * d_vy) / r
        theta_l = np.arctan(d_y / d_x)
        dtheta_l = d_x * d_x * (d_vy * d_x - d_vx * d_y) / (r * r) / (d_vx * d_vx)

        self.r[self.pos] = r
        self.dr[self.pos] = d_r
        self.theta_l[self.pos] = theta_l
        self.dtheta_l[self.pos] = dtheta_l

    '''下面是接口函数'''
    def get_r(self):
        return self.r[self.pos]

    def get_dr(self):
        return self.dr[self.pos]

    def get_theta_l(self):
        return self.theta_l[self.pos]

    def get_dtheta_l(self):
        return self.dtheta_l[self.pos]