UAV012

您所在的位置:网站首页 数值变化率怎么计算出来的 UAV012

UAV012

2024-07-14 06:28| 来源: 网络整理| 查看: 265

写这篇博客,已经是第三次了,花了一个周,一遍遍修改,只为了理解好姿态解算并表述出来。

之前写过一篇姿态解算的博客,UAV021(四):飞控传感器数据融合与姿态估计,在小角度假设条件(俯仰角、滚转角都很小)下做了近似,并且使用欧拉角的方式来实现姿态解算,模型是很简单的。这一次想去除此假设,并且添加四元数来解算姿态,问题一下子变得复杂起来。

一、基本概念 1.1 坐标系 机体坐标系

机体坐标系(机体系)以机头方向为 x 轴,机体水平向右为 y 轴,垂直机体向下为 z 轴,即前右下,符合右手定则。

地球坐标系

地球坐标系(地球系) x 轴方向朝北,y 轴方向朝东,z 轴方向朝地,也即北东地(NED),同样符合右手定则。

航向坐标系

机体系 x、y 轴在水平面的投影为航向坐标系(航向系)的 x、y轴,竖直向下为 z 轴。

可见,航向系可由地球系绕 z 轴旋转一定角度得到。很多地方都没有航向系这个概念,但是匿名使用了,经过反复思考之后,添加这个坐标系是很利于后面姿态解算的,故说明。

1.2 姿态角

姿态解算就是通过融合传感器数据解算出姿态角。姿态角是俯仰角(pitch)、滚转角(roll)和偏航角(yaw)的合称,此文分别使用 α , β , γ \alpha, \beta, \gamma α,β,γ 表示。

俯仰角 俯仰角是无人机机体系 x 轴与水平面夹角,也即机体系与航向系 x 轴的夹角,机头上仰为正,范围 α ∈ [ − π / 2 , π / 2 ] \alpha \in [-\pi/2, \pi/2] α∈[−π/2,π/2]。

滚转角 滚转角是无人机机体系 y 轴与水平面夹角,也即机体系与航向系 y 轴的夹角,机身左升右降为正,范围 β ∈ [ − π , π ] \beta \in [-\pi, \pi] β∈[−π,π]。

偏航角 偏航角是无人机机体系 x 轴在水平面投影与地球系 x 轴(正北方)的夹角,也即航向系 x 轴与地球系 x 轴夹角。俯视机身,顺时针方向(往东)角度递增,逆时针方向角度递减,范围 γ ∈ [ − π , π ] \gamma \in [-\pi, \pi] γ∈[−π,π]。

可见,引入航向系之后可以更加方便地定义姿态角。注意与下面的欧拉角作对比,欧拉角和姿态角不是同样概念,这也是这里使用 α , β , γ \alpha, \beta, \gamma α,β,γ 而不是更常见的 θ , ϕ , ψ \theta, \phi, \psi θ,ϕ,ψ 的原因,后者用于表示欧拉角。

二、姿态解算 2.1 综述 2.1.1 姿态解算与数据融合

姿态解算是指使用传感器数据解算出姿态角,数据融合更强调各传感器之间数据的取长补短。在无人机中,传感器常用九轴传感器(三轴加速度、三轴陀螺仪、三轴磁力计)。加速度计与陀螺仪融合得到俯仰、滚转角,磁力计与陀螺仪融合得到偏航角。解算有姿态角,常用欧拉角、四元数与旋转矩阵三种方式;融合常用互补滤波与卡尔曼滤波。

比如使用欧拉角姿态解算互补滤波数据融合。如下图所示,其中公式1,公式2,角速度积分等都使用了欧拉角,将传感器数据不断变换到姿态角,最后一个互补滤波融合数据。当然,并不一定都是解算后融合,很多时候两者是交叉的,例如四元数姿态解算互补滤波数据融合的时候。 互补滤波上图在后面会有具体说明,现在不怎么理解也没关系。

2.1.2 欧拉角、四元数、旋转矩阵的联系

无人机中,常提到三种姿态解算的方式:欧拉角、四元素和旋转矩阵。欧拉角最为直观简单,但是有死锁问题;四元数添加一个元素,将三维计算映射到四维又返回三维,比较抽象不易理解,但避免了死锁问题;旋转矩阵有九个元素,求解相对复杂,几乎都不被采用。

