Министерство образования и науки Российской Федерации

Калужский филиал федерального государственного бюджетного

образовательного учреждения высшего профессионального
образования
«Московский государственный технический университет
имени Н.Э. Баумана»
(национальный исследовательский университет)
(КФ МГТУ им. Н.Э. Баумана)

ФАКУЛЬТЕТ МК «Машиностроительный»

КАФЕДРА МК7 «Мехатроника и робототехнические системы»

ДОМАШНЕЕ ЗАДАНИЕ №1

ДИСЦИПЛИНА: «Управление МХТ и РТС»

ТЕМА: «Сравнение методов получения гладкой траектории»

Выполнил: студент гр. МК7-72Б Яковенко А.С.______________

Проверил: Романов А.В.________________

Дата сдачи лабораторной работы:

Результаты сдачи:
Количество рейтинговых баллов

Оценка

Калуга, 2021 г.
 Содержание

Цели и задачи ........................................................................................................... 3

Теоретическая часть............................................................................................. 4
Практическая часть .............................................................................................. 8
Вывод ............................................................................................................... 14
Список литературы ............................................................................................ 15
ПРИЛОЖЕНИЕ А ................................................................................................. 16

2
 Цели и задачи

Цель работы: формирование практических навыков поиска, анализа

и обобщения необходимой информации из различных источников с исполь-
зованием современных информационных технологий

Задачи: выполнить анализ представленного механизма, определить

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

3
 Теоретическая часть

При управлении манипулятором, прежде чем приступить к планирова-

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

координат следует наложить следующие ограничения:

1. Следует задать начальное направление движения схвата «от объекта», с

целью предотвращения столкновение с поверхностью, на которой располо-
жен объект.

2. Следует задать «точку ухода», чтобы траектория схвата проходила че-

рез эту точку, тем самым будет задано допустимое движение ухода.

3. Задать время «ухода», т.е. время за которое схват попадет в эту

точку.

4. Аналогично (2, 3) задать «точки подхода»

5. Возможно задание дополнительных условий на участок между точками

«ухода» и «подхода», а также на время движения на этом участке

6. Таким образом будет задано не менее 4х точек, которые следует «посе-

тить» рабочему органу механизма, при перемещении из точки в точку. Также
на траекторию следует наложить дополнительные условия, обеспечивающие
«гладкость» перемещения:

− в начальной и конечной точках заданы скорость и ускорение.

В простейшем случае они равны нулю.

− в точках «ухода» и «подхода» заданы условия, обеспечивающие непрерыв-

ность и положения, скорости и ускорения

7. Значения обобщенных координат должны лежать в пределах физиче-

ских и геометрических ограничений каждого из узлов механизма.

8. При определении времени движения необходимо учесть следующее:

− время прохождения начального и конечного участков траектории выбира-

ется с учетом требуемой скорости подхода и ухода схвата и представляет со-
4
бой некоторую константу, зависящую от характеристик силовых приводов
сочленений,

− Время движения по среднему участку траектории определяется максималь-

ными значениями скоростей и ускорений каждого из сочленений.

При нормировании следует использовать время прохождения участка

самым медленным узлом механизма.

Рассмотрим варианты интерполяции вышеобозначенной траектории по-

линомами (рис. 1).

Первым делом следует определить наименьшую степень используемого

многочлена:

Рис. 1. Планирование гладкой траектории в пространстве обобщенных коор-

динат

5
 Таким образом у нам надо найти многочлен наименьшей степени, удо-
влетворяющий заданным 8 ограничениям — наименьшая степень такого
многочлена равна 7. Определить способ нахождения коэффициентов такого
многочлена предлагается самостоятельно.

Однако использование полинома такой высокой степени обладает целым ря-

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

Для преодоления недостатков такого подхода следует разбить траекторию на

участки, каждый из которых интерполировать полиномами более низкой сте-
пени. Рассмотрим задачи разбиения на 3 участка.

Разбиение траектории на три участка выглядит достаточно привлекательно с

учетом уже существующего разбиения на участки:

1. Начало - Точка ухода

2. Точка ухода - Точка подхода

3. Точка подхода - Конец

6
Определим условия, налагаемые на многочлены:

В зависимости от порядка решения возможны следующие варианты степеней

полинома:

3−3−5

3−5−3

5−3−3

4−3−4

...

Как можно заметить, сумма степеней во всех случаях равна 11, т.к. ко-
личество условий при разбиении на 3 участка равно 14 (2 (точки) ·3 (условия
на концах) +2 (точки) · 4 (условия — равенство двух производных и коорди-
наты соединения, условие прохождения через заданную точку внутри)). Та-
ким образом при разбиении траектории на N составляющих получим уравне-
ний.

Следовательно сумма степеней многочленов будет равна: K = R−N = 3

