这篇博客主要讲解 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
  
2
  
3
  
4
  
5
 

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

q.normalize();

  

 

  
1
 

对于四元数 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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
 

• 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)

  

 

  
1
  
2
  
3
  
4
  
5
 

结果如下

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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
 

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)

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
 

5、 完毕

文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/qq_32761549/article/details/109110598