[发明专利]一种可拼组飞行器的控制方法

权利要求书

1.一种可拼组飞行器的控制方法，其特征是，所述方法根据飞行器传感器采集的数据，计算出飞行器的姿态信息和位置信息，由水平位置控制器根据飞行器期望水平位置与当前水平位置的差计算出目标欧拉角，姿态控制器根据期望欧拉角和当前欧拉角的差计算出所需扭矩；高度控制器根据期望高度与当前高度的差计算出误差推力，并根据期望加速度与当前加速度的差计算出悬停推力，悬停推力与误差推力求和，得到高度控制的总推力，将扭矩和总推力输入到动力系统，实现对飞行器水平位置和高度的控制；

所述高度控制器的计算方法如下：

①计算误差推力：期望高度与当前高度的差通过比例环节形成期望垂直速度，期望垂直速度与飞行器当前垂直速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，形成误差推力；

②计算悬停推力：期望加速度与当前加速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到输出推力增量，推力增量与前一时刻的悬停推力相加，形成悬停推力；期望加速度可以设定为0；

③计算总推力：悬停推力与误差推力求和，得到高度控制的总推力，总推力输入到动力系统，实现对飞行高度的控制。

2.根据权利要求1所述的一种可拼组飞行器的控制方法，其特征是，所述水平位置控制器的计算方法如下：

从遥控器或上位机获取的飞行器期望水平位置与飞行器当前水平位置的差，经比例环节处理后得到期望水平速度，期望水平速度与当前水平速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到期望水平加速度，最后由姿态矩阵将期望水平加速度转换为目标欧拉角，即期望滚转角φ，期望俯仰角θ和期望航向角ψ。

3.根据权利要求1所述的一种可拼组飞行器的控制方法，其特征是，所述姿态控制器的计算方法如下：

位置控制器输出的期望欧拉角与当前欧拉角的差通过比例环节形成期望角速度，期望角速度与当前角速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到期望角加速度，期望角加速度与转动惯量相乘得到所需扭矩，从而控制飞行器转动达到期望角度，进而达到期望位置。

4.根据权利要求3所述的一种可拼组飞行器的控制方法，其特征是，根据飞行器传感器采集的数据计算飞行器的姿态信息和位置信息的具体步骤如下：

a.在建立机体坐标系和导航坐标系的基础上确定核心融合计算算法：

所述核心融合计算算法包括状态空间模型建立和计算流程两部分：

Ⅰ.建立状态空间模型

构建离散系统状态空间方程

x_k＝Φ_k|k-1x_k-1+Bu_k-1+Γ_k|k-1w_k-1

z_k＝H_kx_k+v_k

式中x_k为k时刻系统状态，x_k-1表示k-1时刻系统状态，Φ_k|k-1为系统状态空间转移矩阵，B为控制输入的系数矩阵，u_k-1为k-1时刻的系统输入量，Γ_k|k-1为系统噪声系数，w_k-1为系统噪声，z_k表示由k时刻估计出来的观测量，H_k为观测矩阵，v_k为观测噪声，v_k为k时刻的观测噪声；

Ⅱ.计算流程

包以下五个公式：

①状态预测：

式中为k-1时刻对k时刻估计的系统状态，表示k-1时刻的系统状态最优估计；

②协方差预测：

式中P_k-1|k-1为k-1时刻系统状态协方差的最优估计，P_k|k-1为k-1时刻对k时刻的状态协方差的估计，为Φ_k|k-1的转置，Q_k-1为系统状态的系统噪声；

③卡尔曼滤波增益

式中K_k为卡尔曼增益矩阵，R_k为观测噪声；

④状态估计校正：

式中为k时刻系统状态的最优估计，z_k为观测量，为k-1时刻由系统状态对观测量的估计；

⑤协方差估计校正：

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

式中P_k|k为k时刻协方差的最优估计；

b.姿态融合解算：

①陀螺仪数据处理

