CN109709970B - 一种水下机器人六自由度推力分配优化方法 - Google Patents

Abstract

本发明提供一种水下机器人六自由度推力分配优化方法，属于水下机器人技术领域，本发明建立水下机器人推力分配的数学模型，对等式约束求解，使用随机黑洞算法求解k的最优值，利用k解算各推进器推力u及转角α。本发明对姿态角动力定位更加灵活，降低了优化算法的复杂度，加快了计算速度，避免了传统优化方法迭代计算复杂的弊端，加快了寻优速度，避免了陷入局部最优。

Description

一种水下机器人六自由度推力分配优化方法

技术领域

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

背景技术

随着智能控制技术的不断发展，水下机器人在海洋探测和海洋作业应用愈加广泛，为了实现水下机器人作业所需要的六自由度姿态精确控制，需要提出一种基于冗余推进器布置的六自由度推力分配方法。

冗余推进器推力分配采用伪逆法、序列二次规划(SQP)方法和智能算法，在推力分配优化算法中应用最多的是SQP，但它存在着鲁棒性差、局部收敛等缺点。伪逆法和智能算法常出现不能计算出满足约束条件的结果,只能求得相似解。

本水下机器人配置有四个矢量推进器，以十字型分配在前后左右四个方位。相比于六推进器的水下机器人，不仅减小了整体的体积，在成本和能耗方面也具有经济性优势。这四个推进器不仅能输出满足高层控制器给出的预期推力，也可以在满足预计推力的基础上降低机器人整体的能耗。

结合水下机器人推进器布置的特点，在满足六自由度推力分配的基础上，建立了以能耗最小为寻优目标的优化方法，提出了采用随机黑洞智能算法的推力分配方法。

发明内容

本发明的目的在于提供了一种水下机器人六自由度推力分配优化方法，并在冗余推进器布置的基础上采用随机黑洞算法以能耗最低为目的进行优化。

本发明的目的是这样实现的：

一种水下机器人六自由度推力分配优化方法，具体的实现步骤为：

步骤1.建立水下机器人推力分配的数学模型；

步骤2.对等式约束求解；

步骤3.使用随机黑洞算法求解k的最优值；

步骤4.利用k解算各推进器推力u及转角α。

步骤1所述的水下机器人推力分配的数学模型为：

τ＝B(α)u

其中B表示推力分配的空间位置约束，τ＝{τ_x,τ_y,τ_z,τ_p,τ_q,τ_r}为控制器输出的六个自由度上的力和力矩，u＝{u₁ u₂ u₃ u₄}为推进器的推力，α＝{α₁ α₂ α₃ α₄}为推进器转角，

其中l_xi为第i个推进器在x方向上距离机器人重心的距离，l_yi为第i个推进器在y方向上距离机器人重心的距离。

步骤2所述的对等式约束求解的过程中为：

步骤2.1.将推进器推力依照水平方向和垂直方向分解u_hi＝u_icosα_i，u_zi＝u_isinα_i，i＝1，2，3，4，水下机器人推力分配的数学模型为

即-2Ru_z1+2Ru_z3＝τ_r-Rτ_x+Rτ_y，

其中R为推进器与重心的距离；

步骤2.2.简化等式约束，等式约束为

解集为

其中

步骤3所述的求解k的最优值过程为：

步骤3.1.确定优化问题的目标函数

其中P表示推进器功率，n为推进器转速，Q为推进器的转矩，K_Q和K_T为转矩系数和推力系数视为定值，W为比例，将推进器转角的变化量作为惩罚项加入目标函数

Δα^TQΔα

其中Q为权值矩阵，Δα＝α-α₀，α₀为上一时刻各推进器的转角，Δα为转角变化量；优化问题的目标函数为

步骤3.2.确定星体范围，推进器1、3的推力为

u_z1＝k₁

u_z3＝-k₁+E

u_h1＝k₂+F

u_h3＝-k₂

k的取值范围为max{E-u_max,u_min}＜k₁＜min{E-u_min,u_max}

max{-u_max,u_min-F}＜k₂＜min{-u_min,u_max-F}

其中

u_max表示推进器所能取得的最大，u_min表示推进器所能取得的最小值；

步骤3.3.输入预期合力τ、推进器推力界限u_max与u_min、当前各推进器转角α₀，设置星体为k＝{k₁,k₂}，设置最大迭代次数M、星体个数N、空间维度D、设置寻优范围

