机器人学导论（三）——其他姿态描述转角组合描述与轴角法

2301_80226956

1338人浏览 · 2025-07-08 23:36:41

2301_80226956 · 2025-07-08 23:36:41 发布

九个参数？实际上只有三个独立变量！简化简化简化！！！

我们知道，姿态的描述和旋转变换实际上是一件事。见（二）.Ⅰ

此前，我们描述一次旋转变换都需要输入9个参数，挺麻烦的，那也没有什么方法可以用更少的参数描述姿态呢？

实际上，旋转矩阵的九个参数并非完全独立的，而是受到多个方程约束的，由于基坐标系的正交和单位属性，每个基向量实际上都受到了长度的方向上的约束，三个基向量就是六条约束，因此真正独立的参数实际上只有9-6=3个。

接下来我们引入仅仅用三个参数来描述姿态的方法。

我们其实之前已经接触过了，在解释旋转矩阵的由来时我们干了一件好事——将任意的旋转等效于依次绕三个轴的旋转叠加！！！只要分别描述绕三个轴的旋转角度就能刻画这个旋转变换。

“

”

而叠加的过程实际上就是矩阵相乘，在上一节我们讲到“矩阵的左乘和右乘是不一样的”，对于固定坐标系旋转，操作是从右向左；对于相对坐标系的旋转，操作是从左向右。

因此诞生了两种不同的转角排列设定法——RPY固定角 和 欧拉相对角！

转角排列设定法

RPY固定角/XYZ固定角

绝对系——先绕X再绕Y最后绕Z

RPY角是描述飞机等物体在空中飞行时位姿的一种方法，将坐标系固定在飞机上
1、绕着Z轴转动称为横滚(Roll)
2、绕着Y轴转动称为俯仰(Pitch)
3、绕着X轴转动称为偏航(Yaw)

RPY角属于绕固定坐标系旋转的位姿变换方式。
变换规则:先绕着Xi转γ，再绕着Yi转β，最后绕着Zi转α。转完后将三个旋转矩阵相乘就得到了叠加的旋转矩阵。

抽象出来得到上图。

求逆解（分别绕xyz轴转了多少度）——利用平方与cosβ非负性

利用第一列平方，求得β=atan2，得到β，而β是根下平方式，因此β在-Π/2-Π/2上cosβ>0，因此对于T[1,1]T[2,1]作比，就可以得到α=atan2；对于T[3,2]T[3,3]作比，就可以得到γ=atan2。

为什么cosβ为正这么重要，这是因为由于atan2的值域在-Π-Π，会有两个值，arctan(1,1)=45°和arctan(-1,-1)=-45°。sinαcosβ，cosαcosβ就可能得到同号——同负或同正的情况。

[f,theta] = resolve_f_theta([0 1 0;0 0 -1;-1 0 0])
%用角度生成旋转矩阵
rotz(45)*roty(60)*rotx(30)
%rot永远是正向进行的，就是一般的矩阵乘法
%rpy则是ZYX固定角旋转的顺序，按ZYX输入，但计算的XYZ计算，因此与下式等价
rpy2r(30,60,45)
rpy2tr(30,60,45)
tr2rpy([ans])%将tr矩阵求三个角αβγ
rpy2jac(30,60,45)%转换雅可比

%弧度转角度
atan2(1,2)
atan2d(1,2)

作业：

ZYX相对欧拉角

欧拉角定义（ZYX顺序）

欧拉变换不是“运动”，而是一种“函数变换”

不可以理解为一个旋转过程——“

绕初始物体Z轴旋转 ψ（偏航角 yaw）
绕旋转后物体Y轴旋转 θ（俯仰角 pitch）
绕再次旋转后物体X轴旋转 φ（滚转角 roll）

”
而是理解为一个输入ψ θ φ后，按照ZYX顺序合成生成唯一的位姿Q=f(x(ψ),y(θ),z(φ))的函数！！！

比如说，现在的位姿Q是"X:40 Y:0 Z:0"，接下来的描述是按照Y轴转30度，那么不可以理解为直接在Q上转30度，因为这是相对坐标系，不能用绝对坐标系的XYZ描述。而是要先转换为绝对坐标系再按照ZYX的顺序依次合成。

相对系——先绕Z再绕Y最后绕X

ZYX 欧拉角表示的是围绕物体自身坐标系（body-fixed frame）的连续三次旋转的合成：

绕初始物体Z轴旋转 ψ（偏航角 yaw）
绕旋转后物体Y轴旋转 θ（俯仰角 pitch）
绕再次旋转后物体X轴旋转 φ（滚转角 roll）

