Entrando no Aeronet Episódio 2: Homing Drone

No artigo anterior, examinamos o procedimento para o lançamento de um drone virtual autônomo no ar. Sob a orientação do professor, até as crianças em idade escolar conseguem lançar o drone de acordo com esta instrução. Surge a pergunta: o que vem depois? Bem, drone, bem alto. Além disso, virtual - em brinquedos e simuladores e mais lindamente pintado.

Como próximo passo, propomos a criação de um drone de retorno capaz de ver seu objetivo e alcançá-lo com sucesso. É mais fácil usar um balão colorido como alvo.

Tal tarefa (estourar uma bola com um drone autônomo) foi recentemente realizada por equipes na seção Aeronet do All-Russian Robocross-2019 . Nós fomos inspirados pela música “Seek and destroy” do álbum de estréia de um grupo americano que foi muito popular durante a minha juventude turbulenta.
Na próxima série de artigos, veremos como ensinar um drone autônomo a seguir instruções simples do refrão da música acima mencionada.

Então, precisamos do seguinte:

1. Um drone voador que podemos controlar a partir do programa
2. Sistema de reconhecimento e orientação ao alvo
3. Combine os dois pontos anteriores juntos - e, voila, você pode competir, cujo drone voará mais rápido e mais bolas.

Em seguida, as primeiras coisas primeiro.

jMAVSim controlado por teclado

Imagine um drone vendo uma bola, como na figura acima. A câmera do drone está olhando para a frente.

Para voar com a bola, nosso drone deve ser capaz de: avançar / retroceder, virar à esquerda e à direita, voar mais alto e mais baixo. A bola se moverá na lente da câmera não apenas pelas rajadas de vento, mas também pela inclinação do drone ao longo do movimento e inclinação, mas negligenciamos essas inclinações até o momento.

Controlaremos a velocidade do drone, enviando mensagens através de mavros para o tópico /mavros/setpoint_velocity/cmd_vel_unstamped .

Para manipular o drone do teclado, usamos o módulo maldições ( descrição em russo , em inglês ).

O algoritmo é simples: os valores atuais das velocidades desejadas são armazenados em uma variável setvel do tipo geometry_msgs.msg.Twist . Ao pressionar um botão no teclado, aumentamos / diminuímos o componente de velocidade desejado. No cronômetro, 20 vezes por segundo, publicamos a velocidade no tópico especificado.

Para velocidade de avanço (em relação ao drone), é necessário esclarecimento. O fato é que as velocidades de vôo devem ser definidas no sistema de coordenadas local do "mundo" em que nosso drone voa. Portanto, você precisa acompanhar a posição atual (pose) do drone neste sistema de coordenadas. O eixo X do drone é girado em relação ao eixo X do "mundo" por um certo ângulo de guinada. Mavros publica a posição atual do drone nas coordenadas do "mundo" no tópico /mavros/local_position/pose . Tendo obtido o ângulo de guinada a partir da posição para obter e publicar a velocidade desejada do drone nas coordenadas mundiais, multiplicamos a velocidade de avanço desejada ajustada_forward por cos (guinada) para o eixo X e sin (guinada) para o eixo Y do mundo, respectivamente.

 #!/usr/bin/env python # coding=UTF-8 import rospy import mavros import mavros.command as mc from mavros_msgs.msg import State from geometry_msgs.msg import PoseStamped, Twist, Quaternion from mavros_msgs.srv import CommandBool from mavros_msgs.srv import SetMode import tf.transformations as t import math current_state=State() current_pose = PoseStamped() current_vel = Twist() def localpose_callback(data): global current_pose current_pose = data def publish_setvel(event): global current_pose, setvel_pub, setvel, setvel_forward q=current_pose.pose.orientation.x, current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w roll, pitch, yaw = t.euler_from_quaternion(q) setvel.linear.x = setvel_forward * math.cos(yaw) setvel.linear.y = setvel_forward * math.sin(yaw) setvel_pub.publish(setvel) def main(): global current_pose, setvel, setvel_pub, setvel_forward rospy.init_node("offbrd",anonymous=True) rate=rospy.Rate(10) pose_sub=rospy.Subscriber("/mavros/local_position/pose",PoseStamped,localpose_callback) setvel_pub=rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped",Twist,queue_size=1) arming_s=rospy.ServiceProxy("/mavros/cmd/arming",CommandBool) set_mode=rospy.ServiceProxy("/mavros/set_mode",SetMode) setvel=Twist() setvel_forward = 0 arming_s(True) set_mode(0,"AUTO.TAKEOFF") print 'Taking off.....\r' rospy.sleep(5) # keyboard manipulation import curses stdscr = curses.initscr() curses.noecho() stdscr.nodelay(1) stdscr.keypad(1) for i in range (0,10): setvel_pub.publish(setvel) rate.sleep() set_mode(0,"OFFBOARD") setvel_timer = rospy.Timer(rospy.Duration(0.05), publish_setvel) while (rospy.is_shutdown()==False): rate.sleep() # keyboard hcommands handling c = stdscr.getch() if c == ord('q'): break # Exit the while() elif c == ord('u'): setvel.linear.z += 0.25 elif c == ord('d'): setvel.linear.z -= 0.25 elif c == curses.KEY_LEFT: setvel.angular.z += 0.25 elif c == curses.KEY_RIGHT: setvel.angular.z -= 0.25 elif c == curses.KEY_UP: setvel_forward += 0.25 elif c == curses.KEY_DOWN: setvel_forward -= 0.25 elif c == ord('s'): setvel_forward=setvel.linear.z=setvel.angular.z=0 if c!=curses.ERR: print setvel,'\r' curses.endwin() set_mode(0,"AUTO.LAND") print 'Landing.......\r' if __name__=="__main__": main() 

