多旋翼飞行器设计与控制(五)—— 坐标系和姿态表示

本文详细介绍了多旋翼飞行器姿态表示的方法,包括坐标系的选择(地球固联坐标系与机体坐标系)、欧拉角的概念及其与角速度的关系,以及旋转矩阵的运用。同时,提到了欧拉角存在的万向节死锁问题,并引出了四元数作为解决这一问题的手段,阐述了四元数的基本运算和在姿态表示中的优势。
摘要由CSDN通过智能技术生成

多旋翼飞行器设计与控制(五)—— 坐标系和姿态表示

一、坐标系

一般的,对坐标轴与旋转正方向的定义采用右手定则。介绍两种坐标系:地球固联坐标系机体坐标系

地球固联坐标系用于研究多旋翼飞行器相对于地面的运动状态,确定机体的空间位置坐标。它忽略地球曲率,即将地球表面假设成一张平面。通常以多旋翼起飞位置或者地心作为坐标原点。先让x轴在水平面内指向某一方向,z轴垂直于地面向下,然后按右手定则确定y轴。

机体坐标系与多旋翼固联,其原点取在多旋翼的重心位置。x轴在多旋翼对称平面内指向机头。z轴在飞机对称平面内垂直x轴向下,然后按右手定则确定y轴。

两个坐标系之间可以利用矩阵变换进行转换,地球固联坐标系绕固定点转动三次即可使它与机体坐标系的三轴指向完全一致,转动角度即为欧拉角

二、欧拉角

定义:机体坐标系与地面地球固联坐标系之间的夹角就是飞机的姿态角,又称欧拉角。俯仰角为机体轴与水平面之间的夹角,飞机抬头为证。偏航角为机体轴在水平面上的投影与地轴之间的夹角,以机头右偏为正。滚转角为飞机对称面绕机体轴转过的角度,右滚为正。

在这里插入图片描述

欧拉角变化率与机体角速度的关系

可以通过三次旋转的矩阵相乘得到总的转换矩阵表示欧拉角变化速率与机体角速度的关系。

公式如下:

b ω = [ ω x b     ω y b     ω z b ] T {^b} \omega = [\omega_{xb}~~~ \omega_{yb} ~~~ \omega_{zb} ]^T bω=[ωxb   ωyb   ωzb]T

b ω = Ψ ′ ⋅ b k 3 + θ ′ ⋅ n 2 + ϕ ′ ⋅ b 1 {^b} \omega = \Psi ^\prime \cdot {^b}k_3 + \theta ^\prime \cdot n_2 + \phi ^\prime \cdot b_1 bω=Ψbk3+θn2+ϕb1

其 中 k 3 , n 2 , b 1 即 为 旋 转 轴 其中k_3,n_2,b_1即为旋转轴 k3n2b1

最终两者之间的关系可表示为

欧拉角变化速率 ——> 机体角速度:

[ ω x b ω y b ω z b ] = [ 1 0 − s i n θ 0 c o s ϕ c o s θ s i n ϕ 0 − s i n ϕ c o s θ c o s ϕ ] [ ϕ ′ θ ′ Ψ ′ ] \begin{bmatrix} \omega_{xb} \\ \omega_{yb} \\ \omega_{zb} \end{bmatrix} = \begin{bmatrix} 1& 0 & -sin\theta \\ 0& cos\phi& cos\theta sin\phi \\ 0 & -sin\phi& cos\theta cos\phi \end{bmatrix} \begin{bmatrix} \phi^\prime \\ \theta^\prime \\ \Psi^\prime \end{bmatrix} ωxbωybωzb=1000cosϕsinϕsinθcosθsinϕcosθcosϕϕθΨ

从该式子不难发现从欧拉角变化速率转换到角速的转换矩阵中并没有偏航角的参与

机体角速度 ——> 欧拉角变化速率:

[ ϕ θ Ψ ] = [ 1 t a n θ s i n ϕ t a n θ c o s ϕ 0 c o s ϕ − s i n ϕ 0 s i n ϕ c o s θ c o s ϕ c o s θ ] [ ϕ ′ θ ′ Ψ ′ ] \begin{bmatrix} \phi \\ \theta \\ \Psi \end{bmatrix} = \begin{bmatrix} 1& tan\theta sin\phi & tan\theta cos\phi \\ 0& cos\phi& -sin\phi \\ 0 & \frac{sin\phi}{cos\theta}& \frac{cos\phi}{cos\theta} \end{bmatrix} \begin{bmatrix} \phi^\prime \\ \theta^\prime \\ \Psi^\prime \end{bmatrix} ϕθΨ=100tanθsinϕcosϕcosθsinϕtanθcosϕsinϕcosθcosϕϕθΨ

