一种rov水下机器人悬浮姿态稳定控制方法
【技术领域】
[0002] 本发明涉及一种R0V水下机器人悬浮姿态稳定控制方法，属于水下机器人控制技 术领域。
【背景技术】
[0003] 在我国，水下作业主要是依赖于人和简单的潜水器，而复杂多变的海洋和水下环 境对人的生命安全造成很大的威胁，这就依赖于水下机器人代替人去执行水下作业任务， 例如水下搜救打捞、考古科研、海洋生物观测、调查取证、海洋油气管道调查、能源勘探等工 作，不仅可保障工作人员安全，还可以节约作业成本。同时由于水下环境的复杂性，要求水 下机器人具备较高的机动性、稳定性和操作性，在水下机器人执行水下作业任务的过程中， 水下机器人在水下环境多方面的扰动下仍然能够做出指定动作来完成规定任务，这就要求 水下机器人能够实时调整自身姿态与机体深度，使机体能够保持机体姿态稳定和维持在指 定深度，从而保证机器人能够做出规定的动作来完成目标任务。
【发明内容】
[0004] 本发明的目的是提供一种R0V水下机器人悬浮姿态稳定控制方法，解决现有技术 中存在的不足和缺陷，能够为实现机器人水下悬停和执行任务时保持机体姿态的稳定，提 升水下机器人在水下执行任务时的稳定性、快速性和准确性。
[0005] 为解决上述技术问题，本发明提供了一种R0V水下机器人悬浮姿态稳定控制方法， 包括以下步骤： 步骤一、建立水下机器人的姿态数学模型和深度数学模型； 本步骤对水下机器人模型的机体姿态(翻滚角Φ、俯仰角Θ )与深度(Z轴向）及机体平 面移动(X轴向、Y轴向）与机体姿态(偏航角Ψ )进行分离控制，并分别建立数学模型； 步骤二、给定水下机器人机体的期望姿态稳定参数(翻滚角Φ、俯仰角?、偏航角Ψ)和 期望深度值(深度Z); 步骤三、采用传感器对水下机器人的机体姿态(翻滚角Φ、俯仰角?、偏航角Ψ)和深度 (深度Z)状态的彳目息进行米集； 步骤四、将水下机器人机体的期望姿态稳定参数和深度期望值与传感器采集到的机体 状态的反馈数据进行对比； 步骤五、对水下机器人的机体姿态和深度状态的控制，加入双闭环PID算法进行调节； 步骤六、根据步骤一中的数学模型，对内环PID输出项进行力学模型处理； 本步骤采用步骤一中水下机器人的机体姿态(翻滚角Φ、俯仰角Θ)与深度(Z轴向）及 机体平面移动(X轴向、Y轴向）与机体姿态(偏航角Ψ )建立分离式控制的数学模型，而水下 机器人的机体姿态(翻滚角Φ、俯仰角Θ )的稳定与深度(Z轴向）的调整采用垂直方向的四 台推进器进行控制，参考四旋翼飞行器机体控制模型的原有四个虚拟输入量Μ、％、％:、 每，如下式所示：
式中：％为螺旋桨升力系数、&为螺旋桨对机体质心的扭矩系数、友为推进器到机体 质心的距离； 对四旋翼飞行器机体控制模型的原有四个虚拟输入量巧、％、蘇、试，进行重新配 置，应用于水下机器人机体姿态(翻滚角Φ、俯仰角Θ)与深度(Ζ轴向）的控制，如下式所示：
进一步对上式进行处理，可得：
进一步，可得垂直方向的四台推进器转速％、％、叫、叫，即
进一步，当水下机器人的机体姿态和深度偏离机体的期望姿态稳定参数和期望深度值 时，将机体传感器检测的机体姿态和深度值反馈，并与机体的期望姿态稳定参数和期望深 度值进行做差，得到误差值，经PID控制算法，调整水下机器人的机体姿态和深度状态，确保 水下机器人的机体位置在稳定姿态和深度的状态。
[0006]步骤七、根据力学模型处理的输出转速参数对水下机器人垂直方向推进器进行油 门控制； 步骤八、判断是否满足水下机器人的机体期望姿态和深度状态，判断结果成立，则执行 步骤九，否则执行步骤四； 步骤九、达到机器人机体在水下的期望姿态稳定参数和期望深度值。
[0007] 在本发明中：所述的水下机器人的机体平面移动(X轴向、Y轴向）与机体姿态(偏航 角Ψ )的数学模型，采用水平平面上的四台推进器进行控制，使用三个虚拟输入量&、％、 Εζ,，应用于水下机器人的机体平面移动(X轴向、Y轴向）与姿态(偏航角Ψ)的控制，如下式 所示：
进一步，对3 X 4矩阵Β进行奇异值(SVD)分解，矩阵Β表示为：
可得[?7，Λ,Η = ^￡>04)，然后利用y、Λ和Γ可得矩阵Α的广义逆矩阵#，SP
式4
最后，根据最小二乘法计算，求得：
即可得平面移动的四台推进器转速A、碼、瑪、呷。
[0008] 采用上述技术方案后，本发明的有益效果为:解决现有技术中存在的不足和缺陷， 能够为实现机器人水下悬停和执行任务时保持机体姿态的稳定，提升水下机器人在水下执 行任务时的稳定性、快速性和准确性。
【附图说明】
[0009] 图1为本发明的R0V水下机器人机体姿态稳定和深度控制方法的流程示意图； 图2为本发明的ROV水下机器人的机体姿态(翻滚角Φ、俯仰角θ )与深度(Z轴向）的控 制方法流程示意图； 图3为本发明的R0V水下机器人的机体平面移动(X轴向、Y轴向）与机体姿态(偏航角Ψ ) 的控制方法流程示意图； 图4为本发明的R0V水下机器人推进装置的布置结构示意图； 图5为本发明的R0V水下机器人垂直方向推进装置的布置结构及坐标系结构示意图； 图6为本发明的R0V水下机器人水平方向推进装置的布置结构及坐标系结构示意图。
【具体实施方式】
[0010]本发明所采用的R0V水下机器人的机体采用对称平衡的结构设计，即机体框架以 及机体搭载的装置在几何形状上对称，机体配置的对称平衡设计能够保证机体重心向下、 机体自身稳定有一定的自适应性，R0V水下机器人推进装置的布置结构如图4所示，图中序 号1-8为水下机器人推进器的序号，R0V水下机器人垂直方向推进装置的布置结构如图5所 示，图中序号1-4为水下机器人推进器的序号，R0V水下机器人水平方向推进装置的布置结 构如图6所示，图中序号5-8为水下机器人推进器的序号。
[0011] 为了能够更好的了解本发明的技术特征、技术内容及其达到的技术效果，现将本 发明的附图结合实施例进行更加详细的说明。
[0012] 接下来的内容，将结合附图和实施例对本发明专利的内容做进一步的说明。
[0013] 如图1所示，本发明提供了一种R0V水下机器人悬浮姿态稳定控制方法，包括以下 步骤： 步骤一、建立水下机器人的姿态数学模型和深度数学模型； 本发明所采用的R0V水下机器人推进装置的布置结构如图4所示，机体框架以及机体搭 载的装置在几何形状上对称； 本步骤为方便确切的描述水下机器人机体的运动状态，更好的建立水下机器人的数学 模型，需对机体建立两个坐标系，分别为:地面基坐标系E和机体姿态坐标系B，机体坐标系 的配置如图5所示； 为更好的描述两个坐标系间的转换关系，定义三个欧拉角： 翻滚角Φ :机体绕X轴旋转与Y轴形成的角度； 俯仰角Θ :机体绕Y轴旋转与X轴形成的角度； 偏航角Ψ :机体绕Z轴旋转形成的角度； 两个坐标系间，由地面基坐标系E到机体姿态坐标系B的转换关系如下：
本步骤对水下机器人模型的机体姿态(翻滚角Φ、俯仰角Θ )与深度(Z轴向）及机体平 面移动(X轴向、Y轴向）与机体姿态(偏航角Ψ )进行分离控制，并分别建立数学模型;机体姿 态(翻滚角Φ、俯仰角Θ )与深度Z控制方法流程如图2所示，机体平面移动(X轴向、Y轴向）与 机体姿态(偏航角Ψ )控制方法流程如图3所示； 为方便建立水下机器人的动力学数学模型且便于控制算法的研究，不失一般性地作如 下假设： (1 )、水下机器人是一个均匀对称的刚体，忽略自身的弹性振动和变形； (2) 、地面基坐标系为惯性坐标系，且起始时刻地面基坐标系的原点和水下机器人的质 心即几何中心位于同一位置； (3) 、水下机器人在水下所受阻力和重力不受下沉深度等因素影响，总保持不变； (4) 、水下机器人各个方向的拉力与推进器转速的平方成正比； 进一步，根据以上假设条件对水下机器人建立动力