Виртуальный квадрокоптер на Unity + OpenCV (Часть 1)

Доброго времени суток, дорогие хабравчане!

Что нам стоит дом построить? Нарисуем — будем жить. В этой серии статей я хотел бы поделится опытом строительства (и рассказать как) виртуального квадрокоптера в Unity. А также получить ценные советы от коллективного разума хабра :) Виртуального дрона я задумал с целью тестирования существующих алгоритмов компьютерного зрения, а также их приложения в навигации коптеров. С 5й версии в Unity есть возможность писать C++ плагины, то есть имеется возможность применить всю коровью суперсилу все возможности сторонних C/C++ библиотек, таких как OpenCV, чем я, собственно, и собираюсь заняться. Да, реальный мир намного сложнее Unity. Но мне хочется верить, что использование такого симулятора может послужить хорошим начальным приближением для разработки искусственного интеллекта дрона. Если вам интересно, то добро пожаловать под кат.

В части 1 мы будем создавать свой виртуальный квадрокоптер в Unity и стабилизировать его PID регуляторами. Подключение OpenCV будет в части 2. В части 3 планируется потестировать алгоритм плотной 3D реконструкции из OpenCV. Дальше: как пойдет.

Разработку я веду в Mac OS, но так, как сами инструменты кроссплатформенны, я думаю, что это можно пробовать повторить и под другими системами. Работу в Unity я, поначалу, буду описывать подробно, чтобы можно было, не отрываясь от статьи по крайней мере, не часто отрываясь от статьи, воссоздавать то, что описывается. Итак приступаем. Устанавливаем и запускаем Unity, у меня версия 5.2.0f3. Создаем пустой 3D проект. Я использую лейаут по-умолчанию: меню Window -> Layouts -> Default. Далее идем в меню GameObject -> 3D Object -> Cube, у нас появился белый куб 1 x 1 x 1 в начале координат. И сейчас мы из него сделаем поверхность земли. Справа в инспекторе (Inspector) выставляем у нашего куба Scale, так чтобы он стал похож на поверхность. X: 20 Y:0.1 Z:20 (в этом же блоке можно задавть координаты и ориентацию объектов). Далее покрасим нашу поверхность в зеленый цвет: Меню Assets -> Create -> Material, задаем имя Ground — мы создали текстуру для нашей поверхности. Далее в инспекторе проекта (Project) в папке Assets мы видим только что созданную текстуру (матерьял), выбираем ее и справа в инспекторе (Main Maps -> Albedo) задаем цвет. Находим в Hierarchy нашу поверхность, если она по прежнему называется Cube, можно дать ей какое-то более адекватное имя. Накидываем драг эн дропом текстуру на нашу поверхность и вот уже она покрасилась в нужный нам цвет.



Теперь возьмемся за создание нашего квадрокоптера. Информацию о квадрокоптерах я черпал из замечательной статьи, в ней очень хорошо описаны базовые понятия, принцип работы и управления. Очень советую прочитать эту статью, так как здесь я не буду описывать особенностей квадрокоптеростроения, чтобы излишне не повторятся. Наш дрон будет состоять из Cube — основания, 4х Cylinder — это будут штанги для укрепления двигателей и 4х Capsule — это будут наши двигатели. Создаем в Hierarchy с помощью кнопочки Create пустой объект и называем его Quadrocopter. Правой кнопкой на нем, добавляем туда вышеперечисленные примитивы. Советую сразу делать их размерами, похожими на реальные, чтобы получить похожее на ожидаемое поведение твердых тел. В документации по Unity пишут, что их 1 — это один метр, я сделал scale основания 0.2 x 0.1 x 0.2. Укажите в комментариях более правильный путь задания размеров примитивов, я, глядя на интерфейс Unity, другого интуитивного способа не нашел. Путем манипулирования с позициями, ориентациями и размерами, получаем вот такой квадрик:



