一种基于误差四元数反馈的ROV姿态控制方法技术

本发明专利技术公开一种基于误差四元数反馈的ROV姿态控制方法，建立惯性坐标系和ROV的运动坐标系；并对其三自由度旋转姿态进行建模；姿态传感器对ROV进行检测输出姿态四元数，ROV的当前四元数与目标四元数对比，求出误差四元数；输入PID控制器，计算得出ROV运动坐标系中三个轴上所需的力矩，处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成ROV的姿态调整，使ROV完成360°无死角旋转运动并实现姿态稳定，实现较复杂工作环境下作业。

【技术实现步骤摘要】

本专利技术涉及一种ROV姿态控制方法，具体地说是一种基于误差四元数反馈的ROV姿态控制方法。
技术介绍
无人遥控潜水器(RemoteOperatedVegicle)可用于作业、观测等多种用途，根据具体的用途不同，其技术实现方案也不尽相同。传统的ROV其运动多为前后、左右、上下的直线运动以及Z轴的旋转运动。但在一些较复杂的作业环境下，如贴附船底、壁面、管道等具有连续性、较大附着面的作业，则需要ROV能够完成X、Y、Z三个方向的360°无死角旋转运动并实现姿态稳定。相应ROV控制也不能使用常用的控制欧拉角的方法来进行姿态控制，因为欧拉角法存在万向节锁的缺点，并且大多数的姿态传感器在输出欧拉角时，在±180°或±90°时会出现不连续性。常用的旋转描述方法有欧拉角、四元数、旋转矩阵。其中欧拉角容易理解，形象直观，只需要三个值，但是三个值不同的旋转顺序会导致不同的结果，必须按照一个固定的顺序来旋转，并且会造成万向节锁现象；四元数方法可以避免万向节锁现象，只需要一个四维的四元数就可以执行绕任意过原点的向量的旋转，并可提供平滑差值，但是其理解相对困难，不直观，比欧拉角相对复杂；旋转矩阵法旋转轴可以是任意向量，但是矩阵法需要16个元素，不仅理解困难，并且会大大增加计算量。中国专利201410422945.2公开的一种水下机器人位姿控制方法，主要是通过姿态四元数获得欧拉角将期望值的误差输入PID控制器进行计算，仍无法实现360°无死角旋转运动控制。因此设计一种能够避免奇异点对姿态调整与控制的影响，实现全角度稳定旋转的ROV姿态控制方法是非常必要的。
技术实现思路
本专利技术提供的一种基于误差四元数反馈的ROV姿态控制方法，能够控制ROV完成X、Y、Z三个方向的360°无死角旋转运动，且收敛快、稳定性好。为了解决上述的技术问题，本专利技术采用下述技术方案：建立惯性坐标系和ROV的运动坐标系；ROV对称结构设计，并对其三自由度旋转姿态进行建模；姿态传感器对ROV进行检测输出姿态四元数，得到ROV的当前四元数与目标四元数对比，通过运算求出误差四元数；将误差四元数输入PID控制器，计算得出ROV运动坐标系中三个轴上所需的力矩，处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成ROV的姿态调整。进一步地，ROV对称结构设计为：在ROV的运动坐标系中的X、Y、Z旋转轴上推进器绕每个轴成对配置，控制ROV绕各轴旋转。进一步地，当前四元数的逆与目标四元数进行乘法运算求出误差四元数。进一步地，在软件设计中，主程序进行数据采集、接受指令和传输数据，中断程序进行误差四元数计算、PID计算和电机控制信号计算。采用本专利技术的有益效果是，该ROV的姿态控制方法，可以使ROV完成360°无死角旋转运动并实现姿态稳定，相比基于四元数或者旋转矩阵的控制方法，该控制方法易于理解、设计简单，操作实施方便，相比基于欧拉角的控制方法不会出现万向节锁现象，从而导致控制失灵，能够确保完成如贴附船底、壁面、管道等具有连续性、较大附着面的作业，且该方法收敛快、稳定性好，在实际调试、使用过程中更可以灵活调整、优化升级，以更好的满足使用需要。附图说明图1是本专利技术所述一种基于误差四元数反馈的ROV姿态控制方法的主循环流程图；图2是本专利技术所述一种基于误差四元数反馈的ROV姿态控制方法的中断流程图；图3是本专利技术仿真模型总体框图；图4是本专利技术仿真x轴阶跃响应曲线图；图5是本专利技术仿真y轴阶跃响应曲线图；图6是本专利技术仿真z轴阶跃响应曲线图；图7是本专利技术仿真x轴正弦信号输入响应曲线图；图8是本专利技术仿真x轴阶跃干扰响应曲线图；图9是本专利技术仿真y轴阶跃干扰响应曲线图；图10是本专利技术仿真z轴阶跃干扰响应曲线图；图11是本专利技术仿真x轴正弦干扰响应曲线图；具体实施方式下面结合附图和具体实施方式对本专利技术作进一步详细说明。在图1和图2中，建立惯性坐标系和ROV的运动坐标系；惯性坐标系为坐标原点E为地球上某点；Eξ轴位于水平面上，ROV前进方向为正；Eη轴位于Eξ平面内按右手法则将Eξ顺时针转90°；Eζ轴位于Eζξ平面，指向地心。运动坐标系为坐标原点G取在ROV的中心G处，Gx轴平行于ROV基线指向前进方向；横向轴Gy平行于基面指向ROV的右侧；而Gz轴则指向ROV底部。ROV对称结构设计，并对其三自由度旋转姿态进行建模；根据刚体力学理论，当水下机器人重心和动系原点不重合时，其六自由度运动一般方程为：X=m[u·-vr+wq-xg(q2+r2)+yg(pq-r·)+zg(pr+q·)]]]>Y=m[v·-wp+ur-yg(p2+r2)+zg(qr-p·)+xg(qp+r·)]]]>Z=m[r·-wq+vq-zg(q2+p2)+xg(rp-q·)+yg(rp+p·)]]]>K=Ixp·+(Iz-Iy)qr+m[yg(w·+vp-uq)-zg(v·+ur-wp)]]]>M=Iyq·+(Ix-Iz)rp+m[zg(u·+wq-vr)-xg(w·+vp-uq)]]]>N=IZr·+(Iy-Ix)pq+m[xg(v·+ur-wp)-yg(u·+wq-vr)]]]>其中：m-水下机器人质量；xg、yg、zg-水下机器人的重心坐标；Ix、Iy、Iz-水下机器人的质量m对Ox、Oy、Oz、轴的转动惯量；u、v、w、p、q、r-六自由度的(角)速度；-六自由度的(角)加速度；X、Y、Z、K、M、N-六自由度的力(矩)。水下机器人的任意一个自由度之间的运动都和其他自由度运动有关，也就是说在六个自由度之间存在交叉耦合，这是水下机器人的难点之一。根据ROV的外形与实际工作情况，对ROV的三自由度旋转姿态进行建模仿真分析。在ROV的运动坐标系中的X、Y、Z旋转轴上推进器绕每个轴成对配置，控制ROV绕各轴旋转。当ROV在低速状态下作业时，因为ROV物理模型的设计已经本文档来自技高网...