Para executar o programa para execução, precisamos executar o jMAVSim e conectar o mavros a ele usando o comando roslaunch mavros (depois de executar o roscore se ele não foi iniciado automaticamente):

 roslaunch mavros mavros_sitl.launch fcu_url:="udp://@192.168.7.14:14580" 

Verifique se estamos conectados usando eco rostopic / mavros / state. O campo conectado deve ser = True.

Depois disso, salve o código do programa em um arquivo e execute-o com o python fly_mavsim.py . O quadrocopter virtual deve subir até uma altura de cerca de 2 metros (a altura da decolagem é definida no parâmetro MIS_TAKEOFF_ALT no QGroundControl) e congelar. Utilizando os botões do teclado, ele pode ser controlado: setas da direita para a esquerda - giro, setas para cima - para baixo - movimento para frente / trás, u - voar para cima (PARA CIMA), d - voar para cima (PARA CIMA), d - voar para baixo (PARA BAIXO), s - travar no lugar (PARAR, todas as velocidades = 0), q - saia do programa (QUIT) e aterre o quadrocopter.

Você pode voar pelo mundo virtual, verificar como um drone virtual ideal se comporta em voo.

As mudanças nas velocidades dos pressionamentos de tecla são resumidas. Você pode fazer o drone voar em círculo, em espiral, em torno de um certo ponto, simulando um sistema de mira.

Então começa a diversão: a partir de um drone esférico no vácuo, passamos para o mundo real.

Drone real controlado por teclado

Há muitas instruções na rede para montar e configurar helicópteros na pilha PX4. Um processo bastante detalhado é descrito na documentação dos desenvolvedores .

Como usei um drone pronto, isso me salvou de vários agachamentos durante a montagem e a configuração preliminar do sistema.

Como um computador de bordo, usamos o Raspberry PI 3 Modelo B +, com o Raspbian + ROS Kinetic instalado. O Raspberry é conectado ao controlador de vôo Pixracer via uart, de acordo com o esquema:

O módulo GPS está conectado à porta GPS do controlador de vôo. Eu uso o módulo TS100 da Radiolink, a qualidade é boa, o custo não é alto.

O ROS Kinetic pode ser instalado por conta própria usando as instruções . No Raspberry, você deve configurar o acesso via Wifi e ssh ( aqui está uma instrução, por exemplo ).

Você também pode usar uma imagem pronta para Raspberry da empresa "Copter Express". Nesse caso, você deve desativar o pacote inteligente padrão, do qual não precisamos:

 sudo systemctl stop clever sudo systemctl disable clever 

Outra imagem Raspberry com ROS e OpenCV é descrita aqui , eu não tentei no meu trabalho, mas as ferramentas usadas são semelhantes.

Depois de ligar o drone e conectar-se ao Raspberry via ssh, lançamos mavros e verificamos a conexão com o controlador de vôo:

 roslaunch mavros px4.launch fcu_url:='/dev/ttyAMA0:921600' gcs_url:='tcp-l://0.0.0.0:5760' rostopic echo /mavros/state 

Se tudo funcionar corretamente, uma vez por segundo, recebemos mensagens sobre o status do controlador de vôo com o campo Connected = True.

O parâmetro gcs_url na chamada gcs_url é necessário para que possamos conectar o QGroundControl ao controlador de vôo do drone via Wi-Fi via ponte TCP. Os parâmetros de conexão são definidos no QGroundControl:

Para preparar o drone para o vôo autônomo, sigo alguns passos simples:

1. Verifique se o drone voa bem no modo manual. Para fazer isso, as configurações corretas dos parâmetros devem ser selecionadas e todos os sensores calibrados. O uso de um drone disponível comercialmente me salvou dos problemas neste momento. Usei o estimador LPE com o GPS ativado e um barômetro:

2. Se o sistema iniciar em um novo local ou pela primeira vez, aguarde o módulo GPS "aquecer". Com o trabalho regular na rua, o GPS geralmente captura de 16 a 22 satélites.

3. Antes de voar para um novo local, e se o drone não aguentar um ponto, você deve recalibrar a bússola:

Você deve usar uma bússola externa e verifique se a orientação correta está definida nas configurações:

Se todas as configurações forem feitas corretamente, o drone deve segurar o ponto com segurança no modo HOLD e voar de forma estável no modo Position.

Após voos de teste bem-sucedidos no modo manual, podemos verificar como nosso programa fly_jmavsim.py funciona em um drone real: vídeo .

Um drone real não voa tão perfeito quanto um virtual - mas também precisa obedecer aos comandos do teclado - voe na direção certa e pare ao comando.

Se isso acontecer, em vez do controle do teclado, podemos usar a visão computacional, que discutiremos sobre a configuração no próximo artigo. Os primeiros testes de caça à bola pareciam algo assim .

As configurações do nosso controlador de vôo + código do programa estão publicadas no github .