• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

ROS TF2 中的 四元数 基础部分

人工智能 月照银海似蛟龙 1733次浏览 0个评论

ROS TF2 中的 四元数 基础部分

 

这篇博客主要讲解 ROS中四元数用法的基础知识。

1、四元数的组成

 ROS使用四元数来跟踪和应用旋转。

一个四元素有4个成员(x,y,z,w)。

注意: w 是最后一个,但是一些库像 Eigen 把 w 放在第一的位置。

不绕x / y / z轴旋转的 常用单位 四元数为(0,0,0,1):

 

在一个节点 加入下面 代码 setRPY() 会 把 绕 x、y、z轴旋转的角度 转化 为 四元数

设置不绕x / y / z轴旋转的setRPY( 0, 0, 0 ) 打印出四元数 则 为 (0,0,0,1)

#include <tf2/LinearMath/Quaternion.h>
...
tf2::Quaternion myQuaternion;
myQuaternion.setRPY( 0, 0, 0 );  // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f  %f  %f  %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w());  // Print the quaternion components (0,0,0,1)

在这里插入图片描述

 四元数的大小应为1。 如果数值错误导致四元数的大小不为1,ROS将显示警告。 为避免这些警告,需要对四元数进行标准化:

q.normalize();

对于四元数 ROS 有两种 数据类型:msg 和 tf

msg geometry_msgs::Quaternion quat_msg 这种声明的就是 msg 类型

tf tf2::Quaternion myQuaternion 这种声明的就是 tf 类型

在 tf2_geometry_msgs.h 文件 中 提供 了 数据类型 转换的 函数

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion quat_tf;
geometry_msgs::Quaternion quat_msg = ...;

tf2::convert(quat_msg , quat_tf);
// or
tf2::fromMsg(quat_msg, quat_tf);
// or for the other conversion direction
quat_msg = tf2::toMsg(quat_tf);

 convert() 即 将 msg 类型 转换 为 tf 类型

fromMsg() 即 将 msg 类型 转换 为 tf 类型

toMsg() 即 将 tf 类型 转换 为 msg 类型

2、将 RPY坐标系 下的 角度 转换为 四元数

对于人来说,绕轴旋转很容易,直接写出四元数则很难。

在求 一个 物体 姿态的时候 可以 如下

先以(围绕X轴的滚动)/(围绕Y轴的后续俯仰)/(随后围绕Z轴的偏航)计算目标旋转,然后转换为四元数:

setRPY() 会 把 绕 x、y、z轴旋转的角度 转化 为 四元数 注意 形参 是 弧度

#include <tf2/LinearMath/Quaternion.h>

tf2::Quaternion myQuaternion;
myQuaternion.setRPY(1.5707, 0, -1.5707);  // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f  %f  %f  %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w());  // Print the quaternion components (0,0,0,1)

结果如下

在这里插入图片描述

3、如何通过四元数 做 旋转

将一个姿态(用四元数表示) 做一个 旋转(用四元数表示) ,只需要将 姿态的四元数 乘以旋转的四元数。这个乘法的顺序很重。

例子

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

//q_orig  是原姿态转换的tf的四元数
//q_rot   旋转四元数
//q_new   旋转后的姿态四元数
tf2::Quaternion q_orig, q_rot, q_new;

// commanded_pose.pose.orientation  这个比如说 是 订阅的别的节点的topic 是一个  姿态的 msg 四元数
//通过tf2::convert()  转换成 tf 的四元数
tf2::convert(commanded_pose.pose.orientation , q_orig);

// 设置 绕 x 轴 旋转180度
double r=3.14159, p=0, y=0;  
q_rot.setRPY(r, p, y);//求得 tf 的旋转四元数

q_new = q_rot*q_orig;  // 通过 姿态的四元数 乘以旋转的四元数 即为 旋转 后的  四元数
q_new.normalize(); // 归一化

//  将 旋转后的 tf 四元数 转换 为 msg 四元数
tf2::convert(q_new, commanded_pose.pose.orientation);

4、四元数转置

四元数 转置 直接 把 w 参数 就 负 即 可

5、求两个姿态(四元数)的旋转

假如在一个坐标系下有两个 姿态 用四元数 表示的q_1和q_2,那如何 求这两个姿态的旋转四元数q_r呢。

q_2 = q_r*q_1

可以类似于求解矩阵方程来求解q_r。 颠倒q_1并将两边右乘。 同样,乘法的顺序很重要:

q_r = q_2*q_1_inverse

举个例子

q1_inv[0] = prev_pose.pose.orientation.x
q1_inv[1] = prev_pose.pose.orientation.y
q1_inv[2] = prev_pose.pose.orientation.z
q1_inv[3] = -prev_pose.pose.orientation.w //注意这个负号

q2[0] = current_pose.pose.orientation.x
q2[1] = current_pose.pose.orientation.y
q2[2] = current_pose.pose.orientation.z
q2[3] = current_pose.pose.orientation.w

qr = tf.transformations.quaternion_multiply(q2, q1_inv)

5、 完毕


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS TF2 中的 四元数 基础部分
喜欢 (0)

您必须 登录 才能发表评论!

加载中……