CN105619394A

CN105619394A - 一种基于误差四元数反馈的rov姿态控制方法

Info

Publication number: CN105619394A
Application number: CN201610108631.4A
Authority: CN
Inventors: 温泉; 宋晓暖; 曹雪峰; 徐兴洲; 孙伟志; 熊海霞
Original assignee: QINGDAO HISUN OCEAN EQUIPMENT Co Ltd
Current assignee: QINGDAO HISUN OCEAN EQUIPMENT Co Ltd
Priority date: 2016-02-29
Filing date: 2016-02-29
Publication date: 2016-06-01

Abstract

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

Description

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

技术领域

本发明涉及一种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 [\overset{\cdot}{u} - v r + w q - x_{g} (q^{2} + r^{2}) + y_{g} (p q - \overset{\cdot}{r}) + z_{g} (p r + \overset{\cdot}{q})]

Y = m [\overset{\cdot}{v} - w p + u r - y_{g} (p^{2} + r^{2}) + z_{g} (q r - \overset{\cdot}{p}) + x_{g} (q p + \overset{\cdot}{r})]

Z = m [\overset{\cdot}{r} - w q + v q - z_{g} (q^{2} + p^{2}) + x_{g} (r p - \overset{\cdot}{q}) + y_{g} (r p + \overset{\cdot}{p})]

K = I_{x} \overset{\cdot}{p} + (I_{z} - I_{y}) q r + m [y_{g} (\overset{\cdot}{w} + v p - u q) - z_{g} (\overset{\cdot}{v} + u r - w p)]

M = I_{y} \overset{\cdot}{q} + (I_{x} - I_{z}) r p + m [z_{g} (\overset{\cdot}{u} + w q - v r) - x_{g} (\overset{\cdot}{w} + v p - u q)]

N = I_{Z} \overset{\cdot}{r} + (I_{y} - I_{x}) p q + m [x_{g} (\overset{\cdot}{v} + u r - w p) - y_{g} (\overset{\cdot}{u} + w q - v r)]

其中：

m-水下机器人质量；

x_g、y_g、z_g-水下机器人的重心坐标；

I_x、I_y、I_z-水下机器人的质量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具有近似的三个对称面，所以可以忽略其耦合作用，略去高阶项，只取到二阶。此时模型可简化为：

\{\begin{matrix} K = (I_{x} - K_{\overset{\cdot}{p}}) \overset{\cdot}{P} + [(I_{z} - N_{\overset{\cdot}{r}}) - (I_{y} - M_{\overset{\cdot}{q}})] r q + K_{p} p \\ M = (I_{y} - M_{\overset{\cdot}{q}}) \overset{\cdot}{q} + [(I_{x} - K_{\overset{\cdot}{p}}) - (I_{z} - N_{\overset{\cdot}{r}})] p r + M_{\overset{\cdot}{q}} q \\ N = (I_{z} - N_{\overset{\cdot}{r}}) \overset{\cdot}{r} + [(I_{y} - M_{\overset{\cdot}{q}}) - (I_{x} - K_{\overset{\cdot}{p}})] p q + N_{r} r \end{matrix}

用单位四元数来描述ROV的姿态，其表示的是ROV载体坐标系与惯性参考坐标系的旋转关系。用误差四元数来描述ROV当前姿态与目标姿态的旋转关系。这里声明相关的四元数符号：

表1四元数参数声明

名称	向量	实部	虚部(i)	虚部(j)	虚部(k)
						当前四元数	q	q₀	q₁	q₂	q₃
目标四元数	Q	Q₀	Q₁	Q₂	Q₃
						误差四元数	Δq	Δq₀	Δq₁	Δq₂	Δq₃

Δq＝q^-1×Q

这里的乘号为四元数乘法，q^-1为q的逆，等于q的共轭，即：

q^-1＝[q₀-q₁-q₂-q₃]^T

可求得：

Δ q = [\begin{matrix} q_{0} Q_{0} + q_{1} Q_{1} + q_{2} Q_{2} + q_{3} Q_{3} \\ q_{0} Q_{1} - q_{1} Q_{0} - q_{3} Q_{2} + q_{2} Q_{3} \\ q_{0} Q_{2} - q_{2} Q_{0} - q_{1} Q_{3} + q_{3} Q_{1} \\ q_{0} Q_{3} - q_{3} Q_{0} - q_{2} Q_{1} + q_{1} Q_{2} \end{matrix}]

在软件设计中，主程序进行数据采集、接受指令和传输数据，中断程序进行误差四元数计算、PID计算和电机控制信号计算，中断为定时中断，频率为50Hz。

为验证控制方法与ROV设计的可行性，使用Simulink对姿态控制系统进行仿真分析。

在图3中，控制系统首先读取传感器的数据，采集到当前四元数信息，然后与目标四元数对比，求得误差四元数。根据误差四元数求得当前姿态与目标姿态之间的旋转关系，输入PID控制器，对三个轴上的力矩进行计算，控制六个电机的推力，以控制ROV的姿态。

这样ROV的角速度发生变化，被传感器采集到，传感器便输出新的四元数姿态信息，系统进入下一个循环。

在图4—图7中，系统以阶跃信号做为输入，观察其响应，来判断系统的性能是否满足要求，PID参数是否是最佳参数。每个轴的阶跃响应曲线，可以看出其响应比较迅速，收敛速度非常快。当x轴的输入为一正弦信号，可以看出其输出响应曲线有一定的延时，但是稳定性较好。

在图8—图11中，一个系统的稳定行不能只检测输入信号不同时是否稳定，还要检测在有干扰的情况下是否具有很好的鲁棒性。这里同样采用两组实验，一组为三个轴的干扰全部输入阶跃信号，一组为x轴输入正弦干扰信号。

阶跃干扰信号幅值为10Nm，正弦干扰信号幅值为10Nm，相对于推进器的推力来说是比较大的干扰力矩，阶跃干扰信号的响应曲线显示系统回复很快，在短时间内便能回复目标姿态。正弦信号干扰时系统会出现同频率的摇动，其x轴姿态角会有一个同频率的幅值为0.2的正弦变化，其变化范围较小，系统稳定。

须陈明，以上所述乃是本发明的较佳实施例，若以本发明的构想所作的改变，其产生的功能作用仍未超出说明书与图示所涵盖的精神时，均应在本发明的范围内。

Claims

1.一种基于误差四元数反馈的ROV姿态控制方法，其特征是：

建立惯性坐标系和ROV的运动坐标系；

ROV对称结构设计，并对其三自由度旋转姿态进行建模；

2.根据权利要求1所述的一种基于误差四元数反馈的ROV姿态控制方法，其特征是：ROV对称结构设计为：在ROV的运动坐标系中的X、Y、Z旋转轴上推进器绕每个轴成对配置，控制ROV绕各轴旋转。

3.根据权利要求1所述的一种基于误差四元数反馈的ROV姿态控制方法，其特征是：当前四元数的逆与目标四元数进行乘法运算求出误差四元数。

4.根据权利要求1所述的一种基于误差四元数反馈的ROV姿态控制方法，其特征是：在软件设计中，主程序进行数据采集、接受指令和传输数据，中断程序进行误差四元数计算、PID计算和电机控制信号计算。