Урок 8

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

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

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

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

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

Рассмотрим I-компоненту нашего регулятора. Сразу скажем, что хорошей демонстрации I-компоненты на примере движущегося робота мы не увидим, и в большинстве случаев PD -регулирования достаточно. Однако в сам принцип I-регулятора, мы рассмотрим. Итак, давайте представим что на нашего робота действует какая-то смещающая его сила, например, дует ветер или пол полигона имеет уклон в одну сторону. При этом смещение мешает роботу достичь требуемой нами цели. При этом если полагаться только на P-регулятор, в какой-то момент отклонение от цели будет не настолько большим, чтобы P-компонента смогла развить достаточное управляющее воздействие для достижения цели, а D-компонента нам в этом тоже не поможет - ведь в нашем примере она следит за тем, чтобы робот шел параллельно стене, и в какой-то момент она выровняет робота параллельно. Но из-за наклона пола, робот будет ехать рядом с целевым значением, и не будет достигать его.

На рисунке видно, что dL - отклонение мало, и следовательно dL*KP тоже мало и P-компонента не может сопротивляться наклону пола. А т.к. робот едет параллельно стене то dV*KD = 0 и D-компонента тоже не помогает.

Для решения этой проблемы используют I-компоненту, она просто суммирует отклонение за несколько периодов обращения лидара, и в результате, если отклонение не убывает эта сумма начинает увеличиваться во времени. И с каждым новым оборотом лидара в случае если робот так и не может достигнуть цели, суммарное отклонение начинает давать все больший и больший вклад в определение управляющего воздействия. В результате, в какой-то момент сумма отклонений становится настолько существенной, что робот выравнивается и мгновенное отклонение уменьшается, как следствие, постепенно начинает уменьшаться и сумма. Это и есть достаточно упрощенный пример I-регулятора. Давайте реализуем его на нашем роботе.

Для этого используем массив, в который будем записывать отклонение с каждым оборотом лидара, а затем суммируем этот массив и умножим на коэффициент KI. Размер массива выберем равным, к примеру 10. Это будет соответствовать 2-м секундам реального времени. Размер массива выраженный в секундах называется постоянной времени интегрирования, он показывает как долго регулятор будет накапливать "память" об отклонениях. В нашем случае регулятор будет "помнить" прошедшие 2 секунды. Добавим массив в объявление переменных и опишем правила его заполнения в функции регуляторе.

class RoboMover():

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

    DTI = 10

    vel = Twist()

    distance = 0
    distance_prev = 0
    array_of_dp = []

Если длина массива меньше DTI = 10, то мы добавляем в массив значение, отклонения, и после наполнения массива, когда его длина станет равна DTI, мы будем убирать из массива нулевой - самый старый элемент и добавлять в его конец новый элемент - текущее отклонение.

if len(self.array_of_dp) < self.DTI:
            self.array_of_dp.append(self.TARGET - self.distance)
        else:
            self.array_of_dp.pop(0)
            self.array_of_dp.append(self.TARGET - self.distance)

Теперь добавим в наш регулятор I-компоненту. Она будет представлять из себя сумму массива отклонений умноженную на коэффициент KI. По величине KI должен быть как минимум в 10 раз меньше KP, т.к. сумма массива из 10 отклонений будет примерно в 10 раз больше чем единичное отклонение, которое мы парируем P-компонентой. А вот по направлению угловой скорости I-компонента будет совпадать с P-компонентой. Таким образом наша функция регулятор примет вид:

   def regulator(self):
        w_z = ((self.TARGET - self.distance)*self.KP -
            (self.distance - self.distance_prev) * self.KD +
            sum(self.array_of_dp) * self.KI)
        self.distance_prev = self.distance
        self.pub_mover(w_z)
        if len(self.array_of_dp) < self.DTI:
            self.array_of_dp.append(self.TARGET - self.distance)
        else:
            self.array_of_dp.pop(0)
            self.array_of_dp.append(self.TARGET - self.distance)

А вся программа будет выглядеть вот так:

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
    KI = 0.15
    KD = 100

    DTI = 10

    vel = Twist()

    distance = 0
    distance_prev = 0
    array_of_dp = []

    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 +
            sum(self.array_of_dp) * self.KI)
        self.pub_mover(w_z)
        if len(self.array_of_dp) < self.DTI:
            self.array_of_dp.append(self.TARGET - self.distance)
        else:
            self.array_of_dp.pop(0)
            self.array_of_dp.append(self.TARGET - self.distance)


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

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

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

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

В этом задании вы на практике познакомились с основными принципами создания и настройки PID-регулятора. В дальнейшем мы будем частично использовать применённые здесь принципы.