[发明专利]一种水下机器人深度区间控制方法和系统

说明书

本发明公开了一种舵角舵速约束下的水下机器人深度区间控制方法和系统，属于机器人控制领域。本发明的控制方法包括：计算多步预测状态矩阵；计算多步输入矩阵；更新反馈校正矩阵并计算多步预测状态；根据当前艏舵和艉舵角度计算舵角舵速约束下的舵角增量输出向量；取舵角增量输出向量的第一行作为当前时刻舵角增量输出。本发明提出的舵角舵速约束下的深度区间控制算法和系统有利于减少舵机机械结构的磨损，减少航行过程中的功耗和噪音，适用于舵角舵速约束下的水下机器人深度区间控制。

技术领域

本发明属于机器人控制领域，更具体地，涉及一种水下机器人深度区间控制方法和系统。

背景技术

水下机器人可以执行各种复杂的水下任务，目前水下机器人主要应用于海洋环境检测、地球化学数据采集、海底资源探测、未知区域侦察等方面。受工作环境和自身结构所限，水下机器人在水下工作的时候很难补充能量，因此在进行水下机器人姿态控制的时候，能量消耗也是设计控制器需要考虑的部分。水下机器人在水下航行的过程中容易受到洋流等外界干扰，海水的密度会受到温度、深度、区域的影响，因此需要深度控制器来保证水下机器人在海洋的正常航行，现有技术多是针对深度定值控制这一问题，实际在航行过程中针对洋流、海水密度变化等外界干扰频繁打舵保持深度，不仅会加速舵机的磨损，还会增加打舵带来的功耗，不利于延长水下工作时间和保持隐蔽性。伴随着打舵和变深机动的流体分离，将在舵面的主体和附体表面产生压力脉动，诱发更为显著的水动力噪声。

区间控制不同于定值控制之处在于，区间控制的目的是将被控对象控制在一个给定的区间内而不是控制在一个给定的值。对于航行在海洋中的水下机器人来说，当有扰动存在的时候，若通过将水下机器人的深度控制在一个给定的区间内，就能一定程度上降低航行时的功耗和噪声。因此，亟需一种水下机器人深度区间控制方法和系统以减少打舵的幅度和频率。

发明内容

针对现有技术的缺陷，本发明的目的在于提供一种水下机器人深度区间控制方法和系统，旨在减少水下机器人航行过程舵机机械结构的磨损以及水下机器人航行过程中的功耗和噪音。

为实现上述目的，本发明一方面提供了一种水下机器人深度区间控制方法，包括以下步骤：

根据深度控制线性化状态空间方程中的时变状态矩阵A_c和时变输入矩阵B_c，获取多步预测状态矩阵P和多步预测输入矩阵H；

根据当前k时刻的深度ζ_k、纵倾角θ_k、艏舵舵角和艉舵舵角通过观测器获取当前k时刻状态变量估计值z_k，并更新反馈校正矩阵D；

由当前k时刻状态变量估计值z_k，通过所述多步预测状态矩阵P、多步预测输入矩阵H和更新后的反馈校正矩阵D获取多步预测状态变量

构建包含优化参考轨迹α、舵角增量输出向量Δδ和所述多步预测状态变量的代价函数，结合舵角约束条件、舵速约束条件和区间约束条件，求解约束条件下的二次规划，从而获得舵角舵速约束下的舵角增量输出向量Δδ。

进一步地，根据深度控制线性化状态空间方程中的时变状态矩阵A_c和时变输入矩阵B_c，获取多步预测状态矩阵P和多步预测输入矩阵H具体包括：

a)在当前时刻航速U₀处对深度状态空间方程进行线性化，得到深度控制线性化状态空间方程

其中

X＝[w q θ ζ]^T，