CN110941285A - 一种基于双ip核的无人机飞行控制*** - Google Patents

Abstract

本发明公开了一种基于双IP核的无人机飞行控制***，包括：姿态传感器模组，用于采集飞行器的姿态数据；制导传感器模组，用于采集飞行器的制导数据和捷联惯导数据；制导控制IP核模块，用于采用四元数扩展卡尔曼滤波算法对姿态数据进行计算得到飞行器的姿态信息，以及采用扩展卡尔曼滤波算法对制导数据和捷联惯导数据进行计算得到飞行器的导航信息：飞行控制IP核模块，用于接收第三方发送或预先设置的飞行控制信息，采用模糊自适应PID控制算法对飞行控制信息、姿态信息和导航信息进行计算得到伺服控制信号；伺服机构，用于根据伺服控制信号驱动飞行器的动力源，以完成对飞行器的飞行控制。本发明能够提高指令处理速度，减少逻辑资源占用。

Description

一种基于双IP核的无人机飞行控制***

技术领域

本发明涉及飞行控制技术领域，特别是涉及一种基于双IP核的无人机飞行 控制***。

背景技术

目前大部分无人机是以单片机或DSP芯片作为飞行控制的主控芯片。基于 单片机的飞控***具有***稳定、价格低廉、开发简单等特点，但由于单片机 运算能力、实时性和***资源有限，对一些性能、实时性要求较高的无人机系 统，在完成较复杂的无人机控制时就略显能力不足。DSP芯片具有集成度高、稳 定性好、精度高、嵌入性好等特点，特别适合在处理速度和精度要求较高的无 人机飞行控制***上，但由于DSP芯片特殊的功能，用DSP芯片同时进行运算 和控制工作时，可能达不到***控制要求。目前主流的方案是选用控制能力较 强的单片机作为主控器，DSP芯片作为辅控器负责数据处理，两者组合实现优势 互补，从而解决单DSP芯片不能胜任处理和设备控制两种任务的弊端。然而， 该方案虽然能最大程度发挥硬件的优势，性能强大，但较为复杂，研发周期长。

发明内容

本发明主要解决的技术问题是提供一种基于双IP核的无人机飞行控制***， 能够提高指令处理速度，减少逻辑资源占用。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种基于双IP 核的无人机飞行控制***，包括姿态传感器模组、制导传感器模组、制导控制 IP核模块、飞行控制IP核模块和伺服机构；所述姿态传感器模组用于采集飞行 器的姿态数据，并将所述姿态数据传输至制导控制IP核模块；所述制导传感器 模组用于采集飞行器的制导数据和捷联惯导数据，并将所述制导数据和捷联惯 导数据传输至制导控制IP核模块；所述制导控制IP核模块用于采用四元数扩 展卡尔曼滤波算法对所述姿态数据进行计算得到飞行器的姿态信息，以及采用 扩展卡尔曼滤波算法对所述制导数据和捷联惯导数据进行计算得到飞行器的导 航信息，并将所述姿态信息和导航信息传输至飞行控制IP核模块：所述飞行控 制IP核模块用于接收第三方发送或预先设置的飞行控制信息，采用模糊自适应 PID控制算法对所述飞行控制信息、姿态信息和导航信息进行计算得到伺服控制 信号，并将所述伺服控制信号传输至伺服机构；所述伺服机构用于根据所述伺 服控制信号驱动飞行器的动力源，以完成对飞行器的飞行控制。

作为本发明的一个优选实施例，所述姿态传感器模组包括三轴陀螺仪、三 轴加速度计和三轴磁阻仪，所述姿态信息包括俯仰角、滚转角和航向角。

作为本发明的一个优选实施例，所述制导传感器模组包括MEMS传感器、磁 强计、气压高度计、空速计、GPS接收机和电压/电流计，所述导航信息包括经 纬度、高度、速度和航向。

作为本发明的一个优选实施例，所述飞行器为四旋翼飞行器，所述动力源 为无刷直流电机。

作为本发明的一个优选实施例，所述制导控制IP核模块、飞行控制IP核 模块通过Qsys技术集成在FPGA芯片中。

