CN108375988A - 一种带有不平衡负载的四旋翼无人机位姿控制方法 - Google Patents

Abstract

一种带有不平衡负载的四旋翼无人机位姿控制方法，属于无人机控制技术领域。本发明为了解决装载重心未知、安装位置不确定的不平衡负载时无人机无法继续稳定飞行的问题。本发明首先利用坐标变换和动力学分析推导出四旋翼无人机在装载不平衡负载前后的运动学模型和动力学模型；在得到飞行过程中传感器测量数据后通过扩展卡尔曼滤波方法得到高精度、无滞后的飞行状态，并通过飞行状态辨识出大概的重心位置；设计四旋翼飞行器的姿态控制器、位置控制器，并通过补偿螺旋桨转速的方式改进控制器以抵消重心位置变化所带来的额外旋转运动和线运动。本发明适用于在四旋翼无人机的起飞和悬停阶段估计重心位置并建立补偿控制器。

Description

一种带有不平衡负载的四旋翼无人机位姿控制方法

技术领域

本发明涉及一种四旋翼无人机位姿控制方法，属于无人机控制技术领域。

背景技术

四旋翼飞行器由于结构简单，在复杂环境下具有可低空飞行、定点悬停和垂直起降的能力，目前无论从市场应用还是学术研究上都具有较好的发展前景。四旋翼飞行器当今的发展趋势是逐步自主化，在复杂环境下能够精确地完成导航与控制任务，在不确定的干扰下保证飞行任务的成功率。

我们希望得到在装载重心未知、安装位置不确定的不平衡负载下保持飞行器稳定精确完成任务的控制方法。在货物运输、灾区物资救援中，由于不同飞行任务下所装载货物的形状不确定，很难保证货物重心和装载在飞行器上的位置精确可测，更无法做到装载货物后机身重心位置不发生改变。通常情况下，飞行器由于改变负载而引起的重心位置变化是飞行过程中的主要干扰之一，而飞行器的动力学方程通常是在假设重心位置不变且与机体几何中心重合的条件下建立的，因此这种重心位置的改变会引起力矩的不平衡，从而导致角加速度的产生，使飞行器振动甚至引起侧翻。

针对不平衡负载引起的飞行器重心变化下的控制问题，传统的增益调参方法需要增加对重心参数的调节，控制律的设计更为复杂、繁琐，且各平衡点间的参数调节缺乏规律性，需要通过详尽的仿真和实验进行设计结果验证，重心的变化会增大仿真验证的负担，设计过程更耗时、低效。基于重量分布的重心辨识方法提供重心信息并引入到逆动力学推导中实现飞行控制系统设计，然而该重心辨识的方法依赖于负载的质量和位置信息，在重心未知、安装位置不确定的不平衡负载条件下无法得到重心位置的辨识信息。

发明内容

本发明为了解决装载重心未知、安装位置不确定的不平衡负载时，四旋翼无人机由于重心偏移无法继续稳定飞行的问题。

本发明解决上述技术问题所采用的技术方案为：

一种带有不平衡负载的四旋翼无人机位姿控制方法，包括以下步骤：

步骤一、建立地面坐标系(O_gX_gY_gZ_g)和机体坐标系(OXYZ)，将在载重范围内任意形状、任意重量的负载装载在四旋翼无人机的任意位置，此时，四旋翼飞行器重心位置从几何中心处O(0,0,0)变为G(x_G,y_G,0)；

步骤二、装载能够测量飞行中运动状态的传感器，所述传感器包括安装于飞行器的机体中心位置的陀螺仪、加速度计和磁力计，分别用于测量飞行过程中飞行器的角速度、加速度和航向角信息；超声波安装于飞行器的底端朝向地面，用于测量相对于地面的飞行高度；光流相机安装于飞行器的底端朝向地面，用于测量相对于地面的水平飞行速度；初始化各传感器，记录传感器初始偏置；

步骤三、建立无负载条件下确保四旋翼飞行器起飞和悬停的姿态控制器和位置控制器，选择合适的串级PID参数；

