建立无人机系统如上图所示。无人机的姿态角与四个电机有如下关系：
ϕ = + M 1 + M 2 − M 3 − M 4 (roll) \phi = + M_1 + M_2 - M_3 - M_4 \tag{roll} ϕ=+M1+M2−M3−M4(roll) θ = − M 1 + M 2 + M 3 − M 4 (pitch) \theta = - M_1 + M_2 + M_3 - M_4 \tag{pitch} θ=−M1+M2+M3−M4(pitch) ψ = − M 1 + M 2 − M 3 + M 4 (yaw) \psi = - M_1 + M_2 - M_3 + M_4 \tag{yaw} ψ=−M1+M2−M3+M4(yaw)
写成矩阵的形式为：
[
ϕ
θ
ψ
]
=
[
+
1
+
1
−
1
−
1
−
1
+
1
+
1
−
1
−
1
+
1
−
1
+
1
]
[
M
1
M
2
M
3
M
4
]
\left[\begin{matrix} \phi \\ \theta \\ \psi \\ \end{matrix}\right]= \left[\begin{matrix} +1 & +1 & -1 & -1 \\ -1 & +1 & +1 & -1 \\ -1 & +1 & -1 & +1 \\ \end{matrix}\right] \left[\begin{matrix} M_1 \\ M_2 \\ M_3 \\ M_4 \\ \end{matrix}\right]
⎣
⎡ϕθψ⎦
⎤=⎣
⎡+1−1−1+1+1+1−1+1−1−1−1+1⎦
⎤⎣
⎡M1M2M3M4⎦
⎤
利用矩阵求逆运算，转换一下，有如下关系：
[
M
1
M
2
M
3
M
4
]
=
[
+
1
−
1
−
1
+
1
+
1
+
1
−
1
+
1
−
1
−
1
−
1
+
1
]
[
ϕ
θ
ψ
]
\left[\begin{matrix} M_1 \\ M_2 \\ M_3 \\ M_4 \\ \end{matrix}\right]= \left[\begin{matrix} +1 & -1 & -1 \\ +1 & +1 & +1 \\ -1 & +1 & -1 \\ -1 & -1 & +1 \\ \end{matrix}\right] \left[\begin{matrix} \phi \\ \theta \\ \psi \\ \end{matrix}\right]
⎣
⎡M1M2M3M4⎦
⎤=⎣
⎡+1+1−1−1−1+1+1−1−1+1−1+1⎦
⎤⎣
⎡ϕθψ⎦
⎤
展开后可以得到：
M
1
=
+
ϕ
−
θ
−
ψ
(M1)
M_1 = +\phi - \theta - \psi \tag{M1}
M1=+ϕ−θ−ψ(M1)
M
2
=
+
ϕ
+
θ
+
ψ
(M2)
M_2 = +\phi + \theta + \psi \tag{M2}
M2=+ϕ+θ+ψ(M2)
M
3
=
−
ϕ
+
θ
−
ψ
(M3)
M_3 = -\phi + \theta - \psi \tag{M3}
M3=−ϕ+θ−ψ(M3)
M
4
=
−
ϕ
−
θ
+
ψ
(M4)
M_4 = -\phi - \theta + \psi \tag{M4}
M4=−ϕ−θ+ψ(M4)
扫一扫
508
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