区别于现有技术的情况，本发明的有益效果是：通过传感器采集得到相应 的姿态数据、制导数据和捷联惯导数据，然后导航控制IP核模块采用四元数扩 展卡尔曼滤波算法对姿态数据进行计算得到飞行器的姿态信息，以及采用扩展 卡尔曼滤波算法对制导数据和捷联惯导数据进行计算得到飞行器的导航信息， 同时飞行控制IP核模块采用模糊自适应PID控制算法对飞行控制信息、姿态信 息和导航信息进行计算得到伺服控制信号，伺服机构根据伺服控制信号驱动飞 行器的动力源，以完成对飞行器的飞行控制，从而能够提高指令处理速度，减 少逻辑资源占用，具有***集成度高、鲁棒性好、重构性强等优点，具有一定 的应用价值和实际意义。

附图说明

图1是本发明实施例的基于双IP核的无人机飞行控制***的原理框图。

图2是参数Kp，Ki，Kd在不同e和ec时满足的调整规则示意图。

图3是在Quartus II 13.0软件平台中对制导控制IP核进行编译与仿真的 仿真时序图。

图4是在Quartus II 13.0软件平台中对飞行控制IP核进行编译与仿真的 仿真时序图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清 楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是 全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造 性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参阅图1，是本发明实施例的基于双IP核的无人机飞行控制***的原理框 图。本实施例的基于双IP核的无人机飞行控制***包括姿态传感器模组10、制 导传感器模组20、制导控制IP核模块30、飞行控制IP核模块40和伺服机构 50。

姿态传感器模组10用于采集飞行器的姿态数据，并将姿态数据传输至制导 控制IP核模块30。在本实施例中，姿态传感器模组10包括三轴陀螺仪、三轴 加速度计和三轴磁阻仪。

制导传感器模组20用于采集飞行器的制导数据和捷联惯导数据，并将制导 数据和捷联惯导数据传输至制导控制IP核模块30。在本实施例中，制导传感器 模组20包括MEMS传感器、磁强计、气压高度计、空速计、GPS接收机和电压/ 电流计。

制导控制IP核模块30用于采用四元数扩展卡尔曼滤波算法对姿态数据进 行计算得到飞行器的姿态信息，以及采用扩展卡尔曼滤波算法对制导数据和捷 联惯导数据进行计算得到飞行器的导航信息，并将姿态信息和导航信息传输至 飞行控制IP核模块40。在本实施例中，姿态信息包括俯仰角、滚转角和航向角 导航信息包括经纬度、高度、速度和航向。

飞行控制IP核模块40用于接收第三方发送或预先设置的飞行控制信息， 采用模糊自适应PID控制算法对飞行控制信息、姿态信息和导航信息进行计算 得到伺服控制信号，并将伺服控制信号传输至伺服机构50。制导控制IP核模块 30、飞行控制IP核模块40可以通过Qsys技术集成在FPGA芯片中。FPGA芯片 相比于传统单片机和DSP芯片用于飞行控制***，有着更快的指令处理速度和 更少的逻辑资源占用，且***集成度高，鲁棒性好，重构性强，具有一定的应 用价值和实际意义。

伺服机构50用于根据伺服控制信号驱动飞行器的动力源，以完成对飞行器 的飞行控制。在本实施例中，飞行器为四旋翼飞行器，动力源为无刷直流电机。

卡尔曼滤波器(Kalman Filter，KF)是基于线性***的状态最优估计，然 而飞行控制***是一个非线性***，因此本发明采用非线性的扩展卡尔曼滤波 器(ExtenedKalman Filter，EKF)。扩展卡尔曼滤波器是在卡尔曼滤波器的基 础上，利用泰勒级数方法对非线性***的测量方程和状态方程进行展开，只保 留展开式的线性部分，得到的近似于线性的非线性滤波方法。

通常情况下，非线性离散***的状态方程和观测方程可表示为：

式中x(k)和z(k)分别为k时刻***的状态矢量和观测矢量；f(·)和h(·)分别 是n维和m维矢量函数；g[x(k-1),k-1]为噪声分布函数，w(k-1)是过程噪声矢量。

