Вход в Aeronet, эпизод 2: Самонаводящийся дрон

В предыдущей статье мы рассмотрели порядок действий для запуска в воздух автономного виртуального дрона. Под руководством преподавателя по этой инструкции удаётся запустить дрон даже школьникам. Возникает вопрос: а что дальше? Ну, дрон, ну взлетел. Тем более, виртуальный – в игрушках и симуляторах и покрасивее есть нарисованы.

В качестве следующего шага мы предлагаем создать самонаводящийся дрон, способный увидеть свою цель и успешно её достичь. В качестве цели проще всего использовать цветной воздушный шарик.

Такое задание (лопнуть шарик автономным дроном) недавно выполняли команды на секции Аэронет всероссийского Робокросса-2019. На него нас вдохновила песня “Seek and destroy” с дебютного альбома одной весьма популярной во времена моей бурной молодости американской группы.
В последующем цикле статей мы рассмотрим, как научить автономного дрона выполнять нехитрые инструкции из припева вышеупомянутой песенки.

Итак, нам понадобится следующее:

1. Летающий дрон, которым мы можем управлять из программы
2. Система распознавания цели и наведения на неё
3. Соединить предыдущие два пункта воедино – и, вуаля, можно соревноваться, чей дрон быстрее и больше шариков бабахнет.

Далее, обо всём по порядку.

jMAVSim, управляемый с клавиатуры

Представим себе дрон, видящий шарик, примерно как на картинке выше. Камера дрона смотрит строго вперёд.

Для налёта на шарик наш дрон должен уметь: двигаться вперёд/назад, поворачивать влево и вправо, лететь выше и ниже. Шарик будет двигаться в объективе камеры не только от порывов ветра, но и от наклонов дрона по крену и тангажу, но данными наклонами мы пока пренебрегаем.

Управлять мы будем скоростями дрона, отправляя через mavros сообщения в топик /mavros/setpoint_velocity/cmd_vel_unstamped.

Для манипуляций дроном с клавиатуры используем модуль curses (описание на русском, на английском).

Алгоритм простой: текущие значения желаемых скоростей хранятся в переменной setvel типа geometry_msgs.msg.Twist. По нажатии кнопки на клавиатуре мы увеличиваем/уменьшаем нужную компоненту скорости. По таймеру 20 раз в секунду публикуем скорость в указанный топик.

Для скорости движения вперёд (относительно дрона) потребуется уточнение. Дело в том, что скорости полёта должны задаваться в локальной системе координат «мира», в котором летает наш дрон. Поэтому нужно отслеживать текущее положение (pose) дрона в этой системе координат. Ось Х дрона повёрнута относительно оси Х «мира» на определённый угол yaw. Текущую позицию дрона в координатах «мира» mavros публикует в топик /mavros/local_position/pose. Получив из позиции угол yaw, чтобы получить и опубликовать желаемую скорость дрона в координатах мира – мы умножаем желаемую скорость движения вперёд setvel_forward на cos(yaw) для оси Х и на sin(yaw) для оси Y «мира», соответственно.

#!/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()

Для запуска программы на выполнение нам нужно запустить jMAVSim, и подключить к нему mavros с помощью команды roslaunch mavros (предварительно запустив roscore, если он не был запущен автоматически):

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

Убедиться, что мы подключились с помощью rostopic echo /mavros/state. Поле connected должно быть = True .

После этого сохранить код программы в файл и запустить его на выполнение командой python fly_mavsim.py. Виртуальный квадрокоптер должен подняться на высоту примерно 2 метра (высота взлёта задаётся в параметре MIS_TAKEOFF_ALT в QGroundControl) и зависнуть. С помощью кнопок на клавиатуре им можно управлять: стрелки вправо-влево – поворот, стрелки вверх-вниз – движение вперёд/назад, u – лететь вверх (UP), d – лететь вниз (DOWN), s – зависнуть на месте (STOP, все скорости = 0), q – выйти из программы (QUIT) и посадить квадрокоптер.

Можно полетать по виртуальному миру, проверить как ведёт себя в полёте идеальный виртуальный дрон.

Изменения скоростей от нажатия клавиш суммируются, можно заставить дрон летать по кругу, по спирали, вокруг определённой точки, имитируя систему наведения на цель.

Дальше начинается интересное: от сферического дрона в вакууме мы переходим в реальный мир.

Управляемый с клавиатуры реальный дрон

В сети много инструкций по сборке и настройке коптеров на стеке PX4. Довольно подробно процесс описан в документации разработчиков.

Поскольку я использовал готовый дрон – это избавило меня от многочисленных приседаний при сборке и предварительной настройке системы.

В качестве бортового компьютера мы используем Raspberry PI 3 Model B+, с установленным Raspbian + ROS Kinetic. Подключение Raspberry к полётному контролеру Pixracer осуществляется через uart, по схеме:

GPS модуль подключается в порт GPS полётного контроллера. Я пользуюсь модулем TS100 от Radiolink, качество хорошее, стоимость не высокая.

ROS Kinetic можно установить самостоятельно, воспользовавшись инструкцией. На Raspberry следует настроить доступ по Wifi и ssh (вот инструкция, например).

Также можно использовать готовый образ для Raspberry от компании «Коптер-Экспресс». В этом случае следует отключить включенный по умолчанию пакет clever, который нам не понадобится:

sudo systemctl stop clever sudo systemctl disable clever

Другой образ Raspberry с ROS и OpenCV описан вот здесь, его я не пробовал в работе, но используемые инструменты похожи.

После включения дрона и подключения к Raspberry по ssh – запускаем mavros и проверяем связь с полётным контроллером:

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

Если всё работает правильно – раз в секунду получаем сообщения о состоянии полётного контроллера с полем Connected = True.

Параметр gcs_url в вызове mavros нужен для того, чтоб мы могли подключить QGroundControl к полётному контроллеру дрона по WiFi через TCP-bridge. Параметры подключения задаются в QGroundControl:

Для подготовки дрона к автономному полёту я выполняю несколько простых шагов:

1. Убедимся, что дрон хорошо летает в ручном режиме. Для этого должны быть выбраны правильные настройки параметров и откалиброваны все датчики. Использование серийно производимого дрона избавило меня от мороки на этом этапе. Я использовал LPE estimator с включенным GPS и барометром:

2. Если старт системы происходит в новом месте или первый раз – следует дождаться «прогрева» GPS модуля. При штатной работе на улице GPS у меня обычно ловит от 16 до 22 спутников.

3. Перед полётом на новом месте, и в случае, если дрон не держит точку – следует перекалибровать компас:

Следует использовать внешний компас, и убедиться, что в настройках установлена его правильная ориентация:

Если все настройки выполнены корректно – дрон должен уверенно держать точку в режиме HOLD и летать стабильно в режиме Position.

После успешных тестовых полётов в ручном режиме – мы можем проверить, как работает наша программа fly_jmavsim.py на реальном дроне: видео.

Реальный дрон летает не так идеально, как виртуальный – но он также должен слушаться команд с клавиатуры – лететь в нужном направлении, и останавливаться по команде.

Если это происходит – вместо управления с клавиатуры мы можем задействовать компьютерное зрение, о настройке которого расскажем в следующей статье. Первые тесты охоты на шарик выглядели примерно так.

Настройки нашего полётного контроллера + программный код выложены на Гитхабе.