做项目时用到ABB机器人，直接通过ABB内置的函数可以轻松实现四元数读数与欧拉角的相互转化。但实际项目需要从示教器读出相关位置并自行计算，尤其需要计算旋转矩阵。
本文以ABB IRB120机器人（不确定其他机器人是否与ABB机器人一致）为例如下姿态为例来描述上述几个量的计算。
图1 机器人在Robot studio中的姿态
图2 示教器中四元数读数
图3 示教器中欧拉角读数
值得注意的是，ABB机器人的欧拉角是ZYX欧拉角。
1. 求旋转矩阵
(1) 已知四元数求旋转矩阵
此处给出matlab代码：
q=[0.27367, 0.75058, 0.46598, 0.38025];
fprintf('Quaternion rotation matrix: \n');
disp(Rotation(q));
function R = Rotation(Q)
Q = Q./sqrt(sum(Q.*Q));
q0 = Q(1); q1 = Q(2); q2 = Q(3); q3 = Q(4);
R = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2);
2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1);
2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2;
];
end
结果为：
Quaternion rotation matrix:
0.2765 0.4914 0.8259
0.9076 -0.4159 -0.0564
0.3158 0.7652 -0.5610
(2) 已知欧拉角求旋转矩阵
此处用到了Matlab Robotics Toolbox工具箱，这是一个机器人仿真和建模非常好用的工具箱，以下给出安装链接：
Matlab Robotics Toolbox工具箱安装教程
1.https://www.bilibili.com/read/cv14423551
2.https://petercorke.com/toolboxes/robotics-toolbox/
以下为matlab代码：
fprintf('Euler rotation matrix: \n');
disp(rotz(73.06,'deg')*roty(-18.41, 'deg')*rotx(126.25, 'deg'));
其中rotx，roty, rotz为Matlab Robotics Toolbox工具箱中的函数。
结果为：
Euler rotation matrix:
0.2765 0.4914 0.8259
0.9077 -0.4159 -0.0563
0.3158 0.7652 -0.5610
可以看到采用四元数与欧拉角算得的旋转矩阵一致（有细微的差别，推测来源于小数截断）。
2. 欧拉角、旋转矩阵与四元数转换
(1) 欧拉角转四元数
此处推荐一个python库transformations，安装方法：
pip install transformations
库的github链接：
https://github.com/cgohlke/transformations
直接调用库中函数即可实现转换，以下给出python代码：
import transformations
import numpy
import math
def euler(roll, pitch, yaw, axes='rzyx'):
yaw = float(yaw) / 180 * numpy.pi
pitch = float(pitch) / 180 * numpy.pi
roll = float(roll) / 180 * numpy.pi
return list(transformations.quaternion_from_euler(roll, pitch, yaw, axes=axes))
roll = 73.06#Ez
pitch = -18.41#Ey
yaw = 126.25#Ex
quaternion = euler(roll, pitch, yaw)
print(quaternion)
结果为：
[0.27362606284972685, 0.7505716495097842, 0.46601038304769293, 0.380270035071476]
此处也手动实现了欧拉角到四元数的转换，代码如下：
def self_euler_quaternion(roll, pitch, yaw):
yaw = float(yaw) / 180 * numpy.pi
pitch = float(pitch) / 180 * numpy.pi
roll = float(roll) / 180 * numpy.pi
r, p, y = roll/2, pitch/2, yaw/2
sinr, sinp, siny = math.sin(r), math.sin(p), math.sin(y)
cosr, cosp, cosy = math.cos(r), math.cos(p), math.cos(y)
q0 = cosr * cosp * cosy + sinr * sinp * siny
q3 = sinr * cosp * cosy - cosr * sinp * siny
q2 = cosr * sinp * cosy + sinr * cosp * siny
q1 = cosr * cosp * siny - sinr * sinp * cosy
return numpy.array([q0, q1, q2, q3])
roll = 73.06#Ez
pitch = -18.41#Ey
yaw = 126.25#Ex
quaternion = euler(roll, pitch, yaw)
print(self_euler_quaternion(roll, pitch, yaw))
结果为：
[0.27362606 0.75057165 0.46601038 0.38027004]
可以看到，与示教器上的结果基本一致。
(2) 旋转矩阵转四元数
给出如下matlab代码：
function q = vgg_quat_from_rotation_matrix( R )
% vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix
% q = vgg_quat_from_rotation_matrix(R)
q = [ (1 + R(1,1) + R(2,2) + R(3,3)) (1 + R(1,1) - R(2,2) - R(3,3)) (1 - R(1,1) + R(2,2) - R(3,3)) (1 - R(1,1) - R(2,2) + R(3,3)) ];
%if ~issym(q)
A = true;
if ~A
% Pivot to avoid division by small numbers
[b I] = max(abs(q));
else
% For symbolic quats, just make sure we're nonzero
for k=1:4
if q(k) ~= 0
I = k;
break
end
end
end
q(I) = sqrt(q(I)) / 2 ;
if I == 1
q(2) = (R(3,2) - R(2,3)) / (4*q(I));
q(3) = (R(1,3) - R(3,1)) / (4*q(I));
q(4) = (R(2,1) - R(1,2)) / (4*q(I));
elseif I==2
q(1) = (R(3,2) - R(2,3)) / (4*q(I));
q(3) = (R(2,1) + R(1,2)) / (4*q(I));
q(4) = (R(1,3) + R(3,1)) / (4*q(I));
elseif I==3
q(1) = (R(1,3) - R(3,1)) / (4*q(I));
q(2) = (R(2,1) + R(1,2)) / (4*q(I));
q(4) = (R(3,2) + R(2,3)) / (4*q(I));
elseif I==4
q(1) = (R(2,1) - R(1,2)) / (4*q(I));
q(2) = (R(1,3) + R(3,1)) / (4*q(I));
q(3) = (R(3,2) + R(2,3)) / (4*q(I));
end
end
测试代码为：
vgg_quat_from_rotation_matrix(rotz(95.03,'deg')*roty(-18.37, 'deg')*rotx(174.57, 'deg'))
结果为：
0.0860 -0.6716 -0.7221 -0.1422
与示教器结果一致。
(3) 四元数转欧拉角
由于项目暂时没有这个需求，因此这部分没有实现，感兴趣的小伙伴可以提供相关代码。文章来源：https://www.toymoban.com/news/detail-779837.html
3. 总结
本文以ABB IRB120机器人为例，给出了通过四元数、欧拉角计算旋转矩阵以及四元数到欧拉角的转换代码，实测与示教器的结果一致，相关代码可直接用于工程上坐标转换。文章来源地址https://www.toymoban.com/news/detail-779837.html
到了这里，关于ABB机器人欧拉角与四元数的相互转化以及旋转矩阵的求法的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
