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

权利要求书

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个推进器距水下机器人质心的纵向、横向距离，此时，①②推进器水平放置，③④推进器竖直放置，通过矩阵τ、B、u解出各推进器推力，转步骤12；

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

其中α＝[α₁ α₂ α₃ α₄]；α₁ α₂ α₃ α₄分别为①②③④推进器的转角；

对约束方程降维；x方向上的力和力矩仅与推进器①②相关，y方向上的力和力矩仅与推进器③④相关，变换后变量减少，降维后，在水平面和垂直面的约束方程为：

-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

上述约束方程简化为Au＝T，其中，A相当于推力系数矩阵B，T为所求的力和力矩，变量由8个降为4个，为u₁、u₃、α₁、α₃，约束数量由6个降为2个；u_min为推进器所能提供的最小推力，u_max为推进器所能提供的最大推力；

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

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

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

步骤7：适应度函数计算：

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

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

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

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

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

步骤12：完成输出。

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