步骤四、实时读取步骤二中传感器信息，并建立合适的状态方程和测量方程，通过扩展卡尔曼滤波得到更加准确的飞行姿态角、角速度、线速度；

步骤五、建立并比较四旋翼飞行器在装载不平衡负载前后的动力学方程，实时计算得到重心的位置x_G和y_G，如果辨识得到的重心位置收敛，则跳出，否则，进入步骤六；

步骤六、根据步骤五辨识得到的重心位置x_G和y_G计算抵消不平衡的重力力矩、保证飞行器稳定飞行器螺旋桨补偿转速Δω₁₃和Δω₂₄；

步骤七、用步骤六中计算得到的补偿螺旋桨转速校正步骤三得到的姿态控制器和位置控制器，得到基于重心补偿的四旋翼飞行器补偿控制器，计算得到修正后的四个电机占空比U₁ ^*、U₂ ^*、U₃ ^*和U₄ ^*。

进一步地，步骤三所述建立无负载条件下确保四旋翼飞行器起飞和悬停的控制器过程包括以下步骤：

步骤三A、建立无负载条件下的四旋翼飞行器姿态控制器，包括飞行过程中的俯仰角、滚转角、偏航角控制器，以角速度控制为内环、以姿态角控制为外环的串级PID控制器完成飞行姿态控制，其中，主控制器的输入是期望姿态角和实际姿态角的偏差，输出为期望角速度，该期望角速度和飞行器实际角速度的偏差作为副控制器的输入，输出控制螺旋桨转速的占空比，分别记做U_roll，U_pitch和U_yaw；

俯仰方向的占空比指令U_pitch由PID控制器计算得到

其中，K_Pc、K_Ic、K_Dc和K_Ps、K_Is、K_Ds分别为内、外环控制器参数，为期望俯仰角，为期望俯仰角速度，T为积分时间常数；

步骤三B、通过增加水平飞行位置和飞行高度控制器来完成飞行器的悬停控制，水平飞行位置控制器以飞行器的期望位置作为输入，以飞行姿态角作为控制输出；飞行高度控制器以期望飞行高度作为输入，输出控制螺旋桨转速的基础占空比值，记做U_throttle；

飞行高度的基础占空比指令U_throttle可由PID控制器计算得到

其中，K_Pv、K_Iv、K_Dv和K_Pz、K_Iz、K_Dz分别为内、外环控制器参数，S_z ^*为期望飞行高度，，T为积分时间常数；

步骤三C、组合飞行位置控制器输出和飞行姿态控制器输出，得到在无负载条件下输出给飞行器四个电机的占空比U₁、U₂、U₃和U₄

步骤三中姿态控制器的参数设置：K_Pc＝1.40，K_Ic＝0.45，K_Dc＝0.70，K_Ps＝4.00，K_Is＝0.02，K_Ds＝0.00；

步骤三中位置控制器的参数设置：K_Pv＝1.00，K_Iv＝0.01，K_Dv＝0.00，K_Pz＝6.00，K_Iz＝0.04，K_Dz＝0.00。

进一步地，步骤四所述通过扩展卡尔曼滤波得到更加准确的飞行状态信息的过程包括以下步骤：

航姿参考系统扩展卡尔曼滤波器的状态方程为

其中，x_k为k时刻状态向量，w_k-1为过程噪声向量，q₀、q₁、q₂和q₃为表示飞行器姿态的四元数；

基于加速度计的航姿参考系统扩展卡尔曼滤波器的测量方程为

其中，x_k为k时刻状态向量，y_k为k时刻加速度计的测量输出，v_k为测量噪声向量；

基于磁力计的航姿参考系统扩展卡尔曼滤波器的测量方程为

其中，x_k为k时刻状态向量，y_k为k时刻磁力计的测量输出，v_k为测量噪声向量；

光流辅助的惯性导航系统扩展卡尔曼滤波器设计方法相同，以飞行器速度及位移作为状态向量，以飞行器速度为测量输出，不断更新校正更加准确的水平飞行速度、水平位移状态。

进一步地，步骤五所述通过比较四旋翼飞行器在装载不平衡负载前后的动力学方程实时计算得到重心位置的过程包括以下步骤：