·N −1. В случае разбиения траектории на 4 участка получим следующие зна-
чения: R = 18, K = 14, т.е. разбиение можно произвести с использованием ку-
бических сплайнов: 3 − 4 − 3 − 4 − 3. Заметим, что интерполяция крайних
участков полиномами степенью меньшей 3х в общем случае невозможна, в
силу условий на значение второй производной.

7
 Практическая часть

Объектом изучения и исследования будет манипулятор, схема которо-

го представлена на Рис.2.

Рис.2 – Кинематическая схема манипулятора

3D модель исследуемого манипулятора представлена на Рис.3.

Рис.3 – 3D модель манипулятора

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

Прямая задача.

Результатом расчета данной задачи будут координаты выходного зве-

на (x,y,z) при известных обобщенных координатах (q1, q2, q3).
𝑥 = (𝐿1 ∗ cos(𝑞2 ) + 𝐿2 ∗ cos⁡(−180 + 𝑞2 + 𝑞3 )) ∗ 𝑐𝑜𝑠𝑞1
𝑦 = (𝐿1 ∗ cos(𝑞2 ) + 𝐿2 ∗ cos⁡(−180 + 𝑞2 + 𝑞3 )) ∗ 𝑠𝑖𝑛𝑞1
𝑧 = 𝐻 + 𝐿1 ∗ sin(𝑞2 ) + 𝐿2 ∗ sin⁡(−180 + 𝑞2 + 𝑞3 )
Где 𝐿1 , 𝐿2 – длинны звеньев
H – высота до первого звена
Обратная задача.

Результатом расчета данной задачи будут обобщенные координаты

(q1, q2, q3) при известных координатах (x,y,z).
𝑅 = √𝑥 2 + 𝑦 2
ℎ =𝑧−𝐻

𝑟 = √ℎ 2 + 𝑅 2
𝑦
𝑞1 = arctan⁡( )
𝑥
𝐿1 2 + 𝑟 2 − 𝐿2 2 𝑟 2 + 𝑅 2 − ℎ2
𝑞2 = ⁡𝑎𝑟𝑐𝑜𝑠( ) + 𝑎𝑟𝑐𝑜𝑠( )
2 ∗ 𝐿1 ∗ 𝑟 2∗𝑅∗𝑟
𝐿1 2 + 𝐿1 2 − 𝑟 2
𝑞3 = ⁡𝑎𝑟𝑐𝑜𝑠( )
2 ∗ 𝐿2 ∗ 𝐿1

Теперь определим траекторию движения выходного звена в декарто-

вой системе координат через интерполяцию точек в найденной рабочей обла-
сти механизма.
9
Параметрический закон изменения координат от времени для движения вы-
ходного звена по траектории:
19 179
𝑥 = −( ) ∗ 𝑡3 + ( ) ∗ 𝑡 2 − (179/30) ∗ 𝑡 + 80
1500 300
𝑦 = −(4/1125) ∗ 𝑡 3 + (47/450) ∗ 𝑡 2 − (131/30) ∗ 𝑡 + 74
𝑧 = (1/300) ∗ 𝑡 2 + (43/30) ∗ 𝑡 + 39

Изображение траектории, представляющей собой трех мерную кри-

вую приведено на рисунке 5.

Рис.5 – Траектория движения выходного звена манипулятора

Решение задачи проведения рабочего органа механизма по заданной

траектории может быть решена тремя способами:
1) Нахождением интерполяционного многочлена
2) Интерполирование сплайнами
3) Интерполирование сплайнами без учета ускорений

10
 Нахождение интерполяционного многочлена
Определяется число участков разбиения траектории, находятся значе-
ния обобщенных координат к каждой точке разбиения. Затем, используя зна-
чения обобщенных координат в каждой точке разбиения и время при кото-
ром данное значение обобщенной координат принимает это значение, нахо-
дится интерполяционный многочлен. Решение данной задачи реализовано в
пакете MatLab, а текс программы для решения данной задачи приведен в
ПРИОЖЕНИИ А.

Интерполяция сплайнами
При решении задачи данным способом каждый участок разбиения яв-
ляется отдельным участком со своей интерполяционной функцией. В резуль-
тате получается закон изменения обобщенных координат состоящий из от-
дельных участков плавно переходящих друг в друга. Решение данной задачи
реализовано в пакете MatLab, а текс программы для решения данной задачи
приведен в ПРИОЖЕНИИ А.

Интерполяция сплайнами без учета ускорений

При решении задачи данным способом каждый участок разбиения яв-
ляется отдельным участком со своей интерполяционной функцией, но в от-
личии от предыдущего способа из системы уравнений убираются уравнения
учитывающие гладкость ускорений на стыках. В результате получается закон
изменения обобщенных координат состоящий из отдельных участков плавно
переходящих друг в друга. Решение данной задачи реализовано в пакете
MatLab, а текс программы для решения данной задачи приведен в ПРИ-
ОЖЕНИИ А.

