ROS中的坐标变换

我才睡醒

分类：ROS

发布时间 2022.05.27阅读数 4890 评论数 0

简介

坐标系通过三维空间中的一个点来定义，这个点p被称为坐标原点，从坐标原点延伸出来三个向量，n，t，b，分别对应局部坐标系中的x，y，z。向量轴是标准化的，并且符合右手定则： $b = n \times t$ 。
三个方向的坐标轴变换可以堆在一起，用 $3 \times 3$ 的变换矩阵R表示坐标轴的旋转变换

R = ⎡ ⎣ ⎢ n x n y n z t x t y t z b x b y b z ⎤ ⎦ ⎥

$R=\left[\begin{matrix} n_x & t_x & b_x \\ n_y & t_y & b_y \\ n_z & t_z & b_z \\ \end{matrix}\right]$
将原点向量包含进去，用 4 x 4

的其次变换矩阵：

表示为

T = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ n x n y n z 0 t x t y t z 0 b x b y b z 0 p x p y p z 1 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥

$T=\left[\begin{matrix} n_x & t_x & b_x & p_x\\ n_y & t_y & b_y & p_y\\ n_z & t_z & b_z & p_z\\ 0 & 0 & 0 & 1\\ \end{matrix}\right]$
以上矩阵的计算是可逆的，并且逆矩阵的计算是很方便的。其次变换矩阵之间的计算按照如下形式进行：

A T C = A T B B T C

${^AT_C}={^AT_B} {^BT_C}$ 表示坐标系{C}相对于坐标系{A}的变换。

ROS中提供了tf包来进行坐标变换的各种操作（http://wiki.ros.org/tf）
使用如下命令启动例子：

roslaunch mobot_urdf mobot_w_arm.launch

可以查看tf的话题信息

rostopic info tf

得到如下结果

Type: tf2_msgs/TFMessage

这是消息类型，进一步，可以查看消息

rosmsg show tf2_msgs/TFMessage

得到消息包含的变量

geometry_msgs/TransformStamped[] transforms
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

可以看出，ROS变换数据与 4 x 4 的次坐标变换矩阵并不一致，但是它包含了相同的信息，甚至更多。ROS变换信息包含了一个三维向量（表示位移）和一个四元数（表示旋转）。此外，变换消息还有时间戳，子坐标系和父坐标系等信息。 $4 \times 4$

有许多节点向tf话题发布消息。每一个发布者都发布子坐标系相对于父坐标系的变换关系。在当前的例子中，Gazebo向tf发布消息，因为urdf文件中加入了差分驱动插件：

<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>100</updateRate>
    <leftJoint>right_wheel_joint</leftJoint>
    <rightJoint>left_wheel_joint</rightJoint>
    <wheelSeparation>${track}</wheelSeparation>
    <wheelDiameter>${tirediam}</wheelDiameter>
    <torque>200</torque>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>base_link</robotBaseFrame>
    <publishWheelTF>true</publishWheelTF>
    <publishWheelJointState>true</publishWheelJointState>
  </plugin>
 </gazebo>

运行

rostopic hz tf

可以查看tf的发布频率

subscribed to [/tf]
WARNING: may be using simulated time
average rate: 302.703
    min: 0.000s max: 0.013s std dev: 0.00469s window: 225
average rate: 301.118
    min: 0.000s max: 0.013s std dev: 0.00471s window: 459
average rate: 301.000
    min: 0.000s max: 0.013s std dev: 0.00469s window: 693
average rate: 300.747
    min: 0.000s max: 0.014s std dev: 0.00468s window: 927
average rate: 300.517
    min: 0.000s max: 0.014s std dev: 0.00469s window: 1164
average rate: 300.429
    min: 0.000s max: 0.015s std dev: 0.00470s window: 1401
average rate: 300.368
    min: 0.000s max: 0.015s std dev: 0.00469s window: 1635
average rate: 300.322
    min: 0.000s max: 0.015s std dev: 0.00468s window: 1866
average rate: 300.000
    min: 0.000s max: 0.015s std dev: 0.00468s window: 2101

大概在300Hz。
查看tf消息的数据可以使用

rostopic echo tf