步骤五A、认为无人机的重心与几何中心完全重合，推导无人机无额外负载时的运动学模型和动力学模型：

设飞行器相对于机体坐标系X轴、Y轴和Z轴所受升力分别F_x‘、F_y‘、F_z‘,飞行器的总升力沿地面坐标系X_g轴、Y_g轴和Z_g轴方向上的分力为F_x、F_y和F_z；在机体坐标系下，飞行器所受到的升力为

由步骤一中机体坐标系到地面坐标系旋转矩阵R，可以得到飞行器相对于地面坐标系的升力

同时，根据牛顿第二定律可得到飞行器线运动的方程为

其中，S_x、S_y和S_z分别为飞行器沿X、Y和Z轴方向上的位移；

飞行器在飞行过程的旋转运动方程为

其中，J_x、J_y和J_z分别是飞行器绕X、Y和Z轴的转动惯量，l为初始重心O到各电机的距离；

可以得到飞行器的运动学和动力学方程为

步骤五B、装载不平衡负载后，重心位置发生偏移，有重力产生额外转矩使无人机产生旋转运动，推导出无人机在加载任意负载后的运动学模型和动力学模型：

飞行器线运动方程无变化，而旋转运动方程变为

可以得到带有不平衡负载的飞行器的运动学和动力学方程为

步骤五C、比较有无负载负载下无人机的受力情况，通过无人机此刻的额外旋转运动计算出此刻的重心位置x_G和y_G：

针对俯仰角运动方向，由步骤五A和步骤五B中加载不平衡负载前后的动力学模型可知，飞行器的俯仰角和重心坐标之间存在如下的关系

可得

同理可得

可以根据飞行器的姿态数据辨识出其重心位置的坐标值。

进一步地，步骤六所述计算抵消不平衡的重力力矩、保证飞行器稳定飞行器螺旋桨补偿转速的过程包括以下步骤：

针对重心偏移的螺旋桨转速补偿是通过补偿螺旋桨转速以抵消重心位置变化所带来的额外旋转运动和线运动；实时计算螺旋桨补偿转速直到抵消重力对其几何中心的力矩使飞行器达到力矩平衡状态，记录补偿转速并在位姿控制的输出予以校正；设在俯仰、滚转方向上针对重心偏移的补偿升力分别为F₁₃、F₂₄，针对重心偏移的补偿螺旋桨转速分别为Δω₁₃、Δω₂₄；

在加入不平衡负载下重新恢复力矩平衡，公式(11)修改为

整理得

螺旋桨的升力计算公式

其中C为升力系数，只和螺旋桨的几何参数有关，S为螺旋桨在地面的投影面积，ρ是空气密度，ω是螺旋桨的转速；

可得

解得

在原有转速基础上分别对四只螺旋桨的转速做出修正，设ω′₁、ω′₂、ω′₃和ω′₄分别为螺旋桨的修正转速，则

进一步地，步骤七用修正螺旋桨转速校正姿态控制器和位置控制器的过程包括以下步骤：

考虑到不平衡负载下的飞行，引入步骤六中计算的修正转速Δω₁₃和Δω₂₄来校正步骤三求得的控制器，求得电机转速和PWM占空比之间的近似比例系数K_ω后，计算得到修正后的四个电机占空比U₁ ^*、U₂ ^*、U₃ ^*和U₄ ^*：

如果辨识的重心位置未收敛，则跳到步骤四继续通过更新的传感器数据估计重心位置，如果辨识的重心位置收敛，则得到最终的可以在不平衡负载下稳定飞行的控制器。

本发明具有以下有益效果：

本发明首先利用坐标变换和动力学分析推导出四旋翼无人机在装载不平衡负载前后的运动学模型和动力学模型；在得到飞行过程中传感器测量数据后通过扩展卡尔曼滤波方法得到高精度、无滞后的飞行状态，并通过飞行状态辨识出大概的重心位置；设计四旋翼飞行器的姿态控制器、位置控制器，并通过补偿螺旋桨转速的方式改进控制器以抵消重心位置变化所带来的额外旋转运动和线运动。本发明适用于在四旋翼无人机的起飞和悬停阶段估计重心位置并建立补偿控制器。本发明针对位姿信息的测量问题引入扩展卡尔曼滤波方案，平滑AHRS和INS的测量结果，获取更加准确的位姿信息，提高重心位置辨识精度。本发明实现了四旋翼无人机在起飞和悬停过程中的重心位置辨识和基于重心位置的补偿控制。

