比例导引详解(Proportional navigation guidance,PNG) |
您所在的位置:网站首页 › 弹道倾角 › 比例导引详解(Proportional navigation guidance,PNG) |
模型算法推导
比例导引是一种制导算法,其经典程度相当于控制器中的PID,在本文中,只对其二维平面的情况做分析,考虑一个拦截弹拦截机动目标(固定目标相当于目标速度为0),其运动如下图所示: M i M_i Mi 和 T T T 代表第 i i i 枚拦截弹和目标;连线 M T MT MT 称为拦截弹 M i M_i Mi 与目标 T T T 的视线; r i r_i ri 为弹目间的相对距离, r i r_i ri 关于时间的导数表示为 r ˙ i \dot r_i r˙i, q i q_i qi 为拦截弹 M i M_i Mi 视线角,其关于时间的导数为 q ˙ i \dot q_i q˙i , V M , V T V_M,V_T VM,VT分别表示拦截弹和目标的速度。 θ i \theta_i θi 为弹目间速度方向与水平线的夹角,称为速度方向角。拦截弹和目标的前置角分别为 ψ i = q i − θ i \psi_i = q_i - \theta_i ψi=qi−θi。 考虑平面内拦截单一机动目标的制导问题,拦截弹与目标的相对运动关系可以用如下微分方程表示 r ˙ i = V T cos ( q i − θ T ) − V M i cos ( q i − θ M i ) r i q ˙ i = − V T sin ( q i − θ T ) − V M i sin ( q i − θ M 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} r˙iriq˙i=VTcos(qi−θT)−VMicos(qi−θMi)=−VTsin(qi−θT)−VMisin(qi−θMi) 拦截弹与目标的自身位置机动信息可以用如下微分方程表示 { x ˙ i = V M i cos ( θ M i ) y ˙ i = V M i sin ( θ M i ) θ ˙ M i = a M i V M i \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} ⎩ ⎨ ⎧x˙i=VMicos(θMi)y˙i=VMisin(θMi)θ˙Mi=VMiaMi { x ˙ i = V T cos ( θ T ) y ˙ i = V T sin ( θ T ) θ ˙ T = a T V T \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} ⎩ ⎨ ⎧x˙i=VTcos(θT)y˙i=VTsin(θT)θ˙T=VTaT 比例导引法即使拦截弹速度矢量的旋转角速度与目标线旋转角速度成正比的一种导引方法,这样就可以保证拦截弹拦截到目标,即 a M i = N V M i q ˙ i a_{Mi} = N V_{Mi} \dot q_i aMi=NVMiq˙i 其中N为比例导引系数,一般为2-6之间的一个值,选值应该在 1 < N < ∞ 1 '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 : [email protected] @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] |
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |
CopyRight 2018-2019 实验室设备网 版权所有 win10的实时保护怎么永久关闭 |