在实际运用中,如果无人机没有大机动(俯仰、滚转角小于60°就不算大机动),用欧拉角已经绰绰有余,所以欧拉角还是很实用的。但是想做个空翻的四轴,欧拉角就会有问题了(至少理论上有),需采用四元数。至于旋转矩阵,理论上会说明其概念及其重要性,但不会有最终的测试。

首先来看一个三者之间的关系图,有了整体概念之后再分别研究,不要在学习推导的过程中迷失方向。三者之间是可以相互转换的,后面的内容将一一说明。

欧拉角-四元数-旋转矩阵

2.2 欧拉角 2.2.1 欧拉角的定义

欧拉角描述了两个坐标系之间的转换关系,以机体系和地球系说明问题。机体系是运动的,地球系是固定的。假设初始时两坐标系重合,欧拉角分别是连续绕坐标轴三次旋转的三个角度。但是需要明确三个问题:

绕哪个坐标系的坐标轴先后绕了哪些坐标轴旋转分别绕坐标轴旋转了多少度

只有这三个问题都说清楚了,我们才能够确定机体系相对于地球系的状态。

对于问题一:可以绕机体系的坐标轴,称为内旋;也可以绕地球系的坐标轴,称为外旋。内外容易理解记住,对于无人机而言,自己的轴肯定是内部的,地球系的轴是外部的,内外容易区分。此文采用内旋定义欧拉角。值得注意的是,无论是使用内旋还是外旋定义欧拉角,姿态角和欧拉角都不是完全一致的,不要把姿态角与欧拉角混为一谈。

对于问题二:无人机中最常用的是 z-y-x 的顺序。据说是因为此种顺序万向节死锁的位置比较好(无人机很少会到达此角度),此文采用。

对于问题三:遵循习惯,绕 x 轴旋转角用 ϕ \phi ϕ 表示,绕 y 轴旋转角用 θ \theta θ 表示,绕 z 轴旋转角用 ψ \psi ψ 表示。绕坐标轴逆时针旋转为正,顺时针为负。注意判断顺逆时针时,把坐标系的箭头对准自己(比如 z 轴应该从下往上看),再来判断是顺时针还是逆时针。

综上,初始时机体系与地球系重合,无人机先绕机体系 z 轴旋转 ψ \psi ψ,再绕机体系 y 轴旋转 θ \theta θ,最后绕机体系 x 轴旋转 ϕ \phi ϕ,无人机就有一个确定的姿态,我们把 [ θ , ϕ , ψ ] [\theta, \phi, \psi] [θ,ϕ,ψ] 合称为欧拉角。

2.2.2 从欧拉角与姿态角说起

无论是使用欧拉角,还是四元数、旋转矩阵,目标都是解算姿态角,因此,需要把姿态角与这些方式联系起来。欧拉角是绕机体系旋转轴旋转的角度,姿态角是机体系坐标轴与水平面的夹角(俯仰、滚转角)和与正北方的夹角(偏航角),两者存在什么关系呢?

不要感觉很复杂,先来简单想象一下:

最开始的状态,机体系与地球系重合,机头朝北;然后无人机先绕 z 轴逆时针旋转(俯视无人机是顺时针旋转),比如 ψ = 90 ° \psi=90° ψ=90°,此时无人机头朝东,欧拉角为 [ 0 , 0 , 90 ° ] [0, 0, 90°] [0,0,90°], 姿态角为 [ 0 , 0 , 90 ° ] [0, 0, 90°] [0,0,90°];无人机绕 y 轴逆时针旋转60°,由于是水平的,容易想象无人机有且仅有俯仰角增大,而且刚好为 60°。此时欧拉角为 [ 60 ° , 0 , 90 ° ] [60°, 0, 90°] [60°,0,90°],姿态角也为 [ 60 ° , 0 , 90 ° ] [60°, 0, 90°] [60°,0,90°]。无人机再绕 x 轴逆时针旋转45°,根据定义,欧拉角为 [ 60 ° , 45 ° , 90 ° ] [60°, 45°, 90°] [60°,45°,90°]。但是姿态角也是 [ 60 ° , 45 ° , 90 ° ] [60°, 45°, 90°] [60°,45°,90°]吗?如果不是,应该是多少呢?

为了解决上面的问题,我们不得不先学点东西。不过不要害怕,理论也挺有意思的。

2.2.3 姿态变化率与机体角速度的关系

