四轴无人机的控制方法与流程

本发明涉及无人机控制技术领域，特别是涉及一种四轴无人机的控制方法。

背景技术：

无人驾驶飞机(Unmanned Aerial Vehicle，UAV)又简称无人机，与传统的有人驾驶飞行器不同，是一种利用无线电远程遥控及机载的程序控制器操纵的不载人飞机。其最早出现于20世纪20年代，当时仅被用于作为军事训练中的靶机,此后经过近百年的不断发展，逐渐转向于侦查、攻击等各种多用途领域。由于其相对于载人飞机来说具有成本低、生存能力强、无人员伤亡风险、使用方便等优点，所以不止能在军事上发挥重要作用，在民用领域也具有广阔的应用前景。

根据无人机的总体结构来分类，可以将无人机划分为固定翼无人机、直升机无人机及多旋翼无人机这三种主流结构，除此之外，还有伞翼无人机、扑翼无人机及无人飞艇等较小众的无人机结构，但实际应用较少。其中多旋翼无人机拥有4个或更多个旋翼来提供动力，相比固定翼无人机，可以实现原地垂直起降和空中悬停等灵活的机动动作，而机械结构相较直升机无人机又更为简单，且价格更为低廉，环境适应能力更强，可用于部分军用及大部分民用、消费领域，发展前景最受关注。在多旋翼无人机中，最简单也最常见的就是四旋翼无人机(Quadrotor UAV)，又被称为四轴无人机，其飞行动力由四个旋翼式的飞行引擎各自驱动一片螺旋桨来提供，通过调节引擎转速来改变四个旋翼转动产生的升力和扭矩，就能够实现稳定的飞行。

根据无人机尺寸大小的分类，目前市面上大多数的四轴无人机基本上都属于微小型无人机的范畴，微小型四轴无人机由机架、电机、螺旋桨、电池、遥控器、电机驱动器和飞行控制板组成。由于微小型四轴无人机具有飞行姿态灵活、机动性强、机械结构较为简单、易于拆卸维护、性价比较高等特点，使得其具有较为广泛的平台适应性，可应用于低空军事侦察、电力线巡检、山林灾情搜救、娱乐影视航拍等多种不同领域。

然而，微小型四轴无人机的尺寸较小、重量较轻、飞行速度较低，故其空气动力学模型较难准确地建立，再加上其载重及自身惯性都比较小，在低速飞行时十分容易受到气流的影响，所以对其飞行控制器的控制精度及灵活性都有更高的要求。

因此，针对上述技术问题，有必要提供一种四轴无人机的控制方法。

技术实现要素：

有鉴于此，本发明的目的在于提供一种四轴无人机的控制方法。

为了实现上述目的，本发明实施例提供的技术方案如下：

一种四轴无人机的控制方法，所述控制方法包括：

S1、获取四轴无人机中姿态传感器的姿态数据并进行滤波融合，得到当前的姿态信息，姿态数据包括当前的姿态角以及姿态角对应的角速度；

S2、通过串级双环PID控制方法对四轴无人机进行控制，将当前姿态信息与目标姿态信息的偏差值作为角度PID控制器的输入量，根据当前角速度与角度PID控制器输出量的差值输入角速度PID控制器，角速度PID控制器输出控制电机，直至四轴无人机达到目标姿态。

作为本发明的进一步改进，所述姿态角包括俯仰角、横滚角及航向角。

作为本发明的进一步改进，所述步骤S1中的姿态信息通过欧拉角、四元数、矩阵及轴角中的一种进行表示。

作为本发明的进一步改进，所述步骤S1中的姿态信息计算公式为：

其中，分别为当前四轴无人机的姿态角，θ、ψ为上个解算周期得到的欧拉角，ω_x、ω_y、ω_z为四轴无人机的三轴角速度。

作为本发明的进一步改进，所述步骤S1中的姿态信息计算公式为：

其中，分别为当前四轴无人机的姿态角，q₀、q₁、q₂、q₃为上个解算周期得出的四元数。

作为本发明的进一步改进，所述步骤S1中的滤波融合为：

通过Mahony互补滤波的算法来进行姿态数据的滤波融合。

作为本发明的进一步改进，所述步骤S1具体为：

1)通过加速度计及磁力计测量的数据，计算机体欧拉角形式的初始姿态，并对四元数进行初始化；

2)通过加速度计、陀螺仪及磁力计，获得机体当前的重力加速度分量ax、ay、az，角速度分量gx、gy、gz及磁场强度分量mx、my、mz，并对重力加速度分量及磁场强度分量进行归一化，得到单位值；

3)通过四元数与欧拉角之间的关系，用上一个解算周期得出的四元数来推算出重力加速度分量及磁场强度分量在当前机体坐标系下的估计值；

4)将重力向量和地球磁场的测量值，与利用上次姿态解算结果推算出的估计值进行向量叉乘，得出两者间的误差，并进行累加；

5)用向量叉乘得出的累积误差，通过PI调节的方式来校正陀螺仪的漂移误差；

6)最后求解四元数的微分方程组，对四元数进行更新，并将四元数单位化。

作为本发明的进一步改进，所述步骤S2中的PID控制方法具体为：

计算当前姿态信息与目标姿态信息之间误差的比例P、积分I和微分D，并叠加输出。

作为本发明的进一步改进，所述步骤S2中PID控制方法的输出其中：

比例控制的输出P_out＝K_pe(t)，K_p为比例控制的参数，e为输入的误差信号；

积分控制的输出K_i为比例控制的参数，e为输入的误差信号；

微分控制的输出K_d为比例控制的参数，e为输入的误差信号。

本发明的有益效果是：

只需获取的机体三轴角速度，就可以实现对四周无人机姿态的解算，同时，通过滤波融合对测量数据进行辅助校正，能够得到更加准确的测量数据，提高了姿态数据的准确性；

使用串级双环PID控制，增强了系统的抗干扰性，使得四轴无人机具有更强适应能力。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明中四轴无人机的控制方法的流程示意图；

图2为本发明中四轴无人机的动力模型图；

图3为本发明中四轴无人机飞行运动方式的示意图；

图4为本发明中地理坐标系的示意图；

图5为本发明一具体实施方式中俯仰角Pitch、横滚角Roll、偏航角Yaw的欧拉角表示原理图；

图6为现有技术中角度单环PID控制的流程示意图；

图7为本发明一具体实施方式中串级双环PID控制的流程示意图。

具体实施方式

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

参图1所示，本发明公开了一种四轴无人机的控制方法，该控制方法包括：

S1、获取四轴无人机中姿态传感器的姿态数据并进行滤波融合，得到当前的姿态信息，姿态数据包括当前的姿态角以及姿态角对应的角速度；

S2、通过串级双环PID控制方法对四轴无人机进行控制，将当前姿态信息与目标姿态信息的偏差值作为角度PID控制器的输入量，根据当前角速度与角度PID控制器输出量的差值输入角速度PID控制器，角速度PID控制器输出控制电机，直至四轴无人机达到目标姿态。

以下结合具体实施方式对本发明作进一步说明。

微小型四轴无人机中的“四轴”指的是飞行器的动力由四个旋翼式的飞行引擎驱动螺旋桨来提供，其动力模型如图2所示。这四个旋翼分成两组，分别位于十字对称的机架的悬臂末端，处在对角的两个旋翼作为一组。同组的旋翼旋转方向相同，不同组的旋翼旋转方向相反。在转速相同的情况下，由于结构的对称性，四个旋翼转动产生的转矩能够相互抵消，使得机身能够在水平面上保持平稳。

通过调节四个旋翼的转动速度就可以实现对四轴无人机飞行姿态的调节，从而使四轴无人机完成俯仰、横滚、偏航及升降等基本运动。这四种基本运动的运动方式如图3所示，图3中的四轴无人机工作在“+”模式下，机头朝向X轴正方向，1、3号电机为一组，朝逆时针方向旋转，2、4号电机为一组，朝顺时针方向旋转，朝上的箭头表示转速提升，朝下的箭头表示转速下降。其飞行运动方式包括：

1)俯仰运动：其运动方式如图3a所示，保持2、4号电机的转速不变，改变1、3号电机转速，且转速改变的趋势相反，会使得四轴无人机绕着Y轴旋转一定角度，做俯仰运动，这个绕着Y轴旋转的角度又被称为俯仰角(Pitch)；而1、3号电机转速不同又会导致机体前后升力不一致，进而使机体在水平面的X轴方向上做前后运动，实现四轴无人机的前进与后退；

2)横滚运动：其运动方式如图3b所示，与俯仰运动相反，保持1、3号电机的转速不变，改变2、4号电机转速，且转速改变的趋势相反，使得四轴无人机绕着X轴旋转一定角度，做横滚运动，这个绕着X轴旋转的角度又被称为横滚角(Roll)；2、4号电机转速有异造成机体左右升力的差值，进而使机体在水平面的Y轴方向上左右运动，实现四轴无人机的左移与右移；

3)偏航运动：运动方式如图3c所示，保持对角电机的转速一致，分别以相反的趋势改变两组对角电机的转速，会使得四轴无人机绕着Z轴旋转一定角度，完成偏航运动，进而改变机头的朝向。绕Z轴旋转的角度又被称为偏航角(Yaw)；

4)升降运动：运动方式如图3d所示，保持四个电机转速相同使得四轴无人机能够保持平稳，然后同时提升或降低它们的转速，就能够实现四轴无人机在Z轴方向上的位移，即升降运动。

由对微小型四轴无人机飞行原理的分析可知，对其飞行运动方式的操控，既是对其飞行姿态，即俯仰角Pitch、横滚角Roll、偏航角Yaw这三个姿态角的控制，并最终转化为对相应电机转速的控制。为了定量地实现这样的控制方式，通常都采用“观测—>控制—>再观测—>再控制”循环进行的闭环控制模式，即通过传感器器件不断观测计算出机体当前的姿态角，然后使用一定的控制算法，控制电机转速来使机体朝着目标姿态转变，并不断重复这个过程直到机体达到目标姿态。

在以上的控制模式中，“观测”这一步又被称为姿态解算，负责将加速度计、陀螺仪、磁力计等姿态传感器的数据进行融合，输出当前姿态信息；而“控制”部分在本发明中采用的是PID控制算法，负责将目标姿态转化为对电机转速的控制。以下从姿态解算和PID控制方法两方面对本实施方式作具体说明。

一、姿态解算

四轴无人机的飞行姿态实际上描述的是机体所在的坐标系与地理坐标系之间的角度关系。其中，地理坐标系是以地球为参考物建立的，以正东、正北及正上方向作为此坐标系的X、Y及Z轴，其形式如图4所示。而机体坐标系则如图2中所示，以机体的几何中心作为原点，以垂直于四个电机所在平面的轴作为Z轴，X、Y轴位于电机所在平面且互相垂直，具体的方向由所使用的飞行模式决定。在低空近地条件下，可以认为在地理坐标系中的重力加速度向量及地磁向量都是固定不变的，利用此特征，可以通过加速度计及磁力计等传感器求出机体的姿态。

机体坐标系与地理坐标系之间的关系可以用欧拉角、四元数、矩阵及轴角等方式来表示，其中最常用的是欧拉角表示及四元数表示。

1)欧拉角表示

欧拉角用来描述三维欧氏空间中一个坐标系相对于另一个坐标系的取向。在这里我们将固定不动的地理坐标系作为参考坐标系，则处在同样空间中的机体坐标系的任何取向，都可以用参考坐标系按一定顺序，分别绕自身的某个坐标轴进行三次旋转来得到，而每次旋转的角度就称为欧拉角。

根据旋转顺序及旋转轴选取的不同，欧拉角的表示也不一样，在这里我们采用Tait-Bryan angle的表示方式，得到如图5所示的欧拉角θ、ψ。图5中O-xyz为参考的地理坐标系，首先绕z轴旋转ψ角度，得到O-x′y′z′，再绕y′轴旋转θ角度，得到O-x″y″z″，最后绕x″轴旋转角度，得到O-XYZ的机体坐标系，其中欧拉角θ、ψ分别对应的Pitch、Roll、Yaw三个姿态角。

当使用欧拉角来表示姿态时，利用上个解算周期得到的的欧拉角θ、ψ，加上陀螺仪测量到的机体坐标系下的三轴角速度ω_x、ω_y、ω_z，通过欧拉角运动学微分方程，可以得到当前新的姿态角其公式如下：

通过此种方式可以直接解算出Pitch、Roll、Yaw的数值，且相对易于理解，但在θ达到90度时会产生称为GimbalLock的万向节死锁问题，无法实现全姿态的解算。

2)四元数表示

四元数可理解为由一个实数和一个三维向量组成的超复数，其表示形式如下：

一个单位四元数可以方便地用来表示三维空间中的一次旋转，其中向量部分用来表示旋转轴，实数部分则用以表示绕旋转轴旋转的角度。

在采用四元数来表示姿态时，需要首先在机体静止的情况下，通过加速度计及磁力计测得的数据求出此时机体的欧拉角θ₀、ψ₀，然后通过以下公式求出初始的四元数q₀’、q₁’、q₂’、q₃’：

接着就可以通过一阶毕卡算法来求解四元数的微分方程组，得到四元数的姿态更新公式如下：

式4中等式左侧为更新后的四元数，等式右侧的q₀、q₁、q₂、q₃为上个解算周期得出的四元数。此算法相比使用欧拉角的算法来说，减少了对三角函数的计算，提升了姿态解算速度，同时也不会出现欧拉角算法中的万向节死锁问题，可用于全姿态的解算。

用四元数方式来表示飞行器的姿态虽然易于计算，但却不够直观，所以在姿态解算完毕后，还需将更新后的四元数转换为欧拉角，提供给之后的姿态控制算法，其公式如下所示：

由式3、4及5可知，在知晓机体初始姿态的情况下，理论上只需通过陀螺仪获取的机体三轴角速度，就可以实现对机体姿态的解算。然而由于误差噪声等的存在，陀螺仪测量的数据只能在较短的时间内保持相对的准确性，尤其是运转一段时间以后，积分误差的累加会让得到的姿态和实际的相差甚远，产生较大的漂移，所以必须要使用其它传感器来对陀螺仪的测量数据进行辅助校正。这个将不同传感器测量数据结合起来，以期能够得到更加准确的测量数据的过程被称为传感器数据的滤波融合。

在四轴无人机中常用的滤波融合算法有互补滤波算法、卡尔曼滤波算法等。其中互补滤波算法的思想是利用不同传感器输出数据之间在频域上的互补特性，采用加权融合等方式来得到更为准确的输出数据；而卡尔曼滤波算法实际上是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

卡尔曼滤波在响应速度上要好于互补滤波，但其对目标建模的准确度要求较高，实现起来比较复杂，计算量也相对较大；而互补滤波的计算量则相对较小，实现难度较低，且总体滤波效果与卡尔曼滤波之间的差距并不是很大，用于四轴无人机的姿态解算已然足够。

本实施方式中采用了一种被称为Mahony互补滤波的算法来进行姿态传感器数据的滤波融合，。此算法基于互补滤波的思想，用高频噪声较多，但低频长期信号较为稳定且没有长期漂移的加速度数据来对陀螺仪的漂移进行校正，并且针对加速度计无法测量重力轴上的旋转运动的特性，又加入了磁力计数据来矫正偏航角Yaw。

本实施方式中采用四元数来对姿态进行解算，并使用Mahony互补滤波算法来对姿态数据进行滤波，相互组合成了基于四元数的互补滤波姿态解算算法，其执行流程如下所示：

1)初次执行此算法时，通过加速度计及磁力计测量的数据，计算机体的初始姿态(欧拉角形式)，通过式3对四元数进行初始化。

2)通过加速度计、陀螺仪及磁力计，获得机体当前的重力加速度分量a_x、a_y、a_z，角速度分量g_x、g_y、g_z及磁场强度分量m_x、m_y、m_z。并对重力加速度分量及磁场强度分量进行归一化(各分量除以向量的模)，得到它们的单位值。

3)通过四元数与欧拉角之间的关系，用上一个解算周期得出的四元素(陀螺仪积分得到)来推算出重力加速度分量及磁场强度分量在当前机体坐标系下的估计值，相关算法如下：

hx＝2.0f*(mx*(0.5f-q2q2-q3q3)+my*(q1q2-q0q3)+mz*(q1q3+q0q2))；

hy＝2.0f*(mx*(q1q2+q0q3)+my*(0.5f-q1q1-q3q3)+mz*(q2q3-q0q1))；

hz＝2.0f*mx*(q1q3-q0q2)+2.0f*my*(q2q3+q0q1)+2.0f*mz*(0.5f-q1q1-q2q2)；

bx＝sqrt(hx*hx+hy*hy)；

bz＝hz；

//利用上次姿态解算结果，估计的磁场方向

halfwx＝bx*(0.5f-q2q2-q3q3)+bz*(q1q3-q0q2)；

halfwy＝bx*(q1q2-q0q3)+bz*(q0q1+q2q3)；

halfwz＝bx*(q0q2+q1q3)+bz*(0.5f-q1q1-q2q2)；

//利用上次姿态解算结果，估计重力场方向

halfvx＝q1q3-q0q2；

halfvy＝q0q1+q2q3；

halfvz＝q0q0-0.5f+q3q3；

4)将重力向量和地球磁场的测量值，与利用上次姿态解算结果推算出的估计值进行向量叉乘，得出两者间的误差，并进行累加，相关算法如下：

//计算并累加磁场的估计方向与实测方向间的误差

halfex+＝(my*halfwz-mz*halfwy)；

halfey+＝(mz*halfwx-mx*halfwz)；

halfez+＝(mx*halfwy-my*halfwx)；

halfvz＝q0q0-0.5f+q3q3；

//计算并累加重力估计方向与实测方向间的误差

halfex+＝ay*halfvz-az*halfvy；

halfey+＝az*halfvx-ax*halfvz；

halfez+＝ax*halfvy-ay*halfvx；

5)用向量叉乘得出的累积误差，通过PI调节的方式来校正陀螺仪的漂移误差，相关算法如下所示，其中Kp、Ki两个参数用于控制加速度计及磁力计对陀螺仪积分姿态的修正速度。

//误差积分

gyro_bias[0]+＝twoKi*halfex*dt；

gyro_bias[1]+＝twoKi*halfey*dt；

gyro_bias[2]+＝twoKi*halfez*dt；

//漂移修正

gx+＝(twoKp*halfex+gyro_bias[0])；

gy+＝(twoKp*halfey+gyro_bias[1])；

gz+＝(twoKp*halfez+gyro_bias[2])；

6)最后利用式4求解四元数的微分方程组，对四元数进行更新，并将四元数单位化，以便于下个解算周期的计算，相关算法如下：

dq0＝0.5f*(-q1*gx-q2*gy-q3*gz)；

dq1＝0.5f*(q0*gx+q2*gz-q3*gy)；

dq2＝0.5f*(q0*gy-q1*gz+q3*gx)；

dq3＝0.5f*(q0*gz+q1*gy-q2*gx)；

q0+＝dt*dq0；

q1+＝dt*dq1；

q2+＝dt*dq2；

q3+＝dt*dq3；

//将四元数单位化

recipNorm＝invSqrt(q0*q0+q1*q1+q2*q2+q3*q3)；

q0*＝recipNorm；

q1*＝recipNorm；

q2*＝recipNorm；

q3*＝recipNorm；

至此，便完成了对四轴无人机姿态的解算，得出了机体的当前姿态。

二、PID控制方法

在得到四轴无人机的当前飞行姿态之后，就需要通过一定的控制方式来控制相应电机的转速，使四轴无人机达到目标姿态。目前已提出的一些控制方式包括PID控制(Proportional Integral Derivative Control)、反步控制(BackStepping Control)、滑模变结构控制(Sliding Mode Control)及线性二次型调制器(Linear Quadratic Regulator)等。其中PID控制方式相对来说最传统，但由于其对系统模型的要求不高，编码实现较为简单，控制方法适应性强，故被广泛应用与工程实践中。本发明就采用了串级双环PID算法来实现对四轴无人机姿态角的控制。

PID控制算法是最常见、应用最为广泛的负反馈自动控制算法。其利用观测值与目标值之间误差的比例(P，Proportional)、积分(I，Integral)和微分(D，Derivative)，进行相应的计算和叠加输出，来实现对目标的控制。其中：

比例控制(P)：比例控制既是将输入误差信号按一定比例进行控制输出。当仅有比例控制时，系统输出会存在稳态误差，即输出稳定时会与目标值存在偏差。比例控制的输出P_out如式6所示，其中K_p为比例控制的参数，e为输入的误差信号。

P_out＝K_pe(t) (式6)

从式6中可看出，系数K_p的值越大，则比例控制输出就越大，对误差的修正也就越迅速，但系统稳定性也随之降低。

积分控制(I)：积分控制中引入了误差信号对时间的积分，控制器的输出与此积分项成正比关系。由于积分项是随时间增大的，所以将其与比例控制结合起来组成PI控制，就能消除比例控制中的稳态误差，但输出会有一些时滞。积分项输出的公式如式7所示，其中K_i为比例控制的参数，e为输入的误差信号。

微分控制(D)：在微分控制中，控制器的输出与误差信号对时间的微分成正比。微分调节就是控制误差的变化率，能够实现对被控量的超前控制，增大系统的阻尼系数，增加系统的稳定性。微分项的输出公式如式8所示，其中Kd为比例控制的参数，e为输入的误差信号。

将P、I、D这三种调节方式组合起来，就成为了PID控制，其数学表达式为：

通过调节PID控制器中的K_p、K_i及K_d这三个参数，就能够实现对被控对象稳定、快速且准确的控制。

现有技术中，针对四轴无人机系统，在解算出机体当前三个姿态角后，可以将它们与目标姿态角的偏差值作为输入量，设计相应的PID控制器来实现对俯仰角、横滚角及航向角的控制，其控制流程如图6所示。

角度单环PID控制算法只用了四轴无人机的姿态角信息，为了增加控制的稳定性及控制品质，本实施方式中进一步加入了对四轴无人机角速度的控制，组成了由角度环及角速度环组成的串级双环PID控制算法。

本实施方式中通过串级双环PID控制方法对四轴无人机进行控制，将当前姿态信息与目标姿态信息的偏差值作为角度PID控制器的输入量，根据当前角速度与角度PID控制器输出量的差值输入角速度PID控制器，角速度PID控制器输出控制电机，直至四轴无人机达到目标姿态。

具体地，外环角速度环的组成结构与图6中所示的类似，只是其控制输出不再直接提供给电机进行转速调节，而是作为内环角速度环的期望值；将其配合陀螺仪测得的角速度数据，得到两者的差值输入内环角速度PID控制器，其输出再用于电机控制，其控制流程如图7所示。

由上述技术方案可以看出，本发明具有以下有益效果：

只需获取的机体三轴角速度，就可以实现对四周无人机姿态的解算，同时，通过滤波融合对测量数据进行辅助校正，能够得到更加准确的测量数据，提高了姿态数据的准确性；

使用串级双环PID控制，增强了系统的抗干扰性，使得四轴无人机具有更强适应能力。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。