线性代数&群论应用：正逆运动学 & 变换矩阵

战术摸鱼大师

已于 2024-12-31 17:26:10 修改

阅读量2.3k

点赞数 23

分类专栏：嵌入式轨迹规划文章标签：线性代数矩阵算法

于 2024-10-28 16:53:13 首次发布

本文链接：https://blog.youkuaiyun.com/qq_45983373/article/details/142179497

版权

嵌入式同时被 2 个专栏收录

14 篇文章

订阅专栏

轨迹规划

8 篇文章

订阅专栏

在机器人学中，为了描述机器人的运动，将机器人建模为正运动学和逆运动学

正运动学：从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态，该问题通常是一个几何问题，给定一组关节角度，计算末端坐标系相对于基坐标系的位置和姿态。
逆运动学：从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合，是实现机器人控制的一个基本问题。通常因为正运动学方程是非线性的，因此逆运动学问题较为困难，很难得到封闭解，甚至无解。而正运动学的像空间就形成了逆运动学的有解空间，称为机器人的工作空间。

运动学变换不止在机械臂中有使用，在图形学中也有使用，图形学中通过旋转矩阵，平移矩阵，投影矩阵，将3D的mesh投影到2D的显示器区域中。

这些变换背后的数学原理都是线性代数和群论。

变换的表示方式

变换的表示分为位置和姿态两部分，各三个自由度，
位置变换没有异议，都是用平移向量 $\begin{bmatrix} x \\ y \\ z \end{bmatrix}$ 表示。
旋转分为：旋转矩阵，欧拉角，四元数三种表达方式

变换矩阵

旋转矩阵

旋转矩阵的思想是：想要表达姿态，可以用坐标系的三个轴的单位矢量来表示即 $\begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}\begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} xe_1 + ye_2 + ze_3 \end{bmatrix}$ ，表示旋转就是对这个向量A做变换得到一个新的向量B, $R A = B$ ，求旋转向量可以从坐标系入手，原坐标系中的XYZ轴，经过旋转得到新的XYZ轴，基向量 $\begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}$ 乘以旋转矩阵R变成了 $\begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix}$ ：
$R\begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}= \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix}$
因为基向量之间是正交的且自身又是单位矩向量，所以：
$\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix} = I$
所以可以两边同时乘以 $\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix}$ ,得到R：
$\begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix}\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix}$
R就是两个坐标系的内积，如果只看旋转，即从A坐标系旋转到B坐标系，坐标系原点位置不变。则这种计算方式可以理解为新坐标系B的各轴在原坐标系A的投影，又称为方向余弦矩阵：
$}_{B}^{A} R=\left[\begin{array}{llll} { }^{A} x_{B} & { }^{A} y_{B} & { }^{A} z_{B} \end{array}\right]=\left[\begin{array}{lll} \cos \left(\angle X_{B} X_{A}\right) & \cos \left(\angle Y_{B} X_{A}\right) & \cos \left(\angle Z_{B} X_{A}\right) \\ \cos \left(\angle X_{B} Y_{A}\right) & \cos \left(\angle Y_{B} Y_{A}\right) & \cos \left(\angle Z_{B} Y_{A}\right) \\ \cos \left(\angle X_{B} Z_{A}\right) & \cos \left(\angle Y_{B} Z_{A}\right) & \cos \left(\angle Z_{B} Z_{A}\right) \end{array}\right]$
当只有一个轴旋转时，例如绕Z轴旋转 $\alpha$ 度时，则：
$\begin{bmatrix} \cos \alpha & -\sin\alpha & 0\\ \sin \alpha & -\cos \alpha & 0\\ 0 & 0 & 1 \end{bmatrix}$
平移变换时，平移矩阵T为：
$T(t_x,t_y) = \begin{bmatrix} 1 & 0 & 0 & t_x \\ 0 & 1 & 0 &t_y \\ 0 & 0 & 1 & t_z \end{bmatrix} \\ A+Y=B$ 正好空出来个3*3的位置，为了统一起来，可以扩展一个dimension，让坐标系变为 $[x,y,z,\omega]$ ，其中 $\omega$ 表示放缩，一般都设为1，表示不放缩。这样就可以得到一个4*4的变换矩阵了，前三列表示旋转，最后一列表示平移，称为齐次坐标系变换：
$\begin{bmatrix} x'\\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} a & b & c & t_x\\ d & e & f & t_y\\ g & h & i & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$

在这里插入图片描述

投影矩阵View变换

机器人学里面不会用到，但是在计算机图形学和计算机视觉中会用到，3D的模型要投影到2D中,又被称为视图变换，投影的方式有两种，正交变换和透视变换：
在这里插入图片描述

其中透视变换就是相机的工作原理：
在这里插入图片描述