本发明的具体优点表现在以下几个方面：

1、本发明对传感器的精度、性能要求不高，可以根据不同传感器的测量特性融合得到更加精确的飞行状态数据，同时滤波过程无滞后，可以实现实时测量。

2、大量数据表明，本发明方法辨识无人机重心位置方向的成功率在95％以上

3、大量数据表明，本发明方法在飞行器悬停控制过程中能够使无人机在水平方向偏移误差不超过10cm。

4、本发明方法能够适用于任意形状、质量的负载装载与无人机的任意位置。

5、利用处理器为ARM系列stm32单片机，处理频率达到500Hz，满足实时重心辨识与飞行器控制要求。

附图说明

图1是地面坐标系和机体坐标系的转换关系图；

图2是装载不平衡负载后四旋翼飞行器的受力分析图；

图3是无负载条件下的四旋翼飞行器控制框图；

图4是基于扩展卡尔曼滤波的飞行器位姿估计框图；

图5是重心位置辨识算法的流程图；

图6是装载负载条件下的四旋翼飞行器控制框图。

具体实施方式

具体实施方式一：

基于重心位置辨识的装载不平衡负载的四旋翼无人机补偿控制方法，包括以下步骤：

步骤一、建立地面坐标系(O_gX_gY_gZ_g)和机体坐标系(OXYZ)，如图1所示，地面坐标系原点O_g固连于地面上任意一点；X_g轴指向地理东方；Y_g轴指向地理北极；Z_g沿当地地理垂线指上并与X_g、Y_g构成右手直角坐标系。机体坐标系的坐标原点O为初始重心位置；X轴指向机身右方；Y轴位于机体轴线指向机头；Z轴指向机身上方。机体坐标系为动坐标系，其原点不随重心偏移而变化。

将地面坐标系通过Z-Y-X旋转次序转到机体坐标系可以得到表征飞行器姿态的航向角ψ(t)、俯仰角θ(t)、和滚转角三次旋转的旋转矩阵分别为

机体坐标系到地面坐标系之间的旋转矩阵为

将任意形状、在载重范围内任意重量的负载装载在四旋翼无人机的任意位置，此时，其重心位置和机体总质量发生变化,如图2所示，m′为负载的质量,M为飞行器加负载后的总质量。由于重心沿Z轴方向的偏移通常较小且不会产生导致飞行姿态变化的力矩，飞行器不平衡装载负载时，忽略重心沿Z轴方向的偏移量，设重心位置由O(0,0,0)移至G(x_G,y_G,0)。

步骤二、装载能够测量飞行中运动状态的传感器，包括陀螺仪、加速度计和磁力计安装于飞行器的机体中心位置，分别用于测量飞行过程中飞行器的角速度、加速度和航向角信息；超声波安装于飞行器的底端朝向地面，用于测量相对于地面的飞行高度；光流相机安装于飞行器的底端朝向地面，用于测量相对于地面的水平飞行速度。初始化各传感器，分别记录陀螺仪、加速度计和磁力计等传感器的初始偏置；

步骤三、建立无负载条件下确保四旋翼飞行器起飞和悬停的控制器，选择合适的串级PID参数；

步骤四、实时读取传感器信息，并建立合适的状态方程和测量方程，通过扩展卡尔曼滤波得到更加准确的飞行姿态角、角速度、线速度等；

步骤五、通过比较四旋翼飞行器在装载不平衡负载前后的动力学方程，实时计算得到重心的位置；

步骤六、计算抵消不平衡的重力力矩、保证飞行器稳定飞行器螺旋桨补偿转速；

步骤七、用补偿螺旋桨转速校正姿态控制器和位置控制器，转入步骤四，直到估计的重心位置一段时间内趋于稳定，最终得到基于重心偏移的四旋翼飞行器补偿控制器。

