CC BY
26УДК 62-501.72:556
Разработка системы автоматической стабилизации в вертикальном положении двухколесной платформы
А.Ю.Ивойлов, В.А. Жмудь, В.Г.Трубин НГТУ, Новосибирск, Россия iau13hv@mail.ru
Аннотация: в данной работе рассматриваются вопросы, связанные с разработкой систем автоматической стабилизации и в частности, системы стабилизации в вертикальном положении объекта типа «перевернутый мятник» В статье описаны этапы разработки системы: создание электрической схемы, разработка управляяющих программных модулей, получение математической модели системы. Для обеспечения функционирования систем автоматической стабилизации необходимо измерять значение стабилизируемого параметра. Зачастую, помимо значения параметра необходимо получать данные производной этого параметра. Как правило, измеряемый параметр содержит помеху, которая затрудняет получение производной. В работе рассматриваются вопросы, связанные с фильтрацией сигнала, и в частности, применение дифференцирующих фильтров для получения производной сигнала. Также обсуждается применение средств отображения информации на примере жидкокристаллических дисплеев, в системах, работающих в реальном времени.
Ключевые слова - Системы автоматического управления, перевернутый маятник, акселерометр, сервомотор, микроконтроллер, DISCOVERY, дифференцирующий фильтр, ЖК-дисплей.
ВВЕДЕНИЕ
В данной статье описывается разработка системы автоматической стабилизации в вертикальном положении двухколесной платформы. Объект управления данной системы представляет собой платформу, на которой размещена плата управления с датчиками и процессорным устройством на базе микроконтроллера. К платформе жестко крепятся статоры двигателей, на валах которых закреплены колеса. Внешний вид платформы показан на Рис. 1.
Управляемыми параметрами системы являются угол наклона платформы относительно горизонта и угол поворота колеса относительно начального положения. Математическая модель
объекта идентична модели перевернутого (обратного) маятника. Задача стабилизации такого объекта является одной из классических задач теории автоматического управления и достаточно распространена в технике. Уравнениями такого объекта описывается, например, поведение ракеты при старте или многоступенчатой антенны. Отличие
разрабатываемой системы от классического обратного маятника состоит в том, что точка подвеса маятника закреплена на колесе.
СТРУКТУРА РАЗРАБАТЫВАЕМОЙ СИСТЕМЫ
Разработка системы включает разработку принципиальной схемы платформы, создание программ управления компонентами системы, а так же разработку алгоритма стабилизации платформы. Структурная схема системы приведена на Рис. 2. Система включает в себя следующие компоненты: микропроцессорное устройство, схема управления двигателями, двигатели, датчик угла наклона, ЖК-дисплей и Bluetooth-модуль.
В качестве датчика угла наклона платформы используется акселерометр ADXL-335 [3]. Для передачи информации в процессе отладки работы системы, а так же настройки режима ее работы в управляющую программу включен модуль обмена данными по интерфейсу Universal Asynchronous Receiver Transmitter (UART). По этому интерфейсу к микроконтроллеру может быть подключен Bluetooth-модуль (например HC05) для беспроводной передачи данных или преобразователь USB-TTL (например PL2303) для проводной. Для отображения текущего состояния системы на платформе размещен ЖК-дисплей NOKIA 5110 [4]. С помощью этого дисплея данные о состоянии системы могут быть отображены в виде текстовой информации или графиков.
Управляющая программа разрабатывается таким образом, чтобы каждый блок в системе управлялся независимо от остальных. Такая организация программы позволяет гибко осуществлять разработку системы, оставляя возможность добавления модулей в систему. При этом важным требованием к разработке
программных модулей является требование к быстродействию, поскольку для обеспечения быстродействия системы необходимо обеспечить быстродействие всех ее компонентов.
Рис.1. Внешний вид платформы
Angle Sensor
1_СО Bluetooth
module
А
Motor В driving circuit
I
Servomotor В
А
MCU о-
1
1
Motor А driving circuit
i
Servomotor А
Рис. 2. Структурная схема платформы
Помимо управляющих модулей для компонентов системы программа включает модули, реализующие непосредственно
стабилизацию системы. Такими модулями являются модули регистрации сигналов с датчиков, фильтрации данных, дифференцирования, а так же модуль, реализующий вычисление управляющего воздействия. Далее рассматриваются основные вопросы, связанные с реализацией программных модулей.
АКСЕЛЕРОМЕТР
В системе используется акселерометр ЛВХЬ-335. Микросхема имеет три аналоговых канала, соответствующие величине проекции ускорения по трем осям. Параметры акселерометра:
• Число осей чувствительности: 3,
• Ток потребления: 350 мкА,
• Диапазон измерений: ± 3,6 g,
• Нелинейность: ± 0,3 %,
• Чувствительность: 400 мВ^,
• Рабочее напряжение: 3 В.
Акселерометр измеряет проекции результирующего ускорения на оси. Если на акселерометр действует только сила притяжения земли, то, определив величину проекций ускорения на оси, можно вычислить тангенс угла наклона акселерометра относительно вертикального
положения: tgр = Рх / ¥у .
Этот принцип проиллюстрирован на Рис. 3. Далее, по значению тангенса угла можно восстановить сам угол одним из следующих способов.
Для приложений, в которых угол наклона изменяется в небольшом диапазоне, можно использовать следующую аппроксимацию:
Р /Ру = tgj»j (рад).
Используя такую аппроксимацию можно восстановить угол с точностью 1 % в диапазоне от - 20° до + 20°, а с точностью 5 % - в диапазоне от - 30° до + 30°. Но если требуется работать в более широком диапазоне углов, то данный метод не подходит для определения угла. В этом случае можно применить один из следующих методов.
Первый метод - табличный. Значения углов, соответствующие определенным значениям отношения проекций, заносятся в память микроконтроллера в виде массива данных. Для уменьшения объема занимаемой памяти целесообразно внести в массив данные для углов в диапазоне от 0° до 45°. В случае если угол находится в диапазоне от 45° до 90°, вычисляется обратное отношение, а угол восстанавливается по формуле приведения:
рррад)» р/2- р /Ру
Для значений углов, соответствующих другим квадрантам, значения восстанавливаются по известным из тригонометрии соотношениям.
Рис. 3 Схема определения угла наклона с помощью акселерометра
Для увеличения быстродействия можно использовать метод двоичного приближения. При этом число шагов п, за которые будет определено
значение угла, фиксировано П = log2 N , где N -число значений в таблице. Данный способ экономит вычислительный ресурс
микроконтроллера, но требует большого количества памяти, что в некоторых системах может быть проблемой.
Второй способ - использование ряда Тейлора. В случае если объема памяти недостаточно, можно использовать для вычисления угла разложение в ряд Тейлора для функции тангенса и использовать первые несколько членов ряда:
12 2З
#р = р+—р +—р =...
Этот метод требует существенно большего вычислительного ресурса, но при этом не требует затрат большого количества памяти.
Применение конкретного метода зависит от требований к точности определения угла, диапазону измерения, вычислительного ресурса микроконтроллера и имеющегося объема памяти.
Для измерения показаний акселерометра был задействован встроенный в микроконтроллер АЦП, его разрешение составляет 12 бит. При отладке работы модуля определения угла наклона платформы данные, получаемые контроллером, передавались на ПК по Ш^Г-интерфейсу с помощью преобразователя ШБ-ГГЬ. Сигнал акселерометра содержит значительную высокочастотную помеху. График изменения значения угла, восстановленного по такому сигналу, приведен на Рис. 4.
Как видно, разброс значений угла составляет около двух градусов. Для уменьшения разброса значений угла необходимо использовать фильтрацию данных.
Поскольку для работы системы требуется восстановление не только угла наклона, но и его производной, для фильтрации данных был написан модуль, реализующий дифференцирующий фильтр. С помощью этого фильтра можно получить как сглаженный сигнал, так и его производную. При написании программы операция интегрирования заменяется операцией суммирования.
Рис. 4 График изменения значения угла, восстановленного по исходному сигналу
При использовании этого фильтра необходимо дополнительно фильтровать производную сигнала для уменьшения высокочастотной
помехи производной. Структурная схема фильтра приведена на Рис. 5. Результаты фильтрации сигнала приведены на Рис. 6 и 7. На Рис. 6 приведен исходный и фильтрованный графики угла наклона платформы, на Рис. 7 приведена его производная.
Рис. 5. Структурная схема дифференцирующего фильтра: у - входной сигнал, у - оценка входного
сигнала, У - оценка производной входного сигнала.
Рис. 6. Результат фильтрации: синяя линия - исходный сигнал, красная - фильтрованный
Рис. 7. Результат фильтрации: производная исходного сигнала
Недостатком такого способа является задержка фильтрованного сигнала по сравнению с исходным, причем величина этой задержки тем больше, чем сильнее сглаживание сигнала. При расчете параметров фильтра необходимо, чтобы
процессы в фильтре протекали на порядок быстрее процессов в объекте.
УПРАВЛЕНИЕ СЕРВОМОТОРАМИ
В качестве исполнительных механизмов в системе используются сервомоторы NXT Lego Mindstorm. В двигатели встроен энкодер с разрешением 1°. Одним из самых распространенных способов управления напряжением двигателей является широтно-импульсная модуляция. На вход двигателя подается прямоугольные импульсы постоянной частоты, но различной скважности. При достаточно высокой частоте импульсов напряжение сглаживается обмотками двигателя и эквивалентно постоянному напряжению определенной величины. Существует два способа управления двигателями с помощью ШИМ. Диаграммы сигналов для них представлены на Рис. 8.
В первом случае на один из входов подается нулевое напряжение, а на второй - ШИМ-сигнал. В этом случае в моменты времени, когда на одном из входов высокое напряжение, двигатель находится в режиме разгона, а в остальные - в режиме торможения по инерции, или свободного торможения. Во втором случае, на один из входов подается высокое напряжение, а на второй -ШИМ-сигнал. При таком способе управления в моменты времени, когда на одном из входов высокое напряжение, а на другом - нулевое, двигатель находится в режиме разгона, а в остальные моменты времени - в режиме принудительного, или рекуперативного торможения.
При первом способе управления скорость вращения двигателя не будет зависеть линейно от входного напряжения. При втором способе скорость вращения зависит линейно от напряжения, однако при этом в обмотках двигателя протекают существенно большие токи, чем при первом способе управления. Поэтому при выборе второго способа необходимо учитывать, что тепловая нагрузка на двигатель будет заметно выше.
Для управления двигателем был задействован встроенный в микроконтроллер таймер, который имеет четыре канала управления с возможностью настройки их в режиме генерации ШИМ-сигнала. Модулированное напряжение подается на двигатель через специальные микросхемы -драйверы напряжения.
Информация от энкодеров фиксируется путем регистрации фронтов сигналов, выдаваемых им. Каждый фронт соответствует повороту ротора двигателя на один градус. Для этого также используется таймер. Для получения информации о скорости вращения ротора двигателя применяется фильтр, аналогичный тому, что используется для оценки производной угла наклона платформы по показаниям акселерометра.
Рис. 8 Способы управления уровнем напряжения: а -свободное торможение, Ь - рекуперативное торможение
Для работы двигателя в соответствие с алгоритмом управления системы необходимо получить его математическую модель. Приближенно поведение двигателя постоянного тока описывается следующими соотношениями:
= M-М^ М = cФI; U-E = Ж ; E = cФw; cФ
-u = 3, (1)
R
где 3 - момент инерции вала,
М - момент, создаваемый двигателем,
М1 - нагрузочный момент,
с - конструктивный коэффициент двигателя,
Ф - магнитный поток,
R - сопротивление обмотки якоря,
Е - противо-ЭДС,
С - угловая скорость.
В режиме холостого хода при напряжении на входе двигателя решением дифференциального уравнения (1) является функция:
с(г) = и 0 К (1 - е 7 Т) , (2)
где К = (сФ)-1, Т = JR(cФ) "2.
Если проинтегрировать это уравнение на интервале от 0 до t, получим выражение для угла поворота ротора двигателя:
t
j(t) = J w(t)dt = UoK(t - T) + UoKTe
t / T
(3)
Как видно из уравнения (3), с течением времени угол поворота приближается к прямой, описываемой уравнением:
Щ) = и0(г - Т). (4)
Для получения математической модели двигателя были проведены измерения угла поворота ротора при напряжении и = 4,5 В. Точки на линейном участке графика были аппроксимированы
o
прямой, по коэффициентам которой были определены параметры модели: V - s
К = 1,83——, Г = 0,061л.
тай
Для проверки было проведено моделирование системы с полученными значениями параметров. На Рис. 9 представлены результаты моделирования системы и данные, полученные по результатам измерения для угла поворота ротора двигателя. На Рис. 10 приведены аналогичные данные для скорости вращения ротора двигателя.
Рис. 9. Данные для угла поворота ротора двигателя: синяя кривая - данные эксперимента, красная - данные по результатам моделирования
Рис. 10 Данные для скорости вращения ротора двигателя: синяя кривая - данные эксперимента, красная - данные по результатам моделирования
ЖК-ДИСПЛЕЙ
Для отображения параметров системы, таких как текущий угол наклона платформы, угол поворота ротора двигателя, режима движения системы, на платформе размещен ЖК-дисплей NOKIA 5110. Это монохромный ЖК-дисплей, разрешение экрана - 84 х 48 точек. Управляется встроенным драйвером дисплея PCD8544.
Параметры дисплей приведены ниже.
• Разрешение экрана: 84 х 48 точек,
• Напряжение питания: 2,7 - 3,3 В
• Потребляемый ток: до 320 мкА,
• Интерфейс: БР1,
• Частота тактирования: до 4 МГц.
На аппаратном уровне работа с дисплеем осуществляется путем передачи
информационного или командного байта по интерфейсу БР1. Для работы с дисплеем был написан управляющий программный модуль. Структура модуля двухуровневая. Функции нижнего уровня реализуют работу 5Р/-периферии микроконтроллера, адресацию дисплея, настройку режимов его работы. Функции верхнего уровня реализуют непосредственно отображение информации. Программный модуль позволяет отображать три вида информации: текстовые символы, растровые изображения и векторную графику. Пример работы дисплея приведен на Рис. 11.
Для передачи каждого символа требуется отправить дисплею 14 байт информации. При частоте обмена данными 4 МГц время, затраченное на передачу, составит около 30 мкс. Для передачи дисплею точки требуется ее меньше времени - около 6 мкс. Это позволяет использовать программный модуль для отображения в реальном времени текстовой и графической информации, например - график изменения значения параметра системы.
Рис. 11. Пример реализации графики - построение фигуры Лиссажу по точкам во времени
МАТЕМАТИЧЕСКАЯ МОДЕЛЬ СИСТЕМЫ
С математической точки зрения двухколесная платформа представляет собой перевернутый маятник с точкой подвеса, размещенной на колесе. Вывод уравнений, описывающих поведения этого объекта, можно осуществить с помощью уравнения Лагранжа второго рода [5]:
d_
dt
ГdTЛ
dq t
dT
dq,
= Q, ,
(5)
где Q - кинетическая энергия системы, д -обобщенные координаты, Г - обобщенные силы.
Схема маятника с точкой подвеса на колесе приведена на Рис. 12. Введем следующие обозначения:
j - угол наклона платформы, y - угол поворота колеса, m p - масса стержня маятника, mw - масса колеса,
l - расстояние от центра масс стержня до точки подвеса,
l - радиус инерции стержня маятника, r - радиус колеса, р - радиус инерции колеса,
(xp, yp ) - координаты центра масс маятника,
(, yw ) - координаты центра колеса.
Поскольку yp = 0, координаты центра масс стержня маятника и центра колеса связаны соотношениями:
Xp (t) = Xw (t) +1 sinj(t);
Ур (t) =l cos j(t).
С учетом этих соотношений выражение для кинетической энергии системы имеет вид:
T = mwy2(r2 +р2) + +1 mp(r2y2 + (l2 + 12)j2 + rlyjcos j).
В качестве обобщенных координат выберем угол наклона платформы и угол поворота колеса.
mp (l2 +1)j + mrlyjr cos j:
= mpgl sinj- 2M, (6)
1
2f(mw (r2 +p2) + 2 mpr2) +
+ mprl(lp cos (р-ф2 sin j) = M . (7)
Выражение для момента, создаваемого двигателем, имеет вид:
cF (сФ)2 M = —U-(ф-y) . (8) R R Y r
Подставив выражение (8) в уравнения (6) и (7) получим окончательный вид уравнений системы:
mp (l2 + 1)¿p + mprl y cos р =
(сФ)2 сФ
= mpgl sin р + 2y - p) - 2rU , (9)
mprl p cos р + 2(mw (r2 + p2) +1 mpr 2)y =
= m tl jr2 sin j -
(cF)2
cF
R У -j)-rU . (10)
УПРАВЛЕНИЕ СИСТЕМОЙ
К настоящему времени в системе реализована аппаратная часть и все программные модули, предназначенные для управления компонентами системы. Система находится на стадии отладки работы алгоритмов стабилизации. Реализуется два алгоритма управления. Первый алгоритм предполагает линеаризацию системы и организацию управления с помощью модального метода синтеза. Второй алгоритм основан на использовании метода локализации [6]. Это метод разработан на кафедре автоматики НГТУ и применяется для управления нелинейными объектами. Система стабилизации основана на использовании в управлении старшей производной выходной переменной объекта. Управление формируется по следующему закону:
Рис. 12. Схема обратного маятника с точкой подвеса на колесе
и = К (^ (у, у ,...у(-1), V) - у(п)),
где ^ - желаемое уравнение, К - большой коэффициент, V - задающее воздействие. Желаемое уравнение задается в виде линейного дифференциального уравнения. Такая организация управления позволяет подавить нелинейности объекта.
Продифференцировав выражение для кинетической энергии по обобщенным координатам и подставив в уравнение (5), получим уравнения системы:
ЛИТЕРАТУРА
[1] Официальный STMicroelectronics
STM32VLDISCOVERY STM32 value line Discovery [Электронный ресурс]. - Режим доступа : http://goo.gl/3aSbyB
[2] Официальный STMicroelectronics STM32VLDISCOVERY STM32 value line Discovery [ресурс]. - Режим доступа : http://goo.gl/KBazYE
[3] Analog devices accelerometer ADXL335 [Электронный ресурс]. - Режим доступа: http://goo.gl/LAslK
[4] Philips Semiconductors PCD8544 48 x 84 pixels matrix LCD controller/driver [Электронный ресурс]. -Режим доступа : http://goo.gl/sO6IdH
[5] Формальский А.М. Управление движением неустойчивых объектов. - М.: ФИЗМАТЛИТ, 2012. - 232 с. - ISBN 978-5-9221-1460-8
[6] Основы теории непрерывных и дискретных систем регулирования. - 5-е изд., перераб. и доп.: учеб. пособие / А.С. Востриков, Г.А. Французова, Е.Б. Гаврилов. - Новосибирск : Изд-во НГТУ, 2008. -476 с. ISBN 978-5-7782-1129-9
Development of Automative Stabilization
System in Vertical Position of Two-Wheeled Platform
A. YU. IVOILOV, V.A. ZHMUD, V.G. TRUBIN
Abstract: The paper discusses the problems
involved with the development of automotive
stabilization systems, in particular, such sustem in vertical position of object "inversed pemdulum". The paper outlines the stages of the development: creating of electronic circuit, development of controlling software modules, getting of the mathematical model of the system. For the appropriate functioning of automotive stabilization systems the measuring of the stabilized parameter is necessary. In addition, it is often necessary to get not onle this value, but also its derivative. The measuted signal as a rule contains disturbance which makes difficulties to get its derivative. Also the paper discucces the questions of signal foltering, particularly, the use of derivating filters for the derivative getting. As well the paper discusses the use of liquid-crystall panel for indication of the information of real-time working system.
Key words - Automative control systems, inverse pendulum, accelerometer, servomotor,
mycrocontroller, DISCOVERY, derivative filter, liquid-crystall panel.
