# missile

**Repository Path**: wzx046/professional-practice

## Basic Information

- **Project Name**: missile
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2023-01-22
- **Last Updated**: 2023-04-12

## Categories & Tags

**Categories**: Uncategorized

**Tags**: 飞行器, 导引律, missile, 课设

## README

# 仿真演示missile击落飞行目标
## 参数限制条件
无/敏*感*词/版见[我的博客](https://wzx046.github.io/post/34/) 
1. 飞行器目标可设定高度5-20km，飞行速度400-800m/s，速度大小不变；但可在飞行水平面机动，机动转向最大速度为0.1rad/s；
2. missile初速800m/s，风阻f=-bV， b=0.02m，m为missile质量；重力加速度9.81m/s^2;
3. missile点火提供前进方向推进力，单次推进可持续2s：f=ma, a=100m/s^2；最多可以提供3次点火推进；
4. missile基地雷达感知范围40km，飞行器感知missile范围为20km，missile进入该范围即启动机动规避；
5. 飞行目标可以在飞行平面转向，转向最大速度为0.1rad/s；不考虑飞行目标垂直方向机动。
## missile算法部分
### 导引律
导引律是用来引导飞行器到目的点或与目标相遇的算法。在本项目中，missile的导引律使用了比例导引法，其核心思想是missile飞行过程中，missile速度向量的转动角速度与目标视线的转动角速度成比例。  
比例导引法导引关系式：  
$$\frac{d\sigma}{dt} - K\frac{dq}{dt} = 0$$  
核心实现代码如下：  
```c
void MissileControl() 
{
	double angleXY, angleZ, last_angleXY, last_angleZ;
	double y0 = last_Aerocraft_y - last_Missile_y;
	double x0 = last_Aerocraft_x - last_Missile_x;
	last_angleXY = atan2(y0, x0);  // 上一时刻目标视线水平方位角
	last_angleZ = atan2(last_Aerocraft_z - last_Missile_z, sqrt(y0 * y0 + x0 * x0));  // 上一时刻目标视线竖直方位角

	double y1 = Aerocraft.pos_y - Missile.pos_y;
	double x1 = Aerocraft.pos_x - Missile.pos_x;
	angleXY = atan2(y1, x1);  // 当前时刻目标视线水平方位角
	angleZ = atan2(Aerocraft.height - Missile.height, sqrt(y1 * y1 + x1 * x1));
	Missile.angle_xy = (angleXY - last_angleXY) + Missile.angle_xy;
	Missile.angle_z = (angleZ - last_angleZ) + Missile.angle_z;
}
```
### 点火策略
missile点火提供前进方向推进力，最多可以提供3次点火推进。在missile刚起飞时，记录下此时missile与飞行器的最大距离，并且在此点火第一次。  
当运行到两者的距离达到之前记录的最大距离一半时，如果当前没有加速，进行第二次点火；如果正在加速，则等待加速完毕再点火。同时更新最大距离为当前的距离。  
按照上述流程再进行第三次点火。并且，如果当垂直方向的速度分量小于100米每秒时，就会进行一次点火补充垂直速度。核心代码如下。  
```c
if (distance < 0.5 * max_dis && Missile.if_speedup == 0)  // 一半路程且未加速
{
	Missile.if_speedup = 1;
	max_dis = distance;
}
if (Missile.height - last_Missile_z < 10 && Missile.fire != 0) // 竖直方向速度分量小于100
{
	Missile.if_speedup = 1;
	max_dis = distance;
}
```
## 飞行器算法部分
根据米国airforce基本战斗机动BFM的描述，当遭遇missile威胁时，用方位角对付missile。当missile飞向你，要尽快转弯，使missile在你的 3/9 线上，给missile制造最大的制导难题。missile要击中你需要按照提前量飞行，而你这样就使missile的提前量最大。而且你会以最大视线率穿越missile的制导头视野。  
简单来说，就是尽量要让missile时刻处于飞机的3点钟或者9点钟方向，使missile的提前量达到最大。所以，要尽量让missile的位置距离机头和机尾的距离相同，此时的missile一定位于飞机的3点钟或者9点钟方向。核心代码如下所示。  
```c
double temp_angle;  // 方位角
temp_angle = atan2(Missile.pos_y - Aerocraft.pos_y, Missile.pos_x - Aerocraft.pos_x);
double three_clock = Aerocraft.angle;  // 机头方向
double nine_clock = Aerocraft.angle + 1.57*2;  // 机尾方向
double temp_x = cos(temp_angle);
double temp_y = sin(temp_angle);
double three_x = temp_x - cos(three_clock);
double three_y = temp_y - sin(three_clock);
double nine_x = temp_x - cos(nine_clock);
double nine_y = temp_y - sin(nine_clock);
double three_dis = three_x * three_x + three_y * three_y;
double nine_dis = nine_x * nine_x + nine_y * nine_y;
```
实现飞机的轨迹控制也很简单，在机器人运动学里，只需角速度和线速度便可实现平面内的轨迹控制。而飞行器的高度是固定的，不考虑垂直面的机动，所以可以直接用一个角速度和线速度来控制轨迹。其中，V 和 W 分别表示飞行器的线速度和角速度。当角速度不为 0 时，飞行器作圆周运动；当角速度为 0 时，飞行器作直线运动。W 也是有方向的，逆时针为正，顺时针为负。通过控制 W 和 V 就可以控制飞行器轨迹。核心代码如下。  
```c
Aerocraft.pos_x = Aerocraft.pos_x + cos(Aerocraft.angle) * Aerocraft.speed * TIME;
Aerocraft.pos_y = Aerocraft.pos_y + sin(Aerocraft.angle) * Aerocraft.speed * TIME;
// 逆时针
Aerocraft.angle = Aerocraft.angle + 0.1 * TIME;
// 顺时针
Aerocraft.angle = Aerocraft.angle - 0.1 * TIME;
```