将公式(1)在

处通过泰勒级数法展开，展开式的线性部分保留，可 以得到非线性***近似线性化的状态方程和观测方程：

式中：

根据扩展卡尔曼滤波理论，通过时间更新和量测更新的迭代求得***状态 向量的最优估计，***的扩展卡尔曼滤波递推过程如下：

(1)初始估计

计算状态初始值x(0)，设定过程噪声协方差Q(k)，量测噪声协方差R(k)及误 差方差矩阵P(0)。

(2)时间更新

计算状态转移函数的雅克比矩阵[14]：

由状态方程可得状态量的预测值：

计算误差方差矩阵：

P(k,k-1)＝Φ(k,k-1)P(k,k-1)Φ^T(k,k-1)+G(k-1)Q(k)G^T(k-1) (8)

(3)量测更新

计算量测函数的雅克比矩阵[15]：

由量测函数可得量测向量的预测值：

计算状态增益矩阵：

K(k)＝P(k,k-1)H^T(k)[H(k)P(k,k-1)H^T(k)+R(k)]^-1 (11)

从而k时刻状态向量估计值：

更新状态误差协方差矩阵：

P(k)＝[I-K(k)H(k)]P(k,k-1)[I-K(k)H(k)]^T+K(k)R(k)K^T(k) (13)

2.导航控制IP核组合导航***模型建立

若存在连续非线性***：

x(t)＝f[x(t),w(t)] (14)

那么，根据一阶龙格-库塔法得：

x(k)＝x(k-1)+Tf[x(t),w(t)] (15)

其中，T为***状态更新周期。

本发明将MEMS传感器作为IMU(惯性测量单元)与GPS接收机和气压高度 计的数据进行组合，有效减小SINS(捷联式惯性导航***)单一导航误差，提 高导航精度。

采用MEMS传感器作为惯性测量单元，由于其精度低，自身存在误差，所以 使用陀螺仪无法检测出地球自转角速度，加速度计也无法检测出哥氏加速度， 因此：

式中，a^b为载体坐标系下加速度计测量值向量，

为载体坐标系下加速度偏 误差，

为载体坐标系下加速度计测量噪声向量；ω^b为载体坐标系下陀螺仪测 量值向量，

为载体坐标系下陀螺仪零漂误差，

为载体坐标系下陀螺仪测量 噪声向量。

(1)状态方程

本发明取无人机在导航坐标系下的位置P_n＝[λ,L,h]，线速度V_n＝[V_E,V_N,V_U]，四元数q＝[q₀,q₁,q₂,q₃]和载体坐标系下的陀螺仪零偏误差

作为***状 态变量：

令***噪声向量为

那么，***的状态方程为：

式中有：

对f[x(k-1),k-1]求取Jacobian矩阵：

其中

对g[x(k-1),k-1]求取Jacobian矩阵：

***过程噪声协方差Q(k)为：

其中，

为陀螺仪测量噪声的协方差，

为加速度计测量噪声的协方差，

为陀螺仪零偏噪声协方差。

(2)观测方程

导航位置P_n和导航速度V_n可以通过GPS测量得到，导航高度h可以通过气 压高度计测得，导航方向可以通过磁强计测得在载体坐标系下的磁场强度

得到。

设***观测量为：

其中，

令***测量噪声为

为GPS导航位置测量误差，

为GPS 导航速度测量误差，

为磁强计磁场强度测量误差，v_h为气压高度计高度测量误 差。

那么，***的观测方程为：

对h[x(k),k]求取Jacobian矩阵：

式中：

***量测噪声协方差矩阵R(k)为正定常数对角阵：

其中，

为位置观测噪声，

为速度观测噪声，

为磁强计观测噪声，

为 气压高度计观测噪声。

飞行控制IP核模块40采用模糊自适应PID控制算法，先将测量值y(t)与 期望值r(t)的偏差量e(t)＝r(t)-y(t)和偏差变化率ec(t)＝de(t)/dt作为模糊 控制器的输入，再利用e和ec与Kp，Ki，Kd的模糊关系，依据模糊控制规则 得到校正后的Kp，Ki，Kd值并输出至PID控制器，最后由PID控制器给出被控 对象的输入控制信号u(t)。

