CN105841698B - 一种无需调零的auv舵角精确实时测量系统 - Google Patents

Abstract

本发明公开了一种无需调零的AUV舵角精确实时测量系统，采用惯性导航器件设置于AUV的载体平台的质心位置处、三轴与载体平台三轴重合，测量并解算得到AUV载体平台在地理坐标系下的姿态信息；垂直舵姿态测量模块设置于AUV中的垂直舵板的质心位置、且其三轴与垂直舵板的三轴重合；水平舵姿态测量模块设置于AUV中的水平舵板的质心位置、且其三轴与水平舵板的三轴重合；AUV舵角解算模块解算获得垂直舵板和水平舵板在地理坐标系下的姿态信息；结合AUV载体平台、垂直舵板、水平舵板的姿态信息和垂直舵板、水平舵板与AUV的相对运动关系，解算出垂直舵角和水平舵角。该系统能够实现AUV舵角的精确、实时测量且无须进行调零操作。

Description

一种无需调零的AUV舵角精确实时测量系统

技术领域

本发明属于水下机器人控制技术领域，具体为一种无需调零的AUV(水下自主航行器)舵角精确实时测量系统。

背景技术

AUV作为一种自主水下航行器，当其在水下执行任务时，大多数情况下均处于自主控制状态。为了实现在水下的自主航行功能，AUV都配备有能够感知自身姿态和运动状态的传感器，以及能够通过这些传感器信息推算出AUV自身位置的仪器，这一系统通常为惯性导航系统。AUV自主航行控制系统通过从惯性导航系统获取的AUV姿态、运动状态和位置等信息形成对AUV的运动控制执行机构(包括主推电机，舵机等)的闭环控制，以实现AUV水下自主航行功能。

对于舵角的控制，直接决定AUV在水下自主航行过程中的航向控制和定深控制效果。AUV舵角控制采用闭环控制。当前用于获取舵角反馈信息的传感器主要有电位计、光电编码器和旋转变压器。这些传感器的输出均为相对某一零位的角度。而对于舵角零位的确定目前在传统方法中通常采用目测的方法。这就不可避免地给舵角测量带来了较大的人为误差。从而直接影响到AUV水下自主航行的性能。

发明内容

有鉴于此，本发明提供了一种无需调零的AUV舵角精确实时测量系统，能够消除AUV舵角测量过程中人为因素带来的误差,实现AUV舵角的精确、实时测量且无须进行调零操作。

为了达到上述目的，本发明的技术方案为：一种无需调零的AUV舵角精确实时测量系统，包括水下自主航行器AUV惯性导航系统、垂直舵姿态测量模块、水平舵姿态测量模块和AUV舵角解算模块。

AUV惯性导航系统包含惯性导航器件和导航计算机，其中惯性导航器件设置于AUV的载体平台的质心位置处、且其三轴与载体平台三轴重合，其测量获得的AUV载体平台姿态信息经导航计算机解算得到AUV载体平台在地理坐标系下的姿态信息。

垂直舵姿态测量模块设置于AUV中的垂直舵板的质心位置、且其三轴与垂直舵板的三轴重合。

水平舵姿态测量模块设置于AUV中的水平舵板的质心位置、且其三轴与水平舵板的三轴重合。

AUV舵角解算模块通过有线或无线通信接口实时接收AUV惯性导航系统、垂直舵姿态测量模块及水平舵姿态测量模块的测量信息；通过垂直舵姿态测量模块的测量信息，解算出垂直舵板在地理坐标系下的姿态信息；通过水平舵姿态测量模块的测量信息，解算出水平舵板在地理坐标系下的的姿态信息；结合AUV载体平台、垂直舵板、水平舵板的姿态信息和垂直舵板、水平舵板与AUV的相对运动关系，解算出垂直舵角和水平舵角。

进一步地，姿态信息包括航向角、俯仰角和横滚角。

进一步地，垂直舵姿态测量模块和水平舵姿态测量模块为微机械陀螺MEMS陀螺仪或者三轴MEMS加速度计与三轴磁强计的组合。

进一步地，水平舵板在AUV载体平台上安装时，水平舵板轴向与AUV载体平台右向平行，水平舵姿态测量模块的北向与AUV载体平台坐标系的前向一致，水平舵姿态测量模块的天向与AUV平台坐标系的上向一致，水平舵姿态测量模块的东向与AUV平台坐标系的右向一致。