步骤3.4.初始化星体，

其中k_i,t表示第t次计算的第i个星体，t＝0,…,M，i＝1,…,N，k_(max)表示二维星体k＝{k₁,k₂}取值的最大值，k_(min)表示二维星体k＝{k₁,k₂}取值的最小值。

步骤3.5.计算星体的目标函数值

并计算目标函数值的适应度

若计算星体为初始星体，选择适应度最大的星体为初始黑洞k_BH；

步骤3.6.更新星体，k_i,t+1＝k_i,t+rand×(k_BH-k_i,t)，rand为0～1随机值，计算更新后星体的适应度，若其中适应度的最大值超过黑洞，则将该星体和黑洞交换身份；

设置黑洞的归一化原始边界

设置引力范围权重因子

ε为引力范围常数，黑洞边界为R(t)＝λR₀(t)，星体与黑洞距离为

步骤3.7.当计算到第N代或满足最优寻优精度时，结束计算，获得最优的k取值，否则转到步骤3.5。

步骤4所述的解算各推进器推力u及转角α过程为：

由k＝{k₁,k₂}计算U＝{u_z1 u_z3 u_h1 u_h3}，由配置矩阵得

u_h2＝τ_x-u_h1

u_h4＝τ_y-u_h3

通过各推进器在水平垂直面分解的分力，求得合力和转角

本发明的有益效果在于：本发明提供一种六自由度推力分配方案，相较传统水下机器人多出横滚和纵倾的控制，对姿态角动力定位更加灵活；本发明在进行数学建模时针对水下机器人推进器布置特点，对推进器空间布置的约束矩阵进行线性化并求解，将8个有约束的未知量简化为2个无约束未知量，降低了优化算法的复杂度，加快了计算速度；本发明采用基于随机黑洞算法的智能算法，可以针对能耗最小进行优化，相对于传统的伪逆法和序列二次规划方法，本发明采用的方法避免了传统优化方法迭代计算复杂的弊端；设置了归一化的黑洞引力范围和距离计算公式，使其吸收能力和适应度成正比，加快了寻优速度；同时设置权重因子使算法在计算初期加大了在全局范围搜索，在后期加快局部计算速度，避免了陷入局部最优。

附图说明

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

图2为本发明的流程图。

具体实施方式

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

实施例1

机器人的四个推进器采用十字对称布置在整体的四周，每个推进器都能绕其对称轴旋转，左右推进器可以产生前后方向推力，前后推进器可以产生左右方向推力，当推进器旋转到垂直面时，便可以在垂直方向产生推力。分别将机器人左、右、前、后的推进器编号为1、2、3、4。通过四个推进器器推力和转角的组合，便能在产生六自由度上的合力及力矩。

定义机器人的前进方向为x，右移方向为y，下降方向为z，绕x、y、z轴转动分别为横滚p、纵倾q和转艏r。由于机器人能输出四个推进器推力及其转角，有8个输出量，相对于6自由度的预期推力是冗余的，因此有无穷多满足预期推力的分配方式。通过求解推进器空间配置方程，使计算模型由有约束的8个未知量简化为无约束的2个未知量，并建立能耗模型，以能耗最小为目的，采用随机黑洞智能算法对推力分配问题进行优化，其步骤如下：

步骤1.建立水下机器人推力分配的数学模型：

依据水下机器人推进器的安装位置，建立控制器指令和推进器推力间的映射关系：

τ＝B(α)u

上式为推力分配问题的等式约束，B表示推力分配的空间位置约束，式中τ为控制器输出的六个自由度上的力和力矩，即推力分配模型的输入量τ＝{τ_x,τ_y,τ_z,τ_p,τ_q,τ_r}，x、y、z、p、q、r分别为纵向、横向、垂向、横滚、纵倾、转艏六个自由度方向。u为推进器的推力，由于目标机器人配置有4个推进器，其表达如下：u＝{u₁ u₂ u₃ u₄}，α为推进器转角，表达如下：α＝{α₁ α₂ α₃ α₄}，针对目标机器人建立推进器的配置矩阵B(α)。

其中l_xi、l_yi分别为第i个推进器在x、y方向上距离机器人重心的距离。

步骤2.对等式约束求解：

为了线性化计算，将推进器推力依照水平方向和垂直方向分解：

u_hi＝u_icosα_i，u_zi＝u_isinα_i，于是τ＝B(α)u可改写为：

