O MIRO é uma plataforma de robô indoor aberta. Parte 5 - Componente de software: ARDUINO (AVR), subimos "sob o capô"

Desta vez, vamos examinar um pouco mais a fundo a implementação de alguns métodos principais da biblioteca para o ARDUINO (AVR), responsáveis ​​por mover o robô MIRO. Esta parte será interessante para todos que se perguntaram como controlar a velocidade linear e angular do robô no ARDUINO, equipado com motores com os codificadores mais simples.

Índice: Parte 1 , Parte 2 , Parte 3 , Parte 4 , Parte 5 .

Os métodos responsáveis ​​pela condução com odometria ainda são um problema para explicar como, o que e por quê. A primeira coisa que você precisa saber sobre como controlar o movimento do robô é o fato simples e óbvio de que os motores coletores do robô nunca giram na mesma velocidade sem ajustes adicionais. Embreagem diferente, características de saída diferentes dos canais do motorista, motores elétricos ligeiramente diferentes e lubrificação na caixa de câmbio.

O segundo fato que você deve entender e conhecer é a presença de inércia no motor, mesmo com uma relação de transmissão suficientemente grande. I.e. ao remover a tensão dos terminais do motor, a roda, mesmo sem carga, faz um movimento mais alguns graus. A magnitude dessa rotação adicional depende da força de carga na roda, da velocidade de rotação antes de aliviar o estresse e dos mesmos fatores invisíveis, como o tipo e a quantidade de lubrificante na caixa de engrenagens.

Esses fatos determinam a implementação de um grupo de métodos relacionados ao movimento de um chassi equipado com sensores odométricos (no caso do MIRO, codificadores digitais de cada roda).

Como descobrimos na quarta parte, no modelo de software existe a classe Chassis , que implementa o controle de rotação de motores de chassi individuais. Quero enfatizar - não o controle do movimento do chassi, do carrinho, mas o controle dos motores do carrinho. O controle direto do carrinho é implementado nas classes Robot e Miro .

Vamos começar de cima. Abaixo está um método da classe Miro que implementa o movimento de um robô a uma certa distância (distância, metros) com uma determinada velocidade linear ( velocidade da linha , m / s) e velocidade angular ( velocidade da ang , graus / s). Ainda não estamos prestando atenção no parâmetro en_break .

