Ориентация звеньев ИМ относительно базовой СК

Ориентация СК звеньев относительно базовой СК устанавливается с ипотльзованием систем координат звеньев по матрицам t_0i. Первые столбцы этих матриц – проекции ортов осей X СК_i на оси базовой СК. Вторые столбцы - проекции ортов осей Y СК_i на оси базовой СК. Третьи - проекции ортов осей Z СК_i на оси базовой СК. Элементы матриц t_0j – косинусы углов между одноименными осями СК звеньев и базовой СК.

На практике не удобно определять относительную ориентацию осей СК, рассматривая значения косинусов углов между осями соответствующих СК. В этой связи для определения взаимной ориентации осей СК применяется параметры, более привычные для восприятия. Представление об окружающеей действитиельности человеку более удобно отображать в форме расстояний и углов. Выше мы получили формулы для расчета линейных координат точек на звеньях. Однако эти данные не полностью отражают положение звена ИМ МР в пространстве. Для полного представления о положении звена ИМ нужно определить также ориентацию СК звена относительно базовой СК в удобном для восприятия виде. В Теоретической Механике для этого предложено применять угловые координаты. Для определения ориентации звеньев СК звеньев относительно базовой СК применяются углы Эйлера.

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

Преобразование поворота подвижной СК{uvw} относительно трех осей инерциальной СК{xyz} представляется в виде композиции из трех элементарных поворотов:

- ось u, угол Pitch,

- ось v`, угол Yaw,

- ось w``, угол Roll (см. выше п.1.6.2):

 

t _{xyz, uvw} = t _{xyz, uvw} (ось u, угол Pitch) t _{xyz, uvw} (ось v`, угол Yaw) t _{xyz, uvw} (ось w``, угол Roll)

 

Таким образом для определения орентации СК_i относительно базовой СК₀ используются значения трех углов (R, Y, P).

 

Рассмотрим выражения для определения углов (R, Y, P). С одной стороны,

t _0i = t _0i (ось u, угол Pitch) t _0i (ось v`, угол Yaw) t _0i (ось w``, угол Roll).

 

С другой стороны,

 

_j | t₁₁ t₁₂ t₁₃ |

t_0j = П t_{k-1 k} = | t₂₁ t₂₂ t₂₃ | ,

^k=1 | t₃₁ t₃₂ t₃₃ |

причем значения t_{k-1 k} нам известны; поэтому известны и t_0j .

Полагая известными t_0i , найдем значения углов R, Y, P. Для этого вначале представим предпоследенее соотношение в виде

 

t _0i ^T (ось u, угол P) t _0i = t_0i (ось v`, угол Y) t_0i (ось w``, угол R).

 

Выполняя действия, предписанные последним соотношением, получим

 

| 1 0 0 | | t₁₁ t₁₂ t₁₃ | | cosY 0 sinY || cosR -sinR 0 |

| 0 cos P sinP | | t₂₁ t₂₂ t₂₃ | = | 0 1 0 || sinR cosR 0 | .

| 0 -sin P cos P | | t₃₁ t₃₂ t₃₃ | | -sinY 0 cosY || 0 0 1 |

 

 

Выполняя соответствующие действия, получим (для краткости знак подчеркивания опущен),

 

 

|t₁₁ t₁₂ t₁₃ | | cosYcosR -cosYsinR sinY|

|t₂₁ cosP+t₃₁ sinP t₂₂cos P+t₃₂sin P t₃₃sinP+t₂₃cosP| = | sinR cosR 0 |.

|t₃₂cosP-t₂₂sinP -t₂₂sinP+t₃₂cosP -t₂₃sinP+t₃₃cosP| |-sinYsinR sinYcosR cosY|

 

Здесь и ниже мы обозначили угол Р = угол P, Y = угол Y, R = угол R .

 

Сравнив элементы . ₃₂ матриц в левой и правой части, видим, что

 

t₂₃ cos P+ t₃₃ sin P = 0,

 

откуда

 

sin P / cos P = - t₂₃ ^/ t₃₃ и

 

Р = Arctg (- t_{2 3}^/ t₃₃).

 

Далее сравним элементы . ₁₃ и .₃₃ матриц:

 

sinY = t₁₃ , cosY = - t₂₃ sinP + t₃₃ cos P и

 

Y = Arctg (t₁₃ / (-t₂₃ sinP + t₃₃ cos P)).

 

Наконец, расмотрим элементы .₂₁ и .₂₂ :

 

sinR = t₂₁ cos P + t₃₁ sin P, cosR = t₂₂ cos P + t₃₂ sin P и

 

R = Arctg (( t₂₁ cos P + t₃₁ sin P) / (t₂₂ cos P + t₃₂ sin P)).

 

ПРИМЕР

1. Определить геометрические параметры звеньев ИМ КМР ERA, приведенного на рис.

 

 

 

Вводим СК звеньев как описано выше

 

- оси Х, - оси Y, - оси Z

 

n = 7, s_{1 – 7} = 1.

 

В соответствии с чертежом и введенными СК получим

 

l₁ = (0.5 0 0)^T l₂ = (0 1.0 0)^T l₃ = ( 0 4.15 0)^T

l₄ = (0 4.15 0)^T l₅ = (1.0 0 0)^T l₆ = (0 0 0.5)^T l₇ = (0 0 0.74)^T

a₁ = -90⁰_, b₁ = 0, g₁ = -90⁰ a₂ = -90⁰, b₂ = 0, g₂ = -90⁰

a₃ = b₃ = g₃ = 0⁰ a₄ = b₄ = g₄ = 0⁰ a₅ = 0⁰_, b₅ = 90⁰_, g₅ = 90⁰

