四旋翼无人飞行器的飞行控制方法与流程

技术特征：

1.一种四旋翼无人飞行器的飞行控制方法，包括以下步骤：

S10：建立四旋翼飞行器的动力学模型，QUAV的动力学方程为；

其中，设θ、ψ分别为四旋翼无人飞行器的翻滚角、俯仰角和偏航角，l为其质心至旋翼中心的距离，Ix、Iy、Iz为惯性主距，Ωi为第i个旋翼转速，Fi为第i个旋翼产生的升力，旋翼产生的升力与旋翼传动速度平方成正比，IR为转动惯量，n₁为其升力系数，n₂为反扭矩系数；

为了把四旋翼飞行器的动力学方程转换成四个独立的控制通道,定义四旋翼飞行器的控制输入为

S20：设计基于复合控制的控制方式来控制步骤S10中动力学方程转换而来的四个独立的控制通道，所述四个通道的控制方式分别为高度PID、翻滚ADRC、俯仰ADRC、偏航EACS-PID，经过控制量的转换控制调节四个旋翼的转速来达到姿态控制，具体包括以下方式：

S21:高度通道采用PID控制

采用增量式数字PID控制高度通道，其表达式为：

u(k)＝u(k-1)+Δu(k) (3)

u(k)＝u(k-1)+K_p[e(k)-e(k-1)]+K_ie(k)

+K_d[e(k)-2e(k-1)+e(k-2)] (4)

式中K_p、K_i和K_d为PID控制的控制参数；

S22:偏航通道采用EACS-PID控制

PID控制采用增量式数字PID控制，其表达式为：

$\begin{array}{c}\mathrm{\Δ}u\left(k\right)=K_p\{\[e\left(k\right)-e(k-1)\]+\frac{T}{T_i}e\left(k\right)+\frac{T_d}{T}\[e\left(k\right)\\ -2e(k-1)+e(k-2)\]\}\end{array}---\left(5\right)$

其中K_p为比例系数，e(k)为本次偏差，Δu(k)对应的控制量为u(k)，T为采样周期，T_i为积分时间常数，T_d为微分时间常数，PID控制需要确定的参数分别为K_p、T_i和T_d；

采用(6)式评价控制系统的性能指标：

$\mathrm{\η}={\mathrm{DT}}^2\underset{i=1}{\overset{LP}{\Σ}}i\left|e\left(i\right)\right|---\left(6\right)$

式中LP为仿真计算点数，DT为仿真计算步骤；

式子(6)连续形式为式(7)：

$\mathrm{\η}={\∫}_0^{\mathrm{\∞}}t\left|e\left(t\right)\right|dt---\left(7\right)$

设蚂蚁总数为m，对于每一个蚂蚁，此时刻的点为i，其对应函数值为η_i，下一个可达的点j，对应函数值为η_j：

${\mathrm{\Δ\η}}_{ij}={\mathrm{\η}}_i-{\mathrm{\η}}_j,\∀i,j---\left(8\right)$

蚂蚁会朝着信息素最多的方向移动，当没有信息素时，便会按照原来的运动方向移动，则蚂蚁在t时刻处于i点时朝着j点移动的概率为：

$p_{ij}\left(t\right)=\frac{{{\mathrm{\τ}}_{ij}}^{\mathrm{\α}}\left(t\right){{\mathrm{\Δ\η}}_{ij}}^{\mathrm{\β}}\left(t\right)}{\underset{l\∈allowed}{\Σ}{{\mathrm{\τ}}_{il}}^{\mathrm{\α}}\left(t\right){{\mathrm{\Δ\η}}_{il}}^{\mathrm{\β}}\left(t\right)},j\∈allowed---\left(9\right)$

式中allowed为蚂蚁从地点i直接到达下一个路径点的集合，α为信息素的相对重要程度，β为距离信息的相对重要程度；当α＝0，最靠近i的地点j将有可能被选出，这类似于随机贪心算法，当β＝0，则蚂蚁只受到信息素的影响而忽略了启发式信息带来的偏向性；如果蚂蚁移动方向有障碍物时，则随机选择其他的方向，如果有信息素指引时，则按照其指引行动；寻优时蚂蚁的移动概率根据式(9)来确定，若Δη_ij(t)＜0表示蚂蚁在自身所在地点i的邻域搜索，感知并且行动；若Δη_ij(t)＞0表示蚂蚁按照移动概率从其自身所在地点i的邻域移动至点j的邻域；

局部信息素更新策略为：

τ_ij(t)＝(1-ξ)τ_ij(t-1)+ξτ₀ (10)

其中ξ∈(0,1)，τ₀为信息素初始值；

局部信息素更新策略的作用在于：蚂蚁经过路径(i,j)，该路径的信息素τ_ij将会减少，从而其他蚂蚁再选中此路径的概率减少；

全局信息素更新策略为：

