四旋翼控制系统建模

四旋翼控制系统建模

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˙=ge3mfRbee3Θ˙=WbωJbω˙=bω×Jbω+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+1000cosϕsinϕ0sinϕcosϕ0θ˙0+1000cosϕsinϕ0sinϕcosϕcosθ0sinθ010sinθ0cosθ00ψ˙=1000cosϕ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=100tanθsinϕcosϕsinϕ/cosθtanθcosϕsinϕcosϕ/cosθ = T1

四旋翼控制刚体模型是非线性、多耦合、欠驱动系统,因此要对以上四旋翼刚体模型进行化简。

假设多旋翼的飞行特点是俯仰角和滚转角都非常小,总拉力约等于四旋翼的重力,即:
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 fmg
此时: W = I W = I W=I,并忽略 − b ω × J ⋅ b ω + G a - {}^b\omega \times J \cdot {}^b\omega + {G_a} bω×Jbω+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] Rbee3=θ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=vhv˙h=gAψΘh
    p h = [ p x p y ] , {p_{\text{h}}} = \left[ {\begin{array}{l} {{p_x}} \\ {{p_y}} \end{array}} \right], ph=[pxpy], 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ψ[0110], Θ 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=vzv˙z=gmf

动力学

姿态模型:
{ Θ ˙ = ω 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 = (phdph)注意,本文误差与参考资料中的设定符号相反。下同
    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=Kphephp˙hdp˙h = Kph(phdph)
    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}}} vhd = 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=Kpheph,根据相平面可知,速度与位置误差满足该方程,则误差会最终收敛于0,即 lim ⁡ t → ∞ ∥ e v h ( t ) ∥ = 0 {\lim _{t \to \infty }}\left\| {{e_{{\text{v}}h}}\left( t \right)} \right\| = 0 limtevh(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}} vhd = Kph(phdph)=Kpheph

  • 设计水平通道速度控制
    e v h  =  ( v hd − v h ) {e_{{\text{v}}h}}{\text{ = }}\left( {{v_{{\text{hd}}}} - {v_{\text{h}}}} \right) evh = (vhdvh)
    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=KvhpevhKvhievhKvhde˙vhv˙hdv˙h = Kvhp(vhdvh)Kvhi(vhdvh)Kvhd(v˙hdv˙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(vhdvh) + Kvhi(vhdvh) + Kvhd(v˙hdv˙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(vhdvh) + Kvhi(vhdvh) + Kvhd(v˙hdv˙h)Θhd=g1Aψ1(Kvhpevh+Kvhievh+Kvhde˙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 = (pzdpz)
    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=Kpzepzp˙zdp˙z = Kpz(pzdpz)
    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}}} vzd = 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}}}} vzd = Kpz(pzdpz)=Kpzepz

  • 设计高度通道速度控制
    设: e vz  =  ( v zd − v z ) {e_{{\text{vz}}}}{\text{ = }}\left( {{v_{{\text{zd}}}} - {v_{\text{z}}}} \right) evz = (vzdvz)
    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=KvzpevzKvzievzKvzde˙vzv˙zdv˙z = Kvzp(vhdvh)Kvzi(vzdvz)Kvzd(v˙zdv˙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(vzdvz) + Kvzi(vzdvz) + Kvzd(v˙zdv˙z)
    由四旋翼高度通道运动学方程: v ˙ z = g − f m {{{\dot v}_{\text{z}}} = g - \frac{f}{m}} v˙z=gmf得:
    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=gmfd=Kvzp(vzdvz) + Kvzi(vzdvz) + Kvzd(v˙zdv˙z)fd=m(gKvzpevz+Kvzievz+Kvzde˙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,auu,uau>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. vhd = Kph(phdph)=Kphephevh = satgd(vhdvh,a1)Θhd=satgd(g1Aψ1(Kvhpevh+Kvhievh+Kvhde˙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. vzd = Kpz(pzdpz)=Kpzepzevz = satgd(vzdvz,a4)fd=satgd(m(gKvzpevz+Kvzievz+Kvzde˙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ωpeωKωieωKωde˙ωω˙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ωpeω + Kωieω + Kωde˙ω
    类似位置控制器,姿态控制器也需加入饱和。

加入饱和的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ωpeω + Kωieω + Kωde˙ω,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=cT22 dcT22 dcTcMcT22 dcT22 dcTcMcT22 dcT22 dcTcMcT22 dcT22 dcTcMϖ12ϖ22ϖ32ϖ42fτxτyτz=cT0000dcT0000dcT0000cM122 22 1122 22 1122 22 1122 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 = 1111111111111111σ1σ2σ3σ4

2.4 四旋翼数学模型

四旋翼动力学模型是输入是油门指令,输出是位置及速度,姿态角及角速度。这里的四旋翼动态模型就是四旋翼的数字仿真模型,用来模型四旋翼的飞行。四旋翼动态模型由动力单元模型及四旋翼运动学动力学模型组成。运动学及动力学模型见前文。
动力单元模型是四旋翼模型中的一个模块,输入为油门指令,输出为拉力与力矩。如下:
在这里插入图片描述
在这里插入图片描述

参考链接

北航-全权老师-多旋翼理论课程
北航-全权老师-多旋翼试验课程
北航-全权老师-多旋翼试验课程资料

评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值