CN101462587B - 冗余控制的微小型水下机器人及故障诊断和容错控制方法 - Google Patents

Abstract

本发明提供的是一种冗余控制的微小型水下机器人及故障诊断和容错控制方法。其壳体由下壳体，上前壳、上后壳拼接而成的流线型外形；壳体内部自前向后设置倾角罗盘舱、深度计、电池舱、多普勒速度计、电子舱、水平螺旋桨推进器和转向升沉舵机；深度计、电池舱、多普勒速度计平行排列共同位于倾角罗盘舱之后、转向升沉舵机位于两个水平螺旋桨推进器中间，两个水平螺旋桨推进器和一个升沉转向舵机横向平行排列、并共同位于水下机器人的尾部。本发明具有智能化程度高，容错能力强，安全性好，运动灵活，结构调整方便的特点。

Description

冗余控制的微小型水下机器人及故障诊断和容错控制方法

(一)技术领域

本发明涉及的是一种微小型水下机器人，本发明也涉及这种微小型水下机器人的故障诊断和容错控制方法。

(二)背景技术

地球上海洋面积占地球表面积的70.8％，海洋是人类食物资源的宝库，并且海洋中有取之不尽，用之不竭的自然能源。毋庸置疑，随着陆地资源的开发殆尽，海洋资源的开发和利用对经济增长的推动作用将越趋明显。

水下机器人作为海洋开发的主要工具，需要长时间工作在复杂多变的水下环境中，这就要求水下机器人具有高度的安全性和可靠性。由于容错控制可以明显提高水下机器人的生存能力，并且在受到损伤或危急的情况下仍能使水下机器人保持较好的执行效果，因此，容错控制技术可以看成是保证水下机器人安全工作的最后一道防线。要很好的实现水下机器人容错控制，就需要执行器间存在功能冗余，以保证当某个执行器失效时，也能通过冗余执行器实现功能补偿，达到减小损失，安全返航的目的。

水下机器人具有6个自由度，即进退、侧移、升沉、横摇、纵摇(俯仰)、摇艏(偏航)运动自由度。一般情况下，水下机器人是通过推进器和舵翼实现6个自由度运动的。速度较慢水下机器人一般装有多个推进器，通过推进器及它们之间的差动实现各个自由度的运动，但多个推进器会增加水对水下机器人的阻力，进而消耗更多的能量。很多速度较高的流线式水下机器人尾部装有一个主推进器的同时，还安装有舵，当舵转动时，可使水下机器人呈俯仰(升沉舵)或摇艏(转向舵)运动状态，进而改变下潜深度或艏向角。这种水下机器人的执行器之间不存在功能冗余，一旦某个执行器出现故障将很难实现高效容错，造成机器人损坏乃至丢失等无法挽回的损失。

专利申请号为CN 2892668Y、名称为《舵翼式微小型水下机器人》的专利，执行器之间不具备冗余功能，容错能力差，安全性低。其躯体为具有扁圆截面的长方形壳体，推进器位于躯体两侧，运行时阻力系数较大。

(三)发明内容

本发明的目的在于提供一种智能化程度高，容错能力强，安全性好，运动灵活，结构调整方便的冗余控制的微小型水下机器人。本发明的目的还在于提供这种微小型水下机器人的故障诊断和容错控制方法。

本发明的目的是这样实现的：

本发明的冗余控制的微小型水下机器人的结构组成为：

其壳体由下壳体，上前壳、上后壳拼接而成的流线型外形；壳体内部自前向后设置倾角罗盘舱、深度计、电池舱、多普勒速度计、电子舱、水平螺旋桨推进器和转向升沉舵机；深度计、电池舱、多普勒速度计平行排列共同位于倾角罗盘舱之后、转向升沉舵机位于两个水平螺旋桨推进器中间，两个水平螺旋桨推进器和一个升沉转向舵机横向平行排列、并共同位于水下机器人的尾部；倾角罗盘舱、电池舱和电子舱均为立方体外形的箱体，每个箱体由舱体、上盖、密封圈和螺钉组成，舱体上端面上开有环形密封槽，密封槽内装有O型密封圈，上盖与舱体间通过螺钉连接，并将O型密封圈压紧，实现端面密封，上盖左侧开有螺纹孔，通过螺纹实现与防水电缆螺旋接头连接，舱体外侧留有连接孔，螺栓穿过此连接孔并旋入骨架板上螺纹孔内，实现连接；电池舱内部的锂电池组通过导线同舱侧壁安装的电力传输插座和舱上盖上安装的充电插座相连；电力通过电力传输插座传出给电子舱；电子舱内部封装有核心控制系统，转向升沉舵机驱动控制系统，通信系统。

