Решение прямой задачи кинематики KUKA youBot

Мобильный манипулятор KUKA youBot на рис. 16 состоит из установленных на платформе пяти последовательно соединённых звеньев, связанных соединениями вращательного типа с платформой и между собой.

Рис. 16 Схема мобильного манипулятора KUKA youBot

Будем считать, что звено 1 – твёрдое тело, вращающееся вокруг вертикальной оси С₁Z₁, точка С₁ принадлежит основанию платформы, звенья 2, 3, 4 – стержни, которые вращаются вокруг горизонтальных осей, проходящих через точки C₂, C₃, C₄ соответственно. Звено 5 – твердое тело, ладонь, вращается вокруг оси С₅z, направленной вдоль звена 4. На рис. 16 ладонь представлена отрезком С₅С'₅ оси С_5z. Со звеном 5 в точке С'₅ соединён схват манипулятора в виде двух пальцев, которые могут перемещаться поступательно относительно звена 5, точка E на оси С₅z – крайняя точка схвата при сомкнутых пальцах. Платформа – твёрдое тело с горизонтальным верхним основанием, перемещающееся по неподвижной горизонтальной плоскости с помощью четырёх меканум колёс. Звенья 1–4 и ось вращения звена 5 расположены в одной вертикальной плоскости.

Введём системы координат:

OXYZ – неподвижная, абсолютная, ось OZ вертикальна, горизонтальная плоскость OXY расположена в плоскости верхнего основания платформы;

СX_PY_PZ_P – подвижная, жёстко связанная с платформой, C – фиксированная точка верхнего основания платформы, ось СZ_P вертикальна;

Exyz – жёстко связана со схватом, вращается вокруг оси С₅z;

С₁X₁Y₁Z₁ – вращается вокруг вертикальной оси С₁Z₁ вместе со звеном 1, ось С₁X₁ направлена от оси С₁Z₁ к проекции точки C₂ на плоскость OXY.

Обозначим:

h – расстояние между точками С и С₁; a₁ – расстояние между точкой C₁ и проекцией точки C₂ на горизонтальную плоскость; d₁ – расстояние между точкой C₁ и проекцией точки C₂ на вертикальную ось С₁Z₁, a₂ = |C₂C₃|, a₃ = |C₃C₄|, a₄ = |C₄C₅|, a₅ = |C₅C'₅|, a₆ = |C'₅E| .

Положение платформы и руки манипулятора определим с помощью обобщённых координат X_C,Y_C, θ, θ_i, i = 1 ⁢ − ⁢ 5 , X_C, Y_C - координаты точки C относительно осей OX, OY, θ - угол между осями OX и СX_P, θ₁ - угол между осями СX_P и С₁X₁, θ₂ − ⁢ угол от оси С₁Z₁ до вектора C₂C₃, θ₃ – угол от вектора C₂C₃ до вектора C₃C₄, θ₄ – угол от вектора C₃C₄ до вектора C₄C₅ , θ₅ – угол поворота звена 5 вокруг оси С₅z.

За положительное направление отсчёта углов в координатных плоскостях OXY, С₁X₁Z₁, Exy примем направление против часовой стрелки, если смотреть из положительного направления осей OZ, С₁Y₁, С₅z соответственно [26]. Числовые значения углов должны удовлетворять [22] техническим ограничениям:

-169⁰ ≤ θ₁ ≤ 169⁰, − ⁢ 65⁰ ≤ θ₂ ≤ 90⁰ , − ⁢151⁰ ≤ θ₃ ≤ 146⁰, − ⁢102,5⁰ ≤ θ₄ ≤ ⁢102,5⁰, − 167,5⁰ ≤ θ₅ ≤167,5⁰.

