Способ и устройство мобильного робота для прохождения замкнутых контуров и лабиринтов
METHOD AND MOBILE ROBOT DEVICE FOR PASSING CLOSED CONTOURS AND LABYRINTHS

Изобретение относится к робототехнике и может быть использовано в роботах, например для поиска пути в лабиринтах и пересеченной местности.
Известен мобильный робот [Патент РФ №2487007, кл. B25J 5/00, 2012 (аналог)], который содержит платформу, устройство для обнаружения препятствия, манипулятор, позиционно чувствительный датчик, энкодер.
Недостатком является необходимость использования датчика энкодера с целью вычисления углов поворота мобильного робота, что усложняет конструкцию устройства, а сложная система определения расстояния с помощью позиционно чувствительного датчика на шарнирно закрепленном щупе увеличивает время принятия решения по объезду препятствий.
Наиболее близким изобретением по технической сущности является мобильный робот [Патент РФ №2274543, кл. B25J 5/00, 2006 (прототип)]. Устройство содержит блок обнаружения препятствия, блок определения текущего местоположения, блок наблюдения за текущим пространством перед роботом и источник питания.
Недостаток этого устройства заключается в том, что оно не способно принимать решения о перемещении самостоятельно, а управляется только серверным компьютером через канал Интернет, что не позволит его использование там, где нет сигнала интернета.
Известен способ нечеткого управления движением подвижного состава рельсового транспортного средства с возможностью плавного управления тягой [Патент РФ №2296357, кл. G05B 13/02, B60L 15/20, 2017 (аналог)].
Недостатком является то, что он основан на традиционных моделях нечеткого вывода, где число заключений равно числу нечетких правил управления (НПУ). Причем с ростом числа НПУ увеличивается и число заключений. Это значительно увеличивает сложность вычислений.
Наиболее близким по способу вычисления является способ управления скоростью резания на токарном оборудовании с ЧПУ [Патент РФ №2465115, кл. B23Q 15/00, 2012 (прототип)].
Недостатком описанного способа заключается в использование жестких операторов для реализации операции композиции в структуре нечеткого вывода. Это приводит к появлению зон нечувствительности при минимальных и максимальных значениях, что приводит к неаддитивности системы управления.
Техническим результатом изобретения является снижение сложности вычислений и устранения зон нечувствительности при прохождении мобильным роботом замкнутых контуров и лабиринтов.
Техническая задача изобретения решается тем, что в мобильный робот для прохождения замкнутых контуров и лабиринтов состоящий из платформы, двух электродвигателей, двух колес, платы управления электродвигателями, роликового стабилизатора горизонтальной плоскости, трех инфракрасных датчиков расстояния и платы обработки информации, введены сервопривод, ультразвуковой датчик расстояний, блок нечетких вычислений, блок расчета угла поворота и блок принятия решений.
Изобретение поясняется чертежами: фиг. 1 - общий вид мобильного робота для прохождения замкнутых контуров и лабиринтов, вид сверху; фиг. 2 - общий вид мобильного робота для прохождения замкнутых контуров и лабиринтов, вид справа; фиг. 3 – функциональная схема мобильного робота для прохождения замкнутых контуров и лабиринтов; фиг. 4 – схема– поиска максимального расстояния и угла поворота α;  фиг. 5 – функция принадлежности входной переменной величины сигнала ШИМ (широтно-импульсная модуляция);  фиг. 6 – функция принадлежности входной переменной угол (α);  фиг. 7 – функция принадлежности выходной переменной задержки времени.
Мобильный робот для прохождения замкнутых контуров и лабиринтов содержит платформу 1, правый и левый электродвигатели 2 и 4, правое и левое колеса 3 и 5, роликовый стабилизатор горизонтальной плоскости 6, сервопривод 7, ультразвуковой датчик расстояния 8, центральную неподвижную угловую опору 9, центральный инфракрасный датчик расстояний 10, правое угловое креплении 11,  правый инфракрасный датчик расстояний 12, левое угловое крепление 13, левый инфракрасный датчик расстояний 14, плату управления двигателями 15 (порты rd1, rd2, ld1, ld2, D4, D5, D6, D7), микроконтроллер 16 (порты rid1, rid2, rid3, cid1, cid2, cid3, lid1, lid2,lid3, sp1, sp2, sp3, ud1, ud2, ud3, ud4, M4, M5, M6, M7, POW), содержащий блок нечетких вычислений и принятия решений 17, блок расчета угла поворота 18 и блок питания 19.
Связи в мобильном роботе для прохождения замкнутых контуров и лабиринтов расположены следующим образом. На платформе 1 закреплены правый и левый электродвигатели 2 и 4, на которых жестко установлены правое и левое колеса робота 3 и 5.  Правый электродвигатель 2 соединен проводами с платой управления электродвигателями 15 через порты rd1 и rd2, а левый электродвигатель 4 соединен проводами с платой управления электродвигателями 15 через порты ld1 и ld2.  Входы и выходы платы управления двигателями 15 соединены портами D4, D5, D6, D7 с входами и выходами микроконтроллера 16 M4, M5, M6, M7 соответственно. Левый и правый инфракрасные датчики расстояний 12 и 14 закреплены на угловых креплениях  11 и 13 установленных на платформе 1 под углом 45 градусов по отношению к центральной оси платформы 1, а центральный инфракрасный датчик расстояний 10 закреплен на угловой опоре 9 в передней части платформы 1. Угловые крепления 11 и 13, а также угловая опора 9 жестко закреплены на платформе 1. Информационный выход левого инфракрасного датчика расстояния 14 соединен с портом lid2 микроконтроллера 16, информационный выход правого инфракрасного датчика расстояния 12 соединен с портом rid2 микроконтроллера 16, информационный выход центрального инфракрасного датчика расстояния 10 соединен с портом cid2 микроконтроллера 16. Ультразвуковой датчик 8 закреплен на подвижной опоре сервопривода 7, который закреплен в передней части платформы 1. Информационные выходы ультразвукового датчика 8 расстояний соединены с входами микроконтроллера 16 через порты ud2 и ud3. Вход сервопривода 7 соединен  с выходом микроконтроллера 16 через порт sp2. Блок нечетких вычислений и принятия решений 17 и блок расчета угла поворота 18 встроены в микроконтроллер 16. На платформе 1 закреплен блок питания мобильного робота 19, вход и выход которого подключен к разъему питания POW микроконтроллера 16. На платформе 1 (снизу) установлен роликовый стабилизатор 6. Порты микроконтроллера 16 rid1, cid1, lid1, sp1, ud1 подключены через порт POW к контакту питания +5V блока питания 19. Порты микроконтроллера 16 rid3, cid3, lid3, sp3, ud4 подключены через порт POW к контакту заземления  (GND) блока питания 19.
Принцип работы мобильного робота для прохождения замкнутых контуров и лабиринтов заключается в следующем. Напряжение от блока питания 19 поступает в  микроконтроллер 16. Микроконтроллер 16 предает сигнал ШИМ в диапазоне от [0, 255] через выходы M4 и M5 на входы платы управления электродвигателями 15 D4 и D5. Затем напряжение поступает на правый и левый электродвигатели 2 и 4 через порты rd1, rd2 и ld1,  ld2, что приводит мобильный робот для прохождения замкнутых контуров и лабиринтов в движение. При этом через выходы M6 и M7 микроконтроллера 16 передается сигнал о направлении вращения колес 3 и 5 на входы платы управления электродвигателями 15 D6 и D7 в следующей зависимости: при подаче логической «1» на M6 и M7 – движение вперед,  при подаче логического «0» на M6 и  M7 – движение назад, при подаче логической «1» на M6, а логического «0» на M7 - поворот направо, при подаче логического «0» на M6, а логической «1» на M7 - поворот влево.
1
2
3
n
n
n
max
max
n
n
max
max
max
max
max
max
max
В процессе движения мобильный робот для прохождения замкнутых контуров и лабиринтов постоянно сканирует пространство с помощью инфракрасных датчиков расстояний 10, 12, 14 и передает данные о наличии препятствий по портам  rid2, cid2 и  lid2 в микроконтроллер 16. Данные из микроконтроллера 16 поступают в блок расчета угла поворота 18 и блок нечетких вычислений и принятия решений 17, где они обрабатываются, и принимается решение о следующем действии. При отсутствии препятствий мобильный робот для прохождения замкнутых контуров и лабиринтов продолжает двигаться прямо. Как только препятствие обнаружено, т.е. расстояние от центрального инфракрасного датчика 10 или левого инфракрасного датчика 14 или правого инфракрасного датчика 12 меньше порогового значения расстояния, питание на электродвигатели 2 и 4 отключается, и робот останавливается, т.е. от микроконтроллера 16 через порты M4 и M5 на порты D4 и D5 платы управления двигателями 15 передается сигнал ШИМ равный 0, вследствие этого шпиндели электродвигателей 2 и 4 перестают вращаться и мобильный робот останавливается. Далее от микроконтроллера 16 на порт sp2 сервопривода 7, на котором закреплен ультразвуковой датчик расстояний 8, подается сигнал для вращения. Путем поворота сервопривода 7 от 0° до 180° через каждые 10° делается замер расстояний (l, l, l, …, l) с помощью ультразвукового  датчика расстояний 8. Эти данные попадают в массив, где каждому значению расстояния l ставится в соответствие угол α, на который был повернут сервопривод 7. Построенный массив передается в микроконтроллер 16 и затем в блок расчета угла поворота 18. Из массива размеров находится максимальное расстояние l и определяется  связанный с ним угол поворота α. Причем в массив попадает 19 пар значений [l, α] и если l на 15 позиции, следовательно, угол поворота связанный с ним определиться как α = (15 – 1)⋅10 =140°. Далее для вычисления угла поворота необходимо сделать проверку. Если α больше 90°, то его необходимо преобразовать по правилу α = α – 90° = 140 – 90 = 50° и робот понимает, что необходимо совершить поворот на 50° вправо. Для этого от микроконтроллера 16 на плату управления электродвигателями 15 подается сигнал логической «1» на M6, а логического «0» на M7.  В случае если угол α меньше 90°, например α = 60°, то роботу необходимо повернуться влево и расчет производится по правилу α  = 90° – α = 90 – 60 = 30°. После этого от микроконтроллера 16 на плату управления электродвигателями 15 подается сигнал логического «0» на M6, а логической «1» на M7. Рассчитанный и преобразованный угол α передается в блок нечетких вычислений и принятия решений 17, где согласно способу, рассмотренному ниже, определяется временная задержки t для поворота, в течение которого сигнал ШИМ действует через порты M4 и M5 на электродвигатели 2 и 4. В течении этого времени задержки t мобильный робот поворачивается на требуемый угол α.
Способ для прохождения замкнутых контуров и лабиринтов мобильным роботом с целью расчета времени задержки, в течение которого сигнал ШИМ от микроконтроллера 16 действует через порты M4 и M5 на электродвигатели 2 и 4 для поворота его на требуемый угол α, состоит из следующих шагов.
1
2
3
4
5
1
2
3
1
2
3
4
5
6
7
Шаг 1. Фаззификация входных переменных. Формируется нечеткая MISO-система, у которой на входе величина сигнала ШИМ  S=[s]+[s]+[s]+[s]+[s]  и требуемый угол поворота A=[a]+[a]+[a], а на выходе временная задержка подачи питания на электродвигатели 2 и 4 T=[t]+[t]+[t]+[t]+[t]+[t]+[t]. Входная лингвистическая переменная угла поворота имеет три терма, сигнала ШИМ пять термов, а выходная времени задержки t семь термов.
Шаг 2. Происходит определение степеней принадлежности для каждой из входных переменных на основе информации о текущем значении параметра сигнала ШИМ на двигателях мобильного робота для прохождения замкнутых контуров и лабиринтов и требуемого угла поворота для продолжения движения. В результате определяются фаззифицированные вектора значений для каждого терма функции принадлежности s’, a’.
<img file="00000001.jpg" he="10" wi="44" img-content="undefined" img-format="jpg" /><img file="00000002.jpg" he="10" wi="33" img-content="undefined" img-format="jpg" />
(1)
Шаг 3. Формирование базы знаний, в которой содержатся нечеткие правила вида «Если … То » (табл. 1).
Таблица 1. Нечеткая база знаний
НП
Если
То
НП
Если
То
НП
Если
То
НП<sub>1</sub>
s<sub>1</sub>
a<sub>1</sub>
t<sub>7</sub>
НП<sub>6</sub>
s<sub>2</sub>
a<sub>3</sub>
t<sub>4</sub>
НП<sub>11</sub>
s<sub>4</sub>
a<sub>2</sub>
t<sub>3</sub>
НП<sub>2</sub>
s<sub>1</sub>
a<sub>2</sub>
t<sub>6</sub>
НП<sub>7</sub>
s<sub>3</sub>
a<sub>1</sub>
t<sub>5</sub>
НП<sub>12</sub>
s<sub>4</sub>
a<sub>3</sub>
t<sub>2</sub>
НП<sub>3</sub>
s<sub>1</sub>
a<sub>3</sub>
t<sub>5</sub>
НП<sub>8</sub>
s<sub>3</sub>
a<sub>2</sub>
t<sub>4</sub>
НП<sub>13</sub>
s<sub>5</sub>
a<sub>1</sub>
t<sub>3</sub>
НП<sub>4</sub>
s<sub>2</sub>
a<sub>1</sub>
t<sub>6</sub>
НП<sub>9</sub>
s<sub>3</sub>
a<sub>3</sub>
t<sub>3</sub>
НП<sub>14</sub>
s<sub>5</sub>
a<sub>2</sub>
t<sub>2</sub>
НП<sub>5</sub>
s<sub>2</sub>
a<sub>2</sub>
t<sub>5</sub>
НП<sub>10</sub>
s<sub>4</sub>
a<sub>1</sub>
t<sub>4</sub>
НП<sub>15</sub>
s<sub>5</sub>
a<sub>3</sub>
t<sub>1</sub>
Шаг 4. Построение матрицы нечетких отношений.
Таблица 2. Матрица нечетких отношений
Выходной терм
Композиция
Максимум
t'<sub>7</sub>
b<sub>1</sub>=soft-min(s<sub>1</sub>; a<sub>1</sub>)
b<sub>1</sub>
t'<sub>6</sub>
b<sub>2</sub>=soft-min(s<sub>1</sub>; a<sub>2</sub>)
b<sub>4</sub>=soft-min(s<sub>2</sub>; a<sub>1</sub>)
soft-max(b<sub>2</sub>, b<sub>4</sub>)
t'<sub>5</sub>
b<sub>3</sub>=soft-min(s<sub>1</sub>; a<sub>3</sub>)
b<sub>5</sub>= soft-min(s<sub>2</sub>; a<sub>2</sub>)
b<sub>7</sub>=soft-min(s<sub>3</sub>; a<sub>1</sub>)
soft-max(b<sub>3</sub>, b<sub>5</sub>, b<sub>7</sub>)
t'<sub>4</sub>
b<sub>6</sub>=soft-min(s<sub>2</sub>; s<sub>3</sub>)
b<sub>8</sub>= soft-min(s<sub>3</sub>; a<sub>2</sub>)
b<sub>10</sub>=soft-min(s<sub>4</sub>; a<sub>1</sub>)
soft-max(b<sub>6</sub>, b<sub>8</sub>, b<sub>10</sub>)
t'<sub>3</sub>
b<sub>9</sub>=soft-min(s<sub>3</sub>; a<sub>3</sub>)
b<sub>11</sub>= soft-min(s<sub>4</sub>; a<sub>2</sub>)
b<sub>13</sub>=soft-min(s<sub>5</sub>; a<sub>1</sub>)
soft-max(b<sub>9</sub>, b<sub>11</sub>, b<sub>13</sub>)
t'<sub>2</sub>
b<sub>12</sub>=soft-min(s<sub>4</sub>; a<sub>3</sub>)
b<sub>14</sub>= soft-min(s<sub>5</sub>; a<sub>2</sub>)
soft-max(b<sub>12</sub>, b<sub>14</sub>)
t'<sub>1</sub>
b<sub>15</sub>=soft-min(s<sub>5</sub>; a<sub>3</sub>)
b<sub>15</sub>
На этом шаге согласно матрице нечетких отношений (табл. 2) и нечеткой базы знаний (табл. 1) с помощью мягких арифметической операций минимума (soft-min) и максимума (soft-max) нечеткой логики выбирается минимальное значение из термов входных переменных:
<img file="00000003.jpg" he="6" wi="15" img-content="undefined" img-format="jpg" /><img file="00000004.jpg" he="6" wi="46" img-content="undefined" img-format="jpg" /><img file="00000005.jpg" he="6" wi="52" img-content="undefined" img-format="jpg" /><img file="00000006.jpg" he="6" wi="53" img-content="undefined" img-format="jpg" /><img file="00000007.jpg" he="6" wi="55" img-content="undefined" img-format="jpg" /><img file="00000008.jpg" he="6" wi="49" img-content="undefined" img-format="jpg" /><img file="00000009.jpg" he="6" wi="17" img-content="undefined" img-format="jpg" />
(2)
В качестве мягкой арифметической операций нахождения мягкого минимума (soft-min) используется формула вида:
<maths num="1"><img file="00000010.jpg" he="13" wi="107" img-content="undefined" img-format="jpg" /></maths>
(3)
В качестве мягкой арифметической операций нахождения мягкого максимума (soft-max) используется формула вида:
<maths num="2"><img file="00000011.jpg" he="7" wi="92" img-content="undefined" img-format="jpg" /></maths>
(4)
где γ – оператор параметризации, равный 0,009.
Следует заметить, что количество заключений нечетко-логического вывода в матрице нечетких отношений будет равнять числу термов выходной переменной, то есть 7. В традиционной модели нечеткого вывода, количество заключений равняется числу нечетких правил, то есть 15. За счет рационального расположения элементов в матрице нечетких отношений уменьшается количество вычислений.
Шаг 5. Усечение термов выходной переменной, согласно формуле
<maths num="3"><img file="00000012.jpg" he="11" wi="52" img-content="undefined" img-format="jpg" /></maths>
(5)
где i=1..n – номер заключения нечетко-логического вывода; n = 7.
Шаг 6. Объединение усеченных термов выходной переменной
<maths num="4"><img file="00000013.jpg" he="11" wi="118" img-content="undefined" img-format="jpg" /></maths>
(6)
Шаг 7. Дефаззификация осуществляется на основе метода центра тяжести
<img file="00000014.jpg" he="18" wi="40" img-content="undefined" img-format="jpg" />.
(7)
Результат вычисления блока нечетких вычислений и принятия решений 17 – величина времени задержки t'' по вышеуказанному способу передается от микроконтроллера 16 через плату управления двигателями 15 на электродвигатели 2 и 4. После поворота мобильного робота на требуемый угол α он продолжает движение прямо до необходимости следующей остановки при обнаружении препятствия.
В качестве примера разберем ситуацию, когда на вход нечеткой MISO-системы подаются значение сигнала ШИМ равное 180 и требуемый угол поворота направо равный  50° вправо.
На первом шаге выполняем формирование функций принадлежности термов входной и выходной переменных.
Для входной переменной сигнала ШИМ функция принадлежности имеет следующий вид:
Функция принадлежности для входной переменной угла поворота имеет вид:
Функция принадлежности для выходной переменной времени задержки сигнала ШИМ на двигателях мобильного робота для прохождения замкнутых площадей и лабиринтов имеет вид:
На втором шаге формируем фаззифицированные вектора значений для каждого терма функции принадлежности s’, a’.
.
(0; 0,333; 0,667).
На третьем шаге строится база состояющая из 15 нечетких правил:
1
1
1
7
НП: Если «a = a» И «s = s» TO «t = t»;
2
2
1
6
НП: Если «a = a» И «s = s» TO «t = t»;
3
3
1
5
НП: Если «a = a» И «s = s» TO «t = t»;
4
1
2
6
НП: Если «a = a» И «s = s» TO «t = t»;
5
2
2
5
НП: Если «a = a» И «s = s» TO «t = t»;
6
3
2
4
НП: Если «a = a» И «s = s» TO «t = t»;
7
1
3
5
НП: Если «a = a» И «s = s» TO «t = t»;
8
2
3
4
НП: Если «a = a» И «s = s» TO «t = t»;
9
3
3
3
НП: Если «a = a» И «s = s» TO «t = t»;
10
1
4
4
НП: Если «a = a» И «s = s» TO «t = t»;
11
2
4
3
НП: Если «a = a» И «s = s» TO «t = t»;
12
3
4
2
НП: Если «a = a» И «s = s» TO «t = t»;
13
1
5
3
НП: Если «a = a» И «s = s» TO «t = t»;
14
2
5
2
НП: Если «a = a» И «s = s» TO «t = t»;
15
3
5
1
НП: Если «a = a» И «s = s» TO «t = t».
Далее на четвертом шаге строится матрица нечетких отношений.
Выходной терм
Композиция
Максимум
t'<sub>7</sub>
b<sub>1</sub>= -0,0238
-0,0238
t'<sub>6</sub>
b<sub>2</sub>= -0,0006
b<sub>4</sub>= -0,0238
-0,0018
t'<sub>5</sub>
b<sub>3</sub>= 0,0003
b<sub>5</sub>=  -0,0006
b<sub>7</sub>= -0,0238
-0,0009
t'<sub>4</sub>
b<sub>6</sub>= 0,0003
b<sub>8</sub>=  -0,0006
b<sub>10</sub>= -0,0041
0,00005
t'<sub>3</sub>
b<sub>9</sub>= -0,0006
b<sub>11</sub>= 0,1095
b<sub>13</sub>= 0,0005
0,0988
t'<sub>2</sub>
b<sub>12</sub>= 0,1111
b<sub>14</sub>= 0,3331
0,3220
t'<sub>1</sub>
b<sub>15</sub>= 0,6655
0,6655
На пятом шаге, выполняется усечение термов выходной переменной.
На шестом шаге выполняем объединение усеченных термов выходной переменной.
На седьмом шаге нечеткий вектор выходной функции принадлежности преобразуется в четкое значение временной задержки в миллисекундах с помощью метода центра тяжести:
С помощью предложенного способа осуществляется расчет нового значения временной задержки (t) сигнала ШИМ на электродвигателях 2 и 4 мобильного робота для прохождения замкнутых контуров и лабиринтов, чтобы совершить поворот на требуемый угол (α ), для объезда препятствия.
Таким образом, время задержки сигнала ШИМ на двигателях мобильного робота для прохождения замкнутых контуров и лабиринтов для поворота на 50° градусов и величине сигнала ШИМ 180 равняется 107,14 миллисекундам.