CC BY
76УДК 62-519
Разработка системы стабилизации угла отклонения балансирующего робота
Д.С. Федоров, А.Ю. Ивойлов, В.А. Жмудь, В.Г. Трубин ФГБОУВПО НГТУ, Новосибирск, Россия
Аннотация. В данной работе производится определение математической модели балансирующего робота, её линеаризация, расчет алгоритма стабилизации угла отклонения от вертикали модальным методом, определение угла отклонения с использованием альфа-бета-фильтра и угла поворота колес с помощью энкодеров.
Ключевые слова: Балансирующий робот, система автоматического управления, модальный метод синтеза, комплементарный фильтр, альфа-бета-фильтр, акселерометр, гироскоп, энкодер.
ВВЕДЕНИЕ
В данной статье описан процесс разработки системы стабилизации двухколесного балансирующего робота. Внешний вид робота представлен на Рис. 1. Данное устройство представляет собой платформу, на которой размещена управляющая электроника робота и датчики. К платформе жестко крепятся статоры двигателей постоянного тока. На роторах же двигателей закреплены колеса. Идея работы устройства состоит в том, чтобы поддерживать вертикальное положение робота путем вращения колес, поддерживая равновесие балансировкой.
Балансирующий робот с математической точки зрения представляет собой перевернутый маятник с точкой подвеса на колесе (Рис. 2) [1]. Это нелинейная неустойчивая система, которая может служить для отработки и сравнения различных алгоритмов управления. Многие неустойчивые системы содержат звенья, представляющие собой перевернутый маятник: транспортное средство Segway, двуногие шагающие механизмы, экзоскелеты. Поэтому результаты, полученные в данной статье, могут иметь широкое практическое применение.
Первичной задачей является задача определения математической модели объекта, а также проверка разрешимости задачи синтеза.
Рис 1. Балансирующий двухколесный робот
Рис 2. Маятник с точкой подвеса на колесе: О - точка подвеса, Мк - момент колеса, у - угол поворота колеса, г - радиус колеса, тк - масса колеса, (хк, ук) - центр масс колеса, ф - угол наклона, (хр, ур) - центр масс робота, I - расстояние от центра масс робота до точки подвеса (центра масс колеса), g - ускорение свободного падения
Неотъемлемой частью работы любой системы автоматического управления является измерение выходной переменной. От точности этого измерения зависит качество регулирования и запас устойчивости. В этой статье рассмотрены особенности работы акселерометра и гироскопа, входящих в измерительную систему МРП6050, а также предложено объединять показания этих двух датчиков с помощью комплементарного
фильтра, что позволяет достаточно точно определять угол отклонения в текущий момент времени.
Результатом работы является система стабилизации балансирующего робота, рассчитанная модальным методом. Эта система поддерживает состояние робота в вертикальном положении и может отрабатывать внешние возмущения.
1. ПОЛУЧЕНИЕ МАТЕМАТИЧЕСКОЙ МОДЕЛИ ОБЪЕКТА
Перед началом разработки системы стабилизации необходимо выяснить, что представляет собой объект управления с точки зрения математической модели преобразования сигналов. Для построения системы стабилизации необходимо уточнить, какие величины требуется измерять или оценивать, в каком режиме работает система. На основании этих сведений требуется выбрать способ управления, метод расчета регулятора и способ оценки неизвестных величин.
Чтобы получить математическую модель объекта необходимо составить систему дифференциальных уравнений, описывающих физические процессы в нем. Для решения этой задачи воспользуемся уравнением Лагранжа второго рода:
d_ dt
f d!_ ^ v d& J
■dL=a
d4i
(1)
где T - кинетическая энергия системы, Qt -обобщенная сила, qi - обобщенные координаты.
В данном случае обобщенными координатами являются угол наклона ф и угол поворота колеса у. Кинетическая энергия системы имеет следующий вид:
T = 2Tk + Tp,
где Tk - кинетическая энергия кинетическая энергия робота.
Полная кинетическая энергия каждой из составляющих включает в себя вращательную и поступательную энергии:
T = T + T = J j+тУ* =
к к.вр. к.пос.
колеса, Tp -
2
2 2 2 2 Р mW + ткг y
2
mKy2(r2 +р2)
2
(2)
T
T + T
р.вр. р.пос.
J y
+
т рv 2
2 2
Здесь Jk и ^ - моменты инерции колеса и робота соответственно, р - радиус инерции колеса.
Поступательная скорость робота
определяется скоростью его центра масс:
2 2,2 v = v + v
p p _ x p _ y
22 = x p + y p
р •'р'
Поскольку ук = 0, координаты центра масс стержня маятника и центра колеса связаны
соотношениями:
xp = xk +1 sin j, следовательно
Хр = xk +1 jcosj = ry& +1 j cosj ,
,.2,;,2 . o,.,,-, í •______ . /2 -2___2
xp = r y + 2ry& ■ l j cos j +1 • j cos2 j yp = l cos j, следовательно yp =-l j sin j,
• 2 72 • 2 • 2
y p = l j sin j .
Тогда скорость центра масс робота:
2 _ ,.2.„2 , ______ . /2___2 ,„
' р
2 2 2
vp = r j + 2rl\j/(pcosj+1 cos2 ф-ф + +1p sinp ф-ф2 = r2 j2 + Prljp cosp+12фp.
В итоге кинетическая энергия робота будет иметь следующий вид:
T =
l mvj
+
m,,
+ (r j + Prljpcosp +1 ф ). (3)
где X - радиус инерции робота относительно оси колес. С учетом (P) и (3) кинетическая энергия системы будет равна:
T = тку2(р2 + rp) + т
+ (Л2р2 + Prljp cos ф +12фр). (4)
Получим первое дифференциальное уравнение системы в соответствии с (1), дифференцируя (4) по ф, ф и t:
— = — (21ф + Prlj cos ф +12ф), йф 2
d ( dT ^
dt
dj.
m
= (2l p + 2rl jj cos p - j sin p ■ p) +12 p),
dT mv „ , . . . , . .
— =--- ■ 2rljsvnpp = -mvrljsmpp.
dp 2
d (—1 - dL=
dt ^ dp J dp
= mv (l2 +12)p + mvrljcosp =
= mvgl sin p - 2MK. (5)
Здесь Мк - момент, действующий на платформу со стороны двигателя. Получим втрое дифференциальное уравнение системы в соответствии с (1), дифференцируя (4) по p, p и t:
dT
m
= 2my(p2 + r2) + — (2r y + 2rj sin j),
dy
d_ f dT ^ dt
2
dy.
= 2mKy(p + r ) + mvr y +
+ mvrl (jcos j- j2 sin j), 17
2
2
2
dT
= 0,
dy
d_ ( dT ^ dt
= y(2mK (p2 + r2) + mpr2) +
йу) йу
+ трг1 (&со$р-ф28тр) = Мк. (6) Соотношения для двигателя постоянного тока имеют вид:
( =
U -1Я Яя
ke ' M ДВ = KmI Я JДВе = МДВ -МНАГ ■
(7)
(8) (9)
Здесь Мдв - момент, генерируемый двигателем, МНАГ - нагрузочный момент, JдВ - момент инерции двигателя, ш - угловая скорость вращения ротора относительно статора, £ -угловое ускорение ротора, и, 1Я, ЯЯ - напряжение, ток и сопротивление якорной цепи, кт, ке -конструктивные коэффициенты, причем кт включает в себя передаточный коэффициент редуктора, связывающего колесо с валом двигателя.
Из уравнения (8) выразим ток в якорной цепи и подставим в (7):
( и Мдв Яя
о =-----.
к к, к
(10)
Выражение для момента двигателя будет иметь следующий вид:
к и к к
(11)
М
ДВ
1Я
Подставим (11) в (9) и разрешим уравнение относительно нагрузочного момента:
k U k k
МНАГ = ^m---m-e- w- J ДВе. (12)
НАГ J? J? ДВ
КЯ КЯ
Поскольку платформа и колеса являются нагрузкой для двигателей, примем МК = МндГ. Подставим (12) в (5) и (6), с учетом того, что ( = у-ф и e = ф-у:
(mp (Л2 +12) + 2 Jдв )/+(mprl - 2 Jm )у cos р = = mpgl sin р + 2 ^^ (у/ - ф) - 2 kmU
К
К
У(2(тк (Р2 + r2) + 2 mpr2) + J дв ) +
+ (mprl - JДВ )( фcos р - фsin р) = kU kk
К
К
(y - <ф).
vЯ Я
Проведем линеаризацию в точке р = 0, принимая sin р = р, cos р = 1, Пренебрегая массой колеса, получим:
рф = 0 ■
(mp (Л2 +12) + 2 J дв )р + (mprl - 2J т )y =
ДВ kk
kU
= mpgl sinp + 2-^- (y-<р) - 2
КЯ КЯ
y(mpr 2 + JДВ )(mprl - JДВ )ф =
k U k k
m___m e
КЯ КЯ
Обозначим:
К = mp (Л2 +12) + 2Jдв ;
h12 = mprl - 2 JДВ ;
h13 = mprl - J ДВ ;
h14 = mpr 2 + J ДВ ; kk
h21 = 2; h22 = mvgl;
(y - <&)■ (13)
К
Ь = ктке . К = 2 кти .
"23 0 ' "24 ^ 0 ' КЯ КЯ
к = кти
Из2 Кя
Добавим два уравнения: у = у ; ф = ф. Введем переменные состояния: х1 =ф; х2 = р; х3 = у; х4 = у .
Тогда в матричном виде система (13) примет следующий вид:
Н1 х = Н2х + Н3и , (14)
где
H =
h11 0 h12 0
0 1 0 0
h13 0 h14 0
0 0 0 1
H 2 =
- h21 h 22 h21 0
1 0 0 0
h 23 0 h 23 0
0 0 1 1
- h31" "<&"
0 р
, ^ =
h "32
0 _y_
H
Чтобы перейти к стандартному описанию умножим систему (14) слева на Н1-1. В итоге получим:
х = Ах + Ви , Здесь
A =
al =
— а1 а2 а1 0
1 0 0 0
а3 — а4 - а3 0
0 0 1 0
- ¿1" ' С
0 СА
, в = , N = СА2
¿2
0 СА3
К
+h21
h11h14
"Л12 Л13
а =-
h14 h22
h11h23 + Л13 h21
Л Л — Л Л
11 14 12 13
¿1 =
/^2 2 + Л-
31
¿2 =
h13h22
1Л14 — Л12Л13 1Л3 2 + 3 Л31
h11h14 — Л12Л13 h11h14 — Л12Л13
Для определения устойчивости системы определим ее характеристическое уравнение:
А( р) = ёе1( р1 — А) =
= Р4 + А3 р3 + А2 р2 + А р + А0,
_(Л11 + Л12)Л23 + (Л13 + Л14)Л21
Л11Л14 Л12 ^
13
- Л14Л22
^1^4 hl2h1:
- Л22 Л23
h11h14 0.
ЛЛ
12 13
Поскольку часть коэффициентов этого уравнения отрицательны, система является неустойчивой (что было очевидно).
За выход системы возьмем сумму ф и так как эти величины будут напрямую измеряться с помощью датчиков, тогда матрица связи выхода объекта у и вектора переменной состояния х будет иметь вид: С = [0 1 0 1].
Выполним проверку системы на управляемость.
Матрица управляемости будет иметь вид:
и = [в
АВ А2 В
А2в] .
Так как объект управления является одноканальным, то критерием его управляемости является неравенство определителя этой матрицы нулю: аег(и) Ф 0.
Это несложно проверить с помощью среды МАТЬАВ. В данном случае определитель матрицы управляемости равен:
2 , о 1.2\2
I /I /I г
ёе1(и) = -
(а2 а4Ь2 + 2а3а4Ь2)2
(а1а3 — а^)4
следовательно,
объект
он всегда положителен, управляем.
Выполним проверку системы на наблюдаемость. Матрица наблюдаемости имеет вид:
Данный объект одноканальный, поэтому он будет наблюдаемым, если det(U) Ф 0 .
ае1( N) =
2 2 _ 2 л 2 2 2 1 2 л 2
а2 а4 — 2а2а3а4 — 4а2а4а3 + а3 а4 + ¿а2а4а3 — 4а2а4а3 (а1а3 — а^)2 Следовательно, объект наблюдаем.
Для расчета регулятора был выбран модальный метод синтеза [2]. Данный метод применен для решения задачи стабилизации в силу следующих причин. Во-первых, основным режимом работы системы стабилизации робота является режим отработки начальных условий, а модальный метод дает лучший результат именно в таких системах. Во-вторых, для решения задачи стабилизации необходимо оценивать четыре переменных состояния системы, две из которых могут быть измерены напрямую (угол наклона платформы и угол поворота колеса соответственно). В соответствии с модальным методом синтеза закон управления имеет следующий вид:
и = Кх , и = [к1 к2 к3 к4 ],
где К - матрица коэффициентов регулятора. Здесь параметры матрицы К соответствуют коэффициентам в обратной связи при следующих величинах:
к1 - производная угла наклона платформы ф, к2 - угол наклона платформы ф, к3 - производная угла поворота колеса у, к4 - угол поворота колеса у, Далее необходимо определить характеристическое уравнение системы исходя из следующего соотношения:
А( р) = det( р1 — (А + ВК)) =
= р4 + А3 (К) р3 + А2 (К) р2 + А (К) р + А .
После этого составляется желаемое уравнение системы:
С (р) = р4 + С3 р3 + С2 р2 + С р + С0.
Для нахождения коэффициентов регулятора необходимо приравнять коэффициенты при степенях оператора р характеристического уравнения к соответствующим коэффициентам желаемого уравнения.
2. СТРУКТУРА РОБОТА
В соответствие с полученной математической моделью системы и выбранным методом синтеза регулятора была выбрана структура робота, представленная на Рис. 3.
а3 =
3
2
2
0
M )| |(М
Двигатель А Двигатель Б
Рис.3. Структурная схема робота
Ядром системы является микроконтроллер STM32F100RBT6, установленный на отладочной плате STM32VLDISCOVERY. Данный
микроконтроллер полностью реализует управление системой.
В качестве датчика угла наклона применен измерительный модуль MPU6050. Этот модуль включает в себя трехосевые гироскоп и акселерометр и может быть применен для определения не только угла наклона, но и его первой производной (угловой скорости) [3]. Это позволяет напрямую измерить еще одну переменную состояния, что существенно упрощает процедуру разработки системы стабилизации.
В качестве исполнительных механизмов системы применены сервомоторы NXT Lego Mindstorm. В сервомоторы встроены энкодеры, которые позволяют определить угол поворота ротора двигателя относительно статора. Это означает, что существует возможность определить угол поворота колеса.
Так же в систему включен модуль связи по интерфейсу UART. К данному модулю может быть подключен преобразователь TTL-USB для связи с ПК или модуль Bluetooth, например, HC05. Данный модуль введен в систему для отладки ее работы, получению параметров системы в текущий момент времени, а также отправки команд устройству.
Таким образом, можно сформулировать следующие задачи, которые необходимо решить в процессе разработки системы.
1. Реализация управления сервомоторами и получение данных по углу поворота колеса
2. Определение угла наклона робота с помощью измерительного модуля MPU6050
3. Определение параметров математической модели системы
4. Синтез регулятора модальным методом
В следующих разделах статьи описано последовательное решение этих задач.
3. УПРАВЛЕНИЕ СЕРВОМОТОРАМИ
Одним из самых распространенных способов управления напряжением двигателей является широтно-импульсная модуляция. На вход двигателя подается прямоугольные импульсы постоянной частоты, но различной скважности. При достаточно высокой частоте импульсов напряжение сглаживается обмотками двигателя и эквивалентно постоянному напряжению определенной величины.
Для управления двигателем был задействован встроенный в микроконтроллер таймер, который имеет четыре канала управления с возможностью настройки их в режиме генерации ШИМ-сигнала. Модулированное напряжение подается на двигатель через специальные микросхемы -драйверы напряжения.
4. ОЦЕНКА УГЛА ПОВОРОТА И СКОРОСТИ ВРАЩЕНИЯ КОЛЕСА
Для оценки угла поворота колеса используются встроенные в двигатель квадратурные инкрементальные энкодеры. Каждый энкодер имеет два вывода: канал А и канал В. За один оборот колеса будет сгенерировано 180 импульсов на обоих выводах, причем время импульса Ти равно времени паузы Тп, что позволяет измерять угол поворота колеса с точностью до одного градуса, путем регистрирования не только положительных, но и отрицательных фронтов. Сигнал на одном канале сдвинут относительно другого. Это позволяет определять также направление поворота колеса. На Рис. 4 представлена диаграмма сигналов с каналов энкодера при повороте колеса по часовой стрелке, а на Рис. 5 - при работе против часовой стрелки.
Рис. 4. Сигналы энкодера при повороте колеса по часовой стрелке
Рис. 5. Сигналы энкодера при повороте колеса против часовой стрелки
Для регистрации фронтов был задействован таймер микроконтроллера. Каждый вывод энкодера подключен к отдельному его каналу. Если при регистрации положительного фронта на канале А значение на канале В равно нулю или
при отрицательном фронте - единице, то угол поворота инкрементируется, в других случаях -декрементируется.
Для оценки скорости вращения колеса потребовалось применить специальный фильтр, называемый дифференцирующим фильтром.
Дифференцирующий фильтр - это устройство, позволяющее получить оценку фильтруемой величины, а также её производные. Рассмотрим его работу на примере дифференцирующего фильтра второго порядка. Структурная схема фильтра приведена на Рис. 6.
Рис. 6. Структурная схема дифференцирующего фильтра второго порядка: у - фильтруемый сигнал, у0 -его оценка, у0 = Су0 / А , у0 = сУ0 / А
Данной структурной схеме соответствует дифференциальное уравнение:
у = Т2 у0 + 2ХТу0 + у0.
На вход фильтра подается выходной сигнал объекта, на выходе можно получить оценку выходного сигнала объекта и ее первую производную. Теоретически, данный фильтр так же позволяет получить оценку второй производной (для этого необходимо взять сигнал со входа первого интегратора), но в силу зашумленности выходного сигнала объекта, качество получаемой оценки второй производной, как правило, неудовлетворительно. Для корректного определения п-й производной требуется дифференцирующий фильтр п + 1 порядка [4].
При построении фильтра необходимо правильно выбрать его параметры. Основными параметрами данного фильтра являются постоянная времени фильтра Т и коэффициент демпфирования с. Первый параметр определяет длительность переходных процессов в фильтре, то есть, его быстродействие. Второй параметр определяет колебательность процессов в фильтре. Рекомендуется выбирать данный параметр в пределах 0,5-1. Величина постоянной времени фильтра определяется постоянной времени процессов в объекте. Для корректного оценивания выходного сигнала объекта необходимо, чтобы постоянная времени фильтра была в 3-5 раз меньше, чем наименьшая постоянная времени объекта. Однако, следует помнить, что чем меньше значение постоянной времени фильтра, тем хуже эффект от его применения. Таким образом, значение
постоянной времени должно быть достаточно малым, чтобы задержка выходного сигнала фильтра была приемлемой, но в то же время достаточно большим, чтобы помехи выходного сигнала успешно сглаживались.
Наиболее простой способ реализации дифференцирующего фильтра второго порядка со структурой, описанной выше, состоит в замене операции интегрирования операцией
суммирования:
I
= | уЖ ^ у[к] = у[к — 1] + Ау х Аt.
у ■
где у[к-1] - значение фильтруемого параметра на предыдущем шаге, у[к] - значение на текущем, Ау[к] - входной параметр интегратора (производная выходного сигнала) на текущем шаге, Аt - шаг интегрирования. Фильтр второго порядка может быть реализован
последовательным выполнением следующих операций:
у[к ] 2ХАу[к — 1] Ау[к — 1]
Т2
А2 у[к У Т 2 Т
Ау[к ] = Ау[к — 1] + А2 у[к ]Аt, У[к ] = у[к — 1] + АуУ[к ]Аt,
где, А2у[к] - значение оценки второй производной, Ау - значение оценки первой производной, У - значение оценки фильтруемой величины, у -значение фильтруемой величины, Дt - шаг интегрирования.
В отличие от теоретической модели фильтра, в фильтре, реализованном на микроконтроллере, появляется еще один важный параметр, который необходимо правильно выбрать. Это шаг интегрирования. Основное правило при его выборе - шаг интегрирования должен быть меньше наименьшего параметра фильтра, то есть, постоянной времени. Для корректной работы фильтра необходимо, чтобы шаг интегрирования был меньше постоянной времени хотя бы в 3 раза.
Существует еще одна особенность при реализации фильтра на микроконтроллере. Большинство микроконтроллеров могут работать лишь с целочисленной арифметикой, поэтому, во избежание потерь данных при делении, необходимо выполнять сдвиг влево на несколько разрядов перед операцией деления (что эквивалентно умножению на 2 в некоторой степени), а по завершении всех операций, выполнять обратный сдвиг (эквивалентно делению).
Для работы в реальном времени, операции, приведенные выше, могут быть реализованы в виде функции, которая вызывается через интервалы времени, равные выбранному шагу интегрирования. Ниже представлены результаты работы дифференцирующего фильтра, примененного для оценки скорости вращения колеса. На Рис. 7 приведены результаты измерения угла поворота колеса, на Рис. 8 -
результат оценки скорости вращения колеса по данным угла поворота.
Рис. 7. Угол поворота колеса
Рис. 8. Оценка скорости вращения колеса
5. ПОЛУЧЕНИЕ МАТЕМАТИЧЕСКОЙ МОДЕЛИ СЕРВОМОТОРА
Для работы двигателя в соответствии с алгоритмом управления системы необходимо получить его математическую модель. Приближенно поведение двигателя постоянного тока описывается следующими соотношениями:
Je= М - М1; М = кт1; и - Е = ¡Я ; Е = к Г ;
к^и=Jo+к^ о+м , Я Я
(15)
где J - момент инерции вала; М - момент, создаваемый двигателем; М1 - нагрузочный момент; кт, ке - конструктивные коэффициенты двигателя; Я сопротивление обмотки якоря; Е -противо-ЭДС.
В режиме холостого хода при напряжении и0 на входе двигателя решением дифференциального уравнения (15) является функция:
() = и 0 к (1 - е7 Т )
(16)
где К = 1/ ке, Т = JЯ 1(ктке). Если проинтегрировать это уравнение на интервале от 0 до /, получим выражение для угла поворота ротора двигателя:
I
р(Г) = = и0 К (I - Т) + и0 КТе
■г / Т
(17)
Как видно из уравнения (17), с течением времени угол поворота приближается к прямой, описываемой уравнением:
(г) = ио к (г - Т).
(18)
Для получения математической модели двигателя были проведены измерения угла поворота ротора при напряжении и=5 В. Результат измерения угла поворота двигателя представлены на Рис. 9. Точки на линейном участке графика были аппроксимированы прямой, по коэффициентам которой были определены параметры модели:
К = 1,56 V ■ ^/гай, Т = 0,12 5.
Для проверки было проведено моделирование системы с полученными значениями параметров. На Рис. 10 представлены результаты моделирования системы и данные, полученные по результатам измерения для скорости вращения ротора двигателя.
350
300
200
50
п —
Рис. 9. Данные для угла поворота ротора двигателя: синяя кривая - данные эксперимента, красная - данные по результатам моделирования.
Далее необходимо определить конструктивные коэффициенты двигателя кт и ке. Значение ке полностью определяется параметром К, а именно: К = 1/ке.
Однако данный параметр обладает существенной нелинейностью. Причинами наличия нелинейности являются конструктивные особенности двигателя, а также микросхем драйверов, используемых для управления двигателем. На Рис. 11 представлены переходные характеристики двигателя при значениях входного напряжения 1-10 В с шагом 1 В. На Рис. 12 представлена зависимость параметра ке от входного напряжения.
0
Для определения параметра кт был проведен эксперимент, в ходе которого измерялось значение пускового тока при известном нагрузочном моменте на валу двигателя. Схема опыта приведена на Рис. 13.
500 450 400 350 -Мо е1тд ептегЛ
..........Ехр
/л/
Й" 250
100 50 0
0.2
0.4
0.6
0.8
и
Рис. 10. Данные для скорости вращения ротора двигателя: синяя кривая - данные эксперимента, красная - данные по результатам моделирования.
Рис. 11. Переходные характеристики двигателя при различных значениях входного напряжения
Рис. 12. Зависимость величины параметра ке от входного напряжения
Рис. 13. Схема эксперимента по определению конструктивного коэффициента кт. Мк - момент, создаваемый колесом; О - ось колеса; г - радиус колеса; РН - сила натяжения нити (вектор); Рг - сила давления груза на опору (вектор)
На колесе, соединенным с валом двигателя, был закреплен груз с помощью нити. Сначала была измерена сила давления этого груза на опору при отключенном электродвигателе. Затем на двигатель подавалось напряжение, при этом измерялся якорный ток. Сила давления груза уменьшалась на величину силы натяжения нити. Зная эту силу и радиус колеса, несложно определить создаваемый колесом момент:
МК = г • РН = г (РГ1 - Рг 2 ) ,
где РГ1 - сила давления груза на опору при отключенном электродвигателе, РГ2 - при включенном. На Рис. 14 изображена полученная зависимость момента колеса от тока в якорной цепи.
Рис. 14. Зависимость момента колеса от тока в якорной цепи
Значение кт было определено из выражения: кт = М /1.
На основании эксперимента было определено следующее значение коэффициента: кт = 0,25.
При известных значениях параметров кт и ке, а также активного сопротивления якорной цепи Я, которое может быть определено с помощью мультиметра, можно вычислить значение момента инерции двигателя J. Поскольку в двигатель встроен редуктор, значение момента инерции, полученное согласно процедуре, описанной выше, будет соответствовать выходу редуктора
6. ПРИНЦИП РАБОТЫ АКСЕЛЕРОМЕТРА
Акселерометр - это устройство, измеряющее проекцию на его оси суммы всех сил, приложенных к его корпусу, кроме силы тяжести [3]. То есть он измеряет проекцию кажущегося ускорения (разность между истинным ускорением объекта и гравитационным ускорением). Отсюда следует, что применение акселерометра для определения угла наклона имеет смысл, когда сила, приложенная к корпусу (реакция опоры), равна по модулю силе тяжести и направлена в противоположную сторону, то есть датчик должен находиться в покое или, по крайней мере, двигаться без ускорения, что в реальных системах встречается не часто.
Существует несколько видов акселерометров, отличающихся по чувствительным элементам и принципу действия. На Рис. 15 представлена структурная схема одноосевого акселерометра, который преобразует изменение положения чувствительного элемента 2 в напряжение с помощью потенциометра 3.
Рис. 15. Внутреннее устройство одноосного акселерометра. 1 - пружина, 2 - чувствительный элемент, 3 - потенциометр, 4 - демпфер, 5 - корпус
Чувствительный элемент представляет собой массу, закрепленную на пружинах, которые прикреплены к корпусу. Демпфер используется для уменьшения влияния собственных колебаний чувствительного элемента. На Рис. 15 к корпусу не приложены никакие силы вдоль оси X. Если же корпусу сообщить ускорение, то картина изменится, станет такой, как показано на Рис. 16.
В данном случае к корпусу была приложена сила Е1, чувствительный элемент смещается влево, в сторону, противоположную Е1. Это смещение регистрирует потенциометр, и на выходе датчика появляется напряжение, пропорциональное приложенной силе.
-АА/ДО
]—*
Рис. 17. Внутреннее устройство одноосного акселерометра при воздействии силы вдоль его оси
Если же расположить одноосный акселерометр перпендикулярно земной поверхности, то есть вдоль вектора силы тяжести, то датчик произведет измерение силы реакции опоры, как показано на Рис. 18.
Рис. 18. Внутреннее устройство одноосного акселерометра при измерении силы реакции опоры
При повороте датчика из положения на Рис. 18 в положение на Рис. 19, значение проекции силы Е на ось Х будет уменьшаться. Если к данному одноосевому акселерометру добавить второй такой же, но расположить его ось перпендикулярно оси первого, получится двухосевой акселерометр.
Рис. 19. Расчетная схема угла наклона по проекциям вектора силы реакции опоры на оси акселерометра
Как известно, по проекциям вектора на две оси (ОХ, ОУ) декартовой системы координат можно восстановить угол данного вектора в этой системе, что и требуется.
7. ПРИНЦИП РАБОТЫ ГИРОСКОПА
Существует несколько разновидностей микроэлектромеханических гироскопов, различающихся внутренним устройством, но всех их объединяет то, что их работа основана на использовании силы Кориолиса. В каждом из них есть рабочее тело, совершающее возвратно-поступательные движения. Если вращать подложку, на которой находится это тело, то на него начнет действовать сила Кориолиса, направленная перпендикулярно оси вращения и направлению движения тела. На Рис. 20 представлен механизм работы этой силы.
Рис. 20. Механизм работы силы Кориолиса: О -вектор угловой скорости, V - вектор линейной скорости, Ес - сила Кориолиса
Зная линейную скорость и силу Кориолиса можно определить угловую скорость.
Одна из возможных реализаций гироскопа имеет следующую структуру: закрепленная на гибких подвесках рамка, внутри которой совершает поступательные колебательные движения некая масса [4]. Структура такого сенсора представлена на Рис. 21.
Рис. 21. Внутренняя структура гироскопа: 1 -крепление массы, 2 - рабочая масса, 3 - крепление внутренней рамки, 4 - сенсоры перемещения внутренней рамки, 5 - внутренняя рамка, 6 - подложка
Колебания рабочей массы происходят вдоль оси X и генерируются электростатически, а колебания внутренней рамки возможны только вдоль оси У. Между внутренней рамкой и
подложной расположены обкладки плоских конденсаторов (сенсоры перемещения), таким образом, измеряя их емкость, можно фиксировать движение рамки относительно подложки.
На Рис. 22 представлена структура гироскопа при его вращении в плоскости ХУ по часовой стрелке.
-УШг
V
ЧАЛЛАЛН
^ММ/У ^ММ/У
| Рс
Рис. 22. Структура гироскопа при вращении: О -вектор угловой скорости, V - вектор линейной скорости, Ес - сила Кориолиса
Но колебания внутренней рамки могут вызываться не только силой Кориолиса, но и линейными ускорениями, которые действуют вдоль оси У. Проблема решается путем размещения на одной подложке двух рамок, в каждой из которых находится рабочая масса. Обе массы колеблются в противофазе, следовательно, в конкретный момент времени сила Кориолиса, воздействующая на первую массу, направлена противоположно силе, воздействующей на вторую. Сигналы, генерируемые силой Кори-олиса, будут складываться, а синфазная составляющая, порожденная линейным ускорением, - вычитаться.
Таким образом, измеряя отклонение внутренней рамки с помощью сенсоров перемещения, можно оценить угловую скорость датчика.
8. ПОДКЛЮЧЕНИЕ МРиб050
В качестве датчика угла наклона робота применяется измерительный модуль МРП6050. Данный модуль включает в себя два МЭМС-датчика - акселерометр и гироскоп. Модуль имеет цифровой выход /2С. Встроенный АЦП преобразует аналоговые сигналы датчиков в цифровой код для дальнейшей передачи по интерфейсу /2с. Ниже приведены технические характеристики модуля.
• Трехосевой гироскоп;
• Трехосевой акселерометр;
• Термодатчик;
• Напряжение питания 2,375 К-3,46 V;
• ИГО-буфер объемом 1024 байт;
• Программируемые пользователем
цифровые фильтры для гироскопа, акселерометра и термодатчика;
• Интерфейс I2C для записи и чтения; регистров устройства, работающий на частоте до 400 кГц;
• Программируемый пользователем диапазон измерений: ±250, ±500, ±1000, и ±2000 °/с;
• Встроенный 16-разрядный АЦП;
• Цифровой программируемый ФНЧ;
• Ток в режиме работы - 3,6 мА;
• Ток в режиме ожидания 5 мкА.
МРП6050 был подключен к 12С модулю
микроконтроллера 5ГМ32Р100ЛВГ6£ для отправки команд и считывания из регистров необходимых данных. После отправки модулю команды о начале измерений происходит постоянная оцифровка показаний со всех осей гироскопа, акселерометра и термодатчика. Остается только считывать информацию из необходимых регистров. Частота записи новых данных в эти регистры аналогово-цифровым преобразователем зависит от выбранной пользователем чувствительности сенсора и, следовательно, диапазона измерений.
Связь датчика и микроконтроллера осуществляется по интерфейсу 12С. Данный интерфейс имеет архитектуру ведущий -ведомый, то есть одно устройство (ведущее) производит запрос на чтение или запись ведомых устройств. В нашем случае ведомым является МРП6050, а ведущим 12С модуль, находящийся на микроконтроллере. Для связи используется две двунаправленные линии: тактирования и данных. Ведомое устройство имеет свой адрес, который на данной линии должен быть уникальным (в нашем случае это 0хБ0). Линии должны быть подтянуты к уровню логической единицы. Как правило, устройства подключаются к линиям через выводы с открытым коллектором (стоком). Преимуществом является возможность подключения большого количества ведомых устройств, а также сравнительно высокая дальность передачи данных. Но при увеличении дальности фактическая скорость передачи данных снижается. Это связано с тем, что фронты сглаживаются из-за увеличения емкости проводов.
Стоит обратить внимание на некоторые особенности, связанные с программной реализацией алгоритма получения данных на микроконтроллере серии БТМ32.
Во-первых, необходимо сбрасывать флаги состояния модуля 12С-периферии микроконтроллера. Осуществляется это чтением регистра флагов. Если этого не сделать, 12С -модуль МРП6050 не сможет перейти к следующей стадии работы и произойдет зависание программы. Момент времени, когда необходимо сбрасывать флаги зависит от конкретной микросхемы контроллера. Поэтому необходимо тщательно изучить документацию на данный периферийный
модуль микроконтроллера, а также документ, содержащий сведения об ошибках в документации (ERRATA SHEET).
Вторая особенность связана с завершением сеанса связи при чтении данных из регистров MPU6050. Сигналом завершения обмена данных является установка стоп-условия на линии данных. В случае с микроконтроллером STM32F100 команду на генерацию стоп-условия для него необходимо отдать после прочтения предпоследнего байта. Если принимается один байт, это необходимо сделать после получения бита подтверждения адреса от ведомого устройства. Если это сделать после прочтения последнего байта, микроконтроллер будет ожидать прием еще одного байта, которого не будет, и программа зависнет.
9. ОПРЕДЕЛЕНИЕ УГЛА
ОТКЛОНЕНИЯ ОТ ВЕРИКАЛИ
Измерения угла, полученные акселерометром, содержат высокочастотную помеху даже в статическом режиме. В динамике же линейные ускорения приводят к тому, что показания угла могут быть вообще некорректными. Первую проблему можно решить с помощью сглаживания, но это приводит к сдвигу по фазе полезного сигнала, что может сделать систему неустойчивой. Многие объекты управления могут подавить эту высокочастотную составляющую своей инерционностью, и сглаживание можно не применять, но вторая проблема, связанная с неправильным определением угла отклонения акселерометра при воздействии на него линейных ускорений, легко может сделать систему неустойчивой даже при правильно рассчитанном регуляторе.
Измерение угла гироскопом приводит к появлению низкочастотного шума, возникающего вследствие интегрирования угловой скорости (гироскоп измеряет именно эту величину). Это приводит к дрейфу нуля, и значение угла будет постоянно увеличиваться или уменьшаться, даже если система неподвижна.
Напомним, что акселерометр измеряет проекцию на его оси суммы всех сил, приложенных к нему, за исключением силы тяжести. То есть в статике производится измерение силы реакции опоры на данное устройство. А так как эта сила противоположна по направлению гравитационной, то, зная величины её проекций на оси, можно определить угол отклонения от вертикали. Это можно сделать по формуле:
(
Фа [i +1] = arctg
Fon v [i + 1]
Л
F .
y on x L
(17)
где момент
[i +1],
Факс [i +1] - угол отклонения в текущий
времени, вычисленный с помощью
акселерометра, Fon [i +1] - проекция силы
реакции опоры на ось у, а Еоп [И +1] - на ось х.
Гироскоп измеряет угловую скорость в определенной плоскости, используя эффект Кориолиса. В данном случае нас интересует угловая скорость в плоскости ХУ. Получить величину угла можно проинтегрировав сигнал гироскопа:
I
Рн (0 = \Ог)йт.
При реализации на микроконтроллере данная операция заменяется численным интегрированием:
Рн V +1] = Рн И + Р [ +1] =
= Рн И +О[]&, (18)
где рн [И +1], рн [И] - угол, вычисленный с помощью гироскопа на текущем и предыдущем шаге, О[И] - угловая скорость на предыдущем
шаге, Арн [И +1] - приращение угла за промежуток времени, равный шагу интегрирования, Аt -шаг интегрирования.
На Рис. 23 и Рис. 24 представлены графики углов, полученных с использованием данных акселерометра и гироскопа, входящих измерительную систему МРП6050. Для удобства начальный угол гироскопа выбран максимально близким к углу акселерометра. На Рис. 23 видно, что разброс значений угла, вычисленного из показаний акселерометра, превышает один градус, а на Рис. 24 имеется четырехкратная ошибка его определения в динамике. Показания угла, вычисленного из угловой скорости гироскопа, более корректны на втором графике, но на первом имеется значительный дрейф этого угла.
160 200 Секучда'ЮО
Рис. 23. Угол, вычисленный с помощью показаний акселерометра и гироскопа в статике
Поскольку оба этих датчика имеют недостатки и не могут быть использованы напрямую в системах, где требуется высокая точность оценки угла наклона, необходимо применить специальный фильтр, который бы использовал преимущества каждого датчика и
компенсировал недостатки. Один из возможных вариантов такого фильтра - комплементарный фильтр, или альфа-бета фильтра.
2009
8 о
1-2001) сг
4000
--------- ------------ -------- 1-Угол акселерометр 2-Угол гироскоп
—
Ц 2—
—г
1'
150 200 250 СекундэМОО
Рис. 24. Угол, вычисленный с помощью показаний акселерометра и гироскопа в динамике
10. КОМПЛЕМЕНТАРНЫЙ фильтр
Комплементарный фильтр или альфа-бета фильтр в данном случае используется для определения угла наклона и оперирует в своей работе показаниями акселерометра и гироскопа. Ниже представлена формула для вычисления этого угла:
р[И +1] = (1 - К)(Р] + АРн [И +1]) +
+ КрА [И +1], (19) где К - коэффициент фильтра (имеет значение в пределах от 0 до 1), р[И], р[И +1]- угол, вычисленный с помощью фильтра, на текущем и предыдущем шагах.
Подставим (17) и (18) в (19) и получим окончательное выражение для вычисления угла отклонения:
Р[1 + 1] = (1 - К)(р[И] + а[1 ]Аt) +
С
+ К ■ аг^
Кп у [И + 1]
[и+1]
у оп _ х
л
У
(20)
Данный фильтр является упрощенным вариантом фильтра Калмана для одномерного случая, где первое слагаемое представляет собой звено предсказания. Только приращение угла определяется исходя не из математической модели системы и известной величины управления, а угловой скорости на предыдущем шаге, которая измеряется напрямую гироскопом. Также коэффициент К является постоянной величиной. Всё это значительно упрощает вычислительный процесс, при этом полученное значение угла можно считать достаточно точным приближением к реальному. Стоит отметить, что применение гироскопа хорошо тем, что на его выходе мы получаем величину угловой скорости, что избавляет нас от необходимости дифференцировать величину угла, которая имеет высокочастотную составляющую. Особенно
0
важно, что на его показания не оказывают влияния линейные ускорения. Поэтому и при синтезе алгоритма управления желательно, по возможности, переменные состояния измерять датчиками, а не рассчитывать, исходя из значений других, связанных, величин. Это положительно повлияет на запас устойчивости и качество регулирования.
Изменяя значение коэффициента К, мы определяем, какое из слагаемых будет иметь больший вес. При уменьшении К показания фильтра будут стремиться к показаниям гироскопа, а при увеличении - к показаниям акселерометра. При К = 0 уравнение (19) вырождается в (18), и мы получим величину угла без высокочастотных помех, но постоянно "уплывающую". При К = 1 уравнение (19) вырождается в (17), и вычисленный угол будет сильно зашумлен и подвержен влиянию линейных ускорений. Задача состоит в правильном подборе этого коэффициента, чтобы оценка угла, полученная с выхода этого фильтра, удовлетворяла заданным требованиям. На Рис. 25 и Рис. 26 представлены графики при различных значениях параметра К. Шаг дискретизации равен 1 мс. Начальные значения угла гироскопа и фильтра заданы отличными от начального значения угла акселерометра на 2 градуса для более удобной оценки медленных процессов в фильтре. В работающей системе эти значения должны совпадать, как это показано на Рис. 27.
Рис. 25. Значения углов гироскопа, акселерометра и альфа-бета фильтра при К = 0,01
Как видно из графиков, при уменьшении значения параметра К увеличивается время протекания медленных процессов в фильтре. Это обусловлено ослаблением влияния на его показания данных акселерометра (линия стала более гладкой) и увеличением влияния данных гироскопа. Изменение начальных условий не влияет на время протекания медленных процессов (см. Рис. 26 и Рис. 27). Также можно наблюдать появление статической ошибки, которая будет расти при уменьшении коэффициента К и увеличении скорости смещения угла гироскопа, но эту скорость можно считать постоянной, и она
зависит от точности датчика и вычислительной погрешности: чем меньше шаг интегрирования, тем меньше ошибка и ниже скорость дрейфа. При этом статическая ошибка не зависит от начальных условий. При К = 0,001 она составила 1,2-1,3°. Для наиболее точной работы системы рекомендуется начальное значение фильтра приравнять начальному значению угла акселерометра. При этом смещение можно вычислить опытным путем, и оно будет одинаковым во всем диапазоне измерения угла.
-2300
8 -2600
.-2700
-3000
2
-
___3
1 ' Угол фипьтр 2-Угол акселерометр 3-Угол гироскоп
1
50
100 150
Секунда"100
200
250
Рис. 26. Значения углов гироскопа, акселерометра и альфа-бета фильтра при К = 0,001
150 200 Секувда'100
Рис. 27. Значения углов гироскопа, акселерометра и альфа-бета фильтра при К = 0,001 и одинаковых начальных условиях
На Рис. 28 и Рис. 29 представлены графики углов гироскопа, акселерометра и альфа-бета фильтра в динамике при различных значениях параметра К. На Рис. 28 угол фильтра в большей степени соответствует углу гироскопа. Это означает, что для коэффициента К лучшим значением будет 0,001, несмотря на большее время протекания медленных процессов. Измерив смещение, мы сможем его скомпенсировать, но на протяжении времени протекания медленных процессов (первых трех секунд) угол будет определяться неточно: наибольшее значение ошибки будет в нулевой момент времени и будет
равно величине смещения. Но ошибку в 1-2° регулятор более успешно отработает, чем ошибки, вносимые акселерометром при большем значении параметра К. В динамике задержка фильтра минимальна (при К = 0,001 не превышает 20 мс).
Рис. 28. Значения углов гироскопа, акселерометра и альфа-бета фильтра в динамике при К = 0,01
100 150
Секундэ'ЮО
Рис. 29. Значения углов гироскопа, акселерометра и альфа-бета-фильтра в динамике при K = 0,001
На Рис. 30 представлены графики изменения углов при воздействии линейных ускорений вдоль оси, параллельной поверхности, по которой будет двигаться балансирующий робот. Эти ускорения могут возникать при разгоне и торможении. Из графиков видно, что они практически не оказали влияния на величину оцениваемого угла при десятикратной ошибке акселерометра.
На Рис. 31 показаны графики изменения углов при падении балансирующего робота, который изначально находился в вертикальном положении. Из графиков видно, что падение робота в одну сторону акселерометр изначально определил, как падение в другую, вследствие своей внутренней структуры. В системе с обратной связью это может привести к появлению положительной обратной связи, что может сделать систему неустойчивой. Но использование комплементарного фильтра решило эту проблему.
зооо 20 00 1000 о 1000 -2 ООО
-зооо ^1000 -5000
1-Угол фильт р son
2-1\ Угол аксеп 3-Угол гирос
) Г\ г
1
У
|
50
100 150
Секунда'100
200
250
Рис. 30. Углы при воздействии линейных ускорений
Рис. 31 . Углы при падении робота из вертикального положения
11. ОПРЕДЕЛЕНИЕ ПАРАМЕТРОВ МАТЕМАТИЧЕСКОЙ МОДЕЛИ
Перед тем, как приступать к расчету регулятора, необходимо определить оставшиеся неизвестные параметры математической модели системы. Такими параметрами являются:
1. Масса платформы
2. Расстояние от центра масс платформы до точки подвеса маятника (/);
3. Радиус инерции платформы (X).
Масса робота была определена с помощью электронных весов и равна т = 0,52 кг.
Для определения центра масс платформы был выбран метод подвешивания. Суть метода заключается в том, что исследуемый объект подвешивается в различных точках на нити. При этом линия, на которой лежит нить, будет проходить через центр масс объекта. В нашем случае можно считать, что робот имеет симметрию относительно вертикальной оси, проходящей через его геометрический центр. В этом случае центр масс лежит на этой оси. Остается лишь определить точное положение его на оси. На Рис. 32 и 33 приведены результаты эксперимента по определению центра масс. По его результатам было определено следующее значение искомого параметра: I = 0,07 м.
(робот был подвешен в перевернутом положении). Результат опыта приведен на Рис. 34.
Рис. 32. Опыт по определению расстояния от центра масс робота до точки подвеса
Рис. 33. Опыт по определению расстояния от центра масс робота до точки подвеса
Для определения радиуса инерции платформы воспользуемся уравнением периода малых колебаний физического маятника:
Т = 2р
М-
mgl
Здесь I - момент инерции физического маятника.
Учитывая, что I = тк2:
1■
В ходе эксперимента измерялись колебания робота около нижнего положения равновесия
Рис. 34. Результат измерения момента инерции робота
По измеренному периоду колебаний, а также определенному ранее значению параметра ', было определено следующее значение радиуса инерции робота: X = 0,09 м. Для проверки правильности полученных значений параметров, а также определенной ранее математической модели сервомотора, был проведен следующий эксперимент. На двигатели подавалось фиксированное напряжение, при этом робот был отклонен на некоторый угол от вертикали. После подачи напряжения робот проходил верхнее неустойчивое положение равновесия и падал в протии-воположную сторону. На Рис. 35 представлены результаты моделирования данного эксперимента и данные, полученные в ходе реального опыта. До значений начального угла отклонения ± 10 градусов результаты эксперимента и моделирования совпадают с хорошей точностью. При больших углах начинает сказываться линеаризация модели.
Рис. 34. Переходные процессы в системе при подаче фиксированного напряжения. Сплошные линии - результат моделирования, пунктирные - результат эксперимента
12. РАСЧЕТ РЕГУЛЯТОРА
После того, как получены значения всех неизвестных параметров модели системы, можно приступать к расчету коэффициентов регулятора по процедуре, описанной в первом разделе. Сначала необходимо определить характеристическое уравнение системы с регулятором:
А(0) = р1 - (А + ВК)) =
= р4 + А3 (К)р3 + А2 (К)р2 + Д(К) р + А (К)
А3 (К) = Ъ1к1 - Ъ2 к3 + а1 + а3,
А2(К) = Ъ^ - Ъ2 к4 - а2,
А1 (К) = (а2Ъ2 - а4Ъ1 )к2 + а1а4 - а2а3,
А0(К) = (а2Ъ2 - а4^1 )к4.
Далее необходимо составить желаемое уравнение динамики. Для этого воспользуемся результатами опыта, описанного в предыдущем разделе. Из Рис. 34 видно, что время реакции системы должно быть не менее 0,3 с. В противном случае система не сможет функционировать. Исходя из этого, выберем следующее распределение корней системы с регулятором:
I = 12 = 13 = I =-10.
Данному распределению соответствует следующее желаемое уравнение:
С( р) = р 4 + 40 р3 + 600р2 + 4000р +10000.
Приравнивая коэффициенты при соответствующих степенях оператора р, получим следующие значения коэффициентов:
к1 = 1,34 V• ^/ёе§; к2 = 7,6 V/ёе§;
к3 = 0,11 V• ^/ёе§; к4 = 0,3 V/ёе§.
Следует отметить, что из - за большого количества нелинейностей в реальном объекте управления, которые не учитываются в линеаризованной модели, полученные значения коэффициентов могут не обеспечивать заданное быстродействие, а в худшем случае даже не обеспечивать устойчивость системы. Для проверки работоспособности рассчитанных коэффициентов было проведено моделирование процессов в нелинейной системе с рассчитанным регулятором. Результаты моделирования приведены на Рис. 35. Как видно из этого рисунка, система неустойчива. Следовательно, для обеспечения работоспособности устройства необходимо скорректировать полученные значения коэффициентов.
Для корректировки значений коэффициентов может быть использована процедура автоматической настройки. Суть ее заключается в моделировании процессов в системе при различных значениях коэффициентов регулятора и оценке качества процессов по некоторому единому критерию. В самом простом случае настройка коэффициентов выполняется последовательно по одному. Данная процедура сходна с
процедурой автоматической оптимизации, где так же качество процессов в системе оценивается по единому критерию. Но, в отличие от оптимизации, простая настройка не требует получения оптимальных процессов. В нашем случае требуется лишь получить устойчивые процессы с приемлемым качеством. Это упрощает процесс настройки, поскольку может быть применен сравнительно простой алгоритм. Пример такого алгоритма приведен на Рис. 36. При этом требования к точности модели системы не такие жесткие, как в случае с оптимизацией.
Рис. 35. Моделирование системы со значениями коэффициентов регулятора к1 = 0,134, к2 = 0,76, к3 = 0,011, к4 = 0,030
Для настройки параметров регулятора был выбран следующий критерий качества:
Т
у(Т) = | Г | е(Г) | С.
0
Здесь е(/) - ошибка регулирования. Чем лучше качество переходного процесса, тем меньше суммарная ошибка и, следовательно, меньше значение функции у(Т). Сомножитель t вводит вес ошибки по времени, увеличивая ее вклад в общее значение у(Т) на конечном участке переходного процесса и уменьшая на начальном.
В результате проведенной процедуры настройки были получены следующие значения коэффициентов:
к1 = 0,086 V• ^/ёе§; к2 = 3,96 V/ёе§; к3 = 0,023 V• ^/ёе§; к4 = 0,055 V/ёе§.
На Рис. 37 представлен смоделированный переходный процесс системы, и переходный процесс, полученный на реальном устройстве. Как видно из рисунка, система стабилизирована. Испытания показали, что при данных значениях коэффициентов регулятора робот способен отрабатывать начальные условия и возмущения по углу наклона в пределах 5-7°.
Рис. 36. Алгоритм настройки параметров регулятора. Здесь K - матрица коэффициентов регулятора, Qst -шагов алгоритма, Step - шаг изменения настраиваемого параметра
количество
13. РЕЗУЛЬТАТЫ РАБОТЫ
По результатам работы можно сделать вывод, что модальный метод не всегда дает приемлемые результаты для объекта типа «перевернутый маятник», к которому относится балансирующий робот. Связано это с тем, что при расчетах используется линеаризованная модель. Реальный же объект обладает целым рядом нелинейностей, которые ухудшают работу системы стабилизации. Однако, принцип организации обратной связи, который предполагает модальный метод синтеза, позволяет добиться хорошего результата даже для
нелинейного объекта. Данный метод является удобным в реализации для системы управления роботом, поскольку значения переменных состояния могут быть измерены напрямую с помощью датчиков. Структура же модального регулятора позволяет корректировать его коэффициенты, анализируя влияние на работу системы реальных физических величин, которым соответствуют коэффициенты или используя процедуру автоматической настройки параметров на основе полной нелинейной модели системы.
Рис. 37. Результат моделирования и результат работы системы со значениями коэффициентов регулятора к1 = 0,086; к2 = 3,96; к3 = 0,023; к4 = 0,055
Работа выполнена при финансовой поддержке Минобрнауки России по государственному заданию №2014/138, тема проекта «Новые структуры, модели и алгоритмы для прорывных методов управления техническими системами на основе наукоемких результатов интеллектуальной деятельности».
ЛИТЕРАТУРА
[1] MPU-6000 and MPU-6050 Register Maps and Descriptions [Электронный ресурс]. - режим доступа: http://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/
[2] Задача синтеза в теории регулирования: учеб. пособие / А.С. Востриков. - Новосибирск: Изд-во НГТУ, 2011. - 104 с.
[3] Что меряет акселерометр [Электронный ресурс]. -Режим доступа: http://dpla.ru/acclmmer.img/acclmmer.htm
[4] МЭМС-гироскопы - единство выбора
[Электронный ресурс]. - Режим доступа: http://www.electronics.ru/iournal/article/512
[5] Формальский А.М. Управление движением неустойчивых объектов. - М.: ФИЗМАТЛИТ, 2012. - 232 с. - ISBN 978-5-9221-1460-8
[6] Основы теории непрерывных и дискретных систем регулирования. - 5-е изд., перераб. и доп.: учеб. пособие / А.С. Востриков, Г.А. Французова, Е.Б. Гаврилов. - Новосибирск: Изд-во НГТУ, 2008. -476 с. ISBN 978-5-7782-1129-9
Федоров Дмитрий Сергеевич -
магистрант кафедры Автоматики факультета АВТ НГТУ. E-mail: fds0303@mail.ru
Ивойлов Андрей Юрьевич -
магистрант кафедры Автоматики факультета АВТ НГТУ. E-mail: iau13hv@mail.ru
Вадим Аркадьевич Жмудь -
заведующий кафедрой
Автоматики НГТУ, профессор, доктор технических наук. E-mail: oao nips@bk.ru
Виталий Геннадьевич Трубин -
зав. лаб. кафедры Автоматики НГТУ, директор ООО «КБ Автоматика». E-mail: trubin@ngs.ru
Development of Deflactiuon Angle Stabilizing System for Balansing Robot
DMITRY FEDOROV, ANDREY IVOILOV, VADIM ZHMUD, VITALY TRUBIN
Annotation. This paper performs determination of a mathematical model of the balancing robot, its linearization and algorithm for calculation of stabilization of the deflection angle from the vertical modal method. This algorithm also makes determination of the angle of deviation from the use of either alpha-beta filter and the angle of rotation of the wheels by means of encoders.
Key words: balancing robot, automatic control system, the modal synthesis method, the complementary filter, alpha-beta filter, accelerometer, gyroscope, encoder.
REFERENCES
[1] MPU-6000 and MPU-6050 Register Maps and Descriptions [Electronic resource]. - Acess data: http://www.invensense.com/products/motion-tracking/6-axis/mpu-6050/
[2] Zadacha sinteza v teorii regulirovanija: ucheb. posobie / A.S. Vostrikov. - Novosibirsk: Izd-vo NGTU, 2011. -104 s.
[3] Chto merjaet akselerometr [Electronic resource]. -Acess data: http://dpla.ru/acclmmer.img/acclmmer.htm
[4] MJeMS-giroskopy - edinstvo vybora [Electronic resource]. - Acess data: http://www.electronics.ru/journal/article/512
[5] Formal'skij A.M. Upravlenie dvizheniem neustojchivyh ob#ektov. - M.: FIZMATLIT, 2012. - 232 s. - ISBN 978-5-9221-1460-8
[6] Osnovy teorii nepreryvnyh i diskretnyh sistem regulirovanija. - 5-e izd., pererab. i dop.: ucheb. posobie / A.S. Vostrikov, G.A. Francuzova, E.B. Gavrilov. - Novosibirsk: Izd-vo NGTU, 2008. - 476 s. ISBN 978-5-7782-1129-9