具体实施方式二：图3为无负载条件下的四旋翼飞行器控制框图，结合图3说明本实施方式，

本实施方式步骤三所述的包括以下步骤：

步骤三A、建立无负载条件下的四旋翼飞行器姿态控制器，包括飞行过程中的俯仰角、滚转角、偏航角控制器，本发明以角速度控制为内环、以姿态角控制为外环的串级PID控制器完成飞行姿态控制，其中，主控制器的输入是期望姿态角和实际姿态角的偏差，输出为期望角速度，该期望角速度和飞行器实际角速度的偏差作为副控制器的输入，输出控制螺旋桨转速的占空比，分别记做U_roll，U_pitch和U_yaw。

以俯仰方向为例，占空比指令U_pitch可由PID控制器计算得到

其中，K_Pc、K_Ic、K_Dc和K_Ps、K_Is、K_Ds分别为内、外环控制器参数，为期望俯仰角，为期望俯仰角速度。

步骤三B、在此基础上，通过增加水平飞行位置和飞行高度控制器来完成飞行器的悬停控制。水平飞行位置控制器以飞行器的期望位置作为输入，以飞行姿态角作为控制输出；飞行高度控制器以期望飞行高度作为输入，输出控制螺旋桨转速的基础占空比值，记做U_throttle。

以飞行高度为例，基础占空比指令U_throttle可由PID控制器计算得到

其中，K_Pv、K_Iv、K_Dv和K_Pz、K_Iz、K_Dz分别为内、外环控制器参数，S_z ^*为期望飞行高度。

步骤三C、组合飞行位置控制器输出和飞行姿态控制器输出，得到在无负载条件下输出给飞行器四个电机的占空比

步骤三中姿态控制器的参数设置：K_Pc＝1.40，K_Ic＝0.45，K_Dc＝0.70，K_Ps＝4.00，K_Is＝0.02，K_Ds＝0.00；

步骤三中位置控制器的参数设置：K_Pv＝1.00，K_Iv＝0.01，K_Dv＝0.00，K_Pz＝6.00，K_Iz＝0.04，K_Dz＝0.00。

具体实施方式三：图4为基于扩展卡尔曼滤波的飞行器位姿估计的整体过程，结合图4说明本实施方式，

本实施方式步骤四所述的包括以下步骤：

本发明针对位姿信息的测量问题引入扩展卡尔曼滤波方案，平滑AHRS和INS的测量结果，获取更加准确的位姿信息，提高重心位置辨识精度。

航姿参考系统扩展卡尔曼滤波器的状态方程为

其中，x_k为k时刻状态向量，w_k-1为过程噪声向量，q₀、q₁、q₂和q₃为表示飞行器姿态的四元数。

基于加速度计的航姿参考系统扩展卡尔曼滤波器的测量方程为

其中，x_k为k时刻状态向量，y_k为k时刻加速度计的测量输出，v_k为测量噪声向量。

基于磁力计的航姿参考系统扩展卡尔曼滤波器的测量方程为

其中，x_k为k时刻状态向量，y_k为k时刻磁力计的测量输出，v_k为测量噪声向量。

光流辅助的惯性导航系统扩展卡尔曼滤波器设计方法相同，以飞行器速度及位移作为状态向量，以飞行器速度为测量输出，不断更新校正更加准确的水平飞行速度、水平位移等状态。

具体实施方式四：结合图2说明本实施方式，

本实施方式步骤五所述的包括以下步骤：

步骤五A、认为无人机的重心与几何中心完全重合，推导无人机无额外负载时的运动学模型和动力学模型：

设飞行器相对于机体坐标系X轴、Y轴和Z轴所受升力分别F_x‘、F_y‘、F_z‘,飞行器的总升力沿地面坐标系X_g轴、Y_g轴和Z_g轴方向上的分力为F_x、F_y和F_z。在机体坐标系下，飞行器所受到的升力为

由步骤一中机体坐标系到地面坐标系旋转矩阵R，可以得到飞行器相对于地面坐标系的升力