每次旋转都是相对于前一次旋转后的物体坐标系进行的。

世界坐标系和相对坐标系的关系——逐步推导

轴角变换——变轴不变角，只是轴无法之间表达，因此将转轴用世界坐标系表达！

他们的转角θ是一致的，只是转轴无法直接表达，而是要用世界坐标系合成！！

而计算都是用世界坐标系计算的，因此对欧拉角的计算实际上是先转化为世界坐标系再计算的。

设初始物体坐标系与世界坐标系重合。

第一步：绕初始 Z 轴旋转 Rz(ψ)

这个旋转改变了物体的姿态。
从世界坐标系来看，这相当于物体朝某个方向偏转了 ψ 角度。
此时物体坐标系变为一个新的坐标系，记为 B1。
对应的旋转矩阵为：
R1=Rz(ψ)

第二步：绕新的 Y 轴旋转 Ry(θ)

现在要绕的是旋转后的 Y 轴（也就是 B1 坐标系的 Y 轴）。
由于这是物体坐标系下的旋转，从世界坐标系的角度看，这一操作等价于：

R2=Rz(ψ)⋅Ry(θ)⋅Rz−1(ψ)

这是一个共轭变换（conjugation），表示“先回到原点，绕原始 Y 轴旋转，再变回去”。
所以整体旋转变成：

R1+2=R1⋅R2=Rz(ψ)⋅(Rz(ψ)⋅Ry(θ)⋅Rz−1(ψ))=Rz(ψ)⋅Ry(θ)⋅Rz−1(ψ)⋅Rz(ψ)

简化后得到：

R1+2=Rz(ψ)⋅Ry(θ)

第三步：绕最新的 X 轴旋转 Rx(φ)

此时物体已经经过两次旋转，现在要绕的是当前物体坐标系的 X 轴。
同样地，从世界坐标系来看，这个旋转等价于：

R3=(Rz(ψ)Ry(θ))⋅Rx(ϕ)⋅(Rz(ψ)Ry(θ))−1
总体旋转为：

Rtotal=R1+2⋅R3=Rz(ψ)Ry(θ)⋅[(Rz(ψ)Ry(θ))⋅Rx(ϕ)⋅(Rz(ψ)Ry(θ))−1]
化简后：

Rtotal=Rz(ψ)Ry(θ)Rx(ϕ)

四、结论

所以，尽管每一步旋转都是在物体坐标系下进行的，但最终的总旋转矩阵却可以写成在世界坐标系下按相反顺序施加的旋转组合：

R=Rz(ψ)Ry(θ)Rx(ϕ)

这是因为：

每次在物体坐标系下的旋转，等价于在世界坐标系中对该旋转进行“共轭”处理（即先回到世界坐标系旋转，再还原回去）；
多次这样的操作叠加起来，就相当于在世界坐标系中按相反顺序施加这些旋转。

总结：

XYZ ZYX，转角的命名顺序就是旋转顺序，只是由于有的是相对系旋转，有的是绝对系旋转，因此计算的矩阵顺序不一样！

在绝对坐标系下书写旋转是从前往后，而计算矩阵永远是从后往前，计算的对象永远是绝对坐标系（世界坐标系），因此相对坐标系必须通过“共轭法”转换到世界坐标系才能参与运算。

对于世界坐标系，先转的轴写后面，以此计算。

共轭：相对坐标系的旋转之所以无法用世界坐标系表达，就是其轴不同，因此共轭就是构建了原轴和相对轴的关系，先转回去，再转θ角，再转回来。因为其θ角都是一致的，只是轴需要被表达而已！！！

R=Rz(ψ)Ry(θ)Rx(ϕ)

那么我们就可以得到——“三次绕固定轴旋转的姿态和以相反顺序绕相对轴旋转的姿态是一致的”

因此，对于欧拉角的求解（求旋转矩阵/由旋转矩阵求角度），可以沿用RPY角的公式！

轴角法——旋转关系的逆解（用矩阵判断绕谁旋转）

上文提到的转交排列法依然是围绕着坐标轴进行旋转的，还有没有更简便的表达方式？
有的！就是轴角法，它更一般化，是围绕着任意一根轴旋转θ角得到的变换。

我们通过K这个单位转轴和θ这个转角，就能达到和旋转矩阵一样的效果，而且仅仅需要两个参数！
$_{B}^{A}\textrm{R}(\hat{K},\theta)$ 表示，沿着K这个单位方向向量转θ度，方向是右手。

