CN102087110B

CN102087110B - 微型水下运动体自主姿态检测装置及方法

Info

Publication number: CN102087110B
Application number: CN2010106122216A
Authority: CN
Inventors: 赵新华; 梁锋; 孙尧; 兰贺; 安伟光
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2010-12-29
Filing date: 2010-12-29
Publication date: 2012-08-22
Anticipated expiration: 2030-12-29
Also published as: CN102087110A

Abstract

本发明提供的是一种微型水下运动体自主姿态检测装置该装置。由IMU惯性测量单元模块(1)、运动控制执行部件(2)、运动学模型估计(3)、动态测量数据补偿(4)、水下导航计算机(5)和串口输出(6)组成；IMU惯性测量单元模块(1)、动态测量数据补偿(4)、水下导航计算机(5)和串口输出(6)依此相连，运动控制执行部件(2)连接运动学模型估计(3)，运动学模型估计(3)连接动态测量数据补偿(4)。本发明特别适用于水下环境以及对自主性姿态测量要求较高的环境。具有体积小、重量轻、精度高、隐蔽性强、使用环境广泛等特点。

Description

微型水下运动体自主姿态检测装置及方法

技术领域

本发明涉及一种姿态检测装置，具体地说是一种微型水下运动体自主姿态检测装置。本发明还涉及一种基于此装置的姿态检测方法。

背景技术

运动载体的姿态测量是进行控制和导航的基础。水下运动体所处的特殊环境使其与外界信息相对封闭，因此需要姿态测量系统具有较高的自主性。传统的姿态测量方法主要有惯性测量、卫星无线电测量以及地磁测量等。由于水下运动体所处的环境限制，对卫星和无线电信号具有极大的屏蔽作用，在水下环境很难使用这种测量方法；利用地磁进行姿态检测主要对载体坐标系下的地磁场进行测量，经过换算可以得出运动载体的姿态信息，但地磁模型仅仅是对地球磁场的近似描述，而且运动体的金属外壳对磁场具有一定的屏蔽作用，载体内部的电气机构也会对磁场造成一定的干扰，所以利用地磁场测量运动体的姿态具有很大的误差，常用于和其他方法做组合测量。

惯性测量装置主要有陀螺仪和加速度计两种，按照结构的不同又可以分为平台式和捷联式两种形式，平台式惯性测量系统主要依靠转子陀螺的定轴性和进动性跟踪运动体的导航坐标系，但是平台式惯性测量系统结构复杂，制造成本高，体积大，质量重，严重限制了它的使用范围。捷联式惯性测量装置使用为速率陀螺，速率陀螺成本低、体积小、质量轻，但是精度较低，漂移严重，因此也不能单独用来进行姿态角的测量。

另一种惯性测量方法是利用加速度计测量重力在运动体坐标系下各轴的分量进行解算，但由于加速度计同时也敏感载体运动的加速度，因此容易受到运动加速度干扰，不能在非平衡状态下使用。

发明内容

本发明的目的在于提供一种适用于水下环境以及对自主性姿态测量要求较高的环境的微型水下运动体自主姿态检测装置。本发明的目的还在于提供一种基于本发明的微型水下运动体自主姿态检测装置的检测方法。

本发明的微型水下运动体自主姿态检测装置该装置由IMU惯性测量单元模块1、运动控制执行部件2、运动学模型估计3、动态测量数据补偿4、水下导航计算机5和串口输出6组成；IMU惯性测量单元模块1、动态测量数据补偿4、水下导航计算机5和串口输出6依此相连，运动控制执行部件2连接运动学模型估计3，运动学模型估计3连接动态测量数据补偿4。

所述IMU惯性测量单元模块1包括三轴加速度计、三轴陀螺仪、三轴温度计以及数字信号输出电路。

所述运动控制执行部件2为运动体上的所有的控制执行机构。

所述运动学模型估计3对运动体控制执行部件进行精确的动力学描述。

所述动态测量数据补偿4输出的数据直接输入导航计算机5进行姿态的解算，并通过串口输出。

基于微型水下运动体自主姿态检测装置的检测方法为：

所用微型水下运动体自主姿态检测装置由IMU惯性测量单元模块1、运动控制执行部件2、运动学模型估计3、动态测量数据补偿4、水下导航计算机5和串口输出6组成；IMU惯性测量单元模块1、动态测量数据补偿4、水下导航计算机5和串口输出6依此相连，运动控制执行部件2连接运动学模型估计3，运动学模型估计3连接动态测量数据补偿4；

测量方法包括如下步骤：

第一步：将IMU惯性测量单元模块1按照运动体坐标系方向安装固定，按照航向、俯仰、横滚的顺序构造导航坐标系到运动体坐标系的方向余弦矩阵；

第二步：在运动体稳定环境下设定运动体的初始状态，航向角由上位机给定，俯仰角和横滚角通过三轴加速度计的输出进行解算；