上面需要求解姿态与欧拉角的关系,欧拉角也即机体转动角度。这需要对立体几何进行分析,过程略显复杂,没有推导出来。但是姿态变化率与机体角速度(欧拉角变化率)之间却有一个现成的公式——欧拉运动学方程:

从姿态变化率到机体角速度的关系:

[ ω x ω y ω z ] = [ 1 0 − sin ⁡ α 0 cos ⁡ β cos ⁡ α sin ⁡ β 0 − sin ⁡ β cos ⁡ α cos ⁡ β ] [ α ′ β ′ γ ′ ] (2.1) \left[ \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end {array} \right]= \left[ \begin{array}{cc} 1 & 0 & -\sin \alpha \\ 0 & \cos \beta & \cos \alpha \sin \beta \\ 0 & -\sin \beta & \cos \alpha \cos \beta \end {array} \right] \left[ \begin{array}{c} \alpha' \\ \beta' \\ \gamma' \end {array} \right] \tag{2.1} ⎣⎡​ωx​ωy​ωz​​⎦⎤​=⎣⎡​100​0cosβ−sinβ​−sinαcosαsinβcosαcosβ​⎦⎤​⎣⎡​α′β′γ′​⎦⎤​(2.1)

其中, α , β , γ \alpha, \beta, \gamma α,β,γ 分别为俯仰角、滚转角和偏航角。

反过来,从机体角速度到姿态变化率的转换为式(2.1) 中矩阵的逆:

[ α ′ β ′ γ ′ ] = [ 1 tan ⁡ α sin ⁡ β tan ⁡ α cos ⁡ β 0 cos ⁡ β − sin ⁡ β 0 sin ⁡ β / cos ⁡ α cos ⁡ β / cos ⁡ α ] [ ω x ω y ω z ] (2.2) \left[ \begin{array}{c} \alpha' \\ \beta' \\ \gamma' \end {array} \right]= \left[ \begin{array}{cc} 1 & \tan \alpha \sin \beta & \tan \alpha \cos \beta \\ 0 & \cos \beta & -\sin \beta \\ 0 & \sin \beta / \cos \alpha & \cos \beta / \cos \alpha \end {array} \right] \left[ \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end {array} \right] \tag{2.2} ⎣⎡​α′β′γ′​⎦⎤​=⎣⎡​100​tanαsinβcosβsinβ/cosα​tanαcosβ−sinβcosβ/cosα​⎦⎤​⎣⎡​ωx​ωy​ωz​​⎦⎤​(2.2)

可见, ω x , ω y , ω z \omega_x, \omega_y, \omega_z ωx​,ωy​,ωz​ 已知时,实际中可以使用陀螺仪测量。上式为关于 α , β , γ \alpha, \beta, \gamma α,β,γ 的微分方程组,三个独立方程三个未知数,方程可解。当然,工程中不去实际去解方程,更多的是使用数值计算方法。

为了求解之前的问题,我们模拟一下陀螺仪,把每次角度变化分为1000小份,并假设加机体角速度是均匀变化的。

那么,首先绕 z 轴逆时针旋转90°,假设 1s 完成,角速度为 90°/s,分成1000小份累加;再绕 y 轴逆时针旋转 60°与绕 x 轴逆时针旋转 45°,也分别使用 1s 完成,也都分为1000小份累加。

2.2.3 矩阵表示旋转的理论推导

先说明一下,在下面所有的描述中,涉及的坐标系的原点是重合的(即使不画在一起),因为我们研究的是转动关系,不是空间位置关系。坐标系的各个轴都是正交的,没有斜坐标系,默认此条件。

首先看二维平面内的旋转。坐标系 x O y xOy xOy 绕原点逆时针旋转 ψ \psi ψ得到 x ′ O y ′ x'Oy' x′Oy′,点A在两坐标系下坐标分别为 ( x 1 , y 1 ) (x_1, y_1) (x1​,y1​) 和 ( x 2 , y 2 ) (x_2, y_2) (x2​,y2​),则

[ x 2 y 2 ] = [ cos ⁡ ψ sin ⁡ ψ − sin ⁡ ψ cos ⁡ ψ ] [ x 1 y 1 ] (2.1) \left[ \begin{array}{c} x_2 \\ y_2 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi & \sin \psi \\ -\sin \psi & \cos \psi \end {array} \right] \left[ \begin{array}{c} x_1 \\ y_1 \end {array} \right] \tag{2.1} [x2​y2​​]=[cosψ−sinψ​sinψcosψ​][x1​y1​​](2.1)

证明如下:

设A点与原点距离为 d d d,则: x 2 = d cos ⁡ ( α − ψ ) = d ( cos ⁡ α cos ⁡ ψ + sin ⁡ α sin ⁡ ψ ) = ( d cos ⁡ α ) cos ⁡ ψ + ( d sin ⁡ α ) sin ⁡ ψ = x 1 cos ⁡ ψ + y 1 sin ⁡ ψ \begin{aligned} x_2 &= d \cos(\alpha - \psi) \\ &=d(\cos \alpha \cos \psi + \sin \alpha \sin \psi) \\ &=(d \cos \alpha) \cos \psi + (d\sin \alpha) \sin \psi \\ &=x_1 \cos \psi + y_1 \sin \psi \\ \end {aligned} x2​​=dcos(α−ψ)=d(cosαcosψ+sinαsinψ)=(dcosα)cosψ+(dsinα)sinψ=x1​cosψ+y1​sinψ​

y 2 = d sin ⁡ ( α − ψ ) = d ( sin ⁡ α cos ⁡ ψ − cos ⁡ α sin ⁡ ψ ) = ( d sin ⁡ α ) cos ⁡ ψ − ( d cos ⁡ α ) sin ⁡ ψ = y 1 cos ⁡ ψ − x 1 sin ⁡ ψ \begin{aligned} y_2 &= d \sin(\alpha - \psi) \\ &=d(\sin \alpha \cos \psi - \cos \alpha \sin \psi) \\ &=(d \sin \alpha) \cos \psi - (d\cos \alpha) \sin \psi \\ &=y_1 \cos \psi - x_1 \sin \psi \\ \end {aligned} y2​​=dsin(α−ψ)=d(sinαcosψ−cosαsinψ)=(dsinα)cosψ−(dcosα)sinψ=y1​cosψ−x1​sinψ​ — 证毕 —

对于上述旋转矩阵,从两个方向推广。一是朝三维方向推广,上图中,可看作 O x y z Oxyz Oxyz 坐标系绕 z 轴逆时针旋转 ψ \psi ψ得到 O x ′ y ′ z ′ Ox'y'z' Ox′y′z′ 坐标系,点 A 在两个坐标系下的坐标分别为 ( x 1 , y 1 , z 1 ) (x_1, y_1, z_1) (x1​,y1​,z1​), ( x 2 , y 2 , z 2 ) (x_2, y_2, z_2) (x2​,y2​,z2​)。由于是绕 z 轴旋转,z 坐标是不变的,且z坐标不会影响 x、y轴上的变化,即 z 2 = 0 ⋅ x 1 + 0 ⋅ y 1 + 1 ⋅ z 1 z_2 = 0\cdot x_1 + 0\cdot y_1 + 1\cdot z_1 z2​=0⋅x1​+0⋅y1​+1⋅z1​,结合式(2.1),易得: [ x 2 y 2 z 2 ] = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] [ x 1 y 1 z 1 ] (2.2) \left[ \begin{array}{c} x_2 \\ y_2 \\ z_2 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end {array} \right] \left[ \begin{array}{c} x_1 \\ y_1 \\ z_1 \end {array} \right] \tag{2.2} ⎣⎡​x2​y2​z2​​⎦⎤​=⎣⎡​cosψ−sinψ0​sinψcosψ0​001​⎦⎤​⎣⎡​x1​y1​z1​​⎦⎤​(2.2) 中间的矩阵便叫作旋转矩阵,此处用 R z R_z Rz​ 表示,代表绕 z 轴转动的情况:

R z = [ cos ⁡ ψ sin ⁡ ψ 0 − sin ⁡ ψ cos ⁡ ψ 0 0 0 1 ] (2.3) R_z = \left[ \begin{array}{cc} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end {array} \right] \tag{2.3} Rz​=⎣⎡​cosψ−sinψ0​sinψcosψ0​001​⎦⎤​(2.3)

同理,如果是绕 y 轴旋转 θ \theta θ,那么在二维旋转矩阵的基础上变换,由于y 轴坐标在两个坐标系中大小不变,旋转矩阵第二行为 [ 0 1 0 ] [0\quad1\quad0] [010],容易直接写出:

R y = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] (2.4) R_y = \left[ \begin{array}{cc} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end {array} \right] \tag{2.4} Ry​=⎣⎡​cosθ0−sinθ​010​sinθ0cosθ​⎦⎤​(2.4)

绕 x 轴旋转 ϕ \phi ϕ 时,x 方向坐标在两个坐标系下相等,第一行为 [ 1 0 0 ] [1\quad0\quad0] [100],整个旋转矩阵为: R x = [ 1 0 0 0 cos ⁡ ϕ sin ⁡ ϕ 0 − sin ⁡ ϕ cos ⁡ ϕ ] (2.5) R_x = \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi \end {array} \right] \tag{2.5} Rx​=⎣⎡​100​0cosϕ−sinϕ​0sinϕcosϕ​⎦⎤​(2.5)

