
В предыдущей статье мы рассмотрели порядок действий для запуска в воздух автономного виртуального дрона. Под руководством преподавателя по этой инструкции удаётся запустить дрон даже школьникам. Возникает вопрос: а что дальше? Ну, дрон, ну взлетел. Тем более, виртуальный – в игрушках и симуляторах и покрасивее есть нарисованы.
В качестве следующего шага мы предлагаем создать самонаводящийся дрон, способный увидеть свою цель и успешно её достичь. В качестве цели проще всего использовать цветной воздушный шарик.
Такое задание (лопнуть шарик автономным дроном) недавно выполняли команды на секции Аэронет всероссийского Робокросса-2019. На него нас вдохновила песня “Seek and destroy” с дебютного альбома одной весьма популярной во времена моей бурной молодости американской группы.
В последующем цикле статей мы рассмотрим, как научить автономного дрона выполнять нехитрые инструкции из припева вышеупомянутой песенки.
Итак, нам понадобится следующее:
- Летающий дрон, которым мы можем управлять из программы
- Система распознавания цели и наведения на неё
- Соединить предыдущие два пункта воедино – и, вуаля, можно соревноваться, чей дрон быстрее и больше шариков бабахнет.
Далее, обо всём по порядку.
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 на реальном дроне: видео.
Реальный дрон летает не так идеально, как виртуальный – но он также должен слушаться команд с клавиатуры – лететь в нужном направлении, и останавливаться по команде.
Если это происходит – вместо управления с клавиатуры мы можем задействовать компьютерное зрение, о настройке которого расскажем в следующей статье. Первые тесты охоты на шарик выглядели примерно так.
Настройки нашего полётного контроллера + программный код выложены на Гитхабе.
ссылка на оригинал статьи https://habr.com/ru/post/461887/
Добавить комментарий