В предыдущей статье мы рассмотрели порядок действий для запуска в воздух автономного виртуального дрона. Под руководством преподавателя по этой инструкции удаётся запустить дрон даже школьникам. Возникает вопрос: а что дальше? Ну, дрон, ну взлетел. Тем более, виртуальный – в игрушках и симуляторах и покрасивее есть нарисованы.
В качестве следующего шага мы предлагаем создать самонаводящийся дрон, способный увидеть свою цель и успешно её достичь. В качестве цели проще всего использовать цветной воздушный шарик.
Такое задание (лопнуть шарик автономным дроном) недавно выполняли команды на секции Аэронет всероссийского Робокросса-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 на реальном дроне: видео.
Реальный дрон летает не так идеально, как виртуальный – но он также должен слушаться команд с клавиатуры – лететь в нужном направлении, и останавливаться по команде.
Если это происходит – вместо управления с клавиатуры мы можем задействовать компьютерное зрение, о настройке которого расскажем в следующей статье. Первые тесты охоты на шарик выглядели примерно так.
Настройки нашего полётного контроллера + программный код выложены на Гитхабе.
And_Ray
Мы году так в 14 в реале шарики лопали… :) Было весело