另一个推广时连续旋转,如果坐标系 x ′ ′ O y ′ ′ x''Oy'' x′′Oy′′ 又是坐标系 x ′ O y ′ x'Oy' x′Oy′ 逆时针旋转 ψ 2 \psi_2 ψ2​,点A 在 x ′ ′ O y ′ ′ x''Oy'' x′′Oy′′ 下的坐标为 ( x 3 , y 3 ) (x_3, y_3) (x3​,y3​),那么易得:

[ x 3 y 3 ] = [ cos ⁡ ψ 2 sin ⁡ ψ 2 − sin ⁡ ψ 2 cos ⁡ ψ 2 ] [ x 2 y 2 ] \left[ \begin{array}{c} x_3 \\ y_3 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi_2 & \sin \psi_2 \\ -\sin \psi_2 & \cos \psi_2 \end {array} \right] \left[ \begin{array}{c} x_2 \\ y_2 \end {array} \right] [x3​y3​​]=[cosψ2​−sinψ2​​sinψ2​cosψ2​​][x2​y2​​]

也即: [ x 3 y 3 ] = [ cos ⁡ ψ 2 sin ⁡ ψ 2 − sin ⁡ ψ 2 cos ⁡ ψ 2 ] [ cos ⁡ ψ sin ⁡ ψ − sin ⁡ ψ cos ⁡ ψ ] [ x 1 y 1 ] \left[ \begin{array}{c} x_3 \\ y_3 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi_2 & \sin \psi_2 \\ -\sin \psi_2 & \cos \psi_2 \end {array} \right] \left[ \begin{array}{cc} \cos \psi & \sin \psi \\ -\sin \psi & \cos \psi \end {array} \right] \left[ \begin{array}{c} x_1 \\ y_1 \end {array} \right] [x3​y3​​]=[cosψ2​−sinψ2​​sinψ2​cosψ2​​][cosψ−sinψ​sinψcosψ​][x1​y1​​]

