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

最新推荐文章于 2024-09-25 11:29:10 发布

white_Learner

最新推荐文章于 2024-09-25 11:29:10 发布

阅读量8.6k

点赞数 10

分类专栏：机械臂运动控制

本文链接：https://blog.csdn.net/Kalenee/article/details/80792775

版权

机械臂同时被 2 个专栏收录

19 篇文章 76 订阅

订阅专栏

运动控制

14 篇文章 18 订阅

订阅专栏

一、旋转矩阵

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

为了求 P_w ，我们必须知道机器人坐标系 X_c , Y_c , Z_c 相对与世界坐标 X_w , Y_w , Z_w 做了哪些变换。我们定义世界坐标系经过变换矩阵之后得到机器人坐标系（这可以通过计算里程和IMU的数据进行测量出来）（这也就说明了为什么在机器人刚刚启动的时候odom和base_link坐标系必须是重合的，不然没有办法计算旋转矩阵），另外一般情况下，移动机器人运动是一个刚体运动，也就是说机器人的形状和大小不会因为坐标系不同而改变，这种变换叫做欧式变换。一个欧式变换可以由旋转和平移两个部分组成。首先我们考虑旋转问题，假设在世界坐标系下的单位正交基 (e_x,e_y,e_z) ，在移动机器人坐标系下的单位正交基 $(e{_{x}^{''}},e{_{y}^{''}},e{_{z}^{''}})$ ，那么，根据向量的模可知：

$[e{_{x}},e{_{y}},e{_{z}}]\cdot P_{W}^{T} =[e{_{x}^{''}},e{_{y}^{''}},e{_{z}^{''}}]\cdot P_{C}^{T}$

因此， $P_{C}^{T}=[e_x,e_y,e_z]^T\cdot [e_{x}^{''},e_{y}^{''},e_{z}^{''}]\cdot P_{W}^{T}$ ，我们将 $[e_x,e_y,e_z]^T\cdot [e_{x}^{''},e_{y}^{''},e_{z}^{''}]$ 记做旋转矩阵，因此上面的表达式可以简化为 $P_{C}^{T}=R\cdot P_{W}^{T}$ 。接下来是平移部分，假设平移部分是 $P_{C^{''}}^{T}$ 经过平移向量后得到 $P_{C}^{T}$ ，那么可以得到 $P_{C}^{T}=P_{C^{''}}^{T}+t=R\cdot P_{W}^{T}+t$ 。所以通过旋转矩阵和平移向量，我们可以描述从世界坐标系到移动机器人坐标系的坐标变换。但是这种表达方式存在一个问题，对于连续的位置变换，例如机器人坐标系是随着时间在不断变换的，上面这种表达方式并不是一个线性的表达方式，假设我们经历了两次变换 R_1 , t_1 和 R_2 , t_2 且满足：从到 $b$ 的变换 $b=R_1a+t_1$ ，从 $b$ 到 $c$ 的变换 $c=R_2b+t_2$ ，那么从到 $c$ 的变换是 $c=R_2(R_1a+t_1)+t_2$ .并不是我们希望的的形式 $c=R_a+t$ (然后我们采用齐次坐标的方式进行表达，详细的部分参考李群李代数).

二、旋转向量（轴角，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.$

写成矩阵的样子就是 $q=[w,x,y,z]^T$ ，其中 $|q|^2 = w^2=x^2+y^2+z^2 =1$ 。

注意：若 $ａ$ $|q|^{2} \not= 1$ 不能用于表示旋转。

4.1 四元数转欧拉角

$\begin{bmatrix} {roll} \\ {pitch} \\ {yaw} \end{bmatrix}= \begin{bmatrix} {\operatorname{atan} 2\left(2(w x+y z), 1-2\left(x^{2}+y^{2}\right)\right)} \\ {\arcsin (2(w y-z x))} \\ {\operatorname{atan} 2\left(2(w z+x y), 1-2\left(y^{2}+z^{2}\right)\right)} \end{bmatrix}$

4.2 四元数转旋转矩阵

$R(q)=\begin{bmatrix} {1-2 y^{2}-2 z^{2}} & {2 x y-2 z w} & {2 x z+2 y w} \\ {2 x y+2 z w} & {1-2 x^{2}-2 z^{2}} & {2 y z-2 x w} \\ {2 x z-2 y w} & {2 y z+2 x w} & {1-2 x^{2}-2 y^{2}} \end{bmatrix}$

补充：四元数动画演示

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

5.1 C++

描述形式	描述形式	Eigen类型
欧拉角	eulerAngles(3X1)	Eigen::Vector3d
旋转矩阵	rotationMatrix(3X3)	Eigen::Matrix3d
旋转向量	angleAxisd(3X1)	Eigen::AngleAxisd
四元数	quaterniond(4X1)	Eigen::Quaterniond
平移向量	translationMatrix(3X1)	Eigen::Vector3d
变换矩阵	transformMatrix((4X4)	Eigen::Isometry3d

旋转矩阵

//旋转矩阵初始化
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

函数	说明
`R.from_quat(quat[, normalized])`	输入四元数(x,y,z,w)
`R.from_matrix(matrix)`	输入旋转矩阵
`R.from_rotvec(rotvec)`	输入轴角
`R.from_euler(seq, angles[, degrees]))`	输入欧拉角(seq为顺序)
`R.as_quat()`	输出四元数(x,y,z,w)
`R.as_matrix()`	输出旋转矩阵
`R.as_rotvec()`	输出轴角
`R.as_euler(self,seq,degrees=False)`	输出欧拉角(seq为顺序)