b
V
˙
C
M
∣
i
b
{ }^{b} \dot{\mathrm{V}}_{C M \mid i}^{b}
bV˙CM∣ib
说明：
对应轴的转动惯量
J
b
=
[
J
x
x
0
0
0
J
y
y
0
0
0
J
z
z
]
J^{b}=\left[\begin{array}{ccc} J_{x x} & 0 & 0 \\ 0 & J_{y y} & 0 \\ 0 & 0 & J_{z z} \end{array}\right]
Jb=
Jxx000Jyy000Jzz
T
=
C
T
ρ
A
r
r
2
ϖ
2
T=C_{T} \rho A_{r} r^{2} \varpi^{2}
T=CTρArr2ϖ2
其中：
ρ是空气密度
CT是特定一个旋翼的系数
Ar是螺旋桨旋转的扫掠面积
r是旋翼半径
ϖ
\varpi
ϖ
是旋翼角速度
A current-carrying wire or coil can exert a force on a permanent magnet. This is called the motor effect
同时考虑马达效应和推力，和旋翼的角速度成正比
将常量化为一个系数最后得到：
Q = c Q ϖ 2 T = c T ϖ 2 Q=c_{Q} \varpi^{2}\\ T=c_{T} \varpi^{2} Q=cQϖ2T=cTϖ2
通过输入测试得到的数据，我们提供了一个程序来方便计算这些系统系数，通过这些信息，我们能够建立一个Initial matrix，分别表示推力以及体坐标系下各轴转矩。
[ Σ T τ ϕ τ θ τ ψ ] = [ c T c T c T c T c T c T 0 3 2 d + c T 3 2 d + c T 0 − 3 2 d + c T − 3 2 d + c T − d + c T − 1 2 d + c T 1 2 d + c T d + c T 1 2 d + c T − 1 2 d + c T c Q − c Q c Q − c Q c Q − c Q ] [ ϖ 1 2 ϖ 2 2 ϖ 3 2 ϖ 4 2 ϖ 5 2 ϖ 6 2 ] \left[\begin{array}{l} \Sigma T \\ \tau_{\phi} \\ \tau_{\theta} \\ \tau_{\psi} \end{array}\right]=\left[\begin{array}{cccc} c_{T} & c_{T} & c_{T} & c_{T} & c_{T} & c_{T} \\ 0 & \frac{\sqrt{3}}{2}d_{+} c_{T} & \frac{\sqrt{3}}{2}d_{+}c_{T} & 0 &- \frac{\sqrt{3}}{2}d_{+} c_{T}&-\frac{\sqrt{3}}{2}d_{+} c_{T}\\ -d_{+} c_{T} & -\frac{1}{2}d_{+}c_{T}& \frac{1}{2}d_{+}c_{T} & d_{+}c_{T}&\frac{1}{2}d_{+}c_{T}&-\frac{1}{2}d_{+}c_{T}\\ c_{Q} & -c_{Q} & c_{Q} & -c_{Q}& c_{Q} & -c_{Q} \end{array}\right]\left[\begin{array}{l} \varpi_{1}^{2} \\ \varpi_{2}^{2} \\ \varpi_{3}^{2} \\ \varpi_{4}^{2} \\ \varpi_{5}^{2} \\ \varpi_{6}^{2} \end{array}\right] ΣTτϕτθτψ = cT0−d+cTcQcT23 d+cT−21d+cT−cQcT23 d+cT21d+cTcQcT0d+cT−cQcT−23 d+cT21d+cTcQcT−23 d+cT−21d+cT−cQ ϖ12ϖ22ϖ32ϖ42ϖ52ϖ62
为了控制，由于系数是从推力转矩与rpm之间转换的，所以需要线性回归来将调速命令转换成为rpm
ϖ
S
S
=
(
Throttle
%
)
c
R
+
b
\varpi_{S S}=(\text { Throttle } \%) c_{R}+b
ϖSS=( Throttle %)cR+b
解释：
ϖ
是期望稳定的转速。
\varpi 是期望稳定的转速。
ϖ是期望稳定的转速。
throttle是比例
Cr是rpm的转换系数，b是y的偏移
陀螺力是由每个马达的转动惯量，以及转速，还有角速度Roll and Pitch rate，反力的方向参考这篇文章，简单来说，反力的方向是：
τ
=
T
=
I
ω
马达
×
ω
R
、
P
、
Q
的方向
\tau=T=I \omega_{马达}× \omega_{R、P、Q的方向}
τ=T=Iω马达×ωR、P、Q的方向
τ
ϕ
gyro
=
J
m
Q
(
π
30
)
(
ϖ
1
−
ϖ
2
+
ϖ
3
−
ϖ
4
+
ϖ
5
−
ϖ
6
)
τ
θ
gyro
=
J
m
P
(
π
30
)
(
−
ϖ
1
+
ϖ
2
−
ϖ
3
+
ϖ
4
−
ϖ
5
+
ϖ
6
)
\begin{gathered} \tau_{\phi_{\text {gyro }}}=J_{m} Q\left(\frac{\pi}{30}\right)\left(\varpi_{1}-\varpi_{2}+\varpi_{3}-\varpi_{4}+\varpi_{5}-\varpi_{6}\right) \\ \tau_{\theta_{\text {gyro }}}=J_{m} P\left(\frac{\pi}{30}\right)\left(-\varpi_{1}+\varpi_{2}-\varpi_{3}+\varpi_{4}-\varpi_{5}+\varpi_{6}\right) \end{gathered}
τϕgyro =JmQ(30π)(ϖ1−ϖ2+ϖ3−ϖ4+ϖ5−ϖ6)τθgyro =JmP(30π)(−ϖ1+ϖ2−ϖ3+ϖ4−ϖ5+ϖ6)
pi/30代表从rpm转换到角度速度的转换
M A , T b = [ 3 2 d + c T ϖ 2 2 + 3 2 d + c T ϖ 3 2 − 3 2 d + c T ϖ 5 2 − 3 2 d + c T ϖ 6 2 + J m Q ( π 30 ) ( ϖ 1 − ϖ 2 + ϖ 3 − ϖ 4 + ϖ 5 − ϖ 6 ) − d + c T ϖ 1 2 + − 1 2 d + c T ϖ 2 2 + 1 2 d + c T ϖ 3 2 + d + c T ϖ 4 2 + 1 2 d + c T ϖ 5 2 − 1 2 d + c T ϖ 6 2 + J m P ( π 30 ) ( − ϖ 1 + ϖ 2 − ϖ 3 + ϖ 4 − ϖ 5 + ϖ 6 ) c Q ϖ 1 2 − c Q ϖ 2 2 + c Q ϖ 3 2 − c Q ϖ 4 2 + c Q ϖ 5 2 − c Q ϖ 6 2 ] M_{A, T}^{b}=\left[\begin{array}{c} \frac{\sqrt{3}}{2}d_{+} c_{T}\varpi_{2}^{2} + \frac{\sqrt{3}}{2}d_{+}c_{T}\varpi_{3}^{2} - \frac{\sqrt{3}}{2}d_{+} c_{T}\varpi_{5}^{2}-\frac{\sqrt{3}}{2}d_{+} c_{T}\varpi_{6}^{2}+J_{m} Q\left(\frac{\pi}{30}\right)\left(\varpi_{1}-\varpi_{2}+\varpi_{3}-\varpi_{4}+\varpi_{5}-\varpi_{6}\right) \\ -d_{+} c_{T}\varpi_{1}^{2} + -\frac{1}{2}d_{+}c_{T}\varpi_{2}^{2}+ \frac{1}{2}d_{+}c_{T}\varpi_{3}^{2} + d_{+}c_{T}\varpi_{4}^{2}+ \frac{1}{2}d_{+}c_{T}\varpi_{5}^{2} -\frac{1}{2}d_{+}c_{T}\varpi_{6}^{2} +J_{m} P\left(\frac{\pi}{30}\right)\left(-\varpi_{1}+\varpi_{2}-\varpi_{3}+\varpi_{4}-\varpi_{5}+\varpi_{6}\right) \\ c_{Q} \varpi_{1}^{2}-c_{Q} \varpi_{2}^{2}+c_{Q} \varpi_{3}^{2}-c_{Q} \varpi_{4}^{2}+c_{Q} \varpi_{5}^{2}-c_{Q} \varpi_{6}^{2} \end{array}\right] MA,Tb= 23 d+cTϖ22+23 d+cTϖ32−23 d+cTϖ52−23 d+cTϖ62+JmQ(30π)(ϖ1−ϖ2+ϖ3−ϖ4+ϖ5−ϖ6)−d+cTϖ12+−21d+cTϖ22+21d+cTϖ32+d+cTϖ42+21d+cTϖ52−21d+cTϖ62+JmP(30π)(−ϖ1+ϖ2−ϖ3+ϖ4−ϖ5+ϖ6)cQϖ12−cQϖ22+cQϖ32−cQϖ42+cQϖ52−cQϖ62
这里
M
A
,
T
b
M_{A, T}^{b}
MA,Tb
代表由推力空气动力学推力和扭矩生成的力矩，主体部分收到的推力可以由以下表达。
F
A
,
T
b
=
[
0
0
c
T
(
ϖ
1
2
+
ϖ
2
2
+
ϖ
3
2
+
ϖ
4
2
+
ϖ
5
2
+
ϖ
6
2
)
]
\mathrm{F}_{A, T}^{b}=\left[\begin{array}{c} 0 \\ 0 \\ c_{T}\left(\varpi_{1}^{2}+\varpi_{2}^{2}+\varpi_{3}^{2}+\varpi_{4}^{2}+\varpi_{5}^{2}+\varpi_{6}^{2}\right) \end{array}\right]
FA,Tb=
00cT(ϖ12+ϖ22+ϖ32+ϖ42+ϖ52+ϖ62)
其中f代表在基坐标系下，由空气阻力和推力同时作用下的力。
角速度状态方程，其中包含每个角度的状态方程：
b
ω
˙
b
∣
i
b
=
(
J
b
)
−
1
[
M
A
,
T
b
−
Ω
b
∣
i
b
J
b
ω
b
∣
i
b
]
=
[
P
˙
Q
˙
R
˙
]
{ }^{b} \dot{\omega}_{b \mid i}^{b}=\left(J^{b}\right)^{-1}\left[M_{A, T}^{b}-\Omega_{b \mid i}^{b} J^{b} \omega_{b \mid i}^{b}\right]=\left[\begin{array}{c} \dot{P} \\ \dot{Q} \\ \dot{R} \end{array}\right]
bω˙b∣ib=(Jb)−1[MA,Tb−Ωb∣ibJbωb∣ib]=
P˙Q˙R˙
其中：后面一项可以理解为刚体的科氏加速度，推导借鉴这里以下是借鉴推导部分，Ω矩阵将wb叉乘转化为点乘
Ω
b
∣
i
b
=
[
0
−
R
Q
R
0
−
P
−
Q
P
0
]
\Omega_{b \mid i}^{b}=\left[\begin{array}{ccc} 0 & -\mathrm{R} & \mathrm{Q} \\ \mathrm{R} & 0 & -\mathrm{P} \\ -\mathrm{Q} & \mathrm{P} & 0 \end{array}\right]
Ωb∣ib=
0R−Q−R0PQ−P0
这个部分是角速度
ω b ∣ i b = [ P Q R ] \omega_{b \mid i}^{b}=\left[\begin{array}{l} P \\ Q \\ R \end{array}\right] \\ ωb∣ib= PQR
下一个状态方程是欧拉运动方程，描述了欧拉角在惯性系下的运动速率
Φ ˙ = H ( Φ ) ω b ∣ i b = [ ϕ ˙ θ ˙ ψ ˙ ] \dot{\Phi}=H(\Phi) \omega_{b \mid i}^{b}=\left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right]\\ Φ˙=H(Φ)ωb∣ib= ϕ˙θ˙ψ˙
u
b
=
[
1
0
0
0
c
(
ϕ
)
s
(
ϕ
)
0
−
s
(
ϕ
)
c
(
ϕ
)
]
[
c
(
θ
)
0
−
s
(
θ
)
0
1
0
s
(
θ
)
0
c
(
θ
)
]
[
c
(
ψ
)
s
(
ψ
)
0
−
s
(
ψ
)
c
(
ψ
)
0
0
0
1
]
u
i
\mathrm{u}^{b}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \mathrm{c}(\phi) & \mathrm{s}(\phi) \\ 0 & -\mathrm{s}(\phi) & \mathrm{c}(\phi) \end{array}\right]\left[\begin{array}{ccc} \mathrm{c}(\theta) & 0 & -\mathrm{s}(\theta) \\ 0 & 1 & 0 \\ \mathrm{~s}(\theta) & 0 & \mathrm{c}(\theta) \end{array}\right]\left[\begin{array}{ccc} \mathrm{c}(\psi) & \mathrm{s}(\psi) & 0 \\ -\mathrm{s}(\psi) & \mathrm{c}(\psi) & 0 \\ 0 & 0 & 1 \end{array}\right] \mathrm{u}^{i}\\
ub=
1000c(ϕ)−s(ϕ)0s(ϕ)c(ϕ)
c(θ)0 s(θ)010−s(θ)0c(θ)
c(ψ)−s(ψ)0s(ψ)c(ψ)0001
ui
cb是旋转方程相乘得到
C
b
∣
i
=
[
c
(
θ
)
c
(
ψ
)
c
(
θ
)
s
(
ψ
)
−
s
(
θ
)
(
−
c
(
ϕ
)
s
(
ψ
)
+
s
(
ϕ
)
s
(
θ
)
c
(
ψ
)
)
(
c
(
ϕ
)
c
(
ψ
)
+
s
(
ϕ
)
s
(
θ
)
s
(
ψ
)
)
s
(
ϕ
)
c
(
θ
)
(
s
(
ϕ
)
s
(
ψ
)
+
c
(
ϕ
)
s
(
θ
)
c
(
ψ
)
)
(
−
s
(
ϕ
)
c
(
ψ
)
+
c
(
ϕ
)
s
(
θ
)
s
(
ψ
)
)
c
(
ϕ
)
c
(
θ
)
]
C_{b \mid i}=\left[\begin{array}{ccc} \mathrm{c}(\theta) \mathrm{c}(\psi) & \mathrm{c}(\theta) \mathrm{s}(\psi) & -\mathrm{s}(\theta) \\ (-\mathrm{c}(\phi) \mathrm{s}(\psi)+\mathrm{s}(\phi) \mathrm{s}(\theta) c(\psi)) & (c(\phi) c(\psi)+s(\phi) s(\theta) s(\psi)) & \mathrm{s}(\phi) \mathrm{c}(\theta) \\ (s(\phi) s(\psi)+c(\phi) s(\theta) c(\psi)) & (-s(\phi) c(\psi)+c(\phi) s(\theta) s(\psi)) & \mathrm{c}(\phi) \mathrm{c}(\theta) \end{array}\right] \\
Cb∣i=
c(θ)c(ψ)(−c(ϕ)s(ψ)+s(ϕ)s(θ)c(ψ))(s(ϕ)s(ψ)+c(ϕ)s(θ)c(ψ))c(θ)s(ψ)(c(ϕ)c(ψ)+s(ϕ)s(θ)s(ψ))(−s(ϕ)c(ψ)+c(ϕ)s(θ)s(ψ))−s(θ)s(ϕ)c(θ)c(ϕ)c(θ)
这可以用来解决速度和位置方程
通过引入这个旋转矩阵
角速度可以表达为：
ω b ∣ i b = [ ϕ ˙ 0 0 ] + C ϕ ( [ 0 θ ˙ 0 ] + C θ [ 0 0 ψ ˙ ] ) \omega_{b \mid i}^{b}=\left[\begin{array}{c} \dot{\phi} \\ 0 \\ 0 \end{array}\right]+C_{\phi}\left(\left[\begin{array}{l} 0 \\ \dot{\theta} \\ 0 \end{array}\right]+C_{\theta}\left[\begin{array}{l} 0 \\ 0 \\ \dot{\psi} \end{array}\right]\right) ωb∣ib= ϕ˙00 +Cϕ 0θ˙0 +Cθ 00ψ˙
最后欧拉运动学方程能用如下的的运动学方程来表示
Φ
˙
=
[
ϕ
˙
θ
˙
ψ
˙
]
=
[
1
t
(
θ
)
s
(
ϕ
)
t
(
θ
)
c
(
ϕ
)
0
c
(
ϕ
)
−
s
(
ϕ
)
0
s
(
ϕ
)
/
c
(
θ
)
c
(
ϕ
)
/
c
(
θ
)
]
[
P
Q
R
]
=
H
(
Φ
)
ω
b
∣
i
b
\dot{\Phi}=\left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right]=\left[\begin{array}{ccc} 1 & t(\theta) s(\phi) & t(\theta) c(\phi) \\ 0 & c(\phi) & -s(\phi) \\ 0 & s(\phi) / c(\theta) & c(\phi) / c(\theta) \end{array}\right]\left[\begin{array}{l} P \\ Q \\ R \end{array}\right]=H(\Phi) \omega_{b \mid i}^{b}
Φ˙=
ϕ˙θ˙ψ˙
=
100t(θ)s(ϕ)c(ϕ)s(ϕ)/c(θ)t(θ)c(ϕ)−s(ϕ)c(ϕ)/c(θ)
PQR
=H(Φ)ωb∣ib
虽然这种方法很有效，但是有一个缺陷，当θ等于正负90度时，有奇异点出现，这样当飞行器的的pitch角度达到正负90时候，精确性就要打折了，
但是对于保守的设计来说够了。
解决方法：
使用四元数来模拟就可以解决
下一个是速度状态方程
b
V
˙
C
M
∣
i
b
=
(
1
m
)
F
A
,
T
b
+
g
b
−
Ω
b
∣
i
b
ω
C
M
∣
i
b
=
[
U
˙
V
˙
W
˙
]
{ }^{b} \dot{\mathrm{V}}_{C M \mid i}^{b}=\left(\frac{1}{m}\right) \mathrm{F}_{A, T}^{b}+g^{b}-\Omega_{b \mid i}^{b} \omega_{C M \mid i}^{b}=\left[\begin{array}{c} \dot{U} \\ \dot{V} \\ \dot{W} \end{array}\right]
bV˙CM∣ib=(m1)FA,Tb+gb−Ωb∣ibωCM∣ib=
U˙V˙W˙
考虑施加在主体质心的加速度和力
这里开头是线性加速度实在对应惯性系下的线性加速度，变量m是总质量，gb是转换以后的重力加速度，使用这个可以计算出xyz方向的加速度
最后一个状态方程是位置状态方程，描述了无人机质心的线性速度
i
P
˙
C
M
∣
i
i
=
C
i
∣
b
V
C
M
∣
i
b
=
[
X
˙
Y
˙
Z
˙
]
i \dot{\mathrm{P}}_{C M \mid i}^{i}=C_{i \mid b} \mathrm{~V}_{C M \mid i}^{b}=\left[\begin{array}{c} \dot{X} \\ \dot{Y} \\ \dot{Z} \end{array}\right]
iP˙CM∣ii=Ci∣b VCM∣ib=
X˙Y˙Z˙
这里v代表无人机的速度由c来转化以后的速度，让我们得到无人机的速度。
在工程中我们需要修改电机的数量，初试条件的举矩阵等，本着不多事的原则，我只修改了电机数量，内外反馈没有进行改动。
这是修改内容
整体改了六个旋翼，改了控制器和输入输出状态，反馈回路没有修改。
文件链接：私聊联系我，不要直接拍。
【闲鱼】https://m.tb.cn/h.Utxt7WY?tk=8mCvdOGY6Ck CZ0001 「我在闲鱼发布了【六旋翼SIMULINK仿真模型PID】」
点击链接直接打开
