CN105676867A

CN105676867A - 一种rov水下机器人悬浮姿态稳定控制方法

Info

Publication number: CN105676867A
Application number: CN201610249564.8A
Authority: CN
Inventors: 李宏胜; 汪允鹤; 陈巍
Original assignee: Nanjing Institute of Technology
Current assignee: Nanjing Institute of Technology
Priority date: 2016-04-21
Filing date: 2016-04-21
Publication date: 2016-06-15
Anticipated expiration: 2036-04-21
Also published as: CN105676867B

Abstract

本发明提供了一种ROV水下机器人悬浮姿态稳定控制方法，采用对水下机器人模型的机体姿态与机体深度及机体平面移动与机体姿态进行分离控制的方法，并分别在水下机器人框架结构内建立数学模型，利用传感器对水下机器人的机体状态进行检测，并反馈机体姿态的欧拉角与机体位置参数，与期望的机体姿态的欧拉角与机体位置参数进行做差处理，通过双闭环控制算法对机体位置和姿态的误差进行调节，继而对水下机器人的推进器转速进行控制，最终实现在水下复杂扰动的环境下水下机器人依然能够维持姿态的稳定控和运动到期望位置的控制，以确保水下机器人能够在水下顺利的执行规定动作，并完成水下目标任务。

Description

一种ROV水下机器人悬浮姿态稳定控制方法

技术领域

本发明涉及一种ROV水下机器人悬浮姿态稳定控制方法，属于水下机器人控制技术领域。

背景技术

在我国，水下作业主要是依赖于人和简单的潜水器，而复杂多变的海洋和水下环境对人的生命安全造成很大的威胁，这就依赖于水下机器人代替人去执行水下作业任务，例如水下搜救打捞、考古科研、海洋生物观测、调查取证、海洋油气管道调查、能源勘探等工作，不仅可保障工作人员安全，还可以节约作业成本。同时由于水下环境的复杂性，要求水下机器人具备较高的机动性、稳定性和操作性，在水下机器人执行水下作业任务的过程中，水下机器人在水下环境多方面的扰动下仍然能够做出指定动作来完成规定任务，这就要求水下机器人能够实时调整自身姿态与机体深度，使机体能够保持机体姿态稳定和维持在指定深度，从而保证机器人能够做出规定的动作来完成目标任务。

发明内容

本发明的目的是提供一种ROV水下机器人悬浮姿态稳定控制方法，解决现有技术中存在的不足和缺陷，能够为实现机器人水下悬停和执行任务时保持机体姿态的稳定，提升水下机器人在水下执行任务时的稳定性、快速性和准确性。

为解决上述技术问题，本发明提供了一种ROV水下机器人悬浮姿态稳定控制方法，包括以下步骤：

步骤一、建立水下机器人的姿态数学模型和深度数学模型；

本步骤对水下机器人模型的机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）及机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）进行分离控制，并分别建立数学模型；

步骤二、给定水下机器人机体的期望姿态稳定参数（翻滚角Φ、俯仰角Θ、偏航角Ψ）和期望深度值（深度Z）；

步骤三、采用传感器对水下机器人的机体姿态（翻滚角Φ、俯仰角Θ、偏航角Ψ）和深度（深度Z）状态的信息进行采集；

步骤四、将水下机器人机体的期望姿态稳定参数和深度期望值与传感器采集到的机体状态的反馈数据进行对比；

步骤五、对水下机器人的机体姿态和深度状态的控制，加入双闭环PID算法进行调节；

步骤六、根据步骤一中的数学模型，对内环PID输出项进行力学模型处理；

本步骤采用步骤一中水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）及机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）建立分离式控制的数学模型，而水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）的稳定与深度（Z轴向）的调整采用垂直方向的四台推进器进行控制，参考四旋翼飞行器机体控制模型的原有四个虚拟输入量、、、，如下式所示：

（1）

式中：为螺旋桨升力系数、为螺旋桨对机体质心的扭矩系数、为推进器到机体质心的距离；

对四旋翼飞行器机体控制模型的原有四个虚拟输入量、、、，进行重新配置，应用于水下机器人机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）的控制，如下式所示：

（2）

进一步对上式进行处理，可得：

（3）

式中矩阵；

进一步，可得垂直方向的四台推进器转速、、、，即

（4）

进一步，当水下机器人的机体姿态和深度偏离机体的期望姿态稳定参数和期望深度值时，将机体传感器检测的机体姿态和深度值反馈，并与机体的期望姿态稳定参数和期望深度值进行做差，得到误差值，经PID控制算法，调整水下机器人的机体姿态和深度状态，确保水下机器人的机体位置在稳定姿态和深度的状态。

步骤七、根据力学模型处理的输出转速参数对水下机器人垂直方向推进器进行油门控制；