int Miro::moveDist(float lin_speed, float ang_speed, float dist, bool en_break) { float _wheelSetAngSpeed[WHEEL_COUNT]; _wheelSetAngSpeed[LEFT] = MIRO_PI2ANG * (lin_speed - (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; _wheelSetAngSpeed[RIGHT] = MIRO_PI2ANG * (lin_speed + (ROBOT_DIAMETER * ang_speed / (2 * MIRO_PI2ANG))) / WHEEL_RADIUS; float _wheelSetAng[WHEEL_COUNT]; _wheelSetAng[RIGHT] = _wheelSetAngSpeed[RIGHT] * dist / lin_speed; _wheelSetAng[LEFT] = _wheelSetAngSpeed[LEFT] * dist / lin_speed; return this->chassis.wheelRotateAng(_wheelSetAngSpeed, _wheelSetAng, en_break); } 

Neste método, as velocidades angulares NECESSÁRIAS para os motores esquerdo e direito são calculadas primeiro. De acordo com fórmulas bastante óbvias, o que não é um problema para deduzir. Só é necessário ter em mente que a velocidade linear no método é especificada em metros por segundo e a velocidade angular em graus por segundo (não em radianos). Portanto, pré-calculamos a constante MIRO_PI2ANG = 57.29 = 180 / pi. ROBOT_DIAMETER - distância entre as rodas esquerda e direita do robô (em metros), WHEEL_RADIUS - raio da roda (também em metros). Todas as constantes numéricas para esses casos estão contidas no arquivo defs.h, e os parâmetros personalizados do robô e do chassi estão no arquivo config.h.

Depois disso, o ângulo é calculado pelo qual cada roda deve ser girada para que o robô percorra a distância dist (também em metros).

Portanto, nesta fase, obtemos a que velocidade e em que ângulo você precisa girar cada roda do chassi do robô. E então o método wheelRotateAng () do objeto chassi é chamado.

O método wheelRotateAng (float * speed, float * ang, bool en_break) é usado para girar as rodas do robô com as velocidades angulares especificadas pelo array speed [] (em m / s) pelos ângulos especificados pelo array ang [] (em graus). O último parâmetro en_break (já encontrado por nós anteriormente) define o requisito para uma parada forçada das rodas depois de fazer uma curva aplicando tensão reversa de curto prazo a elas. Isso é necessário para suprimir a inércia do robô, impedindo que ele se mova além da distância necessária já após remover a tensão de controle dos motores. Para satisfação completa, é claro, existe o método wheelRotateAngRad () , semelhante ao wheelRotateAng (), com a diferença de considerar os valores dos ângulos de rotação e das velocidades angulares em radianos e radianos por segundo como parâmetros.

O algoritmo do método wheelRotateAng () é o seguinte.

1. Primeiro, a correspondência dos valores de velocidade [] e ang [] para algumas condições de contorno é verificada. Obviamente, o chassi tem limitações físicas tanto na velocidade angular máxima de rotação das rodas quanto na mínima (velocidade mínima de afastamento). Além disso, os ângulos em ang [] não podem ser menores que o ângulo fixo mínimo de rotação, determinado pela precisão dos codificadores.

2. Em seguida, o sentido de rotação de cada roda é calculado. Obviamente, através do sinal do produto ang [i] * velocidade [i] ;

3. A “distância de rotação” Dw [i] para cada roda é calculada - o número de amostras do codificador que devem ser feitas para girar pelo ângulo [i] fornecido.
Este valor é determinado pela fórmula:

Dw [i] = ang [i] * WHEEL_SEGMENTS / 360 ,
onde WHEEL_SEGMENTS é o número de segmentos da roda do codificador (rotação completa).

4. O valor da tensão no acionador do motor é registrado.


5. De acordo com a tabela de calibração de cada roda, é determinado o valor inicial do sinal PWM, o que garante a rotação da roda com a velocidade necessária [i] . Que tipo de tabela de calibração e de onde ela veio - analisaremos mais adiante.

6. A rotação dos motores é iniciada de acordo com os valores calculados de velocidade e direção de rotação. No texto da implementação da classe, o método privado _wheel_rotate_sync () é responsável por isso.

Nós vamos ainda mais fundo. O método _wheel_rotate_sync () funciona de acordo com o seguinte algoritmo:

1. Em um loop infinito, é feita uma verificação para atingir o contador das respostas do encoder da distância de giro Dw [i] para cada roda. Se QUALQUER um dos contadores Dw [i] for alcançado, todas as rodas param e saem do ciclo e depois saem da função (passo 5). Isso é feito pelos seguintes motivos. Devido à discrição de medir o ângulo de rotação, é uma situação muito comum quando a distância calculada Dw [i] de uma roda é obtida arredondando um valor não inteiro para um lado menor e Dw [j] da segunda roda para um maior. Isso leva ao fato de que, depois de parar uma das rodas, a segunda roda continua a girar. Para um chassi com uma unidade diferencial (e para muitos outros), isso leva a uma "virada" não planejada do robô no final da tarefa. Portanto, no caso de organizar o movimento espacial de todo o chassi, é necessário parar todos os motores de uma só vez.

2. Se Dw [i] não for alcançado, no loop é verificado o fato da próxima operação do codificador (a variável _syncloop [w] , atualizada a partir da interrupção do codificador e redefinida neste loop infinito). Quando a próxima interseção ocorre, o programa calcula o módulo da velocidade angular atual de cada roda (graus / s), de acordo com a fórmula óbvia:

W [i] = (360 * tau [i]) / WHEEL_SEGMENTS ,
onde:
tau [i] - o valor médio do tempo entre as duas últimas respostas dos codificadores. A "profundidade" do filtro de média é determinada por MEAN_DEPTH e o padrão é 8.

3. Com base nas velocidades calculadas das rodas, os erros absolutos são calculados como as diferenças entre as velocidades angulares definidas e reais.

4. Com base nos erros calculados, a ação de controle (valor do sinal PWM) é corrigida para cada motor.

5. Após atingir Dw [i] , no caso de uma quebra ativa, a tensão de curto prazo reversa é aplicada aos motores. A duração desse efeito é determinada na tabela de calibração (veja abaixo) e geralmente varia de 15 a 40 ms.

6. Há uma liberação completa de tensão dos motores e saia _wheel_rotate_sync () .

Eu já mencionei certa tabela de calibração duas vezes. Portanto, na biblioteca, há uma tabela especial de valores armazenados na EEPROM da memória do robô e contendo registros de três valores relacionados:

1. Tensão nos terminais do motor. É calculado convertendo o valor do sinal PWM na tensão real. Para isso, na etapa 4 do método wheelRotateAng () , a tensão real no driver do motor é registrada.

2. A velocidade angular de rotação da roda (sem carga) correspondente a uma determinada tensão.

3. A duração do sinal de parada forçada correspondente a essa velocidade angular.
Por padrão, o tamanho da tabela de calibração é de 10 registros (determinados pela constante WHEEL_TABLE_SIZE no arquivo config.h ) - 10 triplos dos valores “tensão - velocidade angular - duração do sinal de parada”.

Para determinar os valores das entradas 2 e 3 nesta tabela, é usado um método especial - wheelCalibrate (byte wheel) .

Vamos dar uma olhada nisso. Este método implementa uma sequência de ações para determinar os valores ausentes na tabela de calibração do motor / roda, bem como descobrir a velocidade angular mínima de partida e a velocidade angular máxima da roda.

Para executar a calibração, o robô é montado em um suporte; toda a rotação da roda durante a calibração é realizada sem carga.

1. Primeiro você precisa determinar a velocidade inicial mínima. Isso é feito de maneira muito simples. Em um ciclo, o controle PWM é alimentado ao motor, iniciando em 0, com um incremento de 1. A cada etapa, o programa aguarda algum tempo, determinado pela constante WHEEL_TIME_MAX ( atraso normal () ). Após o tempo de espera, ele verifica se o início foi concluído (alterando o valor do contador do codificador). Se a retirada for concluída, a velocidade angular de rotação da roda é calculada. Para maior segurança, o valor 10 é adicionado ao valor do PWM correspondente a essa velocidade inicial, o que dá o primeiro par de valores “tensão no motor” - “velocidade angular”.

2. Depois que a velocidade inicial for encontrada, a etapa PWM é calculada para preencher uniformemente a tabela de calibração.

3. No ciclo, para cada novo valor de PWM, a roda é girada 2 voltas completas e a velocidade angular é medida de acordo com um algoritmo semelhante ao método _wheel_rotate_sync () . No mesmo ciclo, também por aproximação sucessiva, é medido o valor ótimo da duração do sinal de parada forçada. Inicialmente, algum valor obviamente grande é obtido. E então é testado no modo "turn-stop". Como o ideal, o valor máximo da duração do sinal de parada é selecionado, no qual a “distância de rotação” definida não é excedida. Em outras palavras, esse valor da duração do sinal, quando o suprimento é fornecido ao motor, por um lado, a inércia é suprimida e, por outro lado, não há movimento reverso de curto prazo (que é fixado pelo mesmo codificador).

4. Após a calibração, a tensão de controle do motor calibrado deixa de ser aplicada e a tabela de calibração dessa roda é registrada na EEPROM.

Omiti todos os tipos de trivialidades de implementação e tentei afirmar a essência. Você pode perceber que os métodos wheelRotateAng () e wheelRotateAngRad () estão bloqueando funções. Esse é o preço pela precisão do movimento e por uma integração bastante simples nos esboços do usuário. Seria possível criar um gerenciador de tarefas pequeno com tempo fixo, mas isso exigiria que o usuário incorporasse sua funcionalidade estritamente na cota de tempo alocada.

E para um aplicativo sem bloqueio, a API tem a função wheelRotate (float * speed) . Como pode ser visto na lista de parâmetros, simplesmente executa a rotação das rodas com as velocidades definidas. E a velocidade de rotação é ajustada no método Sync () do chassi do robô, chamado no método Sync () do objeto da classe Miro com o mesmo nome. E de acordo com os requisitos para a estrutura do esboço do usuário, esse método deve ser chamado a cada iteração do loop principal () do loop ARDUINO.

Na etapa 4, na descrição do método _wheel_rotate_sync () , mencionei a "correção de controle" do mecanismo. Como você adivinhou)? Este é o controlador PID). Bem, mais precisamente controlador PD. Como você sabe (de fato - nem sempre), a melhor maneira de determinar os coeficientes do regulador é a seleção). Há uma definição no arquivo de configuração config.h:

 #define DEBUG_WHEEL_PID 