垂直舵板在AUV载体平台上安装时，垂直舵板轴向与AUV载体平台上向平行；垂直舵姿态测量模块的北向与AUV载体平台坐标系的前向一致，垂直舵姿态测量模块的天向与AUV平台坐标系的上向一致，垂直舵姿态测量模块的东向与AUV平台坐标系的右向一致。

有益效果：

本发明通过在AUV的载体平台、垂直舵板和水平舵板上分别设置姿态测量模块，能够通过实施解算的方式计算AUV的舵角，该系统能够消除AUV舵角测量过程中人为因素带来的误差,实现AUV舵角的精确、实时测量且无须进行调零操作。

附图说明

图1为无需调零的AUV舵角精确实时检测原理框图：

图2为惯性导航设备坐标系与AUV载体平台坐标系关系。

图3为地磁坐标系、地理坐标系及航向角Ψ、俯仰角Θ、横滚角γ之间的关系。

图4为第一类姿态测量模块结构，主要由三轴MEMS陀螺仪和姿态解算微处理单元组成。

图5为第二类姿态测量模块结构，主要由三轴MEMS加速度计、三轴磁强计和姿态解算微处理单元组成。

图6为第三类姿态测量模块结构，主要由三轴MEMS陀螺仪、三轴MEMS加速度计、三轴磁强计和姿态解算微处理单元组成。

图7为利用三轴加速度计求解俯仰角和横滚角的原理示意。

图8为AUV载体平台与水平舵及其姿态测量模块安装关系及相对运动方式。

图9为AUV载体平台与垂直舵及其姿态测量模块安装关系及相对运动方式。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

实施例1、一种无需调零的AUV舵角精确实时测量系统，其组成如图1所示，包括水下自主航行器AUV惯性导航系统，垂直舵姿态测量模块，水平舵姿态测量模块和AUV舵角解算模块。

AUV惯性导航系统包含惯性导航器件和导航计算机，其中惯性导航器件设置于AUV的质心位置处、且其三轴与AUV三轴重合，二者坐标关系如图2所示。其测量获得的AUV姿态信息经导航计算机解算得到AUV在地理坐标系下的姿态信息。

垂直舵姿态测量模块为三轴角速度测量仪，该垂直舵姿态测量模块设置于AUV中的垂直舵板的质心位置、且其三轴与垂直舵板的三轴重合。

水平舵姿态测量模块为三轴角速度测量仪，该水平舵姿态测量模块设置于AUV中的水平舵板的质心位置、且其三轴与水平舵板的三轴重合。

AUV舵角解算模块通过有线或无线通信接口实时接收AUV惯性导航系统，垂直舵姿态测量模块，水平舵姿态测量模块的测量信息；通过垂直舵姿态测量模块的测量信息，解算出垂直舵板在地理坐标系下的姿态信息；通过水平舵姿态测量模块的测量信息，解算出水平舵板在地理坐标系下的的姿态信息；结合AUV、垂直舵板、水平舵板的姿态信息和垂直舵板、水平舵板与AUV的相对运动关系，解算出垂直舵角和水平舵角。其中地磁坐标系、地理坐标系及航向角Ψ、俯仰角Θ、横滚角γ之间的关系如图3所示。

本实施例中，姿态信息包括航向角、俯仰角和横滚角。

三轴角速度测量仪可以为微机械陀螺MEMS陀螺仪或者由三轴MEMS加速度计与三轴磁强计组合而成。

实施例2、

本实施例中，三轴角速度测量仪可以为微机械陀螺MEMS陀螺仪或者由三轴MEMS加速度计与三轴磁强计组合而成。

其中当三轴角速度测量仪为微机械陀螺MEMS陀螺仪，包括如下步骤：

步骤1、基于三轴陀螺仪信号，采用四元数或旋转矢量、欧拉角、方向余弦等方法，求得三个姿态角，该类姿态测量模块主要结构如图4所示；

(1)利用等效旋转矢量法计算四元数

a、分别计算时刻T的基础上增加t＝1/3h,t＝2/3h,t＝h(h为时间增量)时的各个角(俯仰角、航向角和横滚角，均通过陀螺仪获得)的增量θ₁，θ₂和θ₃；

b、计算当前姿态角 为当前时刻的俯仰角；

c、按如下方式计算增量四元数q(h)：q(h)＝C，其中

φ₀为初始值；

d、利用计算更新后的四元数Q(T+h),其中表示四元数乘法；Q(T)的初始值为初始时刻的四元数，依据初始时刻的姿态和角速度(陀螺仪)计算出来；

e、令Q(T)＝Q(T+h)，返回第一步