模糊自适应PID控制的关键就是要找到PID三个参数Kp，Ki，Kd与输入量 e和ec的模糊关系。通过实时检测e和ec的大小，利用模糊控制器实时调整 Kp，Ki，Kd的大小，以满足在不同的大小e和ec时，PID控制器都有良好的动 态性、精确度和稳定性。根据对飞行器的操作经验和PID控制曲线，参数Kp， Ki，Kd在不同e和ec时满足的调整规则如图2所示。图中：

(1)0A阶段(e>0，ec<0)

这一阶段的特点是：偏差e>0且从大逐渐变小趋于0，偏差率ec<0且|ec| 从0变小再变大。为了获得更好的控制性能，在该阶段采取变增益控制。根据e 和ec的大小变化趋势，将0A阶段细分为OI、IJ和JA三段。

OI段：由于e较大，|ec|从0变大，为加快***响应速度和防止积分饱和， Kp取较大，Kd取较小，Ki取较小或0；

IJ段：e适中，|ec|在最大值附近，为防止***超调，同时为了保证*** 响应速度，Kp.Ki.Kd都不能太大，Kp和Kd取适中，Ki取较小；

JA段：e较小且趋向0，|ec|从大变小，应减小Kp，并增大Ki值，同时为 了防止超调，Kd取中等大小。

(2)AB阶段(e<0，ec<0)

该阶段特点是：偏差e<0且|e|从0逐渐变大，偏差率ec<0且|ec|从大变 小。该阶段目的是快速压低超调，使***稳态误差减小，所以应加强比例和积 分部分控制作用，同时为了使***维持在稳态附近，也应加强微分部分的控制 作用。所以Kp取较大，Ki取较大，Kd取中等大小。

(3)BC阶段(e<0，ec>0)

该阶段特点是：偏差e<0且|e|从大逐渐变小趋于0，偏差率ec>0且|ec| 从0变大。该阶段在向稳态方向趋近，为了使***反向超调减小，应减小或不 加积分作用，可以加大微分部分作为。所以Kp取适中，Ki取较小或为0，Kd取 中等大小。

(4)CD阶段(e>0，ec>0)

该阶段特点是：偏差e>0且|e|从0慢慢变大，偏差率ec>0且|ec|从大变 小趋于0。该阶段***虽然有微小超调，但已经接近稳态，***输出变化很小。 所以Kp取适中，Ki取适中，Kd取适中。

(5)DE阶段(e>0，ec<0)

该阶段特点是：偏差e>0且|e|从大慢慢趋于0，偏差率ec<0且|ec|在0附 近。该阶段偏差很小，且趋于稳态，控制作用不宜太强，所以应降低积分作用， 保持比例和微分作用，使***保持稳定，且有较强的抗干扰能力。所以Kp取适 中，Ki取较小，Kd取适中。

根据上述对不同e和ec时PID参数Kp，Ki，Kd调整原则的分析，对PID 三个参数各发明一模糊控制器来实时调整，设Kp，Ki，Kd与e和ec有如下关 系：

式中，Kp，Ki，Kd是通过模糊控制器整定后的PID参数，Kp0，Ki0，Kd0 是通过临界比例方法获得的PID控制器的初始参数。ΔKp，ΔKi，ΔKd是通过 模糊关系函数fp(e,ec)，fi(e,ec)，.fd(e,ec)得到的与e和ec有关的PID调 整量。ΔKp，ΔKi，ΔKd与E和EC的模糊规则表如表1至3所示。

表1 ΔKp模糊规则表

表2 ΔKi模糊规则表

表3 ΔKd模糊规则表

将***偏差e和偏差变化率ec的变化范围定义为模糊集上的论域为 e,ec＝{-3,-2,-1,0,1,2,3}，其模糊子集E,EC＝{FD,FZ,FX,LO,ZX,ZZ,ZD}，输出 ΔKp，ΔKi，ΔKd论域取[-10,10]，输出模糊子集域输入模糊子集相同。设E， EC和ΔKp，ΔKi，ΔKd均服从正态分布，且隶属度函数相同。

