第六课 ROS的空间描述和变换

1、空间描述与变换

有两个坐标系A和B，B坐标系中有一个点P，如何把B坐标系中的P映射到A坐标系呢，这就涉及到空间描述与变换，

先看一下旋转矩阵：

上面中间的行向量中的元素表示在B坐标系当中的元素用A坐标系来表达。

是B坐标原点在A坐标系中的表达，它也是平移量；^AP表示P在A坐标系中的表达，^BP表示P在坐标系B中的表达。

要想将^BP转换为A坐标系中的表达，就需要乘以旋转矩阵，它是旋转量。

下面目的是将B坐标系中的P点在A坐标系中进行表达

将它可以写成齐次坐标的形式如下，

中间的表达式可以成为旋转算子，

首先，看一下xyz固定角坐标系：

首先将一个坐标系B与一个已知坐标系A重合，先将B坐标系绕X_A坐标系逆时针旋转γ角度，这里X轴坐标不变；然后再绕Y_A坐标轴逆时针旋转β角度；最后绕ZA轴逆时针旋转α角度。每一次旋转都是按照对应的固定参考系。我们将这种姿态的表示方法叫做X-Y-Z固定角坐标系。有时也叫偏航角，横滚角，俯仰角。

再来看一下欧拉表示法：

首先将一个坐标系B与一个已知坐标系A重合，首先将B坐标系绕Z_A轴逆时针旋转α角度；再绕Y_A坐标轴逆时针旋转β角度；最后绕X_A坐标系逆时针旋转γ角度。这个与上面的区别在于，每次旋转都是绕着坐标系B中的坐标轴旋转而不是绕着坐标系A中的坐标旋转。

最后来看一下四元素表示法：

与矩阵相比四元组更加高效，占用的空间更小，且便于插值。四元组表示复数，w+xi+yj+zk,其中i,j,k都是虚数单位。可以把四元组看做是一个标量和一个3维向量的组合；实部w表示标量，虚部(x,y,z)用v表示。其中w与旋转角度相关，v与旋转轴相关。

操作四元组，欧拉角或者X-Y-Z固定角可以用Bullet方法，KDL::Rotation方法等

tf

tf的原理

有三个坐标系，一个未知坐标系query，已知坐标系known，一个世界坐标系world

要求得未知坐标系在世界坐标系当中的表达，可以用旋转算子来表示：

未知坐标系在世界坐标系中的旋转算子=未知坐标系在已知坐标系中的旋转算子*已知坐标系在世界坐标系中的旋转算子，通过例子来看一下，

1、roscore

2、roslaunch turtle_tf turtle_tf_demo.launch

出现两只乌龟一只乌龟随着另外一只乌龟跑，

一只乌龟跟随另一只乌龟的运动而运动，这里面就包含三个坐标系，一个是乌龟1坐标系，一个是乌龟2坐标系，还有一个世界坐标系，

使用命令行工具，

view frames是以pdf的形式来展示运行框架的，

rosrun tf view_frames

打开该pdf，如下

turtle1和turtle2的父坐标系均为world坐标系。

然后再看，

下面看一下turtle1与turtle2作为之间的关系，前者作为父坐标系，后者作为子坐标系

再看一下tf_monitor工具

仍然以turtle1作为父框架，turtle2作为子框架

可以看到上面从turtle1到turtle2的变换的相关信息

TransformBroadcaster有一个方法sendTransform它可以提供的信息是已知坐标系到世界坐标系的旋转算子。

TransformListener提供了一个方法lookupTransform提供了一个未知坐标系到一个已知坐标系的旋转算子。

ExtrapolationException异常是两个frame之间存在请求，但是其中有一个frame是过时的，例如turtle1在listen时请求的是turtle2在50秒之前的请求，则会无效。

编写程序实现turtle tf_demo那个例子

打开eclipse，导入工程，然后新建一个源文件turtle_tf_broadcaster.cpp

#include<ros/ros.h>

#include<tf/transform_broadcaster.h>

#include<turtlesim/Pose.h>//turtlesim广播的是一个位姿消息，包括位置和角度

//定义一个全局变量，用于在命令行中输入turtle的名字

std::string turtle_name;

//回调函数的消息为turtlesim的msg

void posecallback(turtlesim::PoseConstPtr &msg)

{

　　//创建一个TransformBroadcaster存储变换的相关信息

　　static tf::TransformBroadcaster br;

　　//接着创建一个对象Transform存储变换的相关信息,它包含旋转和位移的信息，有一个向量叫做Vector3表示坐标，以及四元组表示方向

　　tf::Transform transform;

　　//下面把turtlepose的相关信息copy到transform中，通过transform在tf树中进行广播，

　　transform.setOrigin(tf::Vector3(msg->x,msg->y,0));

　　//创建一个四元组

　　tf::Quaternion q；

　　//调用它的方法,因为它是绕着z轴旋转故有下面参数形式

　　q.setRPY(0,0,msg->theta);

//为transform的rotation赋值

　　transform.setRotation(q);

　　//然后再广播出去,创建一个transform::stamped对象

　　br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));

}

int main(int agc,char **argv)

{

　　ros::init(argc,argv,"turtle_tf_broadcaster");

　　if(argc!=2)

　　{

　　　　ROS_ERROR("need turtle name as argument.");

　　　　return -1;

　　}

　　turtle_name=argv[1];//传递进来的名字

　　ros::NodeHandle node;

　　//下面创建一个subscriber订阅对象,订阅的主题是turtle_name+"/pose",列表长度为10，回调函数为posecallback

　　ros::Subscriber sub=node.subscribe(turtle_name+"/pose",10，&posecallback);

　　ros::spin();

　　return 0;

}

下面修改CMakeLisets.txt文件

add_executable(turtle_tf_broadcaster  src/turtle_tf_broadcaster.cpp)

target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})