其原理可以解释为，转角排列法是轴角法在三个正交方向的分解，而轴角法是转角排列法三个正交方向旋转的合成，具体的推导是用罗德里克斯公式完成的，稍后再证。

我们先给出轴角法对应的旋转矩阵，沿着f向量转θ度。

。

这两张图摆在一起，就很好地说明了“转角排列法是轴角法在三个正交方向的分解，而轴角法是转角排列法三个正交方向旋转的合成”这个概念。

他有什么用呢？它沟通了 旋转矩阵 与 转轴、转角 之间的关系——这就为我们求逆解提供了前提。
也就是说，我们可以通过九个参数的旋转矩阵，求得其是围绕哪个轴，旋转了多少度？也可以通过转角和转轴，分解到三个坐标轴应该旋转多少度。

求逆解（绕哪根轴转了多少度）

比如下面给出了旋转矩阵，和对应的轴角式，应该如何求得转轴f和转角θ呢？

（1.13）推导细节：

$n_x + o_x + a_z =$
$f_x(1 - \cos\theta) + \cos\theta + f_y^2(1 - \cos\theta) + \cos\theta + f_z^2(1 - \cos\theta) + \cos\theta=$
$= f_x^2 + f_y^2 + f_z^2 - (f_x^2 + f_y^2 + f_z^2)\cos\theta + 3\cos\theta$

由 $f_x^2+f_y^2+f_z^2=1$ 得到：
$= 1 - \cos\theta + 3\cos\theta = 1 + 2\cos\theta$

$\cos\theta = \frac{n_x + o_y + a_z - 1}{2}$

（1.14）推导：为了凑 $f_x^2+f_y^2+f_z^2=1$ ，对三个式子平方后相加，得到（1.15）.
移项得到（1.16）sinθ的表达式。

（1.17）推导：联立cosθ sinθ的表达式，得到tanθ的表达式。通过arctanθ反解出θ值。

接着带入（1.14）式，反解fx,fy,fz。

非对角线元素，ft，缺谁谁是t，上下上。

注意：计算等效转轴和转角时,往往存在多解,即等效转轴和转角不唯一,一般使θ在0~π之间。
并且当0很小或者取某些特殊值时，转轴很难确定。（fi趋于无限了）

由此，我们便从旋转矩阵计算出了其合成后对应的旋转轴和旋转角啦！

你可能会疑惑，哎为什么不直接用arcsinθ 或 arccosθ 计算θ呢？——这与机器人的关节定义域有关：

在Matlab实现轴角矩阵转化为旋转矩阵：

即输入旋转轴和旋转角，输出分解在坐标轴上的旋转矩阵：

---R = angvec2r(theta,V)
---T = angvec2tr(theta,V)

其中2tr指齐次变换。

function [f,theta] = resolve_f_theta(T)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
    %赋值 方便计算
    nx = T(1,1); ox = T(1,2); ax = T(1,3); 
    ny = T(2,1); oy = T(2,2); ay = T(2,3);
    nz = T(3,1); oz = T(3,2); az = T(3,3);

    theta = atan2(sqrt((oz-ay)^2+(az-nz)^2+(ny-ox)^2),(nx+oy+az-1));
    if(theta==0 || theta ==180)
        fpeintf("error!!");
        f = "error";
    else
        fx = oz-ay/(2*sin(theta));
        fy = ax-nz/(2*sin(theta));
        fz = ny-ox/(2*sin(theta));
        
        f = [fx fy fz]';
    end
end

罗德里格斯公式的推导

我们可以得到

四元数表示旋转与罗德里克斯公式

四元数代表的一种变换。四元数的本质还是计算轴和角，通过双乘法得到旋转后的向量。
前面的轴角法是通过几何手段，通过罗德里格斯公式得到旋转后的向量。
二者的变换公式是一样的，因此构建出四元数和旋转矩阵的关系，代数桥梁就是欧拉参数，是旋转矩阵在四元数的映射。
因此用旋转矩阵表示，用四元数计算，中间通过欧拉参数变换就是很常见的方法。

书上啪唧就甩了四个参数上来，这是很不负责的。

理解四元数的关键在于两个部分，一个在于他的几何直观——四维超球面在三维空间的投影；另一个是他从罗德里克斯结论的代数推理。

几何意义这里推荐两个视频，我承认我太菜讲不好（不看不影响）：

首先要建立代数直观，四元数其实就是轴角法！
轴角法需要两个参数K和θ，而K实际上是方向向量，可以用三个基向量表达，而轴角法也是四个参数。接下来我们来看四元数和轴角法的关系。

四元数的标准形式是

