四旋翼控制系統建模

1 四旋翼飛行原理

前端旋翼 1 和后端旋翼 3 逆時針旋轉，而左端旋翼 2 和右端的旋翼 4 順時 針旋轉，以平衡旋翼旋轉所產生的反扭轉矩，由此可知，懸停時：

四只旋翼的轉速應該相等，以相互抵消反扭力矩；
同時等量地增大或減小四只旋翼的轉速，會引起上升或下降運動；
增大某一只旋翼的轉速，同時等量地減小同組另一只旋翼的轉速，則產生俯仰、橫 滾運動；
增大某一組旋翼的轉速，同時等量減小另一組旋翼的轉速，將產生偏航運動，

• 滾轉、俯仰由同組旋翼的轉速不同產生；偏航由不同組旋翼轉速不同產生，即由反扭力矩產生，
• 反扭力矩是電機底座賦予旋翼轉速時，旋翼反作用在電機底座的反作用力，由于底座與機臂固連，因此反扭力矩會使機體產生以旋翼中心為軸，與旋翼轉動方向相反的旋轉運動，

2 四旋翼飛行控制剛體模型

{ e p ˙ = e v e v ˙ = g e 3 ? f m R b e e 3 Θ ˙ = W ? b ω J ? b ω ˙ = ? b ω × J ? b ω + G a + τ \left\{ \begin{array}{lr} {{}^{\rm{e}}\dot p = {}^ev}\\ {{}^e\dot v = g{e_3} - \frac{f}{m}{\mathop{\rm R}\nolimits} _{\rm{b}}^{\rm{e}}{e_3}}\\ {\dot \Theta = W \cdot {}^b\omega }\\ {J \cdot {}^b\dot \omega = - {}^b\omega \times J \cdot {}^b\omega + {G_a} + \tau } \end{array} \right. ????????ep˙?=evev˙=ge3??mf?Rbe?e3?Θ˙=W?bωJ?bω˙=?bω×J?bω+Ga?+τ?
其中，
p—位置
v—速度
Θ—歐拉姿態角
W—三軸角速度到歐拉角的變換矩陣
f—機體向上的總拉力
J—轉動慣量
R b e {\mathop{\rm R}\nolimits} _{\rm{b}}^{\rm{e}} Rbe? —機體到世界坐標系旋轉矩陣
ω \omega ω—三軸角速度
G a G_a Ga?—陀螺力矩
τ \tau τ—轉矩

機體到世界坐標系旋轉矩陣:
R b e = ( R e b ) T = ( R z ( ψ ) ? R y ( θ ) ? R x ( ? ) ) T = [ cos ? θ cos ? ψ cos ? ψ sin ? θ sin ? ? ? sin ? ψ cos ? ? cos ? ψ sin ? θ cos ? ? + sin ? ψ sin ? ? cos ? θ sin ? ψ sin ? ψ sin ? θ sin ? ? + cos ? ψ cos ? ? sin ? ψ sin ? θ cos ? ? ? cos ? ψ cos ? ? ? sin ? θ sin ? ? cos ? θ cos ? ? cos ? θ \begin{array}{l} R_{\rm{b}}^{\rm{e}} = {\left( {R_{\rm{e}}^{\rm{b}}} \right)^{\rm{T}}}{\rm{ = }}{\left( {{R_z}\left( \psi \right) \cdot {R_y}\left( \theta \right) \cdot {R_x}\left( \phi \right)} \right)^{\rm{T}}}\\ = \left[ {\begin{array}{l} {\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 \cos \phi }\\ { - \sin \theta }&{\sin \phi \cos \theta }&{\cos \phi \cos \theta } \end{array}} \right. \end{array} Rbe?=(Reb?)T=(Rz?(ψ)?Ry?(θ)?Rx?(?))T=???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ψcos?cos?cosθ??

三軸角速率與歐拉角速度關系：
ω = [ p q r ] = [ ? ˙ 0 0 ] + [ 1 0 0 0 cos ? ? sin ? ? 0 ? sin ? ? cos ? ? ] ? [ 0 θ ˙ 0 ] + [ 1 0 0 0 cos ? ? sin ? ? 0 ? sin ? ? cos ? ? ] ? [ cos ? θ 0 ? sin ? θ 0 1 0 sin ? θ 0 cos ? θ ] ? [ 0 0 ψ ˙ ] = [ 1 0 ? sin ? θ 0 cos ? ? sin ? ? cos ? θ 0 ? sin ? ? cos ? ? cos ? θ ] ? [ ? ˙ θ ˙ ψ ˙ ] = T ? Θ ˙ \begin{array}{l} \omega = \left[ {\begin{array}{l} p\\ q\\ r \end{array}} \right] = \left[ {\begin{array}{l} {\dot \phi }\\ 0\\ 0 \end{array}} \right] + \left[ {\begin{array}{l} 1&0&0\\ 0&{\cos \phi }&{\sin \phi }\\ 0&{ - \sin \phi }&{\cos \phi } \end{array}} \right] \cdot \left[ {\begin{array}{l} 0\\ {\dot \theta }\\ 0 \end{array}} \right] + \left[ {\begin{array}{l} 1&0&0\\ 0&{\cos \phi }&{\sin \phi }\\ 0&{ - \sin \phi }&{\cos \phi } \end{array}} \right] \cdot \left[ {\begin{array}{l} {\cos \theta }&0&{ - \sin \theta }\\ 0&1&0\\ {\sin \theta }&0&{\cos \theta } \end{array}} \right] \cdot \left[ {\begin{array}{l} 0\\ 0\\ {\dot \psi } \end{array}} \right]\\ = \left[ {\begin{array}{l} 1&0&{ - \sin \theta }\\ 0&{\cos \phi }&{\sin \phi \cos \theta }\\ 0&{ - \sin \phi }&{\cos \phi \cos \theta } \end{array}} \right] \cdot \left[ {\begin{array}{l} {\dot \phi }\\ {\dot \theta }\\ {\dot \psi } \end{array}} \right] = T \cdot \dot \Theta \end{array} ω=???pqr????=????˙?00????+???100?0cos??sin??0sin?cos?????????0θ˙0????+???100?0cos??sin??0sin?cos?????????cosθ0sinθ?010??sinθ0cosθ????????00ψ˙?????=???100?0cos??sin???sinθsin?cosθcos?cosθ?????????˙?θ˙ψ˙?????=T?Θ˙?
得三軸角速度到歐拉角的變換矩陣：
W = [ 1 tan ? θ sin ? ? tan ? θ cos ? ? 0 cos ? ? ? sin ? ? 0 sin ? ? / cos ? θ cos ? ? / cos ? θ ] = T ? 1 W = \left[ {\begin{array}{lr} 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]{\text{ = }}{T^{ - 1}} W=???100?tanθsin?cos?sin?/cosθ?tanθcos??sin?cos?/cosθ???? = T?1

四旋翼控制剛體模型是非線性、多耦合、欠驅動系統，因此要對以上四旋翼剛體模型進行化簡，

假設多旋翼的飛行特點是俯仰角和滾轉角都非常小，總拉力約等于四旋翼的重力，即：
sin ? ? ≈ ? \sin \phi \approx \phi sin?≈?， cos ? ? ≈ 1 \cos \phi \approx {\text{1}} cos?≈1， sin ? θ ≈ θ \sin \theta \approx \theta sinθ≈θ ， cos ? θ ≈ 1 , \cos \theta \approx {\text{1}}, cosθ≈1,， f ≈ m g f \approx mg f≈mg，
此時： W = I W = I W=I，并忽略 ? b ω × J ? b ω + G a - {}^b\omega \times J \cdot {}^b\omega + {G_a} ?bω×J?bω+Ga?項(陀螺力矩，轉動慣性)

R ? b e e 3 = [ θ cos ? ψ + ? sin ? ψ θ sin ? ψ ? ? cos ? ψ 1 ] \operatorname{R} _{\text{b}}^{\text{e}}{e_3} = \left[ {\begin{array}{l} {\theta \cos \psi + \phi \sin \psi } \\ {\theta \sin \psi - \phi \cos \psi } \\ 1 \end{array}} \right] Rbe?e3?=???θcosψ+?sinψθsinψ??cosψ1????

從而可將可將水平通道與高度通道運動學分離，

整理得線性四旋翼剛體模型：

運動學

• 水平通道：
  { p ˙ h = v h v ˙ h = ? g A ψ Θ h \left\{ {\begin{array}{l} {{{\dot p}_{\text{h}}} = {v_{\text{h}}}} \\ {{{\dot v}_{\text{h}}} = - g{A_\psi }{\Theta _{\text{h}}}} \end{array}} \right. {p˙?h?=vh?v˙h?=?gAψ?Θh??
  p h = [ p x p y ] , {p_{\text{h}}} = \left[ {\begin{array}{l} {{p_x}} \\ {{p_y}} \end{array}} \right], ph?=[px?py??], R ψ = [ cos ? ψ ? sin ? ψ sin ? ψ cos ? ψ ] , {R_\psi } = \left[ {\begin{array}{l} {\cos \psi }&{ - \sin \psi } \\ {\sin \psi }&{\cos \psi } \end{array}} \right], Rψ?=[cosψsinψ??sinψcosψ?], A ψ = R ψ [ 0 1 ? 1 0 ] , {A_\psi } = {R_\psi }\left[ {\begin{array}{l} 0&1 \\ { - 1}&0 \end{array}} \right], Aψ?=Rψ?[0?1?10?], Θ h = [ ? θ ] {\Theta _{\text{h}}} = \left[ {\begin{array}{l} \phi \\ \theta \end{array}} \right] Θh?=[?θ?]

• 高度通道：
  { p ˙ z = v z v ˙ z = g ? f m \left\{ {\begin{array}{l} {{{\dot p}_{\text{z}}} = {v_{\text{z}}}} \\ {{{\dot v}_{\text{z}}} = g - \frac{f}{m}} \end{array}} \right. {p˙?z?=vz?v˙z?=g?mf??

動力學

姿態模型：
{ Θ ˙ = ω J ? ω ˙ = τ \left\{ {\begin{array}{l} {\dot \Theta = \omega } \\ {J \cdot \dot \omega = \tau } \end{array}} \right. {Θ˙=ωJ?ω˙=τ?

2 四旋翼控制框圖

其中， ? , \phi , ?, θ , \theta, θ, ψ \psi ψ分別指代滾轉、偏航、俯仰， f f f表示總拉力， τ \tau τ表示三軸轉矩，加后綴d代表期望值，
P P P指位置， Θ Θ Θ指姿態角，
? d , k {\varpi _{\text{d}}}_{,k} ?d?,k?指期望電機轉速， σ d , k {\sigma _{\text{d}}}_{,k} σd?,k?期望油門指令

自駕儀常用PID控制結構

2.1 位置控制器

位置控制器根據期望位置與實際位置差，解算為姿態期望，自駕儀的位置控制器結構如下，

水平位置通道控制設計

• 設計水平通道位置控制：水平通道期望位置 p hd {p_{\text{hd}}} phd? ，實際位置 p h {p_{\text{h}}} ph?
  設： e p h = ( p hd ? p h ) {e_{{\text{p}}h}}{\text{ = }}\left( {{p_{{\text{hd}}}} - {p_{\text{h}}}} \right) eph? = (phd??ph?)，注意，本文誤差與參考資料中的設定符號相反，下同
  e ˙ p h = ? K p h e p h ? p ˙ hd ? p ˙ h = ? K p h ( p hd ? p h ) {\dot e_{{\text{p}}h}} = - {K_{{\text{p}}h}}{e_{{\text{p}}h}} \Rightarrow {\dot p_{{\text{hd}}}} - {\dot p_{\text{h}}}{\text{ = }} - {K_{{\text{p}}h}}\left( {{p_{{\text{hd}}}} - {p_{\text{h}}}} \right) e˙ph?=?Kph?eph??p˙?hd??p˙?h? = ?Kph?(phd??ph?)
  p hd {p_{\text{hd}}} phd?作為常量項，導數 p ˙ hd {\dot p_{{\text{hd}}}} p˙?hd?為0，輸出被控物件為 p h {p_{\text{h}}} ph? ，令 v h d = p ˙ h {v_{\text{h}}}_{\text{d}}{\text{ = }}{\dot p_{\text{h}}} vh?d? = p˙?h?，即控制 p ˙ h {\dot p_{\text{h}}} p˙?h?使水平通道位置與速度滿足 e ˙ p h = ? K p h e p h {\dot e_{{\text{p}}h}} = - {K_{{\text{p}}h}}{e_{{\text{p}}h}} e˙ph?=?Kph?eph?，根據相平面可知，速度與位置誤差滿足該方程，則誤差會最終收斂于0，即 lim ? t → ∞ ∥ e v h ( t ) ∥ = 0 {\lim _{t \to \infty }}\left\| {{e_{{\text{v}}h}}\left( t \right)} \right\| = 0 limt→∞?∥evh?(t)∥=0 ，
  則設計水平通道速度期望：
  ? v h d = K p h ( p hd ? p h ) = K p h e p h \Rightarrow {v_{\text{h}}}_{\text{d}}{\text{ = }}{K_{{\text{p}}h}}\left( {{p_{{\text{hd}}}} - {p_{\text{h}}}} \right) = {K_{{\text{p}}h}}{e_{{\text{p}}h}} ?vh?d? = Kph?(phd??ph?)=Kph?eph?

• 設計水平通道速度控制：
  設 e v h = ( v hd ? v h ) {e_{{\text{v}}h}}{\text{ = }}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right) evh? = (vhd??vh?)
  e ˙ v h = ? K vhp e v h ? K vhi ∫ e v h ? K vhd e ˙ v h ? v ˙ hd ? v ˙ h = ? K vhp ( v hd ? v h ) ? K vhi ∫ ( v hd ? v h ) ? K vhd ( v ˙ hd ? v ˙ h ) {\dot e_{{\text{v}}h}} = - {K_{{\text{vhp}}}}{e_{{\text{v}}h}} - {K_{{\text{vhi}}}}\int {{e_{{\text{v}}h}}} - {K_{{\text{vhd}}}}{\dot e_{{\text{v}}h}} \Rightarrow {\dot v_{{\text{hd}}}} - {\dot v_{\text{h}}}{\text{ = }} - {K_{{\text{vhp}}}}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right) - {K_{{\text{vhi}}}}\int {\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right)} - {K_{{\text{vhd}}}}\left( {{{\dot v}_{{\text{hd}}}} - {{\dot v}_{\text{h}}}} \right) e˙vh?=?Kvhp?evh??Kvhi?∫evh??Kvhd?e˙vh??v˙hd??v˙h? = ?Kvhp?(vhd??vh?)?Kvhi?∫(vhd??vh?)?Kvhd?(v˙hd??v˙h?)
  水平通道速度控制環， v hd {v_{{\text{hd}}}} vhd?作為常量項， v h {v_{\text{h}}} vh?作為控制物件，得期望水平通道加速度為：
  v ˙ h = K vhp ( v hd ? v h ) + K vhi ∫ ( v hd ? v h ) + K vhd ( v ˙ hd ? v ˙ h ) {\dot v_{\text{h}}}{\text{ = }}{K_{{\text{vhp}}}}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right){\text{ + }}{K_{{\text{vhi}}}}\int {\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right)} {\text{ + }}{K_{{\text{vhd}}}}\left( {{{\dot v}_{{\text{hd}}}} - {{\dot v}_{\text{h}}}} \right) v˙h? = Kvhp?(vhd??vh?) + Kvhi?∫(vhd??vh?) + Kvhd?(v˙hd??v˙h?)
  由四旋翼水平通道運動學方程 v ˙ h = ? g A ψ Θ h {{{\dot v}_{\text{h}}} = - g{A_\psi }{\Theta _{\text{h}}}} v˙h?=?gAψ?Θh?得：
  v ˙ h = ? g A ψ Θ hd = K vhp ( v hd ? v h ) + K vhi ∫ ( v hd ? v h ) + K vhd ( v ˙ hd ? v ˙ h ) ? Θ hd = ? g ? 1 A ψ ? 1 ( K vhp e v h + K vhi ∫ e v h + K vhd e ˙ v h ) {\dot v_{\text{h}}} = - g{A_\psi }{\Theta _{{\text{hd}}}} = {K_{{\text{vhp}}}}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right){\text{ + }}{K_{{\text{vhi}}}}\int {\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right)} {\text{ + }}{K_{{\text{vhd}}}}\left( {{{\dot v}_{{\text{hd}}}} - {{\dot v}_{\text{h}}}} \right)\\\Rightarrow {\Theta _{{\text{hd}}}} = - {g^{ - 1}}A_\psi ^{ - 1}\left( {{K_{{\text{vhp}}}}{e_{{\text{v}}h}} + {K_{{\text{vhi}}}}\int {{e_{{\text{v}}h}}} + {K_{{\text{vhd}}}}{{\dot e}_{{\text{v}}h}}} \right) v˙h?=?gAψ?Θhd?=Kvhp?(vhd??vh?) + Kvhi?∫(vhd??vh?) + Kvhd?(v˙hd??v˙h?)?Θhd?=?g?1Aψ?1?(Kvhp?evh?+Kvhi?∫evh?+Kvhd?e˙vh?)

高度位置通道控制設計

• 設計高度通道位置控制：高度通道期望位置 p zd {p_{{\text{zd}}}} pzd? ，實際位置 p z {p_{\text{z}}} pz?
  設： e pz = ( p zd ? p z ) {e_{{\text{pz}}}}{\text{ = }}\left( {{p_{{\text{zd}}}} - {p_{\text{z}}}} \right) epz? = (pzd??pz?)
  e ˙ pz = ? K pz e pz ? p ˙ zd ? p ˙ z = ? K pz ( p zd ? p z ) {\dot e_{{\text{pz}}}} = - {K_{{\text{pz}}}}{e_{{\text{pz}}}} \Rightarrow {\dot p_{{\text{zd}}}} - {\dot p_{\text{z}}}{\text{ = }} - {K_{{\text{pz}}}}\left( {{p_{{\text{zd}}}} - {p_{\text{z}}}} \right) e˙pz?=?Kpz?epz??p˙?zd??p˙?z? = ?Kpz?(pzd??pz?)
  p zd {p_{{\text{zd}}}} pzd?作為常量項，導數 p ˙ zd {\dot p_{{\text{zd}}}} p˙?zd?為0，輸出被控物件為 p z {p_{\text{z}}} pz?，令 v z d = p ˙ z {v_{\text{z}}}_{\text{d}}{\text{ = }}{\dot p_{\text{z}}} vz?d? = p˙?z?，則設計平通道速度期望為：
  ? v z d = K pz ( p zd ? p z ) = K pz e pz \Rightarrow {v_{\text{z}}}_{\text{d}}{\text{ = }}{K_{{\text{pz}}}}\left( {{p_{{\text{zd}}}} - {p_{\text{z}}}} \right) = {K_{{\text{pz}}}}{e_{{\text{pz}}}} ?vz?d? = Kpz?(pzd??pz?)=Kpz?epz?

• 設計高度通道速度控制：
  設： e vz = ( v zd ? v z ) {e_{{\text{vz}}}}{\text{ = }}\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right) evz? = (vzd??vz?)
  e ˙ vz = ? K vzp e vz ? K vzi ∫ e vz ? K vzd e ˙ vz ? v ˙ zd ? v ˙ z = ? K vzp ( v hd ? v h ) ? K vzi ∫ ( v zd ? v z ) ? K vzd ( v ˙ zd ? v ˙ z ) {\dot e_{{\text{vz}}}} = - {K_{{\text{vzp}}}}{e_{{\text{vz}}}} - {K_{{\text{vzi}}}}\int {{e_{{\text{vz}}}}} - {K_{{\text{vzd}}}}{\dot e_{{\text{vz}}}} \Rightarrow {\dot v_{{\text{zd}}}} - {\dot v_{\text{z}}}{\text{ = }} - {K_{{\text{vzp}}}}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right) - {K_{{\text{vzi}}}}\int {\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right)} - {K_{{\text{vzd}}}}\left( {{{\dot v}_{{\text{zd}}}} - {{\dot v}_{\text{z}}}} \right) e˙vz?=?Kvzp?evz??Kvzi?∫evz??Kvzd?e˙vz??v˙zd??v˙z? = ?Kvzp?(vhd??vh?)?Kvzi?∫(vzd??vz?)?Kvzd?(v˙zd??v˙z?)
  高度通道速度控制環， v zd {v_{{\text{zd}}}} vzd?作為常量項， v z {v_{\text{z}}} vz?作為輸出控制物件，得期望高度通道加速度為：
  v ˙ z = K vzp ( v zd ? v z ) + K vzi ∫ ( v zd ? v z ) + K vzd ( v ˙ zd ? v ˙ z ) {\dot v_{\text{z}}}{\text{ = }}{K_{{\text{vzp}}}}\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right){\text{ + }}{K_{{\text{vzi}}}}\int {\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right)} {\text{ + }}{K_{{\text{vzd}}}}\left( {{{\dot v}_{{\text{zd}}}} - {{\dot v}_{\text{z}}}} \right) v˙z? = Kvzp?(vzd??vz?) + Kvzi?∫(vzd??vz?) + Kvzd?(v˙zd??v˙z?)
  由四旋翼高度通道運動學方程： v ˙ z = g ? f m {{{\dot v}_{\text{z}}} = g - \frac{f}{m}} v˙z?=g?mf?得：
  v ˙ z = g ? f d m = K vzp ( v zd ? v z ) + K vzi ∫ ( v zd ? v z ) + K vzd ( v ˙ zd ? v ˙ z ) ? f d = m ( g ? K vzp e vz + K vzi ∫ e vz + K vzd e ˙ vz ) {\dot v_{\text{z}}} = g - \frac{{{f_d}}}{m} = {K_{{\text{vzp}}}}\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right){\text{ + }}{K_{{\text{vzi}}}}\int {\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right)} {\text{ + }}{K_{{\text{vzd}}}}\left( {{{\dot v}_{{\text{zd}}}} - {{\dot v}_{\text{z}}}} \right)\\ \Rightarrow {f_d} = m\left( {g - {K_{{\text{vzp}}}}{e_{{\text{vz}}}} + {K_{{\text{vzi}}}}\int {{e_{{\text{vz}}}}} + {K_{{\text{vzd}}}}{{\dot e}_{{\text{vz}}}}} \right) v˙z?=g?mfd??=Kvzp?(vzd??vz?) + Kvzi?∫(vzd??vz?) + Kvzd?(v˙zd??v˙z?)?fd?=m(g?Kvzp?evz?+Kvzi?∫evz?+Kvzd?e˙vz?)

