Система навигации мобильного робота (стр. 5 из 7)
3.2.2 Коррекция траектории движения робота
Задача коррекции связана с организацией следящего режима посредством списывания ошибок движения и коррекции траектории. Ошибки, поступающие от управляющей ЭВМ к роботу, вызваны, с одной стороны, накоплением ошибок сенсоров линейных перемещений, а с другой – возмущающими воздействиями со стороны окружающей среды. Ошибки сенсоров перемещений обычно зависят от пройденного расстояния. Если пренебречь возмущающими воздействиями, то тогда длина отрезка пути, по прохождении которого следует производить списывание таких накапливающихся ошибок, может быть выражена следующим образом:
S = mах (х1, х2, х3) ∆t. (3.6)
Здесь ∆t — временной интервал списывания.
В то же время:
S = ηE, (3.7)
где Е – максимальная допустимая ошибка в любом измерении;
η – коэффициент пропорциональности.
Тогда из (3.6) и (3.7) можно найти интервал списывания:
∆t = ηE / mах (х1, х2, х3) (3.8)
В каждом цикле списывания управляющая ЭВМ будет производить опрос сенсоров линейных перемещений, получение извне (от отдельной системы) точных значений координат местонахождения робота и коррекцию направления движения. При наличии возмущающих воздействий интервал списывания не может оставаться постоянным, поскольку ошибки движения приобретают случайный характер.
Согласно принятому алгоритму управления роботом его движение, начиная от некоторого начального состояния, в дальнейшем определяется целиком этим и целевым состояниями и не зависит от предыстории движения.
В качестве начального состояния будем каждый раз брать состояние робота в момент связи с управляющей ЭВМ, а целевое сохранять одним и тем же. Кроме того, скорости перемещения (х1, х2, х3) робота из начального состояния без учета возмущений будем выбирать таким образом, чтобы результирующий вектор скорости совпадал с направлением на цель движения. Введем обозначения: х10, х20, х30 – значения координат робота, полученные извне от отдельной навигационной системы; х1, х2, х3— те же координаты, полученные по показаниям сенсоров. Тогда скорости «сноса» по координатам на (n + 1)-м цикле приближенно определяются как:
∆х1 = х10 - х11 / ∆ tn ;
∆х2 = х20 – х21 / ∆ tn ; (3.9)
∆х3 = х30 – х31 / ∆ tn .
Здесь ∆ tn — интервал опроса на n-м цикле.
Рассматривая совместно (3.8) и (3.9), можно записать выражение для ∆ tn+1:
∆ tn+1 = ηE / mах (х1+∆х1, х2+∆х2, х3+∆х3) (3.10)
которое позволяет корректировать значение интервала списывания на предстоящий цикл. Одновременно с помощью (3.9) можно осуществлять коррекцию движения с учетом зафиксированного на данном цикле «сноса» робота с заданной траектории.
3.2.3 Планирование маршрута
Далее робот может произвести акт движения по карте. Пусть цель задана на карте и не попадает в зону восприятия сенсорной подсистемы (СП) робота. Тогда определяется направления движения на каждом шаге. Принятое решение реализуется исполнительными устройствами робота. После этого вновь считывается карта среды, но с учетом произошедшего перемещения робота. Вновь определяется промежуточная цель, определяется и отрабатывается следующее направление движения и т.д. до выхода в зону цели.
Введем следующие обозначения:
– функционал, определяющий возможные расстояния между целью 
и текущим положением робота 
на карте среды; 
– вектор, определяющий промежуточное положение цели; 
– единичный вектор направления к цели на карте среды; 
- максимальный радиус радиального плана; 
– функционал, определяющий допустимые расстояния между истинным положением робота 
в среде и промежуточной «воображаемой» целью на плане; 
– оператор коррекции текущего положения робота на карте; 
– единичный вектор направления на промежуточную цель; 
– модуль скорости робота; 
– модуль вектора градиента; 
– длина траектории.Тогда, алгоритм движения к цели по карте может быть сформулирован следующим образом:
1.
2. если
, то 11, иначе – следующий шаг;3.
4.
5.
6.
7.
8. определить
, положить 
9.
( шаг в направлении );10.
, перейти к 1;11. если
то останов, иначе – следующий шаг;12.
13. определить
14. определить
;15.
(шаг в направлении 
), перейти к 1.Данный алгоритм построен так, что в начале осуществляется проверка наличия цели в поле зрения робота, и если цель отсутствует, то в пунктах 3-9 выполняется процедура движения по карте, а в пункте 10 – коррекция положения робота на карте среды. В случае попадания цели в зону восприятия СП карта не используется и происходит движение к цели только на основе сенсорной информации (пункты 11-15 алгоритма движения) [10].
3.2.4 Алгоритм управления локальными перемещениями
Как было сказано выше, стратегия движения робота состоит в прохождении опорных точек в последовательности, которая предписывается выбранным маршрутом. Переменные состояния робота в опорных точках принимают определенные значения.
Координаты точек соответствуют направлению на подцель на заданном отрезке траектории. При отсутствии случайных возмущений и ошибок в оценке рельефа на этапе обзора такое движение будет происходить по прямой. Однако это предположение является идеальным случаем. При управлении роботом нужно ещё уметь решать задачу коррекции траектории движения и обхода опасных участков вблизи препятствий.
3.2.5 Алгоритмы управления при обходе роботом опасных участков трассы
В соответствии с изложенными ранее принципами управления перемещением робота задачу обхода «пропущенных» при планировании маршрута препятствий и опасных участков можно трактовать как генерацию новых целей и связанных с ними состояний робота в моменты обнаружения новых препятствий.
При обходе одиночных препятствий можно использовать следующий алгоритм управления для случая, когда отсутствует информации об участках поверхности, удаленных от робота более чем на n ячеек матрицы А (рисунок 3.2).
Для каждого из m направлений движения робота, возможных в данный момент, составляется полный вес Рi данного направления, определяемый формулой
Рi = ∑gk Pki , (3.11)
где Pki – оценка i-го направления блоком оценки (Б1 – Б5),
gk – вес оценки.
В качестве направления очередного шага выбирается направление m, получившее наибольший вес Рm.
Рисунок 3.4 – Структурная схема алгоритма обхода опасных участков трассы
Блок Б1 выделяет направление на цель из текущей составляющей на шаге i:
P1i=(x1ц– x1тек)cos π/4(i – 1) + (x2ц– x2тек)sin π/4(i – 1)/
√ (x1ц– x1тек)² + (x2ц– x2тек)² (3.12)
Блок Б2 выделяет среди соседних клеток текущего состояния занятые, а именно Р2i=1, если клетка (x1тек +∆x1i, x2тек +∆x2i) занята и Р2i=0 – в противном случае.