11
 Проведение исследования
Для каждого способа определения гладкой траектории проводится
расчет 3 раза, изменяя количество участков разбиения. Получившиеся дан-
ные объединяются и выводятся для наглядности на один график.
Исследование проведено для N=5,7,10; где N это количество точек
разбиения траектории.
Результаты сравнения решений задач тремя способами приведены
ниже на рисунках (6, 7, 8).

Рис.6 – Три способа определения гладкой траектории (при N=5)

Data1 – точки разбиения траектории

Data2 – требуемая траектория
Data3 – траектория полученная первым способом
Data4 – траектория полученная вторым способом
Data5 – траектория полученная третьим способом
12
Рис.7 – Три способа определения гладкой траектории (при N=7)

Рис.8 – Три способа определения гладкой траектории (при N=15)

13
 Проведение исследования изменением траектории
Для каждого способа определения гладкой траектории проводится
расчет 3 раза, изменяя траекторию движения схвата. Получившиеся данные
объединяются и выводятся для наглядности на один график.
Исследование проведено для траекторий со степенями 1,2 и 3.

Рис. 9 – Траектория с первой степенью

Рис. 10 – Траектория со второй степенью

14
 Рис. 11 – Траектория с третей степенью

РЕЗУЛИТАТЫ АНАЛИЗА

Из графиков видно, что первый способ работает достаточно хорошо,

но менее точно чем второй при увеличении количества участков разбиения.
Второй способ показал себя лучшим среди трех, его точность возрастает с
увеличением количества участков разбиения. Третий способ оказался менее
точным среди всех трех из-за колебаний, но при увеличении количества
участков разбиения, он не теряет стабильность. При изменении траектории,
выяснилось, что чем больше степень закона траектории, тем меньшей точно-
стью обладают все способы.

ВЫВОД

В ходе данной домашней работы была решена задача нахождения

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

1. Романов А.В., Пащенко В.Н. Решение задачи нахождения скоростей и

ускорений изменений обобщенных координат для плавного перемещения
выходного звена манипуляторного механизма по заданной совокупности то-
чек. (Методические указания по выполнению лабораторной работы по кур-
су). – Калуга: Издательство КФ МГТУ им. Н. Э. Баумана, 2017. – 19 с.

16
 ПРИЛОЖЕНИЕ А
clc
clear all
%Количество точек разбиения
N=5;
%Время движения
T=30;
%Параметры конструкции
L1=80;
L2=90;
L3=80;
H=29;

i=1;
%Матраца времени
for g=1:1:N
Tv(g,1)=(T/(N-1))*(g-1);
end
%Шаг времени
tt=T/(N-1);

%Коордитаны точек разбиения

for m=1:1:N
x(1,m)=-(19/1500)*Tv(m,1)^3+(179/300)*Tv(m,1)^2-(179/30)*Tv(m,1)+80; %пара-
метрически заданый закон изменения координаты X
y(1,m)=-(4/1125)*Tv(m,1)^3+(47/450)*Tv(m,1)^2-(131/30)*Tv(m,1)+74; %пара-
метрически заданый закон изменения координаты Y
z(1,m)=(1/300)*Tv(m,1)^2+(43/30)*Tv(m,1)+39; %параметрически заданый закон
изменения координаты Z
end

%Определение обобщенных координат в моментах времени разбиения

for h=1:1:N
c=((L2)^2+(L3)^2)^0.5;
R=((x(1,h))^2+(y(1,h))^2)^0.5;
hh=z(1,h)-H;
r_=(hh^2+R^2)^0.5;

a=acos((L1^2+r_^2-c^2)/(2*L1*r_));
b=acos((r_^2+R^2-hh^2)/(2*r_*R));

Q1(1,h)=atan(y(1,h)/x(1,h));
Q2(1,h)=a+b;
Q3(1,h)=acos((c^2+L1^2-r_^2)/(2*c*L1));
end

%%%%%% ПЕРВЫЙ СПОСОБ %%%%%%

%Определение матрицы А
for i=1:1:N
for j=1:1:N+4
A(i,j)=(Tv(i,1))^(j-1);
end
end
A(N+1,2)=1; %Строчка скорости при нулевом времени
A(N+3,3)=2; %Строчка ускорения при нулевом времени
A(N+2,1)=0; %Первый элемент строчки скорости в последний момент времени
%Строчка скорости в последний момент времени
for p=1:1:N+3
A(N+2,p+1)=p*(Tv(N,1)^(p-1));
end

