Стабилизация ракеты в полёте

Надо вычислить угловую скорость между двумя векторами в движке ODE.
Дано два вектора, один текущий и другой нужный, надо задать такую угловую скорость, чтобы тело развернулось к нужному вектору.
Надо для стабилизации в полёте объекта типа ракета(стрела), чтобы объект только в полёте разворачивался параллельно вектору скорости.
Так алгоритм общем виде, я понял, а вот алгоритм вычисления вектора угловой скорости не понял.
Кто поможет?

ЗЫ
А понял, надо вычислить ось вращения вежду двумя этими вектора, то есть нужный вектор имеет относительно этих векторов угол 90гр. и условно направлен вверх. Или вниз? Upd: для этого есть функция crossproduct.

ЗЫЫ
В общем вроде разобрался.
Грязный код.

Код (Lua):

1.  
2. --стабилизация корпуса в полёте.
3. local stabilized_factor = 2.0
4. local torque = vector():set(0,0,0)
5. local dir = rocket:direction()
6. logf("direction[%.5f, %.5f, %.5f]", dir.x, dir.y, dir.z)
7. local angle = math.acos(clamp(dir:dotproduct(new_dir), -1.0, 1.0))
8. if angle > 0.0001 then
9.     torque:crossproduct(dir, new_dir):normalize_safe():mul(math.sin(angle)*rock_vel*stabilized_factor)
10. end
11. logf("torque[%.5f, %.5f, %.5f] angle=%.5f", torque.x, torque.y, torque.z, angle*RAD2DEG)
12. ph_shell:set_angular_vel(torque)
13.  

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

Код (Lua):

1. --стабилизация корпуса в полёте.
2. local stabilized_factor = 0.25
3. local torque = vector():set(0,0,0)
4. local dir = rocket:direction()
5. local angle = math.acos(clamp(dir:dotproduct(new_dir), -1.0, 1.0))
6. if angle > 0.0001 then
7.     torque:crossproduct(dir, new_dir):set_length(math.sin(angle)*rock_vel*rock_vel*stabilized_factor)
8. end
9. ph_shell:set_torque(torque)

Название топика можно изменить на "Стабилизация ракеты в полёте".

 

тангаж, рысканье, вращение в норме (цитата)
https://ru.wikipedia.org/wiki/Динамика_полёта
https://ru.wikipedia.org/wiki/Шесть_степеней_свободы
лично прошел descent 3 на самых тяжелых настройках, кроме концовки...хочу взяться за overload, но уже надоели компьютерные игры, так что врядли...
"в небе только и мысли что о море..."
"и сниться нам трава-трава у дома, зелёная-зелёная трава"...

 

Задача звучит как управление углом поворота через управление угловой скоростью.
Т.к в трехмерном пространстве общее управление по 3м осям (тангаж, крен, рысканье) одинаковое, рассмотрим только для одной плоскости - для остальных аналогично.

Уравнение, описывающее систему: (где предпросмотр у формул в редактировании сообщений?!!):

[math]\frac{\displaystyle dX_1}{\displaystyle dt}[/math] = [math]X_2[/math], где [math]X_1 - [/math]угол, [math]X_2 - [/math]угловая скорость (начальная, или постоянно действующая. Или равная нулю, если таковых нет).
Управлять углом будем через скорость: [math]\frac{\displaystyle dX_1}{\displaystyle dt} = X_2 + U[/math], где [math]U - [/math]найденное управление.

Синтезируем управление по методу АКАР (процедуры синтеза типовые, их пропущу):
1. Линейное управление
Возьмем линейную макропеременную [math]\psi = (X_1 - a)[/math], где [math]a - [/math]желаемый угол, получим управление: [math]U = - \frac{\displaystyle X_1-a}{\displaystyle T} - X_2[/math], где [math]a - [/math]желаемый угол, [math]T - [/math]постоянная времени (полностью синтез параметров вроде [math]T[/math] не помню, но система должна быть устойчивой здесь при [math]T=1[/math]), тогда управление будет [math]U = a - X_1 - X_2[/math]

