[发明专利]水下游动机械臂旋转壳体的能量优化控制方法及装置

权利要求书

1.一种水下游动机械臂旋转壳体的能量优化控制方法，其特征在于，所述方法包括：

1)设计基于广义超螺旋算法和变结构扩张状态观测器的动力学控制器；

2)将动力学控制器在每个控制周期输出的期望广义力和力矩按照顺序存储在数据库中，使用核递归最小二乘进行基于数据库数据的预测；

3)以数据库数据的预测结果为输入，使用SQP对旋转壳体角度进行优化；

4)使用五次多项式插值法，由SQP优化得到的旋转壳体角度计算得到旋转壳体角速度和角加速度，重新执行步骤1)，直至USM完成预设任务为止；

其中，设计基于广义超螺旋算法和变结构扩张状态观测器的动力学控制器：

将动力学公式改写为：

ζ为描述USM在载体坐标系下位置、姿态和关节角的向量，为描述USM在载体坐标系下速度、角速度和关节角速度的向量；控制输入由τ＝Bu给出，B∈R^17×16表示推力配置矩阵，u∈R¹⁶表示USM的实际输入；d代表环境对USM的作用力；C(q,φ,ζ)是科里奥利力-向心力项，D(q,φ,ζ)是水阻力项，是重力和浮力项；

令则USM动力学公式改写为：

其中，z₁表示对的估计，即对USM速度向量的估计，z₂表示对的估计，表示扩展状态观测器对总扰动的估计结果，e_o1、e_o2表示估计误差，β₁、β₂表示误差系数，为扩展状态观测器估计值z₁的导数，为扩展状态观测器对USM扰动变化速度的估值，表示USM速度；表示使用变结构扩张状态观测器进行估计的状态的真实值；ω₀表示变结构扩张状态观测器带宽；表示估计误差e_o1的导数；M表示USM惯性矩阵；τ表示作用于USM的广义力；

所述以数据库数据的预测结果为输入，使用SQP对旋转壳体角度进行优化具体为：

目标函数：

约束条件：

s＝τ-Bu

f_min≤f≤f_max

δ_φ,min≤(φ-φ₀)≤δ_φ,max

其中，f(τ^*,φ)的计算过程如下：

f₁＝u_v(1)

其中，B为虚拟推力分配矩阵，s为推力分配后期望广义力与实际广义力误差；f_min为推进器的最小推力；f_max为推进器的最大推力；δ_φ,min为旋转壳体角度变化量的最小值；δ_φ,max为旋转壳体角度变化量的最大值；u_v为推力分配中间结果；B_v为虚拟推力分配矩阵；λ为阻尼系数；u_v(1)为推力分配中间结果的第一个分量；f_i为第i个推进器的推力。

2.根据权利要求1所述的一种水下游动机械臂旋转壳体的能量优化控制方法，其特征在于，所述方法根据历史广义力数据库中的数据预测未来一个控制周期的广义力。

3.一种水下游动机械臂旋转壳体的能量优化控制装置，其特征在于，所述装置包括：处理器和存储器，所述存储器中存储有程序指令，所述处理器调用存储器中存储的程序指令以使装置执行权利要求1所述的方法步骤。

4.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令被处理器执行时使所述处理器执行权利要求1所述的方法步骤。