implementado MPC
This commit is contained in:
Diego Freitas
parent
700bc6f713
commit
b7b5a39bc4
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -136,7 +136,8 @@ namespace AgroBase.Forms.IHM
|
|||
_Trajetoria._TrajetoriaDinamica,
|
||||
_Trajetoria.CorredorAtual.Pontos,
|
||||
Variaveis.OperacaoEmAndamento.GPSTrajetoria,
|
||||
_Trajetoria.RuasPlantacao
|
||||
_Trajetoria.RuasPlantacao,
|
||||
_Sensoriamento.Controle.ControladorMPC?.ComandoAtual?.simulacao
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -138,7 +138,8 @@ namespace AgroBase.Forms
|
|||
_Trajetoria._TrajetoriaDinamica,
|
||||
_Trajetoria.CorredorAtual.Pontos,
|
||||
Variaveis.OperacaoEmAndamento.GPSTrajetoria,
|
||||
_Trajetoria.RuasPlantacao
|
||||
_Trajetoria.RuasPlantacao,
|
||||
_Sensoriamento.Controle.ControladorMPC?.ComandoAtual?.simulacao
|
||||
);
|
||||
}
|
||||
|
||||
|
|
@ -197,58 +198,83 @@ namespace AgroBase.Forms
|
|||
GPSService.AtualizarCoordenadasGPS();
|
||||
}
|
||||
|
||||
private void btnAcrescentarGPS_Click(object sender, EventArgs e)
|
||||
private async void btnAcrescentarGPS_Click(object sender, EventArgs e)
|
||||
{
|
||||
GPSModel novaPosicao = new GPSModel();
|
||||
double velocidadeMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle);
|
||||
double velocidade = FuncoesMatematicas.ConverteMsParaKmh(velocidadeMs);
|
||||
|
||||
if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
|
||||
{
|
||||
novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
|
||||
idxPassoOperacao++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
|
||||
{
|
||||
return;
|
||||
}
|
||||
Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeMs;
|
||||
|
||||
var ControleDir = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
|
||||
double anguloControle = double.Parse(txtAnguloControle.Text);
|
||||
|
||||
double.TryParse(txtErroTempoComando.Text, out double fatorErro);
|
||||
int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));
|
||||
|
||||
bool atualizarAnguloControle = (ticksControle >= ticksMax);
|
||||
if (atualizarAnguloControle)
|
||||
{
|
||||
ticksControle = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ticksControle++;
|
||||
}
|
||||
|
||||
if (atualizarAnguloControle)
|
||||
{
|
||||
btnCalcularAngulo_Click(new object(), new EventArgs());
|
||||
anguloControle = double.Parse(txtAnguloControle.Text);
|
||||
}
|
||||
|
||||
// Obtém os parâmetros
|
||||
double anguloAtual = double.Parse(txtAnguloGPS.Text);
|
||||
|
||||
TimeSpan tempoPassado =
|
||||
TimeSpan tempoPassado =
|
||||
!chbAutomatico.Checked || UltimaAtualizacaoGps == DateTime.MinValue ? // esta no modo manual
|
||||
new TimeSpan((int)tempoGps * 10000) : // 1 tick do tempo gps
|
||||
DateTime.Now - UltimaAtualizacaoGps; // tempo decorrido entre ticks do timer de leitura
|
||||
|
||||
// Calcula a nova posicao do robo
|
||||
novaPosicao = VariaveisEquipamento.SimularNovaPosicao(Variaveis.OperacaoEmAndamento.Controle.TipoMovimento, anguloControle, velocidadeMs, tempoPassado.TotalSeconds, anguloAtual, GPSService.UltimaLeitura);
|
||||
// Obtém os parâmetros
|
||||
double anguloAtual = double.Parse(txtAnguloGPS.Text);
|
||||
|
||||
var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
|
||||
|
||||
double anguloControle = 0.0;
|
||||
TipoMovimentoDirecional tipoMovimento = TipoMovimentoDirecional.RodasDianteiras;
|
||||
|
||||
var tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional;
|
||||
switch (tipoControle)
|
||||
{
|
||||
case TiposControladorDirecional.PID:
|
||||
{
|
||||
tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
|
||||
|
||||
if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
|
||||
{
|
||||
novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
|
||||
idxPassoOperacao++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var ControleDir = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
|
||||
anguloControle = double.Parse(txtAnguloControle.Text);
|
||||
|
||||
double.TryParse(txtErroTempoComando.Text, out double fatorErro);
|
||||
int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));
|
||||
|
||||
bool atualizarAnguloControle = (ticksControle >= ticksMax);
|
||||
if (atualizarAnguloControle)
|
||||
{
|
||||
ticksControle = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
ticksControle++;
|
||||
}
|
||||
|
||||
if (atualizarAnguloControle)
|
||||
{
|
||||
btnCalcularAngulo_Click(new object(), new EventArgs());
|
||||
anguloControle = double.Parse(txtAnguloControle.Text);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case TiposControladorDirecional.MPC:
|
||||
{
|
||||
MPCComandoModel comando = Variaveis.OperacaoEmAndamento.Controle.ControladorMPC.ComandoAtual;
|
||||
anguloControle = comando?.angulo ?? 0;
|
||||
tipoMovimento = comando?.tipo ?? TipoMovimentoDirecional.RodasDianteiras;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Calcula a nova posicao do robo
|
||||
novaPosicao = VariaveisEquipamento.SimularNovaPosicao(tipoMovimento, anguloControle, velocidadeMs, tempoPassado.TotalSeconds, anguloAtual, _Gps);
|
||||
|
||||
// Atualiza a interface gráfica
|
||||
txtAnguloGPS.Text = novaPosicao.OrientacaoReal.ToString("0.00");
|
||||
txtVelocidade.Text = velocidade.ToString("0.00");
|
||||
|
|
@ -427,5 +453,11 @@ namespace AgroBase.Forms
|
|||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -160,12 +160,12 @@
|
|||
|
||||
public enum StatusCarroMapa
|
||||
{
|
||||
Parado,
|
||||
EntrandoRua,
|
||||
CaminhandoRua,
|
||||
SaindoRua,
|
||||
Manobrando,
|
||||
Direcionando
|
||||
Parado = 0,
|
||||
EntrandoRua = 1,
|
||||
CaminhandoRua = 2,
|
||||
SaindoRua = 3,
|
||||
Manobrando = 4,
|
||||
Direcionando = 5
|
||||
}
|
||||
|
||||
public enum DirecaoCarroRua
|
||||
|
|
@ -241,6 +241,7 @@
|
|||
Script_GpsViewer = 13,
|
||||
ArquivoModelo3D = 14,
|
||||
ScriptListOAKCaneras = 15,
|
||||
ScriptMPCController = 16,
|
||||
}
|
||||
|
||||
public enum TipoConexao
|
||||
|
|
|
|||
|
|
@ -1,224 +1,186 @@
|
|||
using AgroBase.Models;
|
||||
using AgroBase.Services;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Threading.Tasks;
|
||||
using static AgroBase.Models.Enums;
|
||||
|
||||
public class MPCController
|
||||
{
|
||||
private int horizon;
|
||||
private double maxSteeringAngle;
|
||||
private double maxSteeringAngleFourWheels;
|
||||
private double minVelocity;
|
||||
private double maxVelocity;
|
||||
private double accelerationLimit;
|
||||
|
||||
// Pesos da função de custo ajustados
|
||||
private double distancePredict = 3.0;
|
||||
private double weightPosition = 1.0;
|
||||
private double weightAlignment = 0.8;
|
||||
private double weightSteering = 0.5;
|
||||
private double weightVelocity = 0.3;
|
||||
private double weightAcceleration = 1.0; // Penaliza mudanças bruscas de velocidade
|
||||
private double weightObstacle = 2.5; // Maior penalização para obstáculos
|
||||
private double weightLaneCentering = 1.2; // Mantém o robô no centro do corredor
|
||||
|
||||
private string TopicoRota = "mpc/rota";
|
||||
private string TopicoPosicao = "mpc/posicao";
|
||||
private string TopicoComando = "mpc/comando";
|
||||
private int Horizonte = 20;
|
||||
private double _dt = 0;
|
||||
private DateTime _ultimaAtualizacao = DateTime.MinValue;
|
||||
public Process pythonProcess;
|
||||
public readonly object processLock = new object();
|
||||
public bool Iniciado = false;
|
||||
|
||||
public MPCComandoModel ComandoAtual = new MPCComandoModel();
|
||||
|
||||
public MPCController()
|
||||
{
|
||||
// Definições baseadas em variáveis globais
|
||||
this.horizon = Convert.ToInt32(distancePredict / TrajetoriaMapaOperacaoModel.DistanciaEntrePontos);
|
||||
this.maxSteeringAngle = Variaveis.OperacaoEmAndamento?.Controle?.Angulo_Max ?? 30.0;
|
||||
this.maxSteeringAngleFourWheels = this.maxSteeringAngle * 1.0;
|
||||
this.minVelocity = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeMin ?? 20.0;
|
||||
this.maxVelocity = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeMax ?? 100.0;
|
||||
this.accelerationLimit = 1.5; // Limita aceleração a valores realistas
|
||||
|
||||
}
|
||||
|
||||
public (double steeringAngle, Enums.TipoMovimentoDirecional mode) ComputeControl(
|
||||
double currentX, double currentY, double currentTheta,
|
||||
double targetX, double targetY, double targetTheta,
|
||||
double currentVelocity, double leftEdge, double rightEdge, List<Obstaculo> obstacles)
|
||||
public async Task<bool> Inicializar()
|
||||
{
|
||||
double dt = (DateTime.Now - _ultimaAtualizacao).TotalSeconds;
|
||||
dt = Math.Max(0.01, Math.Min(dt, 0.2));
|
||||
|
||||
// Define o número total de passos e passos por grupo
|
||||
int totalSteps = 10;
|
||||
int stepsPerGroup = 5;
|
||||
|
||||
// Calcula o número de grupos
|
||||
int numGroups = totalSteps / stepsPerGroup;
|
||||
|
||||
// Pré-calcula todas as combinações de controle
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions = PreGenerateControlOptions();
|
||||
|
||||
// Variáveis para armazenar o melhor custo e a melhor sequência
|
||||
double bestCost = double.MaxValue;
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> bestSequence = null;
|
||||
|
||||
// Função de avaliação (passada como parâmetro)
|
||||
Func<List<(double steering, Enums.TipoMovimentoDirecional mode)>, double> evaluateSequence = sequence =>
|
||||
await Variaveis.MqttService.AdicionarNovoTopico(TopicoRota);
|
||||
await Variaveis.MqttService.AdicionarNovoTopico(TopicoPosicao);
|
||||
await Variaveis.MqttService.AdicionarNovoTopico(TopicoComando, true, 2, async (mensagem) =>
|
||||
{
|
||||
return SimulateTrajectory(
|
||||
currentX, currentY, currentTheta,
|
||||
targetX, targetY, targetTheta,
|
||||
currentVelocity, sequence,
|
||||
leftEdge, rightEdge, obstacles, dt, Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.CorredorAtual.Dentro,
|
||||
stepsPerGroup // Passa o número de passos por grupo
|
||||
);
|
||||
if (!Iniciado && mensagem.Mensagem == "OK")
|
||||
{
|
||||
Iniciado = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
try
|
||||
{
|
||||
ComandoAtual = JsonConvert.DeserializeObject<MPCComandoModel>(mensagem.Mensagem);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine("Erro ao deserializar resposta do MPC: " + ex.Message.ToString());
|
||||
}
|
||||
_ultimaAtualizacao = DateTime.Now;
|
||||
}
|
||||
});
|
||||
|
||||
lock (processLock)
|
||||
{
|
||||
pythonProcess = PythonService.RunScript(PythonService.ScriptMPCController, new string[] {});
|
||||
}
|
||||
|
||||
bool ScriptIniciado() => Iniciado;
|
||||
await FuncoesGlobais.AguardarCondicaoAsync(ScriptIniciado);
|
||||
|
||||
return Iniciado;
|
||||
}
|
||||
|
||||
public async Task EnviarDadosTrajetoria(List<PontoTrajetoriaModel> _trajetoria)
|
||||
{
|
||||
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
|
||||
|
||||
var trajetoria = new
|
||||
{
|
||||
pontos = _trajetoria
|
||||
.Select(p => new
|
||||
{
|
||||
lat = p.Posicao.Latitude,
|
||||
lon = p.Posicao.Longitude,
|
||||
tipo = (int)p.Tipo,
|
||||
distanciaMargem = p.LarguraCorredor * 0.8
|
||||
})
|
||||
.ToList(),
|
||||
horizonte = Horizonte,
|
||||
angulo_max_graus = _Controle.Angulo_Max,
|
||||
distancia_entre_eixos = (VariaveisEquipamento.DistanciaEntreEixos / 100.0),
|
||||
velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMin),
|
||||
velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMax)
|
||||
};
|
||||
|
||||
// Gera e avalia todas as sequências dinamicamente
|
||||
GenerateAndEvaluateSequences(
|
||||
new List<(double steering, Enums.TipoMovimentoDirecional mode)>(),
|
||||
numGroups, // Número de grupos
|
||||
controlOptions,
|
||||
ref bestCost,
|
||||
ref bestSequence,
|
||||
evaluateSequence
|
||||
await Variaveis.MqttService.PublishAsync(
|
||||
Variaveis.MqttService.Topicos.First(x => x.Topico == TopicoRota),
|
||||
JsonConvert.SerializeObject(trajetoria)
|
||||
);
|
||||
}
|
||||
|
||||
public async Task<MPCComandoModel> Compute(double _lat, double _long, double _theta, double _velocidadeMs)
|
||||
{
|
||||
DateTime EnviadoEm = DateTime.Now;
|
||||
|
||||
double taxaHz = GPSService.TaxaAmostragemHz; // ex: 10 Hz
|
||||
double tempoIdeal = 1.0 / taxaHz; // = 0.1 s
|
||||
double tempoMinimo = tempoIdeal * 0.5; // 50% abaixo
|
||||
double tempoMaximo = tempoIdeal * 2.0; // até 2x acima
|
||||
_dt = (EnviadoEm - _ultimaAtualizacao).TotalSeconds;
|
||||
double dtCalc = Math.Max(tempoMinimo, Math.Min(tempoMaximo, _dt));
|
||||
|
||||
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
|
||||
|
||||
var dadosEnvio = new
|
||||
{
|
||||
lat = _lat,
|
||||
lon = _long,
|
||||
theta = _theta,
|
||||
velocidade = _velocidadeMs,
|
||||
dt = dtCalc,
|
||||
contexto = new
|
||||
{
|
||||
StatusCarro = (int)_Sensoriamento.Trajetoria.StatusCarro,
|
||||
DentroCorredor = _Sensoriamento.Trajetoria.CorredorAtual.Dentro,
|
||||
ManobrandoEntreRuas = _Sensoriamento.Trajetoria.ManobrandoEntreRuas
|
||||
}
|
||||
};
|
||||
|
||||
await Variaveis.MqttService.PublishAsync(
|
||||
Variaveis.MqttService.Topicos.First(x => x.Topico == TopicoPosicao),
|
||||
JsonConvert.SerializeObject(dadosEnvio)
|
||||
);
|
||||
|
||||
_ultimaAtualizacao = DateTime.Now;
|
||||
bool Respondido() => _ultimaAtualizacao > EnviadoEm;
|
||||
await FuncoesGlobais.AguardarCondicaoAsync(Respondido, 200, 10);
|
||||
|
||||
// Retorna o primeiro comando da melhor sequência
|
||||
return bestSequence[0];
|
||||
return ComandoAtual;
|
||||
}
|
||||
|
||||
|
||||
private List<(double steering, Enums.TipoMovimentoDirecional mode)> PreGenerateControlOptions()
|
||||
public MPCController Clone()
|
||||
{
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions = new List<(double steering, Enums.TipoMovimentoDirecional mode)>();
|
||||
|
||||
// Gera todas as opções de steering (ângulos) e modos
|
||||
List<double> steeringOptions = new List<double>();
|
||||
for (double s = -maxSteeringAngle; s <= maxSteeringAngle; s += 5.0) // Passo de 5 graus
|
||||
return new MPCController()
|
||||
{
|
||||
steeringOptions.Add(s);
|
||||
ComandoAtual = ComandoAtual?.Clone() ?? new MPCComandoModel(),
|
||||
Horizonte = Horizonte,
|
||||
Iniciado = Iniciado,
|
||||
_ultimaAtualizacao = _ultimaAtualizacao,
|
||||
_dt = _dt
|
||||
}
|
||||
; }
|
||||
|
||||
List<Enums.TipoMovimentoDirecional> modeOptions = new List<Enums.TipoMovimentoDirecional>()
|
||||
{
|
||||
Enums.TipoMovimentoDirecional.RodasDianteiras,
|
||||
Enums.TipoMovimentoDirecional.MovimentoArco
|
||||
};
|
||||
|
||||
// Cria todas as combinações de steering e mode
|
||||
foreach (var steering in steeringOptions)
|
||||
{
|
||||
foreach (var mode in modeOptions)
|
||||
{
|
||||
controlOptions.Add((steering, mode));
|
||||
}
|
||||
}
|
||||
|
||||
return controlOptions;
|
||||
}
|
||||
|
||||
private void GenerateControlSequences(List<(double steering, Enums.TipoMovimentoDirecional mode)> currentSequence, int remainingSteps, List<List<(double steering, Enums.TipoMovimentoDirecional mode)>> allSequences, List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions) // Pré-geradas
|
||||
public void Dispose()
|
||||
{
|
||||
// Base da recursão: se não houver mais passos no horizonte, salva a sequência
|
||||
if (remainingSteps == 0)
|
||||
lock (processLock)
|
||||
{
|
||||
allSequences.Add(new List<(double steering, Enums.TipoMovimentoDirecional mode)>(currentSequence));
|
||||
return;
|
||||
pythonProcess?.Kill();
|
||||
pythonProcess?.Dispose();
|
||||
pythonProcess = null;
|
||||
}
|
||||
|
||||
// Itera sobre as combinações pré-calculadas
|
||||
foreach (var control in controlOptions)
|
||||
{
|
||||
// Adiciona o controle atual à sequência
|
||||
currentSequence.Add(control);
|
||||
|
||||
// Chama recursivamente para o próximo passo, diminuindo o horizonte
|
||||
GenerateControlSequences(currentSequence, remainingSteps - 1, allSequences, controlOptions);
|
||||
|
||||
// Remove o controle para explorar a próxima combinação
|
||||
currentSequence.RemoveAt(currentSequence.Count - 1);
|
||||
}
|
||||
Iniciado = false;
|
||||
ComandoAtual = new MPCComandoModel();
|
||||
Task.Run(async () => {
|
||||
Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoRota));
|
||||
Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoPosicao));
|
||||
await Variaveis.MqttService.UnsubscribeAsync(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
|
||||
Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
|
||||
});
|
||||
}
|
||||
|
||||
private void GenerateAndEvaluateSequences(
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> currentSequence,
|
||||
int remainingSteps,
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions,
|
||||
ref double bestCost,
|
||||
ref List<(double steering, Enums.TipoMovimentoDirecional mode)> bestSequence,
|
||||
Func<List<(double steering, Enums.TipoMovimentoDirecional mode)>, double> evaluateSequence)
|
||||
{
|
||||
// Base da recursão: se não houver mais passos, avalia a sequência
|
||||
if (remainingSteps == 0)
|
||||
{
|
||||
double cost = evaluateSequence(currentSequence);
|
||||
|
||||
// Verifica se a sequência atual tem o menor custo
|
||||
if (cost < bestCost)
|
||||
{
|
||||
bestCost = cost;
|
||||
bestSequence = new List<(double steering, Enums.TipoMovimentoDirecional mode)>(currentSequence);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Itera sobre as combinações de controle
|
||||
foreach (var control in controlOptions)
|
||||
{
|
||||
// Adiciona o controle atual à sequência
|
||||
currentSequence.Add(control);
|
||||
|
||||
// Chama recursivamente para o próximo passo, diminuindo o horizonte
|
||||
GenerateAndEvaluateSequences(currentSequence, remainingSteps - 1, controlOptions, ref bestCost, ref bestSequence, evaluateSequence);
|
||||
|
||||
// Remove o controle para explorar a próxima combinação
|
||||
currentSequence.RemoveAt(currentSequence.Count - 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private double SimulateTrajectory(
|
||||
double lat, double lon, double theta,
|
||||
double targetLat, double targetLon, double targetTheta,
|
||||
double currentVelocity,
|
||||
List<(double steering, Enums.TipoMovimentoDirecional mode)> sequence,
|
||||
double leftEdge, double rightEdge, List<Obstaculo> obstacles, double dt, bool insideStreet,
|
||||
int stepsPerGroup)
|
||||
{
|
||||
double cost = 0;
|
||||
|
||||
var posicaoAtual = new GPSModel { Latitude = lat, Longitude = lon, OrientacaoReal = theta };
|
||||
var posicaoAlvo = new GPSModel { Latitude = targetLat, Longitude = targetLon };
|
||||
(double targetX, double targetY) = GPSUtils.ConverterLatLongParaMetros(posicaoAlvo, posicaoAtual);
|
||||
|
||||
foreach (var control in sequence)
|
||||
{
|
||||
double steering = control.steering;
|
||||
Enums.TipoMovimentoDirecional mode = control.mode;
|
||||
|
||||
// Simula "stepsPerGroup" passos para o mesmo comando
|
||||
for (int step = 0; step < stepsPerGroup; step++)
|
||||
{
|
||||
var novaPosicao = VariaveisEquipamento.SimularNovaPosicao(mode, steering, currentVelocity, dt, theta, posicaoAtual);
|
||||
|
||||
posicaoAtual = novaPosicao;
|
||||
theta = novaPosicao.OrientacaoReal;
|
||||
|
||||
(double novaX, double novaY) = GPSUtils.ConverterLatLongParaMetros(novaPosicao, posicaoAlvo);
|
||||
|
||||
double distanceError = Math.Sqrt(Math.Pow(targetX - novaX, 2) + Math.Pow(targetY - novaY, 2));
|
||||
double laneError = insideStreet ? Math.Abs(leftEdge - rightEdge) : 0.0;
|
||||
|
||||
// Acumula o custo
|
||||
cost += (distanceError * 10.0) + (laneError * 20.0);
|
||||
}
|
||||
}
|
||||
|
||||
return cost;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
public class MPCComandoModel
|
||||
{
|
||||
public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras;
|
||||
public double angulo { get; set; } = 0;
|
||||
public List<MPCSimulacaoModel> simulacao { get; set; } = new List<MPCSimulacaoModel>();
|
||||
|
||||
public MPCComandoModel Clone()
|
||||
{
|
||||
return new MPCComandoModel()
|
||||
{
|
||||
tipo = tipo,
|
||||
angulo = angulo,
|
||||
simulacao = new List<MPCSimulacaoModel>(simulacao)
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
public class MPCSimulacaoModel
|
||||
{
|
||||
public double latitude { get; set; }
|
||||
public double longitude { get; set; }
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ namespace AgroBase.Models
|
|||
private List<PontoTrajetoriaModel> TrajetoriaDinamica { get; set; } = new List<PontoTrajetoriaModel>();
|
||||
private List<PontoTrajetoriaModel> TrajetoriaRuaProjetada { get; set; } = new List<PontoTrajetoriaModel>();
|
||||
private List<GPSModel> TrajetoriaRobo { get; set; } = new List<GPSModel>();
|
||||
public List<MPCSimulacaoModel> TrajetoriaSimuladaMPC { get; set; } = new List<MPCSimulacaoModel>();
|
||||
private Obstaculo obstaculoDetectado { get; set; }
|
||||
private float AnguloCaminho { get; set; }
|
||||
private float AnguloCarro { get; set; }
|
||||
|
|
@ -42,7 +43,16 @@ namespace AgroBase.Models
|
|||
pnlZoomMapa.GetType().GetMethod("SetStyle", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic).Invoke(pnlZoomMapa, new object[] { ControlStyles.UserPaint | ControlStyles.AllPaintingInWmPaint | ControlStyles.OptimizedDoubleBuffer, true });
|
||||
}
|
||||
|
||||
public void AtualizarDados(Obstaculo obstaculo, float anguloCaminho, float anguloCarro, GPSModel posicaoAtual, List<PontoTrajetoriaModel> trajetoriaDinamica, List<PontoTrajetoriaModel> trajetoriaRua, List<GPSModel> trajetoriaRobo, List<List<GPSModel>> ruasPlantacao)
|
||||
public void AtualizarDados(
|
||||
Obstaculo obstaculo,
|
||||
float anguloCaminho,
|
||||
float anguloCarro,
|
||||
GPSModel posicaoAtual,
|
||||
List<PontoTrajetoriaModel> trajetoriaDinamica,
|
||||
List<PontoTrajetoriaModel> trajetoriaRua,
|
||||
List<GPSModel> trajetoriaRobo,
|
||||
List<List<GPSModel>> ruasPlantacao,
|
||||
List<MPCSimulacaoModel> trajetoriaSimulada = null)
|
||||
{
|
||||
obstaculoDetectado = obstaculo;
|
||||
AnguloCaminho = anguloCaminho;
|
||||
|
|
@ -52,6 +62,7 @@ namespace AgroBase.Models
|
|||
TrajetoriaRuaProjetada = trajetoriaRua;
|
||||
TrajetoriaRobo = trajetoriaRobo;
|
||||
RuasPlantacao = ruasPlantacao;
|
||||
TrajetoriaSimuladaMPC = trajetoriaSimulada ?? new List<MPCSimulacaoModel>();
|
||||
|
||||
pnlZoomMapa.Invalidate();
|
||||
}
|
||||
|
|
@ -151,6 +162,19 @@ namespace AgroBase.Models
|
|||
{
|
||||
DrawObstacle(g, centerX, centerY, Color.Cyan, Color.Cyan, AnguloCaminho - (float)obstaculoDetectado.AnguloParaDesvio, obstaculoDetectado, PxToCm);
|
||||
}
|
||||
|
||||
// Desenha a simulação do MPC (linha tracejada verde-limão)
|
||||
if (TrajetoriaSimuladaMPC != null && TrajetoriaSimuladaMPC.Count > 1)
|
||||
{
|
||||
var _trajetoriaSimulada = TrajetoriaSimuladaMPC
|
||||
.Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = new GPSModel() { Latitude = x.latitude, Longitude = x.longitude } })
|
||||
.ToList();
|
||||
|
||||
Pen penTracejada = new Pen(Color.LimeGreen, 2);
|
||||
penTracejada.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash;
|
||||
|
||||
DrawTrajectory(g, _trajetoriaSimulada, centerX, centerY, Color.LimeGreen, penTracejada);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -13,7 +13,7 @@ namespace AgroBase.Models.Operacoes
|
|||
|
||||
#region DIRECIONAL
|
||||
|
||||
public void DefinirDadosControleDir()
|
||||
public async void DefinirDadosControleDir()
|
||||
{
|
||||
if (Variaveis.OperacaoEmAndamento.Modo != ModoOperacao.MapaGPS)
|
||||
{
|
||||
|
|
@ -37,13 +37,10 @@ namespace AgroBase.Models.Operacoes
|
|||
}
|
||||
case TiposControladorDirecional.MPC:
|
||||
{
|
||||
var Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria;
|
||||
|
||||
var MPC_A = new MPCControllerAprimorado();
|
||||
(double angulo, TipoMovimentoDirecional tipoControle) = MPC_A.CalcularControle(GPSService.UltimaLeitura, Controle.PercentualVelocidadeSP, Trajetoria.TrajetoriaDinamica, new TipoMovimentoDirecional[] { TipoMovimentoDirecional.RodasDianteiras });
|
||||
|
||||
Controle.Angulo = angulo;
|
||||
Controle.TipoMovimento = tipoControle;
|
||||
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
|
||||
MPCComandoModel comando = await Variaveis.OperacaoEmAndamento.Controle.ControladorMPC.Compute(_Sensoriamento.Gps.Latitude, _Sensoriamento.Gps.Longitude, _Sensoriamento.Gps.AnguloCarroDefinido, _Sensoriamento.Movimentacao.VelocidadeMedia);
|
||||
Controle.Angulo = comando?.angulo ?? 0;
|
||||
Controle.TipoMovimento = comando?.tipo ?? TipoMovimentoDirecional.RodasDianteiras;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -206,7 +203,7 @@ namespace AgroBase.Models.Operacoes
|
|||
Controle.FiltroVelocidade.Reset(Controle.PercentualVelocidadeSP);
|
||||
}
|
||||
|
||||
Console.WriteLine($"Velocidade SP: {Controle.PercentualVelocidadeSP}, ms: {FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP)}, km/h: {FuncoesMatematicas.ConverteMsParaKmh(FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP))}");
|
||||
//Console.WriteLine($"Velocidade SP: {Controle.PercentualVelocidadeSP}, ms: {FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP)}, km/h: {FuncoesMatematicas.ConverteMsParaKmh(FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP))}");
|
||||
|
||||
//double velocidadeFiltrada = Controle.FiltroVelocidade.Filtrar(Controle.PercentualVelocidadeSP);
|
||||
//Controle.PercentualVelocidadeSP = Math.Max(Controle.PercentualVelocidadeMin, velocidadeFiltrada);
|
||||
|
|
|
|||
|
|
@ -440,6 +440,8 @@ namespace AgroBase.Models
|
|||
|
||||
tmrLogs?.Stop();
|
||||
|
||||
Variaveis.OperacaoEmAndamento.Controle.ControladorMPC?.Dispose();
|
||||
|
||||
Variaveis.OperacaoEmAndamento = new OperacaoModel(Modo)
|
||||
{
|
||||
Iniciado = false,
|
||||
|
|
@ -452,7 +454,6 @@ namespace AgroBase.Models
|
|||
QuantidadeCamerasSolo = 1,
|
||||
};
|
||||
|
||||
|
||||
switch (Modo)
|
||||
{
|
||||
case ModoOperacao.Manual:
|
||||
|
|
@ -564,7 +565,7 @@ namespace AgroBase.Models
|
|||
SegmentacaoSemantica = false,
|
||||
PulverizadorAutomatico = true,
|
||||
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
|
||||
TipoControleDirecional = TiposControladorDirecional.PID,
|
||||
TipoControleDirecional = TiposControladorDirecional.MPC,
|
||||
};
|
||||
Variaveis.OperacaoEmAndamento.ModulosMandatorios = new List<OperacaoModulosMandatoriosModel>()
|
||||
{
|
||||
|
|
@ -801,6 +802,11 @@ namespace AgroBase.Models
|
|||
};
|
||||
Controle.FiltroVelocidade = new KalmanFilter(erroMedicao: 2.0, erroProcesso: 0.05);
|
||||
Controle.ControladorMPC = new MPCController();
|
||||
bool mpcIniciado = await Controle.ControladorMPC.Inicializar();
|
||||
if (mpcIniciado)
|
||||
{
|
||||
await Controle.ControladorMPC.EnviarDadosTrajetoria(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa);
|
||||
}
|
||||
}
|
||||
|
||||
Controle.Angulo = 0;
|
||||
|
|
@ -837,6 +843,8 @@ namespace AgroBase.Models
|
|||
|
||||
Variaveis.OperacaoEmAndamento.PararCarroOperacaoInterrompida(true);
|
||||
|
||||
Variaveis.OperacaoEmAndamento.Controle.ControladorMPC?.Dispose();
|
||||
|
||||
await Task.Delay(2000);
|
||||
|
||||
Variaveis.OperacaoEmAndamento.Iniciado = false;
|
||||
|
|
@ -909,21 +917,12 @@ namespace AgroBase.Models
|
|||
|
||||
ID = "";
|
||||
|
||||
if (DispMvd != null)
|
||||
{
|
||||
DispMvd.Dados.ReiniciarLeituras(DispMvd.Dados);
|
||||
}
|
||||
if (DispAtu != null)
|
||||
{
|
||||
DispAtu.Dados.ReiniciarLeituras(DispAtu.Dados);
|
||||
}
|
||||
if (DispSen != null)
|
||||
{
|
||||
DispSen.Dados.ReiniciarLeituras(DispSen.Dados);
|
||||
}
|
||||
DispMvd?.Dados?.ReiniciarLeituras(DispMvd.Dados);
|
||||
DispAtu?.Dados?.ReiniciarLeituras(DispAtu.Dados);
|
||||
DispSen?.Dados?.ReiniciarLeituras(DispSen.Dados);
|
||||
|
||||
Sensoriamento.AtualizarDados();
|
||||
OpMapaGPS.Sensoriamento.ReiniciarLeituras();
|
||||
Sensoriamento?.AtualizarDados();
|
||||
OpMapaGPS?.Sensoriamento?.ReiniciarLeituras();
|
||||
|
||||
if (Modo == ModoOperacao.MapaGPS)
|
||||
{
|
||||
|
|
@ -934,6 +933,14 @@ namespace AgroBase.Models
|
|||
};
|
||||
Controle.FiltroVelocidade = new KalmanFilter(erroMedicao: 2.0, erroProcesso: 0.05);
|
||||
Controle.ControladorMPC = new MPCController();
|
||||
Task.Run(async () =>
|
||||
{
|
||||
bool mpcIniciado = await Controle.ControladorMPC.Inicializar();
|
||||
if (mpcIniciado)
|
||||
{
|
||||
await Controle.ControladorMPC.EnviarDadosTrajetoria(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
Controle.Angulo = 0;
|
||||
|
|
@ -944,6 +951,7 @@ namespace AgroBase.Models
|
|||
|
||||
public void ReiniciarSimulacao()
|
||||
{
|
||||
Variaveis.OperacaoEmAndamento.Controle.ControladorMPC?.Dispose();
|
||||
Variaveis.OperacaoEmAndamento.StatusAtual = StatusOperacao.Parametrizando;
|
||||
Variaveis.OperacaoEmAndamento.Iniciado = false;
|
||||
Variaveis.OperacaoEmAndamento.Simulando = false;
|
||||
|
|
@ -2879,6 +2887,8 @@ namespace AgroBase.Models
|
|||
PercentualVelocidadeSP = PercentualVelocidadeSP,
|
||||
PIDdirecional = PIDdirecional.Clone(),
|
||||
FiltroVelocidade = FiltroVelocidade.Clone(),
|
||||
ControladorMPC = ControladorMPC.Clone(),
|
||||
_tipoMovimento = _tipoMovimento,
|
||||
PressaoLinha = PressaoLinha,
|
||||
PulverizadorAutomatico = PulverizadorAutomatico,
|
||||
RPM_Max = RPM_Max,
|
||||
|
|
|
|||
|
|
@ -217,8 +217,8 @@ namespace AgroBase.Models
|
|||
public double DistanciaDireita { get; private set; }
|
||||
private void AtualizarDistanciasLaterais()
|
||||
{
|
||||
DistanciaEsquerda = CalcularDistanciaLateral(true, GPSService.UltimaLeitura, CorredorAtual.idxRuaEsquerda, PontoAtual.Direcao);
|
||||
DistanciaDireita = CalcularDistanciaLateral(false, GPSService.UltimaLeitura, CorredorAtual.idxRuaDireita, PontoAtual.Direcao);
|
||||
DistanciaEsquerda = CalcularDistanciaLateral(true, GPSService.UltimaLeitura, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
||||
DistanciaDireita = CalcularDistanciaLateral(false, GPSService.UltimaLeitura, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
||||
}
|
||||
|
||||
public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)
|
||||
|
|
|
|||
|
|
@ -63,6 +63,13 @@ namespace AgroBase.Services
|
|||
return VersionamentoService.ArquivoScript(Enums.TipoArquivoVersionado.ScriptListOAKCaneras).NomeArquivoLocal;
|
||||
}
|
||||
}
|
||||
public static string ScriptMPCController
|
||||
{
|
||||
get
|
||||
{
|
||||
return VersionamentoService.ArquivoScript(Enums.TipoArquivoVersionado.ScriptMPCController).NomeArquivoLocal;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static List<Process> processosIniciados = new List<Process>();
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
|
|
@ -225,5 +225,18 @@
|
|||
"Atualizado": true,
|
||||
"ArquivoAtualizado": true,
|
||||
"TipoArquivo": 15
|
||||
},
|
||||
{
|
||||
"id": 20,
|
||||
"Diretorio": "Python\\Scripts\\",
|
||||
"Arquivo": "mpc_controller",
|
||||
"Extensao": ".py",
|
||||
"Versao": "1_0",
|
||||
"ArquivoDownload": "mpc_controller-1_0.py",
|
||||
"NomeArquivoLocal": "mpc_controller-1_0.py",
|
||||
"CaminhoCompleto": "Python\\Scripts\\mpc_controller-1_0.py",
|
||||
"Atualizado": true,
|
||||
"ArquivoAtualizado": true,
|
||||
"TipoArquivo": 16
|
||||
}
|
||||
]
|
||||
|
|
@ -1 +1,221 @@
|
|||
{"type":"FeatureCollection","features":[{"type":"Feature","properties":{"Id":"1","Name":"CidadeJardimTerreno2","Length":14.558011415731592,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"0","type":"LineString","coordinates":[[-47.395205344,-22.172559531333334],[-47.395212610166666,-22.172614638833334],[-47.395219157,-22.172656417833334],[-47.395223544833335,-22.1726892105],[-47.395224140984432,-22.172693691611649],[-47.395225326538004,-22.172702654611555]]}},{"type":"Feature","properties":{"Id":"2","Name":"CidadeJardimTerreno2","Length":14.771318274761821,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"1","type":"LineString","coordinates":[[-47.395210902333332,-22.172708800833334],[-47.395206943666665,-22.172679811],[-47.395202420666664,-22.1726491015],[-47.395198865666664,-22.172615782833333],[-47.395193255833334,-22.172577132833332],[-47.395192610024395,-22.172572657695412],[-47.395191325698136,-22.172563706509337]]}},{"type":"Feature","properties":{"Id":"3","Name":"CidadeJardimTerreno2","Length":14.827915524386164,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"2","type":"LineString","coordinates":[[-47.3951762925,-22.172561603],[-47.395180824166665,-22.172591686166665],[-47.395185745333336,-22.172623080166666],[-47.395190433,-22.172656367166667],[-47.395195199,-22.172693641833334],[-47.395195769044363,-22.172698125891035],[-47.395196902671593,-22.172707094716973]]}},{"type":"Feature","properties":{"Id":"4","Name":"CidadeJardimTerreno2","Length":14.785159385903514,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"3","type":"LineString","coordinates":[[-47.395182932166669,-22.1727127985],[-47.39517819266667,-22.172680514833335],[-47.3951732595,-22.172646752833334],[-47.395168805166669,-22.1726149155],[-47.395163812333337,-22.172581167166665],[-47.395163154299318,-22.172576693573966],[-47.395161845655842,-22.172567745443878]]}},{"type":"Feature","properties":{"Id":"5","Name":"CidadeJardimTerreno2","Length":14.80117692191233,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"4","type":"LineString","coordinates":[[-47.395147317833334,-22.172566031],[-47.395151503166666,-22.172595452333333],[-47.3951561855,-22.172628011166665],[-47.395161598666668,-22.172663757333332],[-47.395166727166668,-22.172697770833334],[-47.395167397572706,-22.172702242832194],[-47.39516873082615,-22.172711187810091]]}}]}
|
||||
{
|
||||
"type": "FeatureCollection",
|
||||
"features": [
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "1",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.558011415731592,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.395205344,
|
||||
-22.172559531333334
|
||||
],
|
||||
[
|
||||
-47.395212610166666,
|
||||
-22.172614638833334
|
||||
],
|
||||
[
|
||||
-47.395219157,
|
||||
-22.172656417833334
|
||||
],
|
||||
[
|
||||
-47.395223544833335,
|
||||
-22.1726892105
|
||||
],
|
||||
[
|
||||
-47.39522414098443,
|
||||
-22.17269369161165
|
||||
],
|
||||
[
|
||||
-47.395225326538004,
|
||||
-22.172702654611555
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "2",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.771318274761821,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.39521090233333,
|
||||
-22.172708800833334
|
||||
],
|
||||
[
|
||||
-47.395206943666665,
|
||||
-22.172679811
|
||||
],
|
||||
[
|
||||
-47.395202420666664,
|
||||
-22.1726491015
|
||||
],
|
||||
[
|
||||
-47.395198865666664,
|
||||
-22.172615782833333
|
||||
],
|
||||
[
|
||||
-47.395193255833334,
|
||||
-22.172577132833332
|
||||
],
|
||||
[
|
||||
-47.395192610024395,
|
||||
-22.17257265769541
|
||||
],
|
||||
[
|
||||
-47.395191325698136,
|
||||
-22.172563706509337
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "3",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.827915524386164,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.3951762925,
|
||||
-22.172561603
|
||||
],
|
||||
[
|
||||
-47.395180824166665,
|
||||
-22.172591686166665
|
||||
],
|
||||
[
|
||||
-47.395185745333336,
|
||||
-22.172623080166666
|
||||
],
|
||||
[
|
||||
-47.395190433,
|
||||
-22.172656367166667
|
||||
],
|
||||
[
|
||||
-47.395195199,
|
||||
-22.172693641833334
|
||||
],
|
||||
[
|
||||
-47.39519576904436,
|
||||
-22.172698125891035
|
||||
],
|
||||
[
|
||||
-47.39519690267159,
|
||||
-22.172707094716973
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "4",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.785159385903514,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.39518293216667,
|
||||
-22.1727127985
|
||||
],
|
||||
[
|
||||
-47.39517819266667,
|
||||
-22.172680514833335
|
||||
],
|
||||
[
|
||||
-47.3951732595,
|
||||
-22.172646752833334
|
||||
],
|
||||
[
|
||||
-47.39516880516667,
|
||||
-22.1726149155
|
||||
],
|
||||
[
|
||||
-47.39516381233334,
|
||||
-22.172581167166665
|
||||
],
|
||||
[
|
||||
-47.39516315429932,
|
||||
-22.172576693573966
|
||||
],
|
||||
[
|
||||
-47.39516184565584,
|
||||
-22.172567745443878
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "5",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.80117692191233,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.395147317833334,
|
||||
-22.172566031
|
||||
],
|
||||
[
|
||||
-47.395151503166666,
|
||||
-22.172595452333333
|
||||
],
|
||||
[
|
||||
-47.3951561855,
|
||||
-22.172628011166665
|
||||
],
|
||||
[
|
||||
-47.39516159866667,
|
||||
-22.172663757333332
|
||||
],
|
||||
[
|
||||
-47.39516672716667,
|
||||
-22.172697770833334
|
||||
],
|
||||
[
|
||||
-47.395167397572706,
|
||||
-22.172702242832194
|
||||
],
|
||||
[
|
||||
-47.39516873082615,
|
||||
-22.17271118781009
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -25,7 +25,7 @@
|
|||
<meta name="viewport" content="width=device-width,
|
||||
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
|
||||
<style>
|
||||
#map_5d2c7f135b188d679c1a4d3824a90e8f {
|
||||
#map_99bb698933046267d33b2256e2758b03 {
|
||||
position: relative;
|
||||
width: 100.0%;
|
||||
height: 100.0%;
|
||||
|
|
@ -39,14 +39,14 @@
|
|||
<body>
|
||||
|
||||
|
||||
<div class="folium-map" id="map_5d2c7f135b188d679c1a4d3824a90e8f" ></div>
|
||||
<div class="folium-map" id="map_99bb698933046267d33b2256e2758b03" ></div>
|
||||
|
||||
</body>
|
||||
<script>
|
||||
|
||||
|
||||
var map_5d2c7f135b188d679c1a4d3824a90e8f = L.map(
|
||||
"map_5d2c7f135b188d679c1a4d3824a90e8f",
|
||||
var map_99bb698933046267d33b2256e2758b03 = L.map(
|
||||
"map_99bb698933046267d33b2256e2758b03",
|
||||
{
|
||||
center: [-22.172636164916668, -47.395186322185666],
|
||||
crs: L.CRS.EPSG3857,
|
||||
|
|
@ -60,13 +60,13 @@
|
|||
|
||||
|
||||
|
||||
var tile_layer_c3102dc310a5c0966b042ddc1d0446a2 = L.tileLayer(
|
||||
var tile_layer_0a630e1ac8e2fe468b74b905e056ba5f = L.tileLayer(
|
||||
"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
|
||||
{"attribution": "\u0026copy; \u003ca href=\"https://www.openstreetmap.org/copyright\"\u003eOpenStreetMap\u003c/a\u003e contributors", "detectRetina": false, "maxNativeZoom": 19, "maxZoom": 19, "minZoom": 0, "noWrap": false, "opacity": 1, "subdomains": "abc", "tms": false}
|
||||
);
|
||||
|
||||
|
||||
tile_layer_c3102dc310a5c0966b042ddc1d0446a2.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
|
||||
tile_layer_0a630e1ac8e2fe468b74b905e056ba5f.addTo(map_99bb698933046267d33b2256e2758b03);
|
||||
|
||||
|
||||
|
||||
|
|
@ -82,7 +82,7 @@
|
|||
}*/
|
||||
});
|
||||
}
|
||||
function geo_json_eda399b26bc88766d3547dbe170f8acd_onEachFeature(feature, layer) {
|
||||
function geo_json_778ae99174b146383df6fca12636dac2_onEachFeature(feature, layer) {
|
||||
layer.on({
|
||||
|
||||
click: function(e) {
|
||||
|
|
@ -118,20 +118,20 @@
|
|||
}*/
|
||||
});
|
||||
};
|
||||
var geo_json_eda399b26bc88766d3547dbe170f8acd = L.geoJson(null, {
|
||||
onEachFeature: geo_json_eda399b26bc88766d3547dbe170f8acd_onEachFeature,
|
||||
var geo_json_778ae99174b146383df6fca12636dac2 = L.geoJson(null, {
|
||||
onEachFeature: geo_json_778ae99174b146383df6fca12636dac2_onEachFeature,
|
||||
|
||||
});
|
||||
|
||||
function geo_json_eda399b26bc88766d3547dbe170f8acd_add (data) {
|
||||
geo_json_eda399b26bc88766d3547dbe170f8acd
|
||||
function geo_json_778ae99174b146383df6fca12636dac2_add (data) {
|
||||
geo_json_778ae99174b146383df6fca12636dac2
|
||||
.addData(data);
|
||||
}
|
||||
geo_json_eda399b26bc88766d3547dbe170f8acd_add({"features": [{"geometry": {"coordinates": [[-47.395205344, -22.172559531333334], [-47.395212610166666, -22.172614638833334], [-47.395219157, -22.172656417833334], [-47.395223544833335, -22.1726892105], [-47.39522414098443, -22.17269369161165], [-47.395225326538004, -22.172702654611555]], "id": "0", "type": "LineString"}, "id": 0, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "1", "Length": 14.558011415731592, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39521090233333, -22.172708800833334], [-47.395206943666665, -22.172679811], [-47.395202420666664, -22.1726491015], [-47.395198865666664, -22.172615782833333], [-47.395193255833334, -22.172577132833332], [-47.395192610024395, -22.17257265769541], [-47.395191325698136, -22.172563706509337]], "id": "1", "type": "LineString"}, "id": 1, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "2", "Length": 14.771318274761821, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951762925, -22.172561603], [-47.395180824166665, -22.172591686166665], [-47.395185745333336, -22.172623080166666], [-47.395190433, -22.172656367166667], [-47.395195199, -22.172693641833334], [-47.39519576904436, -22.172698125891035], [-47.39519690267159, -22.172707094716973]], "id": "2", "type": "LineString"}, "id": 2, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "3", "Length": 14.827915524386164, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39518293216667, -22.1727127985], [-47.39517819266667, -22.172680514833335], [-47.3951732595, -22.172646752833334], [-47.39516880516667, -22.1726149155], [-47.39516381233334, -22.172581167166665], [-47.39516315429932, -22.172576693573966], [-47.39516184565584, -22.172567745443878]], "id": "3", "type": "LineString"}, "id": 3, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "4", "Length": 14.785159385903514, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395147317833334, -22.172566031], [-47.395151503166666, -22.172595452333333], [-47.3951561855, -22.172628011166665], [-47.39516159866667, -22.172663757333332], [-47.39516672716667, -22.172697770833334], [-47.395167397572706, -22.172702242832194], [-47.39516873082615, -22.17271118781009]], "id": "4", "type": "LineString"}, "id": 4, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "5", "Length": 14.80117692191233, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}], "type": "FeatureCollection"});
|
||||
geo_json_778ae99174b146383df6fca12636dac2_add({"features": [{"geometry": {"coordinates": [[-47.395205344, -22.172559531333334], [-47.395212610166666, -22.172614638833334], [-47.395219157, -22.172656417833334], [-47.395223544833335, -22.1726892105], [-47.39522414098443, -22.17269369161165], [-47.395225326538004, -22.172702654611555]], "id": null, "type": "LineString"}, "id": 0, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "1", "Length": 14.558011415731592, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39521090233333, -22.172708800833334], [-47.395206943666665, -22.172679811], [-47.395202420666664, -22.1726491015], [-47.395198865666664, -22.172615782833333], [-47.395193255833334, -22.172577132833332], [-47.395192610024395, -22.17257265769541], [-47.395191325698136, -22.172563706509337]], "id": null, "type": "LineString"}, "id": 1, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "2", "Length": 14.771318274761821, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951762925, -22.172561603], [-47.395180824166665, -22.172591686166665], [-47.395185745333336, -22.172623080166666], [-47.395190433, -22.172656367166667], [-47.395195199, -22.172693641833334], [-47.39519576904436, -22.172698125891035], [-47.39519690267159, -22.172707094716973]], "id": null, "type": "LineString"}, "id": 2, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "3", "Length": 14.827915524386164, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39518293216667, -22.1727127985], [-47.39517819266667, -22.172680514833335], [-47.3951732595, -22.172646752833334], [-47.39516880516667, -22.1726149155], [-47.39516381233334, -22.172581167166665], [-47.39516315429932, -22.172576693573966], [-47.39516184565584, -22.172567745443878]], "id": null, "type": "LineString"}, "id": 3, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "4", "Length": 14.785159385903514, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395147317833334, -22.172566031], [-47.395151503166666, -22.172595452333333], [-47.3951561855, -22.172628011166665], [-47.39516159866667, -22.172663757333332], [-47.39516672716667, -22.172697770833334], [-47.395167397572706, -22.172702242832194], [-47.39516873082615, -22.17271118781009]], "id": null, "type": "LineString"}, "id": 4, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "5", "Length": 14.80117692191233, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}], "type": "FeatureCollection"});
|
||||
|
||||
|
||||
|
||||
geo_json_eda399b26bc88766d3547dbe170f8acd.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
|
||||
geo_json_778ae99174b146383df6fca12636dac2.addTo(map_99bb698933046267d33b2256e2758b03);
|
||||
|
||||
</script>
|
||||
|
||||
|
|
@ -152,7 +152,7 @@
|
|||
}
|
||||
trajeto_json_add({"features": []});
|
||||
|
||||
trajeto_json.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
|
||||
trajeto_json.addTo(map_99bb698933046267d33b2256e2758b03);
|
||||
|
||||
function adicionarGeometria(novaGeometria) {
|
||||
trajeto_json.addData(novaGeometria);
|
||||
|
|
@ -210,7 +210,7 @@
|
|||
}
|
||||
trajeto_dinamico_json_add({"features": []});
|
||||
|
||||
trajeto_dinamico_json.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
|
||||
trajeto_dinamico_json.addTo(map_99bb698933046267d33b2256e2758b03);
|
||||
|
||||
function adicionarGeometriaDinamica(novaGeometria) {
|
||||
trajeto_dinamico_json.addData(novaGeometria);
|
||||
|
|
@ -263,9 +263,9 @@
|
|||
|
||||
var marcadorEquipamento = L.marker([0, 0], {
|
||||
icon: customIcon
|
||||
}).addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
|
||||
}).addTo(map_99bb698933046267d33b2256e2758b03);
|
||||
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
|
||||
var marcadorBase = L.marker([0, 0], {}).addTo(map_99bb698933046267d33b2256e2758b03);
|
||||
var icon = L.AwesomeMarkers.icon(
|
||||
{"extraClasses": "fa-rotate-0", "icon": "info-sign", "iconColor": "white", "markerColor": "red", "prefix": "glyphicon"}
|
||||
);
|
||||
|
|
@ -347,7 +347,7 @@
|
|||
}
|
||||
|
||||
if (foco) {
|
||||
map_5d2c7f135b188d679c1a4d3824a90e8f.setView(novaPosicao, map_5d2c7f135b188d679c1a4d3824a90e8f.getZoom());
|
||||
map_99bb698933046267d33b2256e2758b03.setView(novaPosicao, map_99bb698933046267d33b2256e2758b03.getZoom());
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -364,7 +364,7 @@
|
|||
function atualizarSelecaoRuas(selecionadas) {
|
||||
selecionadas = JSON.parse(selecionadas);
|
||||
RuasSelecionadas = Array.isArray(selecionadas) ? [...selecionadas] : [];
|
||||
geo_json_eda399b26bc88766d3547dbe170f8acd.eachLayer(function (layer) {
|
||||
geo_json_778ae99174b146383df6fca12636dac2.eachLayer(function (layer) {
|
||||
if (RuasSelecionadas.includes(parseInt(layer.feature.id))) {
|
||||
layer.setStyle({ color: 'blue' });
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,318 @@
|
|||
# MPC com MQTT, simulação e visualização da trajetória
|
||||
import numpy as np
|
||||
import paho.mqtt.client as mqtt
|
||||
import json
|
||||
from enum import Enum
|
||||
#import matplotlib.pyplot as plt
|
||||
|
||||
class StatusCarroMapa(Enum):
|
||||
Parado = 0
|
||||
EntrandoRua = 1
|
||||
CaminhandoRua = 2
|
||||
SaindoRua = 3
|
||||
Manobrando = 4
|
||||
Direcionando = 5
|
||||
|
||||
class TipoMovimentoDirecional(Enum):
|
||||
RodasDianteiras = 0
|
||||
RodasTraseiras = 1
|
||||
RotacionarNoEixo = 2
|
||||
MovimentoArco = 3
|
||||
MovimentoLateral = 4
|
||||
MovimentoDiagonal = 5
|
||||
Diagnostico = 6
|
||||
|
||||
class TipoPontoRua(Enum):
|
||||
Indefinido = -1
|
||||
PosicaoRobo = 0
|
||||
LigacaoEntrada = 1
|
||||
BordaEntrada = 2
|
||||
Rua = 3
|
||||
BordaSaida = 4
|
||||
LigacaoSaida = 5
|
||||
CruvaEntreCorredores = 6
|
||||
Desvio = 7
|
||||
|
||||
|
||||
topico_comando = "mpc/comando"
|
||||
topico_posicao = "mpc/posicao"
|
||||
topico_rota = "mpc/rota"
|
||||
|
||||
raio_terra = 6371000
|
||||
|
||||
# Parâmetros do MPC
|
||||
angulo_max_graus = 30.0
|
||||
velocidade_min = 0.4
|
||||
velocidade_max = 1.9
|
||||
distancia_entre_eixos = 0.92
|
||||
horizonte = 20
|
||||
|
||||
# Estado compartilhado
|
||||
trajetoria_latlon = []
|
||||
pontos_info = []
|
||||
visitados_execucao = []
|
||||
lat0, lon0 = None, None
|
||||
|
||||
# Visualização
|
||||
#plt.ion()
|
||||
#fig, ax = plt.subplots()
|
||||
|
||||
# Funções matemáticas auxiliares
|
||||
def latlon_to_xy(lat, lon, lat0, lon0):
|
||||
global raio_terra
|
||||
R = raio_terra
|
||||
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
|
||||
|
||||
def calcular_orientacao(p1, p2):
|
||||
dx = p2[0] - p1[0]
|
||||
dy = p2[1] - p1[1]
|
||||
orient = np.arctan2(dy, dx) - np.radians(90)
|
||||
return (orient + np.pi) % (2 * np.pi) - np.pi
|
||||
|
||||
def calcular_omega(v, angulo_rad, tipo):
|
||||
tan_delta = np.tan(angulo_rad)
|
||||
if abs(angulo_rad) < 0.01:
|
||||
return 0.0
|
||||
|
||||
k = 1.35 + 0.5 * np.exp(-abs(np.degrees(angulo_rad)) / 15.0)
|
||||
|
||||
R = distancia_entre_eixos / tan_delta
|
||||
if tipo == TipoMovimentoDirecional.MovimentoArco:
|
||||
R *= 0.5
|
||||
|
||||
R *= k # Aplica fator de correção
|
||||
return v / R
|
||||
|
||||
def proximo_nao_visitado(visitados):
|
||||
for i, v in enumerate(visitados):
|
||||
if not v:
|
||||
return i
|
||||
return len(visitados) - 1
|
||||
|
||||
def corrigir_pontos_visitados(x, y, pontos_visitados, limite_max_avanço=5):
|
||||
for idx, ponto in enumerate(pontos_info):
|
||||
if pontos_visitados[idx]:
|
||||
continue
|
||||
pos = ponto["xy"]
|
||||
margem = ponto.get("distanciaMargem", 0.7)
|
||||
dist = np.linalg.norm([x - pos[0], y - pos[1]])
|
||||
if dist < margem:
|
||||
for i in range(idx + 1):
|
||||
pontos_visitados[i] = True
|
||||
return idx
|
||||
if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanço:
|
||||
break
|
||||
return proximo_nao_visitado(pontos_visitados)
|
||||
|
||||
def calcular_melhor_candidato(x, y, theta, visitados_sim, contexto):
|
||||
idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
|
||||
if idx_alvo >= len(pontos_info):
|
||||
return 0.0, TipoMovimentoDirecional.RodasDianteiras
|
||||
|
||||
ponto_info = pontos_info[idx_alvo]
|
||||
tipo_ponto = TipoPontoRua(ponto_info.get("tipo"))
|
||||
ponto_alvo = ponto_info["xy"]
|
||||
|
||||
if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < ponto_info.get("distanciaMargem", 0.7):
|
||||
visitados_sim[idx_alvo] = True
|
||||
idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
|
||||
if idx_alvo >= len(pontos_info):
|
||||
return 0.0, TipoMovimentoDirecional.RodasDianteiras
|
||||
|
||||
ponto_info = pontos_info[idx_alvo]
|
||||
tipo_ponto = TipoPontoRua(ponto_info.get("tipo"))
|
||||
ponto_alvo = ponto_info["xy"]
|
||||
|
||||
orient = calcular_orientacao((x, y), ponto_alvo)
|
||||
erro_ori = abs((orient - theta + np.pi) % (2 * np.pi) - np.pi)
|
||||
delta_theta = ((-orient) - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
|
||||
|
||||
# Pesos de custo base
|
||||
peso_erro_pos, peso_erro_ori, peso_movimento, tipos_validos = calcular_pesos_movimento(contexto, erro_ori)
|
||||
|
||||
melhor_custo = float('inf')
|
||||
melhor_tipo = TipoMovimentoDirecional.RodasDianteiras
|
||||
melhor_angulo = angulo
|
||||
|
||||
for tipo in tipos_validos:
|
||||
omega = calcular_omega(1.0, angulo, tipo)
|
||||
x_sim = x + 1.0 * np.cos(theta) * 0.2
|
||||
y_sim = y + 1.0 * np.sin(theta) * 0.2
|
||||
theta_sim = theta + omega * 0.2
|
||||
|
||||
erro_pos = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
|
||||
orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
|
||||
erro_ori_sim = abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
|
||||
|
||||
vetor_alvo = np.array(ponto_alvo) - np.array([x_sim, y_sim])
|
||||
vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo)
|
||||
vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
|
||||
cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
|
||||
penalidade_orientacao = 9999 if cos_angulo < 0 else 0
|
||||
|
||||
custo = erro_pos * peso_erro_pos + erro_ori_sim * peso_erro_ori + penalidade_orientacao + peso_movimento[tipo]
|
||||
|
||||
if custo < melhor_custo:
|
||||
melhor_custo = custo
|
||||
melhor_tipo = tipo
|
||||
melhor_angulo = angulo
|
||||
|
||||
return melhor_angulo, melhor_tipo.value
|
||||
|
||||
def calcular_pesos_movimento(contexto, erro_ori):
|
||||
# Lista de tipos sempre permitidos
|
||||
tipos_validos = [
|
||||
TipoMovimentoDirecional.RodasDianteiras,
|
||||
TipoMovimentoDirecional.MovimentoArco
|
||||
]
|
||||
|
||||
peso_erro_pos = 2.0
|
||||
peso_erro_ori = 1.5
|
||||
peso_movimento = {
|
||||
TipoMovimentoDirecional.RodasDianteiras: 0.5,
|
||||
TipoMovimentoDirecional.MovimentoArco: 1.0
|
||||
}
|
||||
|
||||
status = StatusCarroMapa(contexto.get("StatusCarro", 0))
|
||||
dentro = contexto.get("DentroCorredor", False)
|
||||
manobrando = contexto.get("ManobrandoEntreRuas", False)
|
||||
velocidade = contexto.get("Velocidade", velocidade_min)
|
||||
|
||||
# Penalidade proporcional à velocidade
|
||||
penalidade_por_velocidade = velocidade / velocidade_max
|
||||
|
||||
if manobrando or status in [
|
||||
StatusCarroMapa.EntrandoRua,
|
||||
StatusCarroMapa.SaindoRua,
|
||||
StatusCarroMapa.Manobrando
|
||||
]:
|
||||
peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.2
|
||||
peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 2.0 + penalidade_por_velocidade
|
||||
elif dentro:
|
||||
peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.5 + (erro_ori / np.radians(angulo_max_graus)) * 2.0
|
||||
peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 1.0 + penalidade_por_velocidade
|
||||
else:
|
||||
peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.3
|
||||
peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 1.5 + penalidade_por_velocidade
|
||||
|
||||
return peso_erro_pos, peso_erro_ori, peso_movimento, tipos_validos
|
||||
|
||||
def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, contexto):
|
||||
#print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
|
||||
global pontos_info, visitados_execucao, lat0, lon0, raio_terra
|
||||
if not pontos_info:
|
||||
return None
|
||||
|
||||
x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
|
||||
visitados_sim = visitados_execucao.copy()
|
||||
|
||||
sim = [(x, y)]
|
||||
x_temp, y_temp, theta_temp = x, y, theta
|
||||
for _ in range(horizonte):
|
||||
angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim, contexto)
|
||||
omega = calcular_omega(velocidade, angulo_mpc, TipoMovimentoDirecional(tipo_mpc))
|
||||
theta_plot = (-theta_temp) + np.radians(90)
|
||||
x_temp += velocidade * np.cos(theta_plot) * dt
|
||||
y_temp += velocidade * np.sin(theta_plot) * dt
|
||||
theta_temp += omega * dt
|
||||
sim.append((x_temp, y_temp))
|
||||
|
||||
# Converte os pontos simulados para lat/lon
|
||||
simulacao_latlon = []
|
||||
for sx, sy in sim:
|
||||
dlat = sy / raio_terra
|
||||
dlon = sx / (raio_terra * np.cos(np.radians(lat0)))
|
||||
lat = lat0 + np.degrees(dlat)
|
||||
lon = lon0 + np.degrees(dlon)
|
||||
simulacao_latlon.append({"latitude": lat, "longitude": lon})
|
||||
|
||||
#ax.clear()
|
||||
#if pontos_info:
|
||||
# tx, ty = zip(*[p["xy"] for p in pontos_info])
|
||||
# ax.plot(tx, ty, 'r.-', label="Trajetória alvo")
|
||||
#if sim:
|
||||
# sx, sy = zip(*sim)
|
||||
# ax.plot(sx, sy, 'g:', label="Previsão MPC")
|
||||
#ax.plot(sim[0][0], sim[0][1], 'bo', label="Posição atual")
|
||||
#ax.set_title("Visualização MPC (tempo real)")
|
||||
#ax.set_xlabel("X (m)")
|
||||
#ax.set_ylabel("Y (m)")
|
||||
#ax.axis("equal")
|
||||
#ax.legend()
|
||||
#plt.pause(0.001)
|
||||
|
||||
angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao, contexto)
|
||||
#print(f"angulo: {np.degrees(angulo_final)}, tipo: {tipo_final}")
|
||||
return {
|
||||
"angulo": np.degrees(angulo_final),
|
||||
"tipo": tipo_final,
|
||||
"simulacao": simulacao_latlon
|
||||
}
|
||||
|
||||
def on_connect(client, userdata, flags, rc):
|
||||
#print("Conectado ao MQTT")
|
||||
client.subscribe(topico_rota)
|
||||
client.subscribe(topico_posicao)
|
||||
client.publish(topico_comando, "OK")
|
||||
|
||||
def on_message(client, userdata, msg):
|
||||
global trajetoria_latlon, pontos_info, visitados_execucao, lat0, lon0, angulo_max_graus, distancia_entre_eixos, horizonte, velocidade_max, velocidade_min
|
||||
|
||||
if (not msg.payload):
|
||||
return
|
||||
|
||||
try:
|
||||
dados = json.loads(msg.payload.decode())
|
||||
except json.JSONDecodeError as e:
|
||||
print(f"Erro ao decodificar JSON em '{msg.topic}': {e}")
|
||||
return
|
||||
|
||||
if msg.topic == topico_rota:
|
||||
trajetoria_latlon = dados["pontos"]
|
||||
angulo_max_graus = dados["angulo_max_graus"]
|
||||
velocidade_min = dados["velocidade_min"]
|
||||
velocidade_max = dados["velocidade_max"]
|
||||
distancia_entre_eixos = dados["distancia_entre_eixos"]
|
||||
horizonte = dados["horizonte"]
|
||||
lat0, lon0 = trajetoria_latlon[0]["lat"], trajetoria_latlon[0]["lon"]
|
||||
pontos_info = []
|
||||
for p in trajetoria_latlon:
|
||||
x, y = latlon_to_xy(p["lat"], p["lon"], lat0, lon0)
|
||||
ponto = {
|
||||
"xy": (x, y),
|
||||
"tipo": p.get("tipo", 3),
|
||||
"distanciaMargem": p.get("distanciaMargem", 0.7)
|
||||
}
|
||||
pontos_info.append(ponto)
|
||||
visitados_execucao = [False] * len(pontos_info)
|
||||
print(f"Trajetória recebida com: Pontos={len(pontos_info)}, angulo_max={angulo_max_graus}, vel_min={velocidade_min}, vel_max={velocidade_max}, dist_eixos={distancia_entre_eixos}, horizonte={horizonte}")
|
||||
|
||||
elif msg.topic == topico_posicao:
|
||||
lat = dados["lat"]
|
||||
lon = dados["lon"]
|
||||
theta = np.radians(dados["theta"])
|
||||
velocidade = dados["velocidade"]
|
||||
dt = dados["dt"]
|
||||
# Novo: contexto completo (com valores padrão caso não venha)
|
||||
contexto = dados.get("contexto", {})
|
||||
contexto.setdefault("StatusCarro", 0)
|
||||
contexto.setdefault("DentroCorredor", False)
|
||||
contexto.setdefault("ManobrandoEntreRuas", False)
|
||||
contexto.setdefault("Velocidade", velocidade) # já aproveita
|
||||
|
||||
comando = processar_mpc(lat, lon, theta, velocidade, dt, contexto)
|
||||
if comando:
|
||||
client.publish(topico_comando, json.dumps(comando))
|
||||
|
||||
client = mqtt.Client()
|
||||
client.on_connect = on_connect
|
||||
client.on_message = on_message
|
||||
client.connect("localhost", 1883, 60)
|
||||
|
||||
#print("Aguardando trajetória e posição...")
|
||||
client.loop_forever()
|
||||
Binary file not shown.
|
|
@ -482,4 +482,4 @@ C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.GenerateR
|
|||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.CoreCompileInputs.cache
|
||||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.exe
|
||||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.pdb
|
||||
C:\ZendionINC\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.Up2Date
|
||||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.CopyComplete
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,221 @@
|
|||
{
|
||||
"type": "FeatureCollection",
|
||||
"features": [
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "1",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.558011415731592,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.395205344,
|
||||
-22.172559531333334
|
||||
],
|
||||
[
|
||||
-47.395212610166666,
|
||||
-22.172614638833334
|
||||
],
|
||||
[
|
||||
-47.395219157,
|
||||
-22.172656417833334
|
||||
],
|
||||
[
|
||||
-47.395223544833335,
|
||||
-22.1726892105
|
||||
],
|
||||
[
|
||||
-47.39522414098443,
|
||||
-22.17269369161165
|
||||
],
|
||||
[
|
||||
-47.395225326538004,
|
||||
-22.172702654611555
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "2",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.771318274761821,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.39521090233333,
|
||||
-22.172708800833334
|
||||
],
|
||||
[
|
||||
-47.395206943666665,
|
||||
-22.172679811
|
||||
],
|
||||
[
|
||||
-47.395202420666664,
|
||||
-22.1726491015
|
||||
],
|
||||
[
|
||||
-47.395198865666664,
|
||||
-22.172615782833333
|
||||
],
|
||||
[
|
||||
-47.395193255833334,
|
||||
-22.172577132833332
|
||||
],
|
||||
[
|
||||
-47.395192610024395,
|
||||
-22.17257265769541
|
||||
],
|
||||
[
|
||||
-47.395191325698136,
|
||||
-22.172563706509337
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "3",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.827915524386164,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.3951762925,
|
||||
-22.172561603
|
||||
],
|
||||
[
|
||||
-47.395180824166665,
|
||||
-22.172591686166665
|
||||
],
|
||||
[
|
||||
-47.395185745333336,
|
||||
-22.172623080166666
|
||||
],
|
||||
[
|
||||
-47.395190433,
|
||||
-22.172656367166667
|
||||
],
|
||||
[
|
||||
-47.395195199,
|
||||
-22.172693641833334
|
||||
],
|
||||
[
|
||||
-47.39519576904436,
|
||||
-22.172698125891035
|
||||
],
|
||||
[
|
||||
-47.39519690267159,
|
||||
-22.172707094716973
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "4",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.785159385903514,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.39518293216667,
|
||||
-22.1727127985
|
||||
],
|
||||
[
|
||||
-47.39517819266667,
|
||||
-22.172680514833335
|
||||
],
|
||||
[
|
||||
-47.3951732595,
|
||||
-22.172646752833334
|
||||
],
|
||||
[
|
||||
-47.39516880516667,
|
||||
-22.1726149155
|
||||
],
|
||||
[
|
||||
-47.39516381233334,
|
||||
-22.172581167166665
|
||||
],
|
||||
[
|
||||
-47.39516315429932,
|
||||
-22.172576693573966
|
||||
],
|
||||
[
|
||||
-47.39516184565584,
|
||||
-22.172567745443878
|
||||
]
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "Feature",
|
||||
"properties": {
|
||||
"Id": "5",
|
||||
"Name": "CidadeJardimTerreno2",
|
||||
"Length": 14.80117692191233,
|
||||
"Dist1": 14.558011415731592,
|
||||
"Dist2": 0.0
|
||||
},
|
||||
"geometry": {
|
||||
"id": null,
|
||||
"type": "LineString",
|
||||
"coordinates": [
|
||||
[
|
||||
-47.395147317833334,
|
||||
-22.172566031
|
||||
],
|
||||
[
|
||||
-47.395151503166666,
|
||||
-22.172595452333333
|
||||
],
|
||||
[
|
||||
-47.3951561855,
|
||||
-22.172628011166665
|
||||
],
|
||||
[
|
||||
-47.39516159866667,
|
||||
-22.172663757333332
|
||||
],
|
||||
[
|
||||
-47.39516672716667,
|
||||
-22.172697770833334
|
||||
],
|
||||
[
|
||||
-47.395167397572706,
|
||||
-22.172702242832194
|
||||
],
|
||||
[
|
||||
-47.39516873082615,
|
||||
-22.17271118781009
|
||||
]
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1 @@
|
|||
{"pontos":[[-22.172546316494021,-47.395203906184044],[-22.172554486898338,-47.395197380933986],[-22.172561618921335,-47.39519833484907],[-22.172570764605126,-47.39519955809326],[-22.172579910288917,-47.395200781337451],[-22.172589055972708,-47.395202004581641],[-22.172599434069689,-47.395203559929712],[-22.172609812166666,-47.395205115277776],[-22.17261843375,-47.395206363531244],[-22.172627055333333,-47.39520761178472],[-22.172635676916666,-47.395208860038196],[-22.1726442985,-47.395210108291664],[-22.17265283328398,-47.395211099563944],[-22.172661368067963,-47.395212090836232],[-22.172669902851943,-47.395213082108512],[-22.172678700537212,-47.395214361615096],[-22.17268749822248,-47.395215641121681],[-22.172696612972462,-47.395216877778672],[-22.172705727722445,-47.395218114435664],[-22.172712121750248,-47.395221657035862],[-22.172712121750248,-47.395221657035862],[-22.172716653743002,-47.395219218133455],[-22.172719570120194,-47.395216634939345],[-22.172720870881825,-47.395213907453545],[-22.17272055602789,-47.395211035676063],[-22.1727186255584,-47.395208019606891],[-22.172715079473342,-47.395204859246029],[-22.172707947775152,-47.395203902502459],[-22.172698458110332,-47.395202629428987],[-22.172688968445517,-47.395201356355514],[-22.17268017005609,-47.395200083094423],[-22.172671371666667,-47.395198809833332],[-22.1726625475,-47.395197769708332],[-22.172653723333333,-47.395196729583333],[-22.172644899166666,-47.395195689458333],[-22.172636075,-47.395194649333334],[-22.1726288813,-47.395193619583331],[-22.1726216876,-47.395192589833336],[-22.1726144939,-47.395191560083333],[-22.1726073002,-47.395190530333338],[-22.1726001065,-47.395189500583335],[-22.17259113921552,-47.395188108839434],[-22.17258217193104,-47.395186717095527],[-22.172572413342856,-47.3951852630973],[-22.172562654754667,-47.395183809099066],[-22.172555629052788,-47.395185441649048],[-22.172555629052788,-47.395185441649048],[-22.172551986345685,-47.395181541050519],[-22.172549929165552,-47.395178038176475],[-22.172549457512385,-47.395174933026929],[-22.172550571386196,-47.395172225601875],[-22.172553270786981,-47.39516991590132],[-22.172557555714736,-47.395168003925257],[-22.172564674221938,-47.395169069077923],[-22.172574432046126,-47.395170529155457],[-22.172584189870314,-47.395171989232992],[-22.172593156768489,-47.395173384033164],[-22.172602123666664,-47.395174778833336],[-22.17261050308333,-47.395175988895836],[-22.172618882499997,-47.395177198958336],[-22.172627261916666,-47.395178409020836],[-22.172635641333333,-47.395179619083336],[-22.172644280333333,-47.395180771625],[-22.172652919333331,-47.395181924166664],[-22.172661558333331,-47.395183076708335],[-22.172670197333332,-47.39518422925],[-22.172679758847757,-47.39518560505276],[-22.172689320362185,-47.395186980855513],[-22.172699633485337,-47.395188449137322],[-22.172709946608485,-47.395189917419131],[-22.1727163205133,-47.395193502055555],[-22.1727163205133,-47.395193502055555],[-22.172720834453084,-47.395191044309996],[-22.172723729032157,-47.395188461546944],[-22.172725004250527,-47.395185753766377],[-22.172724660108191,-47.395182920968338],[-22.172722696605149,-47.3951799631528],[-22.172719113741397,-47.39517688031976],[-22.172711993155048,-47.39517583149641],[-22.172701685993907,-47.395174313308047],[-22.172691378832766,-47.395172795119692],[-22.17268182033305,-47.395171394226509],[-22.172672261833334,-47.395169993333333],[-22.172664030479169,-47.395168795479165],[-22.172655799125,-47.395167597625004],[-22.172647567770831,-47.395166399770837],[-22.172639336416665,-47.395165201916669],[-22.172630649604166,-47.395163901166669],[-22.172621962791666,-47.395162600416668],[-22.172613275979167,-47.395161299666668],[-22.172604589166667,-47.395159998916668],[-22.172595331060158,-47.39515866382483],[-22.172586072953649,-47.395157328732992],[-22.172576480587793,-47.395155955238792],[-22.172566888221937,-47.395154581744592],[-22.172552639889869,-47.395152541580387]]}
|
||||
|
|
@ -0,0 +1,70 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Parâmetros do modelo
|
||||
dt = 0.2 # intervalo de tempo (s)
|
||||
v = 1.0 # velocidade linear (m/s)
|
||||
horizonte = 50 # número de passos futuros
|
||||
|
||||
# Ponto inicial
|
||||
x, y, theta = 0.0, 0.0, 0.0
|
||||
|
||||
# Trajetória alvo (trajeto desejado)
|
||||
traj = np.array([
|
||||
[0, 0],
|
||||
[2, 0.5],
|
||||
[4, 1.5],
|
||||
[6, 3],
|
||||
[8, 5],
|
||||
[10, 7],
|
||||
[12, 9],
|
||||
[14, 10],
|
||||
[16, 10],
|
||||
[18, 9]
|
||||
])
|
||||
|
||||
def calcular_orientacao(p1, p2):
|
||||
dx = p2[0] - p1[0]
|
||||
dy = p2[1] - p1[1]
|
||||
return np.arctan2(dy, dx)
|
||||
|
||||
# Trajetória gerada
|
||||
traj_gerada = [(x, y)]
|
||||
|
||||
for step in range(horizonte):
|
||||
# Acha ponto alvo mais próximo
|
||||
distancias = np.linalg.norm(traj - np.array([x, y]), axis=1)
|
||||
idx_alvo = np.argmin(distancias)
|
||||
ponto_alvo = traj[min(idx_alvo + 1, len(traj) - 1)]
|
||||
|
||||
# Orientação desejada
|
||||
orient_desejada = calcular_orientacao((x, y), ponto_alvo)
|
||||
delta_theta = orient_desejada - theta
|
||||
|
||||
# Ajuste de ângulo para [-pi, pi]
|
||||
delta_theta = (delta_theta + np.pi) % (2 * np.pi) - np.pi
|
||||
|
||||
# Calcular omega ideal
|
||||
omega = delta_theta / dt
|
||||
omega = np.clip(omega, -np.pi/4, np.pi/4) # limitar a 45°/s
|
||||
|
||||
# Aplicar movimento
|
||||
x += v * np.cos(theta) * dt
|
||||
y += v * np.sin(theta) * dt
|
||||
theta += omega * dt
|
||||
|
||||
traj_gerada.append((x, y))
|
||||
|
||||
# Separar para plot
|
||||
traj_gerada = np.array(traj_gerada)
|
||||
|
||||
# Plotar
|
||||
plt.plot(traj[:,0], traj[:,1], 'ro--', label='Trajetória desejada')
|
||||
plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label='Trajetória MPC (estimativa por passo)')
|
||||
plt.title("MPC passo a passo com orientação estimada")
|
||||
plt.xlabel("X")
|
||||
plt.ylabel("Y")
|
||||
plt.grid(True)
|
||||
plt.legend()
|
||||
plt.axis("equal")
|
||||
plt.show()
|
||||
|
|
@ -0,0 +1,84 @@
|
|||
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()
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
# 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()
|
||||
|
|
@ -0,0 +1,103 @@
|
|||
# Simulação MPC com recalculagem do delta_theta a cada passo (versão mais próxima do real)
|
||||
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 do robô
|
||||
distancia_entre_eixos = 0.92
|
||||
angulo_max_graus = 30.0
|
||||
angulo_max_rad = np.radians(angulo_max_graus)
|
||||
dt = 0.2
|
||||
horizonte = 10
|
||||
|
||||
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
|
||||
|
||||
def simular_trajetoria_recalculando(x0, y0, theta0, velocidade, tipo_movimento, traj_xy):
|
||||
x, y, theta = x0, y0, theta0
|
||||
pontos = [(x, y)]
|
||||
custo_total = 0.0
|
||||
|
||||
for step in range(horizonte):
|
||||
idx = min(step, len(traj_xy)-1)
|
||||
ponto_alvo = traj_xy[idx]
|
||||
erro_dist = np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]])
|
||||
orient_alvo = calcular_orientacao((x, y), ponto_alvo)
|
||||
delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
|
||||
# Limitando o ângulo de direção
|
||||
angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
|
||||
omega = calcular_omega_fisico(velocidade, angulo_direcional, tipo_movimento)
|
||||
|
||||
# Atualização de estado
|
||||
x += velocidade * np.cos(theta) * dt
|
||||
y += velocidade * np.sin(theta) * dt
|
||||
theta += omega * dt
|
||||
|
||||
pontos.append((x, y))
|
||||
erro_orient = np.abs((orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi)
|
||||
custo_total += erro_dist * 2.0 + erro_orient * 1.5
|
||||
|
||||
return np.array(pontos), custo_total
|
||||
|
||||
# Estado inicial
|
||||
x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
|
||||
trajetorias_testadas = []
|
||||
custos = []
|
||||
|
||||
# Candidatos com recalculagem a cada passo
|
||||
for vel in [0.6, 1.0, 1.4]:
|
||||
for tipo in ["rodasFrontais", "movimentoArco"]:
|
||||
traj, custo = simular_trajetoria_recalculando(x0, y0, theta0, vel, tipo, traj_xy)
|
||||
trajetorias_testadas.append((traj, vel, tipo))
|
||||
custos.append(custo)
|
||||
|
||||
# Melhor resultado
|
||||
melhor_idx = int(np.argmin(custos))
|
||||
traj_melhor, vel_melhor, tipo_melhor = trajetorias_testadas[melhor_idx]
|
||||
|
||||
# Plot
|
||||
for traj, vel, tipo in trajetorias_testadas:
|
||||
plt.plot(traj[:,0], traj[:,1], alpha=0.2, label=f"{vel} m/s | {tipo}")
|
||||
|
||||
plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label=f"Melhor: {vel_melhor} m/s | {tipo_melhor}")
|
||||
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
|
||||
plt.xlabel("X (m)")
|
||||
plt.ylabel("Y (m)")
|
||||
plt.title("MPC com recalculagem por passo (delta_theta dinâmico)")
|
||||
plt.legend(loc="best", fontsize="small")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
plt.show()
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
# MPC híbrido com progressão inteligente: avança na trajetória conforme distância e orientação
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import json
|
||||
|
||||
# Carrega o 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
|
||||
distancia_entre_eixos = 0.92
|
||||
angulo_max_graus = 30.0
|
||||
angulo_max_rad = np.radians(angulo_max_graus)
|
||||
dt = 0.2
|
||||
horizonte = 150
|
||||
velocidade_param = 1.0
|
||||
|
||||
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
|
||||
|
||||
def simular_trajetoria_hibrida_com_progresso(x0, y0, theta0, v, traj_xy):
|
||||
x, y, theta = x0, y0, theta0
|
||||
pontos = [(x, y)]
|
||||
custo_total = 0.0
|
||||
tipos_usados = []
|
||||
idx_atual = 0
|
||||
dist_anterior = None
|
||||
|
||||
for step in range(horizonte):
|
||||
if idx_atual >= len(traj_xy) - 1:
|
||||
break
|
||||
|
||||
ponto_alvo = traj_xy[idx_atual]
|
||||
orient_alvo = calcular_orientacao((x, y), ponto_alvo)
|
||||
delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
|
||||
|
||||
melhor_tipo = None
|
||||
melhor_estado = None
|
||||
menor_custo = float('inf')
|
||||
|
||||
for tipo in ["rodasFrontais", "movimentoArco"]:
|
||||
omega = calcular_omega_fisico(v, angulo_direcional, tipo)
|
||||
x_sim = x + v * np.cos(theta) * dt
|
||||
y_sim = y + v * np.sin(theta) * dt
|
||||
theta_sim = theta + omega * dt
|
||||
|
||||
erro_dist = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
|
||||
orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
|
||||
erro_orient = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
|
||||
custo = erro_dist * 2.0 + erro_orient * 1.5
|
||||
|
||||
if custo < menor_custo:
|
||||
menor_custo = custo
|
||||
melhor_tipo = tipo
|
||||
melhor_estado = (x_sim, y_sim, theta_sim)
|
||||
|
||||
x, y, theta = melhor_estado
|
||||
pontos.append((x, y))
|
||||
tipos_usados.append(melhor_tipo)
|
||||
custo_total += menor_custo
|
||||
|
||||
# Lógica de progressão do índice
|
||||
dist_atual = np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]])
|
||||
if dist_anterior is not None:
|
||||
orient_diff = np.abs((orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi)
|
||||
if (dist_atual > dist_anterior and orient_diff < np.pi / 2) or dist_atual < 0.6:
|
||||
idx_atual += 1
|
||||
dist_anterior = None
|
||||
continue
|
||||
dist_anterior = dist_atual
|
||||
|
||||
return np.array(pontos), custo_total, tipos_usados
|
||||
|
||||
# Simular com progressão
|
||||
x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
|
||||
traj_melhor, custo_final, tipos_melhor = simular_trajetoria_hibrida_com_progresso(x0, y0, theta0, velocidade_param, traj_xy)
|
||||
|
||||
# Plot
|
||||
plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label='MPC híbrido com progressão')
|
||||
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
|
||||
plt.xlabel("X (m)")
|
||||
plt.ylabel("Y (m)")
|
||||
plt.title(f"MPC híbrido com progressão dinâmica (vel {velocidade_param} m/s)")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
# MPC híbrido com controle de pontos visitados via limiar de distância
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import json
|
||||
|
||||
# Carrega o 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
|
||||
distancia_entre_eixos = 0.92
|
||||
angulo_max_graus = 30.0
|
||||
angulo_max_rad = np.radians(angulo_max_graus)
|
||||
dt = 0.2
|
||||
horizonte = 250
|
||||
velocidade_param = 1.0
|
||||
limiar_entrada = 0.7 # distância para considerar ponto como visitado
|
||||
|
||||
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
|
||||
|
||||
def proximo_nao_visitado(visitados):
|
||||
for i, v in enumerate(visitados):
|
||||
if not v:
|
||||
return i
|
||||
return len(visitados) - 1 # fallback para o último ponto
|
||||
|
||||
def simular_trajetoria_com_limiar(x0, y0, theta0, v, traj_xy):
|
||||
x, y, theta = x0, y0, theta0
|
||||
pontos = [(x, y)]
|
||||
custo_total = 0.0
|
||||
tipos_usados = []
|
||||
visitados = [False] * len(traj_xy)
|
||||
|
||||
for step in range(horizonte):
|
||||
idx_alvo = proximo_nao_visitado(visitados)
|
||||
if idx_alvo >= len(traj_xy):
|
||||
break
|
||||
|
||||
ponto_alvo = traj_xy[idx_alvo]
|
||||
pos_atual = np.array([x, y])
|
||||
|
||||
# Verifica se o ponto foi alcançado
|
||||
if np.linalg.norm(pos_atual - ponto_alvo) < limiar_entrada:
|
||||
visitados[idx_alvo] = True
|
||||
idx_alvo = proximo_nao_visitado(visitados)
|
||||
if idx_alvo >= len(traj_xy):
|
||||
break
|
||||
ponto_alvo = traj_xy[idx_alvo]
|
||||
|
||||
orient_alvo = calcular_orientacao(pos_atual, ponto_alvo)
|
||||
delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
|
||||
|
||||
melhor_tipo = None
|
||||
melhor_estado = None
|
||||
menor_custo = float('inf')
|
||||
|
||||
for tipo in ["rodasFrontais", "movimentoArco"]:
|
||||
omega = calcular_omega_fisico(v, angulo_direcional, tipo)
|
||||
x_sim = x + v * np.cos(theta) * dt
|
||||
y_sim = y + v * np.sin(theta) * dt
|
||||
theta_sim = theta + omega * dt
|
||||
|
||||
erro_dist = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
|
||||
orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
|
||||
erro_orient = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
|
||||
custo = erro_dist * 2.0 + erro_orient * 1.5
|
||||
|
||||
if custo < menor_custo:
|
||||
menor_custo = custo
|
||||
melhor_tipo = tipo
|
||||
melhor_estado = (x_sim, y_sim, theta_sim)
|
||||
|
||||
x, y, theta = melhor_estado
|
||||
pontos.append((x, y))
|
||||
tipos_usados.append(melhor_tipo)
|
||||
custo_total += menor_custo
|
||||
|
||||
return np.array(pontos), custo_total, tipos_usados
|
||||
|
||||
# Simular com lógica de ponto visitado
|
||||
x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
|
||||
traj_melhor, custo_final, tipos_melhor = simular_trajetoria_com_limiar(x0, y0, theta0, velocidade_param, traj_xy)
|
||||
|
||||
# Plot
|
||||
plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label='MPC com visitação')
|
||||
plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
|
||||
plt.xlabel("X (m)")
|
||||
plt.ylabel("Y (m)")
|
||||
plt.title(f"MPC com visitação de pontos (vel {velocidade_param} m/s)")
|
||||
plt.grid(True)
|
||||
plt.axis("equal")
|
||||
plt.legend()
|
||||
plt.show()
|
||||
|
|
@ -0,0 +1,242 @@
|
|||
# MPC com MQTT, simulação e visualização da trajetória
|
||||
import numpy as np
|
||||
import paho.mqtt.client as mqtt
|
||||
import json
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Parâmetros do MPC
|
||||
angulo_max_graus = 30.0
|
||||
distancia_entre_eixos = 0.92
|
||||
limiar_entrada = 0.7
|
||||
horizonte = 20
|
||||
|
||||
# Estado compartilhado
|
||||
trajetoria_latlon = []
|
||||
trajetoria_xy = []
|
||||
visitados_execucao = []
|
||||
lat0, lon0 = None, None
|
||||
|
||||
# Visualização
|
||||
plt.ion()
|
||||
fig, ax = plt.subplots()
|
||||
|
||||
# Funções matemáticas auxiliares
|
||||
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
|
||||
|
||||
def normalizar_angulo(angulo):
|
||||
if angulo < 0:
|
||||
angulo += 360
|
||||
angulo %= 360
|
||||
return angulo
|
||||
|
||||
def calcular_orientacao(p1, p2):
|
||||
dx = p2[0] - p1[0]
|
||||
dy = p2[1] - p1[1]
|
||||
orient = np.arctan2(dy, dx)
|
||||
orient -= np.radians(90) # Corrige a base angular para bater com o seu theta ajustado
|
||||
return (orient + np.pi) % (2 * np.pi) - np.pi # Normaliza entre [-pi, pi]
|
||||
|
||||
def calcular_orientacao_gps(p1_lat, p1_lon, p2_lat, p2_lon):
|
||||
lat1 = np.radians(p1_lat)
|
||||
lon1 = np.radians(p1_lon)
|
||||
lat2 = np.radians(p2_lat)
|
||||
lon2 = np.radians(p2_lon)
|
||||
|
||||
delta_lon = lon2 - lon1
|
||||
|
||||
y = np.sin(delta_lon) * np.cos(lat2)
|
||||
x = np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(lat2) * np.cos(delta_lon)
|
||||
|
||||
theta = np.arctan2(y, x)
|
||||
bearing = (np.degrees(theta) + 360) % 360
|
||||
|
||||
return bearing
|
||||
|
||||
def calcular_omega(v, angulo_rad, tipo):
|
||||
tan_delta = np.tan(angulo_rad)
|
||||
if tipo == "rodasFrontais":
|
||||
return (v * tan_delta) / distancia_entre_eixos
|
||||
elif tipo == "movimentoArco":
|
||||
return (2 * v * tan_delta) / distancia_entre_eixos
|
||||
return 0.0
|
||||
|
||||
def corrigir_pontos_visitados(x, y, limite_max_avanço=5):
|
||||
global visitados_execucao
|
||||
|
||||
for idx, ponto in enumerate(trajetoria_xy):
|
||||
if visitados_execucao[idx]:
|
||||
continue
|
||||
|
||||
dist = np.linalg.norm([x - ponto[0], y - ponto[1]])
|
||||
|
||||
# Dentro do limiar: marca esse e os anteriores como visitados
|
||||
if dist < limiar_entrada:
|
||||
for i in range(idx + 1):
|
||||
visitados_execucao[i] = True
|
||||
return idx
|
||||
|
||||
# Se já passou do limite de avanço, para
|
||||
if idx > 0 and not visitados_execucao[idx - 1] and idx >= limite_max_avanço:
|
||||
break
|
||||
|
||||
# Se não marcou nenhum ponto, volta o próximo não visitado
|
||||
return proximo_nao_visitado(visitados_execucao)
|
||||
|
||||
def proximo_nao_visitado(visitados):
|
||||
for i, v in enumerate(visitados):
|
||||
if not v:
|
||||
return i
|
||||
return len(visitados) - 1
|
||||
|
||||
def calcular_melhor_candidato(x, y, theta, visitados_sim):
|
||||
idx_alvo = corrigir_pontos_visitados(x, y)
|
||||
if idx_alvo >= len(trajetoria_xy):
|
||||
return 0.0, "rodasFrontais"
|
||||
|
||||
ponto_alvo = trajetoria_xy[idx_alvo]
|
||||
|
||||
# Verifica se já alcançou o ponto
|
||||
if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < limiar_entrada:
|
||||
visitados_sim[idx_alvo] = True
|
||||
idx_alvo = corrigir_pontos_visitados(x, y)
|
||||
if idx_alvo >= len(trajetoria_xy):
|
||||
return 0.0, "rodasFrontais"
|
||||
ponto_alvo = trajetoria_xy[idx_alvo]
|
||||
|
||||
orient = calcular_orientacao((x, y), ponto_alvo)
|
||||
erro_ori = abs((orient - theta + np.pi) % (2 * np.pi) - np.pi)
|
||||
#delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
delta_theta = ((-orient) - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
|
||||
|
||||
# ✅ Se o erro for grande, prioriza curvas fechadas
|
||||
if erro_ori > np.radians(10):
|
||||
tipos_validos = ["movimentoArco"]
|
||||
else:
|
||||
tipos_validos = ["rodasFrontais", "movimentoArco"]
|
||||
|
||||
melhor_custo = float('inf')
|
||||
melhor_tipo = "rodasFrontais"
|
||||
melhor_angulo = angulo
|
||||
|
||||
for tipo in tipos_validos:
|
||||
omega = calcular_omega(1.0, angulo, tipo)
|
||||
x_sim = x + 1.0 * np.cos(theta) * 0.2
|
||||
y_sim = y + 1.0 * np.sin(theta) * 0.2
|
||||
theta_sim = theta + omega * 0.2
|
||||
|
||||
erro_pos = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
|
||||
orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
|
||||
erro_ori = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
|
||||
|
||||
vetor_alvo = np.array(ponto_alvo) - np.array([x_sim, y_sim])
|
||||
vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo)
|
||||
vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
|
||||
cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
|
||||
|
||||
# Se o ângulo for > 90° → cosseno < 0 → indo no sentido oposto
|
||||
penalidade_orientacao = 9999 if cos_angulo < 0 else 0
|
||||
|
||||
custo = erro_pos * 2.0 + erro_ori * 1.5 + penalidade_orientacao
|
||||
|
||||
if custo < melhor_custo:
|
||||
melhor_custo = custo
|
||||
melhor_tipo = tipo
|
||||
melhor_angulo = angulo
|
||||
|
||||
return melhor_angulo, melhor_tipo
|
||||
|
||||
def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, angulo_controle_atual):
|
||||
print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
|
||||
global trajetoria_xy, visitados_execucao, lat0, lon0
|
||||
if not trajetoria_xy:
|
||||
return None
|
||||
|
||||
x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
|
||||
|
||||
visitados_sim = visitados_execucao.copy()
|
||||
sim = [(x, y)]
|
||||
x_temp, y_temp, theta_temp = x, y, theta
|
||||
for _ in range(horizonte):
|
||||
angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim)
|
||||
omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
|
||||
|
||||
# Corrige o ângulo apenas para visualização
|
||||
theta_plot = theta_temp + np.radians(90)
|
||||
|
||||
# Avança a simulação com o ângulo corrigido para o traço
|
||||
x_temp += velocidade * np.cos(theta_plot) * dt
|
||||
y_temp += velocidade * np.sin(theta_plot) * dt
|
||||
theta_temp += omega * dt
|
||||
|
||||
sim.append((x_temp, y_temp))
|
||||
|
||||
# Atualiza visualização
|
||||
ax.clear()
|
||||
if trajetoria_xy:
|
||||
tx, ty = zip(*trajetoria_xy)
|
||||
ax.plot(tx, ty, 'r.-', label="Trajetória alvo")
|
||||
if sim:
|
||||
sx, sy = zip(*sim)
|
||||
ax.plot(sx, sy, 'g:', label="Previsão MPC")
|
||||
ax.plot(sim[0][0], sim[0][1], 'bo', label="Posição atual")
|
||||
ax.set_title("Visualização MPC (tempo real)")
|
||||
ax.set_xlabel("X (m)")
|
||||
ax.set_ylabel("Y (m)")
|
||||
ax.axis("equal")
|
||||
ax.legend()
|
||||
plt.pause(0.001)
|
||||
|
||||
angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao)
|
||||
#angulo_final = angulo_final * -1
|
||||
print(f"angulo: {np.degrees(angulo_final)}, tipo: {tipo_final}")
|
||||
return {
|
||||
"angulo": np.degrees(angulo_final),
|
||||
"tipo": 0 if tipo_final == "rodasFrontais" else 3,
|
||||
"velocidade": velocidade,
|
||||
"dt": dt,
|
||||
"orientacao": np.degrees(theta)
|
||||
}
|
||||
|
||||
# MQTT setup
|
||||
def on_connect(client, userdata, flags, rc):
|
||||
print("Conectado ao MQTT")
|
||||
client.subscribe("mpc/rota")
|
||||
client.subscribe("mpc/posicao")
|
||||
|
||||
def on_message(client, userdata, msg):
|
||||
global trajetoria_latlon, trajetoria_xy, visitados_execucao, lat0, lon0
|
||||
if msg.topic == "mpc/rota":
|
||||
dados = json.loads(msg.payload.decode())
|
||||
trajetoria_latlon = dados["pontos"]
|
||||
lat0, lon0 = trajetoria_latlon[0][0], trajetoria_latlon[0][1]
|
||||
trajetoria_xy = [latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in trajetoria_latlon]
|
||||
visitados_execucao = [False] * len(trajetoria_xy)
|
||||
print(f"Trajetória recebida com {len(trajetoria_xy)} pontos")
|
||||
|
||||
elif msg.topic == "mpc/posicao":
|
||||
dados = json.loads(msg.payload.decode())
|
||||
lat = dados["lat"]
|
||||
lon = dados["lon"]
|
||||
#angulo = normalizar_angulo(dados["theta"] - 90)
|
||||
theta = np.radians(dados["theta"])
|
||||
velocidade = dados["velocidade"]
|
||||
dt = dados["dt"]
|
||||
angulo_controle = dados.get("anguloControle", 0.0)
|
||||
comando = processar_mpc(lat, lon, theta, velocidade, dt, angulo_controle)
|
||||
if comando:
|
||||
client.publish("mpc/comando", json.dumps(comando))
|
||||
|
||||
client = mqtt.Client()
|
||||
client.on_connect = on_connect
|
||||
client.on_message = on_message
|
||||
client.connect("localhost", 1883, 60)
|
||||
|
||||
print("Aguardando trajetória e posição...")
|
||||
client.loop_forever()
|
||||
|
|
@ -0,0 +1,215 @@
|
|||
# MPC com MQTT, simulação e visualização da trajetória
|
||||
import numpy as np
|
||||
import paho.mqtt.client as mqtt
|
||||
import json
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Parâmetros do MPC
|
||||
angulo_max_graus = 30.0
|
||||
distancia_entre_eixos = 0.92
|
||||
horizonte = 20
|
||||
|
||||
# Estado compartilhado
|
||||
trajetoria_latlon = []
|
||||
pontos_info = []
|
||||
visitados_execucao = []
|
||||
lat0, lon0 = None, None
|
||||
|
||||
# Visualização
|
||||
plt.ion()
|
||||
fig, ax = plt.subplots()
|
||||
|
||||
# Funções matemáticas auxiliares
|
||||
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
|
||||
|
||||
def calcular_orientacao(p1, p2):
|
||||
dx = p2[0] - p1[0]
|
||||
dy = p2[1] - p1[1]
|
||||
orient = np.arctan2(dy, dx) - np.radians(90)
|
||||
return (orient + np.pi) % (2 * np.pi) - np.pi
|
||||
|
||||
def calcular_omega(v, angulo_rad, tipo):
|
||||
tan_delta = np.tan(angulo_rad)
|
||||
if tipo == "rodasFrontais":
|
||||
return (v * tan_delta) / distancia_entre_eixos
|
||||
elif tipo == "movimentoArco":
|
||||
return (2 * v * tan_delta) / distancia_entre_eixos
|
||||
return 0.0
|
||||
|
||||
def proximo_nao_visitado(visitados):
|
||||
for i, v in enumerate(visitados):
|
||||
if not v:
|
||||
return i
|
||||
return len(visitados) - 1
|
||||
|
||||
def corrigir_pontos_visitados(x, y, pontos_visitados, limite_max_avanço=5):
|
||||
for idx, ponto in enumerate(pontos_info):
|
||||
if pontos_visitados[idx]:
|
||||
continue
|
||||
pos = ponto["xy"]
|
||||
margem = ponto.get("distanciaMargem", 0.7)
|
||||
dist = np.linalg.norm([x - pos[0], y - pos[1]])
|
||||
if dist < margem:
|
||||
for i in range(idx + 1):
|
||||
pontos_visitados[i] = True
|
||||
return idx
|
||||
if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanço:
|
||||
break
|
||||
return proximo_nao_visitado(pontos_visitados)
|
||||
|
||||
def calcular_melhor_candidato(x, y, theta, visitados_sim):
|
||||
idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
|
||||
if idx_alvo >= len(pontos_info):
|
||||
return 0.0, "movimentoArco"
|
||||
|
||||
ponto_info = pontos_info[idx_alvo]
|
||||
tipo_ponto = ponto_info.get("tipo", 3)
|
||||
ponto_alvo = ponto_info["xy"]
|
||||
|
||||
if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < ponto_info.get("distanciaMargem", 0.7):
|
||||
visitados_sim[idx_alvo] = True
|
||||
idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
|
||||
if idx_alvo >= len(pontos_info):
|
||||
return 0.0, "movimentoArco"
|
||||
ponto_info = pontos_info[idx_alvo]
|
||||
tipo_ponto = ponto_info.get("tipo", 3)
|
||||
ponto_alvo = ponto_info["xy"]
|
||||
|
||||
orient = calcular_orientacao((x, y), ponto_alvo)
|
||||
erro_ori = abs((orient - theta + np.pi) % (2 * np.pi) - np.pi)
|
||||
delta_theta = ((-orient) - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
|
||||
|
||||
if erro_ori > np.radians(10):
|
||||
tipos_validos = ["movimentoArco"]
|
||||
elif tipo_ponto == 3:
|
||||
tipos_validos = ["rodasFrontais", "movimentoArco"]
|
||||
else:
|
||||
tipos_validos = ["movimentoArco"]
|
||||
|
||||
melhor_custo = float('inf')
|
||||
melhor_tipo = "movimentoArco"
|
||||
melhor_angulo = angulo
|
||||
|
||||
for tipo in tipos_validos:
|
||||
omega = calcular_omega(1.0, angulo, tipo)
|
||||
x_sim = x + 1.0 * np.cos(theta) * 0.2
|
||||
y_sim = y + 1.0 * np.sin(theta) * 0.2
|
||||
theta_sim = theta + omega * 0.2
|
||||
|
||||
erro_pos = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
|
||||
orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
|
||||
erro_ori = abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
|
||||
|
||||
vetor_alvo = np.array(ponto_alvo) - np.array([x_sim, y_sim])
|
||||
vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo)
|
||||
vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
|
||||
cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
|
||||
penalidade_orientacao = 9999 if cos_angulo < 0 else 0
|
||||
|
||||
custo = erro_pos * 2.0 + erro_ori * 1.5 + penalidade_orientacao
|
||||
|
||||
if custo < melhor_custo:
|
||||
melhor_custo = custo
|
||||
melhor_tipo = tipo
|
||||
melhor_angulo = angulo
|
||||
|
||||
return melhor_angulo, melhor_tipo
|
||||
|
||||
def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, angulo_controle_atual):
|
||||
print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
|
||||
global pontos_info, visitados_execucao, lat0, lon0
|
||||
if not pontos_info:
|
||||
return None
|
||||
|
||||
x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
|
||||
visitados_sim = visitados_execucao.copy()
|
||||
|
||||
sim = [(x, y)]
|
||||
x_temp, y_temp, theta_temp = x, y, theta
|
||||
for _ in range(horizonte):
|
||||
angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim)
|
||||
omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
|
||||
theta_plot = (-theta_temp) + np.radians(90)
|
||||
x_temp += velocidade * np.cos(theta_plot) * dt
|
||||
y_temp += velocidade * np.sin(theta_plot) * dt
|
||||
theta_temp += omega * dt
|
||||
sim.append((x_temp, y_temp))
|
||||
|
||||
ax.clear()
|
||||
if pontos_info:
|
||||
tx, ty = zip(*[p["xy"] for p in pontos_info])
|
||||
ax.plot(tx, ty, 'r.-', label="Trajetória alvo")
|
||||
if sim:
|
||||
sx, sy = zip(*sim)
|
||||
ax.plot(sx, sy, 'g:', label="Previsão MPC")
|
||||
ax.plot(sim[0][0], sim[0][1], 'bo', label="Posição atual")
|
||||
ax.set_title("Visualização MPC (tempo real)")
|
||||
ax.set_xlabel("X (m)")
|
||||
ax.set_ylabel("Y (m)")
|
||||
ax.axis("equal")
|
||||
ax.legend()
|
||||
plt.pause(0.001)
|
||||
|
||||
angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao)
|
||||
print(f"angulo: {np.degrees(angulo_final)}, tipo: {tipo_final}")
|
||||
return {
|
||||
"angulo": np.degrees(angulo_final),
|
||||
"tipo": 0 if tipo_final == "rodasFrontais" else 3,
|
||||
"velocidade": velocidade,
|
||||
"dt": dt,
|
||||
"orientacao": np.degrees(theta)
|
||||
}
|
||||
|
||||
def on_connect(client, userdata, flags, rc):
|
||||
print("Conectado ao MQTT")
|
||||
client.subscribe("mpc/rota")
|
||||
client.subscribe("mpc/posicao")
|
||||
client.publish("mpc/comando", "OK")
|
||||
|
||||
def on_message(client, userdata, msg):
|
||||
global trajetoria_latlon, pontos_info, visitados_execucao, lat0, lon0, angulo_max_graus, distancia_entre_eixos, horizonte
|
||||
if msg.topic == "mpc/rota":
|
||||
dados = json.loads(msg.payload.decode())
|
||||
trajetoria_latlon = dados["pontos"]
|
||||
angulo_max_graus = dados["angulo_max_graus"]
|
||||
distancia_entre_eixos = dados["distancia_entre_eixos"]
|
||||
horizonte = dados["horizonte"]
|
||||
lat0, lon0 = trajetoria_latlon[0]["lat"], trajetoria_latlon[0]["lon"]
|
||||
pontos_info = []
|
||||
for p in trajetoria_latlon:
|
||||
x, y = latlon_to_xy(p["lat"], p["lon"], lat0, lon0)
|
||||
ponto = {
|
||||
"xy": (x, y),
|
||||
"tipo": p.get("tipo", 3),
|
||||
"distanciaMargem": p.get("distanciaMargem", 0.7)
|
||||
}
|
||||
pontos_info.append(ponto)
|
||||
visitados_execucao = [False] * len(pontos_info)
|
||||
print(f"Trajetória recebida com {len(pontos_info)} pontos")
|
||||
|
||||
elif msg.topic == "mpc/posicao":
|
||||
dados = json.loads(msg.payload.decode())
|
||||
lat = dados["lat"]
|
||||
lon = dados["lon"]
|
||||
theta = np.radians(dados["theta"])
|
||||
velocidade = dados["velocidade"]
|
||||
dt = dados["dt"]
|
||||
angulo_controle = dados.get("anguloControle", 0.0)
|
||||
comando = processar_mpc(lat, lon, theta, velocidade, dt, angulo_controle)
|
||||
if comando:
|
||||
client.publish("mpc/comando", json.dumps(comando))
|
||||
|
||||
client = mqtt.Client()
|
||||
client.on_connect = on_connect
|
||||
client.on_message = on_message
|
||||
client.connect("localhost", 1883, 60)
|
||||
|
||||
print("Aguardando trajetória e posição...")
|
||||
client.loop_forever()
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
# Simulador interativo de MPC com separação de visitados (execução vs simulação)
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import json
|
||||
|
||||
# Carrega o mapa a partir de pontos.json
|
||||
with open("pontos.json", "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
|
||||
traj_gps = data["pontos"]
|
||||
|
||||
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
|
||||
|
||||
def calcular_orientacao(p1, p2):
|
||||
dx = p2[0] - p1[0]
|
||||
dy = p2[1] - p1[1]
|
||||
return np.arctan2(dy, dx)
|
||||
|
||||
def calcular_omega(v, angulo_rad, tipo):
|
||||
tan_delta = np.tan(angulo_rad)
|
||||
if tipo == "rodasFrontais":
|
||||
return (v * tan_delta) / distancia_entre_eixos
|
||||
elif tipo == "movimentoArco":
|
||||
return (2 * v * tan_delta) / distancia_entre_eixos
|
||||
return 0.0
|
||||
|
||||
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
|
||||
distancia_entre_eixos = 0.92
|
||||
angulo_max_graus = 30.0
|
||||
angulo_max_rad = np.radians(angulo_max_graus)
|
||||
dt = 0.2
|
||||
velocidade = 1.0
|
||||
limiar_entrada = 0.7
|
||||
horizonte = 20
|
||||
|
||||
# Estado atual do robô
|
||||
x, y = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5
|
||||
theta = 0.0
|
||||
trajetoria_percorrida = [(x, y)]
|
||||
visitados_execucao = [False] * len(traj_xy)
|
||||
|
||||
def proximo_nao_visitado(visitados):
|
||||
for i, v in enumerate(visitados):
|
||||
if not v:
|
||||
return i
|
||||
return len(visitados) - 1
|
||||
|
||||
def calcular_melhor_candidato(x, y, theta, visitados):
|
||||
visitados_sim = visitados.copy()
|
||||
idx_alvo = proximo_nao_visitado(visitados_sim)
|
||||
if idx_alvo >= len(traj_xy):
|
||||
return 0.0, "rodasFrontais"
|
||||
|
||||
ponto_alvo = traj_xy[idx_alvo]
|
||||
if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < limiar_entrada:
|
||||
visitados_sim[idx_alvo] = True
|
||||
idx_alvo = proximo_nao_visitado(visitados_sim)
|
||||
if idx_alvo >= len(traj_xy):
|
||||
return 0.0, "rodasFrontais"
|
||||
ponto_alvo = traj_xy[idx_alvo]
|
||||
|
||||
orient = calcular_orientacao((x, y), ponto_alvo)
|
||||
delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi
|
||||
angulo = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
|
||||
|
||||
melhor_custo = float('inf')
|
||||
melhor_tipo = "rodasFrontais"
|
||||
melhor_angulo = angulo
|
||||
|
||||
for tipo in ["rodasFrontais", "movimentoArco"]:
|
||||
omega = calcular_omega(velocidade, angulo, tipo)
|
||||
x_sim = x + velocidade * np.cos(theta) * dt
|
||||
y_sim = y + velocidade * np.sin(theta) * dt
|
||||
theta_sim = theta + omega * dt
|
||||
erro = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
|
||||
orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
|
||||
erro_ori = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
|
||||
custo = erro * 2.0 + erro_ori * 1.5
|
||||
|
||||
if custo < melhor_custo:
|
||||
melhor_custo = custo
|
||||
melhor_tipo = tipo
|
||||
melhor_angulo = angulo
|
||||
|
||||
return melhor_angulo, melhor_tipo
|
||||
|
||||
def simular_dinamico(x, y, theta, visitados):
|
||||
sim = [(x, y)]
|
||||
visitados_sim = visitados.copy()
|
||||
for _ in range(horizonte):
|
||||
angulo_mpc, tipo_mpc = calcular_melhor_candidato(x, y, theta, visitados_sim)
|
||||
omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
|
||||
x += velocidade * np.cos(theta) * dt
|
||||
y += velocidade * np.sin(theta) * dt
|
||||
theta += omega * dt
|
||||
sim.append((x, y))
|
||||
return np.array(sim)
|
||||
|
||||
fig, ax = plt.subplots()
|
||||
plt.ion()
|
||||
|
||||
while True:
|
||||
angulo_mpc, tipo_mpc = calcular_melhor_candidato(x, y, theta, visitados_execucao)
|
||||
caminho = simular_dinamico(x, y, theta, visitados_execucao)
|
||||
|
||||
ax.clear()
|
||||
ax.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
|
||||
ax.plot(*zip(*trajetoria_percorrida), 'b-', label='Percurso real')
|
||||
ax.plot(caminho[:,0], caminho[:,1], 'g:', label='Previsão MPC')
|
||||
ax.set_title("MPC passo a passo (dinâmico por passo)")
|
||||
ax.set_xlabel("X (m)")
|
||||
ax.set_ylabel("Y (m)")
|
||||
ax.axis("equal")
|
||||
ax.legend()
|
||||
plt.pause(0.01)
|
||||
|
||||
print(f"\n>>> Comando MPC: tipo = {tipo_mpc}, ângulo = {np.degrees(angulo_mpc):.2f}°")
|
||||
input("Pressione Enter para o próximo passo...")
|
||||
|
||||
idx_real = proximo_nao_visitado(visitados_execucao)
|
||||
if idx_real < len(traj_xy):
|
||||
ponto_real = traj_xy[idx_real]
|
||||
if np.linalg.norm([x - ponto_real[0], y - ponto_real[1]]) < limiar_entrada:
|
||||
visitados_execucao[idx_real] = True
|
||||
|
||||
omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
|
||||
x += velocidade * np.cos(theta) * dt
|
||||
y += velocidade * np.sin(theta) * dt
|
||||
theta += omega * dt
|
||||
trajetoria_percorrida.append((x, y))
|
||||
|
||||
if proximo_nao_visitado(visitados_execucao) >= len(traj_xy):
|
||||
print("Trajetória completa!")
|
||||
break
|
||||
|
||||
plt.ioff()
|
||||
plt.show()
|
||||
Loading…
Reference in New Issue