第三步：在运动体运动过程中，根据运动体的运动控制执行部件2的输出信息，经过动力学模型估计3，估计运动体在载体坐标系下的三轴加速度；

第四步：IMU惯性测量单元1的测量数据和第三步的输出数据进行动态测量数据补偿4，去除运动控制中产生的加速度，以及惯性测量单元1测量数据的温度漂移；

第五步：在导航计算机5中对第四步输出的数据进行滤波，分别利用补偿、滤波后的加速度计数据、角速度数据进行姿态解算，并进行组合分析，给出运动体姿态的最优估计，通过串口输出。

进行下一个采样周期的姿态测量。

本发明实施时IMU惯性测量单元模块1选用ADIS16355，水下导航计算机需用Cyclone系列FPGA芯片EP1C12Q240I7，运动控制执行部件2即为水下运动体的推力发动机，反馈给动力学模型估计3的是运动体对发动机的控制参数，动力学模型估计3是水下运动载体的力学模型，由于动态测量数据补偿4是算法部分，编程后下载到导航计算机中进行。水下运动体在发射前保持稳定状态，只需上位机给出水下运动体初始航向角，发射出去后微型水下运动体姿态检测装置即可按说明书中算法实时给出水下运动体姿态信息。

该装置体积小、重量轻，于其他惯性导航装置相比，多了一种动态姿态测量方法，增强惯性姿态测量的精度，具有高度自主性，因此特别适用于水下环境以及对自主性姿态测量要求较高的环境。

本发明的水下运动体自主姿态检测装置，具有体积小、重量轻、精度高、隐蔽性强、使用环境广泛等特点。工作时通过对运动控制执行部件的输出进行动力学模型估计，对惯性测量单元的动态测量数据进行补偿，将补偿后的测量数据送入导航计算机进行姿态解算，得出运动体的实时姿态信息，通过串口输出。本发明特别适用于水下环境以及对自主性姿态测量要求较高的环境。

附图说明

图1是微型水下运动体自主姿态检测装置框图。

图2是运动载体受力分析和坐标系。

图3是导航坐标系和载体坐标系的转换。

具体实施方式

下面结合附图举例对本发明做更详细的描述：

结合图1，本发明的微型水下运动体自主姿态检测装置包括IMU惯性测量单元1，运动控制执行部件2，动力学模型估计3，动态测量数据补偿4，导航计算机5，串口输出6，以及相应软件测量方法。

IMU惯性测量单元模块1包括三轴加速度计、三轴陀螺仪、三轴温度计以及配套数字信号输出电路，器件坐标要与运动体坐标对齐，并采用模块化器件便于安装调试。

运动控制执行部件2包含运动体所有的控制执行机构。

运动学模型估计3能够对运动体控制执行部件进行较精确的动力学描述。

动态测量数据补偿4输出的数据直接输入导航计算机5进行姿态的解算，并通过串口输出。

本装置测量步骤如下：

第一步：IMU惯性测量单元1按照运动载体坐标系方向安装，测量载体各轴加速度A和角速度ω分量以及温度信息T。

第二步：载体运动过程中，载体的运动控制执行部件2反馈控制的执行参数ε(比如发动机推动时间)。

第三步：动力学模型估计3根据运动控制执行部件2的反馈参数，对载体运动变化ΔA＝f(ε)+hf进行估计，f(ε)为发动机推力，hf为流体阻力，运动体受力分析和坐标见图2。

第四步：IMU惯性测量单元1的测量数据(A，ω)和第3步的输出数据(ΔA，0)进行动态测量数据补偿4，

去除运动控制中产生的加速度。

第五步：在导航计算机5中对第四步输出的数据进行滤波、温度补偿，分别利用补偿、滤波后的加速度计数据、角速度数据进行姿态解算。

加速度计测姿态解算按照导航坐标系到载体坐标系的转换矩阵计算，坐标转换过程见图3。

C_{p}^{b} = [\begin{matrix} \cos ψ \sin θ & \sin ψ \cos θ & - \sin θ \\ \cos ψ \sin θ \sin γ - \sin ψ \cos γ & \sin ψ \sin θ \sin γ + \cos ψ \cos γ & \cos θ \sin γ \\ \cos ψ \sin θ \cos γ + \sin ψ \sin γ & \sin ψ \sin θ \cos γ - \cos ψ \sin γ & \cos θ \cos γ \end{matrix}] = T^{T}

式中ψ为航向角，θ为俯仰角，γ为横滚角，由于载体运动加速度通过步骤四的补偿，基本消除其对加速度姿态的影响，以

作为姿态角测量方差。另外重力加速度在航向角上没有分量，加速度计姿态测量不包括航向角。

角速度姿态测量利用速率陀螺测量的加速度采用四元数法解算。

令

则载体坐标到导航坐标的转换可以表示为：