17
A(N+4,1)=0; %Первый элемент строчки ускорения в последний момент времени
A(N+4,2)=0; %Второй элемент строчки ускорения в последний момент времени
%Строчка ускорения в последний момент времени
for l=1:1:N+2
A(N+4,l+2)=(l*(l+1))*(Tv(N,1)^(l-1));
end
%Определение матрицы B для Q1
for f=1:1:N
B1(f,1)=Q1(1,f);
end
for f=1:1:4
B1(N+f,1)=0;
end
%Определение матрицы B для Q2
for f=1:1:N
B2(f,1)=Q2(1,f);
end
for f=1:1:4
B2(N+f,1)=0;
end
%Определение матрицы B для Q3
for f=1:1:N
B3(f,1)=Q3(1,f);
end
for f=1:1:4
B3(N+f,1)=0;
end
%Определение коэффициентов интерполяции
a1=A\B1;
a2=A\B2;
a3=A\B3;
%Нахождение обобщенных координат интерполяцией
for t=0:0.1:T
for n=1:1:N+4
A1(n,1)=a1(n,1)*(t^(n-1));
end
r=round((t/0.1)+1);
A1s(r,1)=sum(A1);
Tanalog(r,1)=t;
end
for t=0:0.1:T
for n=1:1:N+4
A2(n,1)=a2(n,1)*(t^(n-1));
end
r=round((t/0.1)+1);
A2s(r,1)=sum(A2);
end
for t=0:0.1:T
for n=1:1:N+4
A3(n,1)=a3(n,1)*(t^(n-1));
end
r=round((t/0.1)+1);
A3s(r,1)=sum(A3);
end

%%%%%% ВТОРОЙ СПОСОБ %%%%%%

%ОПРЕДЕЛЕНИЕ ЧАСТИ МАТРИЦЫ КООРДИНАТ

%Первый участок
for i=1:1:2
for j=1:1:5
Knach(i,j)=(Tv(i,1))^(j-1);
end
18
end

%Последний участок
for i=1:1:2
for j=1:1:5
Kkon(i,j)=(Tv((N-2+i),1))^(j-1);
end
end

%Промежуточные участки
for k=1:1:(N-3)
for i=1:1:2
for j=1:1:4
K(i,j,k)=(Tv(((k+1)+(i-1)),1))^(j-1);
end
end
end
%ОПРЕДЕЛЕНИЕ ЧАСТИ МАТРИЦЫ СКОРОСТЕЙ
%Первый участок
for i=1:1:2
for j=1:1:4
P1(i,j+1)=j*(Tv(i,1))^(j-1);
end
end
%Последующие участки
for k=1:1:(N-2)
for i=1:1:2
for j=1:1:3
P2(i,j+1,k)=((-1)^i)*(j*(Tv(((k+1)+(i-1)),1))^(j-1));
end
end
end
%Дописываем последний участок
P2(1,5,(N-2))=-4*(Tv(N-1,1))^3;
P2(2,5,(N-2))=4*(Tv(N,1))^3;
%ОПРЕДЕЛЕНИЕ ЧАСТИ МАТРИЦЫ УСКОРЕНИЙ
%Первый участок
for i=1:1:2
for j=1:1:3
Py1(i,j+2)=(j*(j+1))*(Tv(i,1))^(j-1);
end
end

%Последующие участки
for k=1:1:(N-2)
for i=1:1:2
for j=1:1:2
Py2(i,j+2,k)=((-1)^i)*((j*(j+1))*(Tv(((k+1)+(i-1)),1))^(j-1));
end
end
end
%Дописываем последний участок
Py2(1,5,(N-2))=-12*(Tv(N-1,1))^2;
Py2(2,5,(N-2))=12*(Tv(N,1))^2;
%СБОР МАТРИЦЫ
%Часть координат
for i=1:1:2
for j=1:1:5
Osn(i,j)=Knach(i,j);
end
end
for k=1:1:(N-3)
for i=1:1:2
19
for j=1:1:4
Osn((k*2)+i,5+(4*(k-1))+j)=K(i,j,k);
end
end
end
for i=1:1:2
for j=1:1:5
Osn(((N-1)*2-2)+i,(10+((N-3)*4)-5)+j)=Kkon(i,j);
end
end
%Часть скоростей
for i=1:1:2
for j=1:1:5
Osn((2*(N-1))+i,j)=P1(i,j);
end
end
for k=1:1:(N-3)
for i=1:1:2
for j=1:1:4
Osn((2*(N-1))+(k+1)+(i-1),5+(4*(k-1))+j)=P2(i,j,k);
end
end
end
for i=1:1:2
for j=1:1:5
Osn((2*(N-1)+N-2)+i,(10+((N-3)*4))-5+j)=P2(i,j,(N-2));
end
end
%Часть ускорений
for i=1:1:2
for j=1:1:5
Osn(N+(2*(N-1))+i,j)=Py1(i,j);
end
end
for k=1:1:(N-3)
for i=1:1:2
for j=1:1:4
Osn((2*(N-1))+N+(k+1)+(i-1),5+(4*(k-1))+j)=Py2(i,j,k);
end
end
end
for i=1:1:2
for j=1:1:5
Osn((2*(N-1)+N+N-2)+i,(10+((N-3)*4))-5+j)=Py2(i,j,(N-2));
end
end
%Матрица равенства 1
Rav1(1,1)=Q1(1,1);
Rav1((2*(N-1)),1)=Q1(1,N);
h=2;
for s=1:2:((2*(N-1))-2)
Rav1(s+1,1)=Q1(1,h);
Rav1(s+2,1)=Q1(1,h);
h=h+1;
end
%Дозаполнение матрицы равенства
Rav1(((2*N)+2*(N-1)),1)=0;
%Матрица равенства 2
Rav2(1,1)=Q2(1,1);
Rav2((2*(N-1)),1)=Q2(1,N);
h=2;
for s=1:2:((2*(N-1))-2)
Rav2(s+1,1)=Q2(1,h);

