yaw(pan)/pitch(tilt)/roll计算 | 您所在的位置:网站首页 › pitch怎么记忆 › yaw(pan)/pitch(tilt)/roll计算 |
1、yaw(pan)/pitch(tilt)/roll
我认为,yaw/pitch/roll绕哪个轴旋转,是要看坐标系的朝向的,如果坐标系的轴如下图,则对应关系是yaw(z轴)、pitch(x轴)、roll(y轴)。
如果换个坐标系,比如下图,则是yaw(Y轴)、pitch(X轴)、roll(Z轴)。 2、yaw/pitch/roll的计算 工业上,一般的旋转顺序是yaw=>pitch=>roll的顺序。 所以,这个坐标系的旋转顺序是ZXY,则对应的旋转矩阵是下图的红色部分。 令旋转矩阵 m21 = sinx, x =asin(m21)=>pitch m20 / m22 = -cosxsiny / cosxcosy = -siny / cosy = -tany, y = -atan2(m20, m22)=>roll m01 / m11 = -cosxsinz / coszcosx = -sinz / cosz = -tanz, z = -atan2(m01, m11)=>yaw 3、yaw/pitch/roll的坐标系转换 设数据源的坐标系如下图(同2的计算), 数据目标的坐标系如下(都是右手坐标系), 则yaw的旋转与上一步计算相反,有 x =asin(m21)=>pitch y = atan2(m01, m11)=>yaw z = -atan2(m20, m22)=>roll 4、代码示例 import numpy as np import math as math from autolab_core import RigidTransform # pip install autolab_core # 写上用四元数表示的orientation和xyz表示的position orientation = {'y': -0.6971278819736084, 'x': -0.716556549511624, 'z': -0.010016582945017661, 'w': 0.02142651612120239} position = {'y': -0.26022684372145516, 'x': 0.6453529828252734, 'z': 1.179122068068349} rotation_quaternion = np.asarray([orientation['w'], orientation['x'], orientation['y'], orientation['z']]) translation = np.asarray([position['x'], position['y'], position['z']]) # 这里用的是UC Berkeley的autolab_core,比较方便吧,当然可以自己写一个fuction来计算,计算公式在https://www.cnblogs.com/flyinggod/p/8144100.html m = RigidTransform(rotation_quaternion, translation) r = m.rotation # pitch, yaw, roll的计算公式 pitch = math.asin(r[2, 1]) yaw = -math.atan2(r[0, 1], r[1, 1]) roll = -math.atan2(r[2, 0], r[2, 2]) pitch_ang = pitch * 180.0 / 3.1415926 yaw_ang = -yaw * 180.0 / 3.1415926 # yaw取反 roll_ang = roll * 180.0 / 3.1415926 print('----- ji result -------') print('pitch: %f' % pitch_ang ) print('yaw: %f' % yaw_ang) print('roll_ang: %f' % roll_ang) |
CopyRight 2018-2019 实验室设备网 版权所有 |