ABB机器人四元素处理

原创 已于 2025-09-26 18:50:02 修改 · 621 阅读 · 4 ·
本内容遵循CC 4.0 BY-SA版权协议
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
GEO检测
于 2025-08-06 16:26:35 首次发布

在用到ABB机器人时，姿态使用的是四元素的方式，如下
在这里插入图片描述
然后，选择几个需求实现一下，这基本上就把互转实现差不多了
1.四元素转旋转矩阵
q=w+xi+yj+zk 中 w x y z 也对应有些文档里写的q0 q1 q2 q3
在这里插入图片描述
2.旋转矩阵转四元素
在这里插入图片描述
在这里插入图片描述
3.四元素转俯仰角(pitch，θ\thetaθ)、滚转角(roll，ϕ\phiϕ)、偏航角(yaw，ψ\psiψ)
计算方法如下:
在这里插入图片描述
和这个也一样，所以公式应该没啥问题，直接套就行了
在这里插入图片描述
4.欧拉角转四元素
直接套下面公式即可：
在这里插入图片描述
测试代码，欢迎取用，留下点赞+收藏

struct Quaternion {
    double w, x, y, z;
};
void matrixToQuaternion(double R[3][3]) {
    Quaternion q;
    double trace = R[0][0] + R[1][1] + R[2][2]; // 矩阵的迹

    if (trace > 0.0) {
        double s = 0.5 / sqrt(trace + 1.0);
        q.w = 0.25 / s;
        q.x = (R[2][1] - R[1][2]) * s;
        q.y = (R[0][2] - R[2][0]) * s;
        q.z = (R[1][0] - R[0][1]) * s;
    } else {
        if (R[0][0] > R[1][1] && R[0][0] > R[2][2]) {
            double s = 2.0 * sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]);
            q.w = (R[2][1] - R[1][2]) / s;
            q.x = 0.25 * s;
            q.y = (R[0][1] + R[1][0]) / s;
            q.z = (R[0][2] + R[2][0]) / s;
        } else if (R[1][1] > R[2][2]) {
            double s = 2.0 * sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]);
            q.w = (R[0][2] - R[2][0]) / s;
            q.x = (R[0][1] + R[1][0]) / s;
            q.y = 0.25 * s;
            q.z = (R[1][2] + R[2][1]) / s;
        } else {
            double s = 2.0 * sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]);
            q.w = (R[1][0] - R[0][1]) / s;
            q.x = (R[0][2] + R[2][0]) / s;
            q.y = (R[1][2] + R[2][1]) / s;
            q.z = 0.25 * s;
        }
    }
    qDebug()<<"四元素:"<<q.w<<" "<<q.x<<" "<<q.y<<" "<<q.z;
}
//先给一个点位姿XYZUVW
st_PosAttitude p1;
p1.dX = 1311.45;
p1.dY = -50.21;
p1.dZ = 979.57;
p1.dA = 0.24258;
p1.dB = 0.04898;
p1.dC = 0.96886;
p1.dD = 0.00833;
//对应的四元素值
//q=w+xi+yj+zk
double w = p1.dA;
double x = p1.dB;
double y = p1.dC;
double z = p1.dD;
//构建旋转矩阵
double a00 = 1.0 - 2.0 * y * y - 2.0 * z * z;
double a01 = 2.0 * x * y - 2.0 * w * z;
double a02 = 2.0 * x * z + 2.0 * w * y;
double a10 = 2.0 * x * y + 2.0 * w * z;
double a11 = 1.0 - 2.0 * x * x -2.0 * z * z;
double a12 = 2.0 * y * z - 2.0 * w * x;
double a20 = 2.0 * x * z - 2.0 * w * y;
double a21 = 2.0 * y * z + 2.0 * w * x;
double a22 = 1.0 - 2.0 * x * x - 2.0 * y * y;
Eigen::Matrix4d xyzdelt;
xyzdelt<<a00,a01,a02,0,
         a10,a11,a12,0,
         a20,a21,a22,0,
         0,0,0,1;
