CN108803634A - 一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法 - Google Patents

Abstract

本发明属于水下机器人技术领域，具体涉及一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法；包括如下步骤：依据输入量确定水下机器人工作模式，建立推力分配模型，依照空间布置特点对复杂约束条件简化，实现遗传算法降维；设定推进器的大小和转角，设定优化目标；采用遗传算法解决推力分配优化问题，反解降维矩阵求出各推进器推力及转角。本发明将推力分配的约束从四自由度提高为六自由度；采用遗传算法避免了传统方法面对非线性联合约束求解困难问题；在推力分配时提出了降维方法，使求解精度提高，计算速度加快，从而实现了六自由度约束条件下推力分配快速求解及能耗降低的优化。

Description

一种基于遗传算法的水下机器人六自由度动力定位推力分配 优化方法

技术领域

本发明属于水下机器人技术领域，具体涉及一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法。

技术背景

水下机器人在水下探测和水下作业任务中的应用越来越广泛。对于一般的水下机器人而言，通常只需要对前进方向、转首和深度进行控制。但出于执行更为复杂的任务的需要，要求水下机器人能够实现六自由度的精确控制。通常具备六自由度动力定位的水下机器人至少需配备六个推进器以产生三个线性方向的平移和旋转。为了实现精确控制和获得更好的定位效果，多数机器人配置更多的推进器，在冗余推进器布置的基础上进行推力分配，其约束函数是线性的，实现起来较为容易。受到体积的限制，本发明所针对的水下机器人只配备了四个矢量推进器，采用十字型方式布置，每个推进器都能在水平和竖直方向之间旋转。这种布置在推力分配时有推力和推进器转角两种输出量，其约束条件是非线性的，实现起来相对复杂。

发明内容

本发明目的在于提供一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，其能够合理分配推力任务，达到能效最优的目的。

一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，具体包括如下步骤：

步骤1：确定推力分配方法，依照输入量所控制的自由度不同分为直接法和智能算法两种求解方法；其中，输入量为输入的纵向、横向、垂向的力以及横滚、纵倾和艏摇力矩，以下称其为控制力矩：τ＝[τ_x τ_y τ_z τ_p τ_q τ_r]；

步骤2：判断其工作模式，如果τ_y＝τ_q＝0，则此时水下机器人处于定身定向工作状态，采用直接法进行动力分配，转步骤3，否则为任意角度控制，采用智能算法即遗传算法进行分配，转步骤4；

步骤3：建立推力分配数学模型τ＝Bu；u＝[u₁ u₂ u₃u₄]，式中u为四个推进器输出的推力，l_xi，l_yi为第i个推进器距水下机器人质心的纵向、横向距离，此时，①②推进器水平放置，③④推进器竖直放置，计算上述矩阵能解出各推进器推力，完成输出；

步骤4：建立机器人推力分配模型，推力系数矩阵B与推进器转角α关系如下，τ＝B(α)u；

对约束方程降维；

步骤5：建立遗传算法的优化目标方程：

其中，P为推进器功率，N为推进器角度改变产生的能耗，α₀为上一步结果中推进器转角，初始为0，Q为权值矩阵；

步骤6：建立初始种群，进行遗传算法的参数包括种群规模，繁殖代数，个人中变量个数，变量的上下限、交叉概率、变异概率及容忍度，初始种群的个体随机产生，随机范围为变量上下限；

步骤7：适度函数计算：

上式中Au-T表示当前个体关于等式约束的偏差值，W为惩罚矩阵；

步骤8：对当前种群中的个体依照其适应度进行选择，记录最佳个体，并且采用轮盘赌选择法计算种群中个体的选择概率：

步骤9：对遗传算法的终止条件进行判断；

步骤10：对步骤8选择后的个体进行交叉变异以产生新的种群，产生新种群后，将新种群带回步骤7开始新一轮的适应度选择；

步骤11：对其他两个推进器进行求解，对求解可得推进器②的推力和转角；对求解可得推进器④的推力和转角，输出各推进器的推力及转角，完成分配。