这张图是视觉SLAM十四讲里面的图，我感觉那个光心平面应该是镜头而不是“相机”，后面的成像平面就是CMOS感光元件，实际相机里面的CMOS也确实是倒置的，而且左右也是反的。

这里面有几个坐标系，可能CV跟CG的符号不太一样，但是具体原理都是一样的：

像素坐标系：其实就是CMOS坐标系，为了区别其他坐标系，把这个坐标系称为 $u - v$ 坐标系：左上角是原点，往右是 $u$ 正方向，往下是 $v$ 正方向
相机坐标系：以相机为原点的局部坐标系
世界坐标系：全局坐标系，原点，方向，由用户自行参考决定

相机成像的过程如下：

CG里面是分为了放缩和正交投影两步，因为CG里面需要考虑整个视场内的所有物体，是自己计算出来的。而相机成像只关注三个面：物平面，像平面，像素平面

将物面坐标归一化到成像平面 $\left\{\begin{matrix} x'=\frac{f}{z}y \\ \\ y'=\frac{f}{z}x \end{matrix}\right.$ ，再将成像平面坐标系(图像物理坐标系)变换到像素坐标系 $\left\{\begin{array}{l} u=\alpha x^{\prime}+c_{x} \\ v=\beta y^{\prime}+c_{y} \end{array}\right.$ ，放缩比例计算方法为CMOS像素点个数/CMOS实际物理尺寸，单位为 $p i x e l / m$ ，平移量为CMOS像素点的一半，单位为 $p i x e l$ ，将两个联立，得到如下：
$z\begin{pmatrix}u\\\\v\\\\1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\\[0.3em]0&f_y&c_y\\[0.3em]0&0&1\end{pmatrix}\begin{pmatrix}x\\[0.3em]y\\[0.3em]z\end{pmatrix}\triangleq\boldsymbol K\boldsymbol P.$
这个变换矩阵定义为相机的内参矩阵K，与相机本身结构相关，出厂就确定了，一般是厂家给，也可以自己标定得到。常见标定方法：张正友标定。
进行一般化，将相机坐标和物体坐标经过旋转 $R$ 和平移 $t$ ，转化为世界坐标系：
$z\boldsymbol{P}_{uv}=z\left[\begin{array}{c}u\\\\v\\\\1\end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R}\boldsymbol{P}_w+\boldsymbol{t}\right)=\boldsymbol{K}\boldsymbol{T}\boldsymbol{P}_w.$
其中的变换矩阵T是相机的外参，随着相机的位置变动而变动，将相机固定在机器人上，通过估计相机的外参就可以估计出机器人的运动

相机由于镜头做工和装配工艺，并不完全是线性关系，还存在畸变，分为径向畸变和纵向畸变
桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。
在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。

欧拉角

姿态可以用围绕XYZ轴旋转的姿态角 $\begin{bmatrix} roll \\ pitch \\ yaw \end{bmatrix}$ 来表示。

注意，这里的位置变换和欧拉角都是相对于局部坐标系 $x yz$ 自身三轴的转动，不是相对于全局坐标系 $X Y Z$ 的变换。
相对于世界坐标系的旋转被称为“静态欧拉角”，但是一般是很难得知世界坐标系信息的，更常用的还是绕自身坐标轴旋转

在这里插入图片描述

该图是左手坐标系，X叉乘Y需要用左手定则才能得到Z，右手坐标系中是使用右手定则。

优点：直观，易理解，没有限制，任何三个 $[0 ， 360]$ 的数都是欧拉角
缺点：

欧拉角是讲旋分解为绕xyz轴的旋转，这种旋转在部分角度会有多解，所以欧拉角会出现别名问题。当两个轴重合时会出现万向节死锁问题
- 三维空间中，旋转顺序的不同结果是不一样的，先x再y再z的顺序和先z再y再x的顺序结果是不一样的，所以欧拉角是按照顺序来转动的，可以分为rpy(ZYX)顺序以及XYZ等欧拉角顺序。
- 无论哪种都会出现万向节死锁问题，以ZYX为例，如果第二个轴的旋转角为90°倍数，那第二次旋转过后的X轴的绝对方向跟第一次旋转的时候的Z轴的绝对方向是重合的，这样就丢失了一个自由度，第三次旋转即yaw的旋转，跟第一次旋转即roll的旋转是一样的效果。即：动态欧拉角的旋转： $[20°, 90°, 30°]$ 等同于静态欧拉角中的 $[50°, 90°, 0°]$ ，且无论怎么改动，绕世界坐标系Z轴的旋转始终为0。
- 静态欧拉角没有万向节死锁问题
在欧拉角表示下，想要表示一个旋转，是没办法进行线性插值的，无论是机械臂旋转，还是图形学中物体旋转，都需要进行插值来将旋转这个动作细化为过程加以控制。
- 90度和275度实际位置只差5度，但是插补就会绕个大圈

四元数

四元数可以解决上述问题，四元数的思想为：不分解为三轴的旋转，而是表达出旋转轴和旋转角度.假如一个四元数 $(w, x, y, z)$ ，旋转轴为 $(x, y, z)$ ，右手法则旋转角度为 $w$ .四元数的后三分别表示三个虚轴 $ijk$ ，所以四元数又可以写成一个标量和一个向量的组合： $s,v]^T$ ，后三个虚轴关系满足: $i^2=j^2=k^2=ijk=-1$
四元数运算也和普通的向量不大一样：