从这个式子中不难发现当俯仰角为正负90度时,矩阵中的元素出现被除数为0的情况,这便是解的奇异性问题。从物理意义来说这叫万向节死锁现象。即俯仰角为正负90度时,偏航角对应了无数种情况,不能唯一确定。所以用欧拉角表示姿态存在缺陷

当俯仰角和滚动角幅度不大,近似为零时,上式可近似为:

[ ϕ ′ θ ′ Ψ ′ ] = [ ω x b ω y b ω z b ] \begin{bmatrix} \phi^\prime \\ \theta^\prime \\ \Psi^\prime \end{bmatrix} = \begin{bmatrix} \omega_{xb} \\ \omega_{yb} \\ \omega_{zb} \end{bmatrix} ϕθΨ=ωxbωybωzb

三、旋转矩阵

从地球固联坐标系到机体坐标系的旋转可以通过三步完成:

在这里插入图片描述

其中

R z ( Ψ ) = [ c o s Ψ s i n Ψ 0 − s i n Ψ c o s Ψ 0 0 0 1 ] R_z(\Psi) = \begin{bmatrix} cos \Psi & sin \Psi & 0\\ -sin \Psi & cos \Psi & 0 \\ 0&0&1 \end{bmatrix} Rz(Ψ)=cosΨsinΨ0sinΨcosΨ0001

R y ( θ ) = [ c o s θ 0 − s i n θ 0 1 0 s i n θ 0 c o s θ ] R_y(\theta) = \begin{bmatrix} cos \theta & 0 & -sin \theta \\ 0&1&0 \\ sin \theta&0& cos \theta \end{bmatrix} Ry(θ)=cosθ0sinθ010sinθ0cosθ

R x ( ϕ ) = [ 1 0 0 0 c o s ϕ s i n ϕ 0 − s i n ϕ c o s θ ] R_x(\phi) = \begin{bmatrix} 1 & 0 & 0\\ 0 & cos \phi & sin \phi \\ 0& -sin \phi & cos \theta \end{bmatrix} Rx(ϕ)=1000cosϕsinϕ0sinϕcosθ

从机身坐标转换到地球固联坐标系可以通过上面三个矩阵相乘得到

R b e = [ c o s θ c o s Ψ c o s Ψ s i n θ s i n ϕ − s i n Ψ c o s ϕ c o s Ψ s i n θ c o s ϕ + s i n Ψ s i n ϕ c o s θ s i n Ψ s i n Ψ s i n θ s i n ϕ + c o s Ψ c o s ϕ s i n Ψ s i n θ c o s ϕ − c o s Ψ s i n ϕ − s i n θ s i n ϕ c o s θ c o s ϕ c o s θ ] R^e_b = \begin{bmatrix} 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{bmatrix} Rbe=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θ

上 式 可 以 简 化 为 R b e = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] 上式可以简化为 R^e_b = \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23} \\ r_{31}& r_{32} & r_{33} \end{bmatrix} Rbe=r11r21r31r12r22r32r13r23r33

我们可以通过旋转矩阵反求欧拉角

{ Ψ = a r c t a n r 21 r 11 θ = a r c s i n ( − r 31 ) ϕ = a r c t a n r 32 r 33 \left\{ \begin{array}{c} \Psi = arctan \frac{r_{21}}{r_{11}} \\ \theta = arcsin(-r_{31}) \\ \phi = arctan \frac{r_{32}}{r_{33}} \end{array} \right. Ψ=arctanr11r21θ=arcsin(r31)ϕ=arctanr33r32

但是用这种方式表示仍然存在万向节死锁问题,在这种情况下,人为设定滚转角为0 此时便有对应的偏航角

旋转矩阵导数与机体角速度关系

d R b e d t ∣ = R b e [ b ω ] x \left. \frac{{\rm d} R^e_b}{{\rm d}t} \right| = R^e_b [ {^b}\omega]_x dtdRbe=Rbe[bω]x

采用旋转矩阵表示避免了奇异性问题。然而,以上方程含有9个自由变量,因此求解微分方程的计算量比较大。

通过对欧拉角和旋转矩阵的学习,我们发现欧拉角表示存在万向节死锁问题,旋转矩阵表示存在计算量过大问题,而人们发现使用四元数表示姿态可以解决上述问题

四、四元数

四元数定义:四元数一般用向量形式表示为:

q = [ q 0 q v ] q= \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix} q=[q0qv]

其 中 q 0 为 四 元 数 的 标 量 部 分 , q v = [ q 1    q 2    q 3 ] T 为 四 元 数 的 向 量 部 分 。 其中q_0 为四元数的标量部分, q_v = [q_1 ~~ q_2 ~~ q_3]^T 为四元数的向量部分。 q0qv=[q1  q2  q3]T
对 于 一 个 实 数 S , 其 四 元 数 表 示 形 式 为 q = [ s 0 3 × 3 ] 对于一个实数S,其四元数表示形式为 q = \begin{bmatrix} s \\ 0_{3 \times 3} \\ \end{bmatrix} Sq=[s03×3]

对 于 一 个 纯 向 量 v , 其 四 元 数 表 示 形 式 q = [ 0 v ] 对于一个纯向量v,其四元数表示形式 q = \begin{bmatrix} 0 \\ v \\ \end{bmatrix} v,q=[0v]

四元数的基本运算法则:

  • 四元数加减法:

p ± q = [ p 0 p v ] ± [ q 0 q v ] = [ p 0 ± q 0 p v ± q v ] p \pm q = \begin{bmatrix} p_0 \\ p_v \\ \end{bmatrix} \pm \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix} = \begin{bmatrix} p_0 \pm q_0 \\ p_v \pm q_v \\ \end{bmatrix} p±q=[p0pv]±[q0qv]=[p0±q0pv±qv]

  • 四元数乘法:

