Например, Бобцов

СИСТЕМА И АЛГОРИТМЫ СТАБИЛИЗАЦИИ БОЛБОТА

СИСТЕМА И АЛГОРИТМЫ СТАБИЛИЗАЦИИ БОЛБОТА
УДК 681.5:621.865.8+519.71
СИСТЕМА И АЛГОРИТМЫ СТАБИЛИЗАЦИИ БОЛБОТА
А.С. Боргуль, В.С. Громов, К.А. Зименко, С.Ю. Маклашевич
Решена задача стабилизации неустойчивой конструкции с шаром в основании и двух присоединенных к нему приводов. Синтезированы регуляторы на основе метода оптимизации линейно-квадратичного функционала и метода обеспечения качественной экспоненциальной устойчивости. Проведены экспериментальные исследования системы управления на макете, собранном на базе робототехнического комплекса LegoNXT. Ключевые слова: болбот, перевернутый математический маятник, линейно-квадратичный регулятор, качественноэкспоненциальная устойчивость.
Введение Болбот (Ballbot) – это мобильный робот, основной задачей которого является удержание собственной конструкции в положении равновесия на сферическом катке (шаре). Динамическая устойчивость болбота в сочетании с шаром вместо колес приводит к ряду уникальных свойств в области наземного транспорта: болбот является всенаправленным, т.е. может перемещаться в любом направлении и в любой момент времени, ограничиваясь лишь собственной динамикой, но не механическими связями, как у других конструкций. Таким образом, он не должен отклоняться от курса для того, чтобы изменить направление. Все это делает его более маневренным по сравнению с другим наземным транспортом. Обладая уникальными характеристиками, такая система может найти ряд практических применений в условиях ограниченного пространства и требований к высокой маневренности, например, в условиях библиотеки или склада. Свойство динамической устойчивости позволяет использовать робота в динамических средах с возмущениями, таких как корабли, поезда [1]. Всенаправленность движения делает болбота пригодным для быстрой навигации в системах с нанесенной координатной сеткой. Также в условиях роста игровой индустрии и повышающегося интереса к созданию роботов, использующихся в сфере обслуживания, болбот обладает большими перспективами к дальнейшему развитию. Однако до сих пор болбот остается объектом научных исследований и служит узко определенным целям.
58 Научно-технический вестник Санкт-Петербургского государственного университета
информационных технологий, механики и оптики, 2011, № 5 (75)

А.С. Боргуль, В.С. Громов, К.А. Зименко, С.Ю. Маклашевич
Постановка задачи
Рассматривается задача стабилизации болбота, представляющего собой робототехнический комплекс на подвижном основании (шаре), которое образует систему независимых перевернутых маятников. Задача решается при дополнительных условиях – неучтенной динамике и внешних возмущениях. Требуется выявить оптимальный метод стабилизации болбота с заданными показателями качества как при позиционировании на месте, так и при траекторном управлении, а также при наличии дополнительных управляющих воздействий.
Устройство робота
Основными элементами конструкции являются: шар, контроллер, два гироскопа, два двигателя постоянного тока со встроенными редукторами и энкодерами. Добавлен ультразвуковой датчик расстояния для контроля препятствий при перемещении и взаимодействия с объектами. Пластиковый шар в основании закреплен с трех сторон – одним свободно вращающимся колесом и двумя присоединенными к двигателям, с помощью которых осуществляется управление конструкцией.

Рис. 1. Основные элементы конструкции

Математическая модель

Рассматривается модель, в которой движения в продольном и поперечном направлениях не связаны, и уравнения движения в этих двух плоскостях одинаковы. Тогда болбот можно рассматривать как две модели отдельных одинаковых перевернутых маятников на сферическом катке [2]. На рис. 2 показа-
на система координат перевернутого маятника на сферическом катке, где  – угол отклонения конст-

рукции;  – угол поворота катка; s – угол поворота катка, задаваемый двигателем. Двигатель вращает

сферический каток через колесо с резиновой покрышкой. Допускается, что между ними нет скольжения.

