Поиск характеристик поворота по известным состояниям системы

[granit201z]

Помогите пожалуйста. Сломал всю голову уже. Есть робот, состоящий из 3-х сервов таким образом, что нижний серв крутит площадку на которой установлены два других серва (его положение можно задавать от 0 до 360). Средний крутит площадку на которой установлен последний серв (0-360). Последний крутит целевую площадку (0-180). Все три серва имеют взаимо-перпендикулярные оси. Состояние системы сервов я могу задавать вышеописанными 3-ми числами. Например, (250, 319, 156) или (217, 106, 180). каждый раз целевая площадка займет определенное угловое положение. Самое ее первое положение я могу задать некоторым произвольным базисом. А дальше, нужно меняя состояния системы сервов (три вышеописанных угла) отслеживать как будет меняться базис площадки при переходе от состояния к состоянию, чтобы всегда знать ее угловое положение. И вот собственно вопрос, как из паоследовательной пары состояний сервов получить характеристику поворота (матрицу, кватернион или что-то подобное этому) от одного состояния к другому

[Brukvalub]

Непонятно, как могут одновременно сочетаться два следующих утверждения:
1.

granit201z в сообщении #1414357 писал(а):

Состояние системы сервов я могу задавать вышеописанными 3-ми числами. Например, (250, 319, 156) или (217, 106, 180). каждый раз целевая площадка займет определенное угловое положение.

2.

granit201z в сообщении #1414357 писал(а):

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

Что такое: "угловое положение"? Видимо, это значения трех углов поворота сервов. Но из процитированного мной в п.1 следует, что каждое положение сервов заранее при управлении набором сервов задается именно тройкой значений углов поворота, после чего становится известным, что "целевая площадка займет определенное угловое положение".
Выходит, нужно " знать ее угловое положение", если оно и так уже известно?

[granit201z]

Brukvalub в сообщении #1414381 писал(а):

Непонятно, как могут одновременно сочетаться два следующих утверждения:
1.

granit201z в сообщении #1414357 писал(а):

Состояние системы сервов я могу задавать вышеописанными 3-ми числами. Например, (250, 319, 156) или (217, 106, 180). каждый раз целевая площадка займет определенное угловое положение.

2.

granit201z в сообщении #1414357 писал(а):

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

Что такое: "угловое положение"? Видимо, это значения трех углов поворота сервов. Но из процитированного мной в п.1 следует, что каждое положение сервов заранее при управлении набором сервов задается именно тройкой значений углов поворота, после чего становится известным, что "целевая площадка займет определенное угловое положение".
Выходит, нужно " знать ее угловое положение", если оно и так уже известно?

Как оказалось, я весьма слабо разбираюсь в вопросе. Но те, кто разбирается чуть лучше - подсказали, что нужно для каждых трех углов (состоянию системы сервов) получить матрицу поворота, соответствующую этим углам. А потом найти матрицу поворота между двумя этими матрицами. Это и будет то, что я назвал характеристикой поворота. Это так?

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

[optimist]