同时，根据牛顿第二定律可得到飞行器线运动的方程为

其中，S_x、S_y和S_z分别为飞行器沿X、Y和Z轴方向上的位移。

另外，飞行器在飞行过程的旋转运动方程为

其中，J_x、J_y和J_z分别是飞行器绕X、Y和Z轴的转动惯量，l为初始重心O到各电机的距离。

可以得到飞行器的运动学和动力学方程为

步骤五B、装载不平衡负载后，重心位置发生偏移，有重力产生额外转矩使无人机产生旋转运动，推导出无人机在加载任意负载后的运动学模型和动力学模型：

飞行器线运动方程无变化，而旋转运动方程变为

可以得到带有不平衡负载的飞行器的运动学和动力学方程为

步骤五C、比较有无负载负载下无人机的受力情况，通过无人机此刻的额外旋转运动计算出此刻大概的重心位置：

以俯仰角运动方向为例，由步骤五A和步骤五B中加载不平衡负载前后的动力学模型可知，飞行器的俯仰角和重心坐标之间存在如下的关系

可得

同理可得

可以根据飞行器的姿态数据辨识出其重心位置的坐标值。图5为重心位置辨识算法的流程图。

具体实施方式五：

本实施方式步骤六所述的包括以下步骤：

针对重心偏移的螺旋桨转速补偿是通过补偿螺旋桨转速以抵消重心位置变化所带来的额外旋转运动和线运动。实时计算螺旋桨补偿转速直到抵消重力对其几何中心的力矩使飞行器达到力矩平衡状态，记录补偿转速并在位姿控制的输出予以校正。设在俯仰、滚转方向上针对重心偏移的补偿升力分别为F₁₃、F₂₄，针对重心偏移的补偿螺旋桨转速分别为Δω₁₃、Δω₂₄。

在加入不平衡负载下重新恢复力矩平衡，公式(11)修改为

整理得

螺旋桨的升力计算公式

其中C为升力系数，只和螺旋桨的几何参数有关，S为螺旋桨在地面的投影面积，ρ是空气密度，ω是螺旋桨的转速。

可得

解得

在原有转速基础上分别对四只螺旋桨的转速做出修正，设ω′₁、ω′₂、ω′₃和ω′₄分别为螺旋桨的修正转速，则

具体实施方式六：图6为装载负载条件下的四旋翼飞行器控制框图，结合图6说明本实施方式，

本实施方式步骤七所述的包括以下步骤：

考虑到不平衡负载下的飞行，引入步骤六中计算的修正转速Δω₁₃和Δω₂₄来校正步骤三求得的控制器，求得电机转速和PWM占空比之间的近似比例系数K_ω后，计算得到修正后的四个电机占空比U₁ ^*、U₂ ^*、U₃ ^*和U₄ ^*：

如果辨识的重心位置未收敛，则跳到步骤四继续通过更新的传感器数据估计重心位置，如果辨识的重心位置收敛，则得到最终的可以在不平衡负载下稳定飞行的控制器。

Claims (6)

1.一种带有不平衡负载的四旋翼无人机位姿控制方法，其特征在于，包括以下步骤：

步骤一、建立地面坐标系(O_gX_gY_gZ_g)和机体坐标系(OXYZ)，将在载重范围内任意形状、任意重量的负载装载在四旋翼无人机的任意位置，此时，四旋翼飞行器重心位置从几何中心处O(0,0,0)变为G(x_G,y_G,0)；

步骤二、装载能够测量飞行中运动状态的传感器，所述传感器包括安装于飞行器的机体中心位置的陀螺仪、加速度计和磁力计，分别用于测量飞行过程中飞行器的角速度、加速度和航向角信息；超声波安装于飞行器的底端朝向地面，用于测量相对于地面的飞行高度；光流相机安装于飞行器的底端朝向地面，用于测量相对于地面的水平飞行速度；初始化各传感器，记录传感器初始偏置；

步骤三、建立无负载条件下确保四旋翼飞行器起飞和悬停的姿态控制器和位置控制器，选择合适的串级PID参数；