步骤八、判断是否满足水下机器人的机体期望姿态和深度状态，判断结果成立，则执行步骤九，否则执行步骤四；

步骤九、达到机器人机体在水下的期望姿态稳定参数和期望深度值。

在本发明中：所述的水下机器人的机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）的数学模型，采用水平平面上的四台推进器进行控制，使用三个虚拟输入量、、，应用于水下机器人的机体平面移动（X轴向、Y轴向）与姿态（偏航角Ψ）的控制，如下式所示：

(5)

即（6）

式中矩阵，；

进一步，对3×4矩阵B进行奇异值（SVD）分解，矩阵B表示为：

可得，然后利用、和可得矩阵A的广义逆矩阵，即

式中；

最后，根据最小二乘法计算，求得：

（7）

即可得平面移动的四台推进器转速、、、。

采用上述技术方案后，本发明的有益效果为：解决现有技术中存在的不足和缺陷，能够为实现机器人水下悬停和执行任务时保持机体姿态的稳定，提升水下机器人在水下执行任务时的稳定性、快速性和准确性。

附图说明

图1为本发明的ROV水下机器人机体姿态稳定和深度控制方法的流程示意图；

图2为本发明的ROV水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）的控制方法流程示意图；

图3为本发明的ROV水下机器人的机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）的控制方法流程示意图；

图4为本发明的ROV水下机器人推进装置的布置结构示意图；

图5为本发明的ROV水下机器人垂直方向推进装置的布置结构及坐标系结构示意图；

图6为本发明的ROV水下机器人水平方向推进装置的布置结构及坐标系结构示意图。

具体实施方式

本发明所采用的ROV水下机器人的机体采用对称平衡的结构设计，即机体框架以及机体搭载的装置在几何形状上对称，机体配置的对称平衡设计能够保证机体重心向下、机体自身稳定有一定的自适应性，ROV水下机器人推进装置的布置结构如图4所示，图中序号1-8为水下机器人推进器的序号，ROV水下机器人垂直方向推进装置的布置结构如图5所示，图中序号1-4为水下机器人推进器的序号，ROV水下机器人水平方向推进装置的布置结构如图6所示，图中序号5-8为水下机器人推进器的序号。

为了能够更好的了解本发明的技术特征、技术内容及其达到的技术效果，现将本发明的附图结合实施例进行更加详细的说明。

接下来的内容，将结合附图和实施例对本发明专利的内容做进一步的说明。

如图1所示，本发明提供了一种ROV水下机器人悬浮姿态稳定控制方法，包括以下步骤：

步骤一、建立水下机器人的姿态数学模型和深度数学模型；

本发明所采用的ROV水下机器人推进装置的布置结构如图4所示，机体框架以及机体搭载的装置在几何形状上对称；

本步骤为方便确切的描述水下机器人机体的运动状态，更好的建立水下机器人的数学模型，需对机体建立两个坐标系，分别为：地面基坐标系E和机体姿态坐标系B，机体坐标系的配置如图5所示；

为更好的描述两个坐标系间的转换关系，定义三个欧拉角：

翻滚角Φ：机体绕X轴旋转与Y轴形成的角度；

俯仰角Θ：机体绕Y轴旋转与X轴形成的角度；

偏航角Ψ：机体绕Z轴旋转形成的角度；

两个坐标系间，由地面基坐标系E到机体姿态坐标系B的转换关系如下：

本步骤对水下机器人模型的机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）及机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）进行分离控制，并分别建立数学模型；机体姿态（翻滚角Φ、俯仰角Θ）与深度Z控制方法流程如图2所示，机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）控制方法流程如图3所示；

为方便建立水下机器人的动力学数学模型且便于控制算法的研究，不失一般性地作如下假设：

（1）、水下机器人是一个均匀对称的刚体，忽略自身的弹性振动和变形；

（2）、地面基坐标系为惯性坐标系，且起始时刻地面基坐标系的原点和水下机器人的质心即几何中心位于同一位置；

（3）、水下机器人在水下所受阻力和重力不受下沉深度等因素影响，总保持不变；

（4）、水下机器人各个方向的拉力与推进器转速的平方成正比；

进一步，根据以上假设条件对水下机器人建立动力学数学模型；

步骤三、采用传感器对水下机器人的机体姿态（翻滚角Φ、俯仰角Θ、偏航角Ψ）和深度（Z轴向）状态的信息进行采集；

其中，所述姿态传感器，采用AHRS航姿参考系统，并融合了电子罗盘、三轴陀螺仪和三轴加速度计，能够直接输出水下机器人当前的翻滚角Φ、俯仰角Θ、偏航角Ψ，以及地面基坐标系E在X、Y、Z三个轴上旋转角速度和角加速度；水深传感器能够反馈水下机器人的机体在水下的深度Z；