20
 Rav2(s+2,1)=Q2(1,h);
h=h+1;
end
%Дозаполнение матрицы равенства
Rav2(((2*N)+2*(N-1)),1)=0;
%Матрица равенства 3
Rav3(1,1)=Q3(1,1);
Rav3((2*(N-1)),1)=Q3(1,N);
h=2;
for s=1:2:((2*(N-1))-2)
Rav3(s+1,1)=Q3(1,h);
Rav3(s+2,1)=Q3(1,h);
h=h+1;
end
%Дозаполнение матрицы равенства
Rav3(((2*N)+2*(N-1)),1)=0;
%Решение
Koef1=Osn\Rav1;
Koef2=Osn\Rav2;
Koef3=Osn\Rav3;
%Коеффициенты участков Q1
%Коеффициенты первого и последнего участка
for i=1:1:5
Kas1(i,1)=Koef1(i,1);
Kak1(i,1)=Koef1(((2*N)+2*(N-1))-5+i,1);
end
%Коеффициенты промежуточных участков
for k=1:1:N-3
for i=1:1:4
KaP1(i,1,k)=Koef1((5+((k-1)*4)+i),1);
end
end
%Коеффициенты участков Q2
%Коеффициенты первого и последнего участка
for i=1:1:5
Kas2(i,1)=Koef2(i,1);
Kak2(i,1)=Koef2(((2*N)+2*(N-1))-5+i,1);
end
%Коеффициенты промежуточных участков
for k=1:1:N-3
for i=1:1:4
KaP2(i,1,k)=Koef2((5+((k-1)*4)+i),1);
end
end
%Коеффициенты участков Q3
%Коеффициенты первого и последнего участка
for i=1:1:5
Kas3(i,1)=Koef3(i,1);
Kak3(i,1)=Koef3(((2*N)+2*(N-1))-5+i,1);
end
%Коеффициенты промежуточных участков
for k=1:1:N-3
for i=1:1:4
KaP3(i,1,k)=Koef3((5+((k-1)*4)+i),1);
end
end
%ПОСТРОЕНИЕ СПЛАЙНОВ Q1
%Первый участок сплайн
i=1
for t=Tv(1,1):(tt/10):Tv(2,1)

Q1s(i,1)=Kas1(1,1,1)+Kas1(2,1,1)*t+Kas1(3,1,1)*t^2+Kas1(4,1,1)*t^3+Kas1(5,1,1
)*t^4;

21
 T1(i,1)=t;
i=i+1;
end
%Последний участок сплайн
i=(N-1)*9+N-10;
for t=Tv(N-1,1):(tt/10):Tv(N,1)

Q1s(i,1)=Kak1(1,1,1)+Kak1(2,1,1)*t+Kak1(3,1,1)*t^2+Kak1(4,1,1)*t^3+Kak1(5,1,1
)*t^4;
T1(i,1)=t;
i=i+1;
end
% Промежуточные участки
for b=2:1:(N-2)
i=11+10*(b-2);
for t=Tv(b,1):(tt/10):Tv(b+1,1)
Q1s(i,1)=KaP1(1,1,b-1)+KaP1(2,1,b-1)*t+KaP1(3,1,b-1)*t^2+KaP1(4,1,b-
1)*t^3;
T1(i,1)=t;
i=i+1;
end
end
%ПОСТРОЕНИЕ СПЛАЙНОВ Q2
%Первый участок сплайн
i=1
for t=Tv(1,1):(tt/10):Tv(2,1)