${\mathrm{\τ}}_{ij}\left(t\right)=(1-\mathrm{\ρ}){\mathrm{\τ}}_{ij}(t-1)+\underset{k=1}{\overset{m}{\Σ}}{{\mathrm{\Δ\τ}}_{ij}}^k+{{\mathrm{\λ\Δ\τ}}_{ij}}^{best}$

其中ρ是信息素挥发参数，Δτ_ij^best为精英蚂蚁给路径(i,j)增加的信息素；

路径的构建：

位于节点i的蚂蚁k在每次选择下一个路径前会产生一个随机数q，那么从节点i到节点j的移动规则为：

$p^{\′}=\left\{\begin{array}{cc}\underset{j\∈allowed}{\arg \max }\left\{{\[{\mathrm{\τ}}_{ij}\]}^{\mathrm{\α}}{\[{\mathrm{\η}}_{ij}\]}^{\mathrm{\β}}\right\},& q\≤q_0\\ p_{ij},& q\>q_0\end{array}\right.---\left(12\right)$

q是随机变量，均匀分布在区间[0,1]中，q₀∈[0,1],

通过以上EACS算法找到PID控制参数最优解；

S23翻滚通道和俯仰通道采用ADRC，设定翻滚角θ和俯仰角Φ的值分别为θ_d、Φ_d，先以翻滚角θ_d计算翻滚通道值，具体算法为：

S231)安排过渡过程(TD)：

安排过渡过程如以下公式(13)

$\left\{\begin{array}{c}{v}_1(k+1)=v_1\left(k\right)+{\mathrm{Hv}}_2\left(k\right)\\ v_2(k+1)=v_2\left(k\right)+Hfst\left(v_1\right(k)-\\ {\mathrm{\θ}}_d\left(k\right),v_2\left(k\right),r,h)\end{array}\right.---\left(13\right)$

式中r为速度因子，h为滤波因子，H为积分步长，其中h＝3H～7H；

其中设fst(·)为u＝fst(v₁,v₂,r,h)：

$\left\{\begin{array}{c}d=rh\\ d_0=dh={\mathrm{rh}}^2\\ y=v_1+{\mathrm{hv}}_2\\ a_0={(d^2+8r|y\left|\right)}^{\frac{1}{2}}\end{array}\right.---\left(14\right)$

$a=\left\{\begin{array}{cc}{v}_2+\frac{(a_0-d)}{2},& \left|y\right|\>d_0\\ v_2+\frac{y}{h},& \left|y\right|\≤d_0\end{array}\right.---\left(15\right)$

$fst(v_1,v_2,r,h)=\left\{\begin{array}{cc}-\frac{ra}{d},& \left|a\right|\≤d\\ -r\mathrm{sgn}\left(a\right),& \left|a\right|\>d\end{array}\right.---\left(16\right)$

S232)状态估计和总扰动(ESO)：

$\left\{\begin{array}{c}{e}_0=z_1\left(k\right)-y\left(k\right)\\ z_1(k+1)=z_1\left(k\right)+H\left(z_2\right(k)-{\mathrm{\λ}}_1{e}_0)\\ z_2(k+1)=z_2\left(k\right)+H\left(z_3\right(k)-\\ {\mathrm{\λ}}_{2}fal(e_0,{\mathrm{\α}}_1,\mathrm{\δ})+bu\left(k\right))\\ z_3(k+1)=z_3\left(k\right)-{\mathrm{H\λ}}_{3}fal(e_0,{\mathrm{\α}}_2,\mathrm{\δ})\end{array}\right.---\left(17\right)$

取α₁＝0.5，α₂＝0.25，λ₁、λ₂、λ₃为观测器参数；

S233)控制量的形成(NLSEF)：

$\left\{\begin{array}{c}{e}_1=v_1\left(k\right)-z_1\left(k\right)\\ e_2=v_2\left(k\right)-z_2\left(k\right)\\ u_0=k_{1}fal(e_1,{\mathrm{\α}}_1,\mathrm{\δ})+k_{2}fal(e_2,{\mathrm{\α}}_2,\mathrm{\δ})\\ u\left(k\right)=u_0-\frac{z_3\left(k\right)}{b}\end{array}\right.---\left(18\right)$

其中

$fal(e,\mathrm{\α},\mathrm{\δ})=\left\{\begin{array}{cc}|e{|}^{\mathrm{\α}}\mathrm{sgn}\left(e\right),& \left|e\right|\>\mathrm{\δ}\\ \frac{e}{{\mathrm{\δ}}^{1-\mathrm{\α}}},& \left|e\right|\≤\mathrm{\δ}\end{array}\right.---\left(19\right)$

上式中δ＞0；

在NLSEF中，取α₁＝0.75，α₂＝1.25或者α₁＝0.5，α₂＝1.5，δ的取值与ESO相同；k₁、k₂为控制器增益系数；b为系统的补偿系数；

另外再以俯仰角Φ计算俯仰通道值，即将翻滚角θ_d替换为俯仰角Φ_d并重复步骤S231、S232和S233。