读取陀螺仪测量的机体角速度ω_m＝[ω_mx ω_my ω_mz]^T，式中三个元素分别表示飞行器绕机体坐标系的x，y，z轴旋转的角速度测量值，用ω_b＝[ω_bx ω_by ω_bz]^T,表示陀螺仪的偏移量，式中三个元素表示绕x，y，z轴的角速度的偏移量，设真实角速度为ω＝[ω_x ω_y ω_z]^T，三个元素表示绕机体坐标x，y，z轴的真实角速度，则陀螺仪测量模型为

ω＝ω_m-ω_b

即，

②读取加速度计测量的比力数据a_b＝[a_x a_y a_z]^T，式中三个元素分别表示机体坐标系下对应x，y，z方向的比力；

③读取磁罗盘测量的地球磁感应强度m_b＝[m_x m_y m_z]^T,三个元素分别表示机体坐标系下对应x，y，z磁感应强度，用m_h＝[hx hy hz]^T表示地磁坐标系下三个轴向的磁感应强度，则有：

其中

表示从n坐标系到b坐标系的旋转矩阵；

④建立姿态解算模型的状态空间方程：

x＝[q ω_b]^T＝[q0 q1 q2 q3 ω_bx ω_by ω_bz]^T

式中q＝[q0 q1 q2 q3]^T为四元数，由此求出状态空间转移矩阵：

式中I_7×7表示7行7列的单位矩阵；

系统输入参数矩阵为：

系统噪声系数矩阵为：

Γ_k|k-1＝diag{1,1,1,1,1,1,1}

系统噪声对应为四元数噪声，角速度偏移量的系统噪声，分别设定为10^-7和10^-11，

Q_k-1＝diag{10^-7,10^-7,10^-7,10^-7,10^-11,10^-11,10^-11}

观测量z＝[a_x a_y a_zm_x m_y m_z]^T，观测噪声为：

R_k＝diag{0.032，0.032，0.032，0.04，0.04，0.04}

观测矩阵为：

其中bx，bz表示地磁向量在水平面的分量和垂直方向的分量：

将上述姿态解算模型的状态空间方程代入计算流程给出的五个公式，得到姿态四元数，再由下式计算出姿态角度：

其中表示滚转角度，θ表示俯仰角度，ψ表示航向角度，表示的第i行第j列元素，其中i，j∈1,2,3，从而得到飞行器的姿态信息；姿态角度为当前欧拉角；

c.位置融合解算

①光流的速度信息转化

光流传感器读取到的速度数据记为[V_X V_Y]^T，则飞行器速度在北向和东向的投影V_N，V_E为：

②GPS测量的大地坐标转化

设导航坐标系原点的GPS坐标为[H_lat H_lon H_alt]^T，三个元素分别表示原点的维度、经度、海拔高度，飞行器的GPS坐标为[P_lat P_lon P_alt]^T，三个元素分别表示飞行器GPS测到的维度、经度、海拔高度，则导航坐标系下飞行器的空间位置为

其中c＝arcos(sin(H_lat)sin(P_lat)+cos(H_lat)cos(P_lat)cos(P_lon-H_lon))，R为地球半径；

③加速度坐标转化

利用下式将加速度计测量的比力数据a_b＝[a_x a_y a_z]^T转化为导航坐标系下的加速度：

式中A＝[A_N A_E A_D]^T为导航坐标系下的加速度，其中的三个元素分别表示北、东、地方向的加速度，A_B＝[A_BN A_BE A_BD]^T为导航坐标系下的加速度偏移量，g为重力加速度；

④位置融合解算建模

首先建立北向位置状态空间模型：

系统状态选择位置P_N，速度V_N，加速度偏移A_BN为状态量，系统输入为加速度A_N，观测量为GPS测量值转换的北向位置GPS_P_N，光流测量转换的速度FLOW_V_N，则北向位置状态空间模型如下：

x＝[P_N V_N A_BN]^T

u_k-1＝A_N

Γ_k,k-1＝I_3×3

Q_k-1＝diag{2.5×10^-6，5×10^-4，0.05}

Z_k＝[GPS_P_N FLOW_V_N]

将北向位置状态空间模型代入计算流程给出的五个公式，得到飞行器北向的实际位置、速度和加速度；

用同样的方法求得飞行器在东向和竖直向下方向的实际位置、速度和加速度，从而得到飞行器的所有位置信息。