2. Нелинейное управление
Возьмем нелинейную макропеременную [math]\psi = th(X_1 - a)[/math]. Тогда получим управление: [math]U = -X_2 - \frac{\displaystyle th(X_1 - a)\cdot ch^{2}(X_1 - a)}{\displaystyle T}[/math], возьмем постоянную времени [math]T = 1[/math], система должна быть устойчивой. Тогда [math]U = -X_2 - th(X_1 - a)\cdot ch^{2}(X_1 - a)[/math]. Переходный процесс в этом случае быстрее.

Вообще, предполагается, что управление будет непрерывным, [math]-[/math] как там оно дискретно будет я не скажу. Возможно, все будет хорошо.

 

это для самолётов, для ракет - не крен, а вращение, посмотри запуски в космос в рилтайм, я не даром привёл цитату...

 

И что, для ракет трехмерное пространство другое? Все то же самое, как ни назови. Думайте, прежде чем писать.

ЗЫ: Mikl___ - формула неправильная после редактирования (2-й случай). Надо именно
[math]\psi = th(X_1 - a)[/math]; [math]U = -X_2 - \frac{\displaystyle th(X_1 - a)\cdot ch^{2}(X_1 - a)}{\displaystyle T}[/math]; где [math]th -[/math] гиперб. тангенс, [math]ch -[/math] гиперб. косинус

 

хорошо, я подумаю

 

Для лаб работы сгодится может быть. Реально параметры всегда подбираются. В любой технике есть сложная временная функция - пид"

Маска техника тоже на ней основана. Это универсальная функция. Для любых моторов, не важно электрический он или на газе.

А формула ваша с певого взгляда не корректна, так как в ней нет лимитов, это очевидно даже из общих соображений: T -> 0 => U -> в бесконечность. А физически никакая материя не может эти значения принимать. Законы сохранения

 

ПИД - это один из методов получения управления, он из классической ТАУ. Мною приведено управление по методу из синергетической теории управления, не классической, там другой подход к синтезу управления . "Это универсальная функция" - это не означает, что это единственный способ.
Использованный метод состоит из 2х этапов:
1. структурный синтез управления
2. параметрический синтез, где определяются коэффициетны (T в том числе) - там определяются допустимые области значений данных коэффициентов, чтобы система была стабильна, и далее выбирается из найденной области какой-то их набор, исходя из максимального быстродействия. Повторюсь - полностью не помню синтез здесь коэфф-в, приведено мною лишь чтобы система была стабильна.
Ограничение на величину управления U конечно можно задать, но ТС не просил этого же, не ? Это же чисто в програмке-физ движке смоделировать что-то - тогда это вполне сгодится. Или вы думаете, что ТС дома ракету в железе собирает ?

"А формула ваша с певого взгляда не корректна, так как в ней нет лимитов" - а вы смоделируйте, смоделируйте ..

 

Что то, тут вы ударились в матан! И честно говоря, я мало чего понял, точней совсем ничего не понял!
Я просто программист-любитель (ПЛ), и как у многих ПЛ, у меня большая проблема с матаном! Некоторые П говорят, что П матан не нужен! Ржу в голос! Если бы у меня не было проблем с матаном, то я бы такой ерунды тут точно не спрашивал.
В общем, задача общем виде.
Есть объект "ракета", которая летит в упреждённую точку, вектор тяги направлен по направлению ракеты. Ракета постоянно разворачивается в упреждённую точку цели, пока не столкнётся с ней, или не потеряет цель.
Теперь конкретно проблема.
Есть два 3д вектора, один текущий, это вектор направления ракеты, и второй вектор направлен на упреждённую точку.
Надо задать такую вращательную скорость объекту чтобы он начал разворачиваться во второй вектор, т.е. в упреждённую точку.
В ODE вектор позиции X,Y,Z - на север, вверх, запад.
Вектор вращения - соответственно скорость вращения по этим осям почасовой стрелки, если не ошибаюсь.
Матрица ориентации объекта
Tvector i; T _14_; вправо
Tvector j; T _24_; верх
Tvector k; T _34_; направления вперёд
Tvector c; T _44_; позиция
Надо вычислить нужный вектор вращения.
Более конкретные входные данные:
XFORM - матрица положения объекта
rocket_dir - вектор направления = XFORM.k
future_dir - вектор упр. точки
Выходные данные:
angular_vel - вектор скорости разворота, длина равна общей скорости разворота и пропорциональна скорости ракеты и углу между векторами rocket_dir и future_dir

Я бы и сам решил, но что-то в последние время туплю, вроде и просто всё.

 