本发明的冗余控制的微小型水下机器人还可以包括这样一些结构：

1、升沉舵轴在与推进器的电机输出轴相交处为曲轴结构。

2、壳体内设置骨架板，各部件均固定在骨架板上，骨架板为具有足够强度的支撑板，板上设计有槽和孔，各部件通过螺钉连接到骨架板上，骨架板位于水下机器人中心对称面偏下处。

3、各部件间电力及通讯连接通过传输接头实现，传输接头由多芯防水电缆、防水电缆螺旋接头、防水圆形连接器组成，防水电缆螺旋接头固连在部件舱体上，与防水电缆的一端连接，防水圆形连接器插头与防水电缆的另一端连接，防水圆形连接器插头与安装在部件侧壁上的防水圆形连接器插座连接。

4、下壳体内有六个支撑柱，两端带有螺纹孔的导柱与穿过骨架扳的支撑柱通过螺纹连接，导柱另一端与穿过上前壳或上后壳的螺钉连接，下壳体、上前壳、上后壳便与水下机器人内的骨架板装配到一起，形成完整的流线型支柱外形。

本发明的微小型水下机器人的故障诊断和容错控制方法为：

首先，进行水下机器人推进器或转向舵故障诊断：机器人运行过程中，推进器自诊断得到自身是否发生故障及故障程度信息并返回给核心控制系统；如果推进器自诊断有故障，则认为转向舵正常，当推进器自诊断无故障，倾角罗盘测得角度与目标角度对比获得的艏向角残差未超出阈值时，认为推进器和转向舵正常；当推进器自诊断无故障，倾角罗盘测得角度与目标角度对比获得的艏向角残差超出阈值时，认为转向舵出现故障，水下机器人转入主动诊断方式，主动诊断的过程如下：水下机器人首先转入定速直航，再转入定角度航行状态，根据故障时艏向角残差及角速度返回值的变化规律判断舵的故障类型；

其次，基于故障诊断结果进行控制力分配，推进器和转向舵正常时，控制力分配方法为：使二水平推进器的出力相同，通过调整转向、升沉舵来控制机器人的艏向角度和俯仰角度；转向舵卡死在固定角度时，通过控制二推进器推力差值的方法在抵消转向舵产生的转矩的同时控制机器人的艏向角度；发生推进器故障或舵缺损、丢失故障时，控制力分配则是通过线性二次型最优控制方法，进行基于故障的控制力优化分配，实现容错控制。

本发明具有如下优点：

1、具有功能上存在冗余的转向舵和水平螺旋桨推进器，为执行器容错控制提供了硬件基础。

本机器人安装了可调角度的转向舵，它与两个水平螺旋桨推进器共同推动水下机器人实现直航和转艏运动，由于转向舵与水平螺旋桨推进器在功能上存在冗余，因此，当它们出现故障时，能够进行相互补偿，达到容错控制的目的。

2、具有故障诊断和容错控制能力，智能化程度高。

水下机器人根据推进器的自诊断信息与倾角罗盘、多普勒速度计等传感器的反馈信息，可以实现对推进器与转向舵的故障诊断；并能够根据故障信息进行控制力重新分配，达到容错控制的目的。

3、具有流线型支柱外形，能充分减小水阻力，使耗能降低。

由于能源问题一直是水下机器人技术不可逾越的瓶颈，因此，低能耗对于水下机器人，特别是内部容量小的微小型水下机器人来说是非常重要的。本发明中的水下机器人具有良好的流线型，充分降低了运动时对能量的需求。

4、拆装和重组方便。

水下机器人各部件通过传输接头实现通讯及电力连接，这便于实现各部件间的分离与连接；另外，各部件均为单独密封结构，与骨架板之间仅通过连接件实现连接，拆装和重组方便。