加减法：与普通向量一致。
乘法： $q=\begin{bmatrix} p_wq_w-p_xq_x-p_yq_y-p_zq_z \\ p_wq_x+p_xq_w+p_yq_z-p_zq_y \\ p_wq_y-p_xq_z+p_yq_w+p_zq_x \\ p_wq_z+p_xq_y-p_yq_x+p_zq_w \end{bmatrix}$
类似于矩阵乘法,不满足交换律,但满足结合律和分配律：
$q=\begin{bmatrix} p_wq_w-\mathbf p_v^T\mathbf q_v \\ p_w\mathbf q_v+q_w\mathbf p_v+\mathbf p_v\times\mathbf q_v \end{bmatrix}$
共轭：实部不变，对虚部取逆
取模 $\|\mathbf q\|=\sqrt{\mathbf q\otimes\mathbf q^*}=\sqrt{\mathbf q^*\otimes\mathbf q}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2} \\ \|\mathbf p\otimes \mathbf q\|=\|\mathbf q\otimes \mathbf p\|=\|\mathbf p\|\|\mathbf q\|$
求逆： $\mathbf q^{-1}=\frac{\mathbf q^*}{\|\mathbf q\|^2} \\\mathbf q\otimes\mathbf q^{-1}=\mathbf q^{-1}\otimes\mathbf q=\mathbf q_1=\begin{bmatrix} 1 \\ \mathbf 0 \end{bmatrix}$

用四元数表示旋转时，先将三维点 $[x, y, z]$ 写成虚四元数形式 $[0, x, y, z]$ ，经过四元数 $q$ 的变换后，结果为：
$p^{\prime} = qpq^{-1}$
在这里插入图片描述
已知两个姿态，求两个姿态间的旋转时：

已知旋转轴和旋转角度时求四元数：

Summary

变换本身确定时，表达方式可以互相转换：
四元数->旋转矩阵：
$R=\left[\begin{array}{ccc} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{0} q_{2}+q_{1} q_{3}\right) \\ 2\left(q_{0} q_{3}+q_{1} q_{2}\right) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{0} q_{1}+q_{2} q_{3}\right) & q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{array}\right]$
旋转矩阵->四元数的转换关系为：
$\begin{aligned} q_{0} & =\frac{1}{2} \sqrt{1+r_{11}+r_{22}+r_{33}}, \\ {\left[\begin{array}{c} q_{1} \\ q_{2} \\ q_{3} \end{array}\right] } & =\frac{1}{4 q_{0}}\left[\begin{array}{l} r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21}-2_{12} \end{array}\right] \end{aligned}$
欧拉角(ZYX顺序)->四元数：

绕X轴旋转角度： $\alpha$
绕Y轴旋转角度： $\beta$
绕Z轴旋转角度： $\gamma$

$\begin{bmatrix} \cos\frac{\gamma}{2} \\ 0 \\ 0 \\ \sin\frac{\gamma}{2} \end{bmatrix} \begin{bmatrix} \cos\frac{\beta}{2} \\ 0 \\ \sin\frac{\beta}{2} \\ 0 \end{bmatrix} \begin{bmatrix} \cos\frac{\alpha}{2} \\ \sin\frac{\alpha}{2} \\ 0 \\ 0 \end{bmatrix}= \begin{bmatrix} \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2} \\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2} \\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2} \\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2} \end{bmatrix}$
MATLAB代码：

