一种自主水下机器人多电机协同的容错控制方法

本发明属于容错控制，涉及一种自主水下机器人多电机协同的容错控制方法。

背景技术：

1、自主水下机器人主要工作在复杂的海洋环境中，其受到海流等外部海洋环境的干扰较大。自主水下机器人在工作的过程中，时常会有发生故障的情况，从而使得水下航行任务的失败，严重时甚至会导致机器人的损坏或者丢失。

2、多电机协同推进系统出现故障会影响到作业中水下机器人的稳定性，甚至出现严重的事故，如果在系统某一部分出现故障之后，其他部分仍然能够维持运行，这对于降低损失是非常有利的，因此需要对自主水下机器人多电机协同推进系统进行容错控制。另外，由于每台电机的技术参数不同，同时受负载变化、电机参数飘移等因素影响，如何保持多台电机高精度同步运行是首要问题。

3、水下机器人推进器容错控制可以分成主动容错控制及被动容错控制两类。但是被动容错控制虽然省去了故障检测的环节，但通常会降低整体的控制性能，主动容错控制方面，常用滑模容错控制等，但这种滑模容错控制器有特定的应用范围，无法应用于推进器完全失效的情况。因此及时诊断出推进器故障并采取相应的控制策略将有力保障auv的安全性。

技术实现思路

1、本发明的目的在于提供一种自主水下机器人多电机协同的容错控制方法。首先针对本发明应用的自主水下机器人进行数学模型的建立。针对推力饱和问题，本发明提出使用伪逆-自适应遗传切换算法进行推力分配求解，在推力饱和时，使用自适应遗传算法进行推力分配二次优化，提高全局优化能力，获得更好的推力分配结果。针对推进器故障问题，本发明提出推力分配容错与容错协调系数相结合的多电机协同容错控制方法，以有效提高推进器故障情况下自主水下机器人本体的动态平衡。

2、本发明的目的通过以下技术方案予以实现：

3、为达到上述目的，本发明提供一种自主水下机器人多电机协同的容错控制方法，所述方法包括以下步骤：

4、(1)建立两路侧推、两路垂推与一路主推的六自由度自主水下机器人推进器模型，其中1号电机为主推，2号、5号电机为前后侧推，3号、4号电机为前后垂推；

5、(2)运用伪逆-自适应遗传切换算法求解推力分配问题，首先利用伪逆法计算得到推力分配问题的初解，然后判断推力分配结果是否超过推进器推力极限，如果超过推力极限，则采用自适应遗传算法进行二次优化分配，更新推力分配的结果，推力二次优化分配所考虑的优化目标包括：消耗功率最小、实际推力输出与期望控制指令间的误差最小、避免推进器奇异结构，约束条件包含等式约束、推进器推力约束；

6、(3)设计基于推力分配的主动容错控制器，获取各个推进器的故障系数s1、s2、s3、s4、s5，其中，s1是主推的故障系数，s2、s5是前后侧推的故障系数，s3、s4是前后垂推的故障系数，引入故障权值矩阵w，当无故障时，基于推力分配矢量矩阵b对自主水下机器人进行推力分配，当故障发生时，基于原分配矩阵b与故障权值矩阵w重新进行推力分配，通过重新分配推力以达到补偿推进器故障的效果；

7、(4)建立虚拟主轴与偏差耦合相结合的多电机控制结构，针对推进器故障时的多电机协同问题，引入容错协调系数λi(t)维持自主水下机器人在动态干扰或者故障下的动态平衡。

8、本发明的目的还可通过以下技术措施进一步实现：

9、进一步的，步骤(1)具体包括：

10、步骤(1.1)：由螺旋桨转速和推力的关系，建立推进器的期望推力与转矩公式：

11、

12、其中，t为螺旋桨的推力，q为螺旋桨的转矩，kp为转矩系数，km为推力系数，ρ为海水密度，d为推进器螺旋桨直径，va为机器人进速，n为螺旋桨转速；

13、步骤(1.2)：auv推进系统模型可以用下式表示：

14、

