Объезд препятствий

В рамках этой статьи мы постараемся составить свой алгоритм объезда препятствий для робота e-puck в среде Webots. Для примера будет построен лабиринт, где робот должен будет успешно избегать столкновений с препятствиями и объезжать их. Сначала нам необходимо будет построить сам лабиринт на арене, этот момент мы пропустим. Затем нам нужно добавить на арену робота e-puck и создать для него новый контроллер, программируемый python. Теперь, когда все готово, переходим к написанию самого алгоритма.

Итак, обращаясь к конфигурации робота e-puck на официальном сайте Webots, мы знаем, что у нашего робота есть 2 двигателя, называемых мотором левого и правого колес соответственно. У нас также имеется 8 датчиков приближения, имеющих имена: PS0; ... ; PS8. Попробуем включить оба двигателя и все датчики.

Включаем все двигатели:

leftMotor = robot.getDevice('left wheel motor')
rightMotor = robot.getDevice('right wheel motor')

leftMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.0)

rightMotor.setPosition(float('inf'))
rightMotor.setVelocity(0.0)

Включаем датчики:

prox_sensors = []
for ind in range(8):
    sensor_name = 'ps' + str(ind)
    prox_sensors.append(robot.getDistanceSensor(sensor_name))
    prox_sensors[ind].enable(timestep)

Считаем показания датчиков:

for ind in range(8):
    print("ind: {}, val: {}".format(ind, prox_sensors[ind].getValue()))

После запуска симуляции, мы наблюдаем следующие показания в консоли:

Из технической документации на сайте Webots, мы знаем, что ps7 и ps0- это передний левый и правый датчики соответственно, ps5 и ps2-левый и правый, расположенные по бокам, а ps4 и ps3-задний левый и задний правый. Из консоли мы получаем значения, считываемые этими датчиками. Эти показания понадобятся нам для того, чтобы создать функцию поворота, по мере приближения к препятствию. e-puck может обнаружить стену перед собой, если мы подключим один из передних датчиков, проделаем это с ps7

left_wall = prox_sensors[5].getValue() > 80
front_wall = prox_sensors[7].getValue() > 80

И так, мы разрабатываем следующий алгоритм движения:

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

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

Здесь возможны ошибки!

В процессе движения робот может слишком близко подойти к стенке, это может затруднить или вовсе остановить процесс его движения. Следовательно, нам нужно сделать так, чтобы робот мог отъехать от левой стенки, если она находится к нему слишком близко. Для этого задействует датчик ps6, который находится между левым боковым-ps5 и левым передним-ps7.

left_wall = prox_sensors[5].getValue() > 80
left_corner = prox_sensors[6].getValue() > 80
front_wall = prox_sensors[7].getValue() > 80

Так же добавляем соответствующие условие:

if left_corner:
    left_speed = max_speed
    right_speed = max_speed/8

Окончательный вариант этого алгоритма выглядит следующим образом:

right_motor.setPosition(float('inf'))
right_motor.setVelocity(0.0)

prox_sensors = []
for ind in range(8):
    sensor_name = 'ps' + str(ind)
    prox_sensors.append(robot.getDistanceSensor(sensor_name))
    prox_sensors[ind].enable(timestep)
    
    
while robot.step(timestep) != -1:

    for ind in range(8):
        print("ind: {}, val: {}".format(ind, prox_sensors[ind].getValue()))
   
    left_wall = prox_sensors[5].getValue() > 80
    left_corner = prox_sensors[6].getValue() > 80
    front_wall = prox_sensors[7].getValue() > 80   
    
    left_speed = max_speed
    right_speed = max_speed
    
    if front wall:
        left_speed = max_speed
        right_speed = -max_speed
    else:
        if left wall:
            left_speed = max_speed
            right_speed = max_speed
        else:
            left_speed = max_speed/8
            right_speed = max_speed
        if left_corner:
            left_speed = max_speed
            right_speed = max_speed/8