Este é o segundo artigo da equipe setUP sobre nossa experiência na criação de robôs autônomos para competições Eurobot Open e no uso de ROS para isso.
O primeiro artigo é sobre a mecânica e a arquitetura geral dos robôs.Os robôs viajam em um campo plano e a maioria dos obstáculos é conhecida com antecedência, no entanto, oponentes insidiosos podem tentar roubar nossos recursos (e às vezes queremos comer algumas dúzias de pontos extras), enquanto queremos chegar ao ponto desejado o mais rápido possível e não tocar nos obstáculos. De uma câmera externa em campo, obtemos dados sobre a posição do inimigo e sabemos onde ele está agora. No entanto, não basta conhecer sua posição - você precisa poder usar essas informações.
Hoje vamos tentar dirigir do ponto A ao ponto B sem ter viajado ao longo da cauda de um gato que adormeceu no campo. Em particular, explicaremos como construímos uma rota e controlamos a velocidade do robô, além de como iniciar tudo em nosso computador.
Tentando sobreviver com um pouco de sangue
Ao resolver esse problema, você pode pegar material pronto, elaborado por profissionais, para que não haja tormento e invenção da próxima “bicicleta”. Usamos uma plataforma omni-wheeled, portanto, embora tenha havido uma tentativa de usar uma plaina local pronta da ROS, por várias razões, eles acharam isso pouco promissor. Abaixo, você pode ver quantas coisas o designer de um planador padrão precisa:
class TrajectoryPlanner{ friend class TrajectoryPlannerTest;
Este é um exemplo de inicialização de parâmetros para regulação de velocidades e trajetória como um todo.Parâmetros importantes para um cálculo completo:
- Parâmetro world_model.
- Parâmetro do mapa de custo: um link para um mapa contendo obstáculos, bem como sua "extensão virtual", levando em consideração a possível colisão.
Das vantagens da pilha padrão, é possível destacar a disponibilidade da documentação e a capacidade de encontrar informações nos fóruns.
Você pode ler mais no site oficial com documentaçãoÉ importante mencionar que os pacotes ROS foram escritos para plataformas de duas rodas e, no Omni, eles foram otimizados aumentando o ângulo de rotação disponível ao subir até 360 graus, o que certamente é uma muleta.
Depois de analisar o projeto, percebemos que haveria uma dificuldade em estudar e suplementar, além de haver muitos chips pesados dos quais não precisamos. Parece, deixe-os estar, mas usamos o Odroid xu4 (o processador do qual ainda estava na Samsung s5), e os resultados de desempenho foram deprimentes e o espaço para algo mais poderoso (e o processador raspberry 4 e o jetson nano fumam nervosamente à parte, em comparação com com ele) não. Eu tive que abandonar a pilha padrão e tentar criar o planejador global, planejador local e regulador de trajetória
Plaina global, planejador local, regulador de trajetória e tudo
Planadores globais e locais são necessários para obter direções para o destino. Por que a separação é necessária? Por que você não pode simplesmente pegar o
A * e montá-lo? Como regra, o planejador global, ao construir uma rota, pode usar o mapa inteiro em seu trabalho, de modo que o algoritmo deve ser o mais rápido possível, talvez com algumas simplificações. Para suavizar essas simplificações, eles também usam o planejador local, que, com base no resultado do planejador global (ou apenas em uma área limitada ao redor do robô), tenta levar em consideração todas as nuances.
Depois que construímos a rota, sabemos para onde o robô deve ir, mas como ele pode ser informado sobre isso? Para isso, existe um regulador de trajetória. Ele calcula em que velocidade e em que direção o robô deve se mover no momento para não se desviar da trajetória. De várias maneiras, este pacote é responsável pela rapidez e beleza do robô.
Além dessas três entidades, há um quarto - um servidor de mapas, que permite processar convenientemente o estado do mundo. Ele define como descrevemos o mapa, que possibilidades temos ao trabalhar com o mapa e, de várias maneiras, determinamos a velocidade dos planadores.
Antes de prosseguir com a descrição da pilha de navegação, seria bom descrever as razões pelas quais cost_map foi escolhido como servidor de mapas. Em geral, tentamos opções diferentes para o manipulador de mapas:
Occupancy_grid ,
Grid_map ,
Cost_map , mas resolvemos o último.
Razões:
- Interaja convenientemente com o mapa.
- Existem vários iteradores que precisamos de várias formas (circular, linear, retangular etc.).
- Você pode armazenar várias camadas de mapa com diferentes parâmetros.
- Bom gerenciamento de memória.
- E o mais importante, velocidade. O mapa de grade funciona com tipo duplo e, por isso, é várias vezes mais lento que os servidores de mapa que usam o int8 para funcionar.
Apesar do fato de a grade de ocupação também funcionar com int8, ela não pode se gabar da mesma usabilidade, então tive que abandoná-la.
No mapa, precisamos saber onde estão as zonas livres, perigosas e irresistíveis. Para cada objeto que está no campo, podemos ajustar o campo de inflação - um valor que, dependendo da distância do objeto, caracteriza a permeabilidade da célula. A inflação é a cauda do gato, é fácil não notar, mas você vai se arrepender por muito tempo. Mapeamos robôs inimigos e adicionamos uma zona de perigo que somente o planejador local leva em consideração. Planejador global ignora todos os pontos, se eles não são um obstáculo.
Planejador global
A primeira coisa que escreveram na navegação é o planejador global. É baseado no algoritmo theta *. Em resumo, este é um
A * modificado, onde a ênfase está em encontrar o nó pai, que pode ser alcançado diretamente, ou seja, não há obstáculos para ela. Isso nos permite criar caminhos convenientes e suaves que são usados no planejador local.
Comparação de A * e teta *Para o planejador global, temos um arquivo com parâmetros (params / path_planner.yaml) que descreve tópicos e tópicos do mapa com a localização de todos os robôs (para todos os quatro robôs em campo, onde "nulo" é o tópico com dados sobre o robô atual).
# small robot debug param list robot_id: "small" ########################### # cost_map_server params ## ########################### cost_map_server/big_robot_size: 0.45 cost_map_server/little_robot_size: 0.45 cost_map_server/cube_size: 0.11 cost_map_server/inscribed_radius: 0.3 cost_map_server/inflation_radius: 0.3 cost_map_server/inflation_exponential_rate: 0.6 cost_map_server/big_robot1: "/aruco/big_robot" cost_map_server/big_robot2: "/aruco/enemy_robot1" cost_map_server/small_robot1: "null" cost_map_server/small_robot2: "/aruco/enemy_robot2" cost_map_server/collision: "collision" cost_map_server/image_resource_name: "$(find cost_map_server)/param/image_resource.yaml" cost_map_server/min_diff_points: 0.01 ########################### ### path_planner params ### ########################### global_planner/path_layer: "inflation_layer" global_planner/path_force_layer: "obstacle_inflation_layer" global_planner/frame_id: "map" global_planner/current_position: "real_corr" global_planner/path_filter_epsilon: 0
Também indica:
- um dos algoritmos que você pode escolher para construir uma rota,
- nomes de camadas nas quais vamos construir a própria rota,
- um tópico sobre a nossa posição, onde os dados filtrados são emitidos (no nosso caso, é uma combinação de dados de localização da câmera e odometria).
O próprio algoritmo de busca de caminho - Theta Star - é destacado em um arquivo separado (src / global_planner.cpp) para a conveniência de adicionar novos algoritmos:
float cost_so_far[300][200]; PriorityPoint neighbors[8]; PriorityPoint current; int come_from[300][200][2]; double makePath_ThetaStar(std::vector<cost_map::Index> &path, PriorityPoint start, PriorityPoint goal, std::string layer, cost_map::CostMap &cost_map, double grid_cost, bool only_cost) { std::priority_queue<PriorityPoint, std::vector<PriorityPoint>, myCompare> openSet; size_t max_x = cost_map.getSize()[0]; size_t max_y = cost_map.getSize()[1]; std::fill_n(*cost_so_far, max_x * max_y, std::numeric_limits<float>::max()); cost_so_far[start.x][start.y] = 0; come_from[start.x][start.y][0] = start.x; come_from[start.x][start.y][1] = start.y; openSet.push(start); grid_cost=5; while (!openSet.empty()) { current = openSet.top(); openSet.pop(); if (current == goal) { break; } current.GetNeighbors(neighbors); float current_cost = cost_so_far[current.x][current.y]; int parent_carent[2] ={come_from[current.x][current.y][0], come_from[current.x][current.y][1]}; for (int i = 0; i < 8; i++) { if (!neighbors[i].OnMap(max_x, max_y)) { continue; } bool onLine = lineOfSight(parent_carent[0], parent_carent[1], neighbors[i].x, neighbors[i].y, cost_map, layer, grid_cost + 1); if (onLine) { float new_cost = cost_so_far[parent_carent[0]][parent_carent[1]] + HeuristicEvclid(parent_carent, neighbors[i], grid_cost*10); if (new_cost < cost_so_far[neighbors[i].x][neighbors[i].y]) { cost_so_far[neighbors[i].x][neighbors[i].y] = new_cost; neighbors[i].priority = HeuristicEvclid(neighbors[i], goal, grid_cost*10) + new_cost; openSet.push(neighbors[i]); come_from[neighbors[i].x][neighbors[i].y][0] = parent_carent[0]; come_from[neighbors[i].x][neighbors[i].y][1] = parent_carent[1]; } } else { float neighbor_price = cost_map.at(layer, cost_map::Index({neighbors[i].x, neighbors[i].y})) + neighbors[i].priority; float new_cost = current_cost + neighbor_price; if (new_cost < cost_so_far[neighbors[i].x][neighbors[i].y]) { cost_so_far[neighbors[i].x][neighbors[i].y] = new_cost; neighbors[i].priority =HeuristicEvclid(neighbors[i], goal, grid_cost*10) + new_cost; openSet.push(neighbors[i]); come_from[neighbors[i].x][neighbors[i].y][0] = current.x; come_from[neighbors[i].x][neighbors[i].y][1] = current.y; } } } } if (only_cost) { return cost_so_far[current.x][current.y]; } path.clear(); PriorityPoint temp_point; while (current != start) { path.push_back({current.x, current.y}); temp_point.x = come_from[current.x][current.y][0]; temp_point.y = come_from[current.x][current.y][1]; current = temp_point; } path.push_back({current.x, current.y}); return 0; }
Um algoritmo para remover pontos extras no caminho RamerDouglasPeucker também é alocado para um arquivo separado.
Ele remove pontos do caminho se estiver além de uma determinada distância da linha que conecta dois pontos vizinhos.
Planejador local
Ele trabalha para nós com base na descida do gradiente em um campo potencial. Como entrada, o caminho do planejador global. No entanto, isso não é tudo o que ele é capaz. No local_planner, existem serviços internos para escolher o modo de construção do caminho. Existem dois modos de operação no total: o modo de pontos de deslocamento ao longo do gradiente, usando várias passagens no mapa, bem como o modo de deslocamento, no qual calculamos imediatamente o incremento em duas coordenadas e movemos o ponto para a borda da zona segura. Se o ponto cair em várias dessas zonas, passamos para os locais de seu cruzamento, porque é mais seguro.
O modo de operação é o seguinte: se não houver obstáculo no caminho da iteração anterior, quebramos o caminho a cada 2 cm e o deslocamos ao longo do gradiente, caso contrário, usamos o segundo modo de operação do planejador local.
A segunda opção é bastante leve e, como o planejador global, não carrega muito o processador. Usamos várias versões desse algoritmo e várias manipulações com o mapa. Por exemplo, tentamos escrever um gráfico no qual os vértices são encontrados a cada 10 cm e são deslocados no máximo em 4 cm, após o qual o algoritmo de Dijkstra foi usado no gráfico obtido para encontrar a menor distância. O terminal neste caso usa o ponto de deslocamento mais próximo. Mas esse algoritmo era mais adequado para o planejador global e decidimos abandonar essa implementação.
Também tentamos usar a construção de caminhos do zero usando o método de descida de gradiente. Este método foi o primeiro que decidimos escrever. Acabou não sendo eficiente em memória (ocupava mais de 400 mb de RAM pura, pois usava um mapa de custos a cada passagem) e lento. O controle de frequência foi desativado devido à baixa otimização e a velocidade foi inferior a 30 vezes por segundo, o que não nos convinha.
Como resultado, decidimos usar a descida do gradiente em um campo potencial com base no caminho da plaina global. Acabou sendo um algoritmo leve e relativamente simples, que nos convém completamente em termos de qualidade do caminho, tempo de trabalho e quantidade de RAM usada na região de 100-150 mb, o que é várias vezes menor do que nas primeiras iterações de desenvolvimento.
Um exemplo de deslocamento de caminho por uma plaina localDiferentemente do global_planner, os parâmetros no local_planner são extremamente pequenos, devido à sua simplicidade, porque todas as tarefas mais importantes estão no global_planner:
grid_map_server/big_robot1 : 0.4 grid_map_server/big_robot2 : 0.4 grid_map_server/small_robot1 : 0.4 grid_map_server/small_robot2 : 0.4 local_planner/radius : 0.15 global_planner/frame_id : "map"
Nesse caso, configuramos:
- Raios de zonas seguras para cada um dos robôs.
- Deslocamento máximo do caminho pela plaina local.
- O nome da camada de mapa com a qual estamos trabalhando.
Em uma classe separada, todas as funções mais importantes foram alocadas. Nesse caso, é uma repartição recursiva do caminho, a criação de um planador e construtores e destruidores virtuais.
class Planner { public: Planner(double inflation_radius_, std::string frame_id_) { inflation_radius = inflation_radius_; for (int i = 0; i < 300; i++) { for(int j = 0; j < 200; j++) our_map[i][j] = 3000.0; } OurPath->poses.resize(50); geometry_msgs::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; for( int i = 0; i < 50; i++) { OurPath->poses[i].pose = pose; } frame_id = frame_id_; } virtual ~Planner() { } virtual void UpdatePath() = 0; ros::Publisher move_pub; ros::Publisher BigEnemyPub; ros::Publisher LittleEnemyPub; ros::Publisher local_path_publisher; protected:
A classe LocalPlanning foi herdada dela, onde está localizado todo o núcleo do planador, ou seja, deslocando pontos para a borda da zona segura e decidindo o que fazer com o caminho especificamente.
Todas as outras funções são destacadas em um arquivo separado fichi.hpp e a descida gradiente em potencial_field.hpp. Abaixo está uma captura instantânea deste arquivo, que mostra as funções para criar um campo potencial no mapa cost_map:
double CalcAttractivePotential(grid_map::Index index, double goalx, double goaly) { return sqrt(pow(index[0] - goalx, 2) + pow(index[1] - goaly, 2)); } void CalcPotential(double startx, double starty, double goalx, double goaly, cost_map::CostMap &cost_map, double (&our_map)[300][200]) {
Regulador de trajectória
Por último, mas não menos importante, é o regulador de trajetória. Ele é responsável por converter o caminho do planejador local em um caminho e fornece a velocidade da etapa atual.
Sua primeira versão, que usamos na final do Eurobot 2018, é uma mistura do controlador pid de aceleração e frenagem, onde o vetor é normalizado para o próximo ponto no caminho, em relação à distância até o ponto final.
O controlador PID, em resumo, é a soma dos três estados do sistema que ajudam a corrigir erros aleatórios e do sistema que às vezes ocorrem.
Essas funções foram selecionadas empiricamente e dependem da distância até o ponto final no caminho (pode ser quadrática, cúbica, inversa, mas depois estabelecemos a quadrática). Isso funcionou, mas a única coisa que não nos convinha era que o robô não podia travar no tempo a velocidades acima de 0,7 metros por segundo. Portanto, quando chegou a hora, decidimos reconstruir todo o algoritmo.
A primeira iteração no caminho para a nova trajetória foi a substituição do vetor para o qual estávamos indo. Agora era a soma dos vetores para os próximos três com diferentes coeficientes. A segunda iteração estava escrevendo Minimum Jerk. Em resumo, trata-se de uma construção de um polinômio do 5º grau, em que as coordenadas x e y dependem da hora de chegada a cada ponto.
A figura mostra um gráfico de uma das coordenadas versus o tempo, bem como a velocidade ao longo dessa coordenadaEsse tipo de regulador de trajetória nos convinha mais, pois exigia menos manipulação com a seleção de diferentes coeficientes, porque todos os coeficientes são os valores no polinômio, que foram calculados com base no tempo de chegada, velocidade e aceleração atuais, velocidade de saída e aceleração.
O resultado da reescrita da trajetória foi que conseguimos dobrar a velocidade média do robô.
Como nos dois casos anteriores, todas as principais funções são destacadas em um arquivo separado para facilitar a interação. Desta vez, a classe PlannerTrajectory é responsável por construir uma trajetória baseada no MinimumJerk
typedef struct State { double velocity; double acceleration; State(double v_, double a_) : velocity(v_), acceleration(a_) {} State() { velocity = 0; acceleration = 0; } }; class PlannerTrajectory { private: nav_msgs::Path global_path; cost_map::CostMap *costmap_ptr; geometry_msgs::PoseStamped Goal_pred; std::vector<double> trajectory_x; std::vector<double> trajectory_y; double average_velocity = 80;
A foto mostra todas as variáveis declaradas que usamos.Todo o resto é destacado em outro arquivo (include / trajectory_regulator.h): recebendo pontos de tópicos, decidindo se deve ir para o próximo ponto (se estiver em um obstáculo, não estamos indo) e muito mais.
Migrando para o ROS Melodic
Até o ano passado, usamos o lançamento do ROS - ROS Kinetic. Ele geralmente nos convinha, mas seu apoio está chegando ao fim no próximo ano, e muitos dos pacotes que precisamos começaram a sair exclusivamente para o ROS Melodic. E então descobriu-se que o costmap_server que usamos não está sob Melodic.
Ocorreu um problema ao processar os dados dos cartões.
Escolhemos o mapa da grade, pois a pilha é semelhante, mas o início do mapa está em um local diferente e os valores do próprio mapa variam de 0 a 1. Isso se tornou um grande problema em toda a pilha de navegação. Se anteriormente, o planejador global era lançado 50 vezes por segundo (havia limitações de frequência e, portanto, o processador não era muito usado, mesmo na metade de um segmento), agora ele preparava o caminho a cada dois segundos e o considerava ruim: carregava completamente um núcleo. Em 2 segundos, o robô poderia atravessar o mapa inteiro. Isso não nos convinha, e as tentativas de paralelizar esse processo terminaram em falha, porque não havia desempenho para outros projetos (levando em consideração os custos da paralelização).
Decidimos mudar a pilha novamente, abandonar o mapa da grade em favor da grade de ocupação. Um novo problema apareceu - a incapacidade de armazenar várias versões de mapas ao mesmo tempo (por exemplo, um mapa completo, com todos os obstáculos e um mapa estático, com apenas obstáculos dinâmicos). Eu teria que mudar metade do código, o que não era particularmente confiável. Portanto, decidimos procurar soluções alternativas para esse problema.
Servidor de Mapa de Custo
Após uma longa pesquisa, encontramos os mapas de garfos costmap_serverr:
https://github.com/lelongg/cost_map.git - extremamente úteis para o nosso sistema de garfos.
E agora, em vez de atender apenas o mapa da grade, para entrega, conseguimos calcular a provável localização do inimigo com base na previsão do filtro Kalman.
Uma das coisas mais importantes para um servidor de mapa é um arquivo de mapa, usado para a criação inicial de todas as camadas, que são atualizadas apenas posteriormente. É uma imagem png binária, onde o preto é um obstáculo e o branco é uma zona livre.
Há também um arquivo de configurações para a configuração cost_map_server. Ele contém tópicos com pontos inimigos, uma zona de inflação e o tamanho do quadrado, que também pode ser usado para colocar a zona perigosa do inimigo no mapa.
########################### # cost_map_server params ## ########################### cost_map_server/big_robot_size: 0.4 cost_map_server/little_robot_size: 0.4 cost_map_server/cube_size: 0.11 cost_map_server/inscribed_radius: 0.25 cost_map_server/inflation_radius: 0.25 cost_map_server/inflation_exponential_rate: 0.6 cost_map_server/big_robot1: "/aruco/robot1" cost_map_server/big_robot2: "/aruco/robot2" cost_map_server/small_robot1: "/aruco/robot3" cost_map_server/small_robot2: "/aruco/robot4" cost_map_server/collision: "collision" cost_map_server/image_resource_name: "$(find cost_map_server)/param/image_resource.yaml" cost_map_server/min_diff_points: 0.01
Todas as camadas são publicadas apenas se alguém as assinar:
void PublishChanges() { nav_msgs::OccupancyGrid msg; cost_map_msgs::CostMap cost_map_msg; if (obstacle_inflation_publisher.getNumSubscribers() > 0) { cost_map::toOccupancyGrid(costmap, "obstacle_layer", msg); obstacle_inflation_publisher.publish(msg); } if (inflation_publisher.getNumSubscribers() > 0) { cost_map::toOccupancyGrid(costmap, "inflation_layer", msg); inflation_publisher.publish(msg); } if (cost_map_publisher.getNumSubscribers() > 0) { cost_map::toMessage(costmap, cost_map_msg); cost_map_publisher.publish(cost_map_msg); } }
Execute no seu computador
Para iniciar a pilha inteira, você deve:
- Coloque ROS
- roslaunch cost_map_server cost_map_server_alone.launch - para iniciar o mapa
- roslaunch global_planner global_planner.launch - inicia a plaina global com parâmetros
- rosparam load $ (encontre local_planner) /param/param.yaml
- rosrun local_planner local_planning
- trajetória_regulador rosrun trajetória_regulador
- rosrun global_planner mover
- rosrun rviz
- adicionar inflação_ camada
- Agora, enviando uma mensagem para o tópico / gp / goal, enviamos o robô para o ponto desejado
Como resultado do lançamento de todos os itens, você terá uma simulação pronta para iniciar nossa pilha no seu computador. Garfo necessárioInicialmente, precisávamos de navegação que ajudasse o nosso robô a dirigir de maneira bonita, rápida e precisa em rodas omni. Durante a preparação para a competição, nenhum gato foi ferido e o robô é bonito. Como resultado, temos uma pilha de navegação leve para concursos semelhantes ao eurobot, com os quais estamos bastante satisfeitos.Para nós, essa pilha é melhor que a padrão, mas ...Nosso Telegrama: t.me/SetUpSberO repositório de toda a nossa criatividade