本文共 2652 字，大约阅读时间需要 8 分钟。

本文介绍了ROS中TF坐标的基本规则，欧拉角和四元数变换，并以一个实际的旋转的例子介绍ROS代码的实现。

1 先复习一下几个结论：

1.1  ROS的坐标系统使用右手定义

对于 ROS 机器人，如果以它为坐标系的原心，那么：

            x轴正向：前方

           y轴正向：左方

           z轴正向：上方

1.2 在一个绕轴线上的旋转，也使用右手定义

 即绕轴旋转的正方向为逆时针旋转，当我们将绕z轴旋转90度时，是绕z轴逆时针旋转90度。

1.3 用于旋转的四元数必须是单位四元数

  欧拉角使用绕x,y,z三个轴的旋转来表示物体旋转

绕 Z轴 旋转，称之为 航向角，使用yaw表示;

绕 X轴 旋转，称之为 横滚角，使用roll表示;

绕 Y轴 旋转，称之为 俯仰角，使用pitch表示;

  四元数是使用一个旋转的向量 + 一个旋转的角度来表示物体旋转。

   Q=(x,y,z,w)

四元数的模使用如下公式计算：

用于旋转的四元数，它的模须为1.

2  ROS中的实现

Ros TF库中定义几个class来完成欧拉角和四元数之间的转换。例如，在ROS Melodic中，位于/opt/ros/melodic/include/tf2/LinearMath

在Quaternion.h中给出了转换使用方法。

在Quaternion.h中，存在以下三种设置方法。

1 setRPY();

这个函数采用固定轴的旋转方式，先绕定轴x旋转（横滚），然后再绕定轴y（俯仰），最后绕定轴z（偏航）。

从数学形式上说，这是绕定轴XYZ矩阵依次左乘，即：R = R(z) R(y)R(x)的顺序。由于是绕着定轴转动，所以很直观，便于人机交互。

/**@brief Set the quaternion using fixed axis RPY

@param roll Angle around X

@param pitch Angle around Y

@param yaw Angle around Z*/

void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)；

2 setEuler

这种方式是绕着动轴转动，先绕Y轴，在绕变换后的X轴，再绕变换后的Z轴旋转。

从数学形式上说，这是绕定轴YXZ矩阵依次右乘，即：R = R(y)R(x)R(z) 的顺序。

/**@brief Set the quaternion using Euler angles

@param yaw Angle around Y

@param pitch Angle around X

@param roll Angle around Z */

void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)；

3 setEulerZYX

与2相同，此种旋转变换也是绕动轴旋转，只不过次序为ZYX，矩阵也是右乘，即R = R(z) R(y)R(x)。我们发现，这里的R与1中的R的最终结果相同，所以1和3的定义最终是等价的。

/**@brief Set the quaternion using euler angles

@param yaw Angle around Z

@param pitch Angle around Y

@param roll Angle around X */

void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) attribute((deprecated))；

总结：

ros中的欧拉角可以分为绕定轴和绕动轴的变换方式，函数没有给出Euler时，是按定轴转动，矩阵依次左乘。若函数的名字中有Euler，则表示为绕动轴转动的方式，矩阵依次右乘。

3 ROS TF 欧拉角四元数代码验证

第一章节中给出几个结论， 下面，通过实际的ROS代码来做一个旋转来验证这些结论。

为了方便理解，我们只绕一个轴旋转。

例如：让三位空间的点（1,1,0）绕Z轴旋转90度， 计算旋转后的点坐标。

如图所示，ROS中，逆时针为正，所以，很容易分析出(1,1,0)绕Z轴旋转90度后的坐标应该为（-1,1,0）。

用ROS TF代码如何实现上面的旋转呢？

代码如下：

#include 
   
    #include 
    
     using namespace std;//定义一个常量表示Pi的值#define PI_CONST 3.141592653589793238462643383279502884int main(int argc, char** argv){//初始化ros::init(argc, argv, "coordinate_transformation");ros::NodeHandle node;tfScalar yaw,pitch,roll;//因为是绕Z轴旋转90度，所以只要yaw值为PI_CONST/2，pitch,roll都为0yaw=PI_CONST/2; pitch=0; roll=0;std::cout<<"欧拉角rpy("<
     
      <<","<
      
       <<","<
       
        <<")";tf::Quaternion q;q.setRPY(roll,pitch,yaw); //将roll, pitch,yas赋给四元数后，可以得到转换后的四元数std::cout<<"，转化到四元数q:"<<"("<
        
         <<","<
         
          <<","<
          
           <<","<
           
            <<")"<
           
          
         
        
       
      
     
    
   

运行结果如下：

$ rosrun tf_demo coordinate_transformation

           欧拉角rpy(0,0,1.5708)，转化到四元数q:(0,0,0.707107,0.707107)

           the quaternion length: 1

           axis of q is:0,0,1

          angle is:1.5708

          (1,1,0) after rotate with Z 90: -1,1,0

输出的结果跟我们分析的完全一致。

转载地址：https://blog.csdn.net/lclfans1983/article/details/106713881 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！