Вход в 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 на реальном дроне: видео.

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

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

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