一种基于双重判据的水下机器人推力优化分配方法
【技术领域】
[0001] 本发明属于水下机器人控制领域，尤其设及一种基于双重判据的水下机器人推力 优化分配方法。
【背景技术】
[0002] 占地球表面积75%的海洋，是一个富饶而远未得到开发的宝库。人类要维持生存 繁衍和发展，充分利用地球仅有的运块最后待开发的疆±，将是无可回避的选择。水下机器 人由于其行动灵活，能够在水中长时间工作而日益成为人类开发利用海洋资源的重要工 具，同时军用智能水下机器人在军事上也有用武之地，因此水下机器人的研制和应用具有 重要的战略意义。
[0003] 水下机器人的推力分配方法对其实现准确的定位、航行、作业、容错控制等具有重 要作用。水下机器人要在水中实现稳定地航行和作业，既要求保持水平位置，又要抵抗海流 影响；既要保持下潜深度，又要在作业中保持姿态，使其不会造成过大的横倾和纵倾;既要 优化使用其推力节约能量，还要在必要的时候实现容错控制。所W必须通过水下机器人的 推力优化分配才能使水下机器人更加广泛地应用于水下工程、海洋石油资源开发、海洋矿 物资源调查、海洋生物资源调查、深海打拱、核潜艇救生，船体检测等方面。
[0004] 申请号为CN201410583348的中国专利文件（公开日：2015年2月4号）中公开的"基 于CAM矩阵的水下机器人矢量推力分配方法"提供了一种针对多自由度矢量推进水下机器 人推力分配的方法。基于CAM矩阵的水下机器人矢量推力分配方法虽然与本专利属于同一
技术领域，但它只是将水下机器人每个多自由度矢量推进器分配贡献系数，通过将各自由 度需求指令组成的向量与每个推进器自由度对应的贡献系数向量做内积，得到推进器在该 方向上的控制指令。
【发明内容】
[0005] 本发明的目的是提供一种控制精度高的，基于双重判据的水下机器人推力优化分 配方法。
[0006] -种基于双重判据的水下机器人推力优化分配方法，包括W下步骤，
[0007] 步骤一，针对水下机器人的推进器布置情况建立推力分配模型，对矢量推进器在 每个自由度方向推力进行2范数和无穷大泛数双重判据的推力优化分配；
[000引步骤二:使用对偶原理将推力优化分配的原问题转化为线性等式的对偶问题，利 用神经网络方法求解该线性等式得到推力分配的优化解；
[0009] 步骤根据每个推进器的控制电压一推力曲线，将优化解得到的每个推进器推 力进行多次艾特金插值，得到推进器的输出电压，实现水下机器人多个自由度的运动控制。
[0010] 本发明一种基于双重判据的水下机器人推力优化分配方法，还可W包括：
[0011] 1、步骤一中双重判据的推力优化分配的求取方法为：
[0012] 首先根据水下机器人的推进器的布置形式建立推力分配模型，建立和水下机器人 惯性主轴重合的坐标系OXYZ分别指向机器人移动的纵向、横向和垂向；在水下机器人坐标 系OXYZ下推进器相对于质屯、的矢量和方向为[ri T2…rn]和[ei 62 ... en]，其中Ti= [Xi yi Zi]，ei=k〇s0i si址i sinai]，第i个推进器对机器人产生的推力是平，力矩是％当自动 控制中的推力分配模块收到水下机器人的控制指令后，根据控制指令和推进器布置形式得 到水下机器人所需的控制力矩阵为：
[0014]水下机器人推力的2范数形式为
.其F=[中2F ...平]，推力的无穷大 范数形式为MF||〇〇=max{|中M刊…1平|}，得到双重判据的推力优化分配问题：
[0016] St T = RF
[0017] 式中ae(0，l]是权值系数。
[0018] 2、将推力优化分配的原问题转化为线性等式为：
[0019] Kfi(g-(Hg+P))-g = 0
[0020] 其中，Kn( ? ):R3nwi一 Q为从空间RSnwl到集合Q的一线性投影操作数，其中Kq (g)中第i个元素定义为：
平1]T，心=TE度，和Fu=[中U 2Fu ...平u]T，分别表示推进器推力的下饱和界和上饱和界，Tu 和Tl分别为T的上界和下界，ln=[l 1 ... 1]T;
[0023] 建立将推力优化分配的原问题转化为线性等式的对偶问题的神经网络优化求解 器：
[0024] 玄= ^/+妒)卢。(《-(礎+巧)-《）
[00剧其中丫 >0是神经网络的比例系数，g为最终求得的优化解;叫是C=I+HT的第i行 第j列元素，tik是S = I-H的第i行第j列元素。
[00%] 有益效果：
[0027] 本发明是通过对矢量推进器在每个自由度方向推力进行2范数和无穷大泛数双重 判据的推力优化分配，可最大限度地利用推进器同时完成机器人的多自由度运动，对水下 机器人的精确控制、容错控制、抗扰控制、作业过程中的姿态稳定控制等有重要作用。
[0028] 本发明目的是提供一种基于双重判据的水下机器人推力优化分配方法，该方法能 够有效地将水下机器人六自由度的控制指令优化分配到各个推进器上，可用于开架式水下 机器人的精确控制、容错控制和水下机器人作业过程中的姿态稳定控制等工作。该发明具 有使用方便、灵活，适应性强并且具有一定通用性等优点。
【附图说明】
[0029] 图1是本发明的实现方法流程图；
[0030] 图2是对偶问题(2)的优化求解器式(4)的框图；
[0031 ]图3是式(5)神经网络优化求解器框图；
[0032] 图4是应用本发明进行容错控制的SY-2水下机器人及其推进器布置图；图4(a)是 SY-2水下机器人实物图，图4(b)是SY-2水下机器人推进器布置示意图；
[0033] 图5是敞水试验得到的SY-2水下机器人推进器的控制电压一推力曲线；图5(a)是 SY-2水下机器人正车推力曲线，图5 (b)是SY-2水下机器人倒车推力曲线；
[0034] 图6是应用本发明对SY-2水下机器人进行容错控制的控制曲线；
[0035] 图7是某作业型无人水下航行器-机械手系统及其推进器布置图；图7(a)是某作业 型无人水下航行器，图7(b)是航行器机械手系统，图7(c)是航行器推进器布置图；
[0036] 图8是作业过程中使用本发明进行优化控制得到的机器人姿态曲线；图8(a)是机 械手关节角曲线，图8 (b)是规划位姿曲线；
[0037] 图9是仿真得到的作业时水下机器人受到的机械手扰动力，图9(a)是机械手扰动 力曲线，图9(b)是机械手扰动力矩曲线。
【具体实施方式】
[0038] 下面将结合附图对本发明做进一步详细说明。
[0039] 发明目的实现步骤为：
[0040] 1)针对水下机器人的推进器布置情况建立推力分配模型，对矢量推进器在每个自 由度方向推力进行2范数和无穷大泛数双重判据的推力优化分配；
[0041] 2)使用对偶原理将推力优化分配的原问题转化为线性等式的对偶问题，利用神经 网络方法求解该线性等式得到推力分配的优化解；
[0042] 3)根据每个推进器的控制电压一推力曲线，将优化解得到的每个推进器推力进行 多次艾特金插值，得到推进器的输出电压，从而实现水下机器人多个自由度的运动控制。
[0043] 发明的详细实现方法为：
[0044] a、首先根据水下机器人的推进器的布置形式建立推力分配模型。建立和水下机器 人惯性主轴重合的坐标系OXYZ分别指向机器人移动的纵向、横向和垂向。在水下机器人坐 标系OXYZ下推进器相对于质屯、的矢量和方向为[ri T2 ... rn]和[ei 62 ... en]，其中Gi = kosPi sinPi SinaiLri= [Xi yi ZiL设第i个推进器对机器人产生的推力是平，力矩是iM， 则当自动控制中的推力分配模块收到水下机器人的控制指令后，根据控制指令和推进器布 置形式得到水下机器人所需的控制力矩阵为：
[0046]设水下机器人推力的2范数形式为
，其F=[中中...T]，推力的无穷 大范数形式为MF||〇〇=max{|iF| I2Fl…|平|}，推力分配问题按下式转化为下面的优化 问题：
(2)
[004引式中ae (0,1]是权值系数
巧=互三;'' E聰口眷。'其中尸1=[中1 2Fl ...平1]了，屯=了居巧，尸。=[中。2Fu…npu]T，分别表示 推进器推力的下饱和界和上饱和界，Tu和Tl分别为T的上界和下界，/€曲。。'，1。=[1 1…1]T， 皆=[化0,']€跟叫M，町=1'()曲4=0居胚8,设g=[s d2 di]T,沪L巧I扣巧]':，《-韦-1、,:阳化:,,]'6及3咖 -咨-《巧] 「々'- 巧=及2灯灯' f= -4。设Kq( ? ):R3"n+l一 Q为从空间RSnwl到集合Q的一线性投影操 巧.灯灯 4 作数，其中Kfi(g)中第i个元素定义为： Si if 《
[0化0] 备，r各：<装<备r 巧店{1，..'.，脚+打卡巧 掛 共 if 各,
[0051 ]则优化问题(2)等价于线性等式：
[0052] Kfi(g-(Hg+P))-g = 0 (4)
[0053] C、根据式(4)得到一个原问题-对偶问题的神经网络优化求解器：
[0054] 容= ^/+妒)(《。(备-(雌+/>))-备） 闽
[005引式中丫 >0是神经网络的比例系数，g即为最终求得的优化解。设u^C = I+HT的第 i行第j列元素，tik是S = I-H的第i行第j列元素，则式巧)的计算机实现形式为：
卿
[0057] d、将优化解得到的每个推进器推力根据所对应的该推进器敞水试验曲线进行多 次艾特金插值运样就可W有效地将水下机器人的六自由度运动控制指令优化分配到各