O capítulo anterior estudou principalmente o processamento antes do planejamento do caminho local no algoritmo teb, incluindo o processamento do mapa local, a pose inicial, a velocidade atual do robô e como extrair o caminho local do caminho global. Este capítulo continua a examinar o processamento da parte de movimento do planejamento de caminho local no algoritmo teb e a ver como o algoritmo calcula uma trajetória adequada sob as condições anteriores conhecidas.
A entrada da função principal para o cálculo da velocidade do algoritmo teb é:
bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel);
Esta função está na classe TebOptimalPlanner, é usada principalmente para realizar as seguintes funções:
1, initTrajectoryToGoal
Esta função é executada apenas uma vez durante a inicialização, sua principal função é fixar o ponto inicial e final, e calcular o tempo ideal entre cada dois pontos: distância/velocidade máxima. Além disso, a direção do próximo ponto também é processada e a direção de cada ponto também é suavizada.
2、updateAndPruneTEB
Esta função é utilizada para limpar os pontos que foram passados no caminho, sua ideia é encontrar um ponto mais próximo da posição atual a partir do ponto inicial do planejamento do caminho local. Se o ponto mais próximo for o primeiro ponto, todos os pontos serão percorridos. Se for um dos pontos, a distância diminuirá gradualmente ao percorrer do primeiro ponto até este ponto, e um valor menor será registrado a cada vez. , quebrando sai do loop quando o valor começa a ser maior que o valor anterior:
double dist_cache = (new_start->position()- Pose(0).position()).norm();
double dist;
int lookahead = std::min<int>( sizePoses()-min_samples, 10); // satisfy min_samples, otherwise max 10 samples
int nearest_idx = 0;
for (int i = 1; i<=lookahead; ++i)
{
dist = (new_start->position()- Pose(i).position()).norm();
if (dist<dist_cache)
{
dist_cache = dist;
nearest_idx = i;
}
else break;
}
Então, de acordo com o ID do ponto registrado, o ponto anterior no contêiner é limpo. Em seguida, registre o ponto final no BackPose
if (new_goal && sizePoses()>0)
{
BackPose() = *new_goal;
}
3. Defina a velocidade inicial
if (start_vel)
setVelocityStart(*start_vel);
Determine a velocidade inicial atual do algoritmo teb de acordo com a velocidade atual retornada por odom.
4. Defina se deseja permitir que o robô alcance o ponto final em uma velocidade diferente de zero
if (free_goal_vel)
setVelocityGoalFree();
else
vel_goal_.first = true;
Isso é determinado pelo parâmetro cfg_.goal_tolerance.free_goal_vel
5、TebOptimalPlanner::optimizeTEB
optimizeTEB realiza a otimização final, e suas principais funções incluem:
5.1, redimensionar caminho
Essa função é determinada pelo parâmetro trajetória.teb_autosize, que é usado para otimizar o caminho novamente antes da otimização. A otimização aqui é excluir e adicionar pontos de caminho, que são afetados por cinco parâmetros:
cfg_->trajectory.dt_ref
cfg_->trajectory.dt_hysteresis
cfg_->trajectory.min_samples
cfg_->trajectory.max_samples
cfg_->obstacles.include_dynamic_obstacles
dt_ref é usado em conjunto com dt_hysteresis. Lembre-se de que calculamos o tempo de movimento entre dois pontos. Aqui, podemos determinar se devemos adicionar ou excluir alguns pontos com base no tempo. Quando o tempo entre dois pontos exceder dt_ref+dt_hysteresis, os pontos serão adicionado; quando Se o tempo entre dois pontos for menor que dt_ref-dt_hysteresis, o ponto será excluído;
min_samples e max_samples são dois limites e o número total de pontos não pode exceder o intervalo entre os dois.
include_dynamic_obstacles é usado para determinar se deve sair após atualizar apenas um ponto ou sair quando as condições acima não forem atendidas.
5.2. Diagrama de construção
O algoritmo principal do algoritmo teb é realizado pelo algoritmo g2o, portanto, alguns parâmetros necessários para a otimização do gráfico precisam ser construídos aqui. A construção do gráfico inclui as seguintes partes: 1) Defina as propriedades do otimizador e decida se deseja registrar informações intermediárias e
informações estatísticas
optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable);
2), adicione vértices
// add TEB vertices
// 添加位姿态和时间间隔顶点
AddTEBVertices();
A borda de Teb contém dois aspectos: atitude e tempo. Anteriormente, calculamos e adicionamos ou excluímos alguns pontos, modificamos a orientação dos pontos e calculamos o tempo de movimento entre as poses. Ambos serão adicionados aqui como informações de vértice otimizadas para g2o.
for (int i=0; i<teb_.sizePoses(); ++i)
{
teb_.PoseVertex(i)->setId(id_counter++);
optimizer_->addVertex(teb_.PoseVertex(i));
if (teb_.sizeTimeDiffs()!=0 && i<teb_.sizeTimeDiffs())
{
teb_.TimeDiffVertex(i)->setId(id_counter++);
optimizer_->addVertex(teb_.TimeDiffVertex(i));
}
iter_obstacle->clear();
(iter_obstacle++)->reserve(obstacles_->size());
}
3) Adicionar restrições
Depois de adicionar vértices, o algoritmo continua a adicionar arestas exigidas pelas restrições. A borda aqui consiste em várias partes:
A primeira coisa a acrescentar é a borda do obstáculo,
// 添加障碍物边
if (cfg_->obstacles.legacy_obstacle_association)
AddEdgesObstaclesLegacy(weight_multiplier);
else
AddEdgesObstacles(weight_multiplier);
Existem duas maneiras de adicionar aqui, e qual delas usar é determinada pelo parâmetro obstáculos.legacy_obstacle_association. Tomando AddEdgesObstacles como exemplo, a função primeiro determina se a expansão do obstáculo é realizada e, em seguida, define uma função para criar arestas. Depois disso, atravesse cada ponto de coordenada, encontre o obstáculo cuja distância do ponto de coordenada é menor que o limite, e o obstáculo mais próximo à esquerda e à direita, e construa um objeto EdgeObstacle como a borda do obstáculo do gráfico. O cálculo do erro de borda é a distância do ponto de coordenada ao obstáculo e, em seguida, uma função de ativação.
Em seguida, adicione bordas de obstáculos dinâmicas:
//添加动态障碍物边
if (cfg_->obstacles.include_dynamic_obstacles)
AddEdgesDynamicObstacles();
É basicamente semelhante a adicionar obstáculos, exceto que os obstáculos mudam com o tempo.
Em seguida, adicione arestas passando pelos pontos de referência:
//添加经过路径点边
AddEdgesViaPoints();
A função AddEdgesViaPoints primeiro percorre cada ponto do caminho, calcula o ponto de coordenada mais próximo do ponto do caminho atual e constrói a aresta do ponto do caminho, cujo tipo é EdgeViaPoint, e o cálculo do erro da aresta é a distância euclidiana.
O próximo passo é adicionar arestas com restrição de velocidade e aceleração:
//添加速度边
AddEdgesVelocity();
//添加加速度边
AddEdgesAcceleration();
O objetivo de adicionar restrições de velocidade e aceleração é evitar que a velocidade linear e a velocidade angular excedam um determinado limite.
O próximo passo é adicionar arestas de restrição de tempo:
//添加时间约束边
AddEdgesTimeOptimal();
arestas otimizadas para o tempo, usadas para otimizar o tempo
e uma aresta de caminho mais curto:
//添加最短路径边
AddEdgesShortestPath();
O objetivo é otimizar o comprimento da trajetória
Finalmente, há uma restrição cinemática:
//添加运动学约束边
if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0)
AddEdgesKinematicsDiffDrive(); // we have a differential drive robot
else
AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.
//添加旋转约束边
AddEdgesPreferRotDir();
A função AddEdgesKinematicsDiffDrive é usada para adicionar arestas limite dinâmicas, o objetivo é fazer com que a trajetória gerada satisfaça as restrições cinemáticas. AddEdgesPreferRotDir é usado para fazer o robô se inclinar para a esquerda ou para a direita durante a prevenção de obstáculos.
Até agora, foram adicionados os relacionamentos de vértice e restrição necessários para a otimização do grafo.
5.3. Gráfico de otimização
Construímos um gráfico anteriormente, aqui está para otimizar o gráfico:
//优化图
success = optimizeGraph(iterations_innerloop, false);
if (!success)
{
clearGraph();
return false;
}
optimized_ = true;
O código em si é relativamente simples, o conteúdo principal contém três linhas de código:
optimizer_->setVerbose(cfg_->optim.optimization_verbose);
optimizer_->initializeOptimization();
int iter = optimizer_->optimize(no_iterations);
As duas primeiras linhas são para definir as propriedades do otimizador e inicializar o otimizador, e a terceira linha é para chamar o otimizador para iterações no_iterations.
5.4, calcule o custo
Esta etapa é avaliada na última conclusão do loop externo:
//在最后一次循环时计算当前cost
if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration
computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost);
custo consiste em duas partes:
A primeira parte é o custo do tempo:
if (alternative_time_cost)
{
cost_ += teb_.getSumOfAllTimeDiffs();
// TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs,
// since we are using an AutoResize Function with hysteresis.
}
O segundo é o custo do obstáculo:
for (std::vector<g2o::OptimizableGraph::Edge*>::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++)
{
double cur_cost = (*it)->chi2();
if (dynamic_cast<EdgeObstacle*>(*it) != nullptr
|| dynamic_cast<EdgeInflatedObstacle*>(*it) != nullptr
|| dynamic_cast<EdgeDynamicObstacle*>(*it) != nullptr)
{
cur_cost *= obst_cost_scale;
}
else if (dynamic_cast<EdgeViaPoint*>(*it) != nullptr)
{
cur_cost *= viapoint_cost_scale;
}
else if (dynamic_cast<EdgeTimeOptimal*>(*it) != nullptr && alternative_time_cost)
{
continue; // skip these edges if alternative_time_cost is active
}
cost_ += cur_cost;
}
Finalmente, o algoritmo retorna um custo total atual.
Até agora, o processo de toda a função do plano está concluído, mas aqui está apenas a otimização da trajetória Como converter essa trajetória na velocidade exigida pelo robô? Veja esta parte em detalhes no próximo capítulo.
referência:
https://blog.csdn.net/flztiii/article/details/121545662