[math]future_{T}+future_{dir} - rocket_{T} =V_1[/math]
[math]rocket_{dir}=V_2[/math]
Вектор [math]V_2[/math] надо довернуть до вектора [math]V_1[/math].
Раскладываем на дирекционные углы
[math]АЗ_{2}=atan2(V_{2}\cdot X,V_{2}\cdot Y)[/math];
[math]УМ_{2}=arcsin(\frac{\displaystyle V_{2}\cdot Z}{\displaystyle Len(V_{2})})[/math]

[math]АЗ_1=atan2(V_1\cdot X,V_1\cdot Y)[/math];
[math]УМ_1=arcsin(\frac{\displaystyle V_1\cdot Z}{\displaystyle Len(V_1)})[/math]

Находим разницу и подставляем в матрицы поворота, которые умножаем между собой.

Скорость подбираем так что бы расстояние до цели было минимальное на n шагов вперед - вернее сумма квадратов. Или можно использовать нечёткий ПИД регулятор.
https://habr.com/ru/post/413539/

 

Как оказалось код в ТС оказался немного неправильный.
Вот исправный вариант.

Код (Lua):

1.  
2. --разворот корпуса в сторону точки встречи.
3. local angular_factor = 0.2
4. angle = math.acos(clamp(rocket_dir:dotproduct(self.future_dir), -1.0, 1.0))
5. local angular_vel = vector():crossproduct(rocket_dir, self.future_dir):set_length(rock_vel*angular_factor*angle)
6. angular_vel:add(rocket_dir:mul(3.1415*10.0)) --вращение вокруг оси
7. ph_shell:set_angular_vel(angular_vel)
8.  

Как видно для определения вектора разворота используется метод crossproduct и задаём длину вектора пропорциональна углу разворота.

Но тут другая проблема. Надо рассчитать угол атаки при котором ракета перестаёт падать вниз. При полёте реальной ракеты за счёт игры центра тяжести и центра давления, ракету разворачивает на определённый угол, который компенсирует силу тяжести, и ракета летит прямолинейно. Сейчас думаю просто рассчитать текущий угол атаки при котором ракета перестаёт падать, и задать соответствующую скорость разворота.
Но нет ли других формул, которые даёт аналогичный результат?

 

Есть такая штука как диздок. Ну вот смотрим.
Реалистичная физика ракеты в игровом движке.
Обычно, в игровых движках, ракета летит по сильно упрощенной физикой. В некоторых ситуациях это не целесообразно. И тут надо создать более реалистичную физику, полностью реалистичную физику делать сложно, и не целесообразно, поэтому формулы, по которым летит ракета, упрощены.
На ракету в полёте воздействует следующие силы: реактивная сила двигателя, силы тяжести, сила сопротивления воздуха, подъёмная сила, стабилизирующая вращающая сила, так же, если ракета управляемая, то ещё, и управляющая вращающая сила.
[math]\alpha[/math] - угол атаки
[math]k_{air drag}[/math] - коэффициент сила сопротивления воздуха
[math]k_{lift}[/math] - коэффициент подъёмной силы
Реактивная сила двигателя направлена точно по направлению ракеты.
Сила воздушного сопротивления складывается с индуктивным сопротивлением, которая зависит от угла атаки.
[math]k_{tren} = k_{air drag} + sin(\alpha)\cdot k_{lift}[/math]
Подъёмная сила зависит от угла атаки, и максимальна при определенном значении, а дальше происходить срыв подъёмной силы и её резкое уменьшение.
[math]k_{lift force} = sin(\alpha)\cdot k_{lift}[/math]
Стабилизирующая сила (вращающая), при положительном [math]stabilized\;factor[/math] стремится уменьшить угол атаки, при отрицательным, если угол атаки не равен нулю, то стремится увеличить. Обычно [math]stabilized\;factor[/math] положительный.
[math]angular = rock_{vel}\cdot rock_{vel}\cdot stabilized\;factor\cdot sin(\alpha)[/math]
Управляющая сила используется управляемыми ракетами.
Работает, так же как и стабилизирующая сила, и пытается развернуть ракету в точку наведения. Должна быть сильней стабилизирующей силы. А так же, гасить лишнюю угловую скорость, чтобы ракета в полёте не раскачивалась.
Так же можно учитывать волновое сопротивление, если ракета летит на сверхзвуковой скорости, и если это целесообразно.