得到

      frame_id: "base_link"
    child_frame_id: "left_wheel"
    transform: 
      translation: 
        x: -4.47909858294e-07
        y: 0.282573167674
        z: 0.165125664005
      rotation: 
        x: -0.452744260782
        y: 0.54316792808
        z: 0.543160481729
        w: 0.452733837175
---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1442
        nsecs: 127000000
      frame_id: "base_link"
    child_frame_id: "right_wheel"
    transform: 
      translation: 
        x: -2.77368998557e-07
        y: -0.282574928023
        z: 0.165114429016
      rotation: 
        x: -0.455898707514
        y: 0.54052002351
        z: 0.54051615107
        w: 0.455891174628
---

在ROS中，旋转变换通常通过四元数来表示，四元数表示旋转是旋转矩阵的替代。它可以变换成 $3 \times 3$ 的旋转矩阵，或者直接进行四元数操作的坐标变换。四元数比旋转矩阵更加简洁，并且有吸引人的数学特性。
关于四元数的细节可以参考其他图书2,[^foot3]，在这里知道四元数与旋转矩阵之间有联系，并且可以相互转换就足够了。
计算旋转变换需要关节角度值，可以通过如下命令得到：

rostopic echo joint_states

Transform listener

Transform listener用于执行从一个坐标系变换到另一个坐标系的变换。 tf_listener通常作为一个独立的线程启动，这一线程订阅tf话题，并且从父子变换关系中集成一个运动学树。由于 tf_listener包含所有的变换发布消息，所以它能够进行特殊的查询比如右手掌相对于左摄像头的变换。查询所回应的是变换消息。
有关tf listener使用的例子都在example_tf_listener包中。主要包含三个代码文件，example_tf_listener.h定义了一个DemoTfListener类，example_tf_listener_fncs.cpp包括类方法的实现；example_tf_listener.cpp中有主程序入口。主程序如下：

//example_tf_listener.cpp:
//wsn, March 2016
//illustrative node to show use of tf listener, with reference to the simple mobile-robot model
// specifically, frames: odom, base_frame, link1 and link2

// this header incorporates all the necessary #include files and defines the class "DemoTfListener"
#include "example_tf_listener.h"
using namespace std;

//main pgm to illustrate transform operations

int main(int argc, char** argv) {
    // ROS set-ups:
    ros::init(argc, argv, "demoTfListener"); //node name
    ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor
    ROS_INFO("main: instantiating an object of type DemoTfListener");
    DemoTfListener demoTfListener(&nh); //instantiate an ExampleRosClass object and pass in pointer to nodehandle for constructor to use

    tf::StampedTransform stfBaseToLink2, stfBaseToLink1, stfLink1ToLink2;
    tf::StampedTransform testStfBaseToLink2;

    tf::Transform tfBaseToLink1, tfLink1ToLink2, tfBaseToLink2, altTfBaseToLink2;

    demoTfListener.tfListener_->lookupTransform("base_link", "link1", ros::Time(0), stfBaseToLink1);
    cout << endl << "base to link1: " << endl;
    demoTfListener.printStampedTf(stfBaseToLink1);
    tfBaseToLink1 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink1);

    demoTfListener.tfListener_->lookupTransform("link1", "link2", ros::Time(0), stfLink1ToLink2);
    cout << endl << "link1 to link2: " << endl;
    demoTfListener.printStampedTf(stfLink1ToLink2);
    tfLink1ToLink2 = demoTfListener.get_tf_from_stamped_tf(stfLink1ToLink2);

    demoTfListener.tfListener_->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);
    cout << endl << "base to link2: " << endl;
    demoTfListener.printStampedTf(stfBaseToLink2);
    tfBaseToLink2 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink2);
    cout << endl << "extracted tf: " << endl;
    demoTfListener.printTf(tfBaseToLink2);

    altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;
    cout << endl << "result of multiply tfBaseToLink1*tfLink1ToLink2: " << endl;
    demoTfListener.printTf(altTfBaseToLink2);

    if (demoTfListener.multiply_stamped_tfs(stfBaseToLink1, stfLink1ToLink2, testStfBaseToLink2)) {
        cout << endl << "testStfBaseToLink2:" << endl;
        demoTfListener.printStampedTf(testStfBaseToLink2);
    }
    cout << endl << "attempt multiply of stamped transforms in wrong order:" << endl;
    demoTfListener.multiply_stamped_tfs(stfLink1ToLink2, stfBaseToLink1, testStfBaseToLink2);

    geometry_msgs::PoseStamped stPose, stPose_wrt_base;
    stPose = demoTfListener.get_pose_from_transform(stfLink1ToLink2);
    cout << endl << "pose link2 w/rt link1, from stfLink1ToLink2" << endl;
    demoTfListener.printStampedPose(stPose);

    demoTfListener.tfListener_->transformPose("base_link", stPose, stPose_wrt_base);
    cout << endl << "pose of link2 transformed to base frame:" << endl;
    demoTfListener.printStampedPose(stPose_wrt_base);

    return 0;
}