方向飽和函式

由于四旋翼的簡化模型都是基于小角度假設，因此需要加入方向飽和函式，對速度誤差及位置控制器輸出的姿態期望進行飽和約束，使無人機飛行滿足小角度假設，
s a t gd ( u , a ) ? { u , ∥ u ∥ ∞ ? a a u ∥ u ∥ ∞ , ∥ u ∥ ∞ > a sa{t_{{\text{gd}}}}\left( {u,a} \right) \triangleq \left\{ {\begin{array}{l} {u,}&{{{\left\| u \right\|}_\infty } \leqslant a} \\ {a\frac{u}{{{{\left\| u \right\|}_\infty }}},}&{{{\left\| u \right\|}_\infty } > a} \end{array}} \right. satgd?(u,a)?{u,a∥u∥∞?u?,?∥u∥∞??a∥u∥∞?>a?
保方向飽和函式，不僅可以限制最終分量每個分量的絕對值不大于a，還可以保證它的方向與原向量u相同，

加入飽和的開源自駕儀的位置PID控制

• 水平通道：加入方向飽和函式，約束俯仰和滾轉角度滿足小角度約束：
  { v h d = K p h ( p hd ? p h ) = K p h e p h e v h = s a t gd ( v hd ? v h , a 1 ) Θ hd = s a t gd ( ? g ? 1 A ψ ? 1 ( K vhp e v h + K vhi ∫ e v h + K vhd e ˙ v h ) , a 2 ) \left\{ \begin{array}{l} {v_{\text{h}}}_{\text{d}}{\text{ = }}{K_{{\text{p}}h}}\left( {{p_{{\text{hd}}}} - {p_{\text{h}}}} \right) = {K_{{\text{p}}h}}{e_{{\text{p}}h}} \\ {e_{{\text{v}}h}}{\text{ = }}sa{t_{{\text{gd}}}}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}},{a_1}} \right) \\ {\Theta _{{\text{hd}}}} = sa{t_{{\text{gd}}}}\left( { - {g^{ - 1}}A_\psi ^{ - 1}\left( {{K_{{\text{vhp}}}}{e_{{\text{v}}h}} + {K_{{\text{vhi}}}}\int {{e_{{\text{v}}h}}} + {K_{{\text{vhd}}}}{{\dot e}_{{\text{v}}h}}} \right),{a_2}} \right) \\ \end{array} \right. ??????vh?d? = Kph?(phd??ph?)=Kph?eph?evh? = satgd?(vhd??vh?,a1?)Θhd?=satgd?(?g?1Aψ?1?(Kvhp?evh?+Kvhi?∫evh?+Kvhd?e˙vh?),a2?)?
• 高度通道：加入方向飽和函式，避免油門指令超出范圍:
  { v z d = K pz ( p zd ? p z ) = K pz e pz e vz = s a t gd ( v zd ? v z , a 4 ) f d = s a t gd ( m ( g ? K vzp e vz + K vzi ∫ e vz + K vzd e ˙ vz ) , a 5 ) \left\{ \begin{array}{l} {v_{\text{z}}}_{\text{d}}{\text{ = }}{K_{{\text{pz}}}}\left( {{p_{{\text{zd}}}} - {p_{\text{z}}}} \right) = {K_{{\text{pz}}}}{e_{{\text{pz}}}}\\ {e_{{\text{vz}}}}{\text{ = }}sa{t_{{\text{gd}}}}\left( {{v_{{\text{zd}}}} - {v_z},{a_4}} \right)\\ {f_{\text{d}}} = sa{t_{{\text{gd}}}}\left( {m\left( {g - {K_{{\text{vzp}}}}{e_{{\text{vz}}}} + {K_{{\text{vzi}}}}\int {{e_{{\text{vz}}}}} + {K_{{\text{vzd}}}}{{\dot e}_{{\text{vz}}}}} \right),{a_5}} \right) \\ \end{array} \right. ????vz?d? = Kpz?(pzd??pz?)=Kpz?epz?evz? = satgd?(vzd??vz?,a4?)fd?=satgd?(m(g?Kvzp?evz?+Kvzi?∫evz?+Kvzd?e˙vz?),a5?)?

2.2 姿態控制

設 e Θ = ( Θ d ? Θ ) {e_\Theta }{\text{ = }}\left( {{\Theta _{\text{d}}} - \Theta } \right) eΘ? = (Θd??Θ)， Θ d = [ Θ h d ψ d ] T {\Theta _d} = {\left[ {\begin{array}{l} {{\Theta _{hd}}}&{{\psi _d}} \end{array}} \right]^T} Θd?=[Θhd??ψd??]T，其中水平兩軸旋轉( (滾轉、俯仰))由水平位置通道控制器解算輸出得到，偏航角( )由外部直接給定，見勺ò肛制框圖，

• 設計姿態角控制：
  e ˙ Θ = ? K Θ e Θ ? Θ ˙ d ? Θ ˙ = ? K Θ ( Θ d ? Θ ) {\dot e_\Theta } = - {K_\Theta }{e_\Theta } \Rightarrow {\dot \Theta _{\text{d}}} - \dot \Theta {\text{ = }} - {K_\Theta }\left( {{\Theta _{\text{d}}} - \Theta } \right) e˙Θ?=?KΘ?eΘ??Θ˙d??Θ˙ = ?KΘ?(Θd??Θ)
  Θ d {\Theta _{\text{d}}} Θd?作為常量項，導數 Θ ˙ d {\dot \Theta _{\text{d}}} Θ˙d?為0，輸出被控物件為 Θ \Theta Θ，令 ω d = Θ ˙ {\omega _d} = \dot \Theta ωd?=Θ˙，得期望的水平通道速度期望：
  ? ω d = K Θ ( Θ d ? Θ ) = K Θ e Θ \Rightarrow {\omega _d}{\text{ = }}{K_\Theta }\left( {{\Theta _{\text{d}}} - \Theta } \right) = {K_\Theta }{e_\Theta } ?ωd? = KΘ?(Θd??Θ)=KΘ?eΘ?

• 設計姿態角速度控制
  e ˙ ω = ? K ω p e ω ? K ω i ∫ e ω ? K ω d e ˙ ω ? ω ˙ d ? ω ˙ = ? K ω p ( ω d ? ω ) ? K ω i ∫ ( ω d ? ω ) ? K ω d ( ω ˙ d ? ω ˙ ) {\dot e_\omega } = - {K_{\omega p}}{e_\omega } - {K_{\omega i}}\int {{e_\omega }} - {K_{\omega {\text{d}}}}{\dot e_\omega } \Rightarrow {\dot \omega _{\text{d}}} - \dot \omega {\text{ = }} - {K_{\omega p}}\left( {{\omega _{\text{d}}} - \omega } \right) - {K_{\omega i}}\int {\left( {{\omega _{\text{d}}} - \omega } \right)} - {K_{\omega {\text{d}}}}\left( {{{\dot \omega }_{\text{d}}} - \dot \omega } \right) e˙ω?=?Kωp?eω??Kωi?∫eω??Kωd?e˙ω??ω˙d??ω˙ = ?Kωp?(ωd??ω)?Kωi?∫(ωd??ω)?Kωd?(ω˙d??ω˙)
  由簡化后的動力學模型 Θ ˙ = ω {\dot \Theta = \omega } Θ˙=ω得：
  J ? ω ˙ = τ ? ω ˙ = τ / J = K ω p ( ω d ? ω ) + K ω i ∫ ( ω d ? ω ) + K ω d ( ω ˙ d ? ω ˙ ) J \cdot \dot \omega = \tau \Rightarrow \dot \omega = \tau /J{\text{ = }}{K_{\omega p}}\left( {{\omega _{\text{d}}} - \omega } \right) + {K_{\omega i}}\int {\left( {{\omega _{\text{d}}} - \omega } \right)} + {K_{\omega {\text{d}}}}\left( {{{\dot \omega }_{\text{d}}} - \dot \omega } \right) J?ω˙=τ?ω˙=τ/J = Kωp?(ωd??ω)+Kωi?∫(ωd??ω)+Kωd?(ω˙d??ω˙)
  J為常數，可由比例系數彌補，則可設計轉矩期望：
  τ d = K ω p ( ω d ? ω ) + K ω i ∫ ( ω d ? ω ) + K ω d ( ω ˙ d ? ω ˙ ) = K ω p e ω + K ω i ∫ e ω + K ω d e ˙ ω {\tau _{\text{d}}}{\text{ = }}{K_{\omega p}}\left( {{\omega _{\text{d}}} - \omega } \right) + {K_{\omega i}}\int {\left( {{\omega _{\text{d}}} - \omega } \right)} + {K_{\omega {\text{d}}}}\left( {{{\dot \omega }_{\text{d}}} - \dot \omega } \right){\text{ = }}{K_{\omega p}}{e_\omega }{\text{ + }}{K_{\omega i}}\int {{e_\omega }} {\text{ + }}{K_{\omega {\text{d}}}}{\dot e_\omega } τd? = Kωp?(ωd??ω)+Kωi?∫(ωd??ω)+Kωd?(ω˙d??ω˙) = Kωp?eω? + Kωi?∫eω? + Kωd?e˙ω?
  類似位置控制器，姿態控制器也需加入飽和，

加入飽和的PID姿態控制

開源自駕儀的姿態PID控制器：
{ ω d = K Θ ( Θ d ? Θ ) = K Θ e Θ e ω = s a t gd ( ω d ? ω , a 1 0 ) τ d = s a t gd ( K ω p e ω + K ω i ∫ e ω + K ω d e ˙ ω , a 11 ) \left\{ \begin{array}{l} {\omega _d}{\text{ = }}{K_\Theta }\left( {{\Theta _{\text{d}}} - \Theta } \right) = {K_\Theta }{e_\Theta } \\ {e_\omega }{\text{ = }}sa{t_{{\text{gd}}}}\left( {{\omega _{\text{d}}} - \omega ,{a_{1{\text{0}}}}} \right) \\ {\tau _{\text{d}}} = sa{t_{{\text{gd}}}}\left( {{K_{\omega p}}{e_\omega }{\text{ + }}{K_{\omega i}}\int {{e_\omega }} {\text{ + }}{K_{\omega {\text{d}}}}{{\dot e}_\omega },{a_{{\text{11}}}}} \right)\\ \end{array} \right. ????ωd? = KΘ?(Θd??Θ)=KΘ?eΘ?eω? = satgd?(ωd??ω,a10?)τd?=satgd?(Kωp?eω? + Kωi?∫eω? + Kωd?e˙ω?,a11?)?

2.3 控制分配器

控制分配器將期望拉力及力矩轉換為期望的電機轉速，

• 旋翼無人機懸停時單個螺旋槳拉力可以表示為： T i = c T ? i 2 {T_i} = {c_{\text{T}}}\varpi _i^{\text{2}} Ti?=cT??i2? ， c T {c_{\text{T}}} cT?為常數且可通過試驗測得，
• 旋翼無人機懸停時單個螺旋槳在機身上產生的反扭力矩可以表示為 M i = c M ? i 2 {M_i} = {c_{\text{M}}}\varpi _i^{\text{2}} Mi?=cM??i2?， c M {c_{\text{M}}} cM?為常數且可通過試驗測得，

X字形四旋翼的控制分配和多旋翼的控制效率模型如下：
[ f τ x τ y τ z ] = [ c T c T c T c T ? 2 2 d c T 2 2 d c T 2 2 d c T ? 2 2 d c T 2 2 d c T ? 2 2 d c T 2 2 d c T ? 2 2 d c T c M c M c M c M ] [ ? 1 2 ? 2 2 ? 3 2 ? 4 2 ] ? [ f τ x τ y τ z ] = [ c T 0 0 0 0 d c T 0 0 0 0 d c T 0 0 0 0 c M ] [ 1 1 1 1 ? 2 2 2 2 2 2 ? 2 2 2 2 ? 2 2 2 2 ? 2 2 1 1 ? 1 ? 1 ] [ ? 1 2 ? 2 2 ? 3 2 ? 4 2 ] \begin{gathered} \left[ {\begin{array}{l} f \\ {{\tau _x}} \\ {{\tau _y}} \\ {{\tau _z}} \end{array}} \right] = \left[ {\begin{array}{l} {{c_T}}&{{c_T}}&{{c_T}}&{{c_T}} \\ { - \frac{{\sqrt 2 }}{2}d{c_T}}&{\frac{{\sqrt 2 }}{2}d{c_T}}&{\frac{{\sqrt 2 }}{2}d{c_T}}&{ - \frac{{\sqrt 2 }}{2}d{c_T}} \\ {\frac{{\sqrt 2 }}{2}d{c_T}}&{ - \frac{{\sqrt 2 }}{2}d{c_T}}&{\frac{{\sqrt 2 }}{2}d{c_T}}&{ - \frac{{\sqrt 2 }}{2}d{c_T}} \\ {{c_M}}&{{c_M}}&{{c_M}}&{{c_M}} \end{array}} \right]\left[ {\begin{array}{l} {\varpi _{\text{1}}^{\text{2}}} \\ {\varpi _{\text{2}}^{\text{2}}} \\ {\varpi _{\text{3}}^{\text{2}}} \\ {\varpi _{\text{4}}^{\text{2}}} \end{array}} \right] \\ \Downarrow \\ \left[ {\begin{array}{l} f \\ {{\tau _x}} \\ {{\tau _y}} \\ {{\tau _z}} \end{array}} \right] = \left[ {\begin{array}{l} {{c_T}}&{\text{0}}&{\text{0}}&{\text{0}} \\ {\text{0}}&{d{c_T}}&{\text{0}}&{\text{0}} \\ {\text{0}}&{\text{0}}&{d{c_T}}&{\text{0}} \\ {\text{0}}&{\text{0}}&{\text{0}}&{{c_M}} \end{array}} \right]\left[ {\begin{array}{l} {\text{1}}&{\text{1}}&{\text{1}}&{\text{1}} \\ { - \frac{{\sqrt 2 }}{2}}&{\frac{{\sqrt 2 }}{2}}&{\frac{{\sqrt 2 }}{2}}&{ - \frac{{\sqrt 2 }}{2}} \\ {\frac{{\sqrt 2 }}{2}}&{ - \frac{{\sqrt 2 }}{2}}&{\frac{{\sqrt 2 }}{2}}&{ - \frac{{\sqrt 2 }}{2}} \\ {\text{1}}&{\text{1}}&{ - 1}&{ - 1} \end{array}} \right]\left[ {\begin{array}{l} {\varpi _{\text{1}}^{\text{2}}} \\ {\varpi _{\text{2}}^{\text{2}}} \\ {\varpi _{\text{3}}^{\text{2}}} \\ {\varpi _{\text{4}}^{\text{2}}} \end{array}} \right] \\ \end{gathered} ?????fτx?τy?τz???????=?????cT??22 ??dcT?22 ??dcT?cM??cT?22 ??dcT??22 ??dcT?cM??cT?22 ??dcT?22 ??dcT?cM??cT??22 ??dcT??22 ??dcT?cM?????????????12??22??32??42?????????????fτx?τy?τz???????=?????cT?000?0dcT?00?00dcT?0?000cM????????????1?22 ??22 ??1?122 ???22 ??1?122 ??22 ???1?1?22 ???22 ???1????????????12??22??32??42????????
c T , d , c M c_T, d, c_M cT?,d,cM?為未知引數，可以用控制器中的比例系數進行補償，設油門、俯仰、滾轉、偏航四個通道的輸出分別為 σ 1 , σ 2 , σ 3 , σ 4 {\sigma _{\text{1}}},{\sigma _{\text{2}}},{\sigma _{\text{3}}},{\sigma _{\text{4}}} σ1?,σ2?,σ3?,σ4?，
則令：
[ ? 1 2 ? 2 2 ? 3 2 ? 4 2 ] = [ 1 ? 1 1 1 1 1 1 1 1 1 ? 1 1 1 ? 1 ? 1 ? 1 ] [ σ 1 σ 2 σ 3 σ 4 ] \left[ {\begin{array}{l} {\varpi _{\text{1}}^{\text{2}}} \\ {\varpi _{\text{2}}^{\text{2}}} \\ {\varpi _{\text{3}}^{\text{2}}} \\ {\varpi _{\text{4}}^{\text{2}}} \end{array}} \right]{\text{ = }}\left[ {\begin{array}{l} {\text{1}}&{ - {\text{1}}}&{\text{1}}&{\text{1}} \\ {\text{1}}&{\text{1}}&{\text{1}}&{\text{1}} \\ {\text{1}}&{\text{1}}&{ - {\text{1}}}&{\text{1}} \\ {\text{1}}&{ - {\text{1}}}&{ - {\text{1}}}&{ - {\text{1}}} \end{array}} \right]\left[ {\begin{array}{l} {{\sigma _{\text{1}}}} \\ {{\sigma _{\text{2}}}} \\ {{\sigma _{\text{3}}}} \\ {{\sigma _{\text{4}}}} \end{array}} \right] ??????12??22??32??42??????? = ?????

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/206610.html

標籤：其他

上一篇：MAX30102 血氧除錯筆記

下一篇：Style(2)：命名空間