根据矩阵运算的结合律,完全可以先计算左边两个旋转矩阵的连乘,得到一个新的旋转矩阵。这说明,连续旋转等效于旋转矩阵的连乘。这个结论完全可以推广到三维,依旧使用矩阵运算的结合律容易证明。所以,如果分别绕 z-y-x 旋转一次,那么可以直接使用这三个旋转矩阵的乘积(一个旋转矩阵)来代替。

R = R z R y R x R=R_zR_yR_x R=Rz​Ry​Rx​

将式(2.3)~(2.5)代入,并用 R b e R_b^e Rbe​ 表示最终的旋转矩阵可得:

R e b = [ cos ⁡ θ cos ⁡ ψ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ cos ⁡ θ sin ⁡ ψ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ − sin ⁡ θ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ cos ⁡ θ ] (2.6) R_e^b={ \left[ \begin{array}{ccc} \cos\theta \cos\psi& \cos\psi \sin\theta \sin\phi-\sin\psi \cos \phi & \cos\psi \sin\theta \cos\phi +\sin\psi \sin\phi \\ \cos\theta \sin\psi & \sin\psi \sin\theta \sin\phi+\cos\psi \cos\phi & \sin\psi \sin\theta \cos\phi-\cos\psi \sin\phi \\ -\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta \end{array} \right ]} \tag{2.6} Reb​=⎣⎡​cosθcosψcosθsinψ−sinθ​cosψsinθsinϕ−sinψcosϕsinψsinθsinϕ+cosψcosϕsinϕcosθ​cosψsinθcosϕ+sinψsinϕsinψsinθcosϕ−cosψsinϕcosϕcosθ​⎦⎤​(2.6)

