一种基于双重判据的水下机器人推力优化分配方法与流程

本发明属于水下机器人控制领域，尤其涉及一种基于双重判据的水下机器人推力优化分配方法。

背景技术：
占地球表面积75％的海洋，是一个富饶而远未得到开发的宝库。人类要维持生存繁衍和发展，充分利用地球仅有的这块最后待开发的疆土，将是无可回避的选择。水下机器人由于其行动灵活，能够在水中长时间工作而日益成为人类开发利用海洋资源的重要工具，同时军用智能水下机器人在军事上也有用武之地，因此水下机器人的研制和应用具有重要的战略意义。水下机器人的推力分配方法对其实现准确的定位、航行、作业、容错控制等具有重要作用。水下机器人要在水中实现稳定地航行和作业，既要求保持水平位置，又要抵抗海流影响；既要保持下潜深度，又要在作业中保持姿态，使其不会造成过大的横倾和纵倾；既要优化使用其推力节约能量，还要在必要的时候实现容错控制。所以必须通过水下机器人的推力优化分配才能使水下机器人更加广泛地应用于水下工程、海洋石油资源开发、海洋矿物资源调查、海洋生物资源调查、深海打捞、核潜艇救生，船体检测等方面。申请号为CN201410583348的中国专利文件(公开日：2015年2月4号)中公开的“基于CAM矩阵的水下机器人矢量推力分配方法”提供了一种针对多自由度矢量推进水下机器人推力分配的方法。基于CAM矩阵的水下机器人矢量推力分配方法虽然与本专利属于同一技术领域，但它只是将水下机器人每个多自由度矢量推进器分配贡献系数，通过将各自由度需求指令组成的向量与每个推进器自由度对应的贡献系数向量做内积,得到推进器在该方向上的控制指令。

