ROS中如何实现将一个基于A坐标系下的三维向量变换到基于B坐标系下？

摘要

ROS中通过tf.TransformListener.lookupTransform方法获取从A坐标系到B坐标系的旋转四元数rot，通过quaternion_multiply函数获取在新坐标系下的四元数坐标表示。基于此，本博客提供将一个基于A坐标系下的三维向量变换到基于B坐标系下的标准代码模板和应用实例，便于后续查阅。

关键词

ROS， TF， 坐标系变换，四元数，向量表示

前言

在构建海洋机器人动力学模型的时候，其中的外力项通常要包含进风和海流的作用力，这些力在全局大地坐标系（静态坐标系）下的向量表示通常是放方向和大小都较为固定的常量，但是如果要将这些作用引入动力学模型，一般都是基于船体坐标系下的，而随着船体的位置和朝向的变化，风海流在船体坐标系下的向量表示，连同其三轴分量，一定时刻变化着。在得到风海流在大地坐标系下的向量表示后，如何快速求得其在船体坐标系下的分量？

在机器人执行目标跟踪任务的时候，由于机器人的路径规划算法中，目标的坐标表示一般是要基于以机器人中心为原点的坐标系下的，而目标检测的视觉算法得到的距离与偏角却是目标和机载摄像头之间的相对位置关系；如果还要知道目标在全局地图中的坐标，还需要进一步求得在大地坐标系下的位置表示。

这些问题的背后是同一个问题：基于A坐标系下的三维向量在另一个坐标系下的坐标是什么，如何快速求得？ 本博客提供了在ROS中快速实现实现将一个基于A坐标系下的三维向量变换到基于B坐标系下的标准代码模板和应用实例，便于后续查阅。

方法

基本思路

抽象为数学问题后可以表述为：假设一个三维实数空间向量$v$在坐标系A中表示为$v=[x,y,z]\in {\mathbb{R}}^3$，坐标系A到坐标系B的旋转可以用四元数向量表示，设此向量为$q$，求空间向量$v$在坐标系B中的坐标表示。

求法是：首先将向量$v$用四元数$p$表达，用四元数表达的实数向量一定是一个虚四元数，即$p=[0,x,y,z]=[0,v]$，然后，向量$v$在B系中的四元数表示（设此四元数为$p^{\prime }$）即为$$p^{\prime }=qp{q}^{-1}$$ $p^{\prime }$也是一个虚四元数。

出处见下图：