f、计算出转动四元数后，即可以由下面的步骤2计算出相应的姿态角。

步骤2、由转动四元数计算姿态角

姿态就是联系载体坐标系和参考坐标系(地理坐标系)角位置变化的参数。欧拉角法和四元素法是常用的表示转动关系变化的两种方式。地理坐标系的矢量到载体坐标系的矢量之间的转动变化关系可以表示为其中T为方向余弦矩阵：

上式中ψ，θ，γ分别为坐标系姿态变换的航向角、俯仰角、横滚角。两坐标系之间的转动变换关系也可以用四元数表示如下：

由上两式可得姿态角与转动四元数的相互关系：

θ＝arcsin(2(q₁q₃-q₀q₂))

由此计算获得当前时刻T+h的姿态角。

2、基于三轴加速度计和三轴磁强计，利用大地磁场和重力场在地理坐标系和AUV载体平台坐标系之间的方向余弦转换进行绝对角度解算得到三个姿态角，该类姿态测量模块主要结构如图5所示；原理如图7所示，包括如下步骤：

步骤(1)利用三轴加速度计数据求解俯仰角和横滚角；

首先用加速度计分别测量出地球的重力加速度在动坐标系三个坐标轴的分量为f_b＝[f_x ^b f_y ^b f_z ^b]^T。由于航向角的方向与地球重力加速度的方向始终是相互垂直的，所以航向角的改变并不会影响姿态角的数值，这时可以令航向角的大小首先为0°，即ψ＝0，从而简化了方向余弦矩阵以利于计算载体的姿态角。此时姿态预先矩阵如下所示：

θ为俯仰角，横滚角。

最终求得的航向角是以磁地理坐标系为基础的，所以写出磁地理坐标系下重力加速度传感器的输出值f_b＝Cf_n

所以将C带入上式得

由上式可求得俯仰角θ，横滚角分别为：

步骤(2)利用三轴磁强计数据求解航向角(方位角)

空间载体的方位角是利用磁场强度传感器测得的。首先利用磁场强度传感器测量地磁场在载体坐标系上的分量H^b＝[H_x ^b H_y ^b H_z ^b]^T。

当磁地理坐标系与载体坐标系重合的时候，由于磁力线垂直于OE轴(地理东方向)，所以磁场在三个轴上的分量为Hⁿ＝[H_x ⁿ 0 H_z ⁿ]^T，H_x ⁿ为磁地理坐标系与载体坐标系重合时由地磁场在X轴产生的分量，H_z ⁿ为磁地理坐标系与载体重合时由地磁场在载体坐标系Z轴上产生的分量。当载体坐标系发生偏转后，在相应坐标轴所产生的分量H^b为

C为姿态预先矩阵，由于C是正交矩阵有关系式C^T＝C^-1，所以可以得到下面的关系：

故

H_y ⁿ＝cosθH_y ^b-sinθH_z ^b

由上面公式可以得到航向角为ψ＝arctan(H_y ⁿ/H_x ⁿ)

由于正切函数的定义域在(-2/π，2/π)，为了避免出现超出定义域范围，有如下定义：

ψ＝270,(H_x ⁿ＝0,H_y ⁿ＜0)

ψ＝90,(H_x ⁿ＝0,H_y ⁿ＞0)

ψ＝180+arctan(H_y ⁿ/H_x ⁿ)*(180/π),(H_x ⁿ＜0)

ψ＝arctan(H_y ⁿ/H_x ⁿ)*(180/π),(H_x ⁿ＞0,H_y ⁿ＞0)

ψ＝360+arctan(H_y ⁿ/H_x ⁿ)*(180/π),(H_x ⁿ＞0,H_y ⁿ＜0)

因为地理北极与地球磁场北极不相同，用磁偏角α表示地理子午平面与地球表面任意地点地磁强度矢量所在垂直平面夹角，该角可通过以经纬度为参考的查表来得到。所以，载体相对于地理北的航向角可用ψ_M＝ψ±α表示。

3、第三类姿态测量模块结构，主要由三轴MEMS陀螺仪、三轴MEMS加速度计、三轴磁强计和姿态解算微处理单元组成。其中，三轴MEMS陀螺仪输出三个轴的角速度；三轴MEMS加速度计输出三个轴向的加速度；三轴磁强计输出三个轴上的磁场强度；姿态解算微处理单元通过三轴MEMS陀螺的输出信息解算出第一组航向角、俯仰角和横滚角信息，通过三轴MEMS加速度计和三轴磁强计的输出信息解算出第二组航向角、俯仰角和横滚角信息，然后利用专家系统、卡尔曼滤波、神经网络等手段对两组航向角、俯仰角和横滚角进行信息融合，得到更为精确的姿态信息。