技术实现要素：
本发明的目的是提供一种控制精度高的，基于双重判据的水下机器人推力优化分配方法。一种基于双重判据的水下机器人推力优化分配方法，包括以下步骤，步骤一，针对水下机器人的推进器布置情况建立推力分配模型，对矢量推进器在每个自由度方向推力进行2范数和无穷大泛数双重判据的推力优化分配；步骤二：使用对偶原理将推力优化分配的原问题转化为线性等式的对偶问题，利用神经网络方法求解该线性等式得到推力分配的优化解；步骤三：根据每个推进器的控制电压—推力曲线，将优化解得到的每个推进器推力进行多次艾特金插值，得到推进器的输出电压，实现水下机器人多个自由度的运动控制。本发明一种基于双重判据的水下机器人推力优化分配方法，还可以包括：1、步骤一中双重判据的推力优化分配的求取方法为：首先根据水下机器人的推进器的布置形式建立推力分配模型，建立和水下机器人惯性主轴重合的坐标系OXYZ分别指向机器人移动的纵向、横向和垂向；在水下机器人坐标系OXYZ下推进器相对于质心的矢量和方向为[r1r2…rn]和[e1e2...en]，其中ri＝[xiyizi]，ei＝[cosβisinβisinαi]，第i个推进器对机器人产生的推力是iF，力矩是iM，当自动控制中的推力分配模块收到水下机器人的控制指令后，根据控制指令和推进器布置形式得到水下机器人所需的控制力矩阵为：水下机器人推力的2范数形式为其F＝[1F2F...nF]，推力的无穷大范数形式为||F||∞＝max{|1F||2F|…|nF|}，得到双重判据的推力优化分配问题：stτ＝RF式中a∈(0,1]是权值系数。2、将推力优化分配的原问题转化为线性等式为：KΩ(g-(Hg+P))-g＝0其中，KΩ(·):R3n+n+1→Ω为从空间R3n+n+1到集合Ω的一线性投影操作数，其中KΩ(g)中第i个元素定义为：g＝[Sd2d1]T,S＝[1F2F...4F||F||∞]，Fl＝[1Fl2Fl...nFl]T，和Fu＝[1Fu2Fu...nFu]T,分别表示推进器推力的下饱和界和上饱和界，τu和τl分别为τ的上界和下界，1n＝[11...1]T；建立将推力优化分配的原问题转化为线性等式的对偶问题的神经网络优化求解器：其中γ＞0是神经网络的比例系数，g为最终求得的优化解；cij是C＝I+HT的第i行第j列元素,tik是S＝I-H的第i行第j列元素。有益效果：本发明是通过对矢量推进器在每个自由度方向推力进行2范数和无穷大泛数双重判据的推力优化分配，可最大限度地利用推进器同时完成机器人的多自由度运动，对水下机器人的精确控制、容错控制、抗扰控制、作业过程中的姿态稳定控制等有重要作用。本发明目的是提供一种基于双重判据的水下机器人推力优化分配方法，该方法能够有效地将水下机器人六自由度的控制指令优化分配到各个推进器上，可用于开架式水下机器人的精确控制、容错控制和水下机器人作业过程中的姿态稳定控制等工作。该发明具有使用方便、灵活，适应性强并且具有一定通用性等优点。附图说明图1是本发明的实现方法流程图；图2是对偶问题(2)的优化求解器式(4)的框图；图3是式(5)神经网络优化求解器框图；图4是应用本发明进行容错控制的SY-2水下机器人及其推进器布置图；图4(a)是SY-2水下机器人实物图，图4(b)是SY-2水下机器人推进器布置示意图；图5是敞水试验得到的SY-2水下机器人推进器的控制电压—推力曲线；图5(a)是SY-2水下机器人正车推力曲线，图5(b)是SY-2水下机器人倒车推力曲线；图6是应用本发明对SY-2水下机器人进行容错控制的控制曲线；图7是某作业型无人水下航行器-机械手系统及其推进器布置图；图7(a)是某作业型无人水下航行器，图7(b)是航行器机械手系统，图7(c)是航行器推进器布置图；图8是作业过程中使用本发明进行优化控制得到的机器人姿态曲线；图8(a)是机械手关节角曲线，图8(b)是规划位姿曲线；图9是仿真得到的作业时水下机器人受到的机械手扰动力，图9(a)是机械手扰动力曲线，图9(b)是机械手扰动力矩曲线。具体实施方式下面将结合附图对本发明做进一步详细说明。发明目的实现步骤为：1)针对水下机器人的推进器布置情况建立推力分配模型，对矢量推进器在每个自由度方向推力进行2范数和无穷大泛数双重判据的推力优化分配；2)使用对偶原理将推力优化分配的原问题转化为线性等式的对偶问题，利用神经网络方法求解该线性等式得到推力分配的优化解；3)根据每个推进器的控制电压—推力曲线，将优化解得到的每个推进器推力进行多次艾特金插值，得到推进器的输出电压，从而实现水下机器人多个自由度的运动控制。发明的详细实现方法为：a、首先根据水下机器人的推进器的布置形式建立推力分配模型。建立和水下机器人惯性主轴重合的坐标系OXYZ分别指向机器人移动的纵向、横向和垂向。在水下机器人坐标系OXYZ下推进器相对于质心的矢量和方向为[r1r2...rn]和[e1e2...en]，其中ei＝[cosβisinβisinαi]，ri＝[xiyizi]，设第i个推进器对机器人产生的推力是iF，力矩是iM，则当自动控制中的推力分配模块收到水下机器人的控制指令后，根据控制指令和推进器布置形式得到水下机器人所需的控制力矩阵为：设水下机器人推力的2范数形式为其F＝[1F2F...nF]，推力的无穷大范数形式为||F||∞＝max{|1F||2F|…|nF|}，推力分配问题按下式转化为下面的优化问题：式中a∈(0,1]是权值系数b、设S＝[1F2F…4F||F||∞]，其中Fl＝[1Fl2Fl…nFl]T，Fu＝[1Fu2Fu…nFu]T,分别表示推进器推力的下饱和界和上饱和界，τu和τl分别为τ的上界和下界，1n＝[11…1]T,设g＝[Sd2d1]T,设KΩ(·):R3n+n+1→Ω为从空间R3n+n+1到集合Ω的一线性投影操作数，其中KΩ(g)中第i个元素定义为：则优化问题(2)等价于线性等式：KΩ(g-(Hg+P))-g＝0(4)c、根据式(4)得到一个原问题-对偶问题的神经网络优化求解器：式中γ＞0是神经网络的比例系数，g即为最终求得的优化解。设cij是C＝I+HT的第i行第j列元素,tik是S＝I-H的第i行第j列元素，则式(5)的计算机实现形式为：d、将优化解得到的每个推进器推力根据所对应的该推进器敞水试验曲线进行多次艾特金插值这样就可以有效地将水下机器人的六自由度运动控制指令优化分配到各个推进器上，得到推进器的输出电压。本发明提供的是一种基于双重判据的水下机器人推力优化分配方法，该方法能够有效地将水下机器人六自由度的控制指令优化分配到各个推进器上。首先针对水下机器人的推进器布置情况建立推力分配模型，其次对矢量推进器在每个自由度方向推力进行2范数和无穷大泛数双重判据的推力优化分配；然后使用对偶原理将推力优化分配的原问题转化为线性等式的对偶问题，利用神经网络方法求解该线性等式得到推力分配的优化解；最后根据每个推进器的控制电压—推力曲线，将优化解得到的每个推进器推力进行多次艾特金插值，得到推进器的输出电压，从而实现水下机器人多个自由度的运动控制。该方法可最大限度地利用推进器同时完成机器人的多自由度运动，对水下机器人的精确控制、容错控制、抗扰控制、作业过程中的姿态稳定控制等方面有重要作用。本发明提供的一种基于双重判据的水下机器人推力优化分配方法，通过对矢量推进器在每个自由度方向的2范数和无穷大泛数的双重判据优化分配，可最大限度地利用推进器同时完成机器人的多自由度运动。应用本发明所提供的方法实现了SY-2开架水下机器人的容错控制实验，某水下机器人作业过程中的手艇协调控制仿真。实施步骤如图1所示：首先根据水下机器人推进器布置形式建立推力分配模型，再依据控制指令和推进器布置形式得到期望控制力矩阵，将其表示成推力的2范数形式和无穷大范数形式，则推力分配问题可转化成基于双重判据的推力优化问题。使用对偶原理将推力优化分配的原问题转化线性等式(7)，利用神经网络方法求解线性等式得到推力分配的优化解(求解框图如图2和图3所示)。然后根据该推进器的控制电压—推力曲线，将优化解得到的每个推进器推力进行多次艾特金插值，得到推进器的输出电压，从而实现水下机器人多个自由度的运动控制。结合图2，设第i个推进器对机器人产生的推力是iF，F＝[1F2F…nF]，其中Fl＝[1Fl2Fl...nFl]T，Fu＝[1Fu2Fu...nFu]T,分别表示推进器推力的下饱和界和上饱和界，τu和τl分别为τ的上界和下界，1n＝[11...1]T,S＝[1F2F…4F||F||∞]，g＝[Sd2d1]T,设KΩ(·):R3n+n+1→Ω为从空间R3n+n+1到集合Ω的一线性投影操作数，其中KΩ(g)中第i个元素定义为：则推力优化分配的原问题(2)转化为线性等式(8)：图3描述了本发明求解线性等式(推力优化分配的对偶问题)神经网络求解结构框图，网络包括输入层、隶属度层、规则层和输出层。设cij是C＝I+HT的第i行第j列元素,tik是S＝I-H的第i行第j列元素，则式(4)的计算机实现形式为(5)：图4(a)为应用本发明进行容错控制的SY-2开架水下机器人，图4(b)为其十字交叉形式的推进器布置图。图4(b)中x0，y0表示水平面坐标系。左、右主推螺旋桨分别固定在框架的两侧分别距离几何中心为r1和r2，艏、艉侧推螺旋桨分别固定在框架的前侧和后侧的正中间分别距离几何中心为为r3和r4，四个螺旋桨形成了冗余的，容错的，可靠的，合理的水平推力布置，控制机器人完成水平面的三自由度运动。结合图5，图(a)和(b)分别是根据SY-2水下机器人所对应的推进器敞水试验得到的正车推力曲线和倒车推力曲线，推进器为TECNADYNE的Model250。通过多次埃特金插值，可以利用该实验结果将推力分配结果转化为控制电压，实现对水下机器人水平面运动的控制。图6是利用本发明进行SY-2水下机器人容错控制的仿真结果曲线。图中比较了两种情况：a.推进器无故障的情况；b.左主推40％故障且艉侧推完全故障时的情况。机器人的期望运动轨迹为[0,0]→[85,130]→[0,200]→[150,200]→[150,130]→[0,0]。由仿真结果可知，本发明所设计的基于双重判据的水下机器人推力优化分配方法可以在推进器故障时重新进行推力分配，虽然在前进方向转变时有些误差，但基本也可完成故障时的点位轨迹控制。图7描述了是某无人水下航行器-机械手系统及其垂向推进器布置图，系统包含一个4自由度的水下机械手，本文将利用本发明的方法实现手艇协调控制时的推力优化分配。机械手每个关节的旋转角度定义为θ1，θ2，θ3和θ4，包含肩部的摆动关节和俯仰关节，肘部的俯仰关节和腕部的回转关节。水下航行器配有4个垂推，垂向推进器采用四边形的布置形式，推进器距离航行器的几何中心为rvi，i＝1～4。图8，为完成机械手的作业任务，无人水下航行器的控制器根据机械手的期望运动轨迹规划了一段AUV的运动轨迹，以期在作业过程中尽量减少机械手给无人水下航行器造成的恢复力(矩)和耦合作用力。图8(a)为机械手各关节的运动情况，图8(b)为规划得到的无人水下航行器轨迹、机械手肩关节(基关节)轨迹、肘关节轨迹和机械手末端轨迹曲线；仿真得到无人水下航行器实际轨迹、机械手肩关节(基关节)实际轨迹、肘关节实际轨迹和机械手末端实际轨迹曲线，仿真曲线是由无人水下航行器的控制算法结合本发明的基于双重判据的水下机器人推力优化分配方法分配垂向推力，同时控制航行器姿态和位移加以实现的。结合图9，无人水下航行器-机械手系统在作业的过程中，机械手会给无人水下航行器带来“扰动力(力矩)”，包括作业过程中浮心和重心变化派生的恢复力矩，无人水下航行器和机械手的耦合性造成的拖拽力。控制器根据系统的动力学模型，由无人水下航行器和机械手的实时运动姿态计算出机械手对无人水下航行器在的实时干扰力，即为图9观测到的干扰力和干扰力矩。由图可得机械手对水下航行器的扰动力与扰动力矩刚开始的时候出现较大抖动，随着时间的推移，横向力趋于零，纵向力，垂向力与转艏力矩趋于平稳，横倾力矩与转艏力矩逐渐增大并趋于平稳。说明本发明提出的基于双重判据的水下航行器推力优化分配方法能够合理地分配垂向推力，从而实现水下航行器作业过程中的的姿态稳定控制。