这也即是欧拉角-四元数-旋转矩阵关系图里的第一个转换关系。注意这个矩阵是无人机分别绕机体系 z-y-x 坐标轴分别逆时针转动 ψ , ϕ , θ \psi, \phi, \theta ψ,ϕ,θ 得到的。再强调一下,是绕机体系,是 z-y-x 的旋转顺序,是分别逆时针旋转 ψ , ϕ , θ \psi, \phi, \theta ψ,ϕ,θ 角度。

推导完了,回到之前的问题,我们继续解决无人机绕 z-y-x 分别是 [ 90 ° , 60 ° , 45 ° ] [90°, 60°, 45°] [90°,60°,45°] 时的姿态。注意不要先入为主,一下子又联系到熟悉的旋转矩阵 R b e R_b^e Rbe​,那只不过是我们顺便提一下的结论而已,真正的“道”是对上面旋转的深入理解,然后具体问题具体分析。

2.2.4 欧拉角与姿态角的联系

首先,对于俯仰角与滚转角。从定义上看,俯仰角与滚转角分别是机体系 x、y 轴与航向系 x、y 轴的夹角,也即是旋转航向系使得和机体系重合的欧拉角。

无人机绕 z 轴逆时针旋转 90° 后,机体系与航向系依旧是重合的,绕 z 轴的旋转矩阵为: R z = [ 1 0 0 0 1 0 0 0 1 ] R_z = \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end {array} \right] Rz​=⎣⎡​100​010​001​⎦⎤​ 无人机绕 y 轴逆时针旋转 60° 后,可得旋转矩阵:

R y = [ cos ⁡ 60 ° 0 sin ⁡ 60 ° 0 1 0 − sin ⁡ 60 ° 0 cos ⁡ 60 ° ] = [ 1 / 2 0 3 / 2 0 1 0 − 3 / 2 0 1 / 2 ] R_y = \left[ \begin{array}{cc} \cos 60° & 0 & \sin 60° \\ 0 & 1 & 0 \\ -\sin 60° & 0 & \cos 60° \end {array} \right] = \left[ \begin{array}{cc} 1/2 & 0 & \sqrt{3}/2 \\ 0 & 1 & 0 \\ -\sqrt{3}/2 & 0 & 1/2 \end {array} \right] Ry​=⎣⎡​cos60°0−sin60°​010​sin60°0cos60°​⎦⎤​=⎣⎡​1/20−3 ​/2​010​3 ​/201/2​⎦⎤​

最后无人机绕 x 轴逆时针旋转 45°, R x = [ 1 0 0 0 cos ⁡ 45 ° sin ⁡ 45 ° 0 − sin ⁡ 45 ° cos ⁡ 45 ° ] = [ 1 0 0 0 2 / 2 2 / 2 0 − 2 / 2 2 / 2 ] R_x = \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \cos 45° & \sin 45° \\ 0 & -\sin 45° & \cos 45° \end {array} \right]= \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \sqrt{2}/2 & \sqrt{2}/2 \\ 0 & -\sqrt{2}/2 & \sqrt{2}/2 \end {array} \right] Rx​=⎣⎡​100​0cos45°−sin45°​0sin45°cos45°​⎦⎤​=⎣⎡​100​02 ​/2−2 ​/2​02 ​/22 ​/2​⎦⎤​

整个旋转矩阵最终为:

R = R z R y R x = [ 1 0 0 0 1 0 0 0 1 ] [ cos ⁡ 60 ° 0 sin ⁡ 60 ° 0 1 0 − sin ⁡ 60 ° 0 cos ⁡ 60 ° ] [ 1 0 0 0 cos ⁡ 45 ° sin ⁡ 45 ° 0 − sin ⁡ 45 ° cos ⁡ 45 ° ] = 1 \begin{aligned} R &=R_z R_y R_x \\ &= \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end {array} \right] \left[ \begin{array}{cc} \cos 60° & 0 & \sin 60° \\ 0 & 1 & 0 \\ -\sin 60° & 0 & \cos 60° \end {array} \right] \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \cos 45° & \sin 45° \\ 0 & -\sin 45° & \cos 45° \end {array} \right] &= 1 \end{aligned} R​=Rz​Ry​Rx​=⎣⎡​100​010​001​⎦⎤​⎣⎡​cos60°0−sin60°​010​sin60°0cos60°​⎦⎤​⎣⎡​100​0cos45°−sin45°​0sin45°cos45°​⎦⎤​​=1​



【本文地址】


今日新闻


推荐新闻


CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3