机器人学与OROCOS-KDL（三）姿态描述与旋转矩阵

最新推荐文章于 2024-08-13 21:40:25 发布

Jelly_Lee2

最新推荐文章于 2024-08-13 21:40:25 发布

阅读量2.9k

点赞数

分类专栏：机器人文章标签：机器人 OROCOS-KDL

本文链接：https://blog.csdn.net/jelatine/article/details/86104252

版权

机器人专栏收录该内容

5 篇文章 3 订阅

订阅专栏

机器人学与OROCOS-KDL（三）姿态描述与旋转矩阵

一、姿态的数学表示
二、OROCOS-KDL应用

一、姿态的数学表示

为表示空间中的一点，不仅需要点的位置，而且需要点的姿态。为规定空间刚体的姿态，用一直角坐标系 ${B\}$ 与此刚体固接。设 ${B\}$ 中单位主矢量为 $x_B,y_B,z_B$ 相对于参考坐标系 ${A\}$ 的方向余弦组成 $3\times3$ 矩阵 $_B^AR$ 来表示刚体B相对于坐标系 ${A\}$ 的方位。 $_B^AR$ 称为旋转矩阵。
$_B^AR= \begin{bmatrix} ^Ax_B & ^Ay_B & ^Az_B\end{bmatrix}= \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{bmatrix}$

二、OROCOS-KDL应用

1.主要成员变量

Rotation类的主要成员变量为一个double型的数组，数组储存旋转矩阵的九个数值。

/*源码 orocos_kdl/src/frames.hpp*/
public:
    double data[9];

2.创建旋转矩阵

/*实例 orocos_kdl/examples/geometry.cpp*/
//默认构造函数
KDL::Rotation r1;
//从三个单位向量（Vx, Vy, Vz）中创建一个旋转矩阵
KDL::Rotation r2(KDL::Vector(0,0,1),
                 KDL::Vector(0,-1,0),
                 KDL::Vector(-1,0,0));
//从九个值中创建一个旋转矩阵
KDL::Rotation r3(0,0,-1,1,0,0,0,-1,0);
//创建一个单位旋转矩阵
KDL::Rotation r4=KDL::Rotation::Identity();
//创建一个绕X轴旋转的旋转矩阵
KDL::Rotation r5=KDL::Rotation::RotX(M_PI/3);
//创建一个绕Y轴旋转的旋转矩阵
KDL::Rotation r6=KDL::Rotation::RotY(M_PI/3);
//创建一个绕Z轴旋转的旋转矩阵
KDL::Rotation r7=KDL::Rotation::RotZ(M_PI/3);
//创建一个绕任意（非单位）向量旋转的旋转矩阵
KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
//创建一个绕任意（单位）向量旋转的旋转矩阵
KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),M_PI/4);
//从ZYZ欧拉角创建旋转矩阵
KDL::Rotation r10=KDL::Rotation::EulerZYZ(1.,2.,3.);
//从ZYX欧拉角创建旋转矩阵
KDL::Rotation r11=KDL::Rotation::EulerZYX(1.,2.,3.);
//从RPY（Roll-Pitch-Yaw）创建旋转矩阵
KDL::Rotation r12=KDL::Rotation::RPY(1.,2.,3.);

//打印旋转矩阵:
std::cout<<"r1: "<<r1<<std::endl;
std::cout<<"r2: "<<r2<<std::endl;
std::cout<<"r3: "<<r3<<std::endl;
std::cout<<"r4: "<<r4<<std::endl;
std::cout<<"r5: "<<r5<<std::endl;
std::cout<<"r6: "<<r6<<std::endl;
std::cout<<"r7: "<<r7<<std::endl;
std::cout<<"r8: "<<r8<<std::endl;
std::cout<<"r9: "<<r9<<std::endl;
std::cout<<"r10: "<<r10<<std::endl;
std::cout<<"r11: "<<r11<<std::endl;
std::cout<<"r12: "<<r12<<std::endl;

打印结果：
在这里插入图片描述

3.设置和获取旋转矩阵

/*实例 orocos_kdl/examples/geometry.cpp*/
//获取单个元素
std::cout<<"r8(1,2): "<<r8(1,2)<<std::endl;
//获取等效旋转向量
std::cout<<"equiv rot vector of r11: "<<r11.GetRot()<<std::endl;
//获取等效旋转向量和角度
double angle=r10.GetRotAngle(v1);
std::cout<<"equiv rot vector of r10:"<<v1<<"and angle: "<<angle<<std::endl;
//获取ZYZ欧拉角
double alfa,beta,gamma;
r9.GetEulerZYZ(alfa,beta,gamma);
std::cout<<"EulerZYZ: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
//获取ZYZ欧拉角
r9.GetEulerZYX(alfa,beta,gamma);
std::cout<<"EulerZYX: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;
//获取RPY（Roll-Pitch-Yaw）角
r9.GetRPY(alfa,beta,gamma);
std::cout<<"Roll-Pitch-Yaw: "<<alfa<<", "<<beta<<", "<<gamma<<std::endl;