T = [\begin{matrix} q_{1}^{2} + q_{0}^{2} - q_{3}^{2} - q_{2}^{2} & 2 (q_{1} q_{2} + q_{0} q_{3}) & 2 (q_{1} q_{3} - q_{0} q_{2}) \\ 2 (q_{1} q_{2} - q_{0} q_{3}) & q_{2}^{2} + q_{0}^{2} - q_{3}^{2} - q_{1}^{2} & 2 (q_{2} q_{3} + q_{0} q_{1}) \\ 2 (q_{1} q_{3} + q_{0} q_{2}) & 2 (q_{2} q_{3} - q_{0} q_{1}) & q_{3}^{2} + q_{0}^{2} - q_{1}^{2} - q_{2}^{2} \end{matrix}]

且有

q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2} = 1;

根据捷联矩阵T和

的关系，以稳态时的加速度计姿态测量给出的俯仰角和横滚角，以及上位机给出的航向角作为初始状态，可以求出四元数的初始状态Q(0)。四元数的微分方程为

利用速率陀螺的输出ω，用4阶龙格库塔法动态求解Q(t)，通过捷联矩阵T实时解算运动载体姿态。

姿态角的取值范围为：ψ∈[0°，360°]，θ∈[-90°，90°]，γ∈[-180°，180°]，通过适当的换算可以将反三角函数求出的角度值范围进行规整。

最后利用联邦卡尔曼滤波器将所测的两组姿态角进行组合，得出最优估计姿态角度信息，通过串口输出。

Claims

1.一种微型水下运动体自主姿态检测装置，其特征是：由IMU惯性测量单元模块(1)、运动控制执行部件(2)、运动学模型估计(3)、动态测量数据补偿(4)、水下导航计算机(5)和串口输出(6)组成；IMU惯性测量单元模块(1)、动态测量数据补偿(4)、水下导航计算机(5)和串口输出(6)依次相连，运动控制执行部件(2)连接运动学模型估计(3)，运动学模型估计(3)连接动态测量数据补偿(4)；所述运动控制执行部件(2)为发动机；运动学模型估计(3)根据运动控制执行部件(2)的反馈参数，对载体运动的加速度变化ΔA＝(f(ε)+hf)/m进行估计，f(ε)为发动机推力，hf为流体阻力，m为载体质量；IMU惯性测量单元模块(1)的测量数据A与ω和对载体运动变化进行估计的输出数据ΔA与0进行动态测量数据补偿(4)，

A为加速度、ω为角速度、

为加速度估计值、

为角速度估计值，去除运动控制中产生的加速度。

2.根据权利要求1所述的微型水下运动体自主姿态检测装置，其特征是：所述IMU惯性测量单元模块(1)包括三轴加速度计、三轴陀螺仪、三轴温度计以及数字信号输出电路。

3.根据权利要求1或2所述的微型水下运动体自主姿态检测装置，其特征是：所述动态测量数据补偿(4)输出的数据直接输入水下导航计算机(5)进行姿态的解算，并通过串口输出。

4.一种基于微型水下运动体自主姿态检测装置的检测方法，其特征是：

所用微型水下运动体自主姿态检测装置由IMU惯性测量单元模块(1)、运动控制执行部件(2)、运动学模型估计(3)、动态测量数据补偿(4)、水下导航计算机(5)和串口输出(6)组成；IMU惯性测量单元模块(1)、动态测量数据补偿(4)、水下导航计算机(5)和串口输出(6)依次相连，运动控制执行部件(2)连接运动学模型估计(3)，运动学模型估计(3)连接动态测量数据补偿(4)；

检测方法包括如下步骤：

第一步：将IMU惯性测量单元模块(1)按照运动体坐标系方向安装固定，按照航向、俯仰、横滚的顺序构造导航坐标系到运动体坐标系的方向余弦矩阵；

第三步：在运动体运动过程中，根据运动体的运动控制执行部件(2)的输出信息，经过运动学模型估计(3)，估计运动体在载体坐标系下的三轴加速度；所述运动控制执行部件(2)为发动机；运动学模型估计(3)根据运动控制执行部件(2)的反馈参数，对载体运动的加速度变化ΔA＝(f(ε)+hf)/m进行估计，f(ε)为发动机推力，hf为流体阻力，m为载体质量；

第四步：IMU惯性测量单元模块(1)的测量数据A与ω和第三步的输出数据ΔA与0进行动态测量数据补偿(4)， A为加速度、ω为角速度、

为加速度估计值、

为角速度估计值，去除运动控制中产生的加速度，以及惯性测量单元模块(1)测量数据的温度漂移；

第五步：在水下导航计算机(5)中对第四步输出的数据进行滤波；分别利用补偿、滤波后的加速度计数据、角速度数据进行姿态解算，并进行组合分析，给出运动体姿态的最优估计；最后通过串口输出。