15、式中，xprop为x轴的推力，yprop为y轴的推力，zprop为z轴的推力，kprop为推力围绕x轴作用的力矩，mprop为推力围绕y轴作用的力矩，nprop为推力围绕z轴作用的力矩；t1、t2、t3、t4、t5分别为1号、2号、3号、4号和5号推进器的推力；l1、l2、l3、l4、l5分别为1号、2号、3号、4号和5号推进器推力的力臂；q1、q2、q3、q4、q5分别为1号、2号、3号、4号和5号推进器的转矩，转矩在实际工作中作为扰动处理；

16、作用在自主水下机器人本体上的六自由度的控制矢量用下式表示：

17、τ＝bt          (3)

18、其中，τ＝[xprop yprop zprop kprop mprop nprop]t为非奇异终端滑模路径跟踪控制器中所输出的期望控制量，t＝[t1 t2 t3 t4 t5]为推进器的推力矢量，b为推力分配矢量矩阵。

19、进一步的，步骤(2)具体包括：

20、步骤(2.1)：采用伪逆法求取推力分配问题的初解：

21、运用拉格朗日乘子法求解目标函数：

22、

23、其中，j1为伪逆法求解推力分配问题的目标函数，tt为矩阵t的转置矩阵，w为对称的正定权值矩阵；

24、采用拉格朗日乘子法求解上式所描述的优化问题，可得推进器的推力分配为：

25、t＝bwτ＝[w-1bt(bw-1bt)-1]τ      (5)

26、其中，bw＝w-1bt(bw-1bt)-1为推进器分布矩阵b的伪逆矩，w-1为矩阵w的逆矩阵，bt为推力分配矢量矩阵b的转置矩阵，(bw-1bt)-1为矩阵bw-1bt的逆矩阵；

27、步骤(2.2)：若推力分配结果超过推进器推力极限，则进行推力分配二次优化，推力二次优化分配的目标函数为：

28、

29、其中，j2为使用自适应遗传算法进行推力分配二次优化的目标函数，s＝τ-bt，st为矩阵s的转置矩阵，ρ和ε是调节参数，det(bbt)为矩阵bbt的行列式；目标函数第一项ttwt反映了功耗最小原则，第二项stqs表明了推力分配应当保证各推进器实际产生的合力与期望控制量之间的误差尽可能地小，第三项是为了避免推进器的奇异结构；

30、等式约束：

31、bt+s＝τ       (7)

32、推力约束：

33、ti,min≤ti≤ti,max

34、δti,min≤δti≤δti,max         (8)

35、其中，i＝1，2，…，5；ti指推进器i的推力，ti,max、ti,min指推进器i推力的上限和下限；δti为推进器i的推力变化值，δti,min、δti,max为推进器i推力变化的上限和下限；

36、步骤(2.3)：使用自适应遗传算法求解推力分配二次优化的目标函数：

37、(1)设定初始种群，设置最大进化代数k，种群规模m，交叉概率pc，变异概率pm，随机生成m个个体作为初始化种群p0；

38、(2)采用实数编码方式；

39、(3)设计适应度函数，即：

40、f＝j2      (9)

41、(4)选择算子，即采用轮盘赌的方法选择个体；

42、(5)交叉算子，采用改进型交叉，具体设计如下：

43、首先，引入加速粒子群算法(apso)进行交叉操作：

44、

45、其中，i＝1，…，m；分别为种群个体i在第k次迭代中的速度与位置，k为自适应遗传算法迭代次数，c1、c2为学习因子，r为(0,1)的随机数，为群体全局极值的位置，c1r为随机数，可提供算法跳出局部最优使用；

46、然后，利用logistic方程来实现混沌变量的演变并对学习因子c2进行参数优化，迭代公式如下：

47、

48、当时，logistic为完全混沌状态；

49、最后，自适应的交叉率如下：

50、

51、其中，pc为交叉概率，p′c为上一代种群的交叉概率，k1、k2表示概率调节系数，k1的范围为：0<k1<1，k2的范围为：1<k2，fi为个体的适应度值，favg为种群中所有个体的平均适应度值，m为种群规模，fit*为被选择用于交叉操作的父代个体中适应度较大个体的适应度比例值；

52、(6)变异操作是跳出局部最优，全局寻优的重要保证，变异算子设计如下：

53、对种群中个体的染色体进行变异，以产生更优秀的个体：

54、xi＝x′i±0.5l×f(k)          (13)

