倾转旋翼机飞控程序设计

type

status

date

slug

summary

第一章飞行性能

1、控制方案

旋翼模式下，姿态控制靠两个倾转电机、两个倾转舵机和尾部的俯仰控制电机实现。横滚方向靠电机的差速控制，俯仰方向靠俯仰电机控制，偏航靠两个倾转舵机往两个相反方向的摆动控制。

固定翼模式下，姿态靠两个电机、两个倾转舵机、一个水平尾翼控制。横滚方向靠两个倾转舵机往两个相反方向的摆动控制，俯仰方向靠水平尾翼控制，偏航方向靠电机的差速控制。

2、电机舱摆动过程中的进动和章动考虑

电机舱的前后运动将会受到左右方向的力，这种力在陀螺中表现为进动和章动，而电机舱由于左右方向被固定，因此前后方向的力矩与螺旋桨是否转动无关。但同时要注意左右方向的结构强度。

进动方向分析如图。

为力矩。

为角速度。

，与同向。

以飞机的左电机舱为例，设螺旋桨逆时针旋转，电机舱向前摆动时，角速度方向向上，角速度变化量方向向前，力矩的方向也就向前，力矩=力臂力，力臂方向向上，力方向向右，由电机舱提供，因此电机舱会受到向左的反作用力。

第二章算法设计

1、PID控制器

电机控制的采样频率为50Hz，舵机控制的采样频率为20Hz。

不完全微分：反馈支路增益在(0,1)之间，越小则滤波越明显，为0时无微分，为1时为完全微分，参考值0.5。上述为离散情况，若为连续情况，同样越小则滤波越明显，参考值5。

积分限幅：积分限幅值的估算方法为：积分系数按0.1计算，误差按积分分离的阈值（10°）计算，积分运算时间不应超过5秒，角度限幅值与角速度限幅值之比为其函数执行频率之比（2:5），即角度限幅值为10*0.1*20*5=100，角速度为250。以角度为例，达到限幅值后积分项输出为10，与比例系数为1时的比例项输出相等。

框图如下：

2、数据校准

设加速度计的测量误差模型为

(2-1)

其中，为实际加速度向量，为加速度计测量的加速度向量，为偶然误差（测量误差），三阶矩阵的作用是对进行缩放和旋转，b为失重情况下加速度计的测量值。和为待确认的常数参数，其计算方法为，在六次校准中，每次校准的已知，为测量结果且已知，求和使每次测量的最小。

加速度计的校准方式为六面校准。忽略偶然误差的误差模型为

(2-2)

以yaw轴校准为例，正面朝上的测量误差式为（按重力加速度归一）

(2-3)

其中，，，为三维行向量，三者组成矩阵，为正面朝上的加速度测量值。此处及下文的角标中，、、分别代表yaw轴、pitch轴和roll轴，U代表朝上，即此轴测量值为正，D代表朝下，即此轴测量值为负。正面朝下的测量误差式为

(2-4)

两式相减得到

(2-5)

其中，为两次测量值之差。以此类推，其余的4次测量两式相减分别得到

(2-6)

(2-7)

整理可得，；，；，，由此可得

(2-8)

另

因此

式(2-2)中，的计算方法为

并将6次测量得出的取平均即可。

3、低通滤波器

对陀螺仪的原始数据不做处理，对加速度计的原始数据进行FIR低通滤波。FIR低通滤波框图如下：

4、卡尔曼滤波器

卡尔曼滤波用于高斯-马尔可夫过程的预测，按照状态方程和观测方程的形式分为4种，分别是标量状态-标量观测、矢量状态-标量观测、矢量状态-矢量观测和扩展卡尔曼滤波。此处使用矢量状态-标量观测卡尔曼滤波对欧拉角中的横滚角和俯仰角进行预测，即状态方程中是矢量，观测方程中是标量。

用卡尔曼滤波进行预测的原理是，陀螺仪测量的是三轴角速度，在状态方程中表示；加速度计测量的是三轴加速度，即近似于重力方向，在观测方程中表示。由于加速度计只能测量横滚角和俯仰角，因此偏航方向在观测方程中无法表示，不能应用于卡尔曼滤波。