(出处来源链接：https://www.cnblogs.com/gaoxiang12/p/5120175.html)

因此，在ROS中为了实现将一个基于A坐标系下的三维向量变换到基于B坐标系下，需要知道：

1. 向量在A系下的坐标；
2. 向量在A系下的坐标的四元数表示；
3. A系到B系变换的旋转四元数和A平移到B的平移向量

对于具体实现，首先在ROS中实现矩阵乘法

$$p^{\prime }=qp{q}^{-1}$$

并取$p^{\prime }$四元数中的虚部部分即得到变换到基于B坐标系下后的旋转坐标表示。这个旋转坐标表示就是说，三维向量跟着A系旋转，当A系旋转到与B系对齐的时候，三维向量的坐标是旋转坐标。然后，再将各个分量分别加上已经各轴对齐到B的A的平移分量即可。

基本模板

import rospy
import tf
from geometry_msgs.msg import Point

def transform_point_from_base_link_to_world(x, y, listener):
    # Represent the point as a quaternion with a real part (w) of zero
    point_base_link = [x, y, 0, 0]

    try:
        # Wait for the listener to get the first transform to ensure accuracy
        listener.waitForTransform("target_frame", "source_frame", rospy.Time(), rospy.Duration(4.0))

        # Get the transformation from base_link to world
        (trans, rot) = listener.lookupTransform("target_frame", "source_frame", rospy.Time(0))

        # Transform the point: rotate it
        rotated_point = tf.transformations.quaternion_multiply(
            tf.transformations.quaternion_multiply(rot, point_base_link),
            tf.transformations.quaternion_conjugate(rot)
        )

        # Add the translation
        point_world = [rotated_point[0] + trans[0], rotated_point[1] + trans[1], rotated_point[2] + trans[2]]

        return point_world

    except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
        rospy.logerr(f"TF Exception: {e}")
        return None

# ROS node initialization
rospy.init_node('point_transformer')

# Create a TF listener
listener = tf.TransformListener()

# Example usage
transformed_point = transform_point_from_base_link_to_world(1.0, 2.0, listener)
if transformed_point is not None:
    print("Transformed Point in World Frame:", transformed_point)

# ...

部分代码解释

# 假设向量v在A系中的坐标和对应的四元数表示
point_base_link 

# wait for a transform to become available.
listener.waitForTransform("target_frame", "source_frame", rospy.Time(), rospy.Duration(4.0))

利用该类的lookupTransform方法获取从源坐标系到目标坐标系的旋转四元数rot和平移向量
 (trans, rot) = listener.lookupTransform("target_frame", "source_frame", rospy.Time(0))

try:
	(trans,rot) = listener.lookupTransform('parent_frame', 'child_frame', rospy.Time(0))

# 利用quaternion_multiply函数实现p' = qpq^{-1}中的矩阵乘法
v_map = tf.transformations.quaternion_multiply(tf.transformations.quaternion_multiply(rot, v_base_link), tf.transformations.quaternion_conjugate(rot))

# 最终结果，注意如果我们先做平移的话，那么平移的方向是以原坐标系为准的，那么这种变换可能就是不准确的。
 point_world = [rotated_point[0] + trans[0], rotated_point[1] + trans[1], rotated_point[2] + trans[2]]

细节问题

Q：四元数是一个向量，为什么会有逆？

四元数取逆其实是取共轭。

出处见下图：

(出处来源链接：https://zhuanlan.zhihu.com/p/52565002)

而tf.transformations.quaternion_conjugate(quaternion)的作用就是返回四元数的共轭。

出处见下图：
(出处来源链接：https://blog.csdn.net/weixin_44682965/article/details/107818474)

Q：listener.lookupTransform的前两个参数中哪个是父系（源坐标系）哪个是子系（目标坐标系）？

第一个参数是目标坐标系，第二个参数是源坐标系。

Q：rospy.Time(0)的作用？

获取距离调用listener.lookupTransform函数时间最近的rot变量。

出处见下图：

(出处来源链接：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29)

这也意味着，在程序执行一开始的时候，可能是没有可用的rot供获取的，这是需要使用try-except语句的原因。

Q：四元数在ROS中的排布？

• 数学上，一个四元数的表达若设置为[w,x,y,z]，则其中的w为实部，x,y,z为虚部。

出处见下图：

(出处来源链接：https://www.cnblogs.com/gaoxiang12/p/5120175.html)

• 在ROS中，这样的四元数是按如下顺序排布的：[x,y,z,w]，即将实部放在了最后。

出处见下图：

（来源链接：http://wiki.ros.org/tf2/Tutorials/Quaternions）。

Q: Suppose that the transformation data between world and base_link is available at once after my ros node starts, then in this case whether the line “listener.waitForTransform(“world”, “base_link”, rospy.Time(), rospy.Duration(4.0))” is meanningless?

If the transformation data between world and base_link is immediately available after your ROS node starts, the line listener.waitForTransform("world", "base_link", rospy.Time(), rospy.Duration(4.0)) is technically not necessary for the operation of your program in that specific instance. However, there are several reasons why it’s still good practice to include it:

1. Robustness in Dynamic Environments: In a real-world robotics application, the environment is often dynamic. The availability of transformation data right after the node starts does not guarantee its availability at subsequent moments. The waitForTransform call adds robustness to your code by handling situations where the transformation data might not be readily available due to delays or asynchronous updates.

2. Handling Latency and Network Issues: In distributed systems like those typically used in ROS, network delays or high system load can cause latency in data transmission. Even if the transformation data is generally available quickly, there might be times when it is delayed. The waitForTransform function helps to manage these inconsistencies.

3. Future-proofing the Code: Including waitForTransform makes your code more adaptable to different scenarios, including those where the transformation data might not be as promptly available as in your current setup. This is especially important if the code will be used on different robots or in different environments.

4. Error Handling: It provides a straightforward way to handle errors related to the unavailability of transformation data. By specifying a timeout, you can control how long your node should wait before it decides that the data isn’t coming and potentially takes some alternative action.

In summary, while listener.waitForTransform might seem unnecessary in an ideal and static testing environment where data is always immediately available, it is a best practice to include it for handling the unpredictability and dynamic nature of real-world robotics applications.

Q：Why the final coordinate of the point under world frame is [rotated_point[0] + trans[0], rotated_point[1] + trans[1], rotated_point[2] + trans[2]]. Obviously, we can just add the rotation vector and the translation vector respectively which would be the solution, but why?

The reason for combining the rotated point with the translation vector in the formula [rotated_point[0] + trans[0], rotated_point[1] + trans[1], rotated_point[2] + trans[2]] to get the final coordinates of a point in the world frame involves understanding how transformations work in 3D space, especially in the context of robotics and coordinate frame transformations.

In ROS and most 3D coordinate systems, a transformation between two frames consists of two components:

1. Rotation: This adjusts the orientation of the object (or point) from one frame to another. In your case, it aligns the point from the base_link frame to the world frame orientation.

2. Translation: This moves the object (or point) in space from the origin of one frame to the origin of another frame.

Here’s why each step is necessary:

• Rotating the Point: The first step is to rotate the point from its original orientation in the base_link frame to match the orientation of the world frame. This is done using the rotation part of the transformation, which is represented as a quaternion. The rotation aligns the point’s orientation with that of the world frame but doesn’t yet position it correctly in space. At this stage, the point is oriented correctly but is still positioned relative to the base_link origin.

• Translating the Point: After rotation, the point needs to be translated (moved) so that it’s positioned correctly relative to the world frame’s origin. This is where the translation vector comes in. By adding the translation vector to the rotated point, we effectively move the point from its position relative to the base_link frame to a position relative to the world frame.

It’s important to rotate first, then translate, because translation values are dependent on the orientation of the frame. If you translate first and then rotate, the translation will occur along the original frame’s axes, not the target frame’s axes, leading to incorrect positioning in the target frame.

In summary, the rotation adjusts the point’s orientation to align with the new frame, and the translation then positions the point correctly within that frame. This sequence of operations ensures that the point’s final position and orientation are correctly represented in the world frame.

应用

目标

承接在ROS中如何实现两个固定坐标系之间的变换？中应用部分的问题。在该问题中，新静态坐标系为NED（东北地坐标系），设向量$v$在world坐标系中的坐标表示是$v=[1,2,3]$，求该向量在NED坐标系中的坐标表示。

操作过程

1. 打开ROS中如何实现两个固定坐标系之间的变换？中应用部分的脚本
2. 修改代码。脚本代码如下（代码已经过测试）：

#!/usr/bin/env python3
# coding: utf-8

# Neccessary library
import rospy
import tf
import tf2_ros
import geometry_msgs.msg
import numpy as np

rospy.init_node("static_transformation")

static_transformStamped = geometry_msgs.msg.TransformStamped()

broadcaster = tf2_ros.StaticTransformBroadcaster()
static_transformStamped.header.stamp = rospy.Time.now()
static_transformStamped.header.frame_id = "world"
static_transformStamped.child_frame_id = "NED"
# ...此处省略
broadcaster.sendTransform(static_transformStamped)

#重点是以下部分
x,y,z = 1,2,3
vector = [x,y,z]
v_base_link= [vector[0], vector[1],vector[2], 0] 
listener = tf.TransformListener()
loop_counter = 0
r = rospy.Rate(100)
while not rospy.is_shutdown():
    try:
        (trans,rot) = listener.lookupTransform('world', 'NED', rospy.Time(0))
        v_map = tf.transformations.quaternion_multiply(tf.transformations.quaternion_multiply(rot, v_base_link), tf.transformations.quaternion_conjugate(rot))
        v_new = v_map[0:3]
        if loop_counter > 100: #控制打印频率
            print(f"v_new:{v_new}")
            loop_counter = 0
        loop_counter += 1
    except Exception as e:
        print(f"Exception:{e}")
    r.sleep()

3. 启动roscore，source Python文件，rosrun 对应Python节点。
   具体过程略

结果

东北天坐标系下的[1,2,3]放在东北地坐标系下来表示就是[2,1,-3]。

打印结果如下图所示：

延伸阅读

四元数相比欧拉角的优势到底在哪里？

参考文献

略