Урок 6

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

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

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

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

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

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

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

Решение: Для начала импортируем все библиотеки и структуры данных, которые нам понадобятся. Определять интервал до стены мы будем по лидару, и для этого нам нужен LaserScan, а управлять роботом будем через уже известный нам Twist.

import rospy

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

Далее создадим класс, в котором опишем весь алгоритм управления. Введем константную переменную TARGET, в которую запишем нашу целевую уставку - желаемый интервал до стены. А так же константу LIN_SPEED, в которой зафиксируем линейную скорость по оси X.

class RoboMover():

    TARGET = 0.3
    LIN_SPEED = 0.08

Далее в функции __init__ мы объявим, Подписчика на данные лидара, и Издателя для управления движением робота.

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

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

Таким образом, чтобы получить расстояние до стены, нам надо в любом случае просто взять минимум массива scan.ranges это и будет искомый перпендикуляр - интервал до стены. Давайте опишем это в нашей функции обратного вызова

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

Обратите внимание, что после того, как Подписчик начнет получать данные лидара - в переменной self.distance всегда будет актуальное значение интервала от робота до стены. Именно этой переменной мы и будем пользоваться в нашем регуляторе для определения управляющего воздействия.

Возьмём функцию-обертку mover() из предыдущего занятия и перепишем ее для класса:

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

Добавим необходимые self для всех переменных класса и зафиксируем линейную скорость по x через ранее объявленную константу LIN_SPEED. Как и в прошлом примере мы будем пользоваться функцией-оберткой, чтобы просто публиковать управляющее воздействие с регулятора на робота, не заботясь о конвертации значений угловой скорости получаемой на регуляторе в структуру данных Twist, понимаемую роботом. За нас это будет делать функция mover().

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

   def regulator(self):
        w_z = ((self.TARGET - self.distance)*self.Kp 
        self.pub_mover(w_z)

w_z это переменная, в которую мы будем записывать значение рассчитанной регулятором угловой скорости. Именно ее мы и будем передавать как аргумент в функцию mover(). При рассуждениях о величине и знаке угловой скорости мы будем использовать те же предпосылки, что и в прошлом примере. Т.е. мы будем отворачивать ОТ стены, если расстояние до нее будет меньше целевого, и поворачивать К стене, если робот будет дальше, чем целевая дистанция. По величине, мы так же будем руководствоваться пропорциональным принципом, т.е. чем дальше от цели робот будет находиться в текущий момент, тем большее значение угловой скорости он будет развивать, чтобы приблизиться к стене. Как и в случае с ездой по прямой, введем коэффициент KP, в который будет регулировать степень пропорциональности нашей угловой скорости.

class RoboMover():

    TARGET = 0.3
    LIN_SPEED = 0.08
    KP = 1.5 

    vel = Twist()

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

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

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

В результате мы должны получить следующую программу:

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

    vel = Twist()

    distance = 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.pub_mover(w_z)


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

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

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

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

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