$q=w+x\vec{i}+y\vec{j}+z\vec{k}$ ----②
或者合成一下
$q=w+\vec{v}$ ----①

他对应着我们最后需要的式子：
$q=cos\frac{\theta}{2}+sin\frac{\theta}{2}\vec{u}$ ----①
其中u向量是任意的，因此可以分解在三个基向量上：
$q=cos\frac{\theta}{2}+sin\frac{\theta}{2}\vec{i}+sin\frac{\theta}{2}\vec{j}+sin\frac{\theta}{2}\vec{k}$ ----②

为了计算方便，我们采用第①种表示方式。

四元数显然包含了实数和虚数（向量）两个部分，因此四元数乘法可以表示为
q1q2=（数量部分，向量部分）

其中，
数量部分 (w1w2−v1⋅v2)：会产生 缩放效应（实部与实部、虚部与虚部的内积）。
向量部分 (w1v2+w2v1+v1×v2)：可以产生 旋转效应（虚部之间的外积）。

因此我们就可以用四元数的乘法表示三维空间的旋转啦！

停停停，但是，他也产生了我们不期望的缩放效应，怎么办呢？

于是，我们引入了双乘法—— $\vec{q}\vec{v}\vec{q}^{-1}$ ，

通过乘以q逆反向抵消拉伸分量。

什么意思呢？

我们用左右手定则来理解！（两手张开掌心向上并在一起，然后四指往自己方向握）：

左右手分别表示 ${q}$ 和 $q^{-1}$ 两种变化，四指代表旋转变化，左右手大拇指代表拉伸变化，
这时拉伸变化正好抵消了!

但是，这又出现了新的问题，如果左右手叠加在一起，那么四指的旋转分量相当于叠加了。

因此为了保证旋转角度不变，左右手的角度变为了θ/2，这样左右手的旋转分量加起来正好是θ了。这一步也叫半角参数化。

最后展开双乘法：

令 $q=cos\frac{\theta}{2}+sin\frac{\theta}{2}\vec{k}$ ${q}^{-1}=cos\frac{\theta}{2}-sin\frac{\theta}{2}\vec{k}$

$\vec{v}' = q \vec{v} q^{-1}$
$= \left( \cos\frac{\theta}{2} + \mathbf{k} \sin\frac{\theta}{2} \right) \vec{v} \left( \cos\frac{\theta}{2} - \mathbf{k} \sin\frac{\theta}{2} \right)$
$= \vec{v} \cos^2\frac{\theta}{2} + (\mathbf{k} \times \vec{v}) \sin\theta + \mathbf{k} (\mathbf{k} \cdot \vec{v}) (1 - \cos\theta)$