由于机器人的目标力和力矩在水平和垂直方向互不影响，并且在纵向方向和横滚推力仅由推进器1、2产生，在横向方向和纵倾方向仅由推进器3、4产生，而且各推进器关于机器人重心对称表示，记其与重心的距离为R，因此可将上述方程简化为：

-2Ru_z1+2Ru_z3＝τ_r-Rτ_x+Rτ_y，

记

则等式约束简化为：

式中有四个未知量，约束方程为两个，因此有无数解，求其解集U＝{u_z1 u_z3 u_h1 u_h3}^T：

通过以上方法得出u、α与未知系数k＝{k₁,k₂}的关系，通过等式方程任意k可计算出相应的推力。通过求解空间配置约束方程的通解，对于任意k，均有推力u满足期望推力，使优化问题不再出现等式约束条件，将8个受约束的未知量降低为2个无约束的未知量，极大的简化了计算，加快了收敛速度。

步骤3.使用随机黑洞算法求解k的最优值。

步骤4.利用k解算各推进器推力u及转角α；通过k1和k2，可以计算出

u_h2＝τ_x-u_h1

u_h4＝τ_y-u_h3

U＝{u_z1 u_z3 u_h1 u_h3}，由配置矩阵得：

通过各推进器在水平垂直面分解以

后的分力，就可求出合力和转角：

所述步骤3的具体步骤为

步骤3.1.确定优化问题的目标函数，以各推进器的功耗最小为优化目标，计算推进器推力与功耗的关系，已知

P表示推进器功率，n为推进器转速，Q为推进器的转矩，K_Q和K_T为转矩系数和推力系数，可视为定值。于是各推进器的能耗和推进器的推力的3/2次方绝对值成正比，比例记为W。

为了减少推进器在转角发生改变时的能耗，将推进器转角的变化量作为惩罚项加入目标函数：Δα^TQΔα，Q为权值矩阵，权值越大转角变化对能耗量化的影响越大，Δα＝α-α₀，α₀为上一时刻各推进器的转角，Δα为转角变化量。

推力分配问题的优化目标函数由推进器能耗和推进器转角组成，将其作为优化问题的目标函数，其表达式为：

步骤3.2.确定未知量范围：

推进器2、4的可由等式约束解出，所以只考虑推进器1、3的推力：

假设

u_max、u_min分别表示推进器所能取得的最大和最小值，因此可以计算出k的取值范围：

max{E-u_max,u_min}＜k₁＜min{E-u_min,u_max}

max{-u_max,u_min-F}＜k₂＜min{-u_min,u_max-F}

步骤3.3.确定推力分配算法的输入量和随机黑洞算法的初始化参数：输入推力分配的预期合力τ、推进器能产生的最大最小推力u_max、u_min和当前各推进器转角α₀。设置求解范围，星体为k＝{k₁,k₂}，寻优范围

设置最大迭代次数M＝100，星体个数N＝50，本算例中空间维度为2，引力范围常数ε为2。

步骤3.4.初始化星体，将星体均匀布置在空间内：

其中k_i,t表示第t次计算的第i个星体，t＝0,…,M，i＝1,…,N。k_(max)、k_(min)分别表示二维星体k＝{k₁,k₂}取值的最大最小值。通过这种方法在二维空间中均匀布置了N个初始星体。

步骤3.5.计算星体的目标函数值

并计算其适应度，

具有更小目标函数值的星体将有更大的适应度，若计算星体为初始星体，选择适应度最大的星体为初始黑洞k_BH。

步骤3.6.将非黑洞星体向黑洞移动，更新星体：k_i,t+1＝k_i,t+rand×(k_BH-k_i,t)，rand为0～1随机值，计算更新后星体的适应度，若其中适应度的最大值超过黑洞，则将该星体和黑洞交换身份。

设置黑洞的归一化原始边界

设置比例因子

ε为引力范围常数，黑洞边界为R(t)＝λR₀(t)，星体与黑洞距离由归一化的欧氏距离

计算得出，在星体在黑洞边界范围内的星体将被黑洞吸收，该星体消失，为了保证星体数目不变，相应的会有新的星体生成；当星体i被黑洞吞噬时，随机产生一个与黑洞距离大于R的新星体；比例因子使星体在生成时易远离黑洞，使其在更大范围中生成，随着迭代进行，距离变小，比例因子使迭代初期更容易在全局范围内寻优，后期加大开发，减少了陷入局部最优的缺陷。