m – угол поворота двигателя. Rwm  Rss .

Опишем уравнение движения перевернутого маятника на сферическом катке методом Лагранжа,

основываясь на системе координат, указанной ранее. Если  =0 при t=0, тогда каждая координата зада-

ется следующим образом: (xs , zs )  (Rs, zb ) , (xs , zs )  (Rs,0) ,
(xb, zb)  (x  Lsin, z  Lcos) , (xb, zb )  (Rs  L cos,L sin) .

Кинетическая энергия поступательного движения T1, вращательного движения T2, потенциальная энергия U записываются следующим образом:

T1



1 2

M

s

(

xs2



zs2

)



1 2

Mb

(xb2

 zb2 )

,

T2



1 2

Js 2s



1 2

J 2



1 2

Jw m2



1 2

Jm m2



1 2

J s 2s



1 2

J 2



1 2

(

J

m



Jw)

Rs2 Rw2

(   )

,

U  M s gzs  M b gzb .

Лагранжиан L имеет вид

L  T1  T2 U .

Используем  и  как обобщенные координаты. Уравнение Лагранжа имеет вид

d dt

 

L 

  

L 



F

,

d dt



L 

 

L 



F

.

Научно-технический вестник Санкт-Петербургского государственного университета информационных технологий, механики и оптики, 2011, № 5 (75)

59

СИСТЕМА И АЛГОРИТМЫ СТАБИЛИЗАЦИИ БОЛБОТА Z zb Mb, J
L  s

zs Ms,Js

R

xs xb X

Рис. 2. Система координат перевернутого маятника

С учетом постоянного крутящего момента двигателя и вязкого трения обобщенные силы равны F  Kti  fm m  fs , F  Kti  fm m ,

где i – ток двигателя;  m  k(   ) – угловая скорость двигателя. Без учета трения внутри двигателя и

индуктивности обобщенные силы выражаются [3] следующим образом:

F

   ( 

fs )   , F

      , 



Kt Rm

,





k

(

Kt Kb Rm



fm) .

Линеаризуем уравнения состояния, рассматривая предел   0 (sin   , cos   1) и пренебре-

гая слагаемым второго порядка  2 , в векторно-матричной форме имеем:

E

  

  



F

    



G 



H

,

E



(M 

b



 M s )Rs2 MbLRs 

 Js  k2Jm

Jm

MbLRs  MbL2  J

k2Jm  k2J

m

  

,

F



  fs

 



 



 

,

G



0 0

0  MbgL

,

H



  

.

Обозначим x – вектор состояния; u – вход.

 x      T ; u   ;

Таким образом, представим уравнения состояния в форме Коши: x  A(t)x(t)  B(t)u(t)

0

A



0 0

0

0 0 A(3,2) A(4,2)

1 0 A(3,3) A(4,3)

0

0

1

 

A(3,4)

,

B



 

0

 

 B(3) 

;

A(4,4)

B(4)

A(3,2)  MbgLE(1,2) / det(E) ;

A(4,2)  MbgLE(1,1) / det(E) ;

A(3,3)  [(  fs )E(2,2)  E(1,2)]/ det(E) ;

A(4,3)  [(  fs )E(1,2)  E(1,1)]/ det(E) ;

A(3,4)  [E(2,2)  E(1,2)]/ det(E) A(4,4)  [E(1,1)  E(1,2)] / det(E) ;

B(3)  [E(2,2)  E(1,2)] / det( E) ;

B(4)  [E(1,1)  E(1,2)]/ det(E) det(E)  E(1,1)E(2,2)  E(1,2)2 .

Расчет регуляторов

На вход системы управления подается напряжение управления двигателями. Выходом системы являяются значения с энкодеров угла поворота двигателя m и угловая скорость отклонения конструкции от вертикали  . Численное значение угла отклонения  получается путем интегрирования угловой
скорости  . Начальные условия при интегрировании определяются из условия вертикального запуска,