Белым цветом отмечены передние двигатели (у квадрокоптера есть перед). Запускаем, он, естественно, никуда не летит. Это нормально :) Теперь надо добавить к нашим примитивам физику. Нам также желательно объединить штанги и тело в одну неделимую раму, рама будет пустым GameObject, в который мы перенесем требуемые примитивы. Для реализации физики твердого тела используется компонент Rigidbody. Выделяем наш элемент, например раму. В инспекторе справа нажимаем Add Component -> Physics -> Rigidbody. В списке, в инспекторе, появляется Rigidbody. Добавим его и на двигатели. Запускаем, все падает и разваливается — это нормально, теперь у нас есть физика.



Чтобы все не разваливалось добавим Add Component -> Physics -> Fixed Joint в составляющие нашего квадрокоптера. Этот компонент будет реализовывать жесткую связь. Параметром Connected Body указываем на объект, к которому хотим привязаться. Привязываем двигателей к раме. И вот теперь у нас ничего не разваливается, просто падает как есть.



Теперь надо добавить мощности в наши двигатели. Для этого мы будем использовать нехитрый C# скрипт. Идем в Assets Create -> C# Script, назовем его motorScript.

using UnityEngine;
using System.Collections;

public class motorScript : MonoBehaviour {

	public float power = 0.0f;

	void FixedUpdate () {
		GetComponent<Rigidbody> ().AddRelativeForce (0, power, 0);
	}
}

Потом выбираем двигатель и делаем Add Component -> Scripts -> Motor Script. У нашего скрипта есть параметр power — это сила нашего мотора. Можно задать для каждого мотора, например, значение 2 и мы увидим, что наш квадрик падает теперь не так быстро: двигатели работают. Теперь осталось стабилизировать наш квадрокоптер, чтобы иметь возможность задавать извне ему углы поворота и он мог их держать. Для этого создадим скрипт quadrocopterScript.cs. Стабилизация квадрокоптера осуществляется PID регуляторами. Для более подробной информации об этом процессе посмотрите вышеуказанную статью. Для каждого угла нам будет необходим свой PID регулятор, в итоге нам необходимо 3 независимых регулятора. Ниже привожу код скрипта. Но сначала упомяну про один нюанс. Рыскание в квадрокоптере реализуется за счет моментов двигателей. Они могут поворачивать всю конструкцию, но в Unity этого почему-то не происходит, несмотря на то, что я пробовал добавлять GetComponent ().AddRelativeTorque в скрипт двигателя. В итоге пришел к тому, что нужно просто немного (на 10 градусов) повернуть двигатели относительно осей штанг так, чтобы передние двигатели были повернуты друг к другу, так же и задние. Это нужно чтобы угловой момент всей конструкции при одинаковой мощности двигателей гасился. Итак наш скрипт квадрокоптера:

using UnityEngine;
using UnityEngine.UI;
using System.Collections;
using System;

public class quadrocopterScript : MonoBehaviour {

	//фактические параметры
	private double pitch; //Тангаж
	private double roll; //Крен
	private double yaw; //Рыскание
	public double throttle; //Газ, газ мы задаем извне, поэтому он public

	//требуемые параметры
	public double targetPitch;
	public double targetRoll;
	public double targetYaw;

	//PID регуляторы, которые будут стабилизировать углы
	//каждому углу свой регулятор, класс PID определен ниже
	//константы подобраны на глаз :) пробуйте свои значения
	private PID pitchPID = new PID (100, 0, 20);
	private PID rollPID = new PID (100, 0, 20);
	private PID yawPID = new PID (50, 0, 50);

	void readRotation () {
		
		//фактическая ориентация нашего квадрокоптера,
		//в реальном квадрокоптере эти данные необходимо получать
		//из акселерометра-гироскопа-магнетометра, так же как делает это ваш
		//смартфон
		Vector3 rot = GameObject.Find ("Frame").GetComponent<Transform> ().rotation.eulerAngles;
		pitch = rot.x;
		yaw = rot.y;
		roll = rot.z;

	}