5、安全性高。

水下机器人一旦出现漏水现象，将会对机器人的安全性产生巨大影响，甚至会导致机器人重要部件的损坏或报废。本机器人各部件采用单独密封的方式，并且均装有漏水检测模块，能充分保证机器人的安全性。

6、体积小，重量轻。

体积小有利于机器人的隐蔽性，并可保证在狭小的水域内运行；重量轻降低了机器人的惯性，使其运动灵活，起、停方便。

(四)附图说明

图1-1为水下机器人外形俯视图；

图1-2为图1-1的左视图；

图1-3为图1-1的A向视图；

图2-1为水下机器人去掉上壳后的俯视图；

图2-2为图2-1的轴剖视图；

图2-3是图2-2的I部放大图；

图3为传输接头断开状态；

图4为左侧升沉舵轴；

图5-1为倾角罗盘密封舱结构；

图5-2为倾角罗盘舱体俯视图；

图6为水下机器人控制结构框图；

图7为故障诊断和容错控制流程图。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1-2、2-1、2-2，水下机器人的壳体由下壳体1，上前壳2、上后壳3组成，下壳体1内设计有六个支撑柱23，两端带有螺纹孔的导柱24与穿过骨架扳16的支撑柱23通过螺纹连接，上后壳3上对应位置设计有四个安装孔，螺钉25穿过上后壳的安装孔与导柱24连接，使上后壳3与下壳体1装配成一体，上前壳2上设计有两个安装孔，螺钉25穿过上前壳导柱24连接，使上前壳能够安装在下壳体及上后壳之上，并使下壳体1、上前壳2、上后壳3与水下机器人内的骨架板装配到一起，形成完整的流线型支柱外形。当水下机器人电能不足或需要调整内部结构时，只需将上前壳上的两个螺钉取下即可充电或进行调整。

水下机器人内部组成如图2-1、2-2所示，由传输接头4、连接螺钉5、倾角罗盘舱6、电池舱7、充电插座8、多普勒速度计9、深度计10、电子舱11、转向升沉舵机12、舵机连接板13、推进器连接板14、水平螺旋桨推进器15、骨架板16、齿轮轴17、圆锥齿轮副18、轴承支架19、升沉舵连接轴I 20、升沉舵21、升沉舵连接轴II 22、转向舵26、螺旋桨27组成。

水下机器人的外壳是密封结构，其内部部件都设计成单独密封的形式，主要组成部件有倾角罗盘舱6、电池舱7、电子舱11、一个转向升沉舵机12、两个水平螺旋桨推进器15。为了保证安全性，每一个密封舱体内部都安装有漏水检测模块。上述各部件通过连接螺钉5及连接板13、14固定在骨架板16上，进而形成统一的整体。

骨架板16由铝合金板制成，板上开出多个孔或槽，用来安装各舱体及其它零部件。

实现各部件间数据通信及电力传输的传输接头4结构如图3所示。它由特制的多芯防水电缆28、防水电缆螺旋接头29、防水圆形连接器30组成。多芯防水电缆28一端与防水圆形连接器30连接，另一端与固连在某部件舱体上的防水电缆螺旋接头29连接，防水圆形连接器30的插座安装在另一部件的舱体上。这种结构有利于各部件之间的拆装，当需要将两个单独密封的部件间的防水电缆断开或连接上时，只要将防水圆形连接器30从其插座上拔下或安装上即可。

本发明中所使用的平螺旋桨推进器和舵机结构分别是，专利申请号为200710144390.X的专利申请文件中记载的螺旋桨推进器，专利申请号为200710072392.2的专利申请文件中记载的舵机。

由于升沉舵连接轴与推进器输出轴处于同一水平面内，为避免干涉，升沉舵连接轴I20和升沉舵连接轴II 22在与推进器输出轴交叉处设计成曲轴结构，详见附图4。