55、其中，xi为变异后基因取值，x′i为变异前基因取值，l为基因取值范围，f(k)＝r(1-k/k)2，r为[0,1]范围内的随机数，k＝1,…,k，k为最大进化次数；

56、自适应的变异率如下：

57、

58、pm为变异概率，p′m为上一代种群的变异概率；

59、(7)终止条件，当连续若干代的最优值没有明显变化时，跳出主循环，否则，返回(4)。

60、进一步的，步骤(3)具体包括：

61、步骤(3.1)：当推进器i发生故障时，引入推进器故障因子εi，推进器故障因子能够反映推进器的故障程度，其取值定义为：

62、0≤εi≤1 i＝1,2,…,5         (15)

63、其中，εi＝1表示推进器处于正常工作状态；0<εi<1表示推进器出现部分故障；εi＝0表示推进器出现完全故障；

64、步骤(3.2)：引入故障权值矩阵w＝diag(w1,w2,…,wi)，对伪逆-自适应遗传切换算法进行改进，定义两个优化目标函数j1、j2中w的元素为：

65、

66、其中，δ为一常数，防止式中出现分母为0。

67、进一步的，步骤(4)具体包括：

68、步骤(4.1)：提出了一种虚拟主轴与偏差耦合相结合的控制策略，使用一条虚拟的主轴和5台电机相连，同时加入了偏差耦合的速度补偿器；

69、由刚体定轴转动定理可得：

70、

71、式中，tm为虚拟机械主轴力矩，ti为从运动轴i的反馈力矩，j为主轴转动惯量，ωm为虚拟机械主轴旋转角速度，为ωm对时间t的一阶导数，其中tm的计算公式如下：

72、tm＝bm(ω*-ωm)+am∫(ω*-ωm)dt      (18)

73、式中,bm为主轴阻尼系数，am为主轴弹性系数,ω*为5台电机给定转速绝对值最大的值作为虚拟主轴的输入；ti的计算公式如下:

74、ti＝bi(ωki-ωei-ωi)+ai∫(ωki-ωei-ωi)dt      (19)

75、式中，bi为虚拟i轴与主轴间的阻尼系数，ai为弹性系数，ωi为电机实际转速，ωki为ωm经过比例变换后的转速，ωei为速度补偿器i的输出转速；

76、为了更好地分配比例系数，防止出现比例系数过小乃至遇到电机给定转速为0的情况，控制器首先通过比较得出这5台电机给定转速绝对值最大的给定转速设为ω*，由此ki的计算公式如下：

77、

78、其中，比例系数ki的绝对值小于等于1，为了防止ωi过小导致其倒数出现较大的值乃至无解，对ki进行限幅，当ki的绝对值小于0.1时，令ki的绝对值为0.1；

79、步骤(4.2)：在虚拟主轴和偏差耦合相结合的多电机控制结构下，若检测到第i台电机故障，i＝1,2,…,5，则输出布尔类型的故障标志信号fi(t)＝1，否则输出fi(t)＝0；容错决策模块根据各电机故障标志fi(t)及输出转速ωi(t)，实时调整各电机系统容错协调系数λi(t)；

80、步骤(4.3)：第i台电机的期望转速为kiω*，当检测出第j台电机出现故障时，即fj(t)＝1，容错决策模块设置λj(t)＝1并计算出容错协调系数：

81、

82、当电机系统无故障发生，即各个电机的故障标志信号均为零，此时设定各个电机的容错协调系数λi(t)＝1，此时容错结构不生效。

83、与现有技术相比，本发明的有益效果是：

84、1、通过本发明所提出的自主水下机器人推力分配方法为各推进器分配的推力值，能够满足对自主水下机器人期望推力的准确跟踪；

85、2、与传统伪逆法相比，本发明提出的伪逆-自适应容错切换算法在求解推力分配的问题上方便处理推进器各种限制条件，计算简单，提高了全局寻优能力；

86、3、通过修改故障权值矩阵，本发明能够在推进器故障情况下，一方面基于故障信息降低故障推进器的优先使用级，另一方面会将期望控制力和控制力矩有现在剩余推进器内分配，实现容错控制；

87、4、建立了虚拟主轴与偏差耦合相结合的多电机控制结构，当推进器故障时，引入了容错协调系数，降低了容错控制方法的实现复杂性，维持容错控制结构下多电机系统的同步性能。