在第17行将DemoTfListener类进行了实例化。类的对象中有一个transform listener，在主程序中有4个位置使用ransform listener：24，29,34和57行。
第一个实例化的24行：

demoTfListener.tfListener_->lookupTransform("base_link", "link1", ros::Time(0), stfBaseToLink1);

transform listener 订阅tf话题，并不断尝试从所有发布的父子空间关系中组装成最可能的变换链。一旦 transform listener 被实例化，并且有一个简短的时间来收集已经发布的变换关系，就有很多方法可以使用。lookupTransform方法可以用来找到两个坐标系之间的变换关系（比如base_link和link1）。lookupTransform方法的结果传给tf::StampedTransform类型的对象stfBaseToLink1。这种带有标记的变换包含的原点位移和旋转。
lookupTransform()函数使用特定参数来定义感兴趣的坐标系与期望的坐标系。参数ros::Time(0) 说明当前坐标系是期望的。
对象stfBaseToLink1有一个时间戳，作为参考坐标系和子坐标系的标签，从tf :: StampedTransform对象中提取tf :: Transform并不像预期的那么简单。函数get_tf_from_stamped_tf（）在DemoTfListener类中定义以进行协助。

tfBaseToLink1 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink1);

tfBaseToLink1变换是从stfBaseToLink1中提取出来的。这一变换描述的是link1相对于base_frame的位置和旋转的变换。其他几行的实例化操作与此类似。

*操作由tf::Transform 对象定义，所以tfBaseToLink1 和 tfLink1ToLink2 可以乘在一起，在41行所示

demoTfListener.tfListener_->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);

这种操作的意思是将两个变换串联到一起。等同于 $4 \times 4$ 矩阵相乘。altTfBaseToLink2的结果与下面的结果一样

demoTfListener.tfListener_->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);

1

然后再从stfBaseToLink2解析得到变换矩阵。

运行这个例子，先启动

roslaunch mobot_urdf mobot_w_arm_and_jnt_pub.launch

然后运行

rosrun robot_state_publisher robot_state_publisher

接着运行

rosrun example_tf_listener example_tf_listener

可以得到如下结果

[ INFO] [1519276137.994933013, 46.997000000]: tf is good

base to link1: 
frame_id: base_link
child_frame_id: link1
vector from reference frame to to child frame: -0.27305,0,0.55
orientation of child frame w/rt reference frame: 
1,0,0
0,1,0
0,0,1
quaternion: 0, 0, 0, 1

link1 to link2: 
frame_id: link1
child_frame_id: link2
vector from reference frame to to child frame: 0,0,1
orientation of child frame w/rt reference frame: 
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

base to link2: 
frame_id: base_link
child_frame_id: link2
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame: 
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

extracted tf: 
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame: 
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

result of multiply tfBaseToLink1*tfLink1ToLink2: 
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame: 
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

testStfBaseToLink2:
frame_id: base_link
child_frame_id: link2
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame: 
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

attempt multiply of stamped transforms in wrong order:
can't multiply transforms; mismatched frames
link2 is not base_link

pose link2 w/rt link1, from stfLink1ToLink2
frame id = link1
origin: 0, 0, 1
quaternion: 0, 0.00998362, 0, 0.99995

pose of link2 transformed to base frame:
frame id = base_link
origin: -0.27305, 0, 1.55
quaternion: 0, 0.00998362, 0, 0.99995

