[发明专利]一种基于四元数球面线性插值的固定翼飞机姿态控制方法

权利要求书

1.一种基于四元数球面线性插值的固定翼飞机姿态控制方法，其特征在于，包括以下步骤：

构建六自由度飞机空气动力学模型，并基于所述动力学模型搭建角速度、过载的增稳系统；

通过所述动力学模型解算得到当前姿态角，分别将所述当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；

计算所述插值姿态四元数与所述当前姿态四元数的误差四元数，并基于所述误差四元数计算得到所述增稳系统的输入指令；

将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。

2.根据权利要求1所述的方法，其特征在于，所述目标姿态角包括：第一俯仰角、第一滚转角和第一偏航角；所述当前姿态角包括：第二俯仰角、第二滚转角和第二偏航角；所述输入指令包括：第一俯仰角速度、第一滚转角速度和第一偏航角速度。

3.根据权利要求2所述的方法，其特征在于，所述将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态，包括：

将所述第一俯仰角速度、第一滚转角速度和第一偏航角速度的增稳系统输入指令输入所述增稳系统；其中，所述增稳系统包括：俯仰角速度控制器、滚转角速度控制器和偏航角速度控制器；

通过所述增稳系统输出升降舵偏转角度、副翼偏转角度和方向舵偏转角度的飞机舵面偏转角控制指令，以控制所述飞机飞行姿态。

4.根据权利要求3所述的方法，其特征在于，

所述升降舵偏转角度δ_elevator：

其中，表示归一化过载误差，表示PID控制器比例系数和积分系数，K_n1为角速度前馈增益，为俯仰角速度K_q为俯仰角速度反馈增益；

所述副翼偏转角度δ_aileron：

其中，表示归一化滚转角速度误差，表示PID控制器比例系数和积分系数，K_p1为滚转角速度前馈增益，表示当前滚转角；

所述方向舵偏转角度δ_rudder：

其中，表示偏航角速度，加入径向过载n_y修正，表示PID控制器比例系数和积分系数。

5.根据权利要求2所述的方法，其特征在于，将第一俯仰角θ_cmd、第一滚转角和第一偏航角ψ_cmd以及第二俯仰角θ、第二滚转角和第二偏航角ψ转换为四元数，则转换关系为：

6.根据权利要求2所述的方法，其特征在于，将所述目标姿态四元数、所述当前姿态四元数以及所述插值姿态四元数分别表示为：

7.根据权利要求6所述的方法，其特征在于，所述将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数，由下面公式计算得到：

其中，Δα＝arccos(q^Tq_cmd)表示当前姿态四元数与目标姿态四元数的夹角，τ为插值系数，表示插值的密集程度。

8.根据权利要求7所述的方法，其特征在于，所述误差四元数，由下面公式表示：

9.根据权利要求8所述的方法，其特征在于，所述第一俯仰角速度所述第一滚转角速度和所述第一偏航角速度分别由下面公式计算得到：

其中，sign(·)函数判断Δw的符号，耦合系数表示为

10.一种基于四元数球面线性插值的固定翼飞机姿态控制装置，其特征在于，包括：

系统构建模块，用于构建六自由度飞机空气动力学模型，并基于所述动力学模型搭建角速度、过载的增稳系统；

四元数计算模块，用于通过所述动力学模型解算得到当前姿态角，分别将所述当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；

指令获取模块，用于计算所述插值姿态四元数与所述当前姿态四元数的误差四元数，并基于所述误差四元数计算得到所述增稳系统的输入指令；

姿态控制模块，用于将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。