CN108089589A - 一种水下机器人姿态控制方法 - Google Patents

Abstract

本发明涉及一种水下机器人姿态控制方法，从控板一将电源的电压电流、电源温度、电子舱内温湿度以及电子舱是否发生漏水的信息采集，经过各模块数据处理，通过RS232通信模块传递到主控板；从控板二将接收到的深度计以及电子罗盘的数据进行数据处理与解析后，将处理结果通过RS232通信模块传递到主控板，主控板接收导航和深度信息后，根据控制台的姿态控制指令调用新型快速非奇异终端滑模控制方法，计算出各个推进器的转速大小，经过DA转换模块后传递到各个推进器，从而实现机器人的姿态控制。本发明的优点在于：本发明能够有效解决传统非奇异终端滑模控制方法局部收敛慢、易波动问题。

Description

一种水下机器人姿态控制方法

技术领域

本发明属于水下机器人控制技术，特别涉及一种水下机器人姿态控制方法，可用于水下机器人航行过程中的姿态及深度调节。

背景技术

近年来，无论是海洋资源开发，还是陆地水资源的利用，水下机器人的研究逐渐成为机器人研究应用中的热点问题。水下机器人在水下搜救打捞、考古科研、海洋生物观测、调查取证、海洋油气管道调查、能源勘探等方面发挥着重要的作用，因此，水下机器人的研究和广泛应用对于充分利用自然资源，发展国民经济具有十分重要的现实意义。

灵活、稳定、可靠的水下机器人姿态控制方法是水下机器人进行多种水下作业的重要保证。目前，水下机器人的姿态控制多采用PID控制方法，PID控制器简单易操作，鲁棒性强，对非线性系统有良好的控制效果，但水下机器人属于MIMO二阶非线性不确定系统，传统方法无法取得良好的控制效果。

中国专利（申请号为201410422945.2）公开了一种水下机器人位姿控制方法，通过PID算法对误差进行调节，对电机进行控制，使姿态保持平衡，该方法采用传统PID算法，未进行任何改进，对于非线性、多耦合的水下机器人运动系统，无法取得理想姿态控制效果。

中国专利（申请号为201610249564.8）公开了一种ROV水下机器人悬浮姿态稳定控制方法，采用对水下机器人模型的机体姿态与机体深度及机体平面移动与机体姿态进行分离控制的方法，该方法依旧采用PID算法进行姿态控制，虽然进行了优化处理，采用双闭环PID算法，但依旧无法适用于水下机器人非线性系统，且控制模型参考四旋翼飞行器而建立，二者不具备通用性，无法取得理想控制效果。

中国专利（申请号为20150750233.8）公开了一种可旋转舵推进器的水下机器人控制方法，实现对AUV的五自由度运动控制，该方法虽然采用了弱机动控制和强机动控制来分别控制姿态调节速度，但采用的是传统滑模控制方法，在误差到达滑模面时，容易产生震荡，收敛速度慢，且未给出具体推导运算过程，不利于工程应用。

发明内容

本发明要解决的技术问题是提供一种能够有效解决传统非奇异终端滑模控制方法局部收敛慢、易波动问题的水下机器人姿态控制方法。

为解决上述技术问题，本发明的技术方案为：一种水下机器人姿态控制方法，其创新点在于：采用主从控制架构，包括控制台、主控板、从孔板一和从孔板二，从控板一将电源的电压电流、电源温度、电子舱内温湿度以及电子舱是否发生漏水的信息采集过来后，经过各模块数据处理之后，通过RS232通信模块传递到主控板；从控板二将接收到的深度计以及电子罗盘的数据进行数据处理与解析后，将处理后得到的深度以及导航数据通过RS232通信模块传递到主控板，主控板接收导航和深度信息后，根据控制台的姿态控制指令调用新型快速非奇异终端滑模控制方法，计算出各个推进器的转速大小，经过DA 转换模块后传递到各个推进器，从而实现机器人的姿态控制；主控板将深度、航向、电源电压电流的信息通过指令解析与数据汇总模块及以太网数据收发模块实时传递到控制台。

进一步地，所述新型快速非奇异终端滑模控制方法包括如下步骤：

(1)建立水下机器人的运动模型，在静水中，水下机器人动力学方程为：

式中：v＝[u v w p q r]；为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R^6×6；C(v)为水下机器人科氏及向心力矩阵，C(v)∈R^{6 ×6}；D(v)是水下机器人流体阻力矩阵， D(v)∈R^6×6；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R^6×1；τ是水下机器人推进器提供的推力，τ∈R^6×1；

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明水下机器人姿态控制方法中的数据流向图。

