基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法及其优化系统

1.本发明属于轨迹优化技术领域，具体涉及一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法及其优化系统。


背景技术：

2.为满足我国探测开发海洋资源与发展深海作业技术的战略需要、进一步发展海洋强国基本战略，水下机器人-机械手系统(underwater vehicle-manipulatorsystem，uvms)，成为现阶段水下作业较为有效的手段。其可代替蛙人满足深海作业的需求，实现深海大范围的连续作业，能够更加有效的开发海洋资源并实现复杂环境下的作业需要。
3.目前水下机器人存在如下问题：水下双臂机器人的双臂在作业时对机器人的艇体分别产生耦合力矩，导致机器人需要耗费更多的能量来维持自身的平衡；水下双臂在作业时，主要的能量都用来维持机械臂的正常运转。
4.其次，水下机器人的运动优化过程实际上是一个约束多目标函数求解过程，对于优化过程中的约束多目标函数求解过程，使用较多的方法是少参数的骨干粒子群优化算法。但是传统的骨干粒子群算法存在着容易丢失种群多样性和陷入早熟收敛的缺点。


技术实现要素：

5.本发明提供一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，由于水下双臂机器人的双臂在作业时对机器人的艇体分别产生耦合力矩，导致机器人需要耗费更多的能量来维持自身的平衡；水下双臂在作业时，主要的能量都用来维持机械臂的正常运转，本发明主要改善双臂水下机器人在作业过程中能量消耗多的问题。
6.本发明提供一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化系统，用以实现水下机器人作业轨迹规划优化方法。
7.本发明通过以下技术方案实现：
8.一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述规划方法包括以下步骤：
9.步骤1：建立双臂水下机器人节能作业规划的数学模型，所述作业内容包括趋近目标和组合机械臂作业，分别设立适应的目标函数；
10.步骤2：基于步骤1的数学模型对目标函数的求解采用自适应动态改变权重的策略；
11.步骤3：基于步骤2权重的策略进行四种群的粒子位置更新；
12.步骤4：基于步骤3的位置更新进行基于四种群的粒子速度更新；
13.步骤5：基于步骤4的速度更新进行基于违反容忍系数的约束多目标算法改进；
14.步骤6：基于步骤5改进的约束多目标算法实现基于多目标多种群骨干粒子群优化
算法的水下机器人作业轨迹规划优化。
15.一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤1具体为：所述双臂水下机器人的双臂是一样且对称布置的，所以机械臂左臂的耦合惯性矩阵等于机械臂右臂的耦合惯性矩阵，并且机器人基座的惯性矩阵恒定，所以对上述的目标函数值进行简化，优化后如下(1)-(6)：
16.f1＝s1+s2+s3＝g(1)+g(2)-r+alti
ꢀꢀꢀ
(1)
[0017][0018][0019]
式中：s1，s2，s3分别代表艇体趋近过程中前进，侧移，下移的距离；g代表固定机械臂末端进行固定的坐标点；r和alti分别代表机械臂运动学的p
x
和py值；ii代表第i个关节的转动惯量；ωi代表第i个关节的角速度；mi代表第i个关节的质量；li代表第i个关节的机械臂连杆的长度；f代表第i个关节的连杆在水中的水的阻力；v代表第i个关节的连杆转动的平均转动速度；t代表设定的系统作业时间。
[0020][0021][0022][0023]
式中：i
ei
代表第i个虚拟组合机械臂的虚拟关节的转动惯量；ω
ei
代表第i个虚拟组合机械臂的虚拟关节的角速度；m
ei
代表第i个虚拟组合机械臂的虚拟关节的质量；l
ei
代表第i个虚拟组合机械臂的虚拟关节的连杆的长度；f代表第i个虚拟组合机械臂的虚拟关节的杆在水中收到的水的阻力；v代表第i个虚拟组合机械臂的虚拟关节的连杆转动的平均转动速度；t代表双臂水下机器人的作业时间；hb代表主体的惯量矩阵；代表左臂与相连基座的耦合惯性矩阵；代表右臂与相连基座的耦合惯性矩阵；分别代表机械臂左臂和右臂各关节的角速度；ja，jb分别代表机械臂左臂和右臂的雅克比矩阵。
[0024]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤2自适应动态改变权重的策略具体为：新的权重改变策略如(7)所示：
[0025][0026]
其中：f
avg
，f
min
，f为骨干粒子群的适应度值；采用上述的自适应权重系数的变更方
式，在骨干粒子群算法进化的过程中，适应度值能进行动态的变化，从而避免粒子群算法陷入局部最优值。
[0027]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤3的四种群的粒子位置更新具体为：采用四个骨干粒子群对整体的算法中的粒子群的位置进行更新；在进行搜索时，四个骨干粒子群在分别找出各自的局部最优解的同时找出一个全局最优解，保证种群的多样性；其中骨干粒子群的种群1和种群2的位置更新如下所示：
[0028][0029]
其中：z＝1，2，分别代表骨干粒子群1和骨干粒子群2的位置更新公式；
[0030]
同样的，骨干粒子群种群3的位置更新公式将受到基本骨干粒子群1和骨干粒子群2的影响，如下所示：
[0031][0032]
其中：γ1，γ2分别为种群1和种群2的相对适应度值，并且γ1+γ2＝1；该种位置更新方式，主要是通过比较骨干粒子群1和骨干粒子群2的适应度值，越好的适应度对当前粒子的影响越大，从而更好地引导骨干粒子群3的进化；
[0033]
相同的，骨干粒子群种群4的位置更新方式，如下所示：
[0034][0035]
其中：α1，α2，α3分别为影响因子且相加为1；
[0036]
骨干粒子群4中的粒子共享其它三个种群中的粒子信息，使得多种群骨干粒子群算法的进化具有多样性，还会使得骨干粒子群4中的粒子的搜索过程更加的精细；
[0037]
改进后的骨干粒子群算法中每个粒子都有变异的可能性，同时为了减少后期变异对算法的影响，变异概率计算公式可以设计如下：
[0038]
pm＝e
at
/t
ꢀꢀꢀ
(11)
[0039]
式中：a代表初始化变异概率；t代表进化的代数；t代表算法种进化的总代数。
[0040]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤4基于四种群的粒子速度更新具体为：基于所采用的四骨干粒子群作为种群的算法，前两个粒子群作为四个子群算法中的基本粒子群，并且粒子群1的速度对粒子群2的速度更新有影响，粒子群1和粒子群2都将采用传统的速度和位置更新公式，速度更新如下所示：
[0041][0042]
式中：z代表粒子群1和粒子群2，z＝1，2；
[0043]
粒子群3的速度更新将受到粒子群1和粒子群2适应度值的影响，粒子群3的速度更新方式如下所示：
[0044][0045]
式中：γ1和γ2分别代表粒子在基本粒子群1和基本粒子群2中的适应度值；γ代表粒子群1和粒子群2适合度值的总和，为γ＝γ1+γ2；
[0046]
对粒子群4的速度更新优化改进，粒子群4的速度更新公式由粒子群1，粒子群2，粒子群3的速度更新引导，如下所示：
[0047][0048]
同时为增强粒子群4的搜索能力，将粒子群4中的最佳引导者朝着全局最优解进化，所以对粒子群4中的位置更新公式改进，如下所示：
[0049][0050]
式中：α1，α2，α3代表相对应项的影响因子，并且α1+α2+α3＝1。
[0051]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤5基于违反容忍系数的约束多目标算法改进具体为：对所有的约束违反程度进行归一化处理，如下所示：
[0052][0053]cmax
＝max(ci)i＝1，2，...，n
ꢀꢀꢀ
(17)
[0054][0055]
在上述公式中，约束集合c是由不等式约束和等式约束共同组成了一个约束集合c，而c
max
就是整个集合c中数值最大数；通过计算，得出决策变量对于每个约束条件的违反程度，最后得到归一化的约束违反程度；
[0056]
约束的违反程度将采用如下计算方法：
[0057][0058]
式中：t代表目前约束进化的代数；t代表约束进化的总代数。
[0059]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化系统，所述优化系统包括双臂水下机器人节能作业规划模块和水下机器人的机械手节能运动优化模块，
[0060]
所述双臂水下机器人节能作业规划模块，用于建立双臂水下机器人节能作业规划的数学模型，所述节能作业包括趋近对虚拟基座进行固定和虚拟组合机械臂作业，分别设立适应的目标函数；
[0061]
所述水下机器人-机械手节能运动优化模块，用于进行自适应动态改变权重的策
略；进行四种群的粒子位置更新；进行基于四种群的粒子速度更新；进行基于违反容忍系数的约束多目标算法改进；实现基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化。
[0062]
一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
[0063]
存储器，用于存放计算机程序；
[0064]
处理器，用于执行存储器上所存放的程序时，实现上述的方法步骤。
[0065]
一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述的方法步骤。
[0066]
本发明的有益效果是：
[0067]
本发明采用了自适应的惯性权重改变策略，在骨干粒子群算法进化的过程中，适应度值可以进行动态的变化，从而避免粒子群算法陷入局部最优值。
[0068]
本发明采用了基于多种群粒子群算法的改进骨干粒子群算法，该算法由四个独立的骨干粒子群组成，每个粒子群都在一个特定的搜索空间中搜索最优解，不同的粒子群可以相互合作，并通过交换信息来提高全局搜索性能。在该算法中，每个粒子群都有自己的局部最优解和全局最优解，并且每个粒子群可以将其个体最优解和全局最优解共享给其他粒子群，从而更好地搜索最优解。
[0069]
本发明将四个独立粒子群中的前两个作为算法中的基本粒子群，采用传统的速度位置更新公式，但粒子群1的速度和位置对粒子群2有移动的影响。为了加强种群间的信息交流，加快算法收敛速度，粒子群3的速度位置更新受到粒子群1和粒子群2的适应度影响；为了进一步加快粒子群4的搜索性能和收敛性能，粒子群4的速度位置更新公式由粒子群1、2、3的速度位置更新公式引导，并向着全局最优解进化。
附图说明
[0070]
图1是本发明的双机械臂d-h坐标系示意图。
[0071]
图2是本发明的双臂水下机器人的单臂抓取示意图。
[0072]
图3是本发明的双臂水下机器人的双臂抓取示意图。
[0073]
图4是本发明的双臂水下机器人的双臂运动空间约束示意图。
[0074]
图5是本发明的多目标多种群骨干粒子群算法的流程图。
[0075]
图6是本发明的进行双臂抓取作业仿真的数据图，其中，(a)为改进骨干粒子群算法与遗传算法在计算耦合力矩时的对比数据；(b)为改进骨干粒子群算法与遗传算法在计算机械臂能耗时的对比数据；(c)为在规划过程中仿真机械臂的关节角度随时间变化关系；(d)为在规划过程中仿真机械臂的关节角速度随时间变化关系；(e)为在规划过程中仿真机械臂的关节角加速度随时间变化关系。
[0076]
图7是本发明的方法流程图。
具体实施方式
[0077]
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本
发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0078]
本发明具有良好多样性、收敛性且能够避免陷入局部最优。
[0079]
本发明能够改善双臂水下机器人在作业过程中能量消耗多的问题。
[0080]
建立双臂水下机器人的运动学、动力学数学模型；
[0081]
本发明的双臂水下机器人是带有两条水下机械臂的仿人型水下机器人，其中双臂水下机器人带有的两条机械臂的结构和尺寸均相同，都拥有四个自由度。
[0082]
对双臂水下机器人的运动学模型进行建立。
[0083]
水下机器人本体在水下航行时具有六个自由度。水下机器人的运动基于世界坐标系和艇体坐标系两个坐标系，所以采用d-h参数法，对双机械臂的运动学模型建立，可以得到末端执行器相对于基座标系之间的位置关系。
[0084]
根据水下机器人-机械手的运动要求，水下机器人的运动学分为正运动学与逆运动学。其中正运动学为由艇体出发，以目标物体为终点进行规划；逆运动学为由目标物体出发，向艇体进行规划。
[0085]
为了方便对水下机器人-机械手结构进行运动学建模，需要对水下机器人-机械手结构列d-h表来表示机械手的模型运动关系。
[0086][0087]
在对机械手结构列d-h表之后，机械臂两个相邻连杆之间的坐标变换矩阵如下：
[0088][0089]
结合d-h表建立机械臂机构的相邻坐标系之间的变换矩阵，将基坐标开始到末端执行器的四个相邻机构的坐标通过对相邻坐标系变换矩阵的连续数乘可以得到末端执行器相对于基座标系的变换矩阵。基于上述原理，末端执行器相对于基座标系之间的位置关系如下：
[0090]
[0091]
其中：为机械臂末端执行器的位置向量；为机械臂末端执行器的姿态向量，且结果分别为：
[0092][0093][0094]
对双臂水下机器人的动力学模型进行数学建模；
[0095]
由于本发明采用的双臂水下机器人的双机械臂简化后有三个自由度，且整体具有高自由度，非线性，耦合性强的特点，所以为了更好的处理机械臂作业时的能耗以及保持艇体的稳定性，可以使用拉格朗日法分析机械臂的动力学。
[0096]
针对双臂水下机器人的单个机械臂动力学方程可以使用拉格朗日函数进行表示：
[0097][0098]
水下机械臂可以看作是一个刚体，因此假设连杆i上的一点相对坐标系{i}的齐次坐标系表示为ir，对该点求导可以得到该点的速度。
[0099][0100][0101]
假设上述该点在连杆上的质量为dm，通过对齐次坐标系的点导数进行平方可以求得伪惯性矩阵如下所示：
[0102][0103]
因此，对整条连杆积分可以求得连杆i的动能为：
[0104][0105]
最后，由连杆组成的机械臂的总动能可以计算为：
[0106][0107]
式中：k
ai
代表各个连杆之间的传动装置的总动能。
[0108]
然后求解机械臂的总势能，机械臂连杆上的i的势能如下所示：
[0109]
ui＝∫dui＝∫-dmigt
iiri
＝-migt
iirꢀꢀꢀ
(30)
[0110]
式中：mi代表连杆i的质量；g代表重力矢量[g
x g
y g
z 0]。
[0111]
因此，由连杆组成的机械臂的总势能进行计算为：
[0112][0113]
综上得到机械臂的拉格朗日函数为：
[0114][0115]
将上文中求解得到的机械臂动能和势能的表达式带回拉格朗日函数表达式，即可得到化简的机械臂动力学方程如下：
[0116][0117]
其中：
[0118][0119][0120][0121]
至此，双臂水下机器人的运动学、动力学数学建模结束。
[0122]
建立双臂水下机器人的单臂抓取作业数学模型；
[0123]
在进行机械臂的作业规划时首先以单机械臂的节能轨迹规划进行研究。由于机械臂工作过程中单臂的四个作业关节的最后一个关节只对机械臂末端的姿态进行调节，不影响最终的规划，所以可以进一步简化为三关节的作业过程。
[0124]
单臂固定单臂抓取的多约束多目标数学模型可以表达为：
[0125]
min y，y＝f(θ)＝[f1(θ)，f2(θ)，f3(θ)
…
，fn(θ)]
ꢀꢀꢀ
(37)
[0126]
s.t gi(θ)≤0，i＝1，2
…ki
ꢀꢀꢀ
(38)
[0127]hi
(θ)＝0，i＝k
i+1
，k
i+2
，
…ki+n
ꢀꢀꢀ
(39)
[0128]
式中：θ＝[θ1，θ2…
，θn]∈dn代表决策向量；d代表决策空间；y＝[f1(θ)，f2(θ)
…
，fn(θ)]代表决策向量在目标函数上的映射；f1(θ)，f2(θ)...fn(θ)代表水下机器人-机械手工作时间的决策目标函数；gi(θ)，hi(θ)分别代表机械手在工作时间的不等式约束和等式约束。
[0129]
设定约束条件具体为，
[0130]
(1)决策变量的确立
[0131]
通过对相关资料的查询，可以得知采用低纬度的决策变量会对算法求得最优解具有很大的帮助。本发明研究的双臂水下机器人系统在进行单臂抓取的时间，主要可以考虑两个方面：艇体趋近作业目标点后进行艇体固定以及固定后的单臂作业轨迹规划问题。机械臂的作业轨迹规划可以简化对一个三关节水下机械臂的轨迹规划。
[0132]
根据上述分析，趋近和抓取的两个过程都可以利用机械臂的关节角的数据进行处理解决，此外，也需要对机械臂的作业轨迹进行规划，所以本发明将使用作业单机械臂的关节角作为决策变量。
[0133]
(2)目标函数的确立
[0134]
本发明基于双臂水下机器人的节能作业对单臂固定单臂作业模式下的目标函数设立主要以系统的能耗作为主体的计算方式。本发明的作业主要由双臂水下机器人本体趋近和单臂作业两个部分组成，所以可以针对这两个方面设立相应的目标函数。
[0135]
针对趋近作业目标的问题，由于双臂水下机器人的双目摄像头视野范围受限，因此在趋近目标物的过程中，机器人主要通过保持艏向角不变的方式进行侧移和前后运动的方式来趋近目标。由于双臂水下机器人的本体在趋近的过程中需要不断地进行位置调整，为了减少能耗，需要艇体趋近过程耗时更短；在假定速度恒定的前提下，可以将目标函数转换为艇体趋近运动的路径最短。
[0136]
针对单臂作业问题，水下机械臂在进行作业的时间主要由三方面的影响：机械臂的转矩；机械臂的重浮力；机械臂的水阻力等，因此机械臂的作业能耗可以由上述三方面的作业能耗叠加进行计算求解。
[0137]
根据上述分析，对于单臂固定单臂作业模式，选定两个性能目标函数设定如下式所示：
[0138]
f1＝s1+s2+s3＝g(1)+g(2)-r+alti
ꢀꢀꢀ
(40)
[0139][0140]
式中：s1，s2，s3分别代表auv趋近过程中前进，侧移，下移的距离；g代表固定机械臂末端进行固定的坐标点；r和alti分别代表第二章机械臂运动学的p
x
和py值；ii代表第i个关节的转动惯量；ωi代表第i个关节的角速度；mi代表第i个关节的质量；li代表第i个关节的机械臂连杆的长度；f代表第i个关节的连杆在水中的水的阻力；v代表第i个关节的连杆转动的平均转动速度；t代表时间。
[0141]
(3)约束项的确立
[0142]
约束项的确立需要根据系统的具体作业情况进行确立。
[0143]
首先，机械臂作业的每个关节都要满足运动学条件的约束，即每个关节的角位移，角速度，角加速度不能超过每个关节的运动极限。每个关节的角位移g0(θ)，角速度g1(θ)，角加速度g2(θ)限制可以表示为：
[0144][0145]
式中：qi(δt)，分别代表每个机械臂关节在δt时刻的角度，速度，加速度；q
imax
，分别代表每个关节的角度，角速度，角加速度极值。
[0146]
其次，需要考虑系统在整个作业中的安全性问题，即系统在整个过程中实际作业高度要大于需要保持的最小作业高度，即h
real
≥h
min
。系统的实际作业高度可以用机械臂的关节角的参数表示为：
[0147]hreal
＝l1*cos(θ1)+l2*cos(θ1)*cos(θ2)+l3*cos(θ1)*cos(θ2+θ3)
ꢀꢀꢀ
(43)
[0148]
最后，由于本发明采用视觉系统进行检测作业，所以在进行单臂作业的时间需要作业物体始终处于摄像机的检测范围之内。假设本发明所用的摄像机具有双目，并且开角为90
°
，根据相机的检测原理，结合上述趋近策略，该条件可以表示为：
[0149][0150]
式中：t
n2b
代表从艇体移动坐标系到像机检测视觉坐标系的转换矩阵；代表目标物体在像机检测视觉坐标系中的位置；代表目标物体在艇体移动坐标系上的表达；r
bino
代表相机的视野检测范围。
[0151]
同时为了实现对多种群骨干粒子群算法的动态权重系数进行更新，针对本发明的多目标优化问题，可以对两个计算结果进行归一化处理，从而得到一个归一化的函数目标值f对权重系数更新，如下所示：
[0152][0153]
式中：α1，α2代表各项影响指标；n1，n2代表目标函数的取值范围。
[0154]
在双臂水下机器人的单臂作业的基础上，基于以上对于双臂水下机器人的运动学、动力学的数学建模可以得到等效的组合水下双臂机器人的微分运动学方程表示如下：
[0155][0156]
其中：jb虚拟基座的雅可比矩阵，jm为虚拟组合机械臂的雅可比矩阵。
[0157]
本发明所要求的水下双机械臂都具有四个关节，所以可以根据以下公式求取jb和jm：
[0158]
[0159][0160]
其中：p
0end
＝p
end-r0，是p
0end
的斜对称矩阵。e3是三阶对角单位矩阵，e3＝diag{1，1，1}，θ＝[θ1ꢀ…ꢀ
θ
end
]
t
，θ为虚拟组合机械臂的各个关节角的角度信息。
[0161]
由此推导虚拟机械臂的动能和势能：
[0162][0163][0164]
综上：虚拟组合机械臂的统一动力学模型：
[0165][0166]
运用虚拟基座建模方法对本发明研究的双臂水下机器人系统进行等效组合后，可以看作是对一个带有基座的一个六关节水下机械臂的抓取作业轨迹规划。
[0167]
根据上述的研究，采用双臂进行作业的多约束多目标数学模型可以表达为：
[0168]
min y，y＝f(θ)＝[f1(θ)，f2(θ)，f3(θ)
…
，fn(θ)]
ꢀꢀꢀ
(52)
[0169]
s.t gi(θ)≤0，i＝1，2
…ki
ꢀꢀꢀ
(53)
[0170]hi
(θ)＝0，i＝k
i+1
，k
i+2
，
…ki+n
ꢀꢀꢀ
(54)
[0171]
双臂水下机器人的作业主要由趋近对虚拟基座进行固定和虚拟组合机械臂作业两个部分组成，所以可以针对这两个部分分别设立适应的目标函数。
[0172]
针对趋近对虚拟基座进行固定的第一部分，可以将艇体在趋近的过程中经过的路径最短作为目标函数。在对虚拟基座进行抓取固定的时间，可以考虑机械臂左臂的能耗最优，关节角度变化最小作为目标函数。
[0173]
针对虚拟组合机械臂作业轨迹规划的第二部分，主要考虑虚拟组合机械臂作业过程的能耗最优为主要目标。考虑实际的运动过程对双臂水下机器人的本体造成的影响，保持艇体运动处于最小化，需要考虑双臂对本体的耦合作用的影响。同时，在两条机械臂分别抓取了一个共同的物体之后，为了将物体运送到目标位置，整个系统必须适当的协调，以遵守物体本身施加的运动约束。
[0174]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述规划方法包括以下步骤：
[0175]
步骤1：建立双臂水下机器人节能作业规划的数学模型，所述节能作业包括趋近对虚拟基座进行固定和虚拟组合机械臂作业，分别设立适应的目标函数；
[0176]
步骤2：基于步骤1的数学模型进行自适应动态改变权重的策略；
[0177]
步骤3：基于步骤2权重的策略进行四种群的粒子位置更新；
[0178]
步骤4：基于步骤3的位置更新进行基于四种群的粒子速度更新；
[0179]
步骤5：基于步骤4的速度更新进行基于违反容忍系数的约束多目标算法改进；
[0180]
步骤6：基于步骤5改进的约束多目标算法实现基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化。
[0181]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方
法，所述步骤1具体为，所述双臂水下机器人的双臂是一样且对称布置的，所以机械臂左臂的耦合惯性矩阵等于机械臂右臂的耦合惯性矩阵，并且机器人基座的惯性矩阵恒定，所以可以对上述的目标函数值进行简化，优化后如下(1)-(6)：
[0182]
f1＝s1+s2+s3＝g(1)+g(2)-r+alti
ꢀꢀꢀ
(1)
[0183][0184][0185]
式中：s1，s2，s3分别代表艇体趋近过程中前进，侧移，下移的距离；g代表固定机械臂末端进行固定的坐标点；r和alti分别代表机械臂运动学的p
x
和py值；ii代表第i个关节的转动惯量；ωi代表第i个关节的角速度；mi代表第i个关节的质量；li代表第i个关节的机械臂连杆的长度；f代表第i个关节的连杆在水中的水的阻力；v代表第i个关节的连杆转动的平均转动速度；t代表设定的系统作业时间。
[0186]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，
[0187][0188][0189][0190]
式中：i
ei
代表第i个虚拟组合机械臂的虚拟关节的转动惯量；ω
ei
代表第i个虚拟组合机械臂的虚拟关节的角速度；m
ei
代表第i个虚拟组合机械臂的虚拟关节的质量；l
ei
代表第i个虚拟组合机械臂的虚拟关节的连杆的长度；f代表第i个虚拟组合机械臂的虚拟关节的杆在水中收到的水的阻力；v代表第i个虚拟组合机械臂的虚拟关节的连杆转动的平均转动速度；t代表双臂水下机器人的作业时间；hb代表主体的惯量矩阵；代表左臂与相连基座的耦合惯性矩阵；代表右臂与相连基座的耦合惯性矩阵；分别代表机械臂左臂和右臂各关节的角速度；ja，jb分别代表机械臂左臂和右臂的雅克比矩阵。
[0191]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤2自适应动态改变权重的策略具体为，针对传统的骨干粒子群算法容易过早陷入局部收敛的缺点，本发明设计了一种改进的粒子群算法，将多种群粒子群算法的思想与传统骨干粒子群算法相结合，形成一种针对双臂机器人节能作业轨迹规划的多种群骨干粒子群算法，实现对双臂水下机器人节能作业的优化。
[0192]
传统的骨干粒子群算法的惯性权重是采用线性递减的方式进行变更的，在算法中具有较大的缺点。新的权重改变策略如下所示：
[0193][0194]
其中：f
avg
，f
min
，f为骨干粒子群的适应度值；采用上述的自适应权重系数的变更方式，在骨干粒子群算法进化的过程中，适应度值可以进行动态的变化，从而避免粒子群算法陷入局部最优值。
[0195]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤3四种群的粒子位置更新具体为，本发明采用四个骨干粒子群对整体的算法中的粒子群的位置进行更新。在进行搜索时，四个骨干粒子群在分别找出各自的局部最优解的同时找出一个全局最优解，保证种群的多样性。其中骨干粒子群的种群1和种群2的位置更新如下所示：
[0196][0197]
其中：z＝1，2，分别代表骨干粒子群1和骨干粒子群2的位置更新公式。
[0198]
同样的，骨干粒子群种群3的位置更新公式将受到基本骨干粒子群1和骨干粒子群2的影响，如下所示：
[0199][0200]
其中：γ1，γ2分别为种群1和种群2的相对适应度值，并且γ1+γ2＝1。该种位置更新方式，主要是通过比较骨干粒子群1和骨干粒子群2的适应度值，越好的适应度对当前粒子的影响越大，从而更好地引导骨干粒子群3的进化。
[0201]
相同的，骨干粒子群种群4的位置更新方式，如下所示：
[0202][0203]
其中：α1，α2，α3分别为影响因子且相加为1。
[0204]
骨干粒子群4中的粒子共享其它三个种群中的粒子信息，可以使得多种群骨干粒子群算法的进化具有多样性，还会使得骨干粒子群4中的粒子的搜索过程更加的精细。
[0205]
为了应对每个子骨干粒子群中的粒子陷入局部最优，本发明在每一代种群中采取变异操作，变异操作在增加算法前期搜索的搜索空间，并在算法在后期的时间，变异概率减小以此增加算法的收敛可能性。改进后的骨干粒子群算法中每个粒子都有变异的可能性，
同时为了减少后期变异对算法的影响，变异概率计算公式可以设计如下：
[0206]
pm＝e
at
/t
ꢀꢀꢀ
(11)
[0207]
式中：a代表初始化变异概率；t代表进化的代数；t代表算法种进化的总代数。
[0208]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤4基于四种群的粒子速度更新具体为，基于本发明所采用的四骨干粒子群作为种群的算法，前两个粒子群作为四个子群算法中的基本粒子群，并且粒子群1的速度对粒子群2的速度更新有影响，粒子群1和粒子群2都将采用传统的速度和位置更新公式，速度更新如下所示：
[0209][0210]
式中：z代表粒子群1和粒子群2，z＝1，2。
[0211]
为了加强种群之间信息的交流，加快算法的收敛速度，粒子群3的速度更新将受到粒子群1和粒子群2适应度值的影响，粒子群3的速度更新方式如下所示：
[0212][0213]
式中：γ1和γ2分别代表粒子在基本粒子群1和基本粒子群2中的适应度值；γ代表粒子群1和粒子群2适合度值的总和，为γ＝γ1+γ2。
[0214]
为了进一步加快第四个粒子群搜索性能和收敛性能，对粒子群4的速度更新优化改进，粒子群4的速度更新公式由粒子群1，粒子群2，粒子群3的速度更新引导，如下所示：
[0215][0216]
同时为增强粒子群4的搜索能力，将粒子群4中的最佳引导者朝着全局最优解进化，所以对粒子群4中的位置更新公式改进，如下所示：
[0217][0218]
式中：α1，α2，α3代表相对应项的影响因子，并且α1+α2+α3＝1。
[0219]
一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，所述步骤5基于违反容忍系数的约束多目标算法改进具体为，针对约束多目标进化算法，不仅需要对可行解进行搜索，还需要对非可行解集的范围的离散可行解进行搜索，为了处理这种问题可以通过构建非可行解储备集的方法进行解决。同时为了使得改进算法在非可行区域的工作效率和性能提升，本发明引入了约束违反容忍系数w的概念，具体来说，当一个解的目标函数值大于约束违反容忍系数w时，这个解将被视为非可行解，否则这个解将被视为可行解。
[0220]
由于多目标优化问题的约束违反程度的取值范围是需要根据不同问题进行计算的，所以取值一般不相同。为了改变这种现象，本发明将对所有的约束违反程度进行归一化处理，如下所示：
[0221][0222]cmax
＝max(ci)i＝1，2，...，n
ꢀꢀꢀ
(17)
[0223][0224]
在上述公式中，约束集合c是由不等式约束和等式约束共同组成了一个约束集合c，而c
max
就是整个集合c中数值最大数。通过计算，可以得出决策变量对于每个约束条件的违反程度，最后得到归一化的约束违反程度。
[0225]
为了获得约束违反程度的高低，本发明将采用约束违反阈值对约束的违反程度进行判定，本发明设定的约束的违反程度取值在0到1之间，并且会随着进化的代数不断地进行调整。在算法的前期，使约束违法程度较大使得更多的非可行解参与进化，随着迭代次数后期的增加，约束违反程度的值将会接近于0，以此来更加集中算法寻找最优解，使得算法的收敛性增强。本发明中约束的违反程度将采用如下计算方法：
[0226][0227]
式中：t代表目前约束进化的代数；t代表约束进化的总代数。
[0228]
实施例二
[0229]
本发明实施例提供了一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化系统，所述优化系统包括双臂水下机器人节能作业规划模块和水下机器人的机械手节能运动优化模块，
[0230]
所述双臂水下机器人节能作业规划模块，用于建立双臂水下机器人节能作业规划的数学模型，所述节能作业包括趋近对虚拟基座进行固定和虚拟组合机械臂作业，分别设立适应的目标函数；
[0231]
所述水下机器人-机械手节能运动优化模块，用于进行自适应动态改变权重的策略；进行四种群的粒子位置更新；进行基于四种群的粒子速度更新；进行基于违反容忍系数的约束多目标算法改进；实现基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化。
[0232]
由上可见，本发明实施例通过建立双臂水下机器人节能作业规划的数学模型及水下机器人的机械手节能运动优化，能够改善双臂水下机器人在作业过程中能量消耗多的问题，以实现具有良好多样性、收敛性且能够避免陷入局部最优的多种群骨干粒子群算法与多目标优化相结合的方法对双臂水下机器人的局部作业轨迹进行规划。
[0233]
实施例三
[0234]
本发明实施例提供了一种电子设备，该电子设备包括存储器、处理器以及存储在上述存储器中并可在上述处理器上运行的计算机程序，其中，存储器用于存储软件程序以及模块，处理器通过运行存储在存储器的软件程序以及模块，从而执行各种功能应用以及数据处理。存储器和处理器通过总线连接。具体地，处理器通过运行存储在存储器的上述计算机程序时实现上述实施例一中的任一步骤。
[0235]
应当理解，在本发明实施例中，所称处理器可以是中央处理单元(central processing unit，cpu)，该处理器还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现成可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑
器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
[0236]
存储器可以包括只读存储器、快闪存储器和随机存储器，并向处理器提供指令和数据。存储器的一部分或全部还可以包括非易失性随机存取存储器。
[0237]
由上可见，本发明实施例提供的电子设备，通过建立双臂水下机器人节能作业规划的数学模型及水下机器人的机械手节能运动优化，能够改善双臂水下机器人在作业过程中能量消耗多的问题，以实现具有良好多样性、收敛性且能够避免陷入局部最优的多种群骨干粒子群算法与多目标优化相结合的方法对双臂水下机器人的局部作业轨迹进行规划。
[0238]
应当理解，上述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，上述计算机程序可存储于以计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，上述计算机程序包括计算机程序代码，上述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。上述计算机可读介质可以包括：能够携带上述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(read-only memory，rom)、随机存取存储器(random access memory，ram)、电载波信号、电信信号以及软件分发介质等。需要说明的是，上述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减。
[0239]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
[0240]
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将上述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
[0241]
需要说明的是，上述实施例所提供的方法及其细节举例可结合至实施例提供的装置和设备中，相互参照，不再赘述。
[0242]
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各实例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟是以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同的方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
[0243]
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其他的方式实现。例如，以上所描述的装置/设备实施例仅仅是示意性的，例如，上述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以由另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
[0244]
上述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于，所述规划方法包括以下步骤：步骤1：建立双臂水下机器人作业规划的数学模型，所述作业包括趋近和组合机械臂作业，并分别设立适应的目标函数；步骤2：基于步骤1的数学模型对目标函数的求解采用自适应动态改变权重的策略；步骤3：基于步骤2权重的策略进行四种群的粒子位置更新；步骤4：基于步骤3的位置更新进行基于四种群的粒子速度更新；步骤5：基于步骤4的速度更新进行基于违反容忍系数的约束多目标算法改进；步骤6：基于步骤5改进的约束多目标算法实现基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化。2.根据权利要求1所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于，所述步骤1具体为：所述双臂水下机器人的双臂是一样且对称布置的，所以机械臂左臂的耦合惯性矩阵等于机械臂右臂的耦合惯性矩阵，并且机器人基座的惯性矩阵恒定，所以可以得到如下的双臂水下机器人作业的目标函数如下(1)-(6)：f1＝s1+s2+s3＝g(1)+g(2)-r+alti
ꢀꢀꢀꢀ
(1)(1)式中：s1，s2，s3分别代表艇体趋近过程中前进，侧移，下移的距离；g代表固定机械臂末端进行固定的坐标点；r和alti分别代表机械臂运动学的p
x
和p
y
值；i
i
代表第i个关节的转动惯量；ω
i
代表第i个关节的角速度；m
i
代表第i个关节的质量；l
i
代表第i个关节的机械臂连杆的长度；f代表第i个关节的连杆在水中的水的阻力；v代表第i个关节的连杆转动的平均转动速度；t代表设定的系统作业时间。3.根据权利要求2所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于：轨迹规划优化方法，其特征在于：轨迹规划优化方法，其特征在于：式中：i
ei
代表第i个虚拟组合机械臂的虚拟关节的转动惯量；ω
ei
代表第i个虚拟组合机械臂的虚拟关节的角速度；m
ei
代表第i个虚拟组合机械臂的虚拟关节的质量；l
ei
代表第i个虚拟组合机械臂的虚拟关节的连杆的长度；f代表第i个虚拟组合机械臂的虚拟关节的杆在水中收到的水的阻力；v代表第i个虚拟组合机械臂的虚拟关节的连杆转动的平均转动速度；t代表双臂水下机器人的作业时间；h
b
代表主体的惯量矩阵；代表左臂与相连基座的耦合惯性矩阵；代表右臂与相连基座的耦合惯性矩阵；分别代表机械臂左臂和右
臂各关节的角速度；j
a
，j
b
分别代表机械臂左臂和右臂的雅克比矩阵。4.根据权利要求1所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于，所述步骤2自适应动态改变权重的策略具体为：新的权重改变策略如(7)所示：其中：f
avg
，f
min
，f为骨干粒子群的适应度值；采用上述的自适应权重系数的变更方式，在骨干粒子群算法进化的过程中，适应度值能进行动态的变化，从而避免粒子群算法陷入局部最优值。5.根据权利要求2所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于，所述步骤3的四种群的粒子位置更新具体为：采用四个骨干粒子群对整体的算法中的粒子群的位置进行更新；在进行搜索时，四个骨干粒子群在分别找出各自的局部最优解的同时找出一个全局最优解，保证种群的多样性；其中骨干粒子群的种群1和种群2的位置更新如下所示：其中：z＝1,2,分别代表骨干粒子群1和骨干粒子群2的位置更新公式；同样的，骨干粒子群种群3的位置更新公式将受到基本骨干粒子群1和骨干粒子群2的影响，如下所示：其中：γ1，γ2分别为种群1和种群2的相对适应度值，并且γ1+γ2＝1；该种位置更新方式，主要是通过比较骨干粒子群1和骨干粒子群2的适应度值，越好的适应度对当前粒子的影响越大，从而更好地引导骨干粒子群3的进化；相同的，骨干粒子群种群4的位置更新方式，如下所示：其中：α1，α2，α3分别为影响因子且相加为1；骨干粒子群4中的粒子共享其它三个种群中的粒子信息，使得多种群骨干粒子群算法
的进化具有多样性，还会使得骨干粒子群4中的粒子的搜索过程更加的精细；改进后的骨干粒子群算法中每个粒子都有变异的可能性，同时为了减少后期变异对算法的影响，变异概率计算公式可以设计如下：atpm＝e
t
(11)式中：a代表初始化变异概率；t代表进化的代数；t代表算法种进化的总代数。6.根据权利要求5所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于，所述步骤4基于四种群的粒子速度更新具体为：基于所采用的四骨干粒子群作为种群的算法，前两个粒子群作为四个子群算法中的基本粒子群，并且粒子群1的速度对粒子群2的速度更新有影响，粒子群1和粒子群2都将采用传统的速度和位置更新公式，速度更新如下所示：式中：z代表粒子群1和粒子群2，z＝1,2；粒子群3的速度更新将受到粒子群1和粒子群2适应度值的影响，粒子群3的速度更新方式如下所示：式中：γ1和γ2分别代表粒子在基本粒子群1和基本粒子群2中的适应度值；γ代表粒子群1和粒子群2适合度值的总和，为γ＝γ1+γ2；对粒子群4的速度更新优化改进，粒子群4的速度更新公式由粒子群1，粒子群2，粒子群3的速度更新引导，如下所示：同时为增强粒子群4的搜索能力，将粒子群4中的最佳引导者朝着全局最优解进化，所以对粒子群4中的位置更新公式改进，如下所示：式中：α1，α2，α3代表相对应项的影响因子，并且α1+α2+α3＝1。7.根据权利要求2所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法，其特征在于，所述步骤5基于违反容忍系数的约束多目标算法改进具体为：对所有的约束违反程度进行归一化处理，如下所示：c
max
＝max(c
i
)i＝1,2,...,n
ꢀꢀꢀꢀ
(17)
在上述公式中，约束集合c是由不等式约束和等式约束共同组成了一个约束集合c，而c
max
就是整个集合c中数值最大数；通过计算，得出决策变量对于每个约束条件的违反程度，最后得到归一化的约束违反程度；约束的违反程度将采用如下计算方法：式中：t代表目前约束进化的代数；t代表约束进化的总代数。8.根据权利要求1-7任一所述一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化系统，其特征在于，所述优化系统包括双臂水下机器人节能作业规划模块和水下机器人的机械手节能运动优化模块；所述双臂水下机器人节能作业规划模块，用于建立双臂水下机器人节能作业规划的数学模型，所述节能作业包括趋近对虚拟基座进行固定和虚拟组合机械臂作业，分别设立适应的目标函数；所述水下机器人-机械手节能运动优化模块，用于进行自适应动态改变权重的策略；进行四种群的粒子位置更新；进行基于四种群的粒子速度更新；进行基于违反容忍系数的约束多目标算法改进；实现基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化。9.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现权利要求1-7任一所述的方法步骤。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-7任一所述的方法步骤。

技术总结
本发明提供一种基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化方法。步骤1：建立双臂水下机器人作业规划的数学模型；步骤2：基于步骤1的数学模型对运动规划目标函数的计算过程采用自适应动态改变权重的策略；步骤3：基于步骤2权重的策略在进行目标函数求解时进行四种群的粒子位置更新；步骤4：基于步骤3的位置更新进行基于四种群的粒子速度更新；步骤5：基于步骤4的速度更新进行基于违反容忍系数的约束多目标算法改进；步骤6：基于步骤5改进的约束多目标算法实现基于多目标多种群骨干粒子群优化算法的水下机器人作业轨迹规划优化。本发明主要改善双臂水下机器人在作业过程中能量消耗多的问题。器人在作业过程中能量消耗多的问题。器人在作业过程中能量消耗多的问题。


技术研发人员：黄海 石健 蔡峰春 姜涛 孙溢泽
受保护的技术使用者：哈尔滨工程大学
技术研发日：2023.07.21
技术公布日：2023/10/15