机械臂运动控制——三维空间刚体运动描述

white_Learner

发布时间 2021.07.10阅读数 5836 评论数 0

一、旋转矩阵
在机器人运动的过程当中，我们通常会设定一个惯性坐标系（或者叫世界坐标系），姑且认为这个坐标系是固定不动的。例如： $X_w$ $Y_w$ $Y_w$ 是固定不动的世界坐标系， $X_c$ , $Y_c$ , $Z_c$ 是机器人坐标系。存在一个向量 $P$ ，在世界坐标系下的坐标是 $P_w$ ，在移动机器人坐标系下的坐标是 $P_c$ ，通常情况下，我们通过传感器已知移动机器人坐标系统下的坐标 $P_c$ ，来求在世界坐标系下的坐标 $P_w$

二、旋转向量（轴角，Axis-Angle）
旋转向量为一个三维向量，实际上即为李代数。

任意旋转可以通过一个旋转轴和一个旋转角来表示，使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量或轴角（Axis-Angle）。

旋转向量转换到旋转矩阵通过罗德里格斯公式（Rodrigues’s Formula）实现：

$R = \cos \theta I + (1-\cos \theta)^Tnn^T+\sin \theta n^\wedge$

旋转矩阵转换到旋转向量，对于转角 $\theta$

tr(A)=the trace of the matrix A 矩阵A的迹，为主对角线（从左上方至右下方的对角线）上各个元素的总和。

$A = \begin{bmatrix} a& b &c \\ d & e & f\\ g & h & i \end{bmatrix}\\ tr(A) = a+e+i$

$\theta = arccos(\frac{tr(R)-1}{2})$

转轴n，由于旋转轴上的向量在旋转后不发生改变，说明

$Rn=n$

因此，转轴n是矩阵R特征值对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

三、欧拉角
旋转本身就是一个很直观的现象。欧拉角可以提供一种非常直观的方式。他利用3个分离的转角，把一次旋转分解成3次绕不同的轴进行旋转。例如先绕x轴旋转，再绕y轴旋转，最后绕z轴旋转，这样就得到一个xyz轴的旋转。在欧拉角中一个常用的是“航偏-俯仰-翻滚”（yaw-pitch-roll）。可以简单记忆rpy-xyz。其中roll-对应着绕x轴旋转后的翻滚角。Pitch对应着绕y轴旋转后的俯仰值，yawd对应着绕z轴旋转后的航偏值。那么旋转部分就可以通过roll-pitch-yaw这三个量来描述。

在使用欧拉角这种表达方式的时候，会存在万象锁的问题。也就是一旦旋转pitch为90度，就会导致第一次旋转和第三次转换等价，丢失了一个表示维度。万象锁现象如下图所示

3.1 欧拉角转旋转矩阵

3.2 欧拉角转四元数

$q=\begin{bmatrix}{w} \\ {x} \\ {y} \\ {z}\end{bmatrix} =\begin{bmatrix} \cos (\text {roll} / 2) \cos (\text {pitch} / 2) \cos (yaw / 2)+\sin (\text {roll} / 2) \sin (\text {pitch} / 2) \sin (yaw / 2)\\ \sin (\text {roll} / 2) \cos (\text {pitch} / 2) \cos (yaw / 2)-\cos (\text {roll} / 2) \sin (\text {pitch} / 2) \sin (yaw / 2)\\ \cos (\text {roll} / 2) \sin (\text {pitch} / 2) \cos (yaw / 2)+\sin (\text {roll} / 2) \cos (\text {pitch} / 2) \sin (yaw / 2)\\ \cos (\text {roll} / 2) \cos (\text {pitch} / 2) \sin (yaw / 2)-\sin (\text {roll} / 2) \sin (\text {pitch} / 2) \cos (yaw / 2) \end{bmatrix}$

四、四元数

旋转矩阵用9个量来描述3自由度的旋转，具有冗余性；欧拉角虽然用3个量来描述3自由度的旋转，但是具有万向锁的问题，因此我们选择用四元数，（ROS当中描述转向的都是采用的四元数）。一个四元数拥有一个实部和三个虚部组成。

三个虚部满足以下关系

$\left\{\begin{array}{l}{i^{2}=j^{2}=k^{2}=-1} \\ {i j=k, j i=-k} \\ {j k=i, k j=-i} \\ {k i=j, j k=-j}\end{array}\right.$

补充：四元数动画演示

 五、不同运动描述转换的程序实现

 5.1 C++
旋转矩阵
//旋转矩阵初始化
Eigen::Matrix3d R=Matrix3d::Identity();
 
//input: wxyz
Eigen::Matrix3d quat2rotm(double w, double x, double y, double z)
{
    Eigen::Quaterniond q;
    q.w() = w;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();
    //Eigen::Matrix3d R = q.normalized().matrix();//另一种方式
    return R;
}
 
Eigen::Matrix3d eul2rotm(double yaw, double pitch, double roll)
{
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    Eigen::Matrix3d R = q.matrix();
    return R;
}
 
Eigen::Matrix3d angle2rotm(Eigen::AngleAxisd angle)
{
    return angle.toRotationMatrix();
    //return angle.matrix();//另一种方式
}
旋转向量
//使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴
Eigen::AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1)); //以（0,0,1）为旋转轴，旋转45度
 