这个 $\vec{v'}$ 就是就是 $\vec{v}$ 经过旋转后的结果了。

这个与 罗德里格斯旋转公式 完全一致！

细心的你也一定发现了，我在推导罗德里格斯公式最后标出的三个对应分量恰好也是平行分量的补充，垂直分量的旋转，以及缩放项。对应着四元数的乘法！！！

总结一下，双乘法完成了一件什么事呢？

第一次乘法 qv：
- 旋转 θ/2 + 拉伸平行分量。
第二次乘法 (qv)q−1：
- 再旋转 θ/2（同方向叠加为 θ）。
- 反向拉伸（抵消第一次的缩放）。

欧拉参数

前面通过展开双乘法，我们得到了四元数q

$q=cos\frac{\theta}{2}+sin\frac{\theta}{2}\vec{i}+sin\frac{\theta}{2}\vec{j}+sin\frac{\theta}{2}\vec{k}$ ----②

其4个项就对应了四个欧拉参数：
$\epsilon_1=k_xsin\frac{\theta}{2}$
$\epsilon_2=k_ysin\frac{\theta}{2}$
$\epsilon_3=k_zsin\frac{\theta}{2}$
$\epsilon_4=cos\frac{\theta}{2}$

显然存在约束 $\epsilon_1^2+\epsilon_2^2+\epsilon_3^2+\epsilon_4^2=1$
从几何上理解可以是四维单位超球面的一点。

如何用四元数计算变换：欧拉参数与旋转矩阵的关系

我们常用旋转矩阵表达变换，而用四元数计算变换，因此探究旋转矩阵和欧拉参数的关系就至关重要了（原因往下就是）。

这是轴角法得到的旋转矩阵：

带入四个欧拉参数，并用升角降次公式 $cos^2\theta=\frac{1+cos2\theta}{2}$ ，将半角升到全角，结合约束方程计算

计算方法：因此，当我们得到一个旋转矩阵后（参数是r11r12这种），就可以直接用上式转换为欧拉参数，用四元数的“双乘法”计算旋转后的向量啦！！！

四元数通过双乘法沟通了罗德里格斯公式，而罗德里格斯公式是轴角法的推导，因此我们建立了四元数和轴角法的关系，也就是上面的矩阵，通过欧拉参数（包含轴角）代替了矩阵。

四元数？为什么？

那么这时我们就建立了轴角法与四元数法描述旋转的方式啦！
你可能又会问，这有什么用吗？

这就不得不提到四元数的两大优点—— 避免万向锁计算高效！

避免万向锁

欧拉角是相对坐标系的旋转，但计算时XYZ实际上应该是绝对坐标系的XYZ转动量！！！

无伤理解欧拉角中的“万向死锁”现象_哔哩哔哩_bilibili
这个视频没讲清楚万向锁的根源，本质就是前面提到的相对系旋转和绝对系旋转，XYZ表示的是绝对坐标系的旋转，不能按照围绕转动后的轴旋转来定义XYZ的值。但是图片很方便理解，引用了。

当绕y轴转90°时，由于xz轴重合了，于是围绕这两个轴的旋转效果就一模一样了，也就是丢失了一个自由度。

计算高效

在我们之前的代码中，无论如何旋转矩阵也有9个参数，需要9个浮点数表达。
而四元数则仅需要4个浮点数表达，大大减少了存储空间和运算效率：

这对于需要实时反馈的关节是极其高效的！

而在实际应用中，旋转矩阵和四元数我们都会用到
我们通常 用旋转矩阵表示三维旋转，但 用四元数进行计算。因为你显然发现四元数过于抽象，旋转矩阵更适合描述，而四元数更适合计算。

补充

atan和atan2的区别：

atan(y/x)：
- 输入：一个比值（y/x）。
- 输出：角度（弧度），范围仅限 [-π/2, π/2]（即 -90° 到 90°）。
- 问题：无法区分方向（例如 atan(1/1) 和 atan(-1/-1) 结果相同）。
atan2(y, x)：
- 输入：两个坐标值（y 和 x）。
- 输出：角度（弧度），范围覆盖 [-π, π]（即 -180° 到 180°）。
- 优势：通过 x 和 y 的符号确定象限，能区分所有方向。

笔记草稿

旋转和拉伸分开看，
四元数包含实数和虚数部分，实数与实数运算是点积，虚数是矢量计算叉积，利用叉积就能表示方向的变化
而四元数乘法可以表示为q1q2=(点积部分，叉积部分)
因此对原向量v做四元数乘积，除了叉积完成了旋转的变化，还有点积产生了不应该的拉伸分量。
因此使用双乘法，qvq^｛-1｝，通过乘以q逆反向抵消拉伸分量。
但是如果q和q^｛-1｝的旋转角度都是θ，那就变成两倍旋转角了，因此两边的θ都应该改为θ/2。
就和楼上所说的，左右手定则（两手张开掌心向上并在一起，然后四指往自己方向握）：左右手分别表示q和q^｛-1｝两种变化，左右手大拇指代表拉伸变化，正好抵消了，而四指弯曲代表同一个方向的旋转变化，是叠加的，因此θ要取θ/2。

我们只讲解罗德里克斯是如何推导出四元数各参数的定义的：

补充误区：

欧拉角是相对系旋转，不是“运动”，而是一种“函数变换”，z=f(x(α),y(β),z(γ))，每次赋予αβγ都是最一个完整欧拉角变换的表诉，而转动只是多个欧拉变换逐次发生后的结果。

可以比喻为每进行一次欧拉变换就是一张照片，从0度到90度，就拍了90张照片，将他们逐张摆放就是一个运动的视频了，这就是我们常常把欧拉变换理解为“运动的过程”的原因。

切记，变化是从初始角度开始的，而不是从y轴等于90度这个姿态下再叠加计算的。

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

使用概率图路径规划的机器人路径规划研究Octave（Matlab代码实现）

针对复杂未知环境下传统机器人路径规划算法适应性差、避障稳定性弱、全局搜索效率低的问题，本文开展基于概率图的机器人路径规划方法研究。概率图路径规划依托概率路线图建模思想，通过环境随机采样、节点连通性构建、最优路径检索的核心逻辑，摆脱了传统算法对环境精准建模的依赖，具备强环境适配性与高运算效率。本文系统阐述概率图路径规划的核心理论、运行机制与技术优势，基于Octave仿真平台搭建多场景机器人运动规划仿