//旋转矩阵打印出来
qDebug()<<xyzdelt(0,0)<<" "<<xyzdelt(0,1)<<" "<<xyzdelt(0,2)<<" "<<xyzdelt(0,3);
qDebug()<<xyzdelt(1,0)<<" "<<xyzdelt(1,1)<<" "<<xyzdelt(1,2)<<" "<<xyzdelt(1,3);
qDebug()<<xyzdelt(2,0)<<" "<<xyzdelt(2,1)<<" "<<xyzdelt(2,2)<<" "<<xyzdelt(2,3);
qDebug()<<xyzdelt(3,0)<<" "<<xyzdelt(3,1)<<" "<<xyzdelt(3,2)<<" "<<xyzdelt(3,3);

//直接使用Eigen库中的四元素转旋转矩阵，做对比
Eigen::Quaterniond q_odom_curr_tmp;
q_odom_curr_tmp.x() = x;
q_odom_curr_tmp.y() = y;
q_odom_curr_tmp.z() = z;
q_odom_curr_tmp.w() =w;
Eigen::Matrix3d R_odom_curr_tmp;
R_odom_curr_tmp= q_odom_curr_tmp.normalized().toRotationMatrix();
qDebug()<<R_odom_curr_tmp(0,0)<<" "<<R_odom_curr_tmp(0,1)<<" "<<R_odom_curr_tmp(0,2);
qDebug()<<R_odom_curr_tmp(1,0)<<" "<<R_odom_curr_tmp(1,1)<<" "<<R_odom_curr_tmp(1,2);
qDebug()<<R_odom_curr_tmp(2,0)<<" "<<R_odom_curr_tmp(2,1)<<" "<<R_odom_curr_tmp(2,2);

//旋转矩阵转四元素，这个可能会遇到w1为0的情况，但是大部分情况下不用管，和下面那种方法结果一致，如果遇到为0，换下面那种方法试一下
double w1 = 1.0/2.0*(sqrt(1.0  + a00 + a11 + a22));
double x1 = (a21 - a12) / (4.0 * w1);
double y1 = (a02 - a20) / (4.0 * w1);
double z1 = (a10 - a01) / (4.0 * w1);
qDebug()<<"四元素:"<<w1<<" "<<x1<<" "<<y1<<" "<<z1;

//旋转矩阵转四元素，直接用的是以下链接的函数，主要做对比使用
//https://blog.csdn.net/qq_44339029/article/details/142431407?ops_request_misc=%257B%2522request%255Fid%2522%253A%252282244049b96523ad4d78ccf477e67b26%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=82244049b96523ad4d78ccf477e67b26&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~blog~baidu_landing_v2~default-4-142431407-null-null.nonecase&utm_term=%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5%E8%BD%AC%E5%9B%9B%E5%85%83%E7%B4%A0&spm=1018.2226.3001.4450
double R[3][3];
R[0][0] = xyzdelt(0,0);R[0][1] = xyzdelt(0,1);R[0][2] = xyzdelt(0,2);
R[1][0] = xyzdelt(1,0);R[1][1] = xyzdelt(1,1);R[1][2] = xyzdelt(1,2);
R[2][0] = xyzdelt(2,0);R[2][1] = xyzdelt(2,1);R[2][2] = xyzdelt(2,2);
matrixToQuaternion(R);

//四元素转UVW 即 θ ϕ ψ
//俯仰角(pitch):θ\thetaθ   x轴
//滚转角(roll):ϕ\phiϕ   y轴
//偏航角(yaw):ψ\psiψ   z轴
double pitch = asin(2.0 * (w1 * y1 - x1 * z1));
double roll = atan2((2.0 * (w1 * x1 + y1 * z1)),(1.0 - 2.0*(x * x + y * y)));
double yaw = atan2((2.0 * (w1 * z1 + x1 * y1)),(1.0 - 2.0*(z * z + y * y)));
qDebug()<<"rxryrz:"<<pitch<<" "<<roll<<" "<<yaw;
qDebug()<<"rxryrz:"<<pitch*180.0/M_PI<<" "<<roll*180.0/M_PI<<" "<<yaw*180.0/M_PI;