【技术保护点】
一种基于误差四元数反馈的ROV姿态控制方法，其特征是：建立惯性坐标系和ROV的运动坐标系；ROV对称结构设计，并对其三自由度旋转姿态进行建模；姿态传感器对ROV进行检测输出姿态四元数，得到ROV的当前四元数与目标四元数对比，通过运算求出误差四元数；将误差四元数输入PID控制器，计算得出ROV运动坐标系中三个轴上所需的力矩，处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成ROV的姿态调整。

【技术特征摘要】
1.一种基于误差四元数反馈的ROV姿态控制方法，其特征是：
建立惯性坐标系和ROV的运动坐标系；
ROV对称结构设计，并对其三自由度旋转姿态进行建模；
姿态传感器对ROV进行检测输出姿态四元数，得到ROV的当前四元数与
目标四元数对比，通过运算求出误差四元数；
将误差四元数输入PID控制器，计算得出ROV运动坐标系中三个轴上所需
的力矩，处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成
ROV的姿态调整。
2.根据权利要求1所述的一种基于误差四元数反馈的RO...

【专利技术属性】
技术研发人员：温泉，宋晓暖，曹雪峰，徐兴洲，孙伟志，熊海霞，
申请(专利权)人：青岛海山海洋装备有限公司，
类型：发明
国别省市：山东;37