图2为图1中新型非奇异终端滑模控制算法的流程图。

图3为无干扰工况下两种算法坐标对比图。

图4为无干扰工况下两种算法姿态角对比图。

图5为有干扰工况下两种算法坐标对比图。

图6为有干扰工况下两种算法姿态角对比图。

具体实施方式

下面的实施例可以使本专业的技术人员更全面地理解本发明，但并不因此将本发明限制在所述的实施例范围之中。

实施例

本实施例水下机器人姿态控制方法，采用主从控制架构，包括控制台1、主控板2、从孔板一3和从孔板二4，从控板一3将电源的电压电流、电源温度、电子舱内温湿度以及电子舱是否发生漏水的信息采集过来后，经过各模块数据处理31之后，通过RS232通信模块32传递到主控板2；从控板二4将接收到的深度计以及电子罗盘的数据进行数据处理与解析41后，将处理后得到的深度以及导航数据通过RS232通信模块42传递到主控板2，主控板2接收导航和深度信息后，根据控制台1的姿态控制指令调用新型快速非奇异终端滑模控制方法(FNTSMC)23，计算出各个推进器的转速大小，经过DA 转换模块26后传递到各个推进器，从而实现机器人的姿态控制；主控板2将深度、航向、电源电压电流的信息通过指令解析与数据汇总模块22及以太网数据收发模块21实时传递到控制台1。

上述新型快速非奇异终端滑模控制方法(FNTSMC)，如图2所示，具体步骤如下：

步骤一：建立水下机器人的运动模型，在静水中，水下机器人动力学方程为：

式中：v＝[u v w p q r]；为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R^6×6；C(v)为水下机器人科氏及向心力矩阵，C(v)∈R^{6 ×6}；D(v)是水下机器人流体阻力矩阵， D(v)∈R^6×6；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R^6×1；τ是水下机器人推进器提供的推力，τ∈R^6×1。

在实际运行时，需要考虑参数不确定项和未知干扰项对机器人运动的影响，假设ξ是环境干扰力，如水流、波浪等，ξ∈R^6×1，v_d表示伴随干扰力产生的速度，则实际动力学模型应为

其中，C(v)＝C_RB+C_As，v_ξ＝v-v_d，这里假定v_d是慢时变的，则有

ROV重心为x_G＝y_G＝z_G＝0，惯性矩阵M为

C(v)由科氏向心力矩阵和附加质量矩阵组成，表示为

式中

阻尼力D(v)主要由阻力一次项和二次项组成，即

D＝-diag{X_u+X_u|u||u|,Y_v+Y_v|v||v|,Z_w+Z_w|w||w|,

K_p+K_p|p||p|,M_q+M_q|q||q|,N_r+N_r|r||r}

回复力矩阵g(η)为重力和浮力向量：

其中，W和B分别为重力和浮力，x_B、y_B和z_B为ROV浮心。

为方便控制方程的推导，对水下机器人模型进行简化。水下机器人模型转化成如下2阶非线性被控对象，即

其中，x＝[x₁,x₂]^T∈Rⁿ是系统的状态变量；f(x)和b(x)是x的非线性函数；d(t)是外界干扰和系统不确定因素，并||d(t)||≤D,D＞0；u是系统的控制输入。

步骤二：建立新型非奇异终端滑模面。首先定义运算：

提出的新型非奇异终端滑模面如下式所示

其中，分段函数δ(e_i)定义为：

其中，0＜ε_i≤0.5。

对所提滑模面的连续性进行验证，当系统误差处于e_i＝0时，滑模面偏导数如下式所示

可得，系统在e_i＝0处是连续的。同理可得，系统在|e_i|＝ε_i处也是连续的。所以，本发明提出的新型滑模面在各个区间上均是连续可导的。

步骤三：建立水下机器人姿态控制方程。滑模趋近律方程选取为：

其中，k₁＝diag(k₁₁,…,k₁₆)，k₂＝diag(k₂₁,…,k₂₆)，k_1i，k_2i为常数，ρ_i＞0， i＝1,2,…,6。

则，水下机器人姿态控制方程如下式所示：

对所提控制方法误差收敛时间的有限性进行验证。由滑模面方程可知，当s＝0时可得

当|e_i|＞ε_i时，假设系统状态从e_i(0)收敛到ε_i所需的时间为t₁，则

可得收敛时间t₁为

当|e_i|≤ε_i时，假设系统状态从e_i(0)收敛到0所需的时间为t₂，则