Так как никто больше не пишет, предложу, скорее всего неоптимальный, путь.
Надеюсь, также, что понял геометрию задачи правильно.
Введем Декартову систему координат, базисные вектора $\boldsymbol{e}_i$ направлены вдоль осей сервов в начальном положении (0,0,0).
Вращение первого мотора на угол $\varphi_1$ равносильно действию тензора поворота
$\boldsymbol{A}^1=A^1_{ij}\boldsymbol{e}_i\boldsymbol{e}_j,\qquad A^1_{ij}=\begin{pmatrix} \cos\varphi_1 & -\sin\varphi_1 & 0 \\ \sin\varphi_1 & \cos\varphi_1 & 0 \\ 0 & 0 & 1 \end{pmatrix}$
Все индексы пишу внизу, потому что работаем в ДСК.
Первый мотор повернул оси, в которых работает второй мотор, поэтому вращение второго мотора на угол $\varphi_2$ равносильно действию тензора поворота
$\boldsymbol{A}^2 = \hat{A}^2_{ij}\left( \boldsymbol{A^1} \cdot \boldsymbol{e}_i \right)\left( \boldsymbol{A^1} \cdot \boldsymbol{e}_j \right) = A^2_{ij}\boldsymbol{e}_i\boldsymbol{e}_j,\qquad \hat{A}^2_{ij}=\begin{pmatrix} \cos\varphi_2 & 0 & -\sin\varphi_2 \\ 0 & 1 & 0 \\ \sin\varphi_2 & 0 & \cos\varphi_2 \end{pmatrix}$
Компоненты $A^2_{ij}$ можно найти так:
$A^2_{kl} = \boldsymbol{e}_k \cdot \boldsymbol{A}^2 \cdot \boldsymbol{e}_l= \boldsymbol{e}_k \cdot \left( \hat{A}^2_{ij}\left( \boldsymbol{A^1} \cdot \boldsymbol{e}_i \right)\left( \boldsymbol{A^1} \cdot \boldsymbol{e}_j \right) \right) \cdot \boldsymbol{e}_l = \hat{A}^2_{ij} A^1_{ki} A^1_{lj}$
С третьим тензором поворота $\boldsymbol{A}^3$ по аналогии.
Тогда результирующий тензор поворота $\boldsymbol{B}=\boldsymbol{A}^3 \cdot \boldsymbol{A}^2 \cdot \boldsymbol{A}^1$.
Думаю, для вычислений вам понадобится матпакет.
Скорее всего в кватернионах можно сделать попроще.

[arseniiv]

granit201z в сообщении #1414388 писал(а):

И их углы уже не будут теми, которые были заданы, а должны будут считаться с этим первым поворотом?

Зависит от формулы. Если правильная, не должны будут. (Или как вы определяете те углы?) Вот у вас есть система координат. В простейшем случае вы поворачиваете её сначала вокруг одной из осей $x$, потом — новую — вокруг второй из осей $y'$ (результата поворота оси $y$ первой системы), и потом вторую новую систему координат — вокруг оси $z''$. Для выражения того как связаны две соседние системы координат в этой последовательности не нужно запоминать, что с ними было раньше, так что последнюю третью систему можно получить композицией трёх линейных преобразований, выражающих орты новой системы через старые (или наоборот, смотря в какую сторону нам надо). Могут быть повороты не вокруг $x, y', z''$, а вокруг например $z, y', z''$, и как у вас, я не знаю. Может статься, один из поворотов будет вообще не вокруг одной из осей системы координат, тогда вид матрицы этого отдельного поворота усложняется (но гуглится).

-- Вт сен 10, 2019 16:46:12 --

optimist в сообщении #1414392 писал(а):

Скорее всего в кватернионах можно сделать попроще.

Ну, вычислений там тоже будет не совсем мало — для произвольной оси косинус-синус считать, всё такое (хотя для $\vec\imath,\vec\jmath,\vec k$ они, конечно, сильно упрощаются). Притом если потом надо будет повернуть много-много векторов, оптимальнее будет перед этим превратить кватернион в матрицу поворота. Хотя удобно, что поворот выражается алгебраически.

Для granit201z: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation.

[granit201z]

спасибо всем

[Утундрий]

Если повороты проводятся строго последовательно, то нужно тупо перемножить три матрицы (или кватерниона) известного вида. Если же повороты можно проводить одновременно с произвольными (пусть просто разными) угловым скоростями, то задачка становится интересней. Можно что-то оптимизировать...

[arseniiv]

Да вроде там ничего особо не оптимизируется? Это же не SLERP (где да).

Ещё наверно перед ТС где-то в будущем стоит задача поворачивать некую штуку так, чтобы она стала в заданной ориентации — тогда задача тоже хитрая в общем случае. Для тех же углов Эйлера её решение должно много где попадаться, но если углы другие, то…

[Утундрий]

Давайте дождём реакции ТС.

[granit201z]

Утундрий в сообщении #1414436 писал(а):

Если повороты проводятся строго последовательно, то нужно тупо перемножить три матрицы (или кватерниона) известного вида. Если же повороты можно проводить одновременно с произвольными (пусть просто разными) угловым скоростями, то задачка становится интересней. Можно что-то оптимизировать...