Q2s(i,1)=Kas2(1,1,1)+Kas2(2,1,1)*t+Kas2(3,1,1)*t^2+Kas2(4,1,1)*t^3+Kas2(5,1,1
)*t^4;
T2(i,1)=t;
i=i+1;
end
%Последний участок сплайн
i=(N-1)*9+N-10;
for t=Tv(N-1,1):(tt/10):Tv(N,1)

Q2s(i,1)=Kak2(1,1,1)+Kak2(2,1,1)*t+Kak2(3,1,1)*t^2+Kak2(4,1,1)*t^3+Kak2(5,1,1
)*t^4;
T2(i,1)=t;
i=i+1;
end
% Промежуточные участки
for b=2:1:(N-2)
i=11+10*(b-2);
for t=Tv(b,1):(tt/10):Tv(b+1,1)
Q2s(i,1)=KaP2(1,1,b-1)+KaP2(2,1,b-1)*t+KaP2(3,1,b-1)*t^2+KaP2(4,1,b-
1)*t^3;
T2(i,1)=t;
i=i+1;
end
end
%ПОСТРОЕНИЕ СПЛАЙНОВ Q3
%Первый участок сплайн
i=1
for t=Tv(1,1):(tt/10):Tv(2,1)

Q3s(i,1)=Kas3(1,1,1)+Kas3(2,1,1)*t+Kas3(3,1,1)*t^2+Kas3(4,1,1)*t^3+Kas3(5,1,1
)*t^4;
T3(i,1)=t;
i=i+1;
end
%Последний участок сплайн
i=(N-1)*9+N-10;

22
for t=Tv(N-1,1):(tt/10):Tv(N,1)

Q3s(i,1)=Kak3(1,1,1)+Kak3(2,1,1)*t+Kak3(3,1,1)*t^2+Kak3(4,1,1)*t^3+Kak3(5,1,1
)*t^4;
T3(i,1)=t;
i=i+1;
end
% Промежуточные участки
for b=2:1:(N-2)
i=11+10*(b-2);
for t=Tv(b,1):(tt/10):Tv(b+1,1)
Q3s(i,1)=KaP3(1,1,b-1)+KaP3(2,1,b-1)*t+KaP3(3,1,b-1)*t^2+KaP3(4,1,b-
1)*t^3;
T3(i,1)=t;
i=i+1;
end
end

%%%%%%%%% ТРЕТИЙ СОСОБ %%%%%%%%%

%ОПРЕДЕЛЕНИЕ ЧАСТИ МАТРИЦЫ КООРДИНАТ

%Часть координат
for k=1:1:(N-1)
for i=1:1:2
for j=1:1:3
Pe(i,j,k)=(Tv((k+i-1),1))^(j-1);
end
end
end
%Часть скоростей
for i=1:1:2
for j=1:1:3
Pv1(i,j+1)=j*(Tv(i,1))^(j-1);
end
end
%Последующие участки
for k=1:1:(N-2)
for i=1:1:2
for j=1:1:3
PvP(i,j+1,k)=((-1)^i)*(j*(Tv((k+i),1))^(j-1));
end
end
end
%СБОР МАТРИЦЫ
for k=1:1:(N-1)
for i=1:1:2
for j=1:1:3
MM((k*2)-2+i,3*k-3+j)=Pe(i,j,k);
end
end
end

for i=1:1:2
for j=1:1:3
MM((2*(N-1))+i,j)=Pv1(i,j);
end
end

for k=1:1:(N-2)
for i=1:1:2
for j=1:1:3

23
 MM((2*(N-1))+(k+1)+(i-1),3+(3*(k-1))+j)=PvP(i,j,k);
end
end
end
%ДОПОЛНЕНИЕ ПОСЛЕДНЕГО СТОЛБЦА
MM((N-1)*2-1,3*(N-1)+1)=Tv(N-1,1)^3;
MM((N-1)*2,3*(N-1)+1)=Tv(N,1)^3;
MM((N-1)*2+N-1,3*(N-1)+1)=-3*(Tv(N-1,1))^2;
MM((N-1)*2+N,3*(N-1)+1)=3*(Tv(N,1))^2;

%Матрица равенства 1
RaM1(1,1)=Q1(1,1);
RaM1((2*(N-1)),1)=Q1(1,N);
h=2;
for s=1:2:((2*(N-1))-2)
RaM1(s+1,1)=Q1(1,h);
RaM1(s+2,1)=Q1(1,h);
h=h+1;
end
%Дозаполнение матрицы равенства
RaM1((N+2*(N-1)),1)=0;

%Матрица равенства 2
RaM2(1,1)=Q2(1,1);
RaM2((2*(N-1)),1)=Q2(1,N);
h=2;
for s=1:2:((2*(N-1))-2)
RaM2(s+1,1)=Q2(1,h);
RaM2(s+2,1)=Q2(1,h);
h=h+1;
end
%Дозаполнение матрицы равенства
RaM2((N+2*(N-1)),1)=0;