以上结果可以与主程序进行对比。

geometry_msgs类型与tf类型的转化是很复杂的，example_tf_listener_fncs.cpp中提供了将tf::StampedTransform解析成geometry_msgs::PoseStamped类型变量的方法。如下所示

geometry_msgs::PoseStamped DemoTfListener::get_pose_from_transform(tf::StampedTransform tf) {
  //clumsy conversions--points, vectors and quaternions are different data types in tf vs geometry_msgs
  geometry_msgs::PoseStamped stPose;
  geometry_msgs::Quaternion quat;  //geometry_msgs object for quaternion
  tf::Quaternion tfQuat; // tf library object for quaternion
  tfQuat = tf.getRotation(); // member fnc to extract the quaternion from a transform
  quat.x = tfQuat.x(); // copy the data from tf-style quaternion to geometry_msgs-style quaternion
  quat.y = tfQuat.y();
  quat.z = tfQuat.z();
  quat.w = tfQuat.w();  
  stPose.pose.orientation = quat; //set the orientation of our PoseStamped object from result

  // now do the same for the origin--equivalently, vector from parent to child frame 
  tf::Vector3 tfVec;  //tf-library type
  geometry_msgs::Point pt; //equivalent geometry_msgs type
  tfVec = tf.getOrigin(); // extract the vector from parent to child from transform
  pt.x = tfVec.getX(); //copy the components into geometry_msgs type
  pt.y = tfVec.getY();
  pt.z = tfVec.getZ();  
  stPose.pose.position= pt; //and use this compatible type to set the position of the PoseStamped
  stPose.header.frame_id = tf.frame_id_; //the pose is expressed w/rt this reference frame
  stPose.header.stamp = tf.stamp_; // preserve the time stamp of the original transform
  return stPose;
}

尽管向量和四元数分别定义在了geometry_msgs和tf中，这些类型并不是兼容的。上述代码表示了他们之间的转化。

当实例化一个transform listener时，查找函数应该针对返回的错误进行测试。这在构造函数中实现：

