| ROS TF2 中的 四元数 基础部分 | 您所在的位置：网站首页 › ros如何重置 › ROS TF2 中的 四元数 基础部分 |
ROS TF2 中的 四元数 基础部分
ROS TF2 中的 四元数 基础部分
月照银海似蛟龙
分类：ROS
发布时间 2021.04.03阅读数 6804 评论数 3
ROS TF2 中的 四元数 基础部分
1、四元数的组成
2、将 RPY坐标系 下的 角度 转换为 四元数
3、如何通过四元数 做 旋转
4、四元数转置
5、求两个姿态（四元数）的旋转
5、 完毕
这篇博客主要讲解 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::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::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::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)结果如下
将一个姿态（用四元数表示） 做一个 旋转（用四元数表示） ，只需要将 姿态的四元数 乘以旋转的四元数。这个乘法的顺序很重。 例子 #include //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、 完毕 坐标变化ROStf四元数姿态解算
打赏 0 点赞 3 收藏 0 分享 微信 微博 QQ 图片 上一篇：ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换 下一篇：opencv调用yolov3模型进行目标检测，以实例进行代码详解 |
【本文地址】