%Матрица равенства 3
RaM3(1,1)=Q3(1,1);
RaM3((2*(N-1)),1)=Q3(1,N);
h=2;
for s=1:2:((2*(N-1))-2)
RaM3(s+1,1)=Q3(1,h);
RaM3(s+2,1)=Q3(1,h);
h=h+1;
end
%Дозаполнение матрицы равенства
RaM3((N+2*(N-1)),1)=0;

%Решение
KoefM1=MM\RaM1;
KoefM2=MM\RaM2;
KoefM3=MM\RaM3;

%Коеффициенты участков Q1
%Коеффициенты последнего участка
for i=1:1:4
KakM1(i,1)=KoefM1((N+2*(N-1))-4+i,1);
end
%Коеффициенты промежуточных участков
for k=1:1:N-3
for i=1:1:3
KasM1(i,1)=KoefM1(i,1);
KaPM1(i,1,k)=KoefM1((3+((k-1)*3)+i),1);
end

24
end

%Коеффициенты участков Q2
%Коеффициенты первого и последнего участка
for i=1:1:4
KakM2(i,1)=KoefM2((N+2*(N-1))-4+i,1);
end
%Коеффициенты промежуточных участков
for k=1:1:N-3
for i=1:1:3
KasM2(i,1)=KoefM2(i,1);
KaPM2(i,1,k)=KoefM2((3+((k-1)*3)+i),1);
end
end

%Коеффициенты участков Q3
%Коеффициенты первого и последнего участка
for i=1:1:4
KakM3(i,1)=KoefM3((N+2*(N-1))-4+i,1);
end
%Коеффициенты промежуточных участков
for k=1:1:N-3
for i=1:1:3
KasM3(i,1)=KoefM3(i,1);
KaPM3(i,1,k)=KoefM3((3+((k-1)*3)+i),1);
end
end

%ПОСТРОЕНИЕ СПЛАЙНОВ Q1
%Первый участок сплайн
i=1
for t=Tv(1,1):(tt/10):Tv(2,1)
Q1sM(i,1)=KasM1(1,1,1)+KasM1(2,1,1)*t+KasM1(3,1,1)*t^2;
T1M(i,1)=t;
i=i+1;
end
%Последний участок сплайн
i=(N-1)*9+N-10;
for t=Tv(N-1,1):(tt/10):Tv(N,1)
Q1sM(i,1)=KakM1(1,1,1)+KakM1(2,1,1)*t+KakM1(3,1,1)*t^2+KakM1(4,1,1)*t^3;
T1M(i,1)=t;
i=i+1;
end
% Промежуточные участки
for b=2:1:(N-2)
i=11+10*(b-2);
for t=Tv(b,1):(tt/10):Tv(b+1,1)
Q1sM(i,1)=KaPM1(1,1,b-1)+KaPM1(2,1,b-1)*t+KaPM1(3,1,b-1)*t^2;
T1M(i,1)=t;
i=i+1;
end
end

%ПОСТРОЕНИЕ СПЛАЙНОВ Q2
%Первый участок сплайн
i=1
for t=Tv(1,1):(tt/10):Tv(2,1)
Q2sM(i,1)=KasM2(1,1,1)+KasM2(2,1,1)*t+KasM2(3,1,1)*t^2;
T2M(i,1)=t;
i=i+1;
end
%Последний участок сплайн
i=(N-1)*9+N-10;
25
for t=Tv(N-1,1):(tt/10):Tv(N,1)
Q2sM(i,1)=KakM2(1,1,1)+KakM2(2,1,1)*t+KakM2(3,1,1)*t^2+KakM2(4,1,1)*t^3;
T2M(i,1)=t;
i=i+1;
end
% Промежуточные участки
for b=2:1:(N-2)
i=11+10*(b-2);
for t=Tv(b,1):(tt/10):Tv(b+1,1)
Q2sM(i,1)=KaPM2(1,1,b-1)+KaPM2(2,1,b-1)*t+KaPM2(3,1,b-1)*t^2;
T2M(i,1)=t;
i=i+1;
end
end