Можно вращать последовательно, но сейчас они крутятся одновременно. Я не предполагал, что это изменит решение. Кстати идея с получением двух матриц по паре троек углов, описывающих предыдущее и последующее состояния сервов провалилась. Опишу задачу в большем количестве деталей. На "целевой" площадке стоят датчики (магнитометр, акселерометр и гироскоп). Стоят весьма произвольно таким образом, что мне неизвестен переход от их локальной системы к системе сервов. Да и что собой представляет система сервов весьма смутно представляю - просто я выбрал их крайние положения за нули, а противоположные крайние положения за 360, 360 и 180 соответственно. С датчиков я могу снимать показания в любой момент времени. Показания - сыроваты (некалиброванные) и оттого не совсем точные. Цель - получив некоторое множество показаний (сейчас речь про акселерометр, который в любом установившемся состоянии сервов будет показывать вектор, норма которого должна быть 9,81) в каком либо положении, усреднив их и скрепя сердце поверив этому усреднению - остальные показания вычислять из поворотов целевой платформы и сверять с реальными показаниями датчиков.


Использовав следующую матрицу:

$$\begin{bmatrix} \cos\alpha\cdot \cos\gamma - \sin\alpha\cdot \cos\beta\cdot\sin\gamma & -\cos\alpha\cdot \sin\gamma - \sin\alpha\cdot \cos\beta\cdot\cos\gamma & \sin\alpha\cdot \sin\beta\\ \sin\alpha\cdot \cos\gamma + \cos\alpha\cdot \cos\beta\cdot\sin\gamma & -\sin\alpha\cdot \sin\gamma + \cos\alpha\cdot \cos\beta\cdot\cos\gamma & \cos\alpha\cdot \sin\beta\\ \sin\beta\cdot\sin\gamma & \sin\beta\cdot\cos\gamma & \cos\beta\ \end{bmatrix}$$

подставляя в качестве $\alpha, \beta, \gamma$ углы, которыми я описываю состояния сервов - я для этих состояний по этой формуле нашел матрицы якобы им соответствующие, нашел матрицу, описывающую поворот от первой ко второй. Применил этот поворот к вектору акселерометра, соответствующего первому состоянию. И должен был бы по задумке таким образом предугадать вектор акселерометра, соответствующий последующему состоянию. Но не тут то было.

Ко мне закрадываются смутные подозрения, что я абсолютно неправильно представляю себе систему координат серво-робота, выражая ее через углы поворота сервов каждый вокруг своей физической оси

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

[Утундрий]

granit201z в сообщении #1414499 писал(а):

Можно вращать последовательно, но сейчас они крутятся одновременно. Я не предполагал, что это изменит решение.

Тогда результат будет сильно зависеть от того как именно они крутятся.

Опишите механику агрегата несколько подробнее. Если он, конечно, не военный.

[wrest]

granit201z в сообщении #1414499 писал(а):

Но я пока не переварю, что с ним дальше делать

Традиционно, нужна картинка что вокруг чего вращается.

[granit201z]

Утундрий в сообщении #1414502 писал(а):

Опишите механику агрегата несколько подробнее. Если он, конечно, не военный.

wrest в сообщении #1414503 писал(а):

нужна картинка что вокруг чего вращается.

Нижний серв вращает площадку со средним сервом. Средний вращает площадку с верхним сервом. верхний непосредственно вращает датчики

[Утундрий]

О как всё эксцентричненько!

Ну, во-первых, кроме углов здесь нужны и размеры. А во-вторых: куда ему, собственно, нужно датчиками дотянуться и как они при этом должны быть повёрнуты? Всё это должно войти в условие задачи, а то математика не запустится.

[Brukvalub]

В целом, все как обычно: вы, парни, попотейте над той чушью, что я вам тут зарядил,, а потом я вам, возможно, скажу, что все не так, как раньше, и зря вы так сильно потели, но задача совсем другая, и все крутится совсем не так, и вокруг совсем других осей, но вы все большие молодцы!