步骤四、将水下机器人机体的期望姿态稳定参数和深度值与传感器采集到的机体状态的反馈数据进行对比；

本步骤采用步骤一中的水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）及机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）建立分离式控制模型，为实现对水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）的稳定与深度（Z轴向）的调整，则采用垂直方向的四台推进器进行控制，推进装置的布置结构如图5所示，参考四旋翼飞行器机体控制模型的原有四个虚拟输入量、、、，如下式所示：

（1）

（2）

进一步对式（2）进行处理，可得：

（3）

式中矩阵；

进一步，可得垂直方向的四台推进器转速、、、，即

（4）

进一步，当水下机器人的机体姿态和深度偏离机体的期望的姿态稳定参数和期望深度时，将机体传感器检测到的机体姿态和深度值进行反馈，并与机体的期望姿态和深度值进行做差，得到误差值，经PID算法，从而调整水下机器人的机体姿态和深度，确保水下机器人的机体能够维持姿态稳定和指定深度的状态；

针对水下机器人机体的机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）的数学模型，采用水平平面上的四台推进器进行控制，推进装置的布置结构如图6所示，使用三个虚拟输入量、、（如式5所示），应用于水下机器人的机体平面移动（X轴向、Y轴向）与姿态（偏航角Ψ）的控制，如下式所示：

(5)

即（6）

式中矩阵，；

进一步，对3×4矩阵B进行奇异值（SVD）分解，矩阵B表示为：

可得，然后利用、和可得矩阵A的广义逆矩阵，即

式中；

最后，根据最小二乘法计算，求得：

（7）

即可得平面移动的四台推进器转速、、、；

步骤九、达到机器人的机体在水下的期望姿态稳定参数和期望深度值。

以上对本发明的具体实施方式进行了描述，但本发明并不限于以上描述。对于本领域的技术人员而言，任何对本技术方案的同等修改和替代都是在本发明的范围之中。因此，在不脱离本发明的精神和范围下所作的均等变换和修改，都应涵盖在本发明的范围内。

Claims

1.一种ROV水下机器人悬浮姿态稳定控制方法，其特征在于，包括以下步骤：

步骤一、建立水下机器人的姿态数学模型和深度数学模型；

步骤四、将水下机器人机体的期望姿态稳定参数和期望深度值与传感器采集到的机体状态的反馈数据进行对比；

步骤六、根据步骤一中的水下机器人的姿态数学模型和深度数学模型，对内环PID输出项进行力学模型处理；

2.根据权利要求1所述的ROV水下机器人悬浮姿态稳定控制方法，其特征在于：所述的水下机器人机体传感器检测数据与水下机器人水下期望姿态稳定参数和期望深度值的偏差，通过双闭环PID控制算法对偏差值进行调节。

3.根据权利要求1所述的ROV水下机器人悬浮姿态稳定控制方法，其特征在于：所述步骤六采用的水下机器人的姿态数学模型和深度数学模型对内环PID输出项进行力学模型处理如下：其中，所述水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）与深度（Z轴向）及机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）建立分离式控制模型，而水下机器人的机体姿态（翻滚角Φ、俯仰角Θ）的稳定与深度（Z轴向）的调整采用垂直方向的四台推进器进行控制，参考四旋翼飞行器机体控制模型的原有四个虚拟输入量、、、，如下式所示：

（1）

（2）

进一步对式（2）进行处理，可得：

（3）

式中矩阵；

进一步，可得垂直方向的四台推进器转速、、、，即

（4）

进一步，当水下机器人的机体姿态和深度偏离机体的期望姿态稳定参数和期望深度值时，将机体传感器检测到的机体姿态和深度值反馈，并与机体的期望姿态稳定参数和期望深度值进行做差，得到误差值，经PID算法，调整水下机器人的机体姿态和深度状态，确保水下机器人的机体位置在稳定姿态和深度的状态。

4.根据权利要求1所述的ROV水下机器人悬浮姿态稳定控制方法，其特征在于：所述的水下机器人的机体平面移动（X轴向、Y轴向）与机体姿态（偏航角Ψ）的数学模型，采用水平平面上的四台推进器进行控制，使用三个虚拟输入量、、，应用于水下机器人的机体平面移动（X轴向、Y轴向）与姿态（偏航角Ψ）的控制，如下式所示：

(5)

即（6）

式中矩阵，；

进一步，对3×4矩阵B进行奇异值（SVD）分解，矩阵B表示为：

可得，然后利用、和可得矩阵A的广义逆矩阵，即

式中；

最后，根据最小二乘法计算，求得：

（7）

即可得平面移动的四台推进器转速、、、。