- •Введение
- •1. Основные понятия и определения.
- •1.2. Кинематические пары и их классификация.
- •1.3. Кинематические цепи.
- •1.4. Краткие сведения из теории матриц.
- •2. Создание алгоритма решения прямой задачи о положениях.
- •2.1. Определение числа степеней подвижности ok
- •2.1.2. Определение числа степеней подвижности для кинематической структуры манипулятора ПР модели М20П.40.01.
- •2.1.3. Определение числа степеней подвижности для кинематической структуры манипулятора ПР модели М10П.62.01.
- •2.1.4. Определение числа степеней подвижности для кинематической структуры манипулятора ПР.
- •2.2. Назначение собственных систем координат.
- •2.2.1. Общие принципы назначения собственных систем координат.
- •2.2.2. Назначение собственных систем координат при решении задачи для кинематической структуры манипулятора ПР мод. М20П.40.01.
- •2.2.3. Назначение собственных систем координат при решении задачи для кинематической структуры манипулятора ПР мод. М10П.62.01.
- •2.2.4. Назначение собственных систем координат при решении прямой задачи о положениях для кинематической структуры ПР.
- •2.3. Преобразования систем координат. (edit)
- •2.3.1. Общие принципы преобразования систем координат.
- •2.3.2. Преобразования систем координат для кинематической структуры манипулятора ПР модели М20П.40.01.
- •2.3.3. Преобразования систем координат для кинематической структуры манипулятора ПР модели М10П.62.01.
- •2.3.4. Преобразования систем координат для кинематической структуры манипулятора ПР.
- •3. Реализация вычислительного алгоритма на ЭВМ.
- •3.1. Работа с системой MathCAD.
- •3.1.1. Общее описание системы и ее запуск.
- •3.1.2. Общие приемы работы в среде.
- •3.1.3. Работа с векторами и матрицами.
- •3.2. Тестирование алгоритма.
- •3.2.1. Классификация ошибок.
- •3.2.2. Проверка правильности решения прямой задачи о положениях манипулятора ПР.
- •Список литературы
- •Приложения
- •Решение прямой задачи о положениях для кинематической структуры манипулятора ПР модели М20П.40.01.
- •Решение прямой задачи о положениях для кинематической структуры манипулятора ПР модели М10П.62.01.
- •Решение прямой задачи о положениях для кинематической структуры манипулятора ПР
- •Варианты заданий.
|
|
|
|
|
|
|
|
|
|
|
æ1 |
|
0 |
|
|
0 |
|
0 öæ1 |
0 |
0 0 |
öæ1 0 0 0 ö |
|||||||||||||
|
|
|
|
|
|
|
|
|
|
|
ç |
0 |
cosϕ1 -sinϕ1 |
0 |
֍ |
0 |
|
0 |
|
1 |
0 |
֍ |
0 |
1 |
0 |
0 |
÷ |
|||||||
M |
4→0 |
= M |
2→1 |
×M |
3→2 |
× M |
4→3 |
= ç |
֍ |
|
|
֍ |
÷ . |
|||||||||||||||||||||
|
|
|
|
|
ç |
0 |
sinϕ1 |
|
cosϕ1 |
|
s2 |
֍ |
0 |
|
-1 |
0 |
L2 |
֍ |
0 |
0 |
1 |
s3 |
÷ |
|||||||||||
|
|
|
|
|
|
|
|
|
|
|
ç |
0 |
|
0 |
|
|
0 |
|
1 |
֍ |
0 |
0 |
0 1 |
֍ |
0 |
0 0 1 |
÷ |
|||||||
|
|
|
|
|
|
|
|
|
|
|
è |
|
|
|
|
øè |
øè |
ø |
||||||||||||||||
|
|
Выражение |
|
A0 |
= M4→0 × A4 |
для нахождения координат точки А относительно |
||||||||||||||||||||||||||||
системы 0 в развернутом виде выглядит следующим образом: |
|
|
|
|
|
|
|
|||||||||||||||||||||||||||
|
æ xA ö |
|
æcosϕ1 |
-sinϕ1 |
0 |
|
0 |
öæ1 |
|
0 |
0 |
0 |
öæ |
1 |
|
0 |
0 |
0 |
öæ0 |
×cosϕ4 |
ö |
|
|
|||||||||||
|
ç |
|
÷ |
= |
ç |
|
|
|
cosϕ1 |
0 |
|
0 |
֍ |
0 |
|
0 |
1 |
0 |
֍ |
0 |
|
1 |
0 |
0 |
֍ |
×sinϕ4 |
÷ |
|
|
|||||
|
ç yA ÷ |
ç sinϕ1 |
|
|
֍ |
|
֍ |
|
֍ 0 |
÷ . |
|
|
||||||||||||||||||||||
|
ç z |
÷ |
|
ç |
0 |
|
|
0 |
1 |
s |
֍ |
0 |
-1 0 L |
֍ |
0 |
0 |
1 |
s |
֍ |
L |
|
÷ |
|
|
||||||||||
|
ç |
|
A ÷ |
|
ç |
0 |
|
|
0 |
0 |
|
2 |
֍ |
0 |
0 |
0 |
2 |
֍ |
0 |
0 |
0 |
3 |
֍ |
|
4 |
|
÷ |
|
|
|||||
|
è |
1 ø |
|
è |
|
|
1 |
øè |
1 |
øè |
1 |
øè |
|
1 |
|
ø |
|
|
Таким образом, прямая задача о положениях решена, поскольку найдено урав- нение, связывающее постоянные (длины звеньев) и переменные (обобщенные коор- динаты) параметры кинематической структуры3 манипулятора ПР с координатами точки схвата.
Последнее, о чем хотелось бы упомянуть в этом пункте связано с многовари- антностью преобразования соседних систем координат. В целях повышения едино-
образия решения прямой задачи о положениях рекомендуется сначала выполнять операции учета линейных смещений, а затем – угловых.
2.3.3. Преобразования систем координат для кинематической структуры манипулятора ПР модели М10П.62.01.
Координаты точки А в собственной системе координат звена 4 из-за отсутствия эксцентриситета в общем виде принимают значения: A4 (0×cosϕ4 ,0×sinϕ4 , L4 ) (рис.
2.5, б). Здесь величина L4 характеризует длину звена 4. Для вычисления координат этой же точки, но в системе 0 необходимо найти законы перехода от системы 4 к системе 3, затем от 3й ко 2й, от 2й к 1й и от 1й к 0й, которые математически описыва- ются с помощью матриц.
Начиная с перехода от 4й к 3й системе, заметим, что точки начал указанных систем не совпадают, что свидетельствует о наличии линейных смещений. Кроме этого, оси у4 и у3 сонаправлены, в то время как остальные имеют угловое смещение (рис. 2.5, б). Для описания перехода учтем указанные факторы.
Величины линейных смещений совпадают с координатами точки В относи- тельно системы 3: B (L3 ,0,0) (рис. 2.15, а). Угловое же смещение происходит во-
круг оси у на угол 90°.
В этом случае также возможны варианты последовательности преобразования
3 Эти параметры задаются преподавателем индивидуально.
36
(рис. 2.15, б–в). Рассмотрим а) |
б) |
в) |
|
последовательность4, изо- |
|
|
|
браженную на рис. 2.15, б. |
|
|
|
В этом случае матрица пе- |
|
|
|
рехода получается |
пере- |
|
|
множением матриц |
пере- |
|
|
мещения вдоль оси х и по- |
Рис. 2.15. Переход от системы 4 к системе 3. |
|
|
ворота вокруг оси у: |
|
|
|
æ1 |
0 |
0 |
L3 |
|
|
|
ç |
0 |
1 |
0 |
0 |
M |
4→3 |
= ç |
||||
|
ç |
0 |
0 |
1 |
0 |
|
|
|
ç |
0 |
0 |
0 |
1 |
|
|
è |
ö |
æ |
cos(90o ) |
÷ |
ç |
0 |
÷ |
ç |
|
×ç |
-sin (90o ) |
|
÷ |
ç |
|
÷ |
ç |
0 |
ø |
è |
0 |
sin (90o ) |
0 |
ö |
æ 0 |
0 |
1 |
L |
ö |
|
1 |
0 |
0 |
÷ |
ç |
0 |
1 |
0 |
3 |
÷ |
÷ |
= ç |
0 |
÷ . |
||||||
0 |
cos(90o ) |
0 |
÷ |
-1 |
0 |
0 |
0 |
||
÷ |
ç |
÷ |
|||||||
0 |
0 |
1 |
÷ |
ç |
0 |
0 |
0 |
1 |
÷ |
ø |
è |
|
|
|
|
ø |
При переходе от 3й ко 2й системе координат (рис. 2.16, а), заметим, что посту- пательное движение в паре 3 происходит вдоль сонаправленных осей z2 и z3 и влия- ет лишь на линейное смещение точек начал указанных систем вдоль оси z. В то же время обобщенная координата ϕ2 влияет не только на угловое смещение осей х и у этих двух систем, но и на координаты точки начала системы 3 относительно систе- мы 2.
a) |
б) |
в) |
Рис. 2.16. Переход от системы 3 к системе 2.
В данном случае при преобразовании систем координат рассмотрим следую- щие варианты: во-первых можно ввести в рассмотрение воображаемое звено длиной
L′ = R22 + L22 , отклоненное от элемента R2 на угол α = arctan(L2 / R2 ) (рис. 2.16, б);
во-вторых можно ввести вспомогательную систему координат в точке излома R2– L2
звена 2 (рис. 2.16, в).
Начнем с первого варианта. Величины линейных смещений системы 3 относи-
4 В дальнейшем при наличии нескольких вариантов будет рассматриваться только один, учитывающий сначала линейные смещения, а затем – угловые. Оставшиеся варианты любознательный читатель может проверить самостоятельно.
37
тельно системы 2
C (L¢cos(ϕ2 +α ), L¢sin (ϕ2
щений будет иметь вид:
M
найдем |
с |
помощью |
координат |
точки |
+α ), s3 ) . Следовательно, матрица учета линейных сме-
|
æ1 |
0 |
0 |
L¢cos(ϕ2 |
+α )ö |
|
||
|
ç |
0 |
1 |
0 |
L¢sin (ϕ2 |
÷ |
|
|
= |
ç |
+α )÷ |
. |
|||||
ç |
0 |
0 |
1 |
s |
÷ |
|||
|
|
|||||||
|
ç |
|
|
|
3 |
÷ |
|
|
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
|
|
è |
ø |
|
Для завершения перехода между системами необходимо учесть угловое сме- щение на величину (ϕ2+π/2) вокруг оси z, что описывается матрицей:
|
æcos(ϕ2 +π / 2) |
-sin(ϕ2 +π / 2) |
0 |
0 |
ö |
|
|
ç |
+π / 2) |
cos(ϕ2 +π / 2) |
0 |
0 |
÷ |
M ¢ = |
ç sin (ϕ2 |
÷ |
||||
ç |
0 |
0 |
1 |
0 |
÷ . |
|
|
ç |
|
|
|
|
÷ |
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
è |
ø |
Полный переход от системы 3 к системе 2 происходит с помощью матрицы:
|
æcos(ϕ2 +π / 2) |
-sin(ϕ2 +π / 2) |
0 |
L¢cos(ϕ2 |
+α )ö |
|
|
ç |
+π / 2) |
cos(ϕ2 +π / 2) |
0 |
|
÷ |
M3→2 = M ×M ¢ = |
ç sin (ϕ2 |
L¢sin (ϕ2 +α )÷ |
||||
ç |
0 |
0 |
1 |
s3 |
÷ . |
|
|
ç |
÷ |
||||
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
è |
ø |
Теперь рассмотрим второй вариант. Введем вспомогательную систему коорди- нат х2'y2'z2' на изломе R2– L2 звена 2. Оси этой системы сонаправим с одноименными координатными векторами системы 3. Тогда переход от системы 3 к х2'y2'z2' проис- ходит с помощью матрицы учета линейных смещений:
æ1 |
0 |
0 |
L |
ö |
|
ç |
|
|
|
2 |
÷ |
M = ç |
0 |
1 |
0 |
0 |
÷ . |
ç |
0 |
0 |
1 |
s |
÷ |
ç |
0 |
0 |
0 |
3 |
÷ |
è |
1 |
ø |
В свою очередь, переход от координат системы х2'y2'z2' к координатам системы 2 происходит путем учета линейных смещений вдоль осей х и у на величины R2 cosϕ2 и R2 sinϕ2 соответственно. Кроме этого, необходимо учесть угловое сме-
щение вокруг оси z на величину (ϕ2+π/2). В результате проведения указанных опе- раций получим матрицу:
38
|
|
|
|
|
æcos |
(ϕ2 +π / 2) |
-sin(ϕ2 +π / 2) |
0 |
R2 cosϕ2 |
ö |
|
|
|
|
|
|||
|
|
|
¢ |
|
ç |
|
cos(ϕ2 +π / 2) |
|
0 |
R2 sinϕ2 |
÷ |
|
|
|
|
|
||
|
|
M |
|
ç sin (ϕ2 +π / 2) |
|
÷ |
|
|
|
|
|
|||||||
|
|
|
== ç |
0 |
0 |
|
1 |
0 |
|
|
÷ . |
|
|
|
|
|||
|
|
|
|
|
ç |
|
|
|
|
|
|
|
÷ |
|
|
|
|
|
|
|
|
|
|
ç |
0 |
0 |
|
0 |
1 |
|
|
÷ |
|
|
|
|
|
|
|
|
|
|
è |
|
|
|
ø |
|
|
|
|
|
||||
Полная конвертация координат системы 3 в координаты системы 2 при таком |
||||||||||||||||||
варианте происходит с помощью матрицы: |
|
|
|
|
|
|
|
|
|
|
|
|||||||
|
|
|
|
|
æcos(ϕ2 +π / 2) |
-sin (ϕ2 +π / 2) |
0 |
|
R2 cosϕ2 |
ö |
|
æ1 |
0 |
0 |
L2 |
ö |
||
|
¢ |
|
|
|
ç sin (ϕ2 +π / 2) |
cos(ϕ2 +π / 2) |
0 |
|
R2 sinϕ2 |
÷ |
|
ç |
0 |
1 |
0 |
0 |
÷ |
|
M3→2 = M |
×M |
= |
ç |
|
|
|
|
|
÷ |
× |
ç |
|
|
|
|
÷ |
||
|
ç |
0 |
0 |
1 |
|
0 |
÷ |
ç |
0 0 1 s3 |
÷ . |
||||||||
|
|
|
|
|
ç |
|
÷ |
|
ç |
÷ |
||||||||
|
|
|
|
|
ç |
0 |
0 |
0 |
|
1 |
÷ |
|
0 0 0 1 |
|||||
|
|
|
|
|
è |
|
ø |
|
è |
ø |
Следует отметить, что в приложении 2 приведен именно последний вариант преобразования координат.
Переход от 2й к 1й системе координат (рис. 2.17, а) происходит с помощью учета постоянного линейного смещения на величину L1 (длина звена 1) вдоль оси z, а также учета угловых смещений, одно из которых получается из-за перпендику- лярности осей вращательных кинематических пар 1 и 2 (оно постоянно), а другое обеспечивается обобщенной координатой ϕ1. Для удобства пояснения хода преоб- разований введем в рассмотрение две вспомогательные системы х'y'z' и х''y''z'', на- чинающиеся в точке D. Причем координатные вектора первой из них сонаправлены с одноименными в системе 1 (другими словами система х'y'z' получается путем ли- нейного смещения системы 1). Система х''y''z'' получается путем поворота системы х'y'z' вокруг оси z на угол ϕ1. И наконец, система координат 2 получается из систе- мы х''y''z'' поворотом последней вокруг оси у на 90°.
Начнем |
по по- а) |
б) |
||
рядку с перехода от |
|
|||
системы 2 к системе |
|
|||
х''y''z''. |
Матрица, |
|
||
учитывающая |
угло- |
|
||
вое смещение вокруг |
Рис. 2.17. Переход от системы 2 к системе 1. |
|||
оси у на |
угол 90°, |
|||
|
обусловленное перпендикулярностью осей вращения кинематических пар 1 и 2, имеет вид:
æ |
cos(90o ) |
ç |
0 |
ç |
|
M ¢¢ = ç |
-sin (90o ) |
ç |
|
ç |
0 |
è |
0 |
sin (90o ) |
0 |
ö |
æ 0 |
0 |
1 |
0 |
ö |
|
1 |
0 |
0 |
÷ |
ç |
0 |
1 |
0 |
0 |
÷ |
÷ |
= ç |
÷ . |
|||||||
0 |
cos(90o ) |
0 |
÷ |
-1 |
0 |
0 |
0 |
||
÷ |
ç |
÷ |
|||||||
0 |
0 |
1 |
÷ |
ç |
0 |
0 |
0 |
1 |
÷ |
ø |
è |
|
|
|
|
ø |
|||
|
39 |
|
|
|
|
|
|
|
|
Далее для перехода от системы х''y''z'' к системе х'y'z' необходимо учесть пере- менное угловое смещение, обусловленное наличием обобщенной координаты ϕ1, регламентирующей угол ротации в кинематической паре 1. Как было указано выше ротация происходит вокруг оси z, следовательно матрица примет вид:
|
æcosϕ1 |
-sinϕ1 |
|
0 |
0 |
ö |
|||
|
ç |
|
|
cosϕ1 |
|
0 |
0 |
÷ |
|
M ¢ = |
ç sinϕ1 |
|
÷ |
||||||
ç |
0 |
|
|
0 |
|
1 |
0 |
÷ . |
|
|
ç |
0 |
|
|
0 |
|
0 |
1 |
÷ |
|
è |
|
|
|
ø |
||||
И наконец, последний переход от х'y'z' к линейно смещенной системе 1. Коор- |
|||||||||
динаты точки D(0;0; L1 ) , значит без лишних раздумий ясна необходимость исполь- |
|||||||||
зования следующей матрицы: |
|
|
|
|
|
|
|
|
|
|
|
æ1 |
0 |
0 |
0 |
ö |
|
|
|
|
|
ç |
0 |
1 |
0 |
0 |
÷ |
|
|
|
M = ç |
÷ . |
|
|
|||||
|
|
ç |
0 |
0 |
1 |
L |
÷ |
|
|
|
|
ç |
0 |
0 |
0 |
1 |
÷ |
|
|
|
|
è |
1 |
ø |
|
|
Окончательно переход от координат в системе 2 к их аналогам в системе 1 по- лучается при использовании расширенной матрицы преобразования, полученной при перемножении матриц промежуточных переходов в соответствии с (2,10):
æ1 |
0 |
0 |
0 |
ö |
æcosϕ1 |
-sinϕ1 |
0 |
0ö |
|
æ |
0 |
0 |
1 |
0 |
ö |
|||
ç |
0 |
1 |
0 |
0 |
÷ |
ç |
|
cosϕ1 |
0 |
0 |
÷ |
|
ç |
0 |
1 |
0 |
0 |
÷ |
ç |
÷ |
ç sinϕ1 |
÷ |
|
ç |
÷ |
||||||||||||
M2→1 = M ×M ¢× M ¢¢ = ç |
0 |
0 |
1 |
L |
÷ |
×ç |
0 |
0 |
1 |
0 |
÷ |
× |
ç |
-1 |
0 |
0 |
0 |
÷ . |
ç |
0 |
0 |
0 |
1 |
÷ |
ç |
0 |
0 |
0 |
1 |
÷ |
|
ç |
0 |
0 |
0 |
1 |
÷ |
è |
1 |
ø è |
ø |
|
è |
ø |
||||||||||||
Последний переход от 1й к 0й системе координат (рис. |
|
|
|
|
|
|
|
|||||||||||
2.18) не столь сложен как предыдущий: достаточно учесть |
|
|
|
|
|
|
|
|||||||||||
лишь угловое смещение вокруг оси х на угол –90°. Линейное |
|
|
|
|
|
|
|
|||||||||||
смещение отсутствует, поскольку точка О общая для указан- |
|
|
|
|
|
|
|
|||||||||||
ных систем и ее координаты в |
системе 0 |
примут |
|
вид: |
|
|
|
|
|
|
|
|||||||
O (0;0;0) . Последнее означает, что матрица линейных сме- |
|
|
Рис. 2.18. Переход от |
|||||||||||||||
щений выродится в единичную и никоим образом не по- |
|
системы 1 к системе 0. |
влияет на ход преобразования систем координат. Следовательно, матрица перехода имеет вид:
|
æ1 |
0 |
0 |
0ö |
æ1 0 |
0 |
0 |
ö |
||||
|
ç |
0 |
cos(-90o ) |
-sin (-90o ) |
0 |
÷ |
ç |
0 |
0 |
1 |
0 |
÷ |
|
ç |
÷ |
||||||||||
M1 0 = ç |
|
sin (-90o ) |
cos(-90o ) |
|
÷ |
= ç |
|
|
|
|
÷ . |
|
→ |
ç |
0 |
0÷ |
ç |
0 |
-1 |
0 |
0 |
÷ |
|||
|
ç |
0 |
0 |
0 |
1 |
÷ |
ç |
0 |
0 |
0 |
1 |
÷ |
|
è |
ø |
è |
|
|
|
|
ø |
||||
|
|
|
|
40 |
|
|
|
|
|
|
|
|