倾角罗盘舱6如图5-1所示。为了防止外界电磁干扰，倾角罗盘舱体采用金属材料制成，倾角罗盘舱由上盖31、螺钉32、O型密封圈33、舱体34组成，舱体34结构参见图5-2，舱体上端面上开有环形密封槽，密封槽内装有O型密封圈33，上盖与舱体间通过螺钉32连接，并将O型密封圈33压紧，实现端面密封；上盖31左侧开有螺纹孔，通过螺纹实现与防水电缆螺旋接头连接。舱体34外侧留有连接孔，连接螺钉5穿过此连接孔并旋入骨架板16上的螺纹孔内，实现连接。电池舱7和电子舱11结构与倾角罗盘舱6类似，由于内部不受电磁场的影响，因此，也可采用非金属材料制成。

水下机器人控制结构参见图6。当水下机器人开始航行时，电池舱7内部电池将电能通过传输接头4传送到电子舱11内，并经过驱动控制元件传递给水平螺旋桨推进器15内的直流电机和转向升沉舵机12内的步进电机。上位机发出工作指令，通过传输系统传给电子舱11内部的核心控制系统，核心控制系统发出控制指令给两个水平螺旋桨推进器15内的直流电机和转向升沉舵机12内的步进电机，直流电机带动螺旋桨27转动产生推力，推动水下机器人产生直航运动和摇艏运动，转向升沉舵机12的两个步进电机分别带动升沉舵21和转向舵26偏转，促使水下机器人产生俯仰运动和摇艏运动。深度计9、多普勒速度计10、倾角罗盘舱6内部的倾角罗盘和各舱体内部的漏水检测系统分别检测水下机器人的深度、速度、角度及漏水信息，并反馈到电子舱11内的核心控制系统，核心控制系统根据检测信息进行控制运算，得出所需的力和转矩，再进行控制力分配得出控制指令、并将检测信息及故障信息通过通信系统传给上位机。

下面参照图7更详细地描述推进器及转向舵的故障诊断和容错控制过程。由于推进器和转向舵同时发生故障的几率很小，因此只对推进器或转向舵单独发生故障情况进行故障诊断和容错控制。

水下机器人推进器或转向舵故障诊断方法如下：

机器人运行过程中，推进器自诊断得到自身的故障信息及相应的故障程度因子w₁，w₂，并将它们返回给核心控制系统；w₁，w₂代表各推进器的能量特性，取值范围为(0，1]，推进器正常时为1、部分失效时为小数。如果推进器自诊断有故障，可以认为转向舵正常，转向舵的故障特征因子α值为1。当推进器自诊断无故障，而倾角罗盘测得角度与目标角度对比获得的艏向角残差也未超出阈值时，认为推进器和转向舵均正常；当推进器自诊断无故障，而倾角罗盘测得角度与目标角度对比获得的艏向角残差超出阈值时，认为转向舵出现故障。为了判断转向舵的故障状态，需要水下机器人转入主动诊断方式。水下机器人首先转入定速直航状态，若残差很大，机器人角速度急剧变化，则认为发生转向舵卡死故障，根据残差及水下机器人的速度信息确定卡死后转向舵产生的力矩A_r；否则转入定向航行。定向航行时，若角度残差很大，机器人角速度基本不发生变化时，认为发生舵丢失故障，故障特征因子α值为0；若残差较小且超出阈值，机器人角速度发生变化时，认为发生舵部分缺损故障，并根据残差的大小确定故障特征因子α(α的取值为小数)；若定向航行时残差超出阈值，又不属于上述故障，则执行紧急处理(如上浮)命令。

转向舵或推进器发生故障时的容错控制方法为：

当推进器和转向舵均正常时，令二水平推进器出力相同，通过调整转向、升沉舵来控制机器人的艏向角度和俯仰角度。

当转向舵卡死在固定角度时，产生的力矩为只与速度相关的值A_r，此时容错控制需要通过增加二推进器推力差值的方法抵消转向舵产生的转矩，左右推进器推力可按如下矩阵计算：

$\left[\begin{array}{c}{X}_A\\ N_A\end{array}\right]=\left[\begin{array}{c}0\\ A_r\end{array}\right]+\left[\begin{array}{cc}1& 1\\ l& -l\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]$

其中：X_A，N_A为所需的力和力矩；l为推进器到水下机器人中心对称面的距离；T₁，T₂为左、右推进器推力。T₁，T₂按下述方程转换成左、右推进器的控制电压u₁，u₂：

$\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]=\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]$

其中：k₁，k₂为正的常数。

