Урок 7

Введение: Мы продолжаем решать задачу, которую начали на прошлом уроке, а именно движение робота вдоль стены с выдерживанием заданного бокового интервала. Мы уже познакомились с пропорциональной компонентой этого регулятора в прошлом уроке, как вы помните она пропорционально изменяет управляющее воздействие на робота в зависимости от величины отклонения от цели. На этом уроке мы дополним наш регулятор Дифференциальной компонентой, определяющей управляющее воздействие в зависимости от скорости изменения отклонения от цели. Т.е. чем быстрее нарастает отклонение, тем большее управляющее воздействие будет передано на робота для компенсации этого отклонения.

Подробнее про PID-регулятор можно прочитать здесь.

Подготовка: Попросите учащихся непосредственно перед выполнением написанной программы, при помощи управления через веб-интерфейс, поставить робота "правым" боком к любой из стенок полигона. По внешней камере полигона убедитесь, чтобы перед роботом было достаточно свободного пространства для движения по прямой. Если программа написана некорректно, и робот делает что-то несогласованное, попросите учащихся прервать выполнение программы, остановить робота при при помощи управления через веб-интерфейс и вернуть его в первоначальную позицию.

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

Общий алгоритм решения:

1. Робот движется вперед с заданной скоростью.

2. В цикле проверяется отличается ли текущий интервал до стены от целевого (к примеру 0.3 м.), и если отличается, то регулятор дает команду на поворот в сторону уменьшения отклонения, с угловой скоростью определяемой P, I и D компонентами регулятора.

Решение:

Рассмотрим вот такие два случая:

В первом случае робот едет в сторону стены, но т.к. расстояние до стены равно целевой уставке, угловая скорость равна нулю, т.е. робот едет прямо.

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

Нулевая угловая скорость для компенсации такого движения определяется P-компонентой, и в обоих этих случаях это плохо, так как мы бы хотели, чтобы робот поворачивался не только по отклонению, но и заранее отворачивал от стены, если он к ней приближается, или поворачивался к стене, если он от нее удаляется. Т.е. мы бы хотели уже сейчас учитывать перпендикулярную стене составляющую скорости робота, чтобы заранее реагировать на изменение положения робота в будущем.

Для этого мы напишем D-компоненту нашего регулятора. Проведем следующие рассуждения:

1. Мы хотим чтобы робот реагировал на скорость с которой он движется к/от стены.
2. Мы хотим чтобы робот отворачивал ОТ стены, если скорость направлена в стену, и поворачивал К стене, если скорость направлена от стены.
3. Мы хотим чтобы робот поворачивал настолько быстро, насколько быстро он движется относительно стены. Т.е. если бы робот двигался перпендикулярно стене - разворот был бы максимально быстрым. А если бы робот двигался параллельно стене, то составляющая угловой скорости, обуславливаемая D-компонентом была бы равна нулю.

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

Где L - это перпендикуляр до стены, а T лидара 0.2 секунды.

Далее, получив линейную скорость относительно стены, мы должны превратить ее в команду с угловой скоростью, умножив эту скорость на какой-то коэффициент KD, который и будет в общем случае коэффициентом при D-компоненте регулятора. Давайте теперь проверим знак при угловой скорости. Если Lтекущее > Lпредыдущего, значит робот едет от стены, значит мы должны поворачивать его направо и значит угловая скорость по правилу правой руки, должна быть отрицательна. Если же Lтекущее < Lпредыдущего, то мы должны поворачивать робота от стены, т.е. налево, и угловая скорость должна быть положительна. Кроме того, необходимо заметить, что Т лидара - константно, и как следствие может быть внесено в коэффициент KD.

В результате наша формула для угловой скорости превратится в следующую:

Давайте впишем эту формулу в нашу функцию регулятор. Для этого нам понадобится еще одна переменная self.distance_prev, в которой мы и будем хранить предыдущее значение L

def regulator(self):
        w_z = ((self.TARGET - self.distance) * self.KP 
                - (self.distance - self.distance_prev) * self.KD
        self.pub_mover(w_z)
        self.distance_prev = self.distance

в эту переменную self.distance_prev мы будем записывать текущее значение, переменной self.distance, и таким образом на следующем шаге цикла оно станет предыдущим. Для корректной работы нашего регулятора, нам необходимо объявить обе эти переменные до начала работы функции регулятора.

class RoboMover():

    distance = 0
    distance_prev = 0

Теперь определим величину коэффициента KD - в самом худшем случае, когда робот едет строго перпендикулярно стене, величина линейной скорости будет равна LIN_SPEED*0.2, т.е. примерно 1,6 см в секунду или 0.016 м / сек. При таком движении робота - строго в стену, мы бы хотели, чтобы наш регулятор передавал максимально возможное значение угловой скорости на робота, для уклонения от стены. Максимальная угловая скорость, с которой наш робот может вращаться порядка 1,5 - 2-х радиан в секунду, чтобы получить это характерное значение угловой скорости, нам надо умножить величину максимальной скорости перпендикулярной стене - 0.016, примерно на 100. После умножения мы получим значение 1.6, что нас вполне устроит как характерное значение угловой скорости уклонения. Таким образом коэффициент KD должен быть по порядку равен 100. Давайте впишем это значение коэффициента в нашу программу.

class RoboMover():

    TARGET = 0.3
    LIN_SPEED = 0.08
    KP = 1.5
    KD = 100

    vel = Twist()

    distance = 0
    distance_prev = 0

В итоге наша программа примет вид:

import rospy

from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class RoboMover():

    TARGET = 0.3
    LIN_SPEED = 0.08
    KP = 1.5
    KD = 100

    vel = Twist()

    distance = 0
    distance_prev = 0

    def __init__(self):
        rospy.Subscriber("/scan", LaserScan, self.laser_cb)
        self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

    def laser_cb(self, scan):
        self.distance = min(scan.ranges)

    def pub_mover(self, vel_z):
        self.vel.linear.x = LIN_SPEED
        self.vel.angular.z = vel_z
        self.pub.publish(self.vel)

    def regulator(self):
        w_z = ((self.TARGET - self.distance)*self.KP -
            (self.distance - self.distance_prev) * self.KD)
        self.distance_prev = self.distance
        self.pub_mover(w_z)


rospy.init_node("Laser")
m = RoboMover()

while not rospy.is_shutdown():
    rospy.sleep(0.2)
    m.regulator()

Проверка решения:

Попросите учащихся перенести модифицированную программу на робота, запустить ее и убедиться, что теперь поведение робота кардинально отличается. Осцилляции - ушли, робот едет четко вдоль стены. Попросите учащихся попробовать применить различные значения коэффициентов KD и KP и понаблюдать за различным поведением робота. Проконтролируйте работу учащихся в режиме трансляции их экрана через Discord.