CC BY
60
Интеллектуальные системы и технологии
DOI: 10.18721/JCSTCS.10106 УДК 62-523.2:004.896
алгоритмы управления динамической 3dof платформы подвижности
Р.А. Ганин1, Д.В. Казунин2
1 АО «Кронштадт Технологии» Санкт-Петербург, Российская Федерация;
2 Санкт-Петербургский государственный университет Санкт-Петербург, Российская Федерация
Рассмотрены базовые алгоритмы управления 3DOF динамической платформы тренажеров автотранспорта и спецтехники. Согласно кинематической схеме 3DOF платформы с кривошипно-шатунным механизмом привода звеньев выведены уравнения прямой и обратной задач кинематики, построена ограничительная характеристика рабочей зоны платформы. Проведены исследования алгоритмов управления в пространстве обобщенных координат и координат платформы.
Ключевые слова: платформа подвижности; тренажеры авто- и спецтехники; рабочая зона; регулятор положения; обратная задача кинематики.
control algorithms for the 3dof dynamic motion platform
R.A. Ganin1, D.V. Kazunin2
1 Kronshtadt Technologies
St. Petersburg, Russian Federation;
2 St. Petersburg State University St. Petersburg, Russian Federation
In this paper, basic control strategies of the 3DOF dynamic motion platform for vehicle simulators are considered. The significance of the motion platform in automotive and special vehicle simulators is emphasized in the beginning of the article. According to the kinematics of the 3DOF motion platform with crank gears, forward and inverse kinematics equations are obtained. The workspace of the platform is calculated from the forward kinematics equations and presented as a 3D-figure in platform space. The algorithm of trajectory-planning with respect to the obtained workspace is introduced. Experimental research of the controller in the task space and joint space is conducted. In conclusion, further research problems in this area are discussed.
Keywords: motion platform; vehicle simulatiors; workspace; position controller; inverse kinematics problem.
Одним из важных компонентов динами- сти (СП), предназначенная для выработки у ческого тренажера автотранспорта и спецтех- обучаемого необходимых моторных навыков ники (АТ и СТ) является система подвижно- в процессе управления техникой. Применяе-
мые в большинстве тренажеров системы подвижности представляют собой манипулятор параллельного типа, состоящий из подвижной платформы, связанной с неподвижным основанием одним или несколькими поступательными звеньями. В зависимости от требований к имитируемым эффектам, используются платформы с числом степеней свободы от одного до шести [6, 7].
В подавляющем большинстве тренажеров АТ и СТ (~70—80 % продукции ведущих производителей тренажерных систем) применяются системы подвижности 2БОР и ЗБОБ с кривошипно-шатунным механизмом (КШМ) привода звеньев, обеспечивающие отработку необходимых при имитации данной техники координат подвижной платформы (крен, тангаж, высота) [5].
Рис. 1. ЗБ-модель СП ЗБОБ (а) и ее кинематическая схема (б)
При разработке ПО управления универсальной системой подвижности (УСП) необходимо учитывать такие факторы, как геометрические параметры платформы, рабочая зона имитационной площадки, мас-согабаритные характеристики платформы в различных положениях, алгоритмы управления положением привода, протоколы обмена с датчиками и исполнительными устройствами [1—4].
Постановка задачи
На рис. 1 а изображена кинематическая схема 3ВОБ платформы со следующими геометрическими параметрами:
длина кривошипа Я = А1В1 = А^ = А^3;
длина шатуна Ь = В1С1 = В2С2 = В3С3; расстояния от точки О до точек крепления кривошипов к стойке а = хА2 = хА1 ,
0 = УА2 = Уа1 , С = хАЗ;
расстояния от точки О1 до точек крепления карданов к подвижной платформе
Г ____7 Г ____I _
а = ХС1 = ХС2, 0 = УС2 = УС2, С = ХС3.
Для решения задачи управления положением платформы предложены две базовых структуры системы управления подвижности 3ВОБ, представленные на рис. 2.
Условно обозначим вариант системы управления, изображенный на рис. 2 а, как «система 1», а вариант, изображенный на рис. 2 б — как «система 2». В состав системы 1 входят:
а)
9х
6у
9х'
Блок 9у' Блок
ОХ озк
г*
Г ^ Дд1
ПИД и1 (1 ПЧ1 М1 МР1 ДУ1
3 ю»
Ч2'
Дд2
дЗ" X ДдЗ!
ПИД
иЗ
3
ПЧЗ
мз
МРЗ
н
ПИД у2 \2 ПЧ2 М2 МР2 ДУ2
J БОЧВ
0У
/1СУ2
___I
I
ДУЗ
ЛСУЗ
б)
9у
Блок ОХ
9х'
Д9х
пзк
9у-
Д9у I
Дг
РПП
и1
г
А БОЧВ
ПЧ1
М1
МР1
и2
иЗ
ДУ1
f2 ПЧ2 М2 МР2 ДУ2
J БЩВ
ОУ
Г fЗ ПЧЗ мз МРЗ ДУЗ
J БОЧВ
Рис. 2. Структурная схема системы управления в пространстве обобщенных координат q2*, q3* (а) и в пространстве координат платформы 0х*, 0у*, (б)
блок ограничительной характеристики (ОХ), осуществляющий проверку корректности значений задаваемых координат платформы (крен — 0х, тангаж — 0y, подъем — zp) с целью обеспечения возможности их достижения с помощью данной системы подвижности;
блок решения обратной задачи кинематики (ОЗК), осуществляющий преобразование прошедших предварительную обработку координат платформы 0х*, 0y*, zp* в углы поворота КШМ q1', q2*, q3*;
локальные системы управления (ЛСУ) углами поворота КШМ, включающие: ПИД-регуляторы, блоки ограничения управляющих воздействий (БОУВ), преобразователи частоты (ПЧ), датчики угла (ДУ) поворота мотор-редукторов, мотор-редукторы (МР) приводных звеньев; объект управления (ОУ). В состав системы 2 дополнительно входят:
блок прямой задачи кинематики (ПЗК), осуществляющий преобразование измеряемых обобщенных координат q1, q3 в текущие координаты платформы 9 *, 9 y, z p;
регулятор положения платформы (РПП), производящий расчет скоростей u1 = q1, u2 = q2, u3 = q3 (частот вращения fv f2, f3) МР, исходя из ошибок позиционирования Д0 , Д0 , Az .
х' y' ^f
БОУВ как для системы 1, так и для системы 2 дополнительно осуществляют масштабирование выходов регуляторов u1, u2, u3 в частоты fv f2, f для корректности задания частоты вращения, передаваемой на ПЧ.
Перейдем к описанию базовых алгоритмов представленной на рис. 2 системы управления.
Ограничительная зона динамической платформы
Ограничительная характеристика строится исходя из заданных ограничений на углы поворота КШМ. Для 3DOF платформы с КШМ рабочий диапазон изменения углов поворота КШМ составляет q. = [0; 180], i = 1, ... n, где n — число степеней свободы. Таким образом, область допустимых значений углов КШМ {q : q е [0;180i = 1, n} представляет собой куб со стороной 180°,
построенный из начала координат.
В общем случае, каждая из координат 0х, 0у, т. имеет естественные ограничения, определяемые геометрическими размерами а, Ь, с, Я, Ь. Так, предельные значения вертикального перемещения тр относительно точки О составляют:
7 i = Ь - Я; 7
Исходя из предположения, что для крайних положений платформы смещения точек С1, С2, С3 по оси х малы, предельные угловые перемещения 0 , 0 , 0 , 0 ,
^ г 1 хшт' хшах' ушт' ушах'
а также предельные совместные угловые перемещения 0 и 0 можно получить
г 1 хушп хутах ^
исходя из геометрических соображений: 9— = - агсэт | Я1; 9хтах = гтят IЯ
L + R.
(1)
0ymin = - arCSÍn
2R
0y max = arCSÍn
0^min = - arCCOS
a + c
2R
a + c
R(a + c)
(2)
0xy max = arcc°s
V(a + c)2(R2 + b2) + R 2b2 R(a + c)
V(a + c )2(R2 + b2) + R 2b2
Однако полученные значения определяют лишь часть точек ограничительной характеристики. Оставшиеся граничные точки рассчитаем с помощью прямой задачи кинематики (ПЗК):
F1(X) = (b sin 0x sin 0y - a cos 0y + a)2 +
+ (b cos 0x + b - R sin ^)2 +
+ (z + a sin 0y + b cos 0y sin 0x +
+ R cos q1)2 - L2 = 0;
F2(X) = (-a cos 0y - b sin 0x sin 0y + a)2 +
+ (b cos 0x + b + R sin q2)2 +
+ (zp + a sin 0y - b cos 0y sin 0x +
+ R cos q2)2 - L2 = 0;
F3(X) = (c cos 0y - c)2 + (-R sin q3)2 +
+ (zp - c sin 0 + R cos q3)2 - IL = 0.
Qm' V
'•>- I<W 4J
б)
бутах ХЗ
9ymin
Рис. 3. Ограничительная зона работы 3DOF подвижности
ПЗК формулируется следующим образом: для заданных обобщенных координат # определить вектор X перемещений платформы, решив систему И(Х) = 0. Формула (3) получена на основании аналитического решения ОЗК (формулы (5)—(7)).
Для решения системы уравнений (3) применим рекуррентную формулу метода Ньютона, преобразованную к виду системы линейных алгебраических уравнений:
/(X*) • ДХ* = -И(Хк)
Хк+1 = Хк + ДХк, к = 0,1,2,...,N, (4) |Хк+1 - Хк\ <е
где N — ограничение на число итераций; е — допустимая погрешность вычислений.
Результат построения ограничительной характеристики представлен на рис. 3 а в виде линий уровня функции = У(0Х, 0у). Символом дт обозначено предельное значение соответствующей обобщенной координаты.
В следящем режиме работы заданные координаты платформы необходимо ограничивать перед передачей на блок решения ОЗК, т. е. значения заданных координат Х, находящиеся за границами возможностей платформы, должны быть «притянуты» к границам ограничительной характеристики. Для определения точки, ближайшей к границе ограничительной зоны, используется алгоритм, проиллюстрированный на рис. 3 б.
Алгоритм функционирует следующим
образом. Строится ограничительная характеристика при z,p = const. Если заданная точка X2 находится за пределами области допустимых перемещений, производится поиск допустимого решения на отрезке OX2 путем его деления в пропорции золотого сечения. На каждом шаге данного алгоритма решается ОЗК для очередной точки внутри малого отрезка X2'X2" после чего проверяется условие останова. При достижении заданной точности искомая точка X2* передается на блок ОЗК.
Формулировка ОЗК
В ходе решения ОЗК необходимо определить вектор q обобщенных координат по заданному вектору положения платформы X = (0х*, 0y*, zp*)T. Для вывода уравнений ОЗК выразим координаты точек С1, C2, C3 через углы крена и тангажа:
(X Л Л С1 ' 1 0 0 \
Ус1 = 0 cos 9X "sin 9*х X
V zci J V 0 sin 9X cos 9Х J
cos 9 У 0 sin 9y ^ (-a Л ( 0 Л
0 1 0 -b + 0
- sin9; 0 cos 9* y J V0 j V Л j
(X Л ЛС 2 Г1 0 0 N
УС 2 = 0 cos 9X -sin 9X X
V zc 2 V0 sin 9*х cos 9* x
(5)
' киеВу 0 е-п Ву ^ (-а ^ Г 0 1
X 0 1 0 Ь + 0
е-п ВУ 0 кие В* у Г V 0 Г V ПР т
(хвз н хсз)2 + (увз н усз)2 + (плз н Псз)2 = Ц (6)
Так как кривошипы вращаются в плоскости уОп, справедливы соотношения:
(10 0 1
0 кие ВХ - о-п В 0 о-п В* кие В*
(5)
(х Л
л с з Ус з V пс з д кие В*
Далее определяем координаты точек В1, В2, Вз:
Х л = Хд
е-п Ву ^ ( с ^ Г 01
0 0 + 0
кие В* у Г V 0 Г V П* Г
(7)
(хв 1 н хс 1)2 + (Ув 1 н Ус 1)2 + (Пв 1 н Пс 1)2 = Ц
(6)
Ув 1 = -Ь + Я е-п q **, Пв1 = -Я кие q *;
Ув2 = Ь + Я е-п q**, Пв 2 = -Я кие q **;
Ув з = Я е-п q *, Пв з = -Я кие q *.
После подстановки (7) в (6) и дополнительных преобразований окончательно получаем:
(Хв2 Хс2 ) + (Ув2 Ус2 ) + (Пв2 Пс2) = Ц;
4Я2( Ус 1 + Ь)2 .
q = -2агк1£ -
(Хс 1 + а)2 + (Ус 1 + Ь)2 + (1 - 4Я2)Пс12 + Я2 + Ц
q * = -2агк1£
4Я2(Ус2 - Ь)2 ± ^4Я2(Пс22 + (Ус2 - Ь)2) - ((Хс2 + а)2 + (Ус2 - Ь)2 + Пс22 + Я2 + Ц2)2 .
q * = -2аге1£
(Хс2 + а)2 + (Ус2 - Ь)2 + (1 - 4Я2)Пс22 + Я2 + Ц2
4Я2Усз2 ± ^4Я2(Псз2 + Усз2) - ((Хсз - с)2 + Ус/ + Пс/ + Я2 + Ц2)2 (Хсз - с)2 + Усз2 + (1 - 4Я2)Пс12 + Я2 + Ц2
(8)
где значения координат Хс1, Ус1, Пс1, Хс2, Ус2, Пс2, Хсз, Усз, Псз определяются по формуле (5) как функции Д0Х*, 0у*, Пр*).
Выбором знака «+» или «—» в (8) можно задать вращение кривошипа в левой (qi < 0) или в правой (^ > 0) полуплоскости уоп. С точки зрения позиционирования подвижной платформы оба решения уравнения являются эквивалентными.
Регулятор положения платформы (РПП)
Для реализации контуров положения ЛСУ системы 1, замкнутых обратными связями, целесообразно применить ПИД-регулятор с ограничением по управляющим воздействиям (скоростям) и1 = q1, и2 = q2, из = ¿з вследствие простоты его реализации и приемлемого качества переходных процессов. Путем настройки коэффициентов регулятора достигаются требуемые характеристики переходных
процессов в контурах ЛСУ при реакции на типовые сигналы задания как для каждого контура в отдельности, так и при совместной работе в пределах ограничительной характеристики (рис. з б).
Рассмотрим контур верхнего уровня системы 2, замкнутый обратными связями по координатам платформы. Исходя из предположения, что объект представим в виде линейной модели шестого порядка, применим метод синтеза регулятора состояния, используемый в теории линейных систем автоматического управления. Для этого определим х = [ВХ ВУ п„
В„ П. В_ В. П1Т
У Р Х У Ър!
вектор переменных состояния;
и = [¿х ¿2 ¿3 ]т — вектор входов объекта. Вектор задающих воздействий обозначим
как у* =[В * х В * У г* р ;х ;; ¿;]т^тогда
вектор ошибки управления Ду = у * — у, где
У — [(3 Х 6у 6р 6х Ву
ер ]т. Окончатель-
но закон управления регулятора состояния
будет иметь вид:
ч
x = Ax + B
У = C6x6 • x, Cj = 1;/, j = 1..6 u = K • Ay
K =
; K
Ai, .
Ke - K i Ke Ke K
\ —ii ey —ii Zp—ii e x—ii ey—ii Z1
Ke - K Ke Ke K
'x ey —ii z,—ii >x—i1 ey— i1 Z1
Ke - K i Ke Ke K
'x —i3 ey —3 Zp—i3 i3 ey— i3 Z1
8y -ft
K0
Ai,
Ai, .
ex -i
; K
г, —ii
Ai,
(9)
ey -й
; K
Aii
r-'O 1^0 I-'O 1^0 I-'O 1^0
где лe, лe , KZ , Ke , лe , KZ — начальные значения коэффициентов матрицы обратных связей; Ai — предельное значение обобщенной скорости (рад/с).
Для получения матриц А и B могут применяться различные методы (идентификация объекта управления, линеаризация уравнений, описывающих объект управления), однако в данной работе этот этап пропущен, т. к. уточнение коэффициентов матрицы K можно осуществить на основании данных эксперимента путем подачи простых тестовых воздействий, не прибегая к использованию полной модели ОУ.
Несмотря на то, что коэффициенты матрицы управления С приняты равными единице, прямое измерение вектора состояний затруднено, поэтому для получения текущих координат применяется прямая задача кинематики (3)—(4) и численное дифференцирование по времени векторов у* и у.
Для оценки качества переходных и установившихся процессов в системе воспользуемся критерием минимума среднего квадрата ошибки:
J =
f (cielx + C2< + Сз4,)dt
— min, (10)
уст
нормирующие коэффициен-— ошибки регулирования по 0 , 0 , Z соответственно.
x' У' p
Данный критерий выбран исходя из основного назначения СП в составе тренажерного комплекса — движение по задан-
где C1, C2, C3 ты; e0x, e0y, e
Zp
ной траектории, формируемой математической моделью техники. Математическая модель АТ и СТ отвечает за корректный расчет параметров акселерационных воздействий на рабочем месте оператора, на основании которых формируется траектория движения СП. К системе управления СП при этом предъявляются следующие требования:
запаздывание в режиме слежения должно составлять не более 200 мс; отсутствие перерегулирования; устойчивость при отработке задающих воздействий в диапазоне частот до 1 Гц.
Имитационные эффекты движения тренажеров АТ и СТ достаточно разнообразны, однако наиболее характерным случаем является движение техники по местности с характерными неровностями рельефа, которые имитируются на СП колебаниями малой частоты по углам крена и тангажа вокруг исходной точки (0х, 0^, I) = = (0, 0, Ь). Дополнительно накладывается высокочастотная составляющая колебаний, амплитуда которой зависит от типа грунта, по которому движется транспортное средство.
Результаты испытаний
Рассмотренные выше варианты решения задачи управления положением платформы были испытаны на стенде тренажера БТР-82А.
В таблице приведены результаты испытаний по отработке простых синусоидальных воздействий следующего вида:
Значение критерия для синусоидальных воздействий различной частоты
Частота задающего воздействия f, Гц л
0,1 0,008з 0,0096
0,2 0,0096 0,0109
0,з 0,0112 0,0126
0,4 0,01з0 0,014з
0,5 0,0150 0,0159
0,6 0,0171 0,0174
0,7 0,0192 0,0188
0,8 0,0217 0,0205
0,9 0,0260 0,0251
0
-10
-20
0 5 10 15 20 25 30 35
10
5
0
-5
0 5 10 15 20 25 30 35 с
0,6
0,5 0,4
0 5 10 15 20 25 30 35 ^ с
Рис. 4. Графики отработки задающих воздействий зБОБ платформы для системы 1 и системы 2 (-) уставка; (------) система 1; (- ) система 2
ех (t ) = е° sin(2n/t ), ev (t ) = е° sin f 2n/t + , z = l.
(11)
Для амплитуды колебаний 0Х =0° = 10 значение критерия /, удовлетворяющее перечисленным выше требованиям к системе, находится в пределах / < 0,0з.
Для сравнения систем 1 и 2 по критерию / были подобраны эквивалентные настройки регуляторов.
На рис. 4 представлены графики процессов при отработке траектории движения техники.
Сравнивая кривые для системы 1 и 2 и данные таблицы можно прийти к выводу, что отработка задающих воздействий в следящем режиме в обоих случаях происходит схожим образом, однако на больших частотах сигнала задания система 2 лучше компенсирует отклонения ошибки по скорости за счет влияния коэффициентов обратной связи. Система 2 также выгодно отличается большими возможностями настройки. Так, можно уменьшить коэффициенты по осям 0х, и 0у для более плавной отработки низкочастотных сигналов или обнулить коэффи-
циент для для задействования большего объема рабочей зоны.
Разработаны алгоритмы управления зБОБ динамической платформой подвижности, позволяющие достичь требуемого качества переходных процессов при отработке имитационных эффектов движения, используемых в тренажерах авто- и спецтехники с целью реализации тактильной обратной связи. Для решения задачи позиционирования реализованы и испытаны на практике два подхода: управление в пространстве обобщенных координат приводных звеньев и управление в пространстве координат платформы.
В качестве основного алгоритма управления выбран подход, базирующийся на решении обратной задачи кинематики. Для устранения выявленных недостатков данного метода были применены алгоритмы, учитывающие текущее положение и динамику платформы, которые позволили воспроизвести как эффекты движения автотранспорта и спецтехники по рельефу местности, так и обратные тактильные воздействия, максимально приближенные к реальным.
список литературы
1. Глазунов В.А., Колискор А.Ш., Крайнев
А.Ф. Пространственные механизмы параллельной структуры. М.: Наука, 1991. 96 с.
2. Merlet J.-P. Parallel robots. Sophia-Antipolis, France: Springer, 2006. 417 p.
3. Ростов Н.В. Анализ алгоритмов решения обратных задач кинематики в системах управления движением роботов // Научно-технические ведомости СПбГПУ. Информатика. Телекоммуникации. Управления. СПб.: Изд-во Политехн. ун-та, 2014. № 5(205). С. 93-98
4. Юревич Е.И. Основы робототехники: Учеб. для вузов. СПб.: БХВ-Петербург, 2005. 416 с.
5. Костыгова Д.М., Казунин Д.В., Ганин Р.А., Кузнецов Н.В. Имитационное воспроизведение
движений тяжелых карьерных экскаваторов // Тезисы докл. Междунар. науч. конф. по механике «Седьмые поляховские чтения». СПб., 2015. С. 216.
6. Leonov G.A., Zegzhda S.A., Zuev S.M., Ershov B.A., Kazunin D.V., Kostygova D.M., Kuznetsov N.V., Tovstik P.E., Tovstik T.P., Yushkov
M.P. Dynamics and Control of the Stewart Platform // Doklady Akademii Nauk. 2014. Vol. 458. No. 1. Pp. 36-41.
7. Альван Х.М., Слоущ А.В. Об управлении движением пространственной платформы с несколькими степенями подвижности // Теория механизмов и машин. Динамика машин. СПб.: Изд-во Политехн. ун-та, 2003. № 1. С. 63.
references
1. Glazunov V.A., Koliskor A.Sh., Kraynev A.F.
Prostranstvennyye mekhanizmy parallelnoy struktury [Spatial mechanisms of parallel structure]. Moscow: Nauka Publ., 1991, 96 p. (rus)
2. Merlet J.-P. Parallel robots, Sophia-Antipolis, France: Springer, 2006, 417 p.
3. Rostov N.V. Analiz algoritmov resheniya obratnykh zadach kinematiki v sistemakh upravleniya dvizheniyem robotov [Algorithm Analysis for Solving the Inverse Kinematic Problems in Robot Motion Control Systems]. Nauchno-tekhnicheskiye vedomosti SPbGPU. Informatika.
Telekommunikatsii. Upravleniya [St. Petersburg State Polytechnical University Journal. Computer Science. Telecommunication and Control Systems], 2014, No. 5(205), Pp. 93-98. (rus)
4. Yurevich Ye.I. Osnovy robototekhniki [Fundamentals of Robotics]. St. Petersburg: BKhV-Petersburg Publ., 2005, 416 p. (rus)
5. Kostygova D.M., Kazunin D.V., Ganin R.A., Kuznetsov N.V. Imitatsionnoye vosproizvedeniye dvizheniy tyazhelykh karyernykh ekskavatorov [Imitating reproduction of movement of heavy mining excavators]. Sedmyye polyakhovskiye chteniya, Tezisy dokladov mezhdunarodnoy nauchnoy konferentsii po mekhanike [Abstracts of
the international scientific conference on mechanics "Seventh polyahovskie reading"], St. Petersburg, 2015, P. 216. (rus)
6. Leonov G.A., Zegzhda S.A., Zuev S.M., Ershov B.A., Kazunin D.V., Kostygova D.M., Kuznetsov N.V., Tovstik P.E., Tovstik T.P., Yushkov M.P. Dynamics and Control of the Stewart Platform. Doklady Akademii Nauk, 2014, Vol. 458, No 1, Pp. 36-41. (rus)
7. Alvan Kh.M., Sloushch A.V. Ob upravlenii dvizheniyem prostranstvennoy platformy s neskolkimi stepenyami podvizhnosti. Teoriya mekhanizmov i mashin. Dinamika mashin, St. Petersburg: Politekhn. un-t Publ., 2003, No. 1, P. 63. (rus)
сведения об авторах / the authors
ТАНИН Роман Андреевич GANIN Roman A.
E-mail: [email protected]
КАЗУНИН Дмитрий Владимирович KAZUNIN Dmitry V.
E-mail: [email protected]
Статья поступила в редакцию 11.07.2016 © Санкт-Петербургский политехнический университет Петра Великого, 2017