可得收敛时间t₂为

可得，e_i可从滑模面上任意位置在有限时间内收敛到平衡点，收敛时间T_s为

综上所述，系统能够在有限时间内收敛到平衡点0。

步骤四：主控板根据实时传输过来的导航和深度的信息，通过姿态控制方程对水下机器人的姿态进行实时调整。

对本发明的一种水下机器人姿态控制方法的有益效果进行分析如下：

将本发明提出的新型快速非奇异终端滑模控制方法(FNTSMC)与传统非奇异终端滑模控制方法(NTSMC)进行仿真对比，分别在无干扰和有干扰的工况下进行仿真研究。假设ROV从原点处，以45°航向角，航行至(x,y,z)＝(10m,10m,-5m)处，并定深在5米深度，机器人的姿态角设定为(yaw,pitch,roll)＝(45°,0,0)。

无干扰工况下仿真对比图如图2、图3所示。由图2可知，水下机器人能够按照设定的运动情况，快速、平稳地到达指定位置。由仿真结果可以看出，在到达阶段，本发明所提FNTSMC明显比传统方法NTSMC的误差收敛速度快。由图3可知，在姿态角度调整过程中， FNTSMC算法在调整俯仰角以及航向角时比NTSMC算法有更快的收敛速度。无干扰工况下跟踪误差的均方差如表1所示。

表1 跟踪误差的均方差（无干扰）

控制器
					NTSMC	0.048	0.095	0.043	0.024
FNTSMC	0.023	0.039	0.029	0.012

有干扰工况下仿真对比图如图4、图5所示。在无干扰实验的基础上加入两次波浪干扰，一次在航行过程中(25s<t<50s)，一次在航行到目标位置后(130s<t<180s)。由图4可知，FNTSMC的误差收敛速度依旧能够比NTSMC更加快速。由图5可知，在航向角的调整上，面对干扰时，FNTSMC的误差收敛速度更快，同时，NTSMC在航向角的调整上有一个较大的波动，而FNTSMC能够很好的钳制住航向角，保持稳定。无干扰工况下跟踪误差的均方差如表2所示。

表2 跟踪误差的均方差（有干扰）

控制器
					NTSMC	0.075	0.110	0.093	0.074
FNTSMC	0.043	0.059	0.061	0.029

以上显示和描述了本发明的基本原理和主要特征以及本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (3)

1.一种水下机器人姿态控制方法，其特征在于：采用主从控制架构，包括控制台、主控板、从孔板一和从孔板二，从控板一将电源的电压电流、电源温度、电子舱内温湿度以及电子舱是否发生漏水的信息采集过来后，经过各模块数据处理之后，通过RS232通信模块传递到主控板；从控板二将接收到的深度计以及电子罗盘的数据进行数据处理与解析后，将处理后得到的深度以及导航数据通过RS232通信模块传递到主控板，主控板接收导航和深度信息后，根据控制台的姿态控制指令调用新型快速非奇异终端滑模控制方法，计算出各个推进器的转速大小，经过DA转换模块后传递到各个推进器，从而实现机器人的姿态控制；主控板将深度、航向、电源电压电流的信息通过指令解析与数据汇总模块及以太网数据收发模块实时传递到控制台。

2.根据权利要求1所述的水下机器人姿态控制方法，其特征在于：所述新型快速非奇异终端滑模控制方法包括如下步骤：

（1）建立水下机器人的运动模型，在静水中，水下机器人动力学方程为：

式中：；；，为坐标转换矩阵；为水下机器人惯性矩阵，；为水下机器人科氏及向心力矩阵，；是水下机器人流体阻力矩阵，；是由重力和浮力组成的回复力矩阵，；是水下机器人推进器提供的推力，；

（2）建立新型非奇异终端滑模面，首先定义运算：

提出的新型非奇异终端滑模面，如下式所示

其中，分段函数定义为：

其中，；

（3）建立水下机器人姿态控制方程，滑模趋近律方程选取为：

其中，，，，为常数，，；则， 水下机器人姿态控制方程如下式所示：

（4）

主控板根据实时传输过来的导航和深度的信息，通过姿态控制方程对水下机器人的姿态进行实时调整。

3.根据权利要求2所述的水下机器人姿态控制方法，其特征在于：建立水下机器人的运 动模型，在实际运行时，需要考虑参数不确定项和未知干扰项对机器人运动的影响，假设 是环境干扰力， ，表示伴随干扰力产生的速度，则实际动力学模型应为

其中，，，这里假定是慢时变的，则有。