所述一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，步骤1中坐标系定义如下：固定坐标系E-ξηζ固定在地面，指向北、东和地面方向，为固定坐标系；随体坐标系O-xyz的原点与水下机器人重心一致，随机器人移动，为随体坐标系；以机器人的前进方向为x轴，平移方向为y轴，下潜方向为z轴；纵倾方向q绕y轴，横滚方向p绕x轴，首摇方向r绕z轴。

所述一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，步骤4中所述的遗传算法约束方程降维为Au＝T即：

-u₁sinα₁(l_y1+l_y2)+u₃sinα₃(l_x3+l_x4)＝τ_r-l_y2τ_x+l_x4τ_y

u_min≤u_i≤u_max

其中变量为u₁、u₃、α₁、α₃，u_min为推进器所能提供的最小推力，u_max为推进器所能提供的最大推力。

本发明的有益效果在于：

本发明提供一种六自由度的推力分配方法，能对机器人任意姿态进行调节。本发明在推力分配策略上依据控制指令对工作模式进行判断，将其划分为定深定向控制和任意姿态控制，定深定向控制为保持航行效率采取传统的伪逆法进行求解。对于任意姿态控制采用遗传算法求解，传统的伪逆法具有求解联合约束问题困难的局限性。采用遗传算法进行推力分配求解，这种方法避免了伪逆法和二次规划方法面对联合约束的求解困难，能在联合约束条件下寻找最优解。

在遗传算法计算时，针对水下机器人推进器布置的空间结构，依据水平面控制和垂直面控制的耦合较弱的特点，对复杂约束条件进行降维，使多变量复杂约束问题简化为少量变量简单约束问题。极大的降低了约束条件的耦合性，加快计算的同时使求解更加准确。

附图说明

图1为本发明的推进器布置示意图；

图2为本发明的流程图；

图3为本发明坐标系定义图；

图4为本发明的整体结构示意图；

图5为本发明定深定向推力分配示意图；

图6为任意推进器角度示意图。

具体实施方式

下面结合附图对本发明做进一步描述：

如附图1所示，为本发明的推进器布置示意图；在机器人的左侧装有推进器①，其水平放置时可产生向前或向后的水平推力，当其旋转为垂直状态时，能够产生向上或向下的推力，当推进器旋转至二者之间位置时便可在纵向和垂直位置均产生推力。右侧的推进器②也是同样，当左右两个推进器在垂直方向提供的推力不相等时，就可以实现机器人的横滚运动。同样对于前后的推进器③、④，其能够提供水平和垂直两个方向上的推力。当推进器③、④在垂直方向提供的推力不同时，便能够提供纵倾方向上的力矩。水下机器人的升沉和转首依靠四个推进器同时控制，前进和后退借助推进器①、②，左右横移借助推进器③、④。这样对四个推进器的转向和产生推力的大小进行合理布置就能完成六个自由度同时调节。

该水下机器人的坐标系定义如下：

如附图3所示，固定坐标系E-ξηζ固定在地面，指向北、东和地面方向，为固定坐标系。随体坐标系O-xyz的原点与水下机器人重心一致，随机器人移动，为随体坐标系。以机器人的前进方向为x轴，平移方向为y轴，下潜方向为z轴。纵倾方向q绕y轴，横滚方向p绕x轴，首摇方向r绕z轴。

图2为基于遗传算法的水下机器人六自由度动力定位推力分配优化方法的流程图，包括以下几个步骤：

步骤1为确定推力分配方法，依照输入量所控制的自由度不同分为直接法和智能算法两种求解方法。其中，输入量为输入的纵向、横向、垂向的力以及横滚、纵倾和艏摇力矩，以下称其为控制力：τ＝[τ_x τ_y τ_z τ_p τ_q τ_r]。

