多旋翼建模之欧拉角微分

本文来自于zing的博文硬核推导-欧拉角微分方程

首先,为了后续推导方便;我们将pitch简写为p ,roll简写为r,yaw简写为y,旋转矩阵可写为

C b n = [ cos ⁡ p cos ⁡ y − cos ⁡ r sin ⁡ y + sin ⁡ r sin ⁡ p cos ⁡ y sin ⁡ r sin ⁡ y + cos ⁡ r sin ⁡ p cos ⁡ y cos ⁡ p sin ⁡ y cos ⁡ r cos ⁡ y + sin ⁡ r sin ⁡ p sin ⁡ y − sin ⁡ r cos ⁡ y + cos ⁡ r sin ⁡ p sin ⁡ y − sin ⁡ p sin ⁡ r cos ⁡ p cos ⁡ r cos ⁡ p ] \mathbf{C}_{b}^{n}=\left[ \begin{array}{ccc}{\cos p \cos y} & {-\cos r \sin y+\sin r \sin p \cos y} & {\sin r \sin y+\cos r \sin p \cos y} \\ {\cos p \sin y} & {\cos r \cos y+\sin r \sin p \sin y} & {-\sin r \cos y+\cos r \sin p \sin y} \\ {-\sin p} & {\sin r \cos p} & {\cos r \cos p}\end{array}\right] Cbn=cospcosycospsinysinpcosrsiny+sinrsinpcosycosrcosy+sinrsinpsinysinrcospsinrsiny+cosrsinpcosysinrcosy+cosrsinpsinycosrcosp

d ( C b n ) d t = C b n = [ − p ˙ c ( y ) s ( p ) − y ˙ c ( p ) s ( y ) r ˙ s ( r ) s ( y ) − y ˙ c ( r ) c ( y ) + p ˙ c ( p ) c ( y ) s ( r ) + r ˙ c ( r ) c ( y ) s ( p ) − y ˙ s ( p ) s ( r ) s ( y ) r ˙ c ( r ) s ( y ) + y ˙ c ( y ) s ( r ) + p ˙ c ( p ) c ( r ) c ( y ) − r ˙ c ( y ) s ( p ) s ( r ) − y ˙ c ( r ) s ( p ) s ( y ) y ˙ c ( p ) c ( y ) − p ˙ s ( p ) s ( y ) p ˙ c ( p ) s ( r ) s ( y ) − y ˙ c ( r ) s ( y ) − r ˙ c ( y ) s ( r ) + r ˙ c ( r ) s ( p ) s ( y ) + y ˙ c ( y ) s ( p ) s ( r ) y ˙ s ( r ) s ( y ) − r ˙ c ( r ) c ( y ) + p ˙ c ( p ) c ( r ) s ( y ) + y ˙ c ( r ) c ( y ) s ( p ) − r ˙ s ( p ) s ( r ) s ( y ) − p ˙ c ( p ) r ˙ c ( p ) c ( r ) − p ˙ s ( p ) s ( r ) − p ˙ c ( r ) s ( p ) − r ˙ c ( p ) s ( r ) ] \frac{d\left(\mathbf{C}_{b}^{n}\right)}{d t}=\mathbf{C}_{b}^{n}=\left[ \begin{array}{ccc}{-\dot{p} c(y) s(p)-\dot{y} c(p) s(y)} & {\dot{r} s(r) s(y)-\dot{y} c(r) c(y)+\dot{p} c(p) c(y) s(r)+\dot{r} c(r) c(y) s(p)-\dot{y} s(p) s(r) s(y)} & {\dot{r} c(r) s(y)+\dot{y} c(y) s(r)+\dot{p} c(p) c(r) c(y)-\dot{r} c(y) s(p) s(r)-\dot{y} c(r) s(p) s(y)} \\ {\dot{y} c(p) c(y)-\dot{p} s(p) s(y)} & {\dot{p} c(p) s(r) s(y)-\dot{y} c(r) s(y)-\dot{r} c(y) s(r)+\dot{r} c(r) s(p) s(y)+\dot{y} c(y) s(p) s(r)} & {\dot{y} s(r) s(y)-\dot{r} c(r) c(y)+\dot{p} c(p) c(r) s(y)+\dot{y} c(r) c(y) s(p)-\dot{r} s(p) s(r) s(y)} \\ {-\dot{p} c(p)} & {\dot{r} c(p) c(r)-\dot{p} s(p) s(r)} & {-\dot{p} c(r) s(p)-\dot{r} c(p) s(r)}\end{array}\right] dtd(Cbn)=Cbn=p˙c(y)s(p)y˙c(p)s(y)y˙c(p)c(y)p˙s(p)s(y)p˙c(p)r˙s(r)s(y)y˙c(r)c(y)+p˙c(p)c(y)s(r)+r˙c(r)c(y)s(p)y˙s(p)s(r)s(y)p˙c(p)s(r)s(y)y˙c(r)s(y)r˙c(y)s(r)+r˙c(r)s(p)s(y)+y˙c(y)s(p)s(r)r˙c(p)c(r)p˙s(p)s(r)r˙c(r)s(y)+y˙c(y)s(r)+p˙c(p)c(r)c(y)r˙c(y)s(p)s(r)y˙c(r)s(p)s(y)y˙s(r)s(y)r˙c(r)c(y)+p˙c(p)c(r)s(y)+y˙c(r)c(y)s(p)r˙s(p)s(r)s(y)p˙c(r)s(p)r˙c(p)s(r)