当推进器发生故障或转向舵发生缺损或丢失故障时，容错控制是基于如下的控制力优化分配方程实现的：

$\underset{T,\mathrm{\δ}}{\min }\{J={\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]}^T\left[\begin{array}{cc}{w}_1& 0\\ 0& w_2\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]+{\mathrm{\α\δ}}^2\}$

subject to：

$\left[\begin{array}{c}{X}_A\\ N_A\end{array}\right]=\left[\begin{array}{c}0\\ N_{\mathrm{\δ}}\end{array}\right]{\mathrm{\δ}}_r+\left[\begin{array}{cc}1& 1\\ l& -l\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]$

$\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]=\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]$

其中：N_δ为与速度有关的舵力参数，δ为舵角。上述方程基于推进器和转向舵的故障程度信息w₁，w₂，α，通过线性二次型最优控制方法，进行基于故障的控制力优化分配，实现容错控制。

下面结合图6和7，阐述整个故障诊断和容错控制步骤：

1、水平螺旋桨推进器15首先进行自诊断，并将诊断结果发送给电子舱11内的核心控制系统。如果推进器出现故障，则核心控制系统基于推进器故障程度通过线性二次型最优控制方法进行的控制力优化分配；如果没有故障，则核心控制系统判断艏向角残差是否超出阈值。

2、如果艏向角残差没超出阈值，则进行正常的控制力分配；如果艏向角超出阈值，则认为转向舵26出现故障，并进行主动诊断，来判断转向舵26的故障状态。

3、进行定速直航主动诊断，以诊断转向舵26是否发生卡死故障，如果是舵卡死故障，则根据诊断得出的转向力矩，进行舵卡死时的控制力分配；如果不是舵卡死故障，则转入定角度直航主动诊断，判断转向舵26是否发生丢失或缺损故障。

4、如果是舵丢失或缺损故障，则基于转向舵26的故障程度，通过线性二次型最优控制方法进行控制力优化分配；否则，认为出现了不可补偿的未知故障，核心控制系统执行紧急处理命令。

5、在执行完上述各控制力分配操作之后，判断是否结束程序。如果不结束，则重复步骤1到4；否则，结束任务。

Claims (7)

1.一种冗余控制的微小型水下机器人，其特征是：其壳体由下壳体，上前壳、上后壳拼接而成的流线型外形；壳体内部自前向后设置倾角罗盘舱、深度计、电池舱、多普勒速度计、电子舱、水平螺旋桨推进器和转向升沉舵机；深度计、电池舱、多普勒速度计平行排列共同位于倾角罗盘舱之后，转向升沉舵机位于两个水平螺旋桨推进器中间，两个水平螺旋桨推进器和一个升沉转向舵机横向平行排列、并共同位于水下机器人的尾部；倾角罗盘舱、电池舱和电子舱均为立方体外形的箱体，每个箱体由舱体、上盖、密封圈和螺钉组成，舱体上端面上开有环形密封槽，密封槽内装有O型密封圈，上盖与舱体间通过螺钉连接，并将O型密封圈压紧，实现端面密封，上盖左侧开有螺纹孔，通过螺纹实现与防水电缆螺旋接头连接，舱体外侧留有连接孔，螺钉穿过此连接孔并旋入骨架板上螺纹孔内，实现连接；电池舱内部的锂电池组通过导线同舱侧壁安装的电力传输插座和舱上盖上安装的充电插座相连；电力通过电力传输插座传出给电子舱；电子舱内部封装有核心控制系统，转向升沉舵机驱动控制系统，通信系统。

2.根据权利要求1所述的冗余控制的微小型水下机器人，其特征是：升沉舵轴在与推进器的电机输出轴相交处为曲轴结构。

3.根据权利要求2所述的冗余控制的微小型水下机器人，其特征是：壳体内设置骨架板，各部件均固定在骨架板上，骨架板为具有足够强度的支撑板，板上设计有槽和孔，各部件通过螺钉连接到骨架板上，骨架板位于水下机器人中心对称面偏下处。

