//////////////////////////////////////////////////
main_train.py源码
from tqdm import tqdm
from pathlib import Path
import argparse
import matplotlib.pyplot as plt
import time
import torch
import os
import datetime
import numpy as np
from replaybuffer.buffer import ReplayBuffer
from agent.PPOagent_d import PPOdiscrete
from agent.PPO import PPOContinuous,set_seed
from utils.file import load_json_file
from envs.afsim_base import AFsim_Base1
from utils.obsdecode import get_real_obs,normalize_state,get_cmd_actions,take_obs,process_action,conversion_action
from utils.file import write_reward
def train(**kwargs):
# 获取当前日期和时间，格式化为字符串
current_datetime = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
# 创建文件名file_name_reward
file_name_reward = f"episode_reward_{current_datetime}.txt"
file_error1 = f"error_{current_datetime}.txt"
file_action = f"action_{current_datetime}.txt"
convergence_plot_path = f"convergence_{current_datetime}.png"
parser = argparse.ArgumentParser()
parser.add_argument('--seed', type=int, default=431,help='seed for numpy and torch')
parser.add_argument('--speed', type=int, default=20,help='speed for afsim')
parser.add_argument('--cuda', action='store_true', default=True,help='use cuda or not')
parser.add_argument("--max_step", type=int, default=400,help='Number of environment steps to train (default: 2e4)')
parser.add_argument("--save-interval", type=int, default=600,help="the interval of model saving")
parser.add_argument("--max_episodes", type=int, default=500,help="Maximum number of Episodes for training")
parser.add_argument("--actor_lr", type=float, default=1e-4,help='learning rate (default: 3e-4)')
parser.add_argument("--critic_lr", type=float, default=2e-4,help="Critic network learning rate (default:1e-3)")
parser.add_argument("--device", type=str, choices=['cpu', 'cuda'], default='cuda',help="Device to use: 'cpu' or 'cuda' (default: 'cuda')")
parser.add_argument("--run_dir", type=str, default=str(Path(os.path.dirname(os.path.abspath(__file__)) + "/results")),help="Directory to save results (default: './results')")
parser.add_argument("--model_dir", type=str, default=str(Path(os.path.dirname(os.path.abspath(__file__)) + "/results/model")),help="Directory to save model (default: './results/model')")
parser.add_argument("--batch_size", type=int, default=64,help="Batch size for training (default: 512)")
parser.add_argument("--hidden_dim", type=int, default=256,help="Number of hidden layers in neural networks")
parser.add_argument("--gamma",type=float, default=0.98,help="Discount factor")
parser.add_argument("--lmbda",type=float, default=0.95,help="GAE funciton parameters")
parser.add_argument("--eps", type=float, default=0.2,help="clip range parameters")
parser.add_argument("--epochs",type=int, default=8,help="Number of network updates for each batch size")
parser.add_argument("--resume_episode", type=int,default=59,help="Episode number to resume training from")
parser.add_argument("--init_angle", type=int,default=0,help="Initialization angle")
parser.add_argument("--step_size", type=float,default=0.00135*3,help="Step size of each movement")
args = parser.parse_args()
# record every Episode reward 4
sum_reward_list = []
device = torch.device(args.device if args.device == 'cuda' and torch.cuda.is_available() else 'cpu')
# get save path
run_dir = Path(args.run_dir)
model_dir = Path(args.model_dir)
if not run_dir.exists():
os.makedirs(str(run_dir))
if not model_dir.exists():
os.makedirs(str(model_dir))
set_seed(args.seed)
# 初始化经验缓存区
repalybuffer = ReplayBuffer()
# 初始化AFSIM 环境
afsim_env = AFsim_Base1(**kwargs)
# state_dim = afsim_env.observation_space.shape[0]
# action_dim = afsim_env.action_space.shape[0]
# action_dim = afsim_env.action_space.n
state_dim = 12
action_dim = 3
# 初始化智能体
# agent_sim = PPOContinuous(args.max_episodes,state_dim, args.hidden_dim, action_dim, args.actor_lr, args.critic_lr,
# args.lmbda, args.epochs, args.eps, args.gamma, device)
agent_sim = PPOdiscrete(args.max_episodes,state_dim, args.hidden_dim, action_dim, args.actor_lr, args.critic_lr,
args.lmbda, args.epochs, args.eps, args.gamma, device)
# 加载模型
# agent_sim.load(args.resume_episode, model_dir)
for i in range(10):
with tqdm(total=int(args.max_episodes/10), desc='Iteration %d' % i) as pbar:
for episode in range(int(args.max_episodes/10)):
try:
# 启动想定(重置仿真环境)
afsim_env.reset()
sp = afsim_env.speed(args.speed)
# 获取obs并对ammon进行赋值
afsim_env.init_obs_ammon()
# episode 回报
episode_return = 0.0
# 获取原始态势
_obs = afsim_env._get_obs()
# 获取 转换后态势
_,state_obs = get_real_obs(_obs)
# 归一化
# state_obs = normalize_state(state_obs)
#判断当前agent状态纬度（是否丢失信息）,如果丢失，跳过
sta_tim = time.time()
current_angle = args.init_angle
for step in range(int(args.max_step)):
# 获取动作
ac_index, action = agent_sim.take_action(state_obs)
# ac_index = process_action(ac_index,indx)
# 处理agent方向
current_angle,action = conversion_action(ac_index,current_angle,args.step_size)
with open(file_action,"a") as f:
f.write(f"{str(current_angle)},{str(action)}\n")
# 处理动作，获取可执行动作
actions,acions_step = get_cmd_actions(_obs,ac_index,action)
# print(actions['command']['wayPoints'][0])
# 位置
# print(f"上一时刻位置：lon:{actions['command']['wayPoints'][-1]['Lon']},lat:{actions['command']['wayPoints'][-1]['Lat']}")
next_obs_reward = afsim_env.step(sp,ac_index,_obs,actions,action,step)
# for i in range(5):
# next_obs_reward = afsim_env.step(sp,ac_index,_obs,actions,action,step)
# time.sleep(0.1)
# if i==1:
# time.sleep(0.3)
# next_obs_reward = afsim_env.step(sp,ac_index,_obs,acions_step,action,step)
# else:
# next_obs_reward = afsim_env.step(sp,ac_index,_obs,actions,action,step)
_obs,next_state, reward, done = next_obs_reward
if isinstance(next_state, list) and len(next_state) == 0:
# 如果 next_state 是空列表，跳出循环
with open(file_error1,"a") as fil:
fil.write(f"{i} {episode} {step}\n")
break
else:
# 如果 next_state 是数组或其他非空内容，继续执行
pass
# 处理异常位置
previous_state = take_obs(_obs)
if previous_state:
break
# print(f"reward:{reward}")
# write_reward(reward)
episode_return += reward
# 将数据存储到经验池
repalybuffer.add(state_obs,ac_index,next_state,reward,done)
state_obs = next_state
print(f"Step {step}: {'*' * 20}")
print(ac_index)
if (step+1) % args.batch_size == 0:
batch_data = repalybuffer.sample()
agent_sim.update(batch_data)
repalybuffer.clear()
if done:
break
if (episode+1) % (args.max_episodes/100) == 0:
agent_sim.save(episode+100*i,model_dir)
# agent_sim.save(episode+args.resume_episode,model_dir)
print(f"Episdoe:{episode}, reward:{episode_return}")
sum_reward_list.append(episode_return)
with open(run_dir / file_name_reward,'a') as f:
f.write(str(episode_return)+'\n')
if (episode+1) % 10 == 0:
pbar.set_postfix({'episode': '%d' % (args.max_episodes/10 * i + episode+1), 'return': '%.3f' % np.mean(sum_reward_list[-10:])})
pbar.update(1)
except:
continue
agent_sim.update_schedulers(episode)
episodes_list = list(range(len(sum_reward_list)))
plt.figure(figsize=(10, 5))
plt.plot(episodes_list, sum_reward_list, label='Episode Return', alpha=0.45, color='tab:blue')
if len(sum_reward_list) >= 10:
smooth_rewards = np.convolve(sum_reward_list, np.ones(10) / 10, mode='valid')
smooth_episodes = list(range(9, len(sum_reward_list)))
plt.plot(smooth_episodes, smooth_rewards, label='Moving Average (10)', linewidth=2, color='tab:red')
plt.xlabel('Episodes')
plt.ylabel('Returns')
plt.title(f'Training Convergence Curve\n{file_name_reward}')
plt.grid(True, alpha=0.3)
plt.legend()
plt.tight_layout()
plt.savefig(run_dir / convergence_plot_path, dpi=300, bbox_inches='tight')
plt.show()
if __name__ == '__main__':
run_args = load_json_file('config/run.json')
process = train(**run_args)
exit(0)
//////////////////////////////////////
plot.py源码
from pathlib import Path
import argparse
import matplotlib.pyplot as plt
import numpy as np
def parse_args():
parser = argparse.ArgumentParser()
parser.add_argument("file", nargs="?", help="Path to an episode_reward_*.txt file")
parser.add_argument("--window", type=int, default=10, help="Moving average window size")
parser.add_argument("--output", type=str, help="Optional path to save the figure")
parser.add_argument("--no-show", action="store_true", help="Save or prepare the figure without showing it")
return parser.parse_args()
def resolve_reward_file(file_arg):
if file_arg:
file_path = Path(file_arg)
if not file_path.exists():
raise FileNotFoundError(f"Reward file not found: {file_path}")
return file_path
candidates = list(Path.cwd().glob("episode_reward_*.txt"))
if not candidates:
raise FileNotFoundError("No episode_reward_*.txt files found in the current directory.")
return max(candidates, key=lambda path: path.stat().st_mtime)
def load_rewards(file_path):
rewards = []
with file_path.open("r", encoding="utf-8") as file:
for line in file:
line = line.strip()
if not line:
continue
rewards.append(float(line))
if not rewards:
raise ValueError(f"Reward file is empty: {file_path}")
return np.array(rewards, dtype=np.float32)
def moving_average(values, window):
if window <= 1 or len(values) < window:
return None
kernel = np.ones(window, dtype=np.float32) / window
return np.convolve(values, kernel, mode="valid")
def main():
args = parse_args()
reward_file = resolve_reward_file(args.file)
rewards = load_rewards(reward_file)
episodes = np.arange(1, len(rewards) + 1)
plt.figure(figsize=(10, 5))
plt.plot(episodes, rewards, label="Episode Return", alpha=0.45, color="tab:blue")
smooth_rewards = moving_average(rewards, args.window)
if smooth_rewards is not None:
smooth_episodes = np.arange(args.window, len(rewards) + 1)
plt.plot(smooth_episodes, smooth_rewards, label=f"Moving Average ({args.window})", linewidth=2, color="tab:red")
plt.title(f"Training Convergence Curve\n{reward_file.name}")
plt.xlabel("Episode")
plt.ylabel("Return")
plt.grid(True, alpha=0.3)
plt.legend()
plt.tight_layout()
if args.output:
output_path = Path(args.output)
output_path.parent.mkdir(parents=True, exist_ok=True)
plt.savefig(output_path, dpi=300, bbox_inches="tight")
print(f"Saved figure to {output_path}")
if not args.no_show:
plt.show()
if __name__ == "__main__":
main()
/////////////////////////////
ppo.py的源码
# 使用连续PPO算法构建智能体
import torch.optim.lr_scheduler as lr_scheduler
import math
import torch
import torch.nn.functional as F
import torch.nn as nn
import numpy as np
import matplotlib.pyplot as plt
from tqdm import tqdm
import utils.rl_utils as rl_utils
import random
def set_seed(seed):
random.seed(seed)
np.random.seed(seed)
torch.manual_seed(seed)
if torch.cuda.is_available():
torch.cuda.manual_seed(seed)
torch.cuda.manual_seed_all(seed) # if you are using multi-GPU.
# Ensure reproducibility in cudnn
torch.backends.cudnn.deterministic = True
torch.backends.cudnn.benchmark = False
class PolicyNet(torch.nn.Module):
def __init__(self, state_dim, hidden_dim, action_dim):
super(PolicyNet, self).__init__()
self.fc1 = torch.nn.Linear(state_dim, hidden_dim)
self.fc2 = torch.nn.Linear(hidden_dim, action_dim)
self._init_weights()
def _init_weights(self):
# 使用 Kaiming 均匀初始化方法对 全连接层 的权重进行初始化，并将偏置初始化为零
nn.init.kaiming_uniform_(self.fc1.weight, nonlinearity='relu')
nn.init.zeros_(self.fc1.bias)
nn.init.kaiming_uniform_(self.fc2.weight, nonlinearity='relu')
nn.init.zeros_(self.fc2.bias)
def forward(self, x):
x = F.relu(self.fc1(x))
return F.softmax(self.fc2(x), dim=1)
class ValueNet(torch.nn.Module):
def __init__(self, state_dim, hidden_dim):
super(ValueNet, self).__init__()
self.fc1 = torch.nn.Linear(state_dim, hidden_dim)
self.fc2 = torch.nn.Linear(hidden_dim, 1)
self._init_weights()
def _init_weights(self):
# 使用 Kaiming 均匀初始化方法对 全连接层 的权重进行初始化，并将偏置初始化为零
nn.init.kaiming_uniform_(self.fc1.weight, nonlinearity='relu')
nn.init.zeros_(self.fc1.bias)
nn.init.kaiming_uniform_(self.fc2.weight, nonlinearity='relu')
nn.init.zeros_(self.fc2.bias)
def forward(self, x):
x = F.relu(self.fc1(x))
return self.fc2(x)
class PPO:
''' PPO算法,采用截断方式 '''
def __init__(self, state_dim, hidden_dim, action_dim, actor_lr, critic_lr,
lmbda, epochs, eps, gamma, device):
self.actor = PolicyNet(state_dim, hidden_dim, action_dim).to(device)
self.critic = ValueNet(state_dim, hidden_dim).to(device)
self.actor_optimizer = torch.optim.Adam(self.actor.parameters(),
lr=actor_lr)
self.critic_optimizer = torch.optim.Adam(self.critic.parameters(),
lr=critic_lr)
self.gamma = gamma
self.lmbda = lmbda
self.epochs = epochs # 一条序列的数据用于训练轮数
self.eps = eps # PPO中截断范围的参数
self.device = device
def take_action(self, state):
state = torch.tensor([state], dtype=torch.float).to(self.device)
probs = self.actor(state)# 表示给定状态下各个动作的概率分布
action_dist = torch.distributions.Categorical(probs)# 创建概率分布对象，表示抽中0的动作概率是probs[0],抽中1的概率概率是probs[1]
action = action_dist.sample()
return action.item()
def update(self, transition_dict):
states = torch.tensor(transition_dict['states'],
dtype=torch.float).to(self.device)
actions = torch.tensor(transition_dict['actions']).view(-1, 1).to(
self.device)
rewards = torch.tensor(transition_dict['rewards'],
dtype=torch.float).view(-1, 1).to(self.device)
next_states = torch.tensor(transition_dict['next_states'],
dtype=torch.float).to(self.device)
dones = torch.tensor(transition_dict['dones'],
dtype=torch.float).view(-1, 1).to(self.device)
td_target = rewards + self.gamma * self.critic(next_states) * (1 -
dones)
td_delta = td_target - self.critic(states)
advantage = rl_utils.compute_advantage(self.gamma, self.lmbda,
td_delta.cpu()).to(self.device)
old_log_probs = torch.log(self.actor(states).gather(1,# torch.log(...)：对上一步获取的概率值取对数。这在强化学习中常用，因为对数概率在计算损失和进行策略更新时更加稳定和有效
actions)).detach()
for _ in range(self.epochs):
log_probs = torch.log(self.actor(states).gather(1, actions))
ratio = torch.exp(log_probs - old_log_probs)
surr1 = ratio * advantage
surr2 = torch.clamp(ratio, 1 - self.eps,
1 + self.eps) * advantage # 截断
actor_loss = torch.mean(-torch.min(surr1, surr2)) # PPO损失函数
critic_loss = torch.mean(
F.mse_loss(self.critic(states), td_target.detach()))
self.actor_optimizer.zero_grad()
self.critic_optimizer.zero_grad()
actor_loss.backward()
critic_loss.backward()
self.actor_optimizer.step()
self.critic_optimizer.step()
class PolicyNetContinuous(torch.nn.Module):
def __init__(self, state_dim, hidden_dim, action_dim):
super(PolicyNetContinuous, self).__init__()
self.fc1 = torch.nn.Linear(state_dim, hidden_dim)
self.fc2 = torch.nn.Linear(hidden_dim,hidden_dim)
self.fc3 = torch.nn.Linear(hidden_dim,hidden_dim)
self.fc_mu = torch.nn.Linear(hidden_dim, action_dim)
self.fc_std = torch.nn.Linear(hidden_dim, action_dim)
# self._init_weights()
def _init_weights(self):
# 使用 Kaiming 均匀初始化方法对 全连接层 的权重进行初始化，并将偏置初始化为零
nn.init.kaiming_uniform_(self.fc1.weight, nonlinearity='relu')
nn.init.zeros_(self.fc1.bias)
nn.init.kaiming_uniform_(self.fc2.weight, nonlinearity='relu')
nn.init.zeros_(self.fc2.bias)
nn.init.kaiming_uniform_(self.fc_mu.weight, nonlinearity='tanh')
nn.init.zeros_(self.fc_mu.bias)
nn.init.kaiming_uniform_(self.fc_std.weight, nonlinearity='relu')
nn.init.zeros_(self.fc_std.bias)
def forward(self, x):
x = F.relu(self.fc1(x))
x = F.relu(self.fc2(x))
x = F.relu(self.fc3(x))
mu = torch.tanh(self.fc_mu(x))# torch.tanh 是双曲正切函数，其输出范围是 [-1, 1]
std = F.softplus(self.fc_std(x))# F.softplus 是一种平滑的 ReLU 变体，输出为正值，避免标准差为负数
return mu, std # std 表示生成动作的标准差，用于控制智能体在当前状态下的探索程度
class ValueNet(torch.nn.Module):
def __init__(self, state_dim, hidden_dim):
super(ValueNet, self).__init__()
self.fc1 = torch.nn.Linear(state_dim, hidden_dim)
self.fc2 = torch.nn.Linear(hidden_dim, hidden_dim)
self.fc3 = torch.nn.Linear(hidden_dim, 1)
# self._init_weights()
def _init_weights(self):
# 使用 Kaiming 均匀初始化方法对 全连接层 的权重进行初始化，并将偏置初始化为零
nn.init.kaiming_uniform_(self.fc1.weight, nonlinearity='relu')
nn.init.zeros_(self.fc1.bias)
nn.init.kaiming_uniform_(self.fc2.weight, nonlinearity='relu')
nn.init.zeros_(self.fc2.bias)
nn.init.kaiming_uniform_(self.fc3.weight, nonlinearity='relu')
nn.init.zeros_(self.fc3.bias)
def forward(self, x):
x = F.relu(self.fc1(x))
x = F.relu(self.fc2(x))
return self.fc3(x)
class PPOContinuous:
''' 处理连续动作的PPO算法 '''
def __init__(self, max_episode,state_dim, hidden_dim, action_dim, actor_lr, critic_lr,
lmbda, epochs, eps, gamma, device):
self.actor = PolicyNetContinuous(state_dim, hidden_dim,
action_dim).to(device)
self.critic = ValueNet(state_dim, hidden_dim).to(device)
self.actor_optimizer = torch.optim.Adam(self.actor.parameters(),
lr=actor_lr)
self.critic_optimizer = torch.optim.Adam(self.critic.parameters(),
lr=critic_lr)
# 学习率调度器
lambda_func = lambda epoch: 1 - epoch / 1000 # 线性衰减，假设训练 1000 episode 完成衰减
lambda_func = lambda episode: 0.5 * (1 + math.cos(math.pi * episode / max_episode)) # 余弦退火
self.actor_scheduler = lr_scheduler.LambdaLR(self.actor_optimizer, lr_lambda=lambda_func)
self.critic_scheduler = lr_scheduler.LambdaLR(self.critic_optimizer, lr_lambda=lambda_func)
self.gamma = gamma
self.lmbda = lmbda
self.epochs = epochs
self.eps = eps
self.device = device
self.max_delta = 0.001
def update_schedulers(self, episode):
"""在每个 episode 后更新调度器"""
self.actor_scheduler.step(episode)
self.critic_scheduler.step(episode)
def take_action(self, state):
state = torch.tensor([state], dtype=torch.float).to(self.device)
mu, sigma = self.actor(state)# 输出均值和标准差
# mu = torch.tanh(mu) * self.max_delta # 缩放到 [-0.00135, 0.00135] 范围内
# sigma = torch.clamp(sigma+1e-6, min=1e-6, max=self.max_delta) # 将标准差裁剪到合适范围
action_dist = torch.distributions.Normal(mu, sigma)# # 定义正态分布
action = action_dist.sample()# 从正态分布中采样
action = action * 1e-3
action = torch.clamp(action, min=-self.max_delta, max=self.max_delta)
# 转为list
action = action.squeeze().tolist()
return action# 返回动作
def update(self, transition_dict):
states = torch.tensor(transition_dict['states'],
dtype=torch.float).to(self.device)
actions = torch.tensor(transition_dict['actions'],
dtype=torch.float).to(self.device)
rewards = torch.tensor(transition_dict['rewards'],
dtype=torch.float).view(-1, 1).to(self.device)
next_states = torch.tensor(transition_dict['next_states'],
dtype=torch.float).to(self.device)
dones = torch.tensor(transition_dict['dones'],
dtype=torch.float).view(-1, 1).to(self.device)
# rewards = (rewards + 250.0) / 250.0 # 和TRPO一样,对奖励进行修改,方便训练
# rewards = (rewards - rewards.mean()) / (rewards.std() + 1e-8) # 和TRPO一样,对奖励进行修改,方便训练
td_target = rewards + self.gamma * self.critic(next_states) * (1 -
dones)
td_delta = td_target - self.critic(states)
advantage = rl_utils.compute_advantage(self.gamma, self.lmbda,
td_delta.cpu()).to(self.device)
# advantage = (advantage - advantage.mean()) / (advantage.std() + 1e-8) # 标准化优势函数
mu, std = self.actor(states)
action_dists = torch.distributions.Normal(mu.detach(), std.detach())# 创建一个正态分布对象 action_dists
# 动作是正态分布
old_log_probs = action_dists.log_prob(actions)# 旧策略下的动作对数概率 old_log_probs
for i in range(self.epochs):
mu, std = self.actor(states)
action_dists = torch.distributions.Normal(mu, std)
log_probs = action_dists.log_prob(actions)
ratio = torch.exp(log_probs - old_log_probs)
surr1 = ratio * advantage
surr2 = torch.clamp(ratio, 1 - self.eps, 1 + self.eps) * advantage
actor_loss = torch.mean(-torch.min(surr1, surr2))
critic_loss = torch.mean(
F.mse_loss(self.critic(states), td_target.detach()))
self.actor_optimizer.zero_grad()
self.critic_optimizer.zero_grad()
actor_loss.backward()
critic_loss.backward()
torch.nn.utils.clip_grad_norm_(self.actor.parameters(), max_norm=0.5) # 添加梯度裁剪
torch.nn.utils.clip_grad_norm_(self.critic.parameters(), max_norm=0.5)
self.actor_optimizer.step()
self.critic_optimizer.step()
# 更新学习率
self.actor_scheduler.step()
self.critic_scheduler.step()
def save(self,episode,moudle_dir):
torch.save(self.actor.state_dict(), f'{moudle_dir}/{episode}PPO_actor_dec.pth')
torch.save(self.critic.state_dict(), f'{moudle_dir}/{episode}PPO_critic_dec.pth')
print('...save model...')
def load(self,moudle_dir):
self.actor.load_state_dict(torch.load(f'{moudle_dir}/PPO_actor_dec.pth'))
self.critic.load_state_dict(torch.load(f'{moudle_dir}/PPO_critic_dec.pth'))
print('...load...')
///////////////////////////////////////////
afsim_base.py源码
import socket
import json
# import gymnasium as gym
import time
import numpy as np
import select
import threading
from utils.obsdecode import cal_reward,decimal_to_dms,print_point,get_real_obs,normalize_state,replace_lat_lon,cal_time_sleep
# import subprocess
def load_json_file(path) -> dict:
with open(path, 'r', encoding='utf-8') as f:
dic = json.load(f)
return dic
def transform_data(data):
# 判断事件类型并设置相应Type值
if data["type"] == "SensorTrackInitiatedEvent":
event_type = "gongji"
else:
event_type = "unknown" # 如有其他事件类型可继续添加分支
# 获取FireID和TargetID
fire_id = str(data["AttackerData"][0]["Index"]) # 将index转换为字符串类型
target_id = str(data["TargetData"][0]["Index"])
# 构造目标结构
transformed_data = [{"Type": event_type, "FireID": fire_id, "TargetID": target_id}]
return transformed_data
# ROOT = 'C:\\NPSim1.0.2'
#def spr(cmd, fileio=None):
# start_up_info = subprocess.STARTUPINFO() # 创建一个 STARTUPINFO 对象，用于设置子进程的启动信息
# start_up_info.dwFlags |= subprocess.STARTF_USESHOWWINDOW # 设置启动信息的标志位，使子进程窗口在启动时不显示
# start_up_info.wShowWindow = False # 指定子进程窗口不显示
# subprocess.Popen(cmd, shell=True, startupinfo=start_up_info, stdout=fileio, stderr=fileio)
import random
from .event_afsim import AFsim_event_Base
# @ray.remote(num_cpus=1)
class AFsim_Base1():
def __init__(self, seed=None,**kwargs) -> None:
self.event_data = None # 用于存储事件数据
self.running = True # 控制线程的运行状态
# self.addres = (kwargs['host'], kwargs['port'])
self.seed_value = seed if seed is not None else random.randint(0, 2**32 - 1) # 随机生成种子
self.seed(self.seed_value)
self.address = ('127.0.0.1', 9654)
self.addressk = ('127.0.0.1', 9657)# 启动想定端口
# self.address = ('192.168.20.12', 9654)
# self.addressk = ('192.168.20.12', 9657)# 启动想定端口
self.udp_sendsocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # 发
self.udp_seversocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # 收
self.udp_seversocket.setblocking(False) # 确保套接字设置为非阻塞模式
try:
self.udp_seversocket.bind(('0.0.0.0', 9659)) # 实时数据 绑定UDP服务器地址和端口
except Exception as e:
print(f"Socket binding error: {e}")
pass # 忽略绑定异常
# self.event_thread = threading.Thread(target=self._event_listener)# 启动事件接收线程
# self.event_thread.start()
self.event_occurss= AFsim_event_Base()
# 根据仿真定义实体状态空间和动作空间 s=[lat_A,lon_A,lat_B,lon_B,(lat_B−lat_A),(lon_B−lon_A)]
# self.observation_space = gym.spaces.Box(
# low=np.array([-180, -180, -90, 0, -180, -90,-4,-4,-4,-4]), # 最低值
# high=np.array([180, 180, 90, 2, 180, 90, 4, 4, 4, 4]), # 最高值
# dtype=np.float32
# )
# self.action_space = gym.spaces.Discrete(4)
self.observation_space = 12
self.action_space = 3
# self.state_dim = 8
# self.action_dim = 4
def seed(self, seed=None):
"""
设置环境的随机种子，保证环境的行为是可复现的。
"""
self.seed_value = seed
np.random.seed(seed)
random.seed(seed)
def reset(self):
cmd = '1,shooterbule_new'# 启动想定命令
self.udp_sendsocket.sendto(cmd.encode('utf-8'), self.addressk) # 通过UDP发送命令
self.event_occurss._recv_event_data()
self._clear_recv_buffer()
self._wait_for_fresh_obs()
print("发送成功")
def _recv_with_timeout(self, timeout=5):
"""
从UDP套接字接收数据,带有超时功能。
"""
ready = select.select([self.udp_seversocket], [], [], timeout)
if ready[0]:
data_obs, addr = self.udp_seversocket.recvfrom(2048)
data_massage = data_obs[:-1].decode('utf-8')
data_obs = None
# return data[:-1].decode('utf-8',errors='ignore') # 假设数据包末尾有一个额外的字符需要去掉
return data_massage
else:
pass
raise TimeoutError("No data received within the timeout period.")
def _clear_recv_buffer(self):
"""
清空 UDP 接收缓冲区。
"""
try:
while True:
ready = select.select([self.udp_seversocket], [], [], 0) # 超时时间设为 0
if ready[0]:
self.udp_seversocket.recvfrom(2048) # 丢弃数据
else:
break # 缓冲区已清空
except BlockingIOError:
pass # 如果套接字非阻塞且无数据，直接返回
def _wait_for_fresh_obs(self, retries=10, timeout=1.0):
for _ in range(retries):
cmd = {'Type': 'entityData'}
self._send(cmd)
try:
datas = self._recv_with_timeout(timeout=timeout)
obs = json.loads(datas)
if obs.get('entityStatusData'):
return obs
except (TimeoutError, json.JSONDecodeError):
continue
raise TimeoutError("Simulator did not provide a fresh observation after reset.")
def _send(self,cmd):
s = json.dumps(cmd) # 将命令转换为json字符串
self.udp_sendsocket.sendto(s.encode('utf-8'), self.address) # 通过UDP发送命令
def _get_obs(self):
# self._clear_recv_buffer()# 清空缓存区
cmd = {'Type': 'entityData'}
self._send(cmd)
try:
datas = self._recv_with_timeout()
return json.loads(datas)
# return datas
except TimeoutError:
print("No data received within the specified time frame")
return {}
except json.JSONDecodeError:
print("Received data is not a valid JSON")
return {}
def _start(self):
cmd = '1,shooterbule_new'# 启动想定命令
self.udp_sendsocket.sendto(cmd.encode('utf-8'), self.addressk) # 通过UDP发送命令
# datazero, client_address = self.udp_seversocket.recvfrom(10240)
print("发送成功")
def _init_ammonNum(self):
init_ammon_cmd = {
"command": {
"Type": "ammon",
"ammonNum": [{"1": "0"},{"2": "0"}]
}}
self._send(init_ammon_cmd)
def init_obs_ammon(self):
self._init_ammonNum()
def opensensor(self):
sensor_open_cmd = {
"command":{
"SensorStatus":"1",
"ID":"1",
"Type": "SensorControl"
}}
self._send(sensor_open_cmd)
def offsensor(self):
sensor_open_cmd = {
"command":{
"SensorStatus":"0",
"ID":"1",
"Type": "SensorControl"
}}
self._send(sensor_open_cmd)
def _recv_event_data(self, timeout=0.05):
"""
从事件数据的UDP套接字接收数据,带有超时功能。
"""
ready = select.select([self.udp_event_seversocket], [], [], timeout)
if ready[0]:
data_event, addr = self.udp_event_seversocket.recvfrom(2048)
print(f"Received event data from {addr}: {data_event.decode()}")
return data_event.decode('utf-8', errors='ignore') # 不去掉末尾字符
else:
return None
def _event_listener(self):
"""事件接收线程函数"""
while self.running:
try:
eventdata = self._recv_event_data(timeout=1)
# self.event_data=data[:-1].decode('utf-8',errors='ignore')
if eventdata:
self.event_data = json.loads(eventdata.rstrip('\x00')) # 仅在事件触发时更新数据
print(f"收到事件信息：{self.event_data}")
except TimeoutError:
pass # 没有事件数据时忽略超时错误
def step(self,sp,indx,state_obs,action,ac,step):
"""
目标:考虑规避设置的危险区以及在任务区可以追踪红方
"""
# 是否开火(开火奖励)
if self.event_occurss._recv_event_data():
self.fire()
reward_fire = 100
self.event_data = None
else:
reward_fire = 0
# reward_fire = 0
self._send(action)
# 时间步惩罚
reward_time_penalty = -1
# TODO
reward_every_step,done,new_point,current_dis = cal_reward(state_obs,action)
time.sleep(3.0/sp)
## TODO new_point 是理论上要移动到的目标点
lat_point = float(new_point['Lat'])
lon_point = float(new_point['Lon'])
# 目标点的经纬度
degree,minute,second = decimal_to_dms(lat_point)
degree1,minute1,second1 = decimal_to_dms(lon_point)
# print(f"理论上要移动到的位置,lon:{degree1}:{minute1}:{second1},lat:{degree}:{minute}:{second},")
original_next_state = self._get_obs()
done, state_obs = get_real_obs(original_next_state)
try:
# 确保 'entityStatusData' 存在并且不为空
entity_status_data = original_next_state.get('entityStatusData', [])
if not entity_status_data:
print("实体状态数据为空或不存在，跳过此轮处理。")
else:
# 获取纬度和经度并转换
degree11, minute11, second11 = decimal_to_dms(float(entity_status_data[0]['Lat']))
degree21, minute21, second21 = decimal_to_dms(float(entity_status_data[0]['Lon']))
print(f"实际获取的下一时刻位置, lon:{degree21}:{minute21}:{second21}, lat:{degree11}:{minute11}:{second11},")
except KeyError as e:
print(f"数据格式错误：缺少预期的字段 - {e}")
pass
except ValueError as e:
print(f"数据转换错误：{e}")
pass
except IndexError as e:
print(f"数据索引错误：数据列表为空或索引超出范围 - {e}")
pass
except Exception as e:
print(f"未知错误：{e}")
pass
reward = reward_fire + reward_time_penalty + reward_every_step
print(f"reward_fire: {reward_fire}, reward_time_penalty: {reward_time_penalty}, reward_every_step: {reward_every_step}")
return original_next_state,state_obs,reward,done# 调试
def render(self):
self._start()
def fire(self):
fire_cmd = {
"command":[{
"FireID":"1",
"Type": "gongji",
"TargetID":"2"}
]
}
# for _ in range(2):
self._send(fire_cmd)
print("攻击指令发送成功")
# 仿真倍速/开始/暂停
def speed(self,sp=1):
speed_cmd = {
"controlCmd":{
"Type":"clockRate","value":str(sp)
}
}
self._send(speed_cmd)
return float(speed_cmd["controlCmd"]["value"])
def continue_cmd(self):
start_cmd = {
"controlCmd":{
"Type":"start",
}
}
self._send(start_cmd)
def pause(self):
start_cmd = {
"controlCmd":{
"Type":"pause",
}
}
self._send(start_cmd)
def restart(self):
resta_cmd = {
"controlCmd":{
"Type":"reStart",
}}
self._send(resta_cmd)
def test(self):
return 5
def close(self):
"""关闭方法，用于停止事件接收线程"""
self.running = False
self.event_thread.join() # 等待线程结束
self.udp_sendsocket.close() # 关闭所有 socket
self.udp_seversocket.close()
self.udp_event_seversocket.close()
# if __name__ == '__main__':
# run_args = load_json_file('config/run.json')
# env = AFsim_Base(**run_args)
# # da1,da2 = env.receive_event_data()
# # env._start()
# # sped_cmd = {
# # "controlCmd":{
# # "Type":"clockRate","Value":'10'
# # }
# # }
# for i in range(5):
# obs = env._get_obs()
# # tim = obs['entity']
# print(obs)
# roll_cmd = {
# "Type":"zitai",
# "Pitch":"45",
# "Roll":"45",
# "ID":"1",
# "Heading":"120"
# }
# sensor_cmd = {
# "SensorStatus":"0",
# "ID":"0",
# "Type": "SensorControl"
# }
# print(env.event_data)
# obs = env._get_obs()
# print(obs)
# init_cmd = {
# "command": {
# "Type": "ammon",
# "ammonNum": [{"1": "12"},{"2": "4"}]
# }}
# env._send(init_cmd)
# obs = env._get_obs()
# print(obs)
# for i in range(1500):
# move_cmd = {
# "command":{
# "ID":"1",
# "Type": "route",
# "wayPoints":[{"Lat": str(23.838383),"Lon": str(120.503411),"Alt": "25000","Speed": "300"}
# ]
# }
# }
# env._send(move_cmd)
# time.sleep(0.1)
# print(i)
# print("hell")
# env._send(move_cmd)
# sensorcontrol = env._send({
# "command":{
# "SensorStatus":"0",
# "ID":"1",
# "Type": "SensorControl"
# }})
# fire_cmd = {
# "command":[{
# "FireID":"1",
# "Type": "gongji",
# "TargetID":"2"}
# ]
# }
# env._send(fire_cmd)
# # env._start()
# move_cmd = {
# "command":{
# "ID":"1",
# "Type": "route",
# "wayPoints":[{"Lat": str(23.838383),"Lon": str(120.503411),"Alt": "25000","Speed": "300"},
# {"Lat": str(24.037283),"Lon": str(120.622508),"Alt": "25000","Speed": "300"},
# {"Lat": str(24.9097283),"Lon": str(119.722508),"Alt": "25000","Speed": "300"}
# ]
# }
# }
# move_cmd = {
# "command":{
# "ID":"1",
# "Type": "route",
# "wayPoints":[{"Lat": str(24.807283),"Lon": str(123.322508),"Alt": "10000","Speed": "300"},
# {"Lat": str(23.407283),"Lon": str(122.222508),"Alt": "10000","Speed": "300"},
# {"Lat": str(22.007283),"Lon": str(121.122508),"Alt": "10000","Speed": "300"}
# ]
# }
# }
# fire_cmd = {
# "command":[{
# "FireID":"1",
# "Type": "gongji",
# "TargetID":"2"}
# ]
# }
# # 初始化载弹量
# init_ammon_cmd = {
# "command": {
# "Type": "ammon",
# "ammonNum": [{"1": "3"},{"2": "3"}]
# }}
//////////////////////////////////////////////////////////
event_afsim.py源码
import socket
import json
import time
import numpy as np
import random
import select
import threading
# from gym import spaces
# import gym
from utils.obsdecode import cal_reward,decimal_to_dms,print_point,get_real_obs,normalize_state,replace_lat_lon,cal_time_sleep
# import subprocess
def load_json_file(path) -> dict:
with open(path, 'r', encoding='utf-8') as f:
dic = json.load(f)
return dic
def transform_data(data):
# 判断事件类型并设置相应Type值
if data["type"] == "SensorTrackInitiatedEvent":
event_type = "gongji"
else:
event_type = "unknown" # 如有其他事件类型可继续添加分支
# 获取FireID和TargetID
fire_id = str(data["AttackerData"][0]["Index"]) # 将index转换为字符串类型
target_id = str(data["TargetData"][0]["Index"])
# 构造目标结构
transformed_data = [{"Type": event_type, "FireID": fire_id, "TargetID": target_id}]
return transformed_data
"""
import time
import json
import socket
def _send(self, cmd):
timestamp = time.time() # 获取当前时间戳
cmd["timestamp"] = timestamp # 将时间戳添加到命令中
s = json.dumps(cmd) # 将命令转换为json字符串
self.udp_sendsocket.sendto(s.encode('utf-8'), self.address) # 通过UDP发送命令
def _recv_with_timeout(self, timeout=5):
ready = select.select([self.udp_seversocket], [], [], timeout)
if ready[0]:
data, addr = self.udp_seversocket.recvfrom(51200)
data_message = data.decode('utf-8', errors='ignore')
# 假设数据中包含时间戳
try:
data_json = json.loads(data_message)
timestamp = data_json.get("timestamp", None)
if timestamp is not None:
current_time = time.time()
if current_time - timestamp < 1.0: # 假设我们只接受1秒内的数据
return data_message
else:
print("Received outdated data")
return None
else:
print("No timestamp found in received data")
return None
except json.JSONDecodeError:
print("Error decoding received data")
return None
else:
return None
"""
# ROOT = 'C:\\NPSim1.0.2'
#def spr(cmd, fileio=None):
# start_up_info = subprocess.STARTUPINFO() # 创建一个 STARTUPINFO 对象，用于设置子进程的启动信息
# start_up_info.dwFlags |= subprocess.STARTF_USESHOWWINDOW # 设置启动信息的标志位，使子进程窗口在启动时不显示
# start_up_info.wShowWindow = False # 指定子进程窗口不显示
# subprocess.Popen(cmd, shell=True, startupinfo=start_up_info, stdout=fileio, stderr=fileio)
class AFsim_event_Base():
def __init__(self, **kwargs) -> None:
self.event_data = None # 用于存储事件数据
self.running = True # 控制线程的运行状态
self.udp_event_seversocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # 收(事件数据)
try:
self.udp_event_seversocket.bind(('0.0.0.0', 9660)) # 事件数据 绑定UDP服务器地址和端口
except Exception as e:
print(f"Socket binding error: {e}")
pass # 忽略绑定异常
def _recv_event_data(self, timeout=0.05):
"""
从事件数据的UDP套接字接收数据,带有超时功能。
"""
ready = select.select([self.udp_event_seversocket], [], [], timeout)
if ready[0]:
data_event, addr = self.udp_event_seversocket.recvfrom(2048)
print(f"Received event data from {addr}: {data_event.decode()}")
return data_event.decode('utf-8', errors='ignore') # 不去掉末尾字符
else:
return None
////////////////////////////////////////
buffer.py源码
import numpy as np
class ReplayBuffer:
def __init__(self):
self.memory = {
'states': [],
'actions': [],
'rewards': [],
'next_states': [],
'dones': []
}
def add(self, state, action, next_state,reward,done):
self.memory['states'].append(state)
self.memory['actions'].append(action)
self.memory['rewards'].append(reward)
self.memory['next_states'].append(next_state)
self.memory['dones'].append(done)
def sample(self):
batch = {
'states': np.array(self.memory['states']),
'actions': np.array(self.memory['actions']),
'rewards': np.array(self.memory['rewards']),
'next_states': np.array(self.memory['next_states']),
'dones': np.array(self.memory['dones']),
}
return batch
def clear(self):
self.memory = {
'states':[],
'next_states':[],
'actions':[],
'rewards':[],
'dones': []
}
//////////////////////////////////
check_point.py源码
import math
class Point:
def __init__(self, lon, lat):
self.lon = lon
self.lat = lat
def dms_to_dd(dms_lat, dms_lon):
"""Converts coordinates from DMS to DD format."""
try:
lat_dms = dms_lat.split(':')
lon_dms = dms_lon.split(':')
if len(lat_dms) != 3 or len(lon_dms) != 3:
return None
lat_dd = float(lat_dms[0]) + float(lat_dms[1]) / 60 + float(lat_dms[2].replace('n', '').replace('s', '')) / 3600
lon_dd = float(lon_dms[0]) + float(lon_dms[1]) / 60 + float(lon_dms[2].replace('e', '').replace('w', '')) / 3600
if 's' in dms_lat.lower():
lat_dd *= -1
if 'w' in dms_lon.lower():
lon_dd *= -1
print(lon_dd, lat_dd)
return Point(lon_dd, lat_dd)
# return [lon_dd, lat_dd]
except (ValueError, IndexError):
return None
def is_point_in_polygon(point, polygon):
"""Checks if a point is inside a polygon using the ray casting algorithm."""
x = point.lon
y = point.lat
n = len(polygon)
inside = False
p1 = polygon[0]
for i in range(1, n + 1):
p2 = polygon[i % n]
if y > min(p1.lat, p2.lat):
if y <= max(p1.lat, p2.lat):
if x <= max(p1.lon, p2.lon):
if p1.lat != p2.lat:
xinters = (y - p1.lat) * (p2.lon - p1.lon) / (p2.lat - p1.lat) + p1.lon
if xinters >= x:
inside = not inside
p1 = p2
return inside
# 矩形坐标 (顺时针)
points = [
"24:30:03.51n 119:02:27.56e", # Top Right
"24:46:32.35n 119:19:50.98e", # Bottom Right
"24:36:48.89n 119:30:55.89e", # Bottom Left
"24:20:12.71n 119:13:51.33e" # Top Left
]
# Convert DMS to DD
polygon_points = []
for point in points:
lat_str, lon_str = point.split(' ')
dd_point = dms_to_dd(lat_str,lon_str)
if dd_point:
polygon_points.append(dd_point)
else:
print("Error: Invalid DMS format.")
exit()
# 测试点 (替换为你需要测试的经纬度)
test_point_dms = "24:35:00n 119:20:00e"
lat_str, lon_str = test_point_dms.split(' ')
test_point = dms_to_dd(lat_str,lon_str)
if test_point:
if is_point_in_polygon(test_point, polygon_points):
print(f"The point ({test_point.lon:.6f}, {test_point.lat:.6f}) is inside the rectangle.")
else:
print(f"The point ({test_point.lon:.6f}, {test_point.lat:.6f}) is outside the rectangle.")
else:
print("Error: Invalid test point DMS format.")
/////////////////////////////////////////////////////
file.py源码
import math
class Point:
def __init__(self, lon, lat):
self.lon = lon
self.lat = lat
def dms_to_dd(dms_lat, dms_lon):
"""Converts coordinates from DMS to DD format."""
try:
lat_dms = dms_lat.split(':')
lon_dms = dms_lon.split(':')
if len(lat_dms) != 3 or len(lon_dms) != 3:
return None
lat_dd = float(lat_dms[0]) + float(lat_dms[1]) / 60 + float(lat_dms[2].replace('n', '').replace('s', '')) / 3600
lon_dd = float(lon_dms[0]) + float(lon_dms[1]) / 60 + float(lon_dms[2].replace('e', '').replace('w', '')) / 3600
if 's' in dms_lat.lower():
lat_dd *= -1
if 'w' in dms_lon.lower():
lon_dd *= -1
print(lon_dd, lat_dd)
return Point(lon_dd, lat_dd)
# return [lon_dd, lat_dd]
except (ValueError, IndexError):
return None
def is_point_in_polygon(point, polygon):
"""Checks if a point is inside a polygon using the ray casting algorithm."""
x = point.lon
y = point.lat
n = len(polygon)
inside = False
p1 = polygon[0]
for i in range(1, n + 1):
p2 = polygon[i % n]
if y > min(p1.lat, p2.lat):
if y <= max(p1.lat, p2.lat):
if x <= max(p1.lon, p2.lon):
if p1.lat != p2.lat:
xinters = (y - p1.lat) * (p2.lon - p1.lon) / (p2.lat - p1.lat) + p1.lon
if xinters >= x:
inside = not inside
p1 = p2
return inside
# 矩形坐标 (顺时针)
points = [
"24:30:03.51n 119:02:27.56e", # Top Right
"24:46:32.35n 119:19:50.98e", # Bottom Right
"24:36:48.89n 119:30:55.89e", # Bottom Left
"24:20:12.71n 119:13:51.33e" # Top Left
]
# Convert DMS to DD
polygon_points = []
for point in points:
lat_str, lon_str = point.split(' ')
dd_point = dms_to_dd(lat_str,lon_str)
if dd_point:
polygon_points.append(dd_point)
else:
print("Error: Invalid DMS format.")
exit()
# 测试点 (替换为你需要测试的经纬度)
test_point_dms = "24:35:00n 119:20:00e"
lat_str, lon_str = test_point_dms.split(' ')
test_point = dms_to_dd(lat_str,lon_str)
if test_point:
if is_point_in_polygon(test_point, polygon_points):
print(f"The point ({test_point.lon:.6f}, {test_point.lat:.6f}) is inside the rectangle.")
else:
print(f"The point ({test_point.lon:.6f}, {test_point.lat:.6f}) is outside the rectangle.")
else:
print("Error: Invalid test point DMS format.")
//////////////////////////////////////////////////////
obsdecode.py源码
import os
import math
import numpy as np
# def get_real_obs(obs_json):
# # 解析obs
# obs_array = []
# for entity in obs_json['entityStatusData']:
# index = int(entity['Index'])
# lon = float(entity['Lon'])
# lat = float(entity['Lat'])
# # alt = float(entity['Alt'])
# obs_array.extend([index,lon,lat])
# obs_array = np.array(obs_array,dtype=np.float32)
# return obs_array
# point 24:30:03.51n 119:02:27.56e // Top Right
# point 24:46:32.35n 119:19:50.98e // Bottom Right
# point 24:36:48.89n 119:30:55.89e // Bottom Left
# point 24:20:12.71n 119:13:51.33e // Top Left
class Point:
def __init__(self, lon, lat):
self.lon = lon
self.lat = lat
def dms_to_dds(dms_lat, dms_lon):
"""Converts coordinates from DMS to DD format."""
try:
lat_dms = dms_lat.split(':')
lon_dms = dms_lon.split(':')
if len(lat_dms) != 3 or len(lon_dms) != 3:
return None
lat_dd = float(lat_dms[0]) + float(lat_dms[1]) / 60 + float(lat_dms[2].replace('n', '').replace('s', '')) / 3600
lon_dd = float(lon_dms[0]) + float(lon_dms[1]) / 60 + float(lon_dms[2].replace('e', '').replace('w', '')) / 3600
if 's' in dms_lat.lower():
lat_dd *= -1
if 'w' in dms_lon.lower():
lon_dd *= -1
# print(lon_dd, lat_dd)
return Point(lon_dd, lat_dd)
# return [lon_dd, lat_dd]
except (ValueError, IndexError):
return None
def dms_to_dd(dms_lat, dms_lon):
"""
将坐标从DMS转换为DD格式。
Args:
dms_lat:dms格式的纬度（例如，“24:32:13.08”）。
dms_lon:dms格式的经度（例如，“119:08:45.57”）。
return:
包含DD格式的纬度和经度的元组（例如，（24.53691119.145992））。
如果输
"""
try:
lat_dms = dms_lat.split(':')
lon_dms = dms_lon.split(':')
if len(lat_dms) != 3 or len(lon_dms) != 3:
return None
lat_dd = float(lat_dms[0]) + float(lat_dms[1]) / 60 + float(lat_dms[2]) / 3600
lon_dd = float(lon_dms[0]) + float(lon_dms[1]) / 60 + float(lon_dms[2]) / 3600
return [lat_dd, lon_dd]
# return Point(lon_dd, lat_dd)
except (ValueError, IndexError):
return None # Handle cases with invalid input format
def get_task_point():
A_lat = '24:30:03.51'
A_lon = '119:02:27.56'
B_lat = '24:46:32.3'
B_lon = '119:19:50.98'
C_lat = '24:36:48.89'
C_lon = '119:30:55.89'
D_lat = '24:20:12.71'
D_lon = '119:13:51.33'
A_point = dms_to_dd(A_lat,A_lon)
B_point = dms_to_dd(B_lat,B_lon)
C_point = dms_to_dd(C_lat,C_lon)
D_point = dms_to_dd(D_lat,D_lon)
#处理DMS格式无效的情况
if any(p is None for p in [A_point, B_point, C_point, D_point]):
return None
# print(A_point,B_point,C_point,D_point)
return A_point,B_point,C_point,D_point
# 计算奖励用到的点
def get_base_point():
A_lat = '24:30:03.51'
A_lon = '119:02:27.56'
B_lat = '24:46:32.3'
B_lon = '119:19:50.98'
C_lat = '24:36:48.89'
C_lon = '119:30:55.89'
D_lat = '24:20:12.71'
D_lon = '119:13:51.33'
A_point = dms_to_dd(A_lat,A_lon)
B_point = dms_to_dd(B_lat,B_lon)
C_point = dms_to_dd(C_lat,C_lon)
D_point = dms_to_dd(D_lat,D_lon)
center_lat_point = (A_point[0]+B_point[0]+C_point[0]+D_point[0])/4
center_lon_point = (A_point[1]+B_point[1]+C_point[1]+D_point[1])/4
return A_point,B_point,C_point,D_point,center_lat_point,center_lon_point
def get_danger_point():
"""
24:26:18.92n 119:15:25.68e
24:32:09.91n 119:08:51.77e
24:25:08.29n 119:07:45.31e
24:20:05.78n 119:13:50.01e
"""
A_lat = '24:26:18.92'
A_lon = '119:15:25.68'
B_lat = '24:32:09.91'
B_lon = '119:08:51.77'
C_lat = '24:25:08.29'
C_lon = '119:07:45.31'
D_lat = '24:20:05.78'
D_lon = '119:13:50.01'
A_point = dms_to_dd(A_lat,A_lon)
B_point = dms_to_dd(B_lat,B_lon)
C_point = dms_to_dd(C_lat,C_lon)
D_point = dms_to_dd(D_lat,D_lon)
center_lat_point = (A_point[0]+B_point[0]+C_point[0]+D_point[0])/4
center_lon_point = (A_point[1]+B_point[1]+C_point[1]+D_point[1])/4
return A_point,B_point,C_point,D_point,center_lat_point,center_lon_point
def get_real_obs(obs_json):
# 解析obs
# 初始化obs_array
done = False
obs_array = []
if obs_json == {}:
done = True
return done,obs_array
# 获取实体数据
if len(obs_json['entityStatusData']) < 2:
done = True
return done,obs_array
entity_A = obs_json['entityStatusData'][0] # 第一个实体
entity_B = obs_json['entityStatusData'][1] # 第二个实体
# 提取实体A的相关信息
index_A = int(entity_A['Index'])
lat_A = float(entity_A['Lat'])
lon_A = float(entity_A['Lon'])
# 提取实体B的相关信息
index_B = int(entity_B['Index'])
lat_B = float(entity_B['Lat'])
lon_B = float(entity_B['Lon'])
# 计算经纬度差
lat_diff = lat_B - lat_A
lon_diff = lon_B - lon_A
task_points = get_task_point()
if task_points is None:
return True, [] #如果DMS转换出错,返回空数组
# 展平列表
flattened_task_points = [item for sublist in task_points for item in sublist]
for i in range(len(flattened_task_points)):
if i % 2 == 0:
flattened_task_points[i] = (flattened_task_points[i] - lat_A)/2
else:
flattened_task_points[i] = (flattened_task_points[i] - lon_A)/2
# 将lat_A lon_A 和lat_B lon_B标准化
lat_A = (lat_A - 23.5)/2
lon_A = (lon_A - 118.5)/2
lat_B = (lat_B - 23.5)/2
lon_B = (lon_B - 118.5)/2
# 0112 加入红方实体信息
# 将所有数据合并到obs_array中。注意这里需要类型检查
obs_array.extend([float(lat_A), float(lon_A),float(lat_B),float(lon_B), *flattened_task_points])
# 转换为NumPy数组并返回
obs_array = np.array(obs_array, dtype=np.float32)
return done,obs_array
# 打印途径点
def print_point(action):
for ac in action['command']['wayPoints']:
lat = float(ac['Lat'])
lon = float(ac['Lon'])
d,m,s = decimal_to_dms(lat)
dd,mm,ss = decimal_to_dms(lon)
print(f"lat:{d}:{m}:{s},lon:{dd}:{mm}:{ss}")
# 获取敌方态势信息
def get_enemy_data(obs_all,path):
file_name = 'obs_enemy_data.txt'
file_path = os.path.join(path, file_name)
with open(file_path, 'w') as file:
print("")
for obs_all in obs_all:
# if obs_all['ID'] in ['3', '4']:
# 构建所需的格式化字符串
data_str = f"ID: {obs_all['ID']}, pitchNED: {obs_all['pitchNED']}, rollNED: {obs_all['rollNED']}, headingNED: {obs_all['headingNED']}\n"
# 将数据追加写入指定文件
with open(file_path, 'a') as file:
file.write(data_str)
return True
# 小数度转经纬度
def decimal_to_dms(decimal_degree):
# 将小数度（Decimal Degrees）转换为度分秒（DMS）
# 获取度（整数部分）
degree = int(decimal_degree)
# 获取分钟（去掉度之后的部分,乘以 60）
minute = int((decimal_degree - degree) * 60)
# 获取秒（去掉分钟之后的部分,乘以 60）
second = round((decimal_degree - degree - minute / 60) * 3600, 4)
return degree,minute,second
# 经纬度转小数度
def dms_to_decimal(degree,minute,second):
# 将度分秒（DMS）转换为小数度（Decimal Degrees, DD）
decimal = degree + minute / 60 + second / 3600
return decimal
def normalize_state(state, lon_min=118.5, lon_max=120.5, lat_min=23.5, lat_max=25.5,k=1):
"""
Normalizes the state array containing latitude and longitude values.
规范化包含纬度和经度值的状态数组
Args:
state: A NumPy array of shape (N,) where N is even, containing alternating latitude and longitude values.
lon_min: Minimum longitude value for normalization.
lon_max: Maximum longitude value for normalization.
lat_min: Minimum latitude value for normalization.
lat_max: Maximum latitude value for normalization.
Returns:
A NumPy array with normalized latitude and longitude values, or None if input is invalid.
Values outside the specified range will be clipped to the range's boundaries.
"""
if len(state) % 2 != 0:
print("Error: State array must have an even number of elements (latitude-longitude pairs).")
return None
latitudes = state[::2] # Extract latitudes
longitudes = state[1::2] # Extract longitudes
#Clip values to the specified range
latitudes = np.clip(latitudes, lat_min, lat_max)
longitudes = np.clip(longitudes, lon_min, lon_max)
# Normalize latitudes and longitudes separately
normalized_latitudes = (latitudes - lat_min) / (lat_max - lat_min)
normalized_longitudes = (longitudes - lon_min) / (lon_max - lon_min)
# 将标准化的纬度和经度交错
normalized_state = np.empty_like(state, dtype=np.float32)
normalized_state[::2] = normalized_latitudes
normalized_state[1::2] = normalized_longitudes
# 对实体位置着重处理
normalized_state = np.hstack([normalized_state[:2] * k, normalized_state[2:]])
return normalized_state
# 将状态归一化处理
def normalize_state_v1(state):
# 假设已知的经纬度和高度的范围
longitude_range = (-180, 180) # 经度范围
latitude_range = (-90, 90) # 纬度范围
altitude_range = (0, 15000) # 高度范围
position_range = (-4, 4)
# 获取 ID 和经纬度高度数据
agent_id_1 = state[0] # 第0个是ID
longitude_1 = state[1]
latitude_1 = state[2]
# altitude_1 = state[3]
agent_id_2 = state[3] # 第4个是ID
longitude_2 = state[4]
latitude_2 = state[5]
# altitude_2 = state[7]
posi_1 = state[6]
posi_2 = state[7]
# 对经度进行归一化
normalized_longitude_1 = (longitude_1 - longitude_range[0]) / (longitude_range[1] - longitude_range[0])
normalized_longitude_2 = (longitude_2 - longitude_range[0]) / (longitude_range[1] - longitude_range[0])
# 对纬度进行归一化
normalized_latitude_1 = (latitude_1 - latitude_range[0]) / (latitude_range[1] - latitude_range[0])
normalized_latitude_2 = (latitude_2 - latitude_range[0]) / (latitude_range[1] - latitude_range[0])
# 对高度进行归一化
# normalized_altitude_1 = (altitude_1 - altitude_range[0]) / (altitude_range[1] - altitude_range[0])
# normalized_altitude_2 = (altitude_2 - altitude_range[0]) / (altitude_range[1] - altitude_range[0])
# 对相对位置进行归一化
norm_lat_diff = (posi_1 - position_range[0]) / (position_range[1] - position_range[0])
norm_lon_diff = (posi_2 - position_range[0]) / (position_range[1] - position_range[0])
# 返回归一化后的状态
normalized_state = np.array([agent_id_1, normalized_longitude_1, normalized_latitude_1,
agent_id_2, normalized_longitude_2, normalized_latitude_2,
norm_lat_diff, norm_lon_diff])
return normalized_state
def generate_waypoints_twopoint(lat, lon, alt,action, num_segments=15):
# 起点到中点的分段（action[0] 和 action[1] 控制第一个部分）
lat1 = float(lat)
lon1 = float(lon)
lat_mid = lat1 + action[0] # 终点的经度
lon_mid = lon1 + action[1] # 终点的纬度
# 中点到终点的分段（action[2] 和 action[3] 控制第二个部分）
lat_end = lat_mid + action[2]
lon_end = lon_mid + action[3]
# 使用np.linspace进行插值:从起点到中点,以及从中点到终点,分成num_segments个小段
waypoints = []
# 插值生成从起点到中点的路径
for i in range(num_segments):
t = i / (num_segments - 1) # 线性插值系数
lat_new = lat1 + t * (lat_mid - lat1)
lon_new = lon1 + t * (lon_mid - lon1)
waypoints.append({"Lat": str(lat_new), "Lon": str(lon_new), "Alt": str(alt), "Speed": "150"})
# 插值生成从中点到终点的路径
for i in range(num_segments):
t = i / (num_segments - 1) # 线性插值系数
lat_new = lat_mid + t * (lat_end - lat_mid)
lon_new = lon_mid + t * (lon_end - lon_mid)
waypoints.append({"Lat": str(lat_new), "Lon": str(lon_new), "Alt": str(alt), "Speed": "600"})
return waypoints
def generate_waypoints_onepoint(idx,speed,lat, lon, alt,action, num_segments=15):
# 起点到终点的分段（action[0] 和 action[1] 控制第一个部分）
lat1 = float(lat)
lon1 = float(lon)
lon_mid = lon1 + action[0] # 终点的纬度
lat_mid = lat1 + action[1]# 终点的经度
# 使用np.linspace进行插值:从起点到中点,以及从中点到终点,分成num_segments个小段
waypoints_last_step = []
waypoints_last_step.append({"Lat": str(lat_mid), "Lon": str(lon_mid), "Alt": str(alt), "Speed": str(0)})
waypoints = []
waypoints.append({"Lat": str(lat_mid), "Lon": str(lon_mid), "Alt": str(alt), "Speed": str(speed[idx])})
# 插值生成从起点到中点的路径
# for i in range(num_segments):
# t = i / (num_segments - 1) # 线性插值系数
# lat_new = lat1 + t * (lat_mid - lat1)
# lon_new = lon1 + t * (lon_mid - lon1)
# waypoints.append({"Lat": str(lat_new), "Lon": str(lon_new), "Alt": str(alt), "Speed": "150"})
return waypoints,waypoints_last_step
# 经纬度替换
def replace_lat_lon(state,lat,lon):
state[1] = lon
state[2] = lat
return state
def get_cmd_actions(obs,index,action):
action_speed = [150,150,150,150,150]
lat = obs['entityStatusData'][0]['Lat']
Lon = obs['entityStatusData'][0]['Lon']
Alt = obs['entityStatusData'][0]['Alt']
waypoints,wayp_ls_step = generate_waypoints_onepoint(index,action_speed,lat, Lon,Alt, action, num_segments=1)
move_cmd = {
"command":{
"ID":"1",
"Type": "route",
"wayPoints":waypoints
}
}
move_cmd_ls_step = {
"command":{
"ID":"1",
"Type": "route",
"wayPoints":wayp_ls_step
}
}
return move_cmd,move_cmd_ls_step
def euclidean_distance(lat_A, lon_A, lat_B, lon_B):
return math.sqrt((lat_B - lat_A) ** 2 + (lon_B - lon_A) ** 2)
#根据二维平面坐标计算奖励（忽略曲率）
def calculate_reward(point_A,point_B, epsilon=0.05, k=1, stability_bonus=0.1):
lat_A = float(point_A['Lat'])
lon_A = float(point_A['Lon'])
lat_B = float(point_B['Lat'])
lon_B = float(point_B['Lon'])
# 计算两点之间的欧几里得距离
distance = euclidean_distance(lat_A, lon_A, lat_B, lon_B)
# 如果距离小于等于阈值 epsilon,奖励为 1,并且加上稳定性奖励
if distance <= epsilon:
reward = 1 + stability_bonus
else:
# 否则,奖励为 1 减去距离的惩罚项
reward = - k * distance
return reward
# 根据度计算距离
def _cal_dis(point_A,point_B):
R = 6371.0
lat_A = float(point_A['Lat'])
lon_A = float(point_A['Lon'])
lat_B = float(point_B['Lat'])
lon_B = float(point_B['Lon'])
# 经纬度转为弧度
lat1 = math.radians(lat_A)
lon1 = math.radians(lon_A)
lat2 = math.radians(lat_B)
lon2 = math.radians(lon_B)
# 计算经纬度差值
dlat = lat2 - lat1
dlon = lon2 - lon1
# Haversine公式计算球面距离
a = math.sin(dlat / 2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2)**2
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
# 计算距离
distance = R * c # 距离单位:公里
return -distance
def cal_dis(point_A,point_B):
lat_A = float(point_A['Lat'])
lon_A = float(point_A['Lon'])
lat_B = float(point_B['Lat'])
lon_B = float(point_B['Lon'])
distance = math.sqrt((lat_A - lat_B) ** 2 + (lon_A - lon_B) ** 2)
return -distance
def cal_location_reward_out(blue_point_obs,point_base):
A_p,B_p,C_p,D_p,Cent_lat_p,Cent_lon_p = point_base
lat_A = float(blue_point_obs['Lat'])
lon_A = float(blue_point_obs['Lon'])
print(f"lat_A:{lat_A},lon_A:{lon_A}")
# 计算欧几里得距离
distance_A = euclidean_distance(lat_A, lon_A, A_p[0], A_p[1])
distance_B = euclidean_distance(lat_A, lon_A, B_p[0], B_p[1])
distance_C = euclidean_distance(lat_A, lon_A, C_p[0], C_p[1])
distance_D = euclidean_distance(lat_A, lon_A, D_p[0], D_p[1])
reward_location = -50*(distance_A+distance_B+distance_C+distance_D)
# reward_center = -50*euclidean_distance(lat_A, lon_A, Cent_lat_p, Cent_lon_p)# 在活动区域外,给一个较大的负奖励
return reward_location
def cal_location_reward_in(blue_point_obs,point_base):
A_p,B_p,C_p,D_p,Cent_lat_p,Cent_lon_p = point_base
lat_A = float(blue_point_obs['Lat'])
lon_A = float(blue_point_obs['Lon'])
reward_center = 10*euclidean_distance(lat_A, lon_A, Cent_lat_p, Cent_lon_p)# 在活动区域内给正奖励# 在危险区给负奖励
return reward_center
def cal_location_reward_out_danger(blue_point_obs,point_base):
A_p,B_p,C_p,D_p,Cent_lat_p,Cent_lon_p = point_base
lat_A = float(blue_point_obs['Lat'])
lon_A = float(blue_point_obs['Lon'])
reward_center = 2*euclidean_distance(lat_A, lon_A, Cent_lat_p, Cent_lon_p)# 在活动区域内给正奖励# 在危险区给负奖励
return reward_center
def cal_location_reward_in_danger(blue_point_obs,point_base):
A_p,B_p,C_p,D_p,Cent_lat_p,Cent_lon_p = point_base
lat_A = float(blue_point_obs['Lat'])
lon_A = float(blue_point_obs['Lon'])
reward_center = -20*euclidean_distance(lat_A, lon_A, Cent_lat_p, Cent_lon_p)# 在活动区域内给正奖励# 在危险区给负奖励
return reward_center
# 任务区
def is_point_in_polygon(Point):
"""射线算法检查一个点是否在多边形内"""
# 矩形坐标 (顺时针)
points = [
"24:30:03.51n 119:02:27.56e", # Top Right
"24:46:32.35n 119:19:50.98e", # Bottom Right
"24:36:48.89n 119:30:55.89e", # Bottom Left
"24:20:12.71n 119:13:51.33e" # Top Left
]
# Convert DMS to DD
polygon_points = []
for point in points:
lat_str, lon_str = point.split(' ')
dd_point = dms_to_dds(lat_str,lon_str)
if dd_point:
polygon_points.append(dd_point)
else:
print("Error: Invalid DMS format.")
x = float(Point['Lon'])
y = float(Point['Lat'])
n = len(polygon_points)
inside = False
p1 = polygon_points[0]
for i in range(1, n + 1):
p2 = polygon_points[i % n]
if y > min(p1.lat, p2.lat):
if y <= max(p1.lat, p2.lat):
if x <= max(p1.lon, p2.lon):
if p1.lat != p2.lat:
xinters = (y - p1.lat) * (p2.lon - p1.lon) / (p2.lat - p1.lat) + p1.lon
if xinters >= x:
inside = not inside
p1 = p2
if inside:
print(f"点{Point}在区域内")
else:
print(f"点{Point}在区域外")
return inside
# 危险区
def is_point_in_polygon_danger(Point):
"""射线算法检查一个点是否在多边形内"""
# 矩形坐标 (顺时针)
points = [
"24:26:18.92n 119:15:25.68e", # Top Right
"24:32:09.91n 119:08:51.77e", # Bottom Right
"24:25:08.29n 119:07:45.31e", # Bottom Left
"24:20:05.78n 119:13:50.01e" # Top Left
]
# Convert DMS to DD
polygon_points = []
for point in points:
lat_str, lon_str = point.split(' ')
dd_point = dms_to_dds(lat_str,lon_str)
if dd_point:
polygon_points.append(dd_point)
else:
print("Error: Invalid DMS format.")
x = float(Point['Lon'])
y = float(Point['Lat'])
n = len(polygon_points)
inside = False
p1 = polygon_points[0]
for i in range(1, n + 1):
p2 = polygon_points[i % n]
if y > min(p1.lat, p2.lat):
if y <= max(p1.lat, p2.lat):
if x <= max(p1.lon, p2.lon):
if p1.lat != p2.lat:
xinters = (y - p1.lat) * (p2.lon - p1.lon) / (p2.lat - p1.lat) + p1.lon
if xinters >= x:
inside = not inside
p1 = p2
if inside:
print(f"点{Point}在危险区域内")
else:
print(f"点{Point}在危险区域外")
return inside
# points = {"Lon":119.5,"Lat":24.5}
# ind = is_point_in_polygon(points)
# print(ind)
# points = {"Lon":119.5,"Lat":24.6}
# ind = is_point_in_polygon(points)
# print(ind)
# 计算奖励
"""
A_lat = '24:30:03.51'
A_lon = '119:02:27.56'
B_lat = '24:46:32.3'
B_lon = '119:19:50.98'
C_lat = '24:36:48.89'
C_lon = '119:30:55.89'
D_lat = '24:20:12.71'
D_lon = '119:13:51.33'
A_point: [24.500975, 119.040985]
B_point: [24.775639, 119.330826]
C_point: [24.61358, 119.515526]
D_point: [24.336864, 119.23093]
"""
def cal_reward(obs,action,max_lat=24.775639,min_lat=24.336864,max_lon=119.515526,min_lon=119.040985):
is_inorout = False
reward_win = 0
# 判断仿真中还有几个实体
if len(obs['entityStatusData'])<2:
done = True
else:
done = False
# 红方位置
red_point_obs = obs['entityStatusData'][-1]
# 蓝方位置
blue_point_obs = obs['entityStatusData'][0]
# 蓝方要移动的目标位置
newblue_point_obs = action['command']['wayPoints'][-1]
# 蓝方纬度和经度
blue_lat = float(blue_point_obs['Lat'])
blue_lon = float(blue_point_obs['Lon'])
# 任务区区域
point_base = get_base_point()
# 危险区区域
point_danger = get_danger_point()
# 任务区奖励
is_inside_Missionarea = is_point_in_polygon(newblue_point_obs)
if is_inside_Missionarea:
reward_location_mission = cal_location_reward_in(newblue_point_obs,point_base)
else:
reward_location_mission = cal_location_reward_out(newblue_point_obs,point_base)
# 危险区惩罚
is_inside_dangerarea = is_point_in_polygon_danger(newblue_point_obs)
if is_inside_dangerarea:
reward_location_danger = cal_location_reward_in_danger(newblue_point_obs,point_danger)
else:
reward_location_danger = cal_location_reward_out_danger(newblue_point_obs,point_danger)
# reward = cal_dis(red_point_obs,newblue_point_obs)
# reward = calculate_reward(red_point_obs,newblue_point_obs)
current_dis = cal_dis(blue_point_obs,newblue_point_obs)
# 胜利奖励
if done and obs['entityStatusData']['index']=='1':
reward_win = 500
elif done:
reward_win = -500
# 边界惩罚（判断实体位置是否在指定位置内(超出固定区域,该Episode结束))
lat_blue = float(obs['entityStatusData'][0]['Lat'])
lon_blue = float(obs['entityStatusData'][0]['Lon'])
if lat_blue >25.5 or lat_blue < 23.5 or lon_blue > 120.5 or lon_blue < 118.5:
done = True
reward_edge = -100
else:
reward_edge = 0
reward = reward_location_mission + reward_location_danger + reward_win + reward_edge
print(f"目标位置:{newblue_point_obs}")
return reward,done,newblue_point_obs,current_dis
# 计算延时
def cal_time_sleep(dis):
times = abs(dis)*1000/150
print(f"延时大小:{times}")
return times
def cal_dis_lonandlat(point_A,point_B):
R = 6371.0
lat_A = point_A
lon_A = point_A
lat_B = point_B
lon_B = point_B
# 经纬度转为弧度
lat1 = math.radians(lat_A)
lon1 = math.radians(lon_A)
lat2 = math.radians(lat_B)
lon2 = math.radians(lon_B)
# 计算经纬度差值
dlat = lat2 - lat1
dlon = lon2 - lon1
# Haversine公式计算球面距离
a = math.sin(dlat / 2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2)**2
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
# 计算距离
distance = R * c # 距离单位:公里
return -distance
#
# print(cal_dis_lonandlat(0.0,0.00135))
def take_obs(obs):
flag = False
entity_A = obs['entityStatusData'][0] # 第一个实体
# 提取实体A的经纬
lat_A = float(entity_A['Lat'])
lon_A = float(entity_A['Lon'])
if lat_A>25.2 or lat_A<24 or lon_A >121 or lon_A<118:
flag = True
return flag
def process_action(ac, ind=0):
# 定义允许的 ac 值的规则
valid_actions = {
0: [1, 3], # ind = 0 时,ac 可取 1 或 3
1: [0, 2], # ind = 1 时,ac 可取 0 或 2
2: [1, 3], # ind = 2 时,ac 可取 1 或 3
3: [0, 2] # ind = 3 时,ac 可取 0 或 2
}
# 如果 ac 不在 valid_actions[ind] 中,则强制将 ac 设为 ind
if ac not in valid_actions[ind]:
return ind
else:
return ac
def conversion_action(ac_index, current_angle, step_size):
"""
根据给定的动作索引和当前角度计算智能体的下一个动作和方向。
参数:
- ac_index (int): 强化学习输出的离散动作索引,0表示保持当前方向,1表示向左偏移45度,2表示向右偏移45度。
- current_angle (float): 当前的移动方向,以角度表示(0到360度之间)。
- step_size (float): 每个时间步的移动距离（经度/纬度变化量）。
返回:
- new_angle (float): 更新后的移动角度。
- action (list): 当前选择的动作映射值,包含经度和纬度的变化量。
"""
# 根据动作索引选择新的方向
possible_angles = [current_angle-15, current_angle,current_angle + 15]
possible_angles = [(angle + 360) % 360 for angle in possible_angles] # 保证角度在0到360之间
# 偏移角度列表
angle_offsets = [-30, -15, 0, 15, 30]
# 获取新的角度
new_angle = (current_angle + possible_angles[ac_index]) % 360 # 保证角度在 0 到 360 之间
# 根据 ac_index 选择动作对应的角度
new_angle = possible_angles[ac_index]
# 计算新的经纬度增量
angle_rad = math.radians(new_angle) # 转换为弧度
delta_lon = math.cos(angle_rad) * step_size
delta_lat = math.sin(angle_rad) * step_size
# 计算新的平面坐标增量（x, y）
delta_x = step_size * (new_angle / 90) # 将角度转换为对应的 x 方向变化量
delta_y = step_size * (new_angle / 90) # 将角度转换为对应的 y 方向变化量
# 返回新的角度和动作映射值
action = [delta_lon, delta_lat]
# action = [delta_x, delta_y]
return new_angle, action
//////////////////////////////////////////////
rl_utils.py源码
from tqdm import tqdm
import numpy as np
import torch
import collections
import random
class ReplayBuffer:
def __init__(self, capacity):
self.buffer = collections.deque(maxlen=capacity) # 使用 collections.deque 的原因是其具有高效的插入和删除操作，特别是在两端（头部和尾部）的操作比列表（list）要快得多
def add(self, state, action, reward, next_state, done):
self.buffer.append((state, action, reward, next_state, done))
def sample(self, batch_size):
transitions = random.sample(self.buffer, batch_size)
state, action, reward, next_state, done = zip(*transitions)# 输出为元组
return np.array(state), action, reward, np.array(next_state), done
def size(self):
return len(self.buffer)
def moving_average(a, window_size):
cumulative_sum = np.cumsum(np.insert(a, 0, 0))
middle = (cumulative_sum[window_size:] - cumulative_sum[:-window_size]) / window_size
r = np.arange(1, window_size-1, 2)
begin = np.cumsum(a[:window_size-1])[::2] / r
end = (np.cumsum(a[:-window_size:-1])[::2] / r)[::-1]
return np.concatenate((begin, middle, end))
def train_on_policy_agent(env, agent, num_episodes):
return_list = []
for i in range(10):
with tqdm(total=int(num_episodes/10), desc='Iteration %d' % i) as pbar:
for i_episode in range(int(num_episodes/10)):
episode_return = 0
transition_dict = {'states': [], 'actions': [], 'next_states': [], 'rewards': [], 'dones': []}
state = env.reset()
done = False
while not done:
action = agent.take_action(state)
next_state, reward, done, _ = env.step(action)
transition_dict['states'].append(state)
transition_dict['actions'].append(action)
transition_dict['next_states'].append(next_state)
transition_dict['rewards'].append(reward)
transition_dict['dones'].append(done)
state = next_state
episode_return += reward
return_list.append(episode_return)
agent.update(transition_dict)
if (i_episode+1) % 10 == 0:
pbar.set_postfix({'episode': '%d' % (num_episodes/10 * i + i_episode+1), 'return': '%.3f' % np.mean(return_list[-10:])})
pbar.update(1)
return return_list
def train_off_policy_agent(env, agent, num_episodes, replay_buffer, minimal_size, batch_size):
return_list = []
for i in range(10):
with tqdm(total=int(num_episodes/10), desc='Iteration %d' % i) as pbar:
for i_episode in range(int(num_episodes/10)):
episode_return = 0
state = env.reset()
done = False
while not done:
action = agent.take_action(state)
next_state, reward, done, _ = env.step(action)
replay_buffer.add(state, action, reward, next_state, done)
state = next_state
episode_return += reward
if replay_buffer.size() > minimal_size:
b_s, b_a, b_r, b_ns, b_d = replay_buffer.sample(batch_size)
transition_dict = {'states': b_s, 'actions': b_a, 'next_states': b_ns, 'rewards': b_r, 'dones': b_d}
agent.update(transition_dict)
return_list.append(episode_return)
if (i_episode+1) % 10 == 0:
pbar.set_postfix({'episode': '%d' % (num_episodes/10 * i + i_episode+1), 'return': '%.3f' % np.mean(return_list[-10:])})
pbar.update(1)
return return_list
def compute_advantage(gamma, lmbda, td_delta):
td_delta = td_delta.detach().numpy()
advantage_list = []
advantage = 0.0
for delta in td_delta[::-1]:
advantage = gamma * lmbda * advantage + delta
advantage_list.append(advantage)
advantage_list.reverse()
return torch.tensor(advantage_list, dtype=torch.float)
/////////////////////////////////////////////////////////////////////
管理
浙公网安备 33010602011771号