85 lines
2.4 KiB
Python
85 lines
2.4 KiB
Python
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
from dataclasses import dataclass
|
|
from typing import List
|
|
|
|
# Simulação com base na estrutura GPSModel simplificada (Lat, Lon)
|
|
@dataclass
|
|
class GPSModel:
|
|
Latitude: float
|
|
Longitude: float
|
|
OrientacaoReal: float = 0.0 # Opcional, se necessário
|
|
|
|
# Conversão simplificada de graus para metros (só para simular corretamente)
|
|
def latlon_to_xy(lat, lon, lat0, lon0):
|
|
R = 6371000 # raio da Terra em metros
|
|
dlat = np.radians(lat - lat0)
|
|
dlon = np.radians(lon - lon0)
|
|
x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
|
|
y = R * dlat
|
|
return x, y
|
|
|
|
# Criar lista de pontos GPSModel simulando a trajetória alvo
|
|
traj_gps = [
|
|
GPSModel(-22.0, -47.0),
|
|
GPSModel(-21.9999, -46.9998),
|
|
GPSModel(-21.9998, -46.9996),
|
|
GPSModel(-21.9995, -46.9990),
|
|
GPSModel(-21.9990, -46.9980),
|
|
GPSModel(-21.9985, -46.9970),
|
|
GPSModel(-21.9980, -46.9960),
|
|
GPSModel(-21.9978, -46.9955),
|
|
GPSModel(-21.9977, -46.9950),
|
|
GPSModel(-21.9976, -46.9945)
|
|
]
|
|
|
|
# Usar o primeiro ponto como referência para converter em metros
|
|
lat0, lon0 = traj_gps[0].Latitude, traj_gps[0].Longitude
|
|
traj_xy = np.array([latlon_to_xy(p.Latitude, p.Longitude, lat0, lon0) for p in traj_gps])
|
|
|
|
# Estado inicial (x, y, theta)
|
|
x, y, theta = 0.0, 0.0, 0.0
|
|
dt = 0.2
|
|
v = 1.0
|
|
horizonte = 50
|
|
|
|
def calcular_orientacao(p1, p2):
|
|
dx = p2[0] - p1[0]
|
|
dy = p2[1] - p1[1]
|
|
return np.arctan2(dy, dx)
|
|
|
|
traj_gerada = [(x, y)]
|
|
|
|
for step in range(horizonte):
|
|
# Encontrar ponto alvo mais próximo
|
|
distancias = np.linalg.norm(traj_xy - np.array([x, y]), axis=1)
|
|
idx_alvo = np.argmin(distancias)
|
|
ponto_alvo = traj_xy[min(idx_alvo + 1, len(traj_xy) - 1)]
|
|
|
|
# Calcular orientação ideal
|
|
orient_desejada = calcular_orientacao((x, y), ponto_alvo)
|
|
delta_theta = (orient_desejada - theta + np.pi) % (2 * np.pi) - np.pi
|
|
|
|
omega = delta_theta / dt
|
|
omega = np.clip(omega, -np.pi / 4, np.pi / 4)
|
|
|
|
# Aplicar movimento
|
|
x += v * np.cos(theta) * dt
|
|
y += v * np.sin(theta) * dt
|
|
theta += omega * dt
|
|
|
|
traj_gerada.append((x, y))
|
|
|
|
traj_gerada = np.array(traj_gerada)
|
|
|
|
# Plotagem
|
|
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória GPS (lat/lon convertida)')
|
|
plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label='MPC passo a passo (estimado)')
|
|
plt.xlabel("X (m)")
|
|
plt.ylabel("Y (m)")
|
|
plt.title("MPC usando GPSModel (Latitude/Longitude)")
|
|
plt.legend()
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
plt.show()
|