CC BY
25
БИОМЕХАТРОНИКА
УДК 681.52
DOI: 10.17586/0021-3454-2018-61-2-174-180
АЛГОРИТМ СОВМЕСТНОГО УПРАВЛЕНИЯ ДВИЖЕНИЕМ ИНТЕЛЛЕКТУАЛЬНОГО ИНВАЛИДНОГО КРЕСЛА
С. А. Колюбин, Н. Ю. Дема
Университет ИТМО, 197101, Санкт-Петербург, Россия E-mail: s.kolyubin@corp.ifmo. ru
Рассматривается задача ручного управления инвалидным креслом с электродвигателем при наличии ошибки в управляющем задании, поступающем с джойстика. Разработан алгоритм совместного управления, обеспечивающий предотвращение столкновений с препятствиями при движении в сложном динамическом окружении. Идентификация препятствий осуществляется с помощью оптических дальномеров, установленных по периметру рассматриваемого объекта управления. Для экспериментальной апробации разработанного алгоритма используется мобильная робототехническая платформа Robotino (Festo-Didactic).
Ключевые слова: мобильный робот, совместное управление, предотвращение столкновений
Введение. По данным ВОЗ, в 2011 году около 1 % населения Земли нуждались в инвалидной коляске [1]. Часть этих людей неспособны, например ввиду болезней, справиться с механической инвалидной коляской. В таких случаях коляска с электроприводом является хорошим решением, но передвижение в общественных местах требует от человека, находящегося в инвалидном кресле (пилота), повышенной концентрации внимания и некоторых специфичных навыков управления. Множество научных групп по всему миру предлагают различные подходы к решению этой задачи с разной степенью вовлеченности пилота в процесс управления.
В работе [2] предложен алгоритм построения модели ошибки задающих команд пилота c их коррекцией в зависимости от показаний датчиков состояния окружающей среды и коляски как объекта управления. Основу алгоритма составляет регрессия на основе гауссовых процессов. Иной подход, предложенный в [3—5], состоит в построении предикторов желаемой траектории движения по предыстории показаний датчиков и действий пилота. В работах [6—8] задача управления коляской полностью перекладывается на алгоритмы автономной навигации, а пилоту доступны только несколько команд, таких как „вперед" или „поворот".
В подавляющем большинстве работ основным устройством, используемым для получения информации об окружающем пространстве, является лазерный сканирующий дальномер (лидар), высокая стоимость которого — серьезное препятствие для выхода подобных систем на рынок средств реабилитации.
В настоящей статье разработаны алгоритмы совместного управления — нового, быстро развивающегося направления в организации взаимодействия людей и роботов, где человек и система автономного управления кооперируются для достижения общей цели. Такой подход позволяет безопасно и комфортно перемещаться даже неопытным пилотам. При этом для
предотвращения столкновении с препятствиями используются комплексированные показания одометрии и массива оптических дальномеров (ОД). Эти дальномеры значительно дешевле лидаров, но результаты их измерений менее точны, поэтому необходима разработка дополнительного алгоритмического обеспечения.
Функциональная схема системы. На рис. 1, а представлена функциональная схема разработанной системы. Блок алгоритма совместного управления принимает на вход значения скорости, поступающие с джойстика, и значения параметров, посредством которых выполняется корректировка скорости мобильной платформы. Построение локальной карты происходит в соответствии с данными одометрии и показаниями оптических дальномеров. Значения параметров высчитываются адаптивно исходя из близости различных объектов к границе мобильной платформы, а также частоты обновления карты относительно частоты поступления задающей команды скорости с джойстика. В блоке фильтра низких частот происходит фильтрация данных, поступающих от оптических дальномеров (этот блок необходим вследствие значительного влияния шумов на качество построенной локальной карты).
Для определения препятствий вне прямой видимости оптических дальномеров строится временная локальная карта (далее — карта) в некоторой области вокруг мобильной платформы.
Карта представляет собой двумерный массив, каждый элемент которого может принимать одно из трех значений: неизвестно (серый цвет), занято (черный) и свободно (белый) (см. рис. 1, б). Карта непрерывно смещается так, чтобы ее центр совпадал с началом отсчета системы координат мобильной платформы. Ось вращения системы координат неподвижна относительно оси вращения системы координат отсчета одометрии. Каждый элемент карты представляет собой квадрат с заранее устанавливаемым периметром, тем самым определяя разрешающую способность карты.
а)
Действие
и, ю
Алгоритм совместного управления
кг|ки
Блок адаптивной настройки параметров
М
и, ю Мобильная платформа
Одометрия ОД
Блок построения карты
к Уг
к V г
Фильтры низких частот
Я
Я
б)
Близко ... о
Далеко Свободно
Занято Неизвестно ■
Грань 6
С
С
Рис. 1
Фиксация препятствий на карте происходит на основе данных об относительном расстоянии от оптических дальномеров и текущих координат самих дальномеров.
При этом на каждом шаге происходит сдвиг карты в соответствии с информацией о перемещении мобильной платформы: текущее значение о положении в системе координат отсчета одометрии сравнивается с предыдущим. Если значения по одной из осей превышают половину длины разрешающей способности карты, то значения каждой ячейки смещаются в направлении, противоположном движению. Расстояние смещения соответствует пройденному вдоль соответствующей оси расстоянию, деленному на разрешающую способность карты.
Метод фильтрации данных оптических дальномеров. Для подавления шума в датчиках расстояния предлагается использовать метод экспоненциально взвешенного скользящего среднего [9]. Фильтр описывается следующей формулой:
рг =ащ +(1 -а) рг_ь (1)
к
X
г
где рг — значение выходной переменной фильтра для текущего измерения, и1 — значение, полученное с лазерного оптического дальномера на текущей итерации измерений, а — сгла-
живающая константа:
АТ
~т
а = 1 - е
где ДТ — период между измерениями, а / — частота фильтра.
Предложенный фильтр обладает меньшими, по сравнению с фильтрами более высокого порядка, ресурсоемкостью и задержкой в сигнале.
Коррекция показаний одометрии выполняется следующим образом:
4' Ъхх Ъху Ъхг ~ хк' " хк"
ук = Ъух ЪУУ Ук + в = В ук + в, (2)
4 _ Ъгх ЪУ Ъа _ 1к
где В — матрица коэффициентов коррекции одометрии, определяемая экспериментально, в — нелинейная составляющая ошибки, которую далее будем считать несущественной,
{хкук, | — координаты платформы, рассчитанные на основе показаний энкодеров,
{х^У^, укг | — скорректированные координаты платформы на плоскости. Для определения матрицы коэффициентов коррекции одометрии В используется метод наименьших квадратов [10].
Алгоритм совместного управления. Для каждого управляющего задания с джойстика, формируемого как совокупность желаемых угловой и линейной скоростей, определяется радиус окружности, который описывает центр системы координат мобильной платформы:
180°их
г = ■
па
Далее рассматривается мгновенный центр скоростей:
Я = (0, г).
Таким образом, зная скорость мобильной платформы, а следовательно и радиус вращения каждой из ее точек, можно предсказать, где окажется каждая такая точка в последующие моменты времени, при условии, что скорость платформы постоянна.
Предлагаемый в настоящей работе алгоритм основан на итеративной обработке управляющего задания скорости и данных с карты. В зависимости от расстояния каждой точки платформы до препятствия считаются параметры ки и кю, на которые умножаются значения соответствующих скоростей. Так как расчет параметров происходит параллельно процессу корректировки скорости, то вводится соответствующий параметр кп учета разности частоты обновления параметров ки и кю и частоты получения управляющего задания скорости. Все введенные параметры нормированы от 0 до 1.
Расчет параметров выполняется следующим образом. Зададим множество точек в системе координат платформы, определяющих границу мобильной платформы, как множество Г (рис. 1):
Г = { /о,.., /,.., Л}> Л ={ х, у*)> * = 0,...Д
Далее зададим множество В для каждого отрезка границы платформы:
В = {%.., В]-,.., Вп }, ] = 0,...,7,
В]- ={Ъ]0,.,Ъя ,..,Ъ]т} ,Ъп ={ хп, ул) ,1 = °,..., <; ;,
где ^ рассчитывается исходя из разрешающей способности карты q
S i =
ё (Л » +1)
2д '
ё (, /]+1) — абсолютная метрика
ё («1, а2 ) = |а2х - а1х| + |а2у - а1у
Значения каждой промежуточной точки Ъц высчитываются следующим образом:
(3)
bjl = (xji, yji)
xjl-i
jl
xjm - xj 0
yji-i
yjm - yj 0
Л
Rji =
2q 2q
Далее для каждой такой точки найдем радиус Rj¡ относительно R, по которому она будет
двигаться при неизменных значениях управляющего задания:
^ + ((¡ - R )2 .
Затем определим множество точек Pj¡ так, что:
Pj¡ = {, j¡1, . ^ Pj¡w^^ pj¡y }, pj¡w = {xj¡w, yj¡w ) Y = —,
q
где D — расстояние, начиная с которого алгоритм будет учитывать препятствие, а каждая точка определяется как
xj¡w = xj¡w-1 cos cos (■Дф) - (yjw-1 - R) sin sin (Дф), yj¡w = (¡w-1 - R) cos cos ( Дф) + xj¡w-1 sin sin ( Дф) + R
где Дф — угол поворота, для определения координат последующих точек
■ф = sign (ю).
Rj¡
Затем ищется такая точка Pj¡w, для которой расстояние s7¡ от границы платформы j минимально:
Sjl = wC.
Обозначим такое расстояние dj. Тогда задающая команда по линейным и угловым перемещениям рассчитывается как:
и = kvu, ш = &п&юю,
(4)
kv =
где соответствующие параметры находятся следующим образом:
mind (d0, d1) / D, и > 0,
mind. (d4, d5 )/ D, и<0,
0 и = 0,
К = mindt (d0,d7) / D,
k = i -/speed _ ./map =1 / ,
speed
где /speed и /map — частота обновления задающих команд и и ю и обновления карты соответственно. Расчет частот происходит при каждом обновлении задающего значения скорости. Все параметры нормированы к единице.
Проведение эксперимента. Для экспериментальной апробации предложенного алгоритма выбрана мобильная робототехническая платформа Robotino (FestoDidactic). На рис. 2
представлены внешний вид робота (а), схема расположения лазерных оптических дальномеров (б) и кинематическая схема колесной базы (в).
б) ОД:_
ОД9 / \
Препятствие
Рис. 2
Кинематическая модель платформы описывается следующим уравнением:
_ _ л/3 _ _
-(coseos у _ _ sin sin у4) ——(coseos у _ + sin sin у 0
"V3
»X ' 3
»У = î(
k
о
k-1
k-1
k -Ь
k-Ь
2'
1
— I
R
k-1
,k-1
k -b
k-b
R
k w>1
k w2
k W3
(5)
где k — шаг дискретизации, вектор (u^, j содержит значения линейных скоростей
соответствующих колес в системе координат платформы, вектор (иХ, и^, шк j содержит значения линейных и угловой скоростей платформы относительно неподвижной системы координат Oxy (рис. 2, в), R — расстояние (радиус), на котором расположены колеса относительно геометрического центра платформы, угол ук — ориентация платформы относительно Oxy, вычисляемая как:
/ = œkh + / -1,
где h — интервал дискретности.
Экспериментально было определено, что линейная составляющая ошибки одометрии является преобладающей, при этом положение платформы относительно Oxy можно рассчитать как:
хк =и> + хк -1, ^^ =ъкук + ук -1.
Данные о реальных перемещениях для расчета матрицы коэффициентов коррекции одо-метрии были получены при первоначальной калибровке системы, когда платформа двигалась по предварительно размеченному координатному полю. На рис. 3, а представлены исходные показания одометрии 1 и их коррекция 2 для поступательного движения вдоль оси х.
Для определения параметров шума оптических дальномеров записывались показания каждого датчика. Среднеквадратичное отклонение наиболее зашумленного сигнала составило 2,82 см. Параметр ДГ определялся итеративно, а частота фильтра /=0,6 Гц была выбрана исходя из спектрального состава сигнала. Результаты фильтрации данных от дальномеров I представлены на рис. 3, б. Среднеквадратичное отклонение после применения фильтра (2) снизилось до 0,64 см.
Заключение. Результаты экспериментальной апробации разработанной системы совместного управления иллюстрирует видеоролик, представленный на сайте <§оо.§1МРУ5Хи>. Основным преимуществом предлагаемого подхода перед аналогами является работоспособность системы при наличии минимальной информации об окружающем пространстве и использовании доступных бюджетных датчиков расстояния.
список литературы
1. Worldreportondisability // World Health Organization [Электронный ресурс]: <http://www.who.int/disabilities/ world_report/2011>.
2. Kucukyilmaz A., Demiris Y. One-shot assistance estimation from expert demonstrations for a shared control wheelchair system // Proc. IEEE Intern. Work. Robot Hum. Interact. Commun. 2015. Vol. 2. Р. 438—443.
3. Vanhooydonck D., Demeester E., Huntemann A. Adaptable navigational assistance for intelligent wheelchairs by means of an implicit personalized user model // Rob. Auton. Syst. 2010. Vol. 58, N 8. Р. 963—977.
4. Carlson T., Demiris Y. Collaborative control for a robotic wheelchair: Evaluation of performance, attention, and workload // IEEE Trans. Syst. Man, Cybern. Pt. B. Cybern. 2012. Vol. 42, N 3. Р. 876—888.
5. Demeester E., Nuttin M., Vanhooydonck D., Van Brussel H. Assessing the User's Intent Using Bayes' Rule: Application to Wheelchair Control // IEEE Intern. Work. Advan. in Serv. Robot. 2003. Vol. 4. Р. 438—443.
6. Del R Millan J. J., Galan F., Vanhooydonck D., Lew E., Philips J., Nuttin M. Asynchronous Non-Invasive Brain-Actuated Control of an Intelligent Wheelchair // Intern. Conf. of the Engineering in Medicine and Biology Society. 2009. Vol. 4. Р. 3361.
7. Trieu H. T., Nguyen H. T. and Willey K. Shared Control Strategies for Obstacle Avoidance Tasks in an Intelligent Wheelchair // Intern. Conf. of the Engineering in Medicine and Biology Society. IEEE. 2008. Vol. 30. Р. 401—405.
8. Мокиенко О. А., Черникова Л. А., Фролов А. А. Интерфейс мозг-компьютер как новая технология нейрореабилитации // Анналы клинической и экспериментальной неврологии. 2011. № 3.
9. Кичкидов А. А., Семенов Д. А., Добреля А. А. Применение методов математической статистики для фильтрации сигналов // НиКа. 2005. № 3.
10. Гафарова Л. М., Завьялова И. Г., Мустафин Н. Н. Состоятельность МНК-оценок множественной линейной регрессионной модели // ЭСГИ. 2016. № 1(9). С. 18—20.
Сведения об авторах
Сергей Алексеевич Колюбин — канд. техн. наук, доцент; Университет ИТМО; кафедра систем
управления и информатики; E-mail: s.kolyubin@corp.ifmo.ru Николай Юрьевич Дема — студент; Университет ИТМО; кафедра систем управления и инфор-
матики; инженер; E-mail: Niko_Dema@protonmail.com
Поступила в редакцию 21.11.17 г.
Ссылка для цитирования: Колюбин С. А., Дема Н. Ю. Алгоритм совместного управления движением интеллектуального инвалидного кресла // Изв. вузов. Приборостроение. 2018. Т. 61, № 2. С. 174—180.
AN ALGORITHM OF SHARED CONTROL OF INTELLECTUAL WHEELCHAIR MOVEMENT
S. A. Kolyubin, N. Yu. Dema
ITMO University, 197101, St. Petersburg, Russia E-mail: s.kolyubin@corp.ifmo.ru
The problem of manual control of a wheelchair with an electric motor is considered when there is an error in the control task coming from the joystick. A joint control algorithm is developed to prevent collisions with obstacles during movement in a complex dynamic environment. Identification of obstacles is carried out with the help of optical range finders installed along the perimeter of the control object in question. Results of experimental approbation of the developed algorithm with the use of the Robotino mobile robotics platform (FestoDidactic) are presented.
Keywords: mobile robot, shared control, collision avoidance
Data on authors
Sergey A. Kolyubin — PhD, Associate Professor; ITMO University, Department of Computer
Science and Control Systems; E-mail: s.kolyubin@corp.ifmo.ru Nikolay Yu. Dema — ITMO University, Department of Computer Science and Control Sys-
tems; Engineer; E-mail: Niko_Dema@protonmail.com
For citation: Kolyubin S. А., Dema N. Yu. An algorithm of shared control of intellectual wheelchair movement. Journal of Instrument Engineering. 2018. Vol. 61, N 2. P. 174—180 (in Russian).
DOI: 10.17586/0021-3454-2018-61-2-174-180