	//функция стабилизации квадрокоптера
	//с помощью PID регуляторов мы настраиваем
	//мощность наших моторов так, чтобы углы приняли нужные нам значения
	void stabilize () {

		//нам необходимо посчитать разность между требуемым углом и текущим
		//эта разность должна лежать в промежутке [-180, 180] чтобы обеспечить
		//правильную работу PID регуляторов, так как нет смысла поворачивать на 350
		//градусов, когда можно повернуть на -10

		double dPitch = targetPitch - pitch;
		double dRoll = targetRoll - roll;
		double dYaw = targetYaw - yaw;

		dPitch -= Math.Ceiling (Math.Floor (dPitch / 180.0) / 2.0) * 360.0;
		dRoll -= Math.Ceiling (Math.Floor (dRoll / 180.0) / 2.0) * 360.0;
		dYaw -= Math.Ceiling (Math.Floor (dYaw / 180.0) / 2.0) * 360.0;

		//1 и 2 мотор впереди
		//3 и 4 моторы сзади
		double motor1power = throttle;
		double motor2power = throttle;
		double motor3power = throttle;
		double motor4power = throttle;

		//ограничитель на мощность подаваемую на моторы
		double powerLimit = throttle > 20 ? 20 : throttle;

		//управление тангажем:
		//на передние двигатели подаем возмущение от регулятора
		//на задние противоположное возмущение
		double pitchForce = - pitchPID.calc (0, dPitch / 180.0);
		pitchForce = pitchForce > powerLimit ? powerLimit : pitchForce;
		pitchForce = pitchForce < -powerLimit ? -powerLimit : pitchForce;
		motor1power +=   pitchForce;
		motor2power +=   pitchForce;
		motor3power += - pitchForce;
		motor4power += - pitchForce;

		//управление креном:
		//действуем по аналогии с тангажем, только регулируем боковые двигатели
		double rollForce = - rollPID.calc (0, dRoll / 180.0);
		rollForce = rollForce > powerLimit ? powerLimit : rollForce;
		rollForce = rollForce < -powerLimit ? -powerLimit : rollForce;
		motor1power +=   rollForce;
		motor2power += - rollForce;
		motor3power += - rollForce;
		motor4power +=   rollForce;

		//управление рысканием:
		double yawForce = yawPID.calc (0, dYaw / 180.0);
		yawForce = yawForce > powerLimit ? powerLimit : yawForce;
		yawForce = yawForce < -powerLimit ? -powerLimit : yawForce;
		motor1power +=   yawForce;
		motor2power += - yawForce;
		motor3power +=   yawForce;
		motor4power += - yawForce;

		GameObject.Find ("Motor1").GetComponent<motorScript>().power = motor1power;
		GameObject.Find ("Motor2").GetComponent<motorScript>().power = motor2power;
		GameObject.Find ("Motor3").GetComponent<motorScript>().power = motor3power;
		GameObject.Find ("Motor4").GetComponent<motorScript>().power = motor4power;
	}

	//как советуют в доке по Unity вычисления проводим в FixedUpdate, а не в Update
	void FixedUpdate () {
		readRotation ();
		stabilize ();
	}
	
}

public class PID {
	
	private double P;
	private double I;
	private double D;
	
	private double prevErr;
	private double sumErr;
	
	public PID (double P, double I, double D) {
		this.P = P;
		this.I = I;
		this.D = D;
	}
	
	public double calc (double current, double target) {
		
		double dt = Time.fixedDeltaTime;
		
		double err = target - current;
		this.sumErr += err;
		
		double force = this.P * err + this.I * this.sumErr * dt + this.D * (err - this.prevErr) / dt;
		
		this.prevErr = err;
		return force;
	}
	
};

Добавляем этот скрипт в наш объект Quadrocopter и у нас появляется возможность задать газ и необходимые углы поворота. У меня при газе 22.3 квадрик медленно садится. Чтобы протестировать стабилизацию по углам, можно в Transform квадрокоптера задавать отдельно углы и смотреть как он принимает горизонтальное положение, в случае если в target… параметрах скрипта указаны нулевые углы.

Задача прикрутить виртуальный джойстик, красивую модельку и окружение я оставляю инициативному читателю.

Как это получилось у меня можно попробовать по ссылке на андроид пакет.

Код того, что сделано в статье можно посмотреть на гитхабе.