Урок 4

Теория: Для отображения пространственного положения предметов, мы привыкли пользоваться совокупностью пространственных Декартовых координат, X, Y и Z

и углов наклона к осям - Тангаж, Крен и Рыскание, так называемые углы Эйлера.

Эти 6 значений привычно определяют для нас положение и ориентацию в пространстве. К примеру, если я скажу, что в 5 километрах от вас по оси Х и в 2км по оси Y , на высоте 10 км по оси Z, летит самолет, с углом вокруг оси Z равным 0 градусов, без крена и тангажа, вы поймете, как именно направлен самолет и где примерно он находится. Единственное, что вам потребуется уточнить, это как именно направлена тройка векторов осей координат и какое правило определения угловой скорости.

Как мы уже говорили, в ROS, координаты образуют правую тройку векторов - X направлен прямо, Y влево и Z вверх, а направление угловой скорости определяется по правилу правой руки.

Таким образом самолет о котором мы говорили должен находится от вас прямо в 10 км, налево в 2х км, на высоте 10 км, и лететь строго по направлению вперед.

Не смотря на то, что наш робот не умеет летать, и перемещается только по плоскости, т.е. казалось бы мы могли использовать только 2 координаты X,Y и один угол для ориентации, в ROS используется полные координаты даже для таких случаев. Это связано с тем, что эти полные координаты удобно использовать для описания движения в случае если робот к примеру поедет по наклонному пандусу или будет перемещаться в лифте, кроме того, для роботов которые умеют наклоняться не придется менять описание их движения. Еще один типов роботов изначально использует такие координаты - это коптеры. Для них вполне естественно оперировать положением в пространстве в виде координат центра масс + углы наклона к осям. И именно для того, чтобы унифицировать подходы к описанию поведения всех типов роботов, используется единая модель данных.

Данная модель используется и в задании скоростей движения и в считывании положения. Как вы помните из вводного курса по ROS, скорости, которыми мы задаем движение робота делятся на линейные - вдоль осей X,Y,Z и угловые - вокруг этих же осей. Так же и с положением робота, в ROS есть структура данных Odometry, которая состоит их части пространственных координат X,Y,Z и ориентации.

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

1. Введение в кватернионы
2. Ликбез по кватернионам
3. Фильтр Маджвика и кватернионы
4. Кватернионы для чайников

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

Основные преимущества кватернионов перед привычными нам углами Эйлера следующие:

1. Вычислительная легкость, чтобы найти новую ориентацию робота по двум известным заданным кватернионами поворотам, надо просто перемножить два кватерниона, отвечающие за эти повороты. $$с = (w_1,x_1,y_1,z_1)*(w_2,x_2,y_2,z_2)$$ Для этого надо провести 16 умножений и 12 сложений и вычитаний по обычным правилам перемножения многочленов с небольшой добавкой правил перемножения мнимых частей кватернионов.

Это существенно проще чем, к примеру, перемножение 2-х матриц поворота, отвечающих за те же два поворота, нужно сделать уже 27 умножений и 18 сложений. А для преобразования поворотов в углах Эйлера надо сделать все те же 27 умножений и 18 сложений, только уже над тригонометрическими функциями. Это естественно существенно сложнее.

2. Кроме того при описании поворотов в Эйлеровых углах, существуют особые точки, в которых система входит в состояние сингулярности, так называемые "шарнирные замки", и пусть для наших роботов это не очень актуальное состояние, тем не менее хочется использовать какой-то универсальный механизм. Описание поворотов в кватернионах лишено этого недостатка.

3. И последнее по очереди, но возможно самое важное по значению - работа инерциальных датчиков. Это основные датчики по которым роботы определяют свою ориентацию. В связи с особенностью их работы и спецификой математики кватернионов, перевод перевод угловых ускорений, получаемых с IMU, в кватернионы, легко осуществляется обычными умножениями и сложениями. А точнее, мы можем составить кватернион ориентации из угловых ускорений по вот такой простой формуле (подробнее о выводе и применении данной формулы тут и тут):

$$q_1 = q_0 + t/2wq_0$$ где $$w = [w_x, w_y, w_z, 0]$$

Где $$q_1$$ это кватернион ориентации объекта в следующий момент времени, $$q_0$$ это начальный кватернион ориентации объекта, а $$w$$ - компоненты угловой скорости по координатным осям. Следовательно, по этой формуле, мы можем в цикле суммировать угловые скорости, получаемые с инерциальных датчиков, и сразу получать кватернион ориентации, а раз для суммирования не используются сложные тригонометрические преобразования, то такие операции легко выполнять даже на не очень мощных микроконтроллерах IMU датчиков, использование этого принципа описано в статье Себастьяна Маджвика, именем которого и назван один из самых популярных рекурентных фильтров для ориентации кватернионов. Еще один немаловажный момент, что использование кватернионов в комплексировании данных IMU существенно снижает математическую ошибку суммирования. Достаточно подробно это разобрано здесь.

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

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

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

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

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

1. Создаем Подписчика на данные одометрии.

2. В функции обратного вызова Подписчика преобразуем данные одометрии из кватернионов в угол поворота вокруг оси z.

3. Публикуем значения угла поворота.

Решение: Для начала импортируем библиотеку rospy, и структуру данных Odometry для данных одометрии. Затем инициализируем ноду и объявим экземпляр подписчика на топик /odom, в котором публикуются данные одометрии.

import rospy
import math
from nav_msgs.msg import Odometry

rospy.init_node('angle_measurer')

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

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

def callback(odom):

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

Расположим нашу функцию обратного вызова таким образом, чтобы она находилась перед объявлением Подписчика. И в конце добавим rospy.spin(), чтобы программа не закрывалась сразу. Всё! Наша программа готова. Вот так она выглядит.

import rospy
import math
from nav_msgs.msg import Odometry

rospy.init_node('angle_measurer')

def callback(odom):
    print(2*math.degrees(math.asin(odom.pose.pose.orientation.z)*
            math.copysign(1,odom.pose.pose.orientation.w)))

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

rospy.spin()

Обратите внимание, что можно было бы не создавать переменную Ph, а сразу написать, что-нибудь типа print(math.degrees(math.atan2(t1, t2))), но на первых порах лучше каждую отдельную логическую инструкцию выделять в отдельную строку программы. Это делается исключительно для улучшения читаемости кода и лучшего понимания структуры программы учащимися.

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

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