步骤2，对其工作模式进行判断，如果τ_y＝τ_q＝0，则此时水下机器人处于定身定向工作状态，和大多数水下机器人一样只需要控制4个自由度的输入，鉴于水下机器人的x方向推力仅由推进器①②产生，为了保证运动的快速性，提高工作效率，因此可以采用多数机器人一样的直接分配方法进行推力分配，跳转步骤3。如果τ_y,τ_q不全为0，则该水下机器人需要产生六个自由度的推力，此时推进器转角改变，为了达到推进器能耗减小的优化目标，要采用智能算法进行分配，跳转步骤4。

步骤3：建立推力分配数学模型τ＝Bu；

其中u＝[u₁ u₂ u₃ u₄]。式中u为四个推进器输出的推力，l_xi，l_yi为第i个推进器距水下机器人质心的纵向、横向距离，在计算时，由于本机器人全对称设计，均为0.23m，B为推力系数矩阵。此时，①②推进器水平放置，③④推进器竖直放置，即：

α＝[α₁ α₂ α₃ α₄]＝[90° 90° 0° 0°]，推进器各推进器转角为α_i。通过计算上述矩阵，求解就能得到各推进器推力，输出所求结果。

步骤4：首先建立机器人推力分配模型，τ＝B(α)u，其中推力系数矩阵B与推进器转角α有关，其值如下：

约束方程降维，一般的对于多维等式约束有：

其中m<n，不失一般性，秩为m的矩阵，线性无关的变量为n-m个，上式可写为：

由于本水下机器人推进器空间布置的特殊性，x方向上的力和力矩仅与推进器①②相关，y方向上的力和力矩仅与推进器③④有关，变换后变量可减少，降维后，在水平面和垂直面的约束方程变为：

-u₁sinα₁(l_y1+l_y2)+u₃sinα₃(l_x3+l_x4)＝τ_r-l_y2τ_x+l_x4τ_y

上式简写为：Au＝T其中未知量数目由8个降为4个，为u₁、u₃、α₁、α₃，约束数量由6个降为2个，达到了简化计算的目的。

步骤5：为了达到节约能源的目的，应该建立遗传算法的优化目标方程：

目标函数中，第一项表示推进器能耗，P为推进器的功率，在计算时取为其估计值，N为推进器旋转产生的能耗，计算时取C为磨损能耗。第二项中α₀为上一步结果中推进器转角，初始为0。Q为权值矩阵，其值>0，此项保证了推进器旋转角度不会过大。

步骤6：开始遗传算法计算，确定遗传算法中优化变量为四项，即推进器①③的推力大小及其旋转角度。生成初始种群，设置种群规模，繁殖代数，个人中变量个数，变量的上下限、交叉概率、变异概率及容忍度。在本发明中设计种群规模为50，繁殖500代，个体变量为4，交叉概率0.8，变异概率0.15，容忍度为1e-8。对于个体的变量，u₁和u₃上下限为[-150,150]kN,α₁和α₃的上下限为[0,90]度。初始种群的个体随机产生，随机范围即为变量上下限。

步骤7：在步骤4的基础上设计遗传算法的适度函数并计算：

上式中Au-T表示当前个体关于等式约束的偏差值，W为惩罚矩阵，当个体越接近等式约束的解时，其适应度也越大，同样当个体越满足优化条件时，它的适应度越大，这样便可以求出种群中更加满足约束条件且适应优化方程的个体。

步骤8：对当前种群中的个体依照其适应度进行选择，依照其适应度大小由小至大排序，记录最佳个体，并轮盘赌选择法计算种群中个体的选择概率：

式中P_i为第i个个体的选择概率，f_i为其适应度，使用度越高的个体，其被选择的概率越大，被选择的个体将参与染色体交叉以产生子代。

步骤9：对遗传算法的终止条件进行判断，如果当前最佳个体的适应度与上代最佳个体适应度之差小于容忍度，则认为达到最优条件，遗传算法结束，跳至步骤11。若不满足，判断是否达到繁殖代数，若达到则停止计算，跳转步骤11.