上式可写为:

C ˙ b n = C b n [ b ω ] × \dot{\mathbf{C}}_{b}^{n}=\mathbf{C}_{b}^{n}\left[^{b} \boldsymbol{\omega}\right]_{ \times} C˙bn=Cbn[bω]×

其中 [ b ω ] × = [ 0 p ˙ s ( r ) − y ˙ c ( p ) c ( r ) p ˙ c ( r ) + y ˙ c ( p ) s ( r ) y ˙ c ( p ) c ( r ) − p ˙ s ( r ) 0 y ˙ s ( p ) − d r − p ˙ c ( r ) − y ˙ c ( p ) s ( r ) d r − y ˙ s ( p ) 0 ] \left[^{b} \omega\right]_{ \times}=\left[ \begin{array}{ccc}{0} & {\dot{p} s(r)-\dot{y} c(p) c(r)} & {\dot{p} c(r)+\dot{y} c(p) s(r)} \\ {\dot{y} c(p) c(r)-\dot{p} s(r)} & {0} & {\dot{y} s(p)-d r} \\ {-\dot{p} c(r)-\dot{y} c(p) s(r)} & {d r-\dot{y} s(p)} & {0}\end{array}\right] [bω]×=0y˙c(p)c(r)p˙s(r)p˙c(r)y˙c(p)s(r)p˙s(r)y˙c(p)c(r)0dry˙s(p)p˙c(r)+y˙c(p)s(r)y˙s(p)dr0

即可写成

b ω = [ r ˙ − y ˙ s ( p ) p ˙ c ( r ) + y ˙ c ( p ) s ( r ) − p ˙ s ( r ) + y ˙ c ( p ) c ( r ) ] ^{b} \omega=\left[ \begin{array}{c}{\dot{r}-\dot{y} s(p)} \\ {\dot{p} c(r)+\dot{y} c(p) s(r)} \\ {-\dot{p} s(r)+\dot{y} c(p) c(r)}\end{array}\right] bω=r˙y˙s(p)p˙c(r)+y˙c(p)s(r)p˙s(r)+y˙c(p)c(r)

所以

[ ω x ω y ω z ] = [ r ˙ − y ˙ s ( p ) p ˙ c ( r ) + y ˙ c ( p ) s ( r ) − p ˙ s ( r ) + y ˙ c ( p ) c ( r ) ] \left[ \begin{array}{c}{\omega_{x}} \\ {\omega_{y}} \\ {\omega_{z}}\end{array}\right]=\left[ \begin{array}{c}{\dot{r}-\dot{y} s(p)} \\ {\dot{p} c(r)+\dot{y} c(p) s(r)} \\ {-\dot{p} s(r)+\dot{y} c(p) c(r)}\end{array}\right] ωxωyωz=r˙y˙s(p)p˙c(r)+y˙c(p)s(r)p˙s(r)+y˙c(p)c(r)

上式可写为:

[ ω x ω y ω z ] = [ 1 0 − s ( r ) 0 c ( r ) s ( r ) c ( p ) 0 − s ( r ) c ( r ) c ( p ) ] [ r ˙ p ˙ y ˙ ] \left[ \begin{array}{c}{\omega_{x}} \\ {\omega_{y}} \\ {\omega_{z}}\end{array}\right]=\left[ \begin{array}{ccc}{1} & {0} & {-s(r)} \\ {0} & {c(r)} & {s(r) c(p)} \\ {0} & {-s(r)} & {c(r) c(p)}\end{array}\right] \left[ \begin{array}{c}{\dot{r}} \\ {\dot{p}} \\ {\dot{y}}\end{array}\right] ωxωyωz=1000c(r)s(r)s(r)s(r)c(p)c(r)c(p)r˙p˙y˙

进一步得到欧拉角的微分形式:

[ r ˙ p ˙ y ˙ ] = [ 1 tan ⁡ θ sin ⁡ ϕ tan ⁡ θ cos ⁡ ϕ 0 cos ⁡ ϕ − sin ⁡ ϕ 0 sin ⁡ ϕ / cos ⁡ θ cos ⁡ ϕ / cos ⁡ θ ] [ ω x ω y ω z ] \left[ \begin{array}{c}{\dot{r}} \\ {\dot{p}} \\ {\dot{y}}\end{array}\right]=\left[ \begin{array}{ccc}{1} & {\tan \theta \sin \phi} & {\tan \theta \cos \phi} \\ {0} & {\cos \phi} & {-\sin \phi} \\ {0} & {\sin \phi / \cos \theta} & {\cos \phi / \cos \theta}\end{array}\right] \left[ \begin{array}{c}{\omega_{x}} \\ {\omega_{y}} \\ {\omega_{z}}\end{array}\right] r˙p˙y˙=100tanθsinϕcosϕsinϕ/cosθtanθcosϕsinϕcosϕ/cosθωxωyωz

你可能感兴趣的:(自控笔记)