a₆ = 0 , b₅ = 90⁰ _, g₆ = 90⁰ a₇ = b₇ = g₇ = 0⁰

2. Найдем выражения для расчета матриц перехода между СК звеньев

 

 

e₁ = t₁₀ (0) = (0-10 (100 = (001

100 00-1 100

001) 010) 010)

 

e_{2 =} t₂₁ (0) = e₁ = (001, 100, 010)

 

e₃ = t₃₂ (0) = E,

 

e₄ = t₄₃ (0) = Е,

 

e₅ = t₅₄ (0) = (010

100)

 

e₆ = t₆₅ (0) = e₅

 

e₇ = t₇₆ (0) = Е.

 

t₀₁ = (Cq₁ -Sq₁ 0 , Sq₁ Cq₁ 0 , 0 0 1 ) e^T₁ =

 

(Cq₁ -Sq₁ 0 (010 (0 Cq₁-Sq₁ ,

Sq₁ Cq₁ 0 001 0 Sq₁ Cq₁

0 0 1 ) 100) = 1 0 0 ).

 

 

t₁₂ = (Cq₂ -Sq₂ 0 , Sq₂ Cq₂ 0 , 0 0 1 ) e^T₂ =

(Cq₂ -Sq₂ 0 , Sq₂ Cq₂ 0 , 0 0 1 ) (010, 001, 100) =

(0 Cq₂ -Sq₂ , 0 Sq₂ Cq₂ 0 , 1 0 0 ).

 

t₂₃ = (Cq₃ -Sq₃ 0 , Sq₃ Cq₃ 0 , 0 0 1 ) e^T₃ =

(Cq₃ -Sq₃ 0 , Sq₃ Cq₃ 0 , 0 0 1 ) (100, 010, 001) =

(Cq₃ -Sq₃ 0 , Sq₃ Cq₃ 0 , 0 0 1 ).

 

t₃₄ = (Cq₄ -Sq₄ 0 , Sq₄ Cq₄ 0 , 0 0 1 ) e^T₄ =

(Cq₄ -Sq₄ 0 , Sq₄ Cq₄ 0 , 0 0 1 ) (100, 010, 001) =

(Cq₄ -Sq₄ 0 , Sq₄ Cq₄ 0 , 0 0 1 ).

 

Вычислим далее по аналогии

t₄₅ = (-Sq₅ 0 Cq₅ , Cq₅ 0 Sq₅ , 0 1 0 ).

t₅₆ = (-Sq₆ 0 Cq₆ , Cq₆ 0 Sq6 , 0 1 0 ).

t₆₇ = (Cq₇ -Sq₇ 0 , Sq₇ Cq₇ 0 , 0 0 1 ).

 

НИЖЕ ОШИБКА t₆₇

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

ПРОСЬБА ЗАМЕНИТЬ НА ФОРУЛЫ Е.ФЕОКТИСТОВОЙ

 

3. Найдем теперь выражения для расчета координат векторов, связывающих начала базовой СК и начала СК звеньев

 

t₁₍₀₎⁽⁰⁾ = t₀₁ l₁ = (0 0 0.5)^T

t₂₍₀₎⁽⁰⁾ = t₀₁ l₁ + t₀₂ l₂ = (0 0 0.5)^T + (Cq₁ Sq₂ Sq₁Sq₂ Cq₂)^T

t₃₍₀₎⁽⁰⁾ = t₀₁ l₁ + t₀₂ l₂ + t₀₃ l₃ = (0 0 0.5)^T + (Cq₁ Sq₂ Sq1Cq2 Cq₂)^T +

4.15 (Sq₁ Sq₃ + Cq₁ Sq₂ Cq₃ -Sq₃ Cq₁ + Sq₁ Sq₂ Cq₃ Cq₂ Cq₃)^T

 

t₄₍₀₎⁽⁰⁾ = t₀₁ l₁ + t₀₂ l₂ + t₀₃ l₃ + t₀₄ l₄ = (0 0 0.5)^T + (Cq₁ Sq₂ Sq₁Sq₂ Cq₂)^T +

4.15 (Sq₁ Sq₃ + Cq₁ Sq₂ Cq₃ -Sq₃ Cq₁ + Sq₁ Sq₂ Cq₃ Cq₂ Cq₃)^T +

4.15 ( (-Sq₁ Cq₃ + Cq₁ Sq₂ Sq₃) - Sq₄ + (Sq₄ Sq₃ + Cq₁ Sq₂ Cq₃) Сq₄)

(Cq₁Cq₃ + Sq₁ Sq₂ Sq₃) - Sq₄ + (- Sq₃Cq₁ + Sq₁ Sq₂ Cq₃) Cq₄

- Cq₂ Sq₃ Sq₄ + Cq₂ Cq₃ Cq₄ )^T

 

…

 

4. В некотором положении ИМ элементы матрицы t₀₇ имеют следующие значения

 

 

 

Вычислим значения углов Эйлера, заданных в последовательности поворотов СК₇ последовательно вокруг осей Х, Y, Z базовой СК.

В соответствии с приведенными выше формулами будем иметь

 

P = Arctg (- t₂₃ / t₃₃ ) = Arctg () = Arctg (1 / ) = 30⁰

 

 

Y = Arctg (t₁₃ / (- t₂₃ SinP + t₃₃ cos P)) = Arctg () =

Arctg () = Arctg () = 60⁰

 

R = Arctg (( t₂₁ cos P + t₃₁ sin P) / (t₂₂ cos P + t₃₂ sin P)) =

 

= Arctg ( = Arctg = Arctg() = 30⁰

 

 

ПРОВЕРКА

 

=