//欧拉角到四元素
double w2 = cos(roll/2.0) * cos(pitch/2.0) * cos(yaw/2.0) + sin(roll/2.0) * sin(pitch/2.0) * sin(yaw/2.0);
double x2 = sin(roll/2.0) * cos(pitch/2.0) * cos(yaw/2.0) - cos(roll/2.0) * sin(pitch/2.0) * sin(yaw/2.0);
double y2 = cos(roll/2.0) * sin(pitch/2.0) * cos(yaw/2.0) + sin(roll/2.0) * cos(pitch/2.0) * sin(yaw/2.0);
double z2 = cos(roll/2.0) * cos(pitch/2.0) * sin(yaw/2.0) - sin(roll/2.0) * sin(pitch/2.0) * cos(yaw/2.0);
qDebug()<<"四元素:"<<w2<<" "<<x2<<" "<<y2<<" "<<z2;

打印出来的结果：

//自己组的旋转矩阵 xyzdelt输出
-0.877518   0.0908681   0.470868   0
0.0989509   0.995063   -0.00762193   0
-0.469236   0.0399043   -0.882177   0
0   0   0   1
//调用Eigen库的输出结果，发现结果是一致的
-0.877512   0.0908679   0.470867
0.0989506   0.995063   -0.0076219
-0.469235   0.0399042   -0.882171
//调用其他博文输出的四元素结果
四元素: 0.24258   0.04898   0.96886   0.00833
//通过公式计算的结果，基本一致
四元素: 0.242573   0.0489813   0.968886   0.00833023
//通过四元素计算的欧拉角
rxryrz: 0.488425   3.09639   3.0293
rxryrz: 27.9847   177.41   173.566
//通过欧拉角计算的四元素，和ABB机器人示教器端显示的一模一样，说明准确
四元素: 0.24258   0.0489824   0.968858   0.00832969

有些公式借鉴该博客：https://blog.csdn.net/weixin_45784125/article/details/144908718?ops_request_misc=%257B%2522request%255Fid%2522%253A%252268b949e6f6310d7a6aa2379907f888f2%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=68b949e6f6310d7a6aa2379907f888f2&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allfirst_rank_ecpm_v1~rank_v31_ecpm-16-144908718-null-null.142v102pc_search_result_base7&utm_term=%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5&spm=1018.2226.3001.4187