DemoTfListener::DemoTfListener(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{ 
    ROS_INFO("in class constructor of DemoTfListener");
    tfListener_ = new tf::TransformListener;  //create a transform listener and assign its pointer
    //here, the tfListener_ is a pointer to this object, so must use -> instead of "." operator
    //somewhat more complex than creating a tf_listener in "main()", but illustrates how
    // to instantiate a tf_listener within a class

    // wait to start receiving valid tf transforms between base_link and link2:
    // this example is specific to our mobot, which has a base_link and a link2
    // lookupTransform will through errors until a valid chain has been found from target to source frames
    bool tferr=true;
    ROS_INFO("waiting for tf between link2 and base_link...");
    tf::StampedTransform tfLink2WrtBaseLink; 
    while (tferr) {
        tferr=false;
        try {
                //try to lookup transform, link2-frame w/rt base_link frame; this will test if
            // a valid transform chain has been published from base_frame to link2
                tfListener_->lookupTransform("base_link", "link2", ros::Time(0), tfLink2WrtBaseLink);
            } catch(tf::TransformException &exception) {
                ROS_WARN("%s; retrying...", exception.what());
                tferr=true;
                ros::Duration(0.5).sleep(); // sleep for half a second
                ros::spinOnce();                
            }   
    }
    ROS_INFO("tf is good");
    // from now on, tfListener will keep track of transforms; do NOT need ros::spin(), since
    // tf_listener gets spawned as a separate thread
}

Transforming ROS datatypes

使用XformUtils进行数据格式转换

ROS节点通过定义的消息类型相互通信。ROS发布者解构消息对象的数据组件，序列化数据并通过话题发送它。对应的subscriber接收连续消息数据并重构消息组件。该过程便于通信，但消息类型对于执行数学运算可能很麻烦。

xform_utils包含一个便利的转换函数库。比如transformPoseToEigenAffine3d()函数可以将geometry_msgs::Pose转换为Eigen::Affine3d类型的数据，如下所示

Eigen::Affine3d XformUtils::transformPoseToEigenAffine3d(geometry_msgs::PoseStamped stPose) {
    Eigen::Affine3d affine;
    geometry_msgs::Pose pose = stPose.pose;
    Eigen::Vector3d Oe;
    //ROS_WARN("xformUtils: input pose:");
    //printPose(pose);
    Oe(0) = pose.position.x;
    Oe(1) = pose.position.y;
    Oe(2) = pose.position.z;
    affine.translation() = Oe;


    Eigen::Quaterniond q;
    q.x() = pose.orientation.x;
    q.y() = pose.orientation.y;
    q.z() = pose.orientation.z;
    q.w() = pose.orientation.w;
    Eigen::Matrix3d Re(q);

    affine.linear() = Re;
    affine.translation() = Oe;
    //printAffine(affine);
    return affine;
}

使用这些函数需要在package.xml中包含xform_utils依赖dependency，源码应该包含头文件 #include < xform_utils/xform_utils.h >。

小结

这篇需要结合代码学习。
坐标变换在机器人学中是普遍存在的，相应的，ROS中也有大量的工具来处理坐标变换的问题。不过，这些可能有点令人困惑，因为有多种表示形式。 tf库提供了一个tf listener，可以在节点内方便地使用，以监听所有转换发布的消息并将其组装成连贯的链。 tf listener可以查询任何两个连接帧之间的空间关系。并用最新的变换信息进行响应。相比之下，规划师会考虑假设关系。因此，tf对于规划目的不太有用，因此可能需要独立计算运动转换以进行规划。此外，虽然tf是动态更新的，但有一个潜在的限制是组合的转换链可能稍微有时间延迟。因此，tf结果不应用于时间关键的反馈环路中（例如，对于力控制，可能需要计算自己的运动转换）。

A Systematic Approach to Learning Robot Programming with ROS ↩
FundaJ, PaulRP. A comparison of transforms and quaternions in robotics. Proceedings. 1988 IEEE International Conference on Robotics and Automation, pages 886–891 vol. 2, Apr 1988.
[^foot3]: RichardM. Murray, Zexiang Li, and S. Boca Raton: Shankar Sastry. A mathematical introduction to robotic manipulation. CRC Press; 1994. ↩

ROS tf 坐标变换 C++

打赏 0

上一篇：用moveit进行机器人运动规划概览

ROS中的坐标变换

我才睡醒

简介

Transform listener

Transforming ROS datatypes

小结

为你推荐

精选深入理解ikd-Tree从阅读代码开始(一)

IntelRealSenseZR300+UR10RG2机械手臂+Linemod算法机械臂抓取可乐罐系列第六篇

室内场景数据集

ROS 使用GDB调试详解

ROS学习（22）TF变换

ORB SLAM2源码解读(八)：Initializer类

关于作者

我才睡醒

13

0

0

1

URDF学习

动力下肢矫形器和外骨骼辅助策略综述

在gazebo中对机器人进行控制，并在rviz中同步显示

相关推荐

Linux下使用VScode开发ROS时clangd插件的使用

ROS理论与实践——二、ROS基础

ROS1可视化利器---Webviz

ROS操作系统学习（十）服务数据的定义与使用

dynamixel舵机在ros下的workbnech使用

DSO源码解读(二)：前端里程计

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

ROS中的坐标变换

我才睡醒

简介

Transform listener

Transforming ROS datatypes

小结

为你推荐

精选深入理解ikd-Tree从阅读代码开始(一)

IntelRealSenseZR300+UR10RG2机械手臂+Linemod算法 机械臂抓取可乐罐 系列第六篇

室内场景数据集

ROS 使用GDB调试详解

ROS学习（22）TF变换

ORB SLAM2源码解读(八)：Initializer类

评论（0）

关于作者

我才睡醒

13

0

0

1

URDF学习

动力下肢矫形器和外骨骼辅助策略综述

在gazebo中对机器人进行控制，并在rviz中同步显示

相关推荐

Linux下使用VScode开发ROS时clangd插件的使用

ROS理论与实践——二、ROS基础

ROS1可视化利器---Webviz

ROS操作系统学习（十）服务数据的定义与使用

dynamixel舵机在ros下的workbnech使用

DSO源码解读(二)：前端里程计

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

IntelRealSenseZR300+UR10RG2机械手臂+Linemod算法机械臂抓取可乐罐系列第六篇

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板