本实施例中，要保证水平舵姿态测量模块设置于AUV中的水平舵板的质心位置、且其三轴与水平舵板的三轴重合，应当采用如下安装方式：水平舵板在AUV载体平台上安装时，需保证水平舵板轴向与AUV载体平台右向平行。水平舵姿态测量模块的北向与AUV载体平台坐标系的前向一致，姿态测量模块的天向与AUV平台坐标系的上向一致；姿态测量模块的东向与AUV平台坐标系的右向一致。则水平舵只能在平行于AUV载体平台前向和上向的平面内以AUV载体平台右向为轴进行旋转运动。即操控水平舵时，水平舵板所在的平面相对AUV载体平台而言，只存在俯仰方向的相对运动。AUV载体平台与水平舵间的安装方式及其相对位置关系和相对运动方式如图8所示。

要保证垂直舵姿态测量模块设置于AUV中的垂直舵板的质心位置、且其三轴与垂直舵板的三轴重合，具体可采用如下安装方式：垂直舵板在AUV载体平台上安装时，需保证垂直舵板轴向与AUV载体平台上向平行；垂直舵中姿态测量模块的北向与AUV载体平台坐标系的前向一致，姿态测量模块的天向与AUV平台坐标系的上向一致，姿态测量模块的东向与AUV平台坐标系的右向一致。则垂直舵只能在平行于AUV载体平台前向和右向的平面内以AUV载体平台上向为轴进行旋转运动。即操控垂直舵时，垂直舵板所在的平面相对AUV载体平台而言，只存在航向方向的相对运动。AUV载体平台与垂直舵间的安装方式及其相对位置关系和相对运动方式如图9所示。

综上，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种无需调零的AUV舵角精确实时测量系统，其特征在于，包括AUV惯性导航系统、垂直舵姿态测量模块、水平舵姿态测量模块和AUV舵角解算模块；

所述AUV惯性导航系统包含惯性导航器件和导航计算机，其中惯性导航器件设置于AUV载体平台的质心位置处、且其三轴与载体平台三轴重合，其测量获得的AUV载体平台姿态信息经所述导航计算机解算得到AUV载体平台在地理坐标系下的姿态信息；

所述垂直舵姿态测量模块设置于AUV中的垂直舵板的质心位置、且其三轴与垂直舵板的三轴重合；

所述水平舵姿态测量模块设置于AUV中的水平舵板的质心位置、且其三轴与水平舵板的三轴重合；

所述AUV舵角解算模块通过有线或无线通信接口实时接收AUV惯性导航系统、垂直舵姿态测量模块及水平舵姿态测量模块的测量信息；通过垂直舵姿态测量模块的测量信息，解算出垂直舵板在地理坐标系下的姿态信息；通过水平舵姿态测量模块的测量信息，解算出水平舵板在地理坐标系下的姿态信息；结合AUV载体平台、垂直舵板、水平舵板的姿态信息和垂直舵板、水平舵板与AUV的相对运动关系，解算出垂直舵角和水平舵角。

2.根据权利要求1所述的一种无需调零的AUV舵角精确实时测量系统，其特征在于：所述姿态信息包括航向角、俯仰角和横滚角。

3.根据权利要求1所述的一种无需调零的AUV舵角精确实时测量系统，其特征在于，所述垂直舵姿态测量模块和水平舵姿态测量模块均为三轴角速度测量仪；

三轴角速度测量仪为微机械陀螺MEMS陀螺仪，

或者三轴角速度测量仪由三轴MEMS加速度计与三轴磁强计组合而成。

4.根据权利要求1所述的一种无需调零的AUV舵角精确实时测量系统，其特征在于，所述水平舵板在AUV载体平台上安装时，水平舵板轴向与AUV载体平台右向平行，水平舵姿态测量模块的北向与AUV载体平台坐标系的前向一致，水平舵姿态测量模块的天向与AUV平台坐标系的上向一致，水平舵姿态测量模块的东向与AUV平台坐标系的右向一致；

垂直舵板在AUV载体平台上安装时，垂直舵板轴向与AUV载体平台上向平行；垂直舵姿态测量模块的北向与AUV载体平台坐标系的前向一致，垂直舵姿态测量模块的天向与AUV平台坐标系的上向一致，垂直舵姿态测量模块的东向与AUV平台坐标系的右向一致。