60 Научно-технический вестник Санкт-Петербургского государственного университета
информационных технологий, механики и оптики, 2011, № 5 (75)

А.С. Боргуль, В.С. Громов, К.А. Зименко, С.Ю. Маклашевич

выбранного положения объекта. Положение равновесия является неустойчивым. При минимальном отклонении необходимо двигать болбот в направлении угла наклона конструкции, чтобы удержать баланс.
Линейно-квадратичный регулятор. Для решения задачи удержания перевернутого маятника в положении неустойчивого равновесия синтезируется пропорционально-интегральный регулятор (рис. 3).

C – матрица выхода для получения  из x.

xref

ref C



kt

NXT Болбот

C

kf

Рис. 3. Схема пропорционально-интегрального регулятора

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

линейно-квадратичного регулятора. Линейно-квадратичный регулятор – в теории управления один из видов оптимальных регуляторов, использующий квадратичный функционал качества [4]. Выберем весовые матрицы Q и R [5]:

1 0 0 0 0 

0 6 105 0 0 Q  0 0 1 0
0 0 0 1

0 0

  ,

R  6 103   180 2

,

0

 



0 0 0 0 103 

где Q(2,2) – элемент весовой матрицы состояния, характеризующий вес значения угла отклонения конст-

рукции, Q(5,5) – элемент весовой матрицы состояния, характеризующий вес по времени интегрирования разницы между ранее измеренным и полученным углами.

Используя полученные ранее математическую модель робота и весовую матрицу, рассчитаем в среде Matlab численные значения коэффициентов для пропорциональной и интегральной составляющих. Функция lqr вычисляет матрицу коэффициентов регулирования со среднеквадратичным функционалом качества [6]. В результате вычислений получается набор коэффициентов:

kf = [–0,015 –1,5698 –0,027 –0,2325], ki = –0,0071. Качественная экспоненциальная устойчивость. Построим регулятор, обеспечивающий экспо-

ненциальную сходимость со следующими показателями качества: tп  0,36 c; п  0 . В линейных системах подлежит минимизации квадратичный критерий качества

 
J x,u   xT tQtxt uT tRtut dt . 0

Непрерывная система экспоненциально устойчива в точке x  0 , если существуют такая квадра-

тичная функция Ляпунова

V xt  xT tPtxt ,

где P  PT – положительно определенная n n матрица, и такой параметр  :   0 , при которых на всех траекториях движения системы в любой момент времени t  0 выполняется условие
Vxt  2V xt .
Данное условие имеет место, если справедливо уравнение
Vxt 2V xt  xT tQtxt uT tRtut .
Подставив квадратичную функцию Ляпунова и ее производную, получим:
Atxt Btut xtT Ptxt xT tPtAtxt Btut xt xT tPtxt
 xT tQtxt uT tRtut  0 ,

Научно-технический вестник Санкт-Петербургского государственного университета информационных технологий, механики и оптики, 2011, № 5 (75)

61

СИСТЕМА И АЛГОРИТМЫ СТАБИЛИЗАЦИИ БОЛБОТА

на основе которого, воспользовавшись методом локальной оптимизации [7], находим оптимальное управление:
ut  R1tBT tPtxt .
Получим систему матричных уравнений типа Риккати [7]:
Pt At BtK t I T Pt PtAt BtK t I  K T tRtK t Qt  0 ,
K t  R1tBT tPt .
Так как матрицы A, B,Q, R постоянны, то процесс достигает установившегося состояния в том
смысле, что становится постоянной матрица P ( P  0 ):
A  BK  I T P  PA  BK  I  KT RK  Q  0 , K  R1BT P .
Из матричных алгебраических уравнений получается следующая матрица обратных связей K регулятора: K f [0,0117 1,4993 0,0284 0,2432].
Сравнительный анализ полученных регуляторов (линейно-квадратичного регулятора на рис. 4 и регулятора на основе метода качественной экспоненциальной устойчивости на рис. 5) проведен на основе экспериментальных данных.
m, рад

15 10

5