状态方程为

(4-1)

即

(4-2)

观测方程为

(4-3)

即

(4-4)

状态方程中，为实际欧拉角，为陀螺仪测量的角速度与实际角速度误差，T为采样周期，为陀螺仪测量的角速度；观测方程中，为加速度计测得的欧拉角。

卡尔曼滤波过程的一个预测周期共5步，分别如下：

预测：

(4-5)

最小预测MSE：

(4-6)

卡尔曼增益：

(4-7)

修正：

(4-8)

最小MSE：

(4-9)

其中，为陀螺仪的测量噪声和测量误差噪声的二阶协方差矩阵，为二阶单位矩阵。

另外补充说明一下扩展卡尔曼滤波（EKF），加速度计给出的结果是三轴加速度，而观测方程中的为欧拉角，是两个方向加速度的比值的反正切，为非线性函数，因此需要用EKF；这里给出的方法是直接求得反正切作为加速度计给出的结果，因此为标量观测。

方差的某次室内静止环境下的实测数据：

加速度计：roll 0.0225；pitch 0.006

陀螺仪：roll 0.0494；pitch 0.0038；yaw 7.0264e-04

第三章建模与仿真

1、差分进化算法进行旋翼建模

本节的主要内容是确定模型的具体参数。受到实验条件限制，只能得到飞机飞行过程中的输入输出数据集，而无法用实验方法测得模型具体参数，因此采用差分进化算法，用输入和输出数据集来拟合模型参数。具体步骤如下：

1、初始化种群：

使用合适的概率分布函数随机产生若干个体，设共有popsize个个体，每个个体包含一套模型参数，共有paranum个，则生成一个[popsize x paranum]的随机数矩阵。倾转旋翼机需要的有效参数共15个，分别为：三轴的转动惯量与力臂长度的比值（，，，3个），左右电机的加速和减速延迟量（4个），左右倾转舵机的延迟量（2个），左右倾转舵机的归中位置（，，2个），升力与转速关系的二次项和一次项系数（4个）。设某次实验随机生成1000个个体，则产生一个[1000x15]的随机数矩阵作为初始化种群。

2、变异：

在种群中随机选择3个个体生成差分向量作为一个变异个体（参考标准差分进化算法）。如果变异个体的某个参数越界，则重新选择3个个体重复以上步骤。

3、交叉与选择：

将变异个体分别与所有其它普通个体进行交叉（参考标准差分进化算法），每交叉一次计算普通个体和变异个体的适应度值，淘汰适应度低的个体，保留适应度高的个体，种群总数保持不变。

个体的适应度计算方法是，选择这个个体的一套模型参数，代入飞行过程中得到的输入数据集，从而可以预测计算出输出数据集，与飞行过程中得到的输出数据集进行比较，求出两者的欧氏距离，距离小的说明这个个体的适应度高，也说明模型预测的数据集与真实的输出数据集相似，模型参数也比较接近真实值。

重复变异、交叉与选择步骤直到出现某个个体的适应度值满足要求，即可得到详细的模型参数。

由输入数据集根据模型参数计算输出数据集的方法是建立飞机的数学模型。以下建模方法将飞机的姿态分解为横滚（roll）、俯仰（pitch）和偏航（yaw）三个方向。

由角动量公式：

横滚方向的角加速度为：

俯仰方向的角加速度为：

偏航方向的角加速度为：

其中，和分别为左右电机提供的升力，和分别为左右倾转舵机的实际角度和归中角度的差值，，，分别为三轴的转动惯量与力臂长度的比值。

2、MATLAB飞行姿态仿真

系统包含比例环节、积分环节、惯性环节。其中，惯性环节的连续情况和离散情况不同，接下来进行重点分析。

图示分别为连续和离散情况下的惯性环节的框图，两个系统的增益均为1。连续系统的传递函数为

冲激响应为

阶跃响应为

离散系统的传递函数为