如何实现两个固定坐标系之间的变换？

曹勖之

已于 2023-11-01 08:26:30 修改

阅读量254

点赞数 3

分类专栏： ROS 文章标签：机器人 python ROS 坐标系变换 tf

于 2023-10-30 09:31:03 首次发布

本文链接：https://blog.csdn.net/a2213086589/article/details/134104611

版权

ROS 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

摘要

本博客提供了在ROS中快速实现固定（静态）坐标系变换的标准模板和应用实例，便于后续查阅。

关键词

ROS， TF，固定（静态）坐标系变换

前言

在许多机器人或控制相关领域，主要依赖的坐标系可能并不是东北天坐标系，比如在海洋工程领域或制导定位领域，东北地坐标系在很多时候反而更加常用。还有很多情况下，机器人的活动范围会被局限在一定区域内，描述机器人在这个区域中的运动时，如果使用的是全局地图下的坐标系，不如在这个局部区域中选取一个固定位置作为参考坐标系来的方便，然而选取新的位置作为坐标系涉及到坐标系的平移和旋转，因此如果要在ROS中快速实现固定坐标系变换（又称静态坐标系变换），有必要学习ROS中提供的对应封装函数，本博客提供了在ROS中快速实现固定坐标系变换的标准模板和应用实例，便于后续查阅。

方法

基本模板

# Neccessary library
import rospy
import tf
import tf2_ros
import geometry_msgs.msg

# ...

#static initial frame transform: parent_frame->child_frame
static_transformStamped = geometry_msgs.msg.TransformStamped()
broadcaster = tf2_ros.StaticTransformBroadcaster()

static_transformStamped.header.stamp = rospy.Time.now()
static_transformStamped.header.frame_id = "parent_frame"
static_transformStamped.child_frame_id = "child_frame"

static_transformStamped.transform.translation.x = 0.0
static_transformStamped.transform.translation.y = 0.0
static_transformStamped.transform.translation.z = 0.0

quat = tf.transformations.quaternion_from_euler(np.pi, 0, np.pi/2) #roll, pitch, yaw
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]

broadcaster.sendTransform(static_transformStamped)

# ...

注意事项

Q：最初始的坐标系是什么坐标系？

Rviz中默认的最初始坐标系名称是map，可以在Rviz侧边栏Global Options中的Fix Frame栏看到。该名称可以被用户在此栏任意修改。

Q：坐标系旋转顺序？

不同的旋转顺序会导致不同的旋转结果。ROS中的标准旋转顺序是：
父系x正轴->父系y正轴->父系z正轴。
出处见下图：（来源链接：http://wiki.ros.org/tf2/Tutorials/Quaternions）
ROS官方文档中对旋转顺序的定义

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）。

应用

目标

为初始全局坐标系取名为world，然后生成一个东北地坐标系，其x正轴朝北，y正轴朝东，z正轴朝地，坐标原点在world坐标系下位于（10,5,2），将该坐标系取名为NED。NED坐标系需要在Rviz中展现出来。

操作过程

在功能包中新建脚本，脚本路径为: catkin_ws/ros_basic/scripts/static_transformation.py
具体过程略
编写脚本。脚本代码如下（代码已经过测试）：

#!/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"

static_transformStamped.transform.translation.x = 10.0
static_transformStamped.transform.translation.y = 5.0
static_transformStamped.transform.translation.z = 2.0

quat = tf.transformations.quaternion_from_euler(np.pi, 0, np.pi/2)
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]

broadcaster.sendTransform(static_transformStamped)
while not rospy.is_shutdown():
    pass