Eigen::AngleAxisd eul2angle(double yaw, double pitch, double roll)
{
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd angle;
    angle = yawAngle * pitchAngle * rollAngle;
    return angle;
}
 
Eigen::AngleAxisd eul2angle(Eigen::Quaterniond q)
{
    // Eigen::AngleAxisd angel;
    // angel = q;
    
    Eigen::AngleAxisd angel(q);
    return angel;
}
 
Eigen::AngleAxisd rotm2angle(Eigen::Matrix3d rotm)
{
    //Eigen::AngleAxisd angel(rotm);
    
    // Eigen::AngleAxisd angel;
    // angel = rotm;
    
    Eigen::AngleAxisd angel;
    return angel.fromRotationMatrix(rotm);
    
}
欧拉角
// input: wxyz
// outpuy: zyx
Eigen::Vector3d quat2eul(double w, double x, double y, double z)
{
    Eigen::Quaterniond q;
    q.w() = w;
    q.x() = x;
    q.y() = y;
    q.z() = z;
    Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
    return euler;
}
 
Eigen::Vector3d rotm2eul(Eigen::Matrix3d R)
{
    Eigen::Matrix3d m;
    m = R;
    Eigen::Vector3d euler = m.eulerAngles(2, 1, 0);
    return euler;
}
 
Eigen::Vector3d angle2eul(Eigen::AngleAxisd angle)
{
    return angle.toRotationMatrix().eulerAngles(2, 1, 0);
}
四元数
//对四元数赋值的三大种方法（注意Eigen库中的四元数前三维是虚部,最后一维是实部，即x,y,z,w）
//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2),
                      0 * sin((M_PI / 4) / 2), 
                      0 * sin((M_PI / 4) / 2), 
                      1 * sin((M_PI / 4) / 2));//以（0,0,1）为旋转轴，旋转45度
//第一种输出四元数的方式
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
 
 //第二种输出四元数的方式
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;
 
//input: zyx
Eigen::Quaterniond eul2quat(double yaw, double pitch, double roll)
{
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    return q;
}
 
Eigen::Quaterniond rotm2quat(Eigen::Matrix3d R)
{
    // Eigen::Quaterniond q;
    // q = R;
    
    Eigen::Quaterniond q = Eigen::Quaterniond(R);
    q.normalize();
    return q;
}
 
Eigen::Quaterniond angle2quat(Eigen::AngleAxisd angle)
{
    // Eigen::Quaterniond q;
    // q = angle;
    
    return Eigen::Quaterniond(angle);
}
变换矩阵
// 通过旋转矩阵和平移矩阵初始化
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵
T.rotate ( rotation_vector ); // 可以使用四元数，旋转矩阵和旋转向量
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
 
// 获取矩阵形式
cout << "Transform matrix = \n" << T.matrix() << endl;// 获取矩阵形式
cout << "rotation = \n" << T.rotation() << endl;// 获取旋转矩阵
cout << "translation = \n" << T.translation() << endl;// 获取平移矩阵
5.2 Matlab欧拉角转换到四元数
quat = eul2quat(eul)
quat = eul2quat(eul,sequence)
sequence参数如下，默认为'ZYX'

'ZYX' (default) – The order of rotation angles is z-axis, y-axis, x-axis.

'ZYZ' – The order of rotation angles is z-axis, y-axis, z-axis.

'XYZ' – The order of rotation angles is x-axis, y-axis, z-axis.

注意：quat输出为 [w x y z]

四元数转换到欧拉角
eul = quat2eul(quat)
eul = quat2eul(quat,sequence)
sequence参数如下，默认为'ZYX'

'ZYX' (default) – The order of rotation angles is z-axis, y-axis, x-axis.

'ZYZ' – The order of rotation angles is z-axis, y-axis, z-axis.

'XYZ' – The order of rotation angles is x-axis, y-axis, z-axis.

旋转矩阵转换到四元数
quat = rotm2quat(rotm)
四元数转换到旋转矩阵
rotm = quat2rotm(quat)
欧拉角转换到旋转矩阵
rotm = eul2rotm(eul)
rotm = eul2rotm(eul,sequence)
sequence参数如下，默认为'ZYX'

'ZYX' (default) – The order of rotation angles is z-axis, y-axis, x-axis.

'ZYZ' – The order of rotation angles is z-axis, y-axis, z-axis.

'XYZ' – The order of rotation angles is x-axis, y-axis, z-axis.

旋转矩阵转换到欧拉角
eul = rotm2eul(rotm)
eul = rotm2eul(rotm,sequence)
sequence参数如下，默认为'ZYX'

'ZYX' (default) – The order of rotation angles is z-axis, y-axis, x-axis.

'ZYZ' – The order of rotation angles is z-axis, y-axis, z-axis.

'XYZ' – The order of rotation angles is x-axis, y-axis, z-axis.

5.3 python
from scipy.spatial.transform import Rotation as R
参考
Eigen库使用教程之旋转矩阵，旋转向量和四元数的初始化和相互转换的实现
Rotation Matrix To Euler Angles
四元数与欧拉角（RPY角）的相互转换
四元数、欧拉角、旋转矩阵之间互相转换C++源码
中国大学MOOC———《机器人操作系统入门》
三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换
scipy.spatial.transform.Rotatio
Rodrigues into Eulerangles and vice versa
三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换
Eigen学习与使用笔记4
《视觉SLAM十四讲》