[发明专利]一种四旋翼飞行机器人非线性轨迹跟踪控制器及其跟踪控制方法

权利要求书

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，从而实现对四旋翼飞行机器人的飞行控制；

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