Коррекция траектории движения робота

Задача коррекции связана с организацией следящего режима посредством списывания ошибок движения и коррекции траектории. Ошибки, поступающие от управляющей ЭВМ к роботу, вызваны, с одной стороны, накоплением ошибок сенсоров линейных перемещений, а с другой – возмущающими воздействиями со стороны окружающей среды. Ошибки сенсоров перемещений обычно зависят от пройденного расстояния. Если пренебречь возмущающими воздействиями, то тогда длина отрезка пути, по прохождении которого следует производить списывание таких накапливающихся ошибок, может быть выражена следующим образом:

 

S = mах (х₁, х₂, х₃) ∆ t. (3.6)

 

Здесь ∆ t — временной интервал списывания.

В то же время:

 

S = ηE, (3.7)

 

где Е – максимальная допустимая ошибка в любом измерении;

η – коэффициент пропорциональности.

Тогда из (3.6) и (3.7) можно найти интервал списывания:

 

∆ t = ηE / mах (х₁, х₂, х₃) (3.8)

В каждом цикле списывания управляющая ЭВМ будет производить опрос сенсоров линейных перемещений, получение извне (от отдельной системы) точных значений координат местонахождения робота и коррекцию направления движения. При наличии возмущающих воздействий интервал списывания не может оставаться постоянным, поскольку ошибки движения приобретают случайный характер.

Согласно принятому алгоритму управления роботом его движение, начиная от некоторого начального состояния, в дальнейшем определяется целиком этим и целевым состояниями и не зависит от предыстории движения.

В качестве начального состояния будем каждый раз брать состояние робота в момент связи с управляющей ЭВМ, а целевое сохранять одним и тем же. Кроме того, скорости перемещения (х₁, х₂, х₃) робота из начального состояния без учета возмущений будем выбирать таким образом, чтобы результирующий вектор скорости совпадал с направлением на цель движения. Введем обозначения: х₁₀, х₂₀, х₃₀ – значения координат робота, полученные извне от отдельной навигационной системы; х₁, х₂, х₃— те же координаты, полученные по показаниям сенсоров. Тогда скорости «сноса» по координатам на (n + 1)-м цикле приближенно определяются как:

 

∆х₁ = х₁₀ - х₁₁ / ∆ t _n ;

∆х₂ = х₂₀ – х₂₁ / ∆ t _n ; (3.9)

∆х₃ = х₃₀ – х₃₁ / ∆ t _n .

 

Здесь ∆ t _n — интервал опроса на n -м цикле.

Рассматривая совместно (3.8) и (3.9), можно записать выражение для ∆ t _n+1 :

  ∆ t _n+1 = ηE / mах (х₁+∆х₁, х₂+∆х₂, х₃+∆х₃) (3.10)

которое позволяет корректировать значение интервала списывания на предстоящий цикл. Одновременно с помощью (3.9) можно осуществлять коррекцию движения с учетом зафиксированного на данном цикле «сноса» робота с заданной траектории.

 

Планирование маршрута

Далее робот может произвести акт движения по карте. Пусть цель задана на карте и не попадает в зону восприятия сенсорной подсистемы (СП) робота. Тогда определяется направления движения на каждом шаге. Принятое решение реализуется исполнительными устройствами робота. После этого вновь считывается карта среды, но с учетом произошедшего перемещения робота. Вновь определяется промежуточная цель, определяется и отрабатывается следующее направление движения и т.д. до выхода в зону цели.

Введем следующие обозначения:

 – функционал, определяющий возможные расстояния между целью  и текущим положением робота  на карте среды;

 – вектор, определяющий промежуточное положение цели;

 – единичный вектор направления к цели на карте среды;

 - максимальный радиус радиального плана;

 – функционал, определяющий допустимые расстояния между истинным положением робота  в среде и промежуточной «воображаемой» целью на плане;

 – оператор коррекции текущего положения робота на карте;

 – единичный вектор направления на промежуточную цель;

 – модуль скорости робота;

 – модуль вектора градиента;

 – длина траектории.

Тогда, алгоритм движения к цели по карте может быть сформулирован следующим образом:

 

1.

2. если , то 11, иначе – следующий шаг;

3.

4.

5.

6.

7.

8. определить , положить  

9.  (шаг в направлении );

10. , перейти к 1;

11. если  то останов, иначе – следующий шаг;

12.

13. определить

14. определить ;

15.  (шаг в направлении ), перейти к 1.

 

Данный алгоритм построен так, что в начале осуществляется проверка наличия цели в поле зрения робота, и если цель отсутствует, то в пунктах 3-9 выполняется процедура движения по карте, а в пункте 10 – коррекция положения робота на карте среды. В случае попадания цели в зону восприятия СП карта не используется и происходит движение к цели только на основе сенсорной информации (пункты 11-15 алгоритма движения) [10].