步骤四、实时读取步骤二中传感器信息，并建立合适的状态方程和测量方程，通过扩展卡尔曼滤波得到更加准确的飞行姿态角、角速度、线速度；

步骤五、建立并比较四旋翼飞行器在装载不平衡负载前后的动力学方程，实时计算得到重心的位置x_G和y_G，如果辨识得到的重心位置收敛，则跳出，否则，进入步骤六；

步骤六、根据步骤五辨识得到的重心位置x_G和y_G计算抵消不平衡的重力力矩、保证飞行器稳定飞行器螺旋桨补偿转速Δω₁₃和Δω₂₄；

步骤七、用步骤六中计算得到的补偿螺旋桨转速校正步骤三得到的姿态控制器和位置控制器，得到基于重心补偿的四旋翼飞行器补偿控制器，计算得到修正后的四个电机占空比U₁ ^*、U₂ ^*、U₃ ^*和U₄ ^*。

2.根据权利要求1所述一种带有不平衡负载的四旋翼无人机位姿控制方法，其特征在于，步骤三所述建立无负载条件下确保四旋翼飞行器起飞和悬停的控制器过程包括以下步骤：

步骤三A、建立无负载条件下的四旋翼飞行器姿态控制器，包括飞行过程中的俯仰角、滚转角、偏航角控制器，以角速度控制为内环、以姿态角控制为外环的串级PID控制器完成飞行姿态控制，其中，主控制器的输入是期望姿态角和实际姿态角的偏差，输出为期望角速度，该期望角速度和飞行器实际角速度的偏差作为副控制器的输入，输出控制螺旋桨转速的占空比，分别记做U_roll，U_pitch和U_yaw；

俯仰方向的占空比指令U_pitch由PID控制器计算得到

其中，K_Pc、K_Ic、K_Dc和K_Ps、K_Is、K_Ds分别为内、外环控制器参数，为期望俯仰角，为期望俯仰角速度，T为积分时间常数；

步骤三B、通过增加水平飞行位置和飞行高度控制器来完成飞行器的悬停控制，水平飞行位置控制器以飞行器的期望位置作为输入，以飞行姿态角作为控制输出；飞行高度控制器以期望飞行高度作为输入，输出控制螺旋桨转速的基础占空比值，记做U_throttle；

飞行高度的基础占空比指令U_throttle可由PID控制器计算得到

其中，K_Pv、K_Iv、K_Dv和K_Pz、K_Iz、K_Dz分别为内、外环控制器参数，S_z ^*为期望飞行高度，，T为积分时间常数；

步骤三C、组合飞行位置控制器输出和飞行姿态控制器输出，得到在无负载条件下输出给飞行器四个电机的占空比U₁、U₂、U₃和U₄

步骤三中姿态控制器的参数设置：K_Pc＝1.40，K_Ic＝0.45，K_Dc＝0.70，K_Ps＝4.00，K_Is＝0.02，K_Ds＝0.00；

步骤三中位置控制器的参数设置：K_Pv＝1.00，K_Iv＝0.01，K_Dv＝0.00，K_Pz＝6.00，K_Iz＝0.04，K_Dz＝0.00。

3.根据权利要求2所述一种带有不平衡负载的四旋翼无人机位姿控制方法，其特征在于，步骤四所述通过扩展卡尔曼滤波得到更加准确的飞行状态信息的过程包括以下步骤：

航姿参考系统扩展卡尔曼滤波器的状态方程为

其中，x_k为k时刻状态向量，w_k-1为过程噪声向量，q₀、q₁、q₂和q₃为表示飞行器姿态的四元数；

基于加速度计的航姿参考系统扩展卡尔曼滤波器的测量方程为

其中，x_k为k时刻状态向量，y_k为k时刻加速度计的测量输出，v_k为测量噪声向量；

基于磁力计的航姿参考系统扩展卡尔曼滤波器的测量方程为

其中，x_k为k时刻状态向量，y_k为k时刻磁力计的测量输出，v_k为测量噪声向量；

光流辅助的惯性导航系统扩展卡尔曼滤波器设计方法相同，以飞行器速度及位移作为状态向量，以飞行器速度为测量输出，不断更新校正更加准确的水平飞行速度、水平位移状态。