0 –5
0 5 10 15 20 25 t, с

, рад/с

а

0

–50 –100
–150
–200 0 5 10 15 20 25 t, с
б

Рис. 4. Показания при использовании линейно-квадратичного регулятора: угол поворота двигателя (а); скорость изменения угла крена (б)

m, рад

15 10

5

0 –5
0 5 10 15 20 25 t, с

, рад/с

а

0

–50 –100
–150
–200 0 5 10 15 20 25 t, с
б

Рис. 5. Показания при использовании регулятора на основе метода качественной экспоненциальной устойчивости: угол поворота двигателя (а); скорость изменения угла крена (б)

62 Научно-технический вестник Санкт-Петербургского государственного университета
информационных технологий, механики и оптики, 2011, № 5 (75)

А.А. Бобцов, А.С. Боргуль, К.А. Зименко, А.А. Пыркин

Из полученных графиков видно, что использование качественно-экспоненциального регулятора приводит к уменьшению колебательности процесса стабилизации болбота и более точному позиционированию конструкции.
Заключение
В результате проделанной работы был получен опытный образец и синтезирован алгоритм, позволяющий стабилизировать в вертикальном положении объект на сферическом катке, обладающий достаточным запасом качества для отработки дополнительных управляющих воздействий. Был произведен синтез линейно-квадратичного регулятора и регулятора на основе качественной экспоненциальной устойчивости. Анализ полученных данных показал, что регулятор на основе качественной экспоненциальной устойчивости обеспечивает лучшие точностные и качественные показатели, а, следовательно, является более предпочтительным в условиях поставленной задачи.
Литература
1. Lauwers T.B., Kantor G.A., Hollis R.L. A dynamically stable single-wheeled mobile robot with inverse mouse-ball drive // IEEE International Conference on Robotics and Automation. – 2006. – 2884 р.
2. Nagarajan U., Mampetta A., Kantor G., Hollis R. State transition, balancing, station keeping, and yaw control for a dynamically stable single spherical wheel mobile robot // IEEE International Conference on Robotics and Automation. – 2009. – P. 3161–3166.
3. Колюбин С.А., Пыркин А.А. Управление нетривиальными маятниковыми системами в условиях параметрической и функциональной неопределенностей // Научно-технический вестник СПбГУ ИТМО. – 2010. – № 69. – С. 34–39.
4. Anderson B.D.O., Moore J.B. Optimal control: linear quadratic methods. – Prentice-Hall, 1989. – 394 p. 5. Ha Y.-S., Yuta S. Trajectory tracking control for navigation of self-contained mobile inverse pendulum //
Proc. IEEE/RSJ Int’l. Conf. on Intelligent Robots and Systems. – 1994. – Р. 1875–1882. 6. Linear-Quadratic-Regulator (LQR) design – MATLAB [Электронный ресурс]. – Режим доступа:
http://www.mathworks.com/help/toolbox/control/ref/lqr.html, св. Яз. англ. (дата обращения 18.03.2011). 7. Быстров С.В., Григорьев В.В., Рабыш Е.Ю., Черевко Н.А. Экспоненциальная устойчивость непре-
рывных динамических систем // Научно-технический вестник СПбГУ ИТМО. – 2011. – № 73. – С. 44–47.

Боргуль Александр Сергеевич

– Санкт-Петербургский государственный университет информационных

технологий, механики и оптики, студент, borgulalexandr@gmail.com

Громов Владислав Сергеевич

– Санкт-Петербургский государственный университет информационных

технологий, механики и оптики, студент, object253@yandex.ru

Зименко Константин Александ- – Санкт-Петербургский государственный университет информационных

рович

технологий, механики и оптики, студент, kostyazimenko@gmail.com

Маклашевич Сергей Юрьевич

Санкт-Петербургский государственный университет информационных

технологий, механики и оптики, студент, s.maklashevich@gmail.com

Научно-технический вестник Санкт-Петербургского государственного университета информационных технологий, механики и оптики, 2011, № 5 (75)

63