function q = euler2quat(eulerAngles)
    % eulerAngles是一个1x3的向量，依次表示绕Z、Y、X轴的旋转角度（单位：弧度）
    % 获取欧拉角
    gamma = eulerAngles(1); % 绕Z轴旋转角度
    beta = eulerAngles(2); % 绕Y轴旋转角度
    alpha = eulerAngles(3); % 绕X轴旋转角度

    % 计算四元数的各个分量
    cos_half_g = cos(gamma / 2);
    sin_half_g = sin(gamma / 2);
    cos_half_b = cos(beta / 2);
    sin_half_b = sin(beta / 2);
    cos_alpha_a = cos(alpha / 2);
    sin_alpha_a = sin(alpha / 2);

    % 四元数的实部
    q0 = cos_alpha_a * cos_half_b * cos_half_g + sin_alpha_a * sin_half_b * sin_half_g;
    % 四元数的i分量（对应x轴）
    q1 = sin_alpha_a * cos_half_b * cos_half_g - cos_alpha_a * sin_half_b * sin_half_g;
    % 四元数的j分量（对应y轴）
    q2 = cos_alpha_a * sin_half_b * cos_half_g + sin_alpha_a * cos_half_b * sin_half_g;
    % 四元数的k分量（对应z轴）
    q3 = cos_alpha_a * cos_half_b * sin_half_g - sin_alpha_a * sin_half_b * cos_half_g;

    % 组合成四元数向量 [w, x, y, z]，w为实部
    q = [q0, q1, q2, q3];
end

四元数-> 欧拉角(ZYX顺序)：
$\begin{bmatrix} \alpha \\ \beta \\ \gamma \end{bmatrix}= \begin{bmatrix} \arctan\frac{2(q_0q_1+q_2q_3)}{1-2(q_1^2+q_2^2)} \\ \arcsin(2(q_0q_2-q_1q_3)) \\ \arctan\frac{2(q_0q_3+q_1q_2)}{1-2(q_2^2+q_3^2)} \end{bmatrix}$
MATLAB代码：

function eulerAngles = quat2ZYXEular(q)
    % 获取四元数的各个分量，q的格式为[w, x, y, z]，其中w为实部
    w = q(1);
    x = q(2);
    y = q(3);
    z = q(4);
    
    % 计算绕X轴的旋转角度（单位：弧度），对应psi
    sin_psi = 2 * (w * x + y * z);
    cos_psi = w^2 + x^2 - y^2 - z^2;
    psi = atan2(sin_psi, cos_psi);
    
    % 计算绕Y轴的旋转角度（单位：弧度），对应theta
    sin_theta = 2 * (w * y - z * x);
    % 处理奇异情况，避免asin函数输入超出范围
    if abs(sin_theta) >= 1
        theta = sign(sin_theta) * pi / 2;
    else
        theta = asin(sin_theta);
    end
    
    % 计算绕Z轴的旋转角度（单位：弧度），对应phi
    sin_phi = 2 * (w * z + x * y);
    cos_phi = w^2 + x^2 - y^2 - z^2;
    phi = atan2(sin_phi, cos_phi);
    
    % 将计算得到的欧拉角按ZYX顺序组合成向量返回
    eulerAngles = [phi, theta, psi];
end

此外旋转的参考点不同结果也不同：基于固定坐标系的旋转变换左乘旋转矩阵，基于运动坐标系的旋转变换右乘旋转矩阵。

推导参考：旋转矩阵左右乘不同的推导
左乘右乘的区别仅局限于多个旋转时，这里的固定矩阵也不一定是世界坐标系，而是旋转开始时的初始坐标系。运动坐标系指旋转后的坐标系

李群与李代数

群的概念

集合+运算符号。要求满足：

封闭性
结合律
单位元/中性元
逆

感觉有点像线性代数里面的向量空间的mini版，运算符号的要求变弱了

常见群：

一般线性群，可逆矩阵，对矩阵乘法成群。
特殊正交群SO(n)：n维旋转矩阵群，可以用来当作旋转矩阵。
特殊欧式群SE(n)：n维欧式变换群。
n维循环群，只有有限个元素，形成循环，例如 $Z_{2}$ 只有0和1两个元素，运算符号为1，定义1+1=0。

作用

例如研究旋转矩阵，旋转矩阵乘以旋转矩阵还是旋转矩阵，但是加上就不一定了，因此如果用线性代数中向量空间的方法去分析，会有很多运算以及规律都不能用，所以提出了群，研究群的性质。
一个简单的案例，以四元数为例，在控制过程中需要进行插值来得到两个位姿中间每个时刻的位姿，如果使用线性变换就会出问题，效果如下：
在这里插入图片描述

明显可以看出中间走过的弧度宽，两边弧度窄，也就是角速度不是匀速的。
而且由于直接线性插补计算为： $q = q 1 * (1 - t) + q 2 * t$ ，所以计算出来的 $q$ 并不是单位四元数，导致其物体本身也发生了形变，虽然可以通过归一化将其变为单位四元数，但角速度依旧是不稳定的。效果如下：
在这里插入图片描述

所以想要保持角速度稳定，可以在李群空间进行插值

参考这篇文章的讲解

Ref

GAMES101
GAMES202
视觉SLAM十四讲
机器人学导论
Sola, Joan, Jeremie Deray, and Dinesh Atchuthan. “A micro Lie theory for state estimation in robotics.” arXiv preprint arXiv:1812.01537 (2018).