Кватернионы
В робототехнике, особенно, для трехмерной ориентации робота, используют систему координат в кватернионах.
Квартернион (для определения ориентации) представляет собой трехмерный вектор направления смещения относительно начального базиса и угол вращения тела вокруг этого вектора.
Например, если мы посмотрим в топик /odom, то мы увидим текущую ориентацию робота, выраженную как кватернион нормализованный к единице длины.
orientation:
x: 0.0
y: 0.0
z: 0.10795690539
w: 0.994155574635
Однако, учитывая что робот TurtleBro при движении может вращаться только вокруг оси Z (курс). То для некоторых случаев нам проще оперировать более наглядными углами Эйлера.
В двухмерной плоскости, ориентацию робота в углах Эйлера обычно обозначают как угол theta и измеряют в радианах.
Для перевода квартерниона в угол theta мы можем воспользоваться функцией.
import math
def quaternion_to_theta(orientation):
t1 = +2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
t2 = +1.0 - 2.0 * (orientation.y ** 2 + orientation.z**2)
return math.degrees(math.atan2(t1, t2))
Если вам необходимы все углы (крен, тангажа и рыскания), лучше воспользоваться встроенными функциями ROS в модуле tf. При помощи них вы можете конвертировать углы Эйлера в квартернион и обратно.
from tf.transformations import *
# Преобразование из углов Эйлера в кватернион
quaternion = quaternion_from_euler(0, 0, 0)
# Преобразование из кватерниона в углы Эйлера
euler = euler_from_quaternion(quaternion)
Мы можем вычислить угол theta для приведенной выше ориентации.
import math
from tf.transformations import *
quaternion = [0.0,0.0,0.10795690539,0.994155574635]
euler = euler_from_quaternion(quaternion)
theta = math.degrees(euler[2])
//12.39510694372898
Расчет ориентации
Для вычисления кватерниона новой ориентации после поворота на произвольный угол (например: текущий угол + pi/2 (90 градусов)) мы должны взять текущую ориентацию выраженную в кватернионе (до поворота), и умножить ее на квартернион угла поворота.
import math
from tf.transformations import *
current_quaternion = [0.0, 0.0, 0.10795690539, 0.994155574635]
delta_quaternion = quaternion_from_euler(0, 0, math.pi/2)
new_quaternion = quaternion_multiply(current_quaternion, delta_quaternion)
Далее мы можем привести полученный квартернион в угол theta
euler = euler_from_quaternion(new_quaternion)
theta = math.degrees(euler[2])
// 102.39510694372898