103 lines
3.2 KiB
Python
103 lines
3.2 KiB
Python
# Integração do cálculo físico de omega no script MPC passo a passo
|
|
|
|
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import json
|
|
|
|
# Carregar mapa
|
|
with open("CidadeJardimTerreno2_Corrigido.json", "r", encoding="utf-8") as f:
|
|
data = json.load(f)
|
|
|
|
traj_gps = []
|
|
for feature in data["features"]:
|
|
coords = feature["geometry"]["coordinates"]
|
|
for lon, lat in coords:
|
|
traj_gps.append((lat, lon))
|
|
|
|
def latlon_to_xy(lat, lon, lat0, lon0):
|
|
R = 6371000
|
|
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
|
|
|
|
lat0, lon0 = traj_gps[0]
|
|
traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])
|
|
|
|
# Parâmetros físicos do robô
|
|
distancia_entre_eixos = 0.92 # L
|
|
dt = 0.2
|
|
v = 1.0
|
|
angulo_max_graus = 30.0
|
|
tipo_movimento = "movimentoArco" # "rodasFrontais" ou "movimentoArco"
|
|
horizonte = 100
|
|
|
|
def calcular_orientacao(p1, p2):
|
|
dx = p2[0] - p1[0]
|
|
dy = p2[1] - p1[1]
|
|
return np.arctan2(dy, dx)
|
|
|
|
def calcular_omega_fisico(v, angulo_rad, tipo_movimento):
|
|
L = distancia_entre_eixos
|
|
tan_delta = np.tan(angulo_rad)
|
|
if tipo_movimento == "rodasFrontais":
|
|
return (v * tan_delta) / L
|
|
elif tipo_movimento == "movimentoArco":
|
|
return (2 * v * tan_delta) / L
|
|
return 0.0
|
|
|
|
# Início da simulação
|
|
x, y, theta = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
|
|
traj_gerada = [(x, y)]
|
|
indice_atual = 0
|
|
dist_anterior = None
|
|
angulo_max_rad = np.radians(angulo_max_graus)
|
|
|
|
for step in range(horizonte):
|
|
if indice_atual >= len(traj_xy) - 1:
|
|
break
|
|
|
|
ponto_atual = traj_xy[indice_atual]
|
|
ponto_proximo = traj_xy[min(indice_atual + 1, len(traj_xy) - 1)]
|
|
pos_atual = np.array([x, y])
|
|
dist_atual = np.linalg.norm(pos_atual - ponto_atual)
|
|
|
|
if dist_anterior is not None:
|
|
orient_ponto = calcular_orientacao(ponto_atual, ponto_proximo)
|
|
orient_robo = calcular_orientacao(pos_atual, ponto_proximo)
|
|
erro_orient = np.abs((orient_robo - orient_ponto + np.pi) % (2 * np.pi) - np.pi)
|
|
if (dist_atual > dist_anterior and erro_orient < np.pi / 2) or dist_atual < 0.6:
|
|
indice_atual += 1
|
|
dist_anterior = None
|
|
continue
|
|
|
|
dist_anterior = dist_atual
|
|
orient_desejada = calcular_orientacao(pos_atual, ponto_proximo)
|
|
delta_theta = (orient_desejada - theta + np.pi) % (2 * np.pi) - np.pi
|
|
|
|
# Converter erro de orientação em um ângulo de direção
|
|
angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
|
|
|
|
# Calcular omega físico com base no tipo de movimento
|
|
omega = calcular_omega_fisico(v, angulo_direcional, tipo_movimento)
|
|
|
|
# Atualizar posição
|
|
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 final
|
|
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória original')
|
|
plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label=f'MPC físico ({tipo_movimento}, {angulo_max_graus}°)')
|
|
plt.xlabel("X (m)")
|
|
plt.ylabel("Y (m)")
|
|
plt.title("Simulação MPC física com tipo de movimento real")
|
|
plt.grid(True)
|
|
plt.axis("equal")
|
|
plt.legend()
|
|
plt.show()
|