При решении прямой задачи кинематики считаются известными обобщённые координаты X_C,Y_C, θ, θ_i, i = 1 ⁢ − ⁢ 5. Требуется определить параметры локации рабочего органа, за которые примем координаты X_E,Y_E, Z_E точки E относительно системы координат OXYZ и углы Эйлера ψ, θ_E, φ [26], задающие ориентацию базиса системы координат Exyz относительно базиса системы OXYZ. Интервалы изменения этих углов определяются неравенствами

0 ≤ ψ < 2 ⁢ π , 0 ≤ θ_E ≤ π , 0 ≤ φ < 2 ⁢ π.

Из рис. 16 видно

X_E= X_C + h ⁢ cosθ + cos⁡( θ + θ₁ ) ⁢( a₁ + a₂ ⁢ sinθ₂ + a₃ ⁢ sin( θ₂ + θ₃ ) + ( a₄ + a₅ + a₆ ) ⁢ sin( θ₂ + θ₃ + θ₄ ) ) ,

Y_E = Y_C + h ⁢ sinθ + sin( θ + θ₁ ) ⁢ ( a₁ + a₂ ⁢ sinθ₂ + a₃ ⁢ sin( θ₂ + θ₃ ) + ( a₄ + a₅ + a₆ ) ⁢ sin( θ₂ + θ₃ + θ₄ ) ) ,

Z_E = d₁ + a₂ ⁢ cosθ₂ + a₃ ⁢ cos( θ₂ + θ₃ ) + ( a₄ + a₅ + a₆ ) ⁢ cos( θ₂ + θ₃ + θ₄ )

Углы Эйлера и обобщённые координаты связаны соотношениями:

если 0 < θ₂ + θ₃ + θ₄ < π , то

ψ = θ + θ₁ + π /2 ,

θ_E = θ₂ + θ₃ + θ₄,

φ = θ₅ ⁢ − ⁢ π /2 ;

если 0 < θ₂ + θ₃ + θ₄ >π , то

ψ =θ + θ₁ − π /2 ,

θ_E = 2 ⁢ π ⁢ − ⁢ ( θ₂ + θ₃ + θ₄ ) ,

φ = θ₅ ⁢ + ⁢ π /2 ;

если -π < θ₂ + θ₃ + θ₄ < 0 , то

ψ =θ + θ₁ − π /2 ,

θ_E = ⁢ − ⁢ ( θ₂ + θ₃ + θ₄ ) ,

φ = θ₅ ⁢ + ⁢ π /2 ;

если θ₂ + θ₃ + θ₄ < -π , то

ψ =θ + θ₁ + π /2 ,

θ_E = ⁢2 ⁢ π + ⁢ θ₂ + θ₃ + θ₄ ,

φ = θ₅ ⁢ − ⁢ π /2 ;

если θ₂ + θ₃ + θ₄ = 0 , то

ψ + φ = θ + θ₁ + θ₅ ,

θ_E = ⁢0;

если θ₂ + θ₃ + θ₄ =±π, то

ψ + φ = θ + θ₁ − θ₅ + π,

θ_E = ⁢π;

В несингулярных случаях, оси Ez и OZ непараллельны, т. е. θ₂ + θ₃ + θ₄ ≠ 0 и θ₂ + θ₃ + θ₄ ≠ ±π, каждый из углов ψ, θ_E, φ может быть вычислен по представленным выше формулам. При этом выполняются условия:

ψ ∈ [ − ⁢336,5 ⁢ π /180 , 3 ⁢π + 336,5 π /180 ] , θ_E ∈ ( 0, π ) , φ ∈ [ − ⁢257,5 ⁢ π / 180 , 257,5 π /180 ].

Если же оси Ez и OZ параллельны (сингулярные случаи), т. е. θ₂ + θ₃ + θ₄ = 0 или θ₂ + θ₃ + θ₄ = ±π, то возможно нахождение только суммы

ψ + φ ∈ [ − ⁢156,5 ⁢ π /180 , 876,5 π /180 ].

Реализовать вычисления по приведённым соотношениям можно, обратившись к представленной на следующей странице функции FK на языке системы Mathematica [27].