步骤10：对有交叉权的个体进行染色体交叉，本发明中采用浮点数交叉法，即交换父母中某一段浮点位，对有交叉权但不满足交叉概率的保留父代。对于产生的子代，依照编译概率对其某浮点位进行随机数变异。在产生的子代中，如果它的变量值超过了变量上下限，则将其值修改为上下限的值。同时为了保证种群的优选性，将上代最优个体进行保留。将交叉、变异后的产生的新种群带回步骤7.

步骤11：经过遗传算法计算出了u₁，α₁，u₃，α₃。为了完成推力分配，对其他两个推进器进行求解：

对求解可得推进器②的推力和转角，同样对：对求解可得推进器④的推力和转角。输出各推进器的推力及转角完成分配。

Claims (3)

1.一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，其特征在于，具体包括如下步骤：

步骤1：确定推力分配方法，依照输入量所控制的自由度不同分为直接法和智能算法两种求解方法；其中，输入量为输入的纵向、横向、垂向的力以及横滚、纵倾和艏摇力矩，以下称其为控制力矩：τ＝[τ_x τ_y τ_z τ_p τ_q τ_r]；

步骤2：判断其工作模式，如果τ_y＝τ_q＝0，则此时水下机器人处于定身定向工作状态，采用直接法进行动力分配，转步骤3，否则为任意角度控制，采用智能算法即遗传算法进行分配，转步骤4；

步骤3：建立推力分配数学模型τ＝Bu；u＝[u₁ u₂ u₃ u₄]，式中u为四个推进器输出的推力，l_xi，l_yi为第i个推进器距水下机器人质心的纵向、横向距离，此时，①②推进器水平放置，③④推进器竖直放置，计算上述矩阵能解出各推进器推力，完成输出；

步骤4：建立机器人推力分配模型，推力系数矩阵B与推进器转角α关系如下，τ＝B(α)u；

对约束方程降维；

步骤5：建立遗传算法的优化目标方程：

其中，P为推进器功率，N为推进器角度改变产生的能耗，α₀为上一步结果中推进器转角，初始为0，Q为权值矩阵；

步骤6：建立初始种群，进行遗传算法的参数包括种群规模，繁殖代数，个人中变量个数，变量的上下限、交叉概率、变异概率及容忍度，初始种群的个体随机产生，随机范围为变量上下限；

步骤7：适度函数计算：

上式中Au-T表示当前个体关于等式约束的偏差值，W为惩罚矩阵；

步骤8：对当前种群中的个体依照其适应度进行选择，记录最佳个体，并且采用轮盘赌选择法计算种群中个体的选择概率：

步骤9：对遗传算法的终止条件进行判断；

步骤10：对步骤8选择后的个体进行交叉变异以产生新的种群，产生新种群后，将新种群带回步骤7开始新一轮的适应度选择；

步骤11：对其他两个推进器进行求解，对求解可得推进器②的推力和转角；对求解可得推进器④的推力和转角，输出各推进器的推力及转角，完成分配。

2.根据权利要求1所述一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，其特征在于，所述步骤1中坐标系定义如下：固定坐标系E-ξηζ固定在地面，指向北、东和地面方向，为固定坐标系；随体坐标系O-xyz的原点与水下机器人重心一致，随机器人移动，为随体坐标系；以机器人的前进方向为x轴，平移方向为y轴，下潜方向为z轴；纵倾方向q绕y轴，横滚方向p绕x轴，首摇方向r绕z轴。

3.根据权利要求1所述一种基于遗传算法的水下机器人六自由度动力定位推力分配优化方法，其特征在于，步骤4中所述的遗传算法约束方程降维为Au＝T即：

-u₁sinα₁(l_y1+l_y2)+u₃sinα₃(l_x3+l_x4)＝τ_r-l_y2τ_x+l_x4τ_y

u_min≤u_i≤u_max

其中变量为u₁、u₃、α₁、α₃，u_min为推进器所能提供的最小推力，u_max为推进器所能提供的最大推力。