p ⨂ q = [ p 0 p v ] ⨂ [ q 0 q v ] = [ p 0 q 0 − q v T p v p v × q v + p 0 q v + q 0 p v ] p \bigotimes q = \begin{bmatrix} p_0 \\ p_v \\ \end{bmatrix} \bigotimes \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix} = \begin{bmatrix} p_0q_0 - q^T_vp_v \\ p_v \times q_v + p_0q_v +q_0p_v \\ \end{bmatrix} pq=[p0pv][q0qv]=[p0q0qvTpvpv×qv+p0qv+q0pv]

  • 四元数共轭:
    若 q = [ q 0 q v ] , 则 q ∗ = [ q 0 − q v ] 若q= \begin{bmatrix} q_0 \\ q_v \\ \end{bmatrix}, 则q^*= \begin{bmatrix} q_0 \\ -q_v \\ \end{bmatrix} q=[q0qv]q=[q0qv]

  • 四元数范数:

∣ ∣ q ∣ ∣ 2 = ∣ ∣ q ⨂ q ∗ ∣ ∣ = q 1 2 + q 2 2 + q 3 2 + q 4 2 \mid \mid q \mid \mid ^2 = \mid \mid q \bigotimes q^* \mid \mid = q^2_1+q^2_2+q^2_3+q^2_4 q2=qq=q12+q22+q32+q42

  • 四元数的逆:

q ⨂ q − 1 = [ 1 0 3 × 3 ] q \bigotimes q^-1 = \begin{bmatrix} 1 \\ 0_{3 \times 3} \\ \end{bmatrix} qq1=[103×3]

q − 1 = q ∗ ∣ ∣ q ∣ ∣ q^-1 = \frac{q^*}{\mid \mid q \mid \mid} q1=qq

  • 基本运算性质:

q ⨂ ( r + m ) = q ⨂ r + q ⨂ m q \bigotimes (r+m) = q \bigotimes r + q \bigotimes m q(r+m)=qr+qm

q ⨂ r ⨂ m = ( q ⨂ r ) ⨂ m = q ⨂ ( r ⨂ m ) q \bigotimes r \bigotimes m = (q \bigotimes r) \bigotimes m = q \bigotimes (r \bigotimes m ) qrm=(qr)m=q(rm)

s q = q s = [ s q 0 s q v ] sq =qs = \begin{bmatrix} sq_0 \\ sq_v \\ \end{bmatrix} sq=qs=[sq0sqv]

[ 0 u ] ⨂ [ 0 v ] = [ − u T v u × v ] \begin{bmatrix} 0 \\ u \\ \end{bmatrix} \bigotimes \begin{bmatrix} 0 \\ v \\ \end{bmatrix} = \begin{bmatrix} -u^Tv \\ u \times v \\ \end{bmatrix} [0u][0v]=[uTvu×v]

四元数与旋转:

在这里插入图片描述

这 个 旋 转 过 程 可 以 表 示 为 向 量 v 1 绕 着 v 轴 按 照 右 手 定 则 旋 转 θ 变 换 到 了 向 量 v 1 ′ 这个旋转过程可以表示为向量v_1绕着v轴按照右手定则旋转 \theta 变换到了向量v_1 \prime v1vθv1

四元数与旋转矩阵的转换:

在这里插入图片描述

四元数与欧拉角的转换:

在这里插入图片描述
在这里插入图片描述

四元数变化率与机体角速度关系:

在这里插入图片描述

五、三种姿态表示方法的比较

在这里插入图片描述

  • 3
    点赞
  • 21
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值