一种基于误差四元数反馈的rov姿态控制方法
【技术领域】
[0001]本发明涉及一种ROV姿态控制方法，具体地说是一种基于误差四元数反馈的ROV姿 态控制方法。
【背景技术】
[0002] 无人遥控潜水器(Remote Operated Vegicle)可用于作业、观测等多种用途，根据 具体的用途不同，其技术实现方案也不尽相同。传统的ROV其运动多为前后、左右、上下的直 线运动以及Z轴的旋转运动。但在一些较复杂的作业环境下，如贴附船底、壁面、管道等具有 连续性、较大附着面的作业，则需要ROV能够完成X、Y、Z三个方向的360°无死角旋转运动并 实现姿态稳定。相应ROV控制也不能使用常用的控制欧拉角的方法来进行姿态控制，因为欧 拉角法存在万向节锁的缺点，并且大多数的姿态传感器在输出欧拉角时，在± 180°或± 90° 时会出现不连续性。
[0003] 常用的旋转描述方法有欧拉角、四元数、旋转矩阵。其中欧拉角容易理解，形象直 观，只需要三个值，但是三个值不同的旋转顺序会导致不同的结果，必须按照一个固定的顺 序来旋转，并且会造成万向节锁现象；四元数方法可以避免万向节锁现象，只需要一个四维 的四元数就可以执行绕任意过原点的向量的旋转，并可提供平滑差值，但是其理解相对困 难，不直观，比欧拉角相对复杂;旋转矩阵法旋转轴可以是任意向量，但是矩阵法需要16个 元素，不仅理解困难，并且会大大增加计算量。
[0004] 中国专利201410422945.2公开的一种水下机器人位姿控制方法，主要是通过姿态 四元数获得欧拉角将期望值的误差输入PID控制器进行计算，仍无法实现360°无死角旋转 运动控制。
[0005] 因此设计一种能够避免奇异点对姿态调整与控制的影响，实现全角度稳定旋转的 ROV姿态控制方法是非常必要的。
【发明内容】
[0006] 本发明提供的一种基于误差四元数反馈的ROV姿态控制方法，能够控制ROV完成X、 Y、Z三个方向的360°无死角旋转运动，且收敛快、稳定性好。
[0007] 为了解决上述的技术问题，本发明采用下述技术方案：
[0008] 建立惯性坐标系和ROV的运动坐标系；
[0009] ROV对称结构设计，并对其三自由度旋转姿态进行建模；
[0010]姿态传感器对ROV进行检测输出姿态四元数，得到ROV的当前四元数与目标四元数 对比，通过运算求出误差四元数；
[0011]将误差四元数输入PID控制器，计算得出ROV运动坐标系中三个轴上所需的力矩， 处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成ROV的姿态调整。
[0012]进一步地，ROV对称结构设计为:在ROV的运动坐标系中的Χ、Υ、Ζ旋转轴上推进器绕 每个轴成对配置，控制ROV绕各轴旋转。
[0013] 进一步地，当前四元数的逆与目标四元数进行乘法运算求出误差四元数。
[0014] 进一步地，在软件设计中，主程序进行数据采集、接受指令和传输数据，中断程序 进行误差四元数计算、PID计算和电机控制信号计算。
[0015] 采用本发明的有益效果是，该ROV的姿态控制方法，可以使ROV完成360°无死角旋 转运动并实现姿态稳定，相比基于四元数或者旋转矩阵的控制方法，该控制方法易于理解、 设计简单，操作实施方便，相比基于欧拉角的控制方法不会出现万向节锁现象，从而导致控 制失灵，能够确保完成如贴附船底、壁面、管道等具有连续性、较大附着面的作业，且该方法 收敛快、稳定性好，在实际调试、使用过程中更可以灵活调整、优化升级，以更好的满足使用 需要。
【附图说明】
[0016] 图1是本发明所述一种基于误差四元数反馈的ROV姿态控制方法的主循环流程图；
[0017] 图2是本发明所述一种基于误差四元数反馈的ROV姿态控制方法的中断流程图；
[0018] 图3是本发明仿真模型总体框图；
[0019] 图4是本发明仿真X轴阶跃响应曲线图；
[0020] 图5是本发明仿真y轴阶跃响应曲线图；
[0021]图6是本发明仿真z轴阶跃响应曲线图；
[0022]图7是本发明仿真X轴正弦信号输入响应曲线图；
[0023] 图8是本发明仿真X轴阶跃干扰响应曲线图；
[0024] 图9是本发明仿真y轴阶跃干扰响应曲线图；
[0025] 图10是本发明仿真z轴阶跃干扰响应曲线图；
[0026]图11是本发明仿真X轴正弦干扰响应曲线图；
【具体实施方式】
[0027]下面结合附图和【具体实施方式】对本发明作进一步详细说明。
[0028] 在图1和图2中，建立惯性坐标系和ROV的运动坐标系；
[0029] 惯性坐标系为坐标原点E为地球上某点;Εξ轴位于水平面上，ROV前进方向为正;En 轴位于Εξ平面内按右手法则将Εξ顺时针转90° ;Εζ轴位于Εζξ平面，指向地心。运动坐标系为 坐标原点G取在ROV的中心G处，Gx轴平行于ROV基线指向前进方向；横向轴Gy平行于基面指 向ROV的右侧;而Gz轴则指向ROV底部。
[0030] ROV对称结构设计，并对其三自由度旋转姿态进行建模；
[0031] 根据刚体力学理论，当水下机器人重心和动系原点不重合时，其六自由度运动一 般方程为：
[0038]其中：
[0039] m-水下机器人质量；
[0040] xg、yg、Zg-水下机器人的重心坐标；
[00411 Ix、Iy、IZ-水下机器人的质量m对Ox、Oy、Oz、轴的转动惯量；
[0042] u、v、w、p、q、r_六自由度的（角)速度；
[0043] 众、旮、旮、'令、六自由度的（角）加速度；
[0044] 父、丫、2、1(^-六自由度的力(矩）。
[0045] 水下机器人的任意一个自由度之间的运动都和其他自由度运动有关，也就是说在 六个自由度之间存在交叉耦合，这是水下机器人的难点之一。根据ROV的外形与实际工作情 况，对ROV的三自由度旋转姿态进行建模仿真分析。
[0046]在ROV的运动坐标系中的X、Y、Z旋转轴上推进器绕每个轴成对配置，控制ROV绕各 轴旋转。当ROV在低速状态下作业时，因为ROV物理模型的设计已经使ROV具有近似的三个对 称面，所以可以忽略其耦合作用，略去高阶项，只取到二阶。此时模型可简化为：
[0048]姿态传感器对ROV进行检测输出姿态四元数，得到ROV的当前四元数与目标四元数 对比，通过运算求出误差四元数；
[0049]用单位四元数来描述ROV的姿态，其表示的是ROV载体坐标系与惯性参考坐标系的 旋转关系。用误差四元数来描述ROV当前姿态与目标姿态的旋转关系。这里声明相关的四元 数符号：
[0050] 表1四元数参数声明
[0052] Δ q = q_1 X Q
[0053] 这里的乘号为四元数乘法，(T1为q的逆，等于q的共辄，即：
[0054] q-1=[qQ-q1 -q2 -q3]T
[0055] 可求得：
[0057] 将误差四元数输入PID控制器，计算得出ROV运动坐标系中三个轴上所需的力矩， 处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成ROV的姿态调整。
[0058] 在软件设计中，主程序进行数据采集、接受指令和传输数据，中断程序进行误差四 元数计算、PID计算和电机控制信号计算，中断为定时中断，频率为50Hz。
[0059]为验证控制方法与ROV设计的可行性，使用Simulink对姿态控制系统进行仿真分 析。
[0060] 在图3中，控制系统首先读取传感器的数据，采集到当前四元数信息，然后与目标 四元数对比，求得误差四元数。根据误差四元数求得当前姿态与目标姿态之间的旋转关系， 输入PID控制器，对三个轴上的力矩进行计算，控制六个电机的推力，以控制ROV的姿态。
[0061] 这样ROV的角速度发生变化，被传感器采集到，传感器便输出新的四元数姿态信 息，系统进入下一个循环。
[0062]在图4一图7中，系统以阶跃信号做为输入，观察其响应，来判断系统的性能是否满 足要求，PID参数是否是最佳参数。每个轴的阶跃响应曲线，可以看出其响应比较迅速，收敛 速度非常快。当X轴的输入为一正弦信号，可以看出其输出响应曲线有一定的延时，但是稳 定性较好。
[0063]在图8-图11中，一个系统的稳定行不能只检测输入信号不同时是否稳定，还要检 测在有干扰的情况下是否具有很好的鲁棒性。这里同样采用两组实验，一组为三个轴的干 扰全部输入阶跃信号，一组为X轴输入正弦干扰信号。
[0064]阶跃干扰信号幅值为10Nm，正弦干扰信号幅值为10Nm，相对于推进器的推力来说 是比较大的干扰力矩，阶跃干扰信号的响应曲线显示系统回复很快，在短时间内便能回复 目标姿态。正弦信号干扰时系统会出现同频率的摇动，其X轴姿态角会有一个同频率的幅值 为0.2的正弦变化，其变化范围较小，系统稳定。
[0065]须陈明，以上所述乃是本发明的较佳实施例，若以本发明的构想所作的改变，其产 生的功能作用仍未超出说明书与图示所涵盖的精神时，均应在本发明的范围内。
【主权项】
1. 一种基于误差四元数反馈的ROV姿态控制方法，其特征是： 建立惯性坐标系和R0V的运动坐标系； R0V对称结构设计，并对其三自由度旋转姿态进行建模； 姿态传感器对R0V进行检测输出姿态四元数，得到R0V的当前四元数与目标四元数对 比，通过运算求出误差四元数； 将误差四元数输入PID控制器，计算得出R0V运动坐标系中三个轴上所需的力矩，处理 器根据所需力矩控制推进器电机的电压，产生相应的推力，完成R0V的姿态调整。2. 根据权利要求1所述的一种基于误差四元数反馈的R0V姿态控制方法，其特征是:R0V 对称结构设计为:在R0V的运动坐标系中的X、Y、Z旋转轴上推进器绕每个轴成对配置，控制 R0V绕各轴旋转。3. 根据权利要求1所述的一种基于误差四元数反馈的R0V姿态控制方法，其特征是：当 前四元数的逆与目标四元数进行乘法运算求出误差四元数。4. 根据权利要求1所述的一种基于误差四元数反馈的R0V姿态控制方法，其特征是:在 软件设计中，主程序进行数据采集、接受指令和传输数据，中断程序进行误差四元数计算、 PID计算和电机控制彳目号计算。
【专利摘要】本发明公开一种基于误差四元数反馈的ROV姿态控制方法，建立惯性坐标系和ROV的运动坐标系；并对其三自由度旋转姿态进行建模；姿态传感器对ROV进行检测输出姿态四元数，ROV的当前四元数与目标四元数对比，求出误差四元数；输入PID控制器，计算得出ROV运动坐标系中三个轴上所需的力矩，处理器根据所需力矩控制推进器电机的电压，产生相应的推力，完成ROV的姿态调整，使ROV完成360°无死角旋转运动并实现姿态稳定，实现较复杂工作环境下作业。
【IPC分类】B63C11/52, B25J9/04
【公开号】CN105619394
【申请号】CN201610108631
【发明人】温泉, 宋晓暖, 曹雪峰, 徐兴洲, 孙伟志, 熊海霞
【申请人】青岛海山海洋装备有限公司
【公开日】2016年6月1日
【申请日】2016年2月29日