Se você descomentá-lo, quando você chamar o método moveDist () da classe Miro, o seguinte gráfico invertido do erro relativo no controle da velocidade angular de uma das rodas do robô (esquerda) será exibido no console do robô.



Não se parece com nada)? Down é o tempo (cada barra é uma etapa do ciclo de controle) e o valor do erro é salvo à direita (com o sinal preservado). Aqui estão dois pares de gráficos na mesma escala com diferentes coeficientes do controlador PD. "Humps" são apenas as "ondas" do overshoot. Os números nas barras horizontais são um erro relativo (com preservação do sinal). Visualização simples do regulador, ajudando a ajustar manualmente os coeficientes. Com o tempo, espero fazer uma configuração automática, mas por enquanto.

Aqui está uma delícia :-)

Bem, por fim, vejamos um exemplo. Diretamente da biblioteca API_Miro_moveDist:

 #include <Miro.h> using namespace miro; byte PWM_pins[2] = { 5, 6 }; byte DIR_pins[2] = { 4, 7 }; byte ENCODER_pins[2] = { 2, 3 }; Miro robot(PWM_pins, DIR_pins, ENCODER_pins); int laps = 0; void setup() { Serial.begin(115200); } void loop() { for (unsigned char i = 0; i < 4; i++) { robot.moveDist(robot.getOptLinSpeed(), 0, 1, true); delay(500); robot.rotateAng(0.5*robot.getOptAngSpeed(), -90, true); delay(500); } Serial.print("Laps: "); Serial.println(laps); laps++; } 

A partir do texto do programa, tudo deve ficar claro. Como funciona - no vídeo.


Ladrilhos de 600 x 600 mm e intervalos de 5 mm. Em teoria, o robô deve contornar um quadrado com um lado de 1 metro. Obviamente, a trajetória “flutua”. Mas, para ser sincero, vale a pena dizer que na versão do robô que me resta para os testes, existem motores bastante rotativos que são difíceis de conduzir lentamente. Mas em alta velocidade e derrapagem, há um lugar para se estar, e a inércia não é fácil de lidar. Motores com uma relação de transmissão mais alta (como mesmo em nossos robôs MIRO, não estavam disponíveis durante o teste) devem se comportar um pouco melhor.

Se houver momentos incompreensíveis, fico feliz em esclarecer, discutir e melhorar. O feedback geralmente é interessante.