Урок 5

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

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

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

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

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

2. В цикле проверяется отличается ли текущая ориентация от целевой (в простом случае от начальной ориентации робота), и если отличается, то регулятор дает команду на поворот в сторону противоположную отклонению, с угловой скоростью по значению пропорциональной отклонению от целевого значения.

Решение: Для начала импортируем все библиотеки и структуры данных, которые нам понадобятся, это rospy и math. И если rospy стандартна для любой программы под ROS, то math нам понадобится специально, чтобы быстро реализовывать математические преобразования при переводе углов поворота.

import rospy
import math

from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

Теперь инициализируем Ноду и Издателя

rospy.init_node('line_mover')
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

Издатель нам нужен для непосредственного управления роботом, на основании тех данных, которые мы будем получать с IMU датчика.

И перед тем как перейти непосредственно к реализации логики, нам необходимо инициализировать несколько констант:

theta = 0
TARGET_ANGLE = 0
KP = 0.01

print("Init done")

Это начальный угол theta, для того чтобы функция регулятора запустилась корректно. После того как стартует Подписчик данные в этой переменной будут реальные.

TARGET_ANGLE = 0, это наша целевая уставка. Тот угол к которому будет стремиться наш робот. И KP - коэффициент пропорциональности нашего регулятора. О нем мы подробнее поговорим, когда будем писать функцию регулятор.

Как вы знаете из курса по Python в нем нет функциональности реализующей константные переменные по умолчанию. Однако есть соглашение о наименовании переменных, в котором описывается, что переменные именованные ЗАГЛАВНЫМИ БУКВАМИ являются константами и не должны изменяться в процессе работы программы. Этого принципа мы будем придерживаться здесь и далее. Обратите внимание, что мы вынесли объявление этих константных переменных в отдельный блок нашей программы, так что в дальнейшем, для того, чтобы поменять какие-либо уставки нам не потребуется искать их по всему коду, достаточно будет просто поменять их в этом блоке.

Далее нам надо будет данные об угле поворота с IMU датчика, передаваемые в кватернионах превратить в угол поворота вокруг собственной вертикальной оси робота.

Напомним, что для перевода кватерниона в угол Z, нам необходимо использовать следующую функцию

def quaternion_to_theta(odom):

    return(2*math.degrees(math.asin(odom.pose.pose.orientation.z)*
           math.copysign(1,odom.pose.pose.orientation.w)))

Эта функция берет полное сообщение структуры данных Одометрия, передаваемое из топика /odom и превращает его в угол поворота в градусах.

Теперь опишем функцию обратного вызова для Подписчика на данные одометрии.

def odomcb(odom):
    global theta
    theta = quaternion_to_theta(odom)

Данная функция берет переданное ей в качестве аргумента odom сообщение от Подписчика и передает в функцию переводящую кватернионы в угол, которую мы написали чуть ранее. Дальше результат работы функции quaternion_to_theta кладется в глобальную переменную theta. Использование глобальной переменной здесь оправдано тем, что мы должны асинхронно передавать значение текущего положения робота во вне функции. Таким образом теперь для того чтобы знать текущее направление робота, нам будет достаточно прочитать глобальную переменную theta из любой точки нашей программы.

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

rospy.Subscriber("/odom", Odometry, odomcb)

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

def mover(vel_z):
    pub_vel = Twist()
    pub_vel.linear.x = 0.05
    pub_vel.angular.z = vel_z
    pub.publish(pub_vel)

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

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

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

Максимальная угловая скорость, которую может развивать робот около 2-х радиан в секунду. Однако, если мы будем вычитать максимальный угол расхождения 180 градусов из нуля, то при прямом переводе в радианы мы получим угловую скорость в 180 радиан в секунду, что очень много. Следовательно нам надо уменьшить нашу угловую скорость по величине, чтобы она не превышала скажем 1.8-х радианов в секунду.

Очевидно, что самым удобным способом учета всех этих условий является разность углов, это одновременно позволит учитывать правильный знак угловой скорости, занулять ее в нуле и максимизировать при увеличении угла расхождения. Однако для соблюдения величины максимальной скорости мы должны домножить эту разность углов на коэффициент пропорциональности 0,01, который мы объявили в самом начале нашей программы. Это уменьшит рассчитываемую угловую скорость до приемлемых значений.

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

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

def regulator():
    vel_z = (target_angle - theta)*Kp
    mover(vel_z)

Ну и в итоге, для полной реализации задуманного алгоритма, нам надо в цикле вызывать функцию регулятор с какой-то резонной частотой. Скажем 10 раз в секунду. Для этого применим стандартный РОСовский цикл:

while not rospy.is_shutdown():
    regulator()
    rospy.sleep(0.1)

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

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

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

Код решения:

import rospy
import math

from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

rospy.init_node('line_mover')
pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)

theta = 0 #начальное значение
TARGET_ANGLE = 0 #целевое значение
KP = 0.01 #коэфициент пропорциональности P-компонента регулятора

print("Init done")


def quaternion_to_theta(odom):

    return(2*math.degrees(math.asin(odom.pose.pose.orientation.z)*
    math.copysign(1,odom.pose.pose.orientation.w)))

def odomcb(odom):
    global theta
    theta = quaternion_to_theta(odom)

rospy.Subscriber("/odom", Odometry, odomcb)

def mover(vel_z):
    pub_vel = Twist()
    pub_vel.linear.x = 0.05
    pub_vel.angular.z = vel_z
    pub.publish(pub_vel)

def regulator():
    vel_z = (TARGET_ANGLE - theta)*KP
    mover(vel_z)

while not rospy.is_shutdown():
    regulator()
    rospy.sleep(0.1)