标签
#qt #c++ #ABB机器人 #XYZQ1234
ABB机器人通讯进阶——四元素与欧拉角转化
A567852的博客
03-07 8765
机器人四元数是指用四元数来表示机器人的姿态和旋转信息。四元数是一种扩展了复数的数学概念，可以用来表示三维空间中的旋转。在机器人领域，四元数广泛应用于描述机器人的姿态。机器人的姿态通常由位置和姿态角组成，而姿态角可以由四元数表示。四元数可以用来描述机器人在三维空间中的旋转，可以表示旋转的方向和角度。
ABB机器人姿态与四元数（笔记）
Zythonista的博客
07-07 1万+
通常使用机器人作业时，机器人的法兰轴（5轴）往往是垂直与作业平面的，此时需要将机器人的姿态调整（示教）到垂直于工作平面的状态，然后记录当前的姿态，作为之后作业的参考标准姿态。不同机器人表示控件姿态的方式不一样，但也无外乎3中：旋转矩阵、欧拉角和四元数。 ABB机器人使用的是四元数（可转欧拉角）作为机器人控件姿态的表示方式。这里主要是记录在不通过示教的方式下，如何定义四元数，使机器人达到垂直于工作平面的姿态。 5轴竖直向下（常用）：此时，各轴角度为 [0,0,0,0,9...
10、四元数和欧拉角的关系
feiante1的博客
09-25 1330
四元数和欧拉角是可以互相转换的，但是欧拉角是可以直接加减运算，四元数不可以直接加减运算。ABB机器人使用四元数（q1,q2,q3,q4）表示机器人的姿态方向。所以ABB机器人需要先转化为欧拉角计算完毕后再转换为四元数。通常机器人也使用ABC欧拉角表示机器人的姿态。四元数到欧拉角的函数：EulerZYX。欧拉角到四元数：orientZYX。
SLAM中刚体的运动描述
m0_61847109的博客
06-09 1122
物体在真实世界中是运动的，SLAM中求解的机器人位姿就是求解机器人的运动。SLAM的问题就是求解世界坐标系中的静态物理点坐标以及运动的机器人点坐标集合（机器人运动点的集合构成机器人的运动轨迹）。而通常我们获取的信息是以机器人本体为坐标系下的世界点的坐标信息，因此需要完成运动坐标系下的坐标到世界坐标系坐标的转换。
机器人----四元素
m0_60274660的博客
07-02 926
-1,1]
ABB机器人如何与视觉建立通讯
最新发布
weixin_38570076的博客
04-11 1648
ABB机器人如何与视觉建立通讯
ABB机器人---基础编程
m0_69023493的博客
05-25 1万+
1、ABB 2、编程基础
QT与ABB（robotstudio）通讯及ABB机器人基本指令
weixin_50624597的博客
06-02 4264
在机器人执行的rapid程序中也可以对可变量存储类型进行赋值操作，在程序执行后，赋值的结果会一直保持，直到对其进行重新赋值。ABB机器人数据存储描述了机器人控制器内部的各项属性，ABB机器人控制器数据类型多达100余种，其中常见的数据类型包括基本数据、i/o数据、运动相关数据。他在编程语言中表示文本的数据类型。tooldata 工具数据：用于定义工具的特征，包含工具中心点（TCP）的位置和方向，以及工具的负载。dionum 数字值：取值为0或1用于处理数字i/o信号，数字i/o信号中0作为低电平0。
ABB机器人socket通讯
kukakkkkk的博客
03-10 1万+
文章目录前言一、效果展示二、知识储备1.准备工作2.套接字通信的示意图3.客户端指令4.服务器指令三、源码说明 前言 Socket Messaging的作用是允许RAPID程序员通过TCP/IP网络协议在各台计算机之 间传输应用数据。一个套接字代表了一条独立于当前所用网络协议的通用通信通道。 “套接字通信”是源于Berkeley所发布软件Unix的一套标准，而除Unix外，Microsoft Windows等平台也支持该项标准。有了Socket Messaging，机器人控制器上的RAPID 程序就能与另
ABB机器人的系统IO功能详解
热门推荐
sdrzwangyanxin的博客
02-26 4万+
ABB机器人的系统I/O功能可以指定具体的系统输入输出项，将I/O信号与机器人系统的一些特殊功能相关联，从而使信号实现某些特殊功能。使得我们不需要通过示教器或者其它硬件就可以对机器人进行某种控制和监控机器人系统的状态。比如系统输入中的Start可用于机器人的启动控制，系统输出中的CycleOn可用机器人是否正在运行的监控。那么这一次我们就来一起看一下ABB机器人中常用的System I/O的功能。...
ABB 机器人调试总结1
jixiedao的博客
01-28 1万+
前些天去某地出差调试IRB1600机器人遇到各种坑 以前我们在现场调试用的都是标准柜，结果现场搞的紧凑型的小柜子,从供电柜的供电一直到里面的信号配置真是一脸蒙蔽。控制柜包装盒里面也没有什么参考价值的资料，甚至连附带的光盘都没有(后来客服解释说这个要单独购买)，不带这么坑爹呀！调试过程中问题不断。 问题1 转数计数器未更新: 这个问题经验老道的工程师都遇到过，转数计数器未更新翻译成人话就是:机器
ABB机器人发送实时位置数据
疯狂的石头
07-18 1万+
对于不同的主控系统，机器人发送当前位置数据的方式也多种多样。如果使用PC机作为上位机来读取机器人实时位置信息，那么我们就可以通过使用IRC5 OPC Server来读取机器人位置数据，然后再发送给PC上位机；当然，也可以通过PC SDK对机器人控制器进行二次开发，然后通过PC Interface选项，直接读取控制器中机器人的位置信息。如果是使用PLC作为上位机来读取机器人实时位置信息，那么我们就可以通过工业现场通信，如ProfiBus、ProfiNet、DeviceNet等，然后使用ABB机器人内置的数据处
ABB机器人FTP传输机器人文件
龙猫
05-19 9202
ABB机器人使用FTP传输机器人文件
abb机器人编程指令写字_最全的ABB机器人编程指令与函数
weixin_36108036的博客
12-23 1万+
原标题：最全的ABB机器人编程指令与函数1 指令AccSet－降低加速度ActEventBuffer - 事件缓冲启用ActUnit - 启用机械单元Add-增加数值AliasIO - 确定I/O 信号以及别名AliasIOReset-重置I/O 信号以及别名":=" - 分配一个数值BitClear - 在一个字节或双数值数据中清除一个特定位BitSet - 在一个字节或者双数值数据中设置一个特...
ABB机器人编程随记
weixin_43047324的博客
12-07 1万+
ABB编程知识的随笔，记得有点杂乱，等有时间再整理，暂时先这样
ABB机器人指令列表分类详解
sdrzwangyanxin的博客
04-12 2万+
任何带有逻辑的机器都离不开程序，程序是他们的“灵魂”，当然机器人也不例外。光有“魁梧”的躯体还不够，他们也需要思维赋予他们灵性。在ABB机器人中，机器人的程序被称为RAPID，所有的机器人的行为都是通过RAPID语言或者RAPID指令来进行描述与控制的。不同的操作对应不同的指令，比如移动机器人需要一个指令，设置机器人的输出又需要另外的指令。无论是通过FlexPendant示教器还是RobotStu...
ABB机器人开发基础之零点校准
2201_76033684的博客
05-08 1万+
ABB机器人使用绝对值编码器来确定自身位置状态，由于减速比的缘故，机器人手臂旋转180°，电机轴旋转角度实际会旋转数圈，故机器人需要校准零位置时圈数数据。机器人圈数数据记录在串行测量板（SMB板）上，SMB板在机器人本体上，每次开机上电时通过通讯电缆将数据传递给控制器。新机首次调试、更换SMB电池时，拆卸本体电机或者减速齿轮时，或其他情况下造成的计数器数据丢失时，圈数计数器需要重新校零。
ABB机器人如何配置Socket通信
weixin_42031602的博客
02-25 2217
ABB机器人的socket配置及样例
abb机器人指令手册_ABB机器人计算坐标系原点姿态
weixin_39527487的博客
11-06 3969
更多内容请点击上方ABB机器人实战技巧关注也可点击公众号下方往期经典浏览更多内容转载请先后台留言，大家一起支持原创，推动机器人使用和发展本公众号对各类ABB机器人应用，仿真，毕业设计提供技术支持，详细后台留言本公众号诚挚希望与各机器人培训机构，机器人使用单元合作，提供技术支持，详细后台留言1. ABB机器人可以通过如上三点，确立坐标系原点，其中x1到x2构成x方向，y1点到x1x...
abb机器人指令手册_ABB机器人CCLink 配置
weixin_39628405的博客
11-02 3186
更多内容请点击上方ABB机器人实战技巧关注也可点击公众号下方往期经典浏览更多内容转载请先后台留言，大家一起支持原创，推动机器人使用和发展本公众号对各类ABB机器人应用，仿真，毕业设计提供技术支持，详细后台留言本公众号诚挚希望与各机器人培训机构，机器人使用单元合作，提供技术支持，详细后台留言ABB机器人提供CC-Link总线的支持。ABB机器人通过DSQC 378B(如下图)模块，...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值