[发明专利]考虑推进器输出饱和的海底飞行节点预设性能轨迹跟踪控制方法

权利要求书

1.考虑推进器输出饱和的海底飞行节点预设性能轨迹跟踪控制方法，其特征在于：所述方法包括以下步骤：

步骤一：建立Fossen大纲六自由度非线性动力学模型；

步骤二：对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程；所述OBFN为海底飞行节点；

所述OBFN的动力学模型为：

其中为v的一阶导数，M^-1为M的逆，M为质量惯性矩阵，B₀代表OBFN推力分配矩阵B的标称值，u代表OBFN推进器的控制输出，C_v0代表C_v的标称值，C_v为OBFN的科氏力和向心力矩阵，D_v0代表D_v的标称值，D_v为OBFN的水动力阻尼矩阵，g_η0代表g_η的标称值，g_η为OBFN重力和浮力产生的力和力矩向量，v＝[u′,a,w,p,q,r]^T表示OBFN在运动坐标系下的速度与角速度，式中，u′为OBFN在运动坐标系下纵荡速度，a为OBFN在运动坐标系下横荡速度，w为OBFN在运动坐标系下垂荡速度，p为OBFN在运动坐标系下横倾角速度，q为OBFN在运动坐标系下纵倾角速度，r为OBFN在运动坐标系下摇首角速度，T为转置符号；F表示系统的总不确定度；

步骤三：建立性能函数，利用性能函数将跟踪误差表示为：

其中e_i(t)为OBFN位置与姿态角误差，i＝1,2,3,4,5,6；e_i(0)为初始时间OBFN位置与姿态角误差，ρ_i(t)为第i个自由度的性能函数，δ_i为辅助变量，满足0≤δ_i≤1；

步骤四：将步骤三的跟踪误差进行误差变换，得到变换后的误差s；

步骤五：引入推进器输出饱和，确定用于处理推进器输出饱和的辅助系统，当辅助变量z_a满足||z_a||≥σ时，辅助系统开始工作；当||z_a||＜σ时，代表辅助系统不工作，即不存在推进器输出饱和；所述σ为正定向量；

步骤六：根据步骤四得到的变换后的误差s和步骤五确定的辅助系统，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器；

所述步骤一中建立Fossen大纲六自由度非线性动力学模型的具体过程为：

运动坐标系的原点G取在OBFN的重心处，x轴、y轴和z轴分别为经过原点的水线面、横剖面和中纵剖面的交线；

固定坐标系的原点E选在海面的某一点，Eξ轴和Eη轴置于水平面内且互相垂直，Eξ轴正向指向正北方向，Eζ垂直于Eξη平面，正向指向地心；

Fossen大纲六自由度非线性动力学模型：

式中η＝[x,y,z,φ,θ,ψ]^T表示OBFN在固定坐标系下的六自由度位置与姿态，式中：x为OBFN固定坐标系下Eξ轴方向位移，y为OBFN在固定坐标系下Eη轴方向位移，z为OBFN在固定坐标系下Eζ轴方向位移，φ为OBFN在固定坐标系下横倾角度，θ为OBFN在固定坐标系下纵倾角度，ψ为OBFN在固定坐标系下摇首角度；J(η)是固定坐标系与运动坐标系之间的转换矩阵，τ为OBFN推进系统产生的控制力和力矩，为v的一阶导数；

所述步骤二中对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程的具体过程为：

OBFN的推进器的故障影响采用推力分配矩阵形式表示，定义为ΔB；实际的控制力与力矩改写为τ+Δτ：

τ+Δτ＝(B₀-KB)u＝(B₀+ΔB)u (5)

式中B为OBFN的推力分配矩阵，τ为OBFN的推进器的实际的控制力，Δτ是推进器故障对推进器推力的影响值，K是一个对角矩阵，其元素k_ii∈[0,1]，表示相应的推进器故障程度，其中1代表故障程度最高，推进器完全失效，u是OBFN的推进器的控制输入，B₀为OBFN的推力分配矩阵的标称值；等式(4)改写为公式(1)的形式；

系统的总不确定度F的表达式如下：

式中：表示海流扰动对OBFN的干扰，ΔM即代表OBFN质量惯性矩阵M的不确定值，ΔB为B的不确定值，ΔC_v为C_v的不确定值，ΔD_v为D_v的不确定值，Δg_η为g_η的不确定值，C_Aη为C_A的导出变量，即C_Aη＝C_A(v_r)J^-1，式中：C_A为OBFN附加质量的科氏力和向心力矩阵，D_η为D的导出变量，即D_η＝D(v_r)J^-1，式中：D为水动力阻尼矩阵，η_r为OBFN在固定坐标系下相对于海流的位移向量；

在OBFN的动力学模型(1)的基础上，给出OBFN的跟踪误差方程：

式中代表OBFN在固定坐标系下轨迹跟踪误差的二阶导数，表示OBFN实际的位置与姿态角，表示OBFN期望的位置与姿态角，v_e＝v-v_d代表OBFN在运动坐标系下速度与角速度的跟踪误差，v_d代表OBFN在运动坐标系下期望的速度与角速度，表OBFN在运动坐标系下速度与角速度的跟踪误差的一阶导数，令D＝-J(η)F，其中G、H、D均为简化变量；等式(7)简写如下：