4.根据权利要求3所述的冗余控制的微小型水下机器人，其特征是：各部件间电力及通讯连接通过传输接头实现，传输接头由多芯防水电缆、防水电缆螺旋接头、防水圆形连接器组成，防水电缆螺旋接头固连在部件舱体上，与防水电缆的一端连接，防水圆形连接器插头与防水电缆的另一端连接，防水圆形连接器插头与安装在部件侧壁上的防水圆形连接器插座连接。

5.根据权利要求4所述的冗余控制的微小型水下机器人，其特征是：下壳体内有六个支撑柱，两端带有螺纹孔的导柱与穿过骨架扳的支撑柱通过螺纹连接，导柱另一端与穿过上前壳或上后壳的螺钉连接，下壳体、上前壳、上后壳便与水下机器人内的骨架板装配到一起，形成完整的流线型支柱外形。

6.一种如权利要求1所述的冗余控制的微小型水下机器人的故障诊断和容错控制方法，其特征是：

首先进行水下机器人推进器或转向舵故障诊断：机器人运行过程中，推进器自诊断得到自身是否发生故障及故障程度信息并返回给核心控制系统；如果推进器自诊断有故障，则认为转向舵正常，当推进器自诊断无故障，倾角罗盘测得角度与目标角度对比获得的艏向角残差未超出阈值时，认为推进器和转向舵正常；当推进器自诊断无故障，倾角罗盘测得角度与目标角度对比获得的艏向角残差超出阈值时，认为转向舵出现故障，水下机器人转入主动诊断方式，主动诊断的过程如下：水下机器人首先转入定速直航，再转入定角度航行状态，根据故障时艏向角残差及角速度返回值的变化规律判断舵的故障类型；

其次，基于故障诊断结果进行控制力分配；推进器和转向舵正常时，控制力分配方法为：使二水平推进器的出力相同，通过调整转向、升沉舵来控制机器人的艏向角度和俯仰角度；转向舵卡死在固定角度时，控制力分配时通过控制二推进器推力差值的方法在抵消转向舵产生的转矩的同时控制机器人的艏向角度；发生推进器故障或舵缺损、丢失故障时，控制力分配则是通过线性二次型最优控制方法，进行基于故障的控制力优化分配，实现容错控制。

7.根据权利要求6所述的冗余控制的微小型水下机器人的故障诊断和容错控制方法，其特征是：所述的基于故障诊断结果进行控制力分配的具体方法为：

当推进器和转向舵均正常时，令二水平推进器出力相同，通过调整转向、升沉舵来控制机器人的艏向角度和俯仰角度；

当转向舵卡死在固定角度时，产生的力矩为只与速度相关的值A_r，此时容错控制需要通过增加二推进器推力差值的方法抵消转向舵产生的转矩，左右推进器推力按如下矩阵计算：

$\left[\begin{array}{c}{X}_A\\ N_A\end{array}\right]=\left[\begin{array}{c}0\\ A_r\end{array}\right]+\left[\begin{array}{cc}1& 1\\ l& -l\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]$

其中：X_A，N_A为所需的力和力矩；l为推进器到水下机器人中心对称面的距离；T₁，T₂为左、右推进器推力；T₁，T₂按下述方程转换成左、右推进器的控制电

压u₁，u₂：

$\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]=\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]$

其中：k₁，k₂为正的常数；

当推进器发生故障或转向舵发生缺损或丢失故障时，容错控制是基于如下的控制力优化分配方程实现的：

$\underset{T,\mathrm{\δ}}{\min }\{J={\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]}^T\left[\begin{array}{cc}{w}_1& 0\\ 0& w_2\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]+\mathrm{\α}{\mathrm{\δ}}^2\}$

subject to ：

$\left[\begin{array}{c}{X}_A\\ N_A\end{array}\right]=\left[\begin{array}{c}0\\ N_{\mathrm{\δ}}\end{array}\right]{\mathrm{\δ}}_r+\left[\begin{array}{cc}1& 1\\ l& -l\end{array}\right]\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]$

$\left[\begin{array}{c}{T}_1\\ T_2\end{array}\right]=\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right]$

其中：N_δ为与速度有关的舵力参数，δ为舵角；上述方程基于推进器和转向舵的故障程度信息w₁，w₂，α，取值范围为(0，1)，通过线性二次型最优控制方法，进行基于故障的控制力优化分配，实现容错控制。

Images