%ПОСТРОЕНИЕ СПЛАЙНОВ Q3
%Первый участок сплайн
i=1
for t=Tv(1,1):(tt/10):Tv(2,1)
Q3sM(i,1)=KasM3(1,1,1)+KasM3(2,1,1)*t+KasM3(3,1,1)*t^2;
T3M(i,1)=t;
i=i+1;
end
%Последний участок сплайн
i=(N-1)*9+N-10;
for t=Tv(N-1,1):(tt/10):Tv(N,1)
Q3sM(i,1)=KakM3(1,1,1)+KakM3(2,1,1)*t+KakM3(3,1,1)*t^2+KakM3(4,1,1)*t^3;
T3M(i,1)=t;
i=i+1;
end
% Промежуточные участки
for b=2:1:(N-2)
i=11+10*(b-2);
for t=Tv(b,1):(tt/10):Tv(b+1,1)
Q3sM(i,1)=KaPM3(1,1,b-1)+KaPM3(2,1,b-1)*t+KaPM3(3,1,b-1)*t^2;
T3M(i,1)=t;
i=i+1;
end
end

%%%%%% ПРЯМАЯ ЗАДАЧА %%%%%%

%1 СПОСОБ
for v=1:1:(T*10+1)
RR(v,1)=L1*cos(A2s(v,1))+c*cos(-pi+A2s(v,1)+A3s(v,1));
zI(v,1)=H+L1*sin(A2s(v,1))+c*sin(-pi+A2s(v,1)+A3s(v,1));
xI(v,1)=RR(v,1)*cos(A1s(v,1));
yI(v,1)=RR(v,1)*sin(A1s(v,1));
%Используются A1s(v,1) A2s(v,1) A3s(v,1)
end
%2 СПОСОБ
for v=1:1:((N-1)*9+N)
RR(v,1)=L1*cos(Q2s(v,1))+c*cos(-pi+Q2s(v,1)+Q3s(v,1));
zII(v,1)=H+L1*sin(Q2s(v,1))+c*sin(-pi+Q2s(v,1)+Q3s(v,1));
xII(v,1)=RR(v,1)*cos(Q1s(v,1));
yII(v,1)=RR(v,1)*sin(Q1s(v,1));
%Используются Q1s(v,1) Q2s(v,1) Q3s(v,1)
end

%3 СПОСОБ
for v=1:1:((N-1)*9+N)
RR(v,1)=L1*cos(Q2sM(v,1))+c*cos(-pi+Q2sM(v,1)+Q3sM(v,1));
zIII(v,1)=H+L1*sin(Q2sM(v,1))+c*sin(-pi+Q2sM(v,1)+Q3sM(v,1));
26
xIII(v,1)=RR(v,1)*cos(Q1sM(v,1));
yIII(v,1)=RR(v,1)*sin(Q1sM(v,1));
%Используются Q1s(v,1) Q2s(v,1) Q3s(v,1)
end

%ЗАДАННАЯ ТРАЕКТОРИЯ
%Матраца времени (высокой частоты)
D=100;
for g=1:1:D
Tvf(g,1)=(T/(D-1))*(g-1);
end
%Траектория схвата (вставить закон)
for m=1:1:D
xa(m,1)=-(19/1500)*Tvf(m,1)^3+(179/300)*Tvf(m,1)^2-(179/30)*Tvf(m,1)+80;
%параметрически заданый закон изменения координаты X
ya(m,1)=-(4/1125)*Tvf(m,1)^3+(47/450)*Tvf(m,1)^2-(131/30)*Tvf(m,1)+74; %па-
раметрически заданый закон изменения координаты Y
za(m,1)=(1/300)*Tvf(m,1)^2+(43/30)*Tvf(m,1)+39; %параметрически заданый за-
кон изменения координаты Z
%Используется Tvf(m,1)
end

%%%%%% ПОСТРОЕНИЕ ГРАФИКОВ %%%%%%

%ПОСТРОЕНИЕ ГРАФИКОВ ОБОБЩЕННЫХ КООРДИНАТ

figure
plot (Tv, Q1, 'ok')
hold on
plot (Tanalog, A1s, 'm')
plot (T1,Q1s, 'c')
hold off
title('График обобщенной координаты Q1');
xlabel('Ось времени T,сек');
ylabel('Q1,рад');

figure
plot (Tv, Q2, 'ok')
hold on
plot (Tanalog, A2s, 'm')
plot (T2,Q2s, 'c')
hold off
title('График обобщенной координаты Q2');
xlabel('Ось времени T,сек');
ylabel('Q2,рад');

figure
plot (Tv, Q3, 'ok')
hold on
plot (Tanalog, A3s, 'm')
plot (T3,Q3s, 'c')
hold off
title('График обобщенной координаты Q3');
xlabel('Ось времени T,сек');
ylabel('Q3,рад');
%ПОСТРОЕНИЕ ТРАЕКТОРИИ ДВИЖЕНИЯ

figure
plot (x,y,'o')
hold on
plot (xa,ya, 'k')
plot (xI,yI, 'm')
27
plot (xII,yII, 'c')
plot (xIII,yIII, 'b')
hold off
title('Траектория движения в декартовых координатах');
xlabel('Ось X');
ylabel('Ось Y');
zlabel('Ось Z');

28