© НаноБот 26.11.2020 г.

 

https://js1k.com/2010-first/details/133

 

Intro, весь принцип управления построен на простом принципе == от акселерометров получаешь данные отклонения от предыдущего положения продольной оси ракеты и делаешь импульсы двигом, дабы парировать эти углы. скорость реакции двига должна укладываться в 30-50мс , а на больших лагах идут нелинейные процессы и ни акий матан не поможет. Вообще, про матан в плане практики надо знать одну простую вещь == любое тех решение успешно ровно настолько, насколько успешно оно содействует линейному поведению целевого процесса.

 

Реализую дрон для сталкера, FPV и RTF. Как тут(XRay) понятно, поворот реализован через вектор поворота, т.е. есть правильные векторы матрицы получаю из xform, длина управляющих сил float yaw, pitch, roll;. Надо вычислить сумму векторов, т.е. получить вектор поворота. Я знаю, что тут обычно реализуют матрицу поворота через кватернионы, но мне надо просто вычислить вектор поворота, чтобы движок сам развернул модель как надо. Доступа к изменению xform из класса дрона НЕТ! Набросал код, вроде работает, но в какой-то момент моделька дрона начинает странно поворачиваться, метод rotate_angle90(v1,v2,angle) сферическая интерполяция между векторами на 90 гр(angle = 0 вектор указан на v1, angle = 90 вектор на v2), то есть векторами матрицы, угол определил как angle = pitch/(yaw+pitch)*Deg90, что очевидно не правильно. Так же длина конечного вектора задана как pitch+yaw+pitch, что так же не правильно, надо вычислить длину вектора вот так sqrt(pitch**2+yaw**2+pitch**2)
Забавно, но мой быстрый говнокод кое как да работал, хотя если тангаж, крен и рыскание одновременно близко к 45 гр, моделька начинает вести себя не так как надо, а если управляющие силы заданы по отдельности, то моделька разворачивается как надо.
А вообще, физ модель квадрокоптера довольно простая, намного проще например самолёта, но от матана никуда.
Получается надо как-то так:

Код (Lua):

1.  
2. if pitch~=0 then
3.     dir_angular:rotate_angle90(axis_yaw, axis_pitch, math.atan(yaw/pitch))
4. end
5.  

Ну и ещё раз развернуть вектор по длине roll.
Пока не проверял, но думаю так правильно, наверное.

 

Intro, у реального объекта может бегать цм + ветровая нагрузка + по ходу разрядки батарей мощность на двиги падает неравномерно.

 

Это ерунда, сопротивления я решил вчера, есть три параметра:
local k_front, k_side, k_top = 0.01, 0.025, 0.06 --коэффициент трения передняя, боковая, верхняя. //coefficient of friction front, side, top
Текущая сила зависит от угла тангажа и крена, зависимость синусоидальная. Ну относительно скорости, а ветер просто сложение к вектору скорости, в общем я ветер не буду учитывать, может потом. Хотя максимальную тягу двигателя по мере разрядки батареи надо учитывать, но это элементарный код.
ЗЫ
На счёт сопротивления, поначалу была постоянная, но потом решил сделать переменная, код в голову пришёл не сразу, и думал сделать трассировкой модели, но это совсем сложно, точней затратно. И ночью как-то решил задачу в голове, утром написал код, и тот почти сразу заработал как мне надо.

 

главная трабла, что один и тот же код не подходит к разным моделям ==>> по-большей части тащит сноровка оператора..

 

Любой источник информации - достоверный. Если его не проверять.

 

В общем всё просто, надо лишь получить гипотенузу параллелепипеда, точней вектор гипотенузы, размер сторон параллелепипеда это и есть сила тангажа, рыскания и крена, а гипотенуза собственно и будет нужный вектор поворота, точней вектор угловой скорости. Так же необходима обратное вычисление от вектора угловой скорости параметров скорости тангажа, рыскания и крена, это работа гироскопа. Ну у нас модели полная физика, например в вертолёте эта физика частично отключена, у НПС и ГГ физика вообще своя... Забавно, тут мысля пришла в голову, а если игры где НПС персонажи с полной физикой? Вся анимация реализуется контролем физических костей модели персонажей, вроде как, в ГТА так сделано. Надо для реализации коллизий с живыми персонажами.