步骤3.7.当计算到第N代或满足最优寻优精度时，结束计算，输出黑洞的k取值，否则回到步骤3.5进行新一轮寻优。

Claims (1)

1.一种水下机器人六自由度推力分配优化方法，其特征在于，具体的实现步骤为：

步骤1.建立水下机器人推力分配的数学模型；

依据水下机器人推进器的安装位置，建立控制器指令和推进器推力间的映射关系：

τ＝B(α)u

上式为推力分配问题的等式约束，B表示推力分配的空间位置约束；τ为控制器输出的六个自由度上的力和力矩，即推力分配模型的输入量τ＝{τ_x,τ_y,τ_z,τ_p,τ_q,τ_r}；x、y、z、p、q、r分别为纵向、横向、垂向、横滚、纵倾、转艏六个自由度方向；u为推进器的推力，由于目标机器人配置有4个推进器，其表达为u＝{u₁ u₂ u₃ u₄}；α为推进器转角，α＝{α₁ α₂ α₃ α₄}；l_xj、l_yj分别为第j个推进器在x、y方向上距离机器人重心的距离；j＝1,2,3,4；

步骤2.对等式约束求解；

为了线性化计算，将推进器推力依照水平方向和垂直方向分解：

u_hj＝u_jcosα_j，u_zj＝u_jsinα_j，于是τ＝B(α)u改写为：

由于机器人的目标力和力矩在水平和垂直方向互不影响，并且在纵向方向和横滚推力仅由第一推进器、第二推进器产生，在横向方向和纵倾方向仅由第三推进器、第四推进器产生，而且各推进器关于机器人重心对称表示，记其与重心的距离为R，因此将上述方程简化为：

-2Ru_z1+2Ru_z3＝τ_r-Rτ_x+Rτ_y

记

则等式约束简化为：

式中有四个未知量，约束方程为两个，因此有无数解，求其解集U＝{u_z1 u_z3 u_h1 u_h3}^T：

通过以上方法得出u、α与未知系数k＝{k₁,k₂}的关系，通过等式方程任意k可计算出相应的推力；通过求解空间配置约束方程的通解，对于任意k，均有推力u满足期望推力，使优化问题不再出现等式约束条件；

步骤3.使用随机黑洞算法求解k的最优值；

步骤3.1.确定优化问题的目标函数；

其中，P_i表示推进器功率，n为推进器转速，Q_i为推进器的转矩；K_Q和K_T为转矩系数和推力系数视为定值；W为比例；将推进器转角的变化量作为惩罚项加入目标函数：

Δα^TQΔα；

其中，Q为权值矩阵；Δα＝α-α₀，α₀为上一时刻各推进器的转角，Δα为转角变化量；优化问题的目标函数为：

步骤3.2.确定星体范围，第一推进器1、第三推进器的推力为：

k的取值范围为：

其中，

u_max表示推进器所能取得的最大，u_min表示推进器所能取得的最小值；

步骤3.3.输入预期合力τ、推进器推力界限u_max与u_min、当前各推进器转角α₀，设置星体为k＝{k₁,k₂}，设置最大迭代次数M、星体个数N、空间维度D、设置寻优范围：

步骤3.4.初始化星体：

其中，k_i,t表示第t次计算的第i个星体，t＝0,…M；i＝1,2,…,N；k_(max)表示二维星体k＝{k₁,k₂}取值的最大值，k_(min)表示二维星体k＝{k₁,k₂}取值的最小值；

步骤3.5.计算星体的目标函数值：

并计算目标函数值的适应度：

若计算星体为初始星体，选择适应度最大的星体为初始黑洞k_BH；

步骤3.6.更新星体，k_i,t+1＝k_i,t+rand×(k_BH-k_i,t)，rand为0～1随机值，计算更新后星体的适应度，若其中适应度的最大值超过黑洞，则将该星体和黑洞交换身份；

设置黑洞的归一化原始边界

设置引力范围权重因子

ε为引力范围常数，黑洞边界为R(t)＝λR₀(t)，星体与黑洞距离为

步骤3.7.当计算到第N代或满足最优寻优精度时，结束计算，获得最优的k取值，否则转到步骤3.5；

步骤4.利用k解算各推进器推力u及转角α；

通过k₁和k₂，可以计算出U＝{u_z1 u_z3 u_h1 u_h3}，由配置矩阵得：

通过各推进器在水平垂直面分解以后的分力，就可求出合力和转角：

Images