由***测量值与期望值之差得到e和ec，然后根据参数Kp，Ki，Kd在不同 e和ec时满足的调整原则确定的模糊矩阵规则表，利用模糊化后的E和EC查出 修正参数ΔKp，ΔKi，ΔKd，完成PID参数的校正，然后利用已校正参数的PID 控制器完成***控制。

在一个具体应用中，制导控制IP核模块30利用Nios IDE集成开发环境， 采用Verilog HDL语言进行算法代码编辑，并最终通过Qsys组件将其集成为IP 核。该制导控制IP核模块30以Altera DE2的EP2C35F672C6为目标芯片，在 Quartus II 13.0软件平台中进行编译与仿真，仿真时序图如图3所示。

由图3可以看出，当三轴加速度计、三轴陀螺仪、GPS接收机、磁强计和高 度计分别输入数据30045、63348、54147、33965、41253时，由制导控制IP核 模块30计算出来的姿态、速度和位置数据分别为64545.36401和36599，仿真 结果与理论计算结果一致。

飞行控制IP核模块40同样利用Nios IDE集成开发环境，采用Verilog HDL 语言进行算法代码编辑，并最终通过Qsys组件将其集成为IP核。该飞行控制 IP核模块40以AlteraDE2的EP2C35F672C6为目标芯片，在Quartus II 13.0 软件平台中对飞行控制IP核进行编译与仿真，仿真时序图如图4所示。

由图4可以看出，不同的输入量经模糊自适应PID控制算法解算后得到的 输出量是不同的。当输入偏差E为25，偏差变化率EC为-3时，模糊自适应PID 控制器输出的值为-160，这说明当输入偏差为正，偏差变化率为负且较大时， 应减小输出量的输出，故输出量的值为负，且较小。输出量出现了一定延迟， 这是由时序仿真期间本身的信号传输延迟造成的，当输入量改变时，各阶段延 迟不一样，这也符合控制器的实际特性。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利 用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运 用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (5)

1.一种基于双IP核的无人机飞行控制***，其特征在于，包括姿态传感器模组、制导传感器模组、制导控制IP核模块、飞行控制IP核模块和伺服机构；

所述姿态传感器模组用于采集飞行器的姿态数据，并将所述姿态数据传输至制导控制IP核模块；

所述制导传感器模组用于采集飞行器的制导数据和捷联惯导数据，并将所述制导数据和捷联惯导数据传输至制导控制IP核模块；

所述制导控制IP核模块用于采用四元数扩展卡尔曼滤波算法对所述姿态数据进行计算得到飞行器的姿态信息，以及采用扩展卡尔曼滤波算法对所述制导数据和捷联惯导数据进行计算得到飞行器的导航信息，并将所述姿态信息和导航信息传输至飞行控制IP核模块：

所述飞行控制IP核模块用于接收第三方发送或预先设置的飞行控制信息，采用模糊自适应PID控制算法对所述飞行控制信息、姿态信息和导航信息进行计算得到伺服控制信号，并将所述伺服控制信号传输至伺服机构；

所述伺服机构用于根据所述伺服控制信号驱动飞行器的动力源，以完成对飞行器的飞行控制。

2.根据权利要求1所述的基于双IP核的无人机飞行控制***，其特征在于，所述姿态传感器模组包括三轴陀螺仪、三轴加速度计和三轴磁阻仪，所述姿态信息包括俯仰角、滚转角和航向角。

3.根据权利要求2所述的基于双IP核的无人机飞行控制***，其特征在于，所述制导传感器模组包括MEMS传感器、磁强计、气压高度计、空速计、GPS接收机和电压/电流计，所述导航信息包括经纬度、高度、速度和航向。

4.根据权利要求3所述的基于双IP核的无人机飞行控制***，其特征在于，所述飞行器为四旋翼飞行器，所述动力源为无刷直流电机。

5.根据权利要求4所述的基于双IP核的无人机飞行控制***，其特征在于，所述制导控制IP核模块、飞行控制IP核模块通过Qsys技术集成在FPGA芯片中。

Images