文章目录
四旋翼控制系统建模
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−mfRbee3Θ˙=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⎦⎤+⎣⎡1000cosϕ−sinϕ0sinϕcosϕ⎦⎤⋅⎣⎡0θ˙0⎦⎤+⎣⎡1000cosϕ−sinϕ0sinϕcosϕ⎦⎤⋅⎣⎡cosθ0sinθ010−sinθ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θ⎦⎤ = 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] 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ψ[0−110], Θ 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=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=−Kpheph⇒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}}} 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 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}} ⇒vhd = Kph(phd−ph)=Kpheph -
设计水平通道速度控制:
设 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=−Kvhpevh−Kvhi∫evh−Kvhde˙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(Kvhpevh+Kvhi∫evh+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 = (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=−Kpzepz⇒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}}} 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(pzd−pz)=Kpzepz -
设计高度通道速度控制:
设: 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=−Kvzpevz−Kvzi∫evz−Kvzde˙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−Kvzpevz+Kvzi∫evz+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,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. ⎩⎪⎨⎪⎧vhd = Kph(phd−ph)=Kphephevh = satgd(vhd−vh,a1)Θhd=satgd(−g−1Aψ−1(Kvhpevh+Kvhi∫evh+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(pzd−pz)=Kpzepzevz = satgd(vzd−vz,a4)fd=satgd(m(g−Kvzpevz+Kvzi∫evz+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ωi∫eω−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ωi∫eω + 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ωi∫eω + 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⎦⎥⎥⎤=⎣⎢⎢⎡cT−22dcT22dcTcMcT22dcT−22dcTcMcT22dcT22dcTcMcT−22dcT−22dcTcM⎦⎥⎥⎤⎣⎢⎢⎡ϖ12ϖ22ϖ32ϖ42⎦⎥⎥⎤⇓⎣⎢⎢⎡fτxτyτz⎦⎥⎥⎤=⎣⎢⎢⎡cT0000dcT0000dcT0000cM⎦⎥⎥⎤⎣⎢⎢⎡1−22221122−22112222−11−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⎦⎥⎥⎤ = ⎣⎢⎢⎡1111−111−111−1−1111−1⎦⎥⎥⎤⎣⎢⎢⎡σ1σ2σ3σ4⎦⎥⎥⎤
2.4 四旋翼数学模型
四旋翼动力学模型是输入是油门指令,输出是位置及速度,姿态角及角速度。这里的四旋翼动态模型就是四旋翼的数字仿真模型,用来模型四旋翼的飞行。四旋翼动态模型由动力单元模型及四旋翼运动学动力学模型组成。运动学及动力学模型见前文。
动力单元模型是四旋翼模型中的一个模块,输入为油门指令,输出为拉力与力矩。如下: