CN105739513B - 一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法，属于飞行机器人控制技术领域。所述控制器包括位置控制子***和姿态控制子***。位置控制子***包含外环和内环伪动态逆和PI控制器、位置配置环节；姿态控制子***也包含外环和内环伪动态逆和PI控制器、姿态配置环节。本发明避免了基于欧拉角控制器存在的奇异点和线性控制器制约机器人性能发挥的不足；简化了控制器的设计，提高了控制器***跟踪精度。所设计的控制器不仅具有全姿态轨迹跟踪能力，而且抗干扰能力强，跟踪精度高，显著提高了四旋翼飞行机器人机动性和环境交互能力，为四旋翼飞行机器人的进一步推广应用奠定了技术基础。

Description

一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制 方法

技术领域

本发明属于飞行机器人控制技术领域，涉及轨迹自动跟踪技术，具体地说，是指一种四旋翼飞行机器人非线性轨迹跟踪控制***的设计方法。

背景技术

随着飞行机器人技术的快速发展，飞行机器人的应用越来越广泛。特别是四旋翼飞行机器人因为具有垂直起降功能，对场地无特别要求，特别适合侦察、营救、航拍等领域，目前已逐渐在民用和军事等领域得到了应用。为了提高四旋翼飞行机器人作业能力和应用范围，人们不再满足于四旋翼飞行机器人能平稳飞行等基本要求，而对其机动能力和环境交互能力的要求越来越高，这就需要四旋翼飞行机器人控制器具有很高的轨迹跟踪能力。四旋翼飞行机器人是一个典型的欠驱动、强耦合的非线性***，这给其飞行轨迹跟踪控制器的设计带来了挑战。目前普遍使用的四旋翼飞行机器人飞行轨迹跟踪控制器存在着如下的不足：通常是基于欧拉角进行姿态表达的，采用欧拉角进行姿态表达的控制器，虽然有着姿态表达直观等特点，但在俯仰角为90度时，存在奇异点，此时飞行机器人失控，限制了飞行机器人的机动性和环境交互能力。另外一个问题是常用的控制器，通常采用的控制方法是通过对***进行线性化后再采用线性控制方法进行设计，该方法的优点是简化了控制器的设计过程，但由于四旋翼飞行机器人本身是一个非线性***，采用线性控制方法进行控制，严重制约了其性能的发挥，特别是机动性和环境交互能力。

发明内容

为了解决基于四旋翼飞行机器人轨迹跟踪控制器存在的上述问题，本发明提出了一种四旋翼飞行机器人非线性轨迹跟踪控制***设计方法，包括跟踪控制器及其跟踪控制方法。本发明采用基于四元素的姿态表达，通过建立四旋翼飞行机器人六自由度运动学和动力学模型，使用非线性控制方法-轨迹线性化控制方法具体设计了四旋翼飞行机器人六自由度非线性轨迹跟踪控制器，达到了避免基于欧拉角控制器存在的奇异点和线性控制器制约机器人性能发挥的不足。采用经过误差修正控制量的命令值代替标称值对各个控制环进行设计，不仅简化了控制器的设计，而且提高了控制器***跟踪精度。所设计的控制器不仅具有全姿态轨迹跟踪能力，而且抗干扰能力强，跟踪精度高，显著提高了四旋翼飞行机器人机动性和环境交互能力，为四旋翼飞行机器人的进一步推广应用奠定了技术基础。

本发明首先提供一种四旋翼飞行机器人非线性轨迹跟踪控制器，整个跟踪控制器由两部分组成：位置控制子***和姿态控制子***。位置控制子***中又包含内外两个控制环和一个位置配置环节，每个控制环都包含一个完整的轨迹线性化控制法控制结构：一个伪动态逆和一个起镇定调节作用的PI控制器。位置配置环节的主要作用是根据力指令F_com计算得到姿态指令值β_com和四个旋翼应该产生的总拉力T_com。姿态控制子***中包含内外两个控制环和姿态配置环节，每个控制环也都包含一个完整的轨迹线性化控制法控制结构，姿态配置环节的主要作用是根据力矩指令M_com和位置控制子***输出的总拉力T_com来映射得到四个旋翼的转速，从而实现对四旋翼飞行机器人的控制。

本发明的跟踪控制器接收位置指令P_com，位置控制子***第一外环伪动态逆根据该值计算出标称速度v_nom,第一外环PI控制器根据位置误差P_err计算出位置误差修正控制量v_ctrl，标称速度v_nom和位置误差修正控制量v_ctrl之和构成速度指令v_com；速度指令v_com作为位置控制子***内环的输入。位置控制子***第一内环伪动态逆根据速度指令v_com计算出标称力F_nom,第一内环PI控制器根据速度误差v_err计算出速度误差修正控制量F_ctrl，标称力F_nom和速度误差修正控制量F_ctrl之和构成力指令F_com；力指令F_com作为位置控制子***中位置配置环节的输入，位置配置环节根据力指令F_com值计算出姿态指令β_com和总拉力指令T_com，并作为姿态控制子***的输入；所述的总拉力指令T_com直接输出给姿态控制子***中的姿态配置环节。姿态指令β_com作为姿态控制子***中第二外环伪动态逆的输入量，第二外环伪动态逆根据该姿态指令计算出标称角速度ω_nom；第二外环PI控制器根据姿态误差β_err计算出姿态误差修正控制量ω_ctrl，标称角速度ω_nom和姿态误差修正控制量ω_ctrl之和构成角速度指令ω_com；角速度指令ω_com作为姿态控制子***内环的输入，姿态控制子***中第二内环伪动态逆根据角速度指令ω_com值计算出标称力矩M_nom,第二内环PI控制器根据角速度误差ω_err计算出角速度误差修正控制量M_ctrl，标称力矩M_nom和角速度误差修正控制量M_ctrl之和构成力矩指令M_com；力矩指令M_com输出给姿态配置环节，姿态配置环节根据输入的力矩指令M_com和总拉力指令T_com计算出四旋翼飞行机器人四个旋翼的转速，从而实现对四旋翼飞行机器人的控制。其中，四旋翼飞行机器人的位置、速度、姿态、角速度的测量值*_sen(*为P、v、β、ω)由组合导航***测量给出。

本发明一种四旋翼飞行机器人轨迹跟踪控制***设计方法的优点在于：

1、采用四元素来表达四旋翼飞行机器人姿态进行控制器设计，有效避免基于欧拉角传统控制器存在奇异点的不足。

2、采用轨迹线性化非线性控制方法设计轨迹跟踪控制器，在设计中不需要对方程进行小扰动线性化处理，因而避免了一般飞行控制***设计中由于线性化所带来的误差，并具有自动解耦能力，有效提高了控制器的控制精度，避免了线性化的不足，从而为飞行机器人的高机动飞行和环境交互能力的提高奠定了基础。

3、在控制器结构上，通过采用经过误差控制量修正的命令值代替标称值作为下一个控制环的输入，不仅简化了设计步骤，还提高了控制器轨迹跟踪性能。

附图说明

图1为本发明所应用的四旋翼飞行机器人坐标系与配置图；

图2为本发明四旋翼飞行机器人非线性轨迹跟踪控制器总体结构图；

图3为本发明四旋翼飞行机器人非线性轨迹跟踪控制器位置跟踪效果图；

图4为本发明四旋翼飞行机器人非线性轨迹跟踪控制器速度跟踪效果图；

图5为本发明四旋翼飞行机器人非线性轨迹跟踪控制器姿态跟踪效果图；

图6为本发明四旋翼飞行机器人非线性轨迹跟踪控制器角速度跟踪效果图。

具体实施方式

下面结合附图对本发明做详细说明。

本发明提供一种四旋翼飞行机器人非线性轨迹跟踪控制器及其控制方法，采用命令值代替标称值进行控制环路设计，如图2所示，本发明提供的跟踪控制器由两部分组成：位置控制子***和姿态控制子***。所述的位置控制子***中包含第一外环伪动态逆、第一外环PI控制器、第一内环伪动态逆、第一内环PI控制器和位置配置环节，位置配置环节根据力指令F_com计算得到姿态指令值β_com和四个旋翼应该产生的总拉力T_com，输出给姿态控制子***。所述的姿态控制子***中包含第二外环伪动态逆、第二外环PI控制器、第二内环伪动态逆、第二内环PI控制器和姿态配置环节。姿态配置环节根据力矩指令M_com和位置控制子***输出的总拉力T_com来映射得到四个旋翼的转速，从而实现对四旋翼飞行机器人的控制。

本发明的跟踪控制器接收位置指令P_com，位置控制子***第一外环伪动态逆根据该值计算出标称速度v_nom,第一外环PI控制器根据位置误差P_err计算出位置误差修正控制量v_ctrl，标称速度v_nom和位置误差修正控制量v_ctrl之和构成速度指令v_com；速度指令v_com作为位置控制子***内环的输入。位置控制子***第一内环伪动态逆根据速度指令v_com计算出标称力F_nom,第一内环PI控制器根据速度误差v_err计算出速度误差修正控制量F_ctrl，标称力F_nom和速度误差修正控制量F_ctrl之和构成力指令F_com；力指令F_com作为位置控制子***中位置配置环节的输入，位置配置环节根据力指令F_com值计算出姿态指令β_com和总拉力指令T_com，并作为姿态控制子***的输入；所述的总拉力指令T_com直接输出给姿态控制子***中的姿态配置环节。姿态指令β_com作为姿态控制子***中第二外环伪动态逆的输入量，第二外环伪动态逆根据该姿态指令计算出标称角速度ω_nom；第二外环PI控制器根据姿态误差β_err计算出姿态误差修正控制量ω_ctrl，标称角速度ω_nom和姿态误差修正控制量ω_ctrl之和构成角速度指令ω_com；角速度指令ω_com作为姿态控制子***内环的输入，姿态控制子***中第二内环伪动态逆根据角速度指令ω_com值计算出标称力矩M_nom,第二内环PI控制器根据角速度误差ω_err计算出角速度误差修正控制量M_ctrl，标称力矩M_nom和角速度误差修正控制量M_ctrl之和构成力矩指令M_com；力矩指令M_com输出给姿态配置环节，姿态配置环节根据输入的力矩指令M_com和总拉力指令T_com计算出四旋翼飞行机器人四个旋翼的转速，从而实现对四旋翼飞行机器人的控制。其中，四旋翼飞行机器人的位置、速度、姿态、角速度的测量值*_sen(*为P、v、β、ω)由组合导航***测量给出。

基于所述的跟踪控制器，本发明还提供一种四旋翼飞行机器人非线性轨迹跟踪控制方法，包括以下步骤：

第一步，基于四元素四旋翼飞行机器人建模，包括运动学建模与动力学建模；

由于旋翼的质量相对于四旋翼飞行机器人可以忽略，此时可以把四旋翼飞行机器人看成一个6自由度的刚体，其运动可以分解为平移运动和旋转运动，建立的坐标系和配置如图1所示的。图1中Σ_E表示地面坐标系O_DX_NY_EZ_D，Σ_b为机体坐标系xyz，T₁、T₂、T₃、T₄分别表示四个旋翼所产生的拉力，总拉力为T。用Ω₁、Ω₂、Ω₃、Ω₄分别表示四旋翼飞行机器人的四个旋翼的转速，用P表示地面坐标系下飞行机器人重心的位置矢量，v＝(v_x,v_y,v_z)表示地面坐标系下飞行机器人重心的速度矢量，F表示地面坐标系下飞行机器人所受合外力总力(包括重力)，M表示机体坐标系下飞行机器人的总力矩，ω表示机体坐标系下的角速度矢量，表示机体坐标系相对于地面坐标系的姿态旋转矩阵，是一个3×3的矩阵，具体计算见公式(20)～(25),用四元素来表示姿态旋转矩阵则为：

其中a,b,c,d为四元素β的四个分量，四旋翼飞行机器人在地面坐标系下平移运动的运动学方程为：

动力学方程为：

式中的m为四旋翼飞行机器人的质量，基于四元素的四旋翼飞行机器人旋转运动的运动学方程为：

近似为刚体的四旋翼飞行机器人的旋转动力学方程为：

上式中，J为四旋翼飞行机器人在机体坐标系下描述的惯性张量,定义如下：

其中，J_xx、J_yy、J_zz、J_zx、J_yz、J_xy为飞行机器人在机体坐标系下描述的转动惯量和惯性积。由于四旋翼飞行机器人的对称性，有J_xy＝J_yz＝0，通过整理，式(5)可以改写为如下所示的以ω为状态变量的状态方程：

其中，和与飞行机器人在机体坐标系下的惯性参数有关，和可以用J_xx，J_yy，J_zz，J_xz来表示，具体表达如下：

四旋翼飞行机器人四个旋翼产生的力矩M_a和总拉力T为：

其中，k_t和k_d分别为旋翼产生拉力和扭矩的系数，d_r是两个对角旋翼之间的距离，

每个旋翼产生的陀螺效应为：

其中J_r是电机转动惯量，e_z＝(0,0,1)^T，则四旋翼飞行机器人所受的总力矩M为：

M＝M_a+M_G (9)

四旋翼飞行机器人在地面坐标系下所受的总力F为：

其中，表示姿态旋转矩阵，m为四旋翼飞行机器人的质量，g是重力常数，v_x、v_y，v_z分别是四旋翼飞行机器人在地面坐标系下的三个方向的速度分量，C_Dv是阻力系数。

第二步，基于以上所建立的四旋翼飞行机器人的运动和动力学模型，采用轨迹线性化控制方法进行控制器的具体设计。

位置控制子***根据位置指令P_com，计算出四旋翼飞行机器人的姿态指令β_com和总拉力指令T_com，并作为指令输出给姿态控制子***。

如图2所示，位置控制子***的外环在控制结构上，由第一外环伪动态逆和第一外环PI控制器组成。地面坐标系下的标称速度v_nom由方程(2)求伪逆得到。

式中P_com是地面坐标系下的位置指令，为了保证的因果性，由位置指令P_com利用二阶伪微分运算器得到，二阶伪微分运算器如下式传递函数所示：

其中，ζ_d、ω_n,d分别是伪微分运算器阻尼比和带宽频率，s为传递函数的符号，复参数。

位置跟踪误差P_err定义为：

P_err＝P_sen-P_com (13)

其中，P_sen是传感器测量的四旋翼飞行机器人位置的测量值。

位置跟踪误差的动力学方程为：

其中v_ctrl为位置误差修正控制量，根据轨迹线性化的设计方法，位置控制子***第一外环PI控制器的控制率为：

v_ctrl＝-K_P1(t)P_err-K_I1(t⁾∫P_err (15)

其中,

A_cl11＝diag{-α_1j1},A_cl12＝diag{-α_1j2}和α_1jk(j＝1,2,3；k＝1,2)是控制参数，根据PD谱理论，系数α_1jk(j＝1,2,3；k＝1,2)由期望的闭环***阻尼比和带宽频率得到，具体计算方法为：

其中，ω_n,j(t)是带宽频率，ζ_j是闭环***阻尼比，t是时间。位置控制子***的外环输出是速度指令值v_com，计算公式为：

v_com＝v_nom+v_ctrl (19)

由于各个控制环设计过程基本相似，就不再赘述，下面分别对两个分配环节进行设计。

位置配置环节是把位置控制子***的内环输出的力指令F_com作为输入值，计算输出四旋翼飞行机器人姿态命令β_com和沿着飞行机器人机体坐标系Z轴的总拉力指令T_com。四旋翼飞行机器人的姿态旋转矩阵表达为方向余弦矩阵的形式为：

其中，r₁、r₂、r₃分别表示姿态旋转矩阵的三个列向量，c_ij为方向余弦矩阵参数，i＝1,2,3；j＝1,2,3。

把上式(20)代入式(10)，可以计算得到r₃：

其中，F_com＝[F_x,com F_y,com F_z,com]，F_x,com、F_y,com、F_z,com分别为力指令在地面坐标系三个方向上的分量。

从图1中我们可以看到，四旋翼飞行机器人四个旋翼所产生的合力总是在机体坐标系的Z轴方向上，并不依赖于飞行机器人的航向，因此，航向角可以设置为任意值。但考虑到工程实际，通常让机体坐标系的x轴始终指向位置指令轨迹的切线方向上，此时，航向角ψ按下式进行计算：

其中X_com和Y_com是四旋翼飞行机器人的命令位置。

此时，姿态旋转矩阵的r₁列向量投影到参考面X_NO_DY_E上的单位向量可以表示为：

h＝[cos(ψ),sin(ψ),0]^T (23)

考虑到单位向量r₂正交于向量h和r₃所构成的平面，因此r₂列向量的计算公式为:

由于方向余弦矩阵是一个正交阵，根据右手定则，单位列向量r₂的计算公式为：

r₁＝r₂×r₃ (25)

由式(21)、(24)和(25)则可计算出姿态旋转矩阵对于小角度的旋转，用来表示姿态标称值的四元素β_nom的各个分量a_nom、b_nom、c_nom、d_nom与姿态旋转矩阵R_b ^e参数c_ij(i＝1,2,3,j＝1,2,3)之间的关系式为：

其中，c_ij是由式(20)定义的以方向余弦形式表示的姿态旋转矩阵的参数。

从控制结构框图2中可以看到，位置配置环节并不需要单独计算出姿态误差，因此，位置控制子***输出给姿态控制子***的姿态指令就是姿态的标称值，即β_com＝β_nom。总拉力指令T_com直接输出给姿态控制子***的姿态配置环节，由于总拉力的方向总是沿着四旋翼飞行机器人的机体坐标系的Z轴上，T_com大小为：

其中F_x、F_y、F_z由下式给出：

F_x＝F_x,com+C_Dv|v_x,com|v_x,com

F_y＝F_y,com+C_Dv|v_y,com|v_y,com

F_z＝F_z,com-mg+C_Dv|v_z,com|v_z,com

v_x,com、v_y,com、v_z,com分别为速度指令在地面坐标系上的三个分量。

至此，完成了整个位置控制子***的设计，位置控制子***以位置指令作为输入值，输出姿态和总拉力指令值，并作为姿态控制子***的输入量。

第三步，姿态控制子***接收位置控制子***的内环输出的总拉力T_com和姿态指令β_com，姿态配置环节结合总拉力T_com和力矩M_com，计算输出四旋翼飞行机器人四个旋翼的转速指令Ω_i,com(i＝1,2,3，4，分别代表四个旋翼)，从而实现对四旋翼飞行机器人的飞行控制。

姿态控制子***的姿态配置环节的设计非常简单，直接把方程(7)求逆即可，从中可以看出，基于轨迹线性化的控制方法，具有直接解耦的功能，简化了设计过程。四个旋翼转速指令计算公式为：

至此，完成了整个四旋翼飞行机器人的飞行建模和基于四元素姿态表达和轨迹线性化控制方法的非线性全姿态飞行控制器的设计。

采用该控制方法对四旋翼飞行机器人的实际飞行跟踪效果进行了测试，所使用的阻尼系数和角频率等控制参数如表1所示。

表1控制参数

跟踪效果如图3～图6所示，其中图3为位置跟踪，图4为速度跟踪，图5为姿态跟踪，图6为角速度跟踪，其中*_com、*_sen(*为P、v、γ、ω)分别表示指令值和测量值。在姿态跟踪效果图中，由于基于四元素的姿态表达不够直观，在作图时，利用四元素与欧拉角之间的转换关系，转换为欧拉角，但在整个控制过程中，依然是以四元素的形式来表达飞行机器人的姿态，如图5所示，Ф、θ、ψ分别表示横滚、俯仰和方向角。从这些效果图中可以看到，四个控制环路的跟踪误差都非常小，表明采用本发明控制方法所设计的四旋翼飞行机器人六自由度非线性轨迹跟踪控制器具有良好的轨迹跟踪能力。

Claims (3)

1.一种四旋翼飞行机器人非线性轨迹跟踪控制器，其特征在于：包括位置控制子***和姿态控制子***；所述的位置控制子***中包含第一外环伪动态逆、第一外环PI控制器、第一内环伪动态逆、第一内环PI控制器和位置配置环节，位置配置环节根据力指令F_com计算得到姿态指令值β_com和四个旋翼应该产生的总拉力T_com，输出给姿态控制子***；所述的姿态控制子***中包含第二外环伪动态逆、第二外环PI控制器、第二内环伪动态逆、第二内环PI控制器和姿态配置环节；姿态配置环节根据力矩指令M_com和位置控制子***输出的总拉力T_com来映射得到四个旋翼的转速，从而实现对四旋翼飞行机器人的控制。

2.根据权利要求1所述的一种四旋翼飞行机器人非线性轨迹跟踪控制器，其特征在于：跟踪控制器接收位置指令P_com，位置控制子***第一外环伪动态逆根据该值计算出标称速度v_nom,第一外环PI控制器根据位置误差P_err计算出位置误差修正控制量v_ctrl，标称速度v_nom和位置误差修正控制量v_ctrl之和构成速度指令v_com；速度指令v_com作为位置控制子***内环的输入；位置控制子***第一内环伪动态逆根据速度指令v_com计算出标称力F_nom,第一内环PI控制器根据速度误差v_err计算出速度误差修正控制量F_ctrl，标称力F_nom和速度误差修正控制量F_ctrl之和构成力指令F_com；力指令F_com作为位置控制子***中位置配置环节的输入，位置配置环节根据力指令F_com值计算出姿态指令β_com和总拉力指令T_com，并作为姿态控制子***的输入；所述的总拉力指令T_com直接输出给姿态控制子***中的姿态配置环节；姿态指令β_com作为姿态控制子***中第二外环伪动态逆的输入量，第二外环伪动态逆根据该姿态指令计算出标称角速度ω_nom；第二外环PI控制器根据姿态误差β_err计算出姿态误差修正控制量ω_ctrl，标称角速度ω_nom和姿态误差修正控制量ω_ctrl之和构成角速度指令ω_com；角速度指令ω_com作为姿态控制子***内环的输入，姿态控制子***中第二内环伪动态逆根据角速度指令ω_com值计算出标称力矩M_nom,第二内环PI控制器根据角速度误差ω_err计算出角速度误差修正控制量M_ctrl，标称力矩M_nom和角速度误差修正控制量M_ctrl之和构成力矩指令M_com；力矩指令M_com输出给姿态配置环节，姿态配置环节根据输入的力矩指令M_com和总拉力指令T_com计算出四旋翼飞行机器人四个旋翼的转速；其中，四旋翼飞行机器人的位置、速度、姿态、角速度的测量值*sen由组合导航***测量给出，*为P、v、β、ω。

3.一种四旋翼飞行机器人非线性轨迹跟踪控制方法，包括以下步骤：

第一步，基于四元素四旋翼飞行机器人建模，包括运动学建模与动力学建模；

把四旋翼飞行机器人看成一个6自由度的刚体，其运动分解为平移运动和旋转运动，建立的坐标系，Σ_E表示地面坐标系O_DX_NY_EZ_D，Σ_b为机体坐标系xyz，T₁、T₂、T₃、T₄分别表示四个旋翼所产生的拉力，总拉力为T，用Ω₁、Ω₂、Ω₃、Ω₄分别表示四旋翼飞行机器人的四个旋翼的转速，用P表示地面坐标系下飞行机器人重心的位置矢量，v＝(v_x,v_y,v_z)表示地面坐标系下飞行机器人重心的速度矢量，F表示地面坐标系下飞行机器人所受合外力总力，M表示机体坐标系下飞行机器人的总力矩，ω表示机体坐标系下的角速度矢量，表示机体坐标系相对于地面坐标系的姿态旋转矩阵，是一个3×3的矩阵，用四元素来表示姿态旋转矩阵则为：

其中a,b,c,d为四元素β的四个分量，四旋翼飞行机器人在地面坐标系下平移运动的运动学方程为：

动力学方程为：

式中的m为四旋翼飞行机器人的质量，基于四元素的四旋翼飞行机器人旋转运动的运动学方程为：

其中p、q、r为角速度ω的三个分量，分别表示机体坐标系下四旋翼飞行机器人横滚、俯仰和航向角速度，

四旋翼飞行机器人的旋转动力学方程为：

上式中，J为四旋翼飞行机器人在机体坐标系下描述的惯性张量,定义如下：

其中，J_xx、J_yy、J_zz、J_zx、J_yz、J_xy为飞行机器人在机体坐标系下描述的转动惯量和惯性积；由于四旋翼飞行机器人的对称性，有J_xy＝J_yz＝0，通过整理，式(5)改写为如下所示的以ω为状态变量的状态方程：

其中，和与飞行机器人在机体坐标系下的惯性参数有关，和用J_xx，J_yy，J_zz，J_xz来表示，具体表达如下：

四旋翼飞行机器人四个旋翼产生的力矩M_a和总拉力T为：

其中，k_t和k_d分别为旋翼产生拉力和扭矩的系数，d_r是两个对角旋翼之间的距离，

每个旋翼产生的陀螺效应为：

其中J_r是电机转动惯量，e_z＝(0,0,1)^T，则四旋翼飞行机器人所受的总力矩M为：

M＝M_a+M_G (9)

四旋翼飞行机器人在地面坐标系下所受的总力F为：

其中，表示姿态旋转矩阵，g是重力常数，v_x、v_y，v_z分别是四旋翼飞行机器人在地面坐标系下的三个方向的速度分量，C_Dv是阻力系数；

第二步，基于以上所建立的四旋翼飞行机器人的运动和动力学模型，采用轨迹线性化控制方法进行控制器的具体设计；

位置控制子***根据位置指令P_com，计算出四旋翼飞行机器人的姿态指令β_com和总拉力指令T_com，并作为指令输出给姿态控制子***；

位置控制子***的外环在控制结构上，由第一外环伪动态逆和第一外环PI控制器组成，地面坐标系下的标称速度v_nom由方程(11)求伪逆得到；

式中P_com是地面坐标系下的位置指令，由位置指令P_com利用二阶伪微分运算器得到，二阶伪微分运算器如下式传递函数所示：

其中，ζ_d、ω_n,d分别是伪微分运算器阻尼比和带宽频率，s为传递函数的符号，复参数；

位置跟踪误差P_err定义为：

P_err＝P_sen-P_com (13)

其中，P_sen是传感器测量的四旋翼飞行机器人位置的测量值；

位置跟踪误差的动力学方程为：

其中v_ctrl为位置误差修正控制量，根据轨迹线性化的设计方法，位置控制子***第一外环PI控制器的控制律为：

v_ctrl＝-K_P1(t)P_err-K_I1(t)∫P_err (15)

其中,

A_cl11＝diag{-α_1j1},A_cl12＝diag{-α_1j2}和α_1jk是控制参数，根据PD谱理论，系数α_1jk由期望的闭环***阻尼比和带宽频率得到，j＝1,2,3；k＝1,2，具体计算方法为：

其中，ω_n,j(t)是带宽频率，ζ_j是闭环***阻尼比，t是时间，位置控制子***的外环输出是速度指令值v_com，计算公式为：

v_com＝v_nom+v_ctrl (19)

位置配置环节是把位置控制子***的内环输出的力指令F_com作为输入值，计算输出四旋翼飞行机器人姿态命令β_com和沿着飞行机器人机体坐标系Z轴的总拉力指令T_com，四旋翼飞行机器人的姿态旋转矩阵表达为方向余弦矩阵的形式为：

其中，r₁、r₂、r₃分别表示姿态旋转矩阵的三个列向量，c_ij为方向余弦矩阵参数，i＝1,2,3；j＝1,2,3；

把上式(20)代入式(10)，计算得到r₃：

记

则得

其中，F_com＝[F_x,com F_y,com F_z,com]，F_x,com、F_y,com、F_z,com分别为力指令在地面坐标系三个方向上的分量；

让机体坐标系的x轴始终指向位置指令轨迹的切线方向上，此时，航向角ψ按下式进行计算：

其中X_com和Y_com是四旋翼飞行机器人的命令位置；

此时，姿态旋转矩阵的r₁列向量投影到参考面X_NO_DY_E上的单位向量表示为：

h＝[cos(ψ),sin(ψ),0]^T (23)

考虑到单位向量r₂正交于向量h和r₃所构成的平面，因此r₂列向量的计算公式为:

由于方向余弦矩阵是一个正交阵，根据右手定则，单位列向量r₂的计算公式为：

r₁＝r₂×r₃ (25)

由式(21)、(24)和(25)计算出姿态旋转矩阵对于小角度的旋转，用来表示姿态标称值的四元素β_nom的各个分量a_nom、b_nom、c_nom、d_nom与姿态旋转矩阵参数c_ij之间的关系式为：

总拉力指令T_com直接输出给姿态控制子***的姿态配置环节，由于总拉力的方向总是沿着四旋翼飞行机器人的机体坐标系的Z轴上，T_com大小为：

其中F_x、F_y、F_z由下式给出：

F_x＝F_x,com+C_Dv|v_x,com|v_x,com

F_y＝F_y,com+C_Dv|v_y,com|v_y,com

F_z＝F_z,com-mg+C_Dv|v_z,com|v_z,com

v_x,com、v_y,com、v_z,com分别为速度指令在地面坐标系上的三个分量；

至此，位置控制子***以位置指令作为输入值，输出姿态和总拉力指令值，并作为姿态控制子***的输入量；

第三步，姿态控制子***接收位置控制子***的内环输出的总拉力T_com和姿态指令β_com，姿态配置环节结合总拉力T_com和力矩M_com，计算输出四旋翼飞行机器人四个旋翼的转速指令Ω_i,com，i＝1,2,3，4，从而实现对四旋翼飞行机器人的飞行控制；

四个旋翼转速指令计算公式为：