本文介绍了ROS中TF坐标的基本规则,欧拉角和四元数变换,并以一个实际的旋转的例子介绍ROS代码的实现。
对于 ROS 机器人,如果以它为坐标系的原心,那么:
x轴正向:前方
y轴正向:左方
z轴正向:上方
即绕轴旋转的正方向为逆时针旋转,当我们将绕z轴旋转90度时,是绕z轴逆时针旋转90度。
欧拉角使用绕x,y,z三个轴的旋转来表示物体旋转
绕 Z轴 旋转,称之为 航向角,使用yaw表示;
绕 X轴 旋转,称之为 横滚角,使用roll表示;
绕 Y轴 旋转,称之为 俯仰角,使用pitch表示;
四元数是使用一个旋转的向量 + 一个旋转的角度来表示物体旋转。
Q=(x,y,z,w)
四元数的模使用如下公式计算:
用于旋转的四元数,它的模须为1.
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,则表示为绕动轴转动的方式,矩阵依次右乘。
第一章节中给出几个结论, 下面,通过实际的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.141592653589793238462643383279502884
int 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都为0
yaw=PI_CONST/2; pitch=0; roll=0;
std::cout<<"欧拉角rpy("<
运行结果如下:
$ 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
输出的结果跟我们分析的完全一致。