Обратные задачи динамики и моделирование движения ЛА

Автор: Сергей Резник

В настоящее время усиленно занимаюсь проектом нашей кафедры "Системы и процессы управления" - моя задача состоит в моделировании движения беспилотного летательного аппарата. Не так давно решил переписать все, что было наработано за два года. Собственно уже неделю сижу ночами - пишу. В общем задача состоит в том, чтобы по известной задаваемой траектории найти такое управление летательным аппаратом, которое бы приводило к воспроизведению этой траектории. Нелегкая это работа, я вам скажу. За четыре дня так и не добрался даже до прямого интегрирования уравнений движения, не говоря уж о решении обратной задачи динамики. Зато написал адский шаблонный класс для работы с полиномами и сплайнами. Сегодня добавил модель гравитационного поля Земли и модель стандартной атмосферы. Кому интересно (или пригодиться) могу показать:
Модель гравитационного поля Земли - по заданным широте, высоте и долготе вычисляется вектор ускорения свободного падения в геоцентрической СК с учетом центробежной силы, после чего он преобразуется в (если не ошибаюсь в термине) географическую систему координат:

typedef double PRECISION; typedef vector3<PRECISION> vec3p; #define EARTH_LONG_RADIUS PRECISION(6378136.49) #define EARTH_ECCENTRICITY_SQUARE PRECISION(0.00669438) #define EARTH_PARAMETER_MU PRECISION(3.9860009e+14) #define EARTH_PARAMETER_C20 PRECISION(-1082.63e-6) #define EARTH_ANGULAR_VELOCITY PRECISION(7.292116e-5) ... void Ce2Geolocation::calculateGravity() { PRECISION N = EARTH_LONG_RADIUS / ::sqrt(1.0 - EARTH_ECCENTRICITY_SQUARE * et::sqr(sin(_latitude))); PRECISION NH = N + _altitude; vec3p R(( NH - EARTH_ECCENTRICITY_SQUARE * N) * sin(_latitude), NH * cos(_latitude) * cos(_longitude), NH * cos(_latitude) * sin(_longitude) ); PRECISION r = R.length(); PRECISION rp3 = r * r * r; PRECISION g1 = 1.5 * EARTH_PARAMETER_C20 * et::sqr(EARTH_LONG_RADIUS / r); PRECISION RX5 = 1.0 - 5.0 * et::sqr(R.x / r); vec3p local_gravity = g1 * vec3p(2.0 + RX5, RX5, RX5) - vec3p(1.0); local_gravity *= R * EARTH_PARAMETER_MU / rp3; _gravity = local_gravity - vec3p(0.0, -sqr(EARTH_ANGULAR_VELOCITY) * R.y, -sqr(EARTH_ANGULAR_VELOCITY) * R.z); _gravity = (Quaternionp(_longitude, 1) * Quaternionp(-_latitude, 3)).invtransform(_gravity); }

class Ce2StandartAthmosphere { public: static const PRECISION getDensity(const PRECISION& atAltitude, const PRECISION& gravity); }; ... const PRECISION Ce2StandartAthmosphere::getDensity(const PRECISION& atAltitude, const PRECISION& gravity) { const PRECISION R = 8.31447; // universal gas constant const PRECISION standartPressure = 101325; const PRECISION temperatureLapseRate = 0.0065; const PRECISION standartTemperature = 288.15; const PRECISION airMolarMass = 0.0289644; const PRECISION air_temperature = standartTemperature - temperatureLapseRate * atAltitude; const PRECISION air_density = standartPressure * pow(1.0 - atAltitude * temperatureLapseRate / standartTemperature, (gravity * airMolarMass) / (R * temperatureLapseRate) ); return air_density * airMolarMass / (R * air_temperature); }