4.根据权利要求1、2或3所述一种带有不平衡负载的四旋翼无人机位姿控制方法，其特征在于，步骤五所述通过比较四旋翼飞行器在装载不平衡负载前后的动力学方程实时计算得到重心位置的过程包括以下步骤：

步骤五A、认为无人机的重心与几何中心完全重合，推导无人机无额外负载时的运动学模型和动力学模型：

设飞行器相对于机体坐标系X轴、Y轴和Z轴所受升力分别F_x‘、F_y‘、F_z‘,飞行器的总升力沿地面坐标系X_g轴、Y_g轴和Z_g轴方向上的分力为F_x、F_y和F_z；在机体坐标系下，飞行器所受到的升力为

由步骤一中机体坐标系到地面坐标系旋转矩阵R，可以得到飞行器相对于地面坐标系的升力

同时，根据牛顿第二定律可得到飞行器线运动的方程为

其中，S_x、S_y和S_z分别为飞行器沿X、Y和Z轴方向上的位移；

飞行器在飞行过程的旋转运动方程为

其中，J_x、J_y和J_z分别是飞行器绕X、Y和Z轴的转动惯量，l为初始重心O到各电机的距离；

可以得到飞行器的运动学和动力学方程为

步骤五B、装载不平衡负载后，重心位置发生偏移，有重力产生额外转矩使无人机产生旋转运动，推导出无人机在加载任意负载后的运动学模型和动力学模型：

飞行器线运动方程无变化，而旋转运动方程变为

可以得到带有不平衡负载的飞行器的运动学和动力学方程为

步骤五C、比较有无负载负载下无人机的受力情况，通过无人机此刻的额外旋转运动计算出此刻的重心位置x_G和y_G：

针对俯仰角运动方向，由步骤五A和步骤五B中加载不平衡负载前后的动力学模型可知，飞行器的俯仰角和重心坐标之间存在如下的关系

可得

同理可得

可以根据飞行器的姿态数据辨识出其重心位置的坐标值。

5.根据权利要求4所述基于重心位置辨识的装载不平衡负载的四旋翼无人机补偿控制方法，其特征在于，步骤六所述计算抵消不平衡的重力力矩、保证飞行器稳定飞行器螺旋桨补偿转速的过程包括以下步骤：

针对重心偏移的螺旋桨转速补偿是通过补偿螺旋桨转速以抵消重心位置变化所带来的额外旋转运动和线运动；实时计算螺旋桨补偿转速直到抵消重力对其几何中心的力矩使飞行器达到力矩平衡状态，记录补偿转速并在位姿控制的输出予以校正；设在俯仰、滚转方向上针对重心偏移的补偿升力分别为F₁₃、F₂₄，针对重心偏移的补偿螺旋桨转速分别为Δω₁₃、Δω₂₄；

在加入不平衡负载下重新恢复力矩平衡，公式(11)修改为

整理得

螺旋桨的升力计算公式

其中C为升力系数，只和螺旋桨的几何参数有关，S为螺旋桨在地面的投影面积，ρ是空气密度，ω是螺旋桨的转速；

可得

解得

在原有转速基础上分别对四只螺旋桨的转速做出修正，设ω＇₁、ω＇₂、ω＇₃和ω＇₄分别为螺旋桨的修正转速，则

6.根据权利要求1或5所述一种带有不平衡负载的四旋翼无人机位姿控制方法，其特征在于，步骤七用修正螺旋桨转速校正姿态控制器和位置控制器的过程包括以下步骤：

考虑到不平衡负载下的飞行，引入步骤六中计算的修正转速Δω₁₃和Δω₂₄来校正步骤三求得的控制器，求得电机转速和PWM占空比之间的近似比例系数K_ω后，计算得到修正后的四个电机占空比U₁ ^*、U₂ ^*、U₃ ^*和U₄ ^*：

如果辨识的重心位置未收敛，则跳到步骤四继续通过更新的传感器数据估计重心位置，如果辨识的重心位置收敛，则得到最终的可以在不平衡负载下稳定飞行的控制器。