四旋翼无人机避障飞行
四旋翼无人机飞行
无人机从起点出发飞向目标点，途径随机生成的障碍物
源码：https://pan.baidu.com/s/1WgEJuvlQmXrNfNmNolwbGg?
基于控制避障函数(CBF)的四旋翼飞行控制
采用航点飞行，利用给定航点规划轨迹，从起点出发，沿着轨迹飞行，同时躲避轨迹上的障碍物
源码： https://pan.baidu.com/s/1u4VasALLQ20dAti_I0Xi5g?
四旋翼无人机轨迹跟踪
建模
平面移动
从 F = m a F=ma F=ma开始：
F a l l = m [ a x a y a z ] F_{a l l}=m\left[\begin{array}{l} a_{x} \\ a_{y} \\ a_{z} \end{array}\right] Fall=m⎣⎡axayaz⎦⎤
加速度a是相对于地理坐标系的，对于四旋翼，主要考虑四个电机的升力 f 1 , f 2 , f 3 , f 4 f_1,f_2,f_3,f_4 f1,f2,f3,f4,同时假设Z轴向上为正，那么有：
[ 0 0 f 1 + f 2 + f 3 + f 4 ] − [ 0 0 m g ] = m [ a x a y a z ] \left[\begin{array}{c}0 \\0 \\f_{1}+f_{2}+f_{3}+f_{4}\end{array}\right]-\left[\begin{array}{c}0 \\0 \\m g\end{array}\right]=m\left[\begin{array}{c}a_{x} \\a_{y} \\a_{z}\end{array}\right] ⎣⎡00f1+f2+f3+f4⎦⎤−⎣⎡00mg⎦⎤=m⎣⎡axayaz⎦⎤
四个电机的升力都在Z轴方向，重力也在Z轴方向（机体坐标系），但具体分析无人机运动时要在地理坐标系进行分析，这样可描述无人机的运动轨迹及其姿态，那么就需要机体坐标系转地理坐标系的旋转矩阵：
T 0 ′ = [ cos ϕ cos θ cos ϕ sin θ sin ψ − sin ϕ cos ψ sin θ cos ϕ cos ψ + sin ϕ sin ψ sin ϕ cos θ cos ϕ cos ψ + sin ϕ sin θ sin ψ sin θ cos ϕ sin ψ − sin ϕ cos ψ − sin θ cos θ sin ϕ cos θ cos ϕ ] T_{0}^{\prime}=\left[\begin{array}{ccc}\cos \phi \cos \theta & \cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi & \sin \theta \cos \phi \cos \psi+\sin \phi \sin \psi \\\sin \phi \cos \theta & \cos \phi \cos \psi+\sin \phi \sin \theta \sin \psi & \sin \theta \cos \phi \sin \psi-\sin \phi \cos \psi \\-\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi\end{array}\right] T0′=⎣⎡cosϕcosθsinϕcosθ−sinθcosϕsinθsinψ−sinϕcosψcosϕcosψ+sinϕsinθsinψcosθsinϕsinθcosϕcosψ+sinϕsinψsinθcosϕsinψ−sinϕcosψcosθcosϕ⎦⎤
俯仰角 θ \theta θ：机体坐标系 O b X b O_{b} X_{b} ObXb 轴与地面坐标系 O e x e y e O_{e} x_{e} y_{e} Oexeye 平面的夹角。
横滚角 ϕ \phi ϕ：机体对称平面与过机体坐标系 O b X b \mathrm{O}_{\mathrm{b}} \mathrm{X}_{\mathrm{b}} ObXb轴铅垂面的夹角。
偏航角 ψ \psi ψ :机体轴 O b X b O_{b} X_{b} ObXb 轴在地面坐标系 O e x e y e O_{e} x_{e} y_{e} Oexeye 平面内的投影与地面坐标系 O e x e O_{e} x_{e} Oexe 轴的夹角。
将机体坐标系转地理坐标系：
T 0 ′ [ 0 0 f 1 + f 2 + f 3 + f 4 ] − [ 0 0 m g ] = m [ a x a y a z ] T_{0}^{\prime}\left[\begin{array}{c}0 \\0 \\f_{1}+f_{2}+f_{3}+f_{4}\end{array}\right]-\left[\begin{array}{c}0 \\0 \\m g\end{array}\right]=m\left[\begin{array}{c}a_{x} \\a_{y} \\a_{z}\end{array}\right] T0′⎣⎡00f1+f2+f3+f4⎦⎤−⎣⎡00mg⎦⎤=m⎣⎡axayaz⎦⎤
写成微分方程的形式有：
m x ¨ = T 0 ′ [ 0 0 u 1 ] − [ 0 0 m g ] m \ddot{x}=T_{0}^{\prime}\left[\begin{array}{c}0 \\0 \\u_{1}\end{array}\right]-\left[\begin{array}{c}0 \\0 \\m g\end{array}\right] mx¨=T0′⎣⎡00u1⎦⎤−⎣⎡00mg⎦⎤
其中 x ¨ \ddot{x} x¨是状态向量，包含 [ x ¨ , y ¨ , z ¨ ] [\ddot{x},\ddot{y},\ddot{z}] [x¨,y¨,z¨],那么有：
{ m x ¨ = ( cos ϕ sin θ cos ψ + sin ϕ sin ψ ) u 1 m y ¨ = ( cos ϕ sin θ sin ψ − sin ϕ cos ψ ) u 1 m z ¨ = ( cos ϕ cos θ ) u 1 − m g \left\{\begin{array}{c}m \ddot{x}=(\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi) u_{1} \\m \ddot{y}=(\cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi) u_{1} \\m \ddot{z}=(\cos \phi \cos \theta) u_{1}-m g\end{array}\right. ⎩⎨⎧mx¨=(cosϕsinθcosψ+sinϕsinψ)u1my¨=(cosϕsinθsinψ−sinϕcosψ)u1mz¨=(cosϕcosθ)u1−mg
旋转过程
力矩与力的大小与作用距离有关，其关系如下：
M = F ∗ l M=F*l M=F∗l
式中 ∗ l *l ∗l* 为旋翼旋转中心到无人机中心的距离。
当无人机进行悬停平稳飞行时，角加速度为0，所以各个电机的力矩为：( M i M_{i} Mi桨叶转矩， M f M_{f} Mf桨叶阻力力矩)
M i = M f = k M ω i 2 M_{i}=M_{f}=k_{M} \omega_{i}^{2} Mi=Mf=kMωi2
M Ψ M_Ψ MΨ为偏航力矩， M θ M_θ Mθ为俯仰力矩， M ϕ M_ϕ Mϕ为滚转力矩，则有：
M = [ M ψ M θ M ϕ ] = [ ∑ i = 1 4 M i ( f 4 − f 2 ) l ( f 3 − f 1 ) l ] M=\left[\begin{array}{c}M_{\psi} \\M_{\theta} \\M_{\phi}\end{array}\right]=\left[\begin{array}{c}\sum_{i=1}^{4} M_{i} \\\left(f_{4}-f_{2}\right) l \\\left(f_{3}-f_{1}\right) l\end{array}\right] M=⎣⎡MψMθMϕ⎦⎤=⎣⎡∑i=14Mi(f4−f2)l(f3−f1)l⎦⎤
假设无人机机体绕质心的惯性矩阵为：
J = [ J x 0 0 0 J y 0 0 0 J z ] J=\left[\begin{array}{ccc}J_{x} & 0 & 0 \\0 & J_{y} & 0 \\0 & 0 & J_{z}\end{array}\right] J=⎣⎡Jx000Jy000Jz⎦⎤
根据转动定理:
{ ϕ ¨ = J y − J z J x θ ˙ ψ ˙ + I M J x ( ω 1 − ω 2 + ω 3 − ω 4 ) θ ˙ + l J x ( f 4 − f 2 ) θ ¨ = J z − J x J x ϕ ˙ ψ ˙ − I M J y ( ω 1 − ω 2 + ω 3 − ω 4 ) ϕ ˙ + l J y ( f 3 − f 1 ) ψ ¨ = J x − J y J z θ ˙ ϕ ˙ + k M J z ( ω 2 2 + ω 4 2 − ω 1 2 − ω 3 2 ) \left\{\begin{array}{c}\ddot{\phi}=\frac{J_{y}-J_{z}}{J_{x}} \dot{\theta} \dot{\psi}+\frac{I_{M}}{J_{x}}\left(\omega_{1}-\omega_{2}+\omega_{3}-\omega_{4}\right) \dot{\theta}+\frac{l}{J_{x}}\left(f_{4}-f_{2}\right) \\\ddot{\theta}=\frac{J_{z}-J_{x}}{J_{x}} \dot{\phi} \dot{\psi}-\frac{I_{M}}{J_{y}}\left(\omega_{1}-\omega_{2}+\omega_{3}-\omega_{4}\right) \dot{\phi}+\frac{l}{J_{y}}\left(f_{3}-f_{1}\right) \\\ddot{\psi}=\frac{J_{x}-J_{y}}{J_{z}} \dot{\theta} \dot{\phi}+\frac{k_{M}}{J_{z}}\left(\omega_{2}^{2}+\omega_{4}^{2}-\omega_{1}^{2}-\omega_{3}^{2}\right)\end{array}\right. ⎩⎪⎨⎪⎧ϕ¨=JxJy−Jzθ˙ψ˙+JxIM(ω1−ω2+ω3−ω4)θ˙+Jxl(f4−f2)θ¨=JxJz−Jxϕ˙ψ˙−JyIM(ω1−ω2+ω3−ω4)ϕ˙+Jyl(f3−f1)ψ¨=JzJx−Jyθ˙ϕ˙+JzkM(ω22+ω42−ω12−ω32)
最终有：
{ x ¨ = 1 ( m u + m k ) ( cos ϕ sin θ cos ψ + sin ϕ sin ψ ) u 1 y ¨ = 1 ( m u + m k ) ( cos ϕ sin θ sin ψ − sin ϕ cos ψ ) u 1 z ¨ = 1 ( m u + m k ) ( cos ϕ cos θ ) u 1 − g ϕ ¨ = J y − J z J x θ ˙ ψ ˙ + I M J x ( ω 1 − ω 2 + ω 3 − ω 4 ) θ ˙ + l J x ( f 4 − f 2 ) θ ¨ = J z − J x J x ϕ ˙ ψ ˙ − I M J y ( ω 1 − ω 2 + ω 3 − ω 4 ) ϕ ˙ + l J y ( f 3 − f 1 ) ψ ¨ = J x − J y J z θ ˙ ϕ ˙ + k M J z ( ω 2 2 + ω 4 2 − ω 1 2 − ω 3 2 ) \left\{\begin{array}{l}\ddot{x}=\frac{1}{\left(m_{u}+m_{k}\right)}(\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi) u_{1} \\\ddot{y}=\frac{1}{\left(m_{u}+m_{k}\right)}(\cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi) u_{1} \\\ddot{z}=\frac{1}{\left(m_{u}+m_{k}\right)}(\cos \phi \cos \theta) u_{1}-g \\\ddot{\phi}=\frac{J_{y}-J_{z}}{J_{x}} \dot{\theta} \dot{\psi}+\frac{I_{M}}{J_{x}}\left(\omega_{1}-\omega_{2}+\omega_{3}-\omega_{4}\right) \dot{\theta}+\frac{l}{J_{x}}\left(f_{4}-f_{2}\right) \\\ddot{\theta}=\frac{J_{z}-J_{x}}{J_{x}} \dot{\phi} \dot{\psi}-\frac{I_{M}}{J_{y}}\left(\omega_{1}-\omega_{2}+\omega_{3}-\omega_{4}\right) \dot{\phi}+\frac{l}{J_{y}}\left(f_{3}-f_{1}\right) \\\ddot{\psi}=\frac{J_{x}-J_{y}}{J_{z}} \dot{\theta} \dot{\phi}+\frac{k_{M}}{J_{z}}\left(\omega_{2}^{2}+\omega_{4}^{2}-\omega_{1}^{2}-\omega_{3}^{2}\right)\end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧x¨=(mu+mk)1(cosϕsinθcosψ+sinϕsinψ)u1y¨=(mu+mk)1(cosϕsinθsinψ−sinϕcosψ)u1z¨=(mu+mk)1(cosϕcosθ)u1−gϕ¨=JxJy−Jzθ˙ψ˙+JxIM(ω1−ω2+ω3−ω4)θ˙+Jxl(f4−f2)θ¨=JxJz−Jxϕ˙ψ˙−JyIM(ω1−ω2+ω3−ω4)ϕ˙+Jyl(f3−f1)ψ¨=JzJx−Jyθ˙ϕ˙+JzkM(ω22+ω42−ω12−ω32)