CN115060274B - 一种水下一体式自主导航装置及其初始对准方法 - Google Patents
一种水下一体式自主导航装置及其初始对准方法 Download PDFInfo
- Publication number
- CN115060274B CN115060274B CN202210984493.1A CN202210984493A CN115060274B CN 115060274 B CN115060274 B CN 115060274B CN 202210984493 A CN202210984493 A CN 202210984493A CN 115060274 B CN115060274 B CN 115060274B
- Authority
- CN
- China
- Prior art keywords
- navigation
- measurement unit
- micro
- inertial measurement
- electromechanical
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 17
- 238000005259 measurement Methods 0.000 claims abstract description 146
- 238000004364 calculation method Methods 0.000 claims abstract description 33
- 238000006243 chemical reaction Methods 0.000 claims abstract description 32
- 238000013500 data storage Methods 0.000 claims abstract description 15
- 239000011159 matrix material Substances 0.000 claims description 49
- 238000001914 filtration Methods 0.000 claims description 21
- 238000007789 sealing Methods 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 13
- 230000033001 locomotion Effects 0.000 claims description 12
- 238000009434 installation Methods 0.000 claims description 11
- 239000013598 vector Substances 0.000 claims description 11
- 230000007704 transition Effects 0.000 claims description 9
- 230000005540 biological transmission Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000004891 communication Methods 0.000 claims description 3
- 238000003780 insertion Methods 0.000 claims 2
- 230000037431 insertion Effects 0.000 claims 2
- 230000008878 coupling Effects 0.000 description 8
- 238000010168 coupling process Methods 0.000 description 8
- 238000005859 coupling reaction Methods 0.000 description 8
- 238000013461 design Methods 0.000 description 5
- 238000012423 maintenance Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 239000007788 liquid Substances 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 239000000969 carrier Substances 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000005611 electricity Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000036544 posture Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
本发明涉及水下导航定位技术领域,公开了一种水下一体式自主导航装置,包括导航计算模块、信号转换模块、数据存储模块、微机电惯性测量单元、多普勒计程仪和电压转换模块,将微机电惯性测量单元和多普勒计程仪一体化设计,实现水下没有GPS、北斗等信号的环境下,不依赖任何外部信息,对水下蛙人、小型运载器的导航定位,能够满足水下蛙人导航的自主性强、体积小、功耗低、成本低廉、使用方便的特点。本发明还公开了一种导航装置初始对准方法,用于实现导航装置的初始对准。
Description
技术领域
本发明涉及水下导航定位技术领域,尤其涉及一种水下一体式自主导航装置及其初始对准方法。
背景技术
由于水下环境的特殊性和复杂性,水下光线不足,无线电信号衰减迅速,因此通常的视觉、GNSS卫星导航(包括GPS、北斗等)无法对水下目标进行导航、定位。
水下蛙人在水下救援、打捞、清淤、摄影、检修等作业时,一般通过蛙人运载器辅助其航行至指定地点进行作业,因此需要一种能类似GPS的水下导航定位装置,并且要求体积小、功耗低、实用性强等。这样尽量避免需要频繁上浮到水面接收GPS信号,提高水下工作效率。
现有的水下导航常用的方法有:
(1)水声定位,在任务区预先布置一个或多个声呐基站,与GNSS的原理相似,利用目标接收基站声呐信号来确定相对位置和方向。
(2)地球物理导航,利用预先储存的水下地形、磁场或者重力场背景图与当前测量值进行匹配,从而确定当前时刻的位置坐标。
然而,无论是水声定位还是地球物理导航方法,都需要先验的条件,比如水声定位***需要在水面或者海底布置应答器,地球物理导航需要先验的地图。此外,在使用先验的背景图进行导航时,需要配备对应的传感器实时测量磁场、重力、地形和水深等参数,这些传感器体积较大、价格昂贵,这对于水下蛙人使用极其不便。
水下蛙人执行任务更多的场合是陌生的领域,不能事先布置应答器或其它声学基站,也没有地图。因此,需要一种全自主的导航定位装置,不依赖外部信息,可以实时导航、定位。
惯性导航(INS)依靠其内部的惯性器件(陀螺测量角运动,加速度计测量线运动),可以独立、自主地提供所有的导航信息,包括姿态、速度和位置等,具有可靠性高,不需要与外界有任何声、光、电的联系,基本不受外界干扰,短时间内精度高等优点。然而,INS的误差会随着时间积累,误差发散快。但是多普勒计程仪(Doppler Velocity Log)可以测量运动的速度,并且误差不发散,因此,可以将INS与DVL进行组合导航,可以克服误差快速发散的缺点。
目前的INS与DVL组合导航装置中的INS一般采用能完成初始自对准的高精度光纤陀螺、甚至激光陀螺,这对于大型潜器,比如潜艇和大型水下机器人(AUV或ROV)等可以接受,但对于蛙人运载器,其体积大、功耗高且成本昂贵。
发明内容
本发明所要解决的技术问题是克服现有技术中存在的不足,提供一种水下一体式自主导航装置及其初始对准方法。
本发明是通过以下技术方案予以实现:
一种水下一体式自主导航装置,包括导航计算模块、信号转换模块、数据存储模块、微机电惯性测量单元和多普勒计程仪,其中,所述微机电惯性测量单元用于采集蛙人运载器的角速度、加速度数据;所述多普勒计程仪用于采集蛙人运载器的运动速度数据;所述信号转换模块分别与微机电惯性测量单元、多普勒计程仪和导航计算模块相连接,用于接收微机电惯性测量单元和多普勒计程仪采集的数据并传输至导航计算模块;所述信号转换模块还用于接收母船导航***的位置、速度和方位角数据并传输至导航计算模块;所述导航计算模块用于处理母船导航***提供的位置、速度和方位角数据,以对微机电惯性测量单元进行初始对准;所述导航计算模块还用于处理微机电惯性测量单元和多普勒计程仪采集的数据,以得到优化的导航结果;所述信号转换模块还用于接收导航计算模块计算的导航结果并输出至蛙人运载器;所述数据存储模块与导航计算模块相连接,用于存储微机电惯性测量单元采集的角速度和加速度数据,多普勒计程仪采集的运动速度数据,导航计算模块计算的导航结果以及信号转换模块接收到的母船导航***位置、速度和方位角数据。
优选地,所述导航装置还包括顶端开口的密封舱体和扣合至密封舱体顶端的舱盖,所述密封舱体内侧中部凸设有卡阶,所述卡阶固接有安装板,所述微机电惯性测量单元固接至安装板下方,所述安装板上方固接有数据存储模块、导航计算模块和信号转换模块,所述多普勒计程仪固接至密封舱体外侧下方,所述舱盖设有第一水密头插接件和第二水密头插接件,所述第一水密头插接件用于供电,所述第二水密头插接件用于数据传输。
优选地,所述舱盖下侧设有与密封舱体顶端开口过盈配合的插接部,所述插接部与密封舱体间设有密封圈,所述舱盖表面还设有泄压孔,所述泄压孔内设有相适配的气密塞。
优选地,所述多普勒计程仪上侧面通过第一螺栓固接有安装底板,所述安装底板设有凸出多普勒计程仪上侧面外周的外延部,所述密封舱体的底端设有与安装底板相适配的凹槽,所述安装底板嵌设于凹槽内,所述安装底板的外延部通过第二螺栓固接至凹槽底面。
优选地,所述第二螺栓在凹槽底面相对应的螺栓孔为盲孔。
优选地,所述信号转换模块设有以太网输出接口、RS422输出接口和CAN输出接口。
优选地,所述数据存储模块设有USB输出接口。
本发明还公开了一种导航装置初始对准方法,用于上述水下一体式自主导航装置,包括如下步骤:
S10,将导航装置固定到母船,使导航装置与母船导航***建立通讯连接;
S20,导航计算模块获取母船导航***的位置数据、速度数据装订至导航装置;
S30,导航计算模块获取母船导航***的方位角数据,微机电惯性测量单元采集的角速度、加速度数据,多普勒计程仪采集的运动速度数据,结合母船导航***的位置数据、速度数据,通过卡尔曼滤波算法得出导航装置的姿态误差角、微机电惯性测量单元的陀螺仪常值误差和微机电惯性测量单元的加速度计常值误差。
优选地,所述S30步骤中,卡尔曼滤波算法的状态向量X的表达式如下:
其中,表示微机电惯性测量单元在东向的姿态误差角,表示微机电惯性测量
单元在北向的姿态误差角,表示微机电惯性测量单元在天向的姿态误差角,表示微
机电惯性测量单元在东向的速度误差,表示微机电惯性测量单元在北向的速度误差,表示微机电惯性测量单元在天向的速度误差,表示微机电惯性测量单元纬度的位置
误差,表示微机电惯性测量单元经度的位置误差,表示微机电惯性测量单元高度的
位置误差,表示微机电惯性测量单元的陀螺仪x轴的零偏值,表示微机电惯性测量单元
的陀螺仪y轴的零偏值,表示微机电惯性测量单元的陀螺仪z轴的零偏值,表示微机电
惯性测量单元的加速度计x轴的零偏值,表示微机电惯性测量单元的加速度计y轴的零
偏值,表示微机电惯性测量单元的加速度计z轴的零偏值,表示导航装置与母船之间
的安装误差角,
卡尔曼滤波算法的量测向量Z的表达式如下:
优选地,所述S30步骤中,卡尔曼滤波算法的状态方程为:
状态转移矩阵F的表达式如下,
其中,
其中,矩阵为状态转移矩阵F的子块,矩阵均为矩阵的子块,矩阵均为矩阵的子块,为的转置矩阵,为载体坐标系与导航
坐标系之间的姿态变换矩阵,通过粗对准得到,为地球自转角速度,为地球卯酉圈主
曲率半径,为地球子午圈主曲率半径,表示由微机电惯性测量单元测得的母船在导
航坐标系相对于惯性坐标系的旋转角速度在地球坐标系的投影,表示由微机电惯性测
量单元测得的母船在导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系的投影,表示由微机电惯性测量单元测得的母船在导航坐标系相对于地球坐标系的旋转角速度
在导航坐标系的投影,为微机电惯性测量单元的加速度计所测量的比力值,表示微机
电惯性测量单元在导航坐标系下测量得到的速度值,表示微机电惯性测量单元在导航坐
标系下测量得到的东向速度值,表示微机电惯性测量单元在导航坐标系下测量得到的北
向速度值,
卡尔曼滤波算法的量测方程为:
其中,H为量测矩阵,V为量测噪声,
量测矩阵H的表达式如下,
本发明的有益效果是:
本发明针对水下蛙人导航定位的特点,以低成本、微小型的微机电惯性测量单元MINS与多普勒计程仪DVL一体式设计,并对两者的信息进行融合,实现水下没有GPS、北斗等信号的环境下,不依赖任何外部信息,对水下蛙人、小型运载器的导航定位,能够满足水下蛙人导航的自主性强、体积小、功耗低、成本低廉、使用方便的特点;信号转换模块能够通过多种方式将导航结果输出至蛙人运载器,使导航装置具有较强的通用性;通过一体化设计,可以有效降低导航装置的整体体积,适应蛙人运载器、小型机器人狭小安装空间;密封圈及过盈配合的方式能够提高舱盖与密封舱体间的密封性,泄压孔的设置便于舱盖的安装或者拆卸,在安装好舱盖后,气密塞用于对泄压孔进行封堵;安装底板的设置便于DVL的前期装配及后期维护,凹槽的设置使得DVL与密封舱体的结构更加紧凑;盲孔,能够完全避免导航装置外部的液体从凹槽底面的螺栓孔位置渗入至密封舱体内部,对密封舱体内部的各个模块起到保护作用;导航装置初始对准方法通过母船GNSS辅助导航装置完成方位初始对准,在传递过程中,母船GNSS的位置数据、速度数据直接装订至导航装置,姿态误差角、MINS陀螺仪常值误差及MINS加速度计常值误差则通过卡尔曼滤波得出,进而实现导航装置的方位初始化参数装订,使其可以进入下一阶段的导航。
附图说明
图1是本发明提供的水下一体式自主导航装置的***结构示意图;
图2是本发明所述密封舱体的底部结构示意图;
图3是本发明所述密封舱体的内部结构示意图;
图4是本发明提供的水下一体式自主导航装置的模块框图;
图5是本发明提供的水下一体式自主导航装置的信号流程图。
图中:1、第一水密头插接件;2、气密塞;3、泄压孔;4、舱盖;5、密封舱体;6、安装底板;7、外延部;8、多普勒计程仪;9、第二水密头插接件;10、凹槽;11、盲孔;12、舱盖安装孔;13、舱体螺纹孔;14、密封圈;15、安装板;16、卡阶;17、微机电惯性测量单元;18、插接部;19、数据存储模块;20、电压转换模块;21、导航计算模块;22、信号转换模块。
具体实施方式
为了使本技术领域的技术人员更好地理解本发明的技术方案,下面结合附图和最佳实施例对本发明作进一步的详细说明。
在本发明的描述中,需要理解的是,术语“上”、“下”、“前”、“后”、“左”、“右”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
如图1-图5所示,本发明提供了一种水下一体式自主导航装置,包括导航计算模块21、信号转换模块22、数据存储模块19、微机电惯性测量单元17、多普勒计程仪8和电压转换模块20,所述微机电惯性测量单元(MEMS strapdown inertial system)17简称MINS,所述多普勒计程仪(Doppler velocity log)8简称DVL,其中,所述电压转换模块20用于将***电源电压转换为其余各模块所需电压并进行供电;所述微机电惯性测量单元17用于采集蛙人运载器的角速度、加速度数据;所述多普勒计程仪8用于采集蛙人运载器的运动速度数据;所述信号转换模块22分别与微机电惯性测量单元17、多普勒计程仪8和导航计算模块21相连接,用于接收微机电惯性测量单元17和多普勒计程仪8采集的数据并传输至导航计算模块21;所述信号转换模块22还用于接收母船导航***的位置、速度和方位角数据并传输至导航计算模块21,所述母船导航***即为母船GNSS***;所述导航计算模块21用于处理母船导航***提供的位置、速度和方位角数据,以对微机电惯性测量单元17进行初始对准;所述导航计算模块21还用于处理微机电惯性测量单元17和多普勒计程仪8采集的数据,以得到优化的导航结果,具体流程如图5所示,卡尔曼滤波算法对来自MINS和DVL的数据进行相关误差参数的最优估计,然后将其输出的结果对MINS原始输出的导航参数进行修正,从而得到优化的导航结果;所述信号转换模块22还用于接收导航计算模块21计算的导航结果并输出至蛙人运载器;所述数据存储模块19与导航计算模块21相连接,用于存储微机电惯性测量单元17采集的角速度和加速度数据,多普勒计程仪8采集的运动速度数据,导航计算模块21计算的导航结果以及信号转换模块22接收到的母船导航***位置、速度和方位角数据,针对水下蛙人导航定位的特点,以低成本、微小型的微机电惯性测量单元17与多普勒计程仪8一体式设计,并对两者的信息进行融合,能够满足水下蛙人导航的自主性强、体积小、功耗低、成本低廉、使用方便的要求。
本发明提供的一种水下一体式自主导航装置还包括顶端开口的密封舱体5和扣合至密封舱体5顶端的舱盖4,所述密封舱体5整体呈柱形,所述密封舱体5内侧中部凸设有卡阶16,所述卡阶16固接有安装板15,所述微机电惯性测量单元17固接至安装板15下方,所述安装板15上方固接有数据存储模块19、电压转换模块20、导航计算模块21和信号转换模块22,所述多普勒计程仪8固接至密封舱体5外侧下方,所述舱盖设有第一水密头插接件1和第二水密头插接件9,所述第一水密头插接件1用于供电,所述第二水密头插接件9用于数据传输,所述第一水密头插接件1和第二水密头插接件9均可设置多个,在本实施例中,所述第一水密头插接件1设有一个,所述第二水密头插接件9设有两个,通过一体化设计,可以有效降低导航装置的整体体积,适应蛙人运载器、小型机器人狭小安装空间。
所述舱盖下侧设有与密封舱体5顶端开口过盈配合的插接部18,所述舱盖在插接部18外周设有多个等角度分布的舱盖安装孔12,所述密封舱体5顶端设有与舱盖安装孔12相适配的舱体螺纹孔13,将安装螺栓穿过舱盖安装孔12并旋紧至舱体螺纹孔13即可将舱盖固定安装至密封舱体5,所述插接部18与密封舱体5间设有密封圈14,所述舱盖表面还设有泄压孔3,所述泄压孔3内设有相适配的气密塞2,密封圈14及过盈配合的方式能够提高舱盖与密封舱体5间的密封性,在舱盖的安装或者拆卸过程中,泄压孔3的设置使得密封舱体5内的气压与密封舱体5外的气压保持一致,便于舱盖的安装或者拆卸,在安装好舱盖后,气密塞2用于对泄压孔3进行封堵。
所述DVL上侧面通过第一螺栓固接有安装底板6,所述安装底板6设有凸出DVL上侧面外周的外延部7,所述密封舱体5的底端设有与安装底板6相适配的凹槽10,所述安装底板6嵌设于凹槽10内,所述安装底板6的外延部7通过第二螺栓固接至凹槽10底面,由于DVL的螺栓孔一般设置在其上侧面,若直接通过DVL自带的螺栓孔将其安装至密封舱体5,则需要在密封舱体5底部开设通孔,并且从密封舱体5内部将螺栓穿过通孔对DVL进行固定,不便于对DVL进行安装,若DVL出现故障,也不便于对DVL进行拆卸与更换,安装底板6的设置使工作人员能够在密封舱体5的外部直接完成对DVL的固定,便于DVL的前期装配及后期维护,凹槽10的设置使得DVL与密封舱体5的结构更加紧凑。
所述第二螺栓在凹槽10底面相对应的螺栓孔为盲孔11,能够完全避免导航装置外部的液体从凹槽10底面的螺栓孔位置渗入至密封舱体5内部,对密封舱体5内部的各个模块起到保护作用。
所述信号转换模块22设有以太网输出接口、RS422输出接口和CAN输出接口,信号转换模块22能够通过多种方式将导航结果输出至蛙人运载器,使导航装置具有较强的通用性。
所述数据存储模块19设有USB输出接口,便于工作人员读取数据存储模块19内存储的数据。
本发明还公开了一种导航装置初始对准方法,用于上述水下一体式自主导航装置,包括如下步骤:
S10,将导航装置固定到母船,使导航装置与母船导航***建立通讯连接;
S20,导航计算模块获取母船导航***的位置数据、速度数据装订至导航装置;
S30,导航计算模块获取母船导航***的方位角数据,微机电惯性测量单元采集的角速度、加速度数据,多普勒计程仪采集的运动速度数据,结合母船导航***的位置数据、速度数据,通过卡尔曼滤波算法得出导航装置的姿态误差角、微机电惯性测量单元的陀螺仪常值误差和微机电惯性测量单元的加速度计常值误差,本技术方案通过母船导航***辅助导航装置完成方位初始对准,在传递过程中,母船GNSS的位置数据、速度数据直接装订至导航装置,姿态误差角、MINS陀螺仪常值误差及MINS加速度计常值误差则通过卡尔曼滤波得出,进而实现导航装置的方位初始化参数装订,使其可以进入下一阶段的导航,即对导航装置完成初始对准后,再将其从母船拆卸并安装至蛙人运载器,从而实现蛙人运载器的导航工作。
本技术方案中,载体坐标系(b系)采用右前上坐标系,即以导航装置的质心为原点,导航装置前进方向的右侧方向为x轴,导航装置的前进方向为y轴,z轴由右手定则确定;
导航坐标系(n系)采用东北天坐标系,即以导航装置的质心为原点,导航装置的地理东向为E轴,导航装置的地理北向为N轴,U轴由右手定则确定。
所述S30步骤中,将MINS的姿态误差角、速度误差、位置误差、陀螺仪零偏值、加速度计零偏值及导航装置的安装误差角作为卡尔曼滤波算法的状态向量X,所述状态向量X的表达式如下:
其中,表示微机电惯性测量单元在东向的姿态误差角,表示微机电惯性测量
单元在北向的姿态误差角,表示微机电惯性测量单元在天向的姿态误差角,表示微
机电惯性测量单元在东向的速度误差,表示微机电惯性测量单元在北向的速度误差,表示微机电惯性测量单元在天向的速度误差,表示微机电惯性测量单元纬度的位置
误差,表示微机电惯性测量单元经度的位置误差,表示微机电惯性测量单元高度的
位置误差,表示微机电惯性测量单元的陀螺仪x轴的零偏值,表示微机电惯性测量单元
的陀螺仪y轴的零偏值,表示微机电惯性测量单元的陀螺仪z轴的零偏值,表示微机电
惯性测量单元的加速度计x轴的零偏值,表示微机电惯性测量单元的加速度计y轴的零
偏值,表示微机电惯性测量单元的加速度计z轴的零偏值,表示导航装置与母船之间
的安装误差角,
将母船GNSS采集的位置、速度和方位角作为卡尔曼滤波算法的量测向量Z,所述量测向量Z的表达式如下:
所述S30步骤中,
卡尔曼滤波算法的姿态角误差方程如下:
其中,为载体坐标系与导航坐标系之间的姿态变换矩阵,通过粗对准得到,即利
用重力加速度和地球自转角速度粗略计算得到姿态变换矩阵,为姿态变换矩阵第行
第列的元素,,,为地球自转角速度,为地球卯酉圈主曲率半径,
为地球子午圈主曲率半径,为微机电惯性测量单元的陀螺仪零偏值转换到导航坐标系下
东向的分量,为微机电惯性测量单元的陀螺仪零偏值转换到导航坐标系下北向的分量,为微机电惯性测量单元的陀螺仪零偏值转换到导航坐标系下天向的分量,为的一
阶微分,为的一阶微分,为的一阶微分,
卡尔曼滤波算法的速度误差方程如下:
其中,
其中,为微机电惯性测量单元的加速度测量值转换到导航坐标系下东向的分
量,为微机电惯性测量单元的加速度测量值转换到导航坐标系下北向的分量,为微机
电惯性测量单元的加速度测量值转换到导航坐标系下天向的分量,为微机电惯性测量
单元的加速度计零偏值转换到导航坐标系下东向的分量,为微机电惯性测量单元的加
速度计零偏值转换到导航坐标系下北向的分量,为微机电惯性测量单元的加速度计零
偏值转换到导航坐标系下天向的分量,为的一阶微分,为的一阶微分,
为的一阶微分,
卡尔曼滤波算法的位置误差方程如下:
将姿态角误差方程、速度误差方程、位置误差方程合并,得到卡尔曼滤波算法的状态方程:
状态转移矩阵F的表达式如下,
其中,
其中,矩阵为状态转移矩阵F的子块,矩阵均为矩阵的子块,矩阵均为矩阵的子块,为的转置矩阵,表示由微机电惯性测
量单元测得的母船在导航坐标系相对于惯性坐标系的旋转角速度在地球坐标系的投影,表示由微机电惯性测量单元测得的母船在导航坐标系相对于惯性坐标系的旋转角速度
在导航坐标系的投影,表示由微机电惯性测量单元测得的母船在导航坐标系相对于地
球坐标系的旋转角速度在导航坐标系的投影,为微机电惯性测量单元的加速度计所测
量的比力值,表示微机电惯性测量单元在导航坐标系下测量得到的速度值,表示微机
电惯性测量单元在导航坐标系下测量得到的东向速度值,表示微机电惯性测量单元在导
航坐标系下测量得到的北向速度值,
卡尔曼滤波算法的位置误差量测方程如下:
其中,表示位置误差向量,表示微机电惯性测量单元测得的高度值,表示
微机电惯性测量单元测得的经度值,表示微机电惯性测量单元测得的纬度值,表示
母船导航***测得的高度值,表示母船导航***测得的经度值,表示母船导航系
统测得的纬度值,
卡尔曼滤波算法的速度误差量测方程如下:
其中,表示速度误差向量,表示微机电惯性测量单元在导航坐标系下测量得
到的天向速度值,表示多普勒计程仪在导航坐标系下东向的速度测量值,表示多普
勒计程仪在导航坐标系下北向的速度测量值,表示多普勒计程仪在导航坐标系下天向
的速度测量值,
将位置误差量测方程与速度误差量测方程合并,得到卡尔曼滤波算法的量测方程:
其中,H为量测矩阵,V为量测噪声,所述量测噪声V根据母船GNSS的测量精度设定,
量测矩阵H的表达式如下,
本发明针对水下蛙人导航定位的特点,以低成本、微小型的微机电惯性测量单元与多普勒计程仪一体式设计,并对两者的信息进行融合,实现水下没有GPS、北斗等信号的环境下,不依赖任何外部信息,对水下蛙人、小型运载器的导航定位,能够满足水下蛙人导航的自主性强、体积小、功耗低、成本低廉、使用方便的特点;信号转换模块能够通过多种方式将导航结果输出至蛙人运载器,使导航装置具有较强的通用性;通过一体化设计,可以有效降低导航装置的整体体积,适应蛙人运载器、小型机器人狭小安装空间;密封圈及过盈配合的方式能够提高舱盖与密封舱体间的密封性,泄压孔的设置便于舱盖的安装或者拆卸,在安装好舱盖后,气密塞用于对泄压孔进行封堵;安装底板的设置便于DVL的前期装配及后期维护,凹槽的设置使得DVL与密封舱体的结构更加紧凑;盲孔能够完全避免导航装置外部的液体从凹槽底面的螺栓孔位置渗入至密封舱体内部,对密封舱体内部的各个模块起到保护作用;导航装置初始对准方法通过母船GNSS辅助导航装置完成方位初始对准,在传递过程中,母船GNSS的位置数据、速度数据直接装订至导航装置,姿态误差角、MINS陀螺仪常值误差及MINS加速度计常值误差则通过卡尔曼滤波得出,进而实现导航装置的方位初始化参数装订,使其可以进入下一阶段的导航。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (9)
1.一种水下一体式自主导航装置,包括导航计算模块,其特征在于,还包括信号转换模块、数据存储模块、微机电惯性测量单元和多普勒计程仪,其中,
所述微机电惯性测量单元用于采集蛙人运载器的角速度、加速度数据;
所述多普勒计程仪用于采集蛙人运载器的运动速度数据;
所述信号转换模块分别与微机电惯性测量单元、多普勒计程仪和导航计算模块相连接,用于接收微机电惯性测量单元和多普勒计程仪采集的数据并传输至导航计算模块;所述信号转换模块还用于接收母船导航***的位置、速度和方位角数据并传输至导航计算模块;
所述导航计算模块用于处理母船导航***提供的位置、速度和方位角数据,通过卡尔曼滤波算法以对微机电惯性测量单元进行初始对准;所述导航计算模块还用于处理微机电惯性测量单元和多普勒计程仪采集的数据,通过卡尔曼滤波算法以得到优化的导航结果,
所述卡尔曼滤波算法的状态向量X的表达式如下:
其中,表示微机电惯性测量单元在东向的姿态误差角,表示微机电惯性测量单元
在北向的姿态误差角,表示微机电惯性测量单元在天向的姿态误差角,表示微机电
惯性测量单元在东向的速度误差,表示微机电惯性测量单元在北向的速度误差,表
示微机电惯性测量单元在天向的速度误差,表示微机电惯性测量单元纬度的位置误差,表示微机电惯性测量单元经度的位置误差,表示微机电惯性测量单元高度的位置误
差,表示微机电惯性测量单元的陀螺仪x轴的零偏值,表示微机电惯性测量单元的陀螺
仪y轴的零偏值,表示微机电惯性测量单元的陀螺仪z轴的零偏值,表示微机电惯性测
量单元的加速度计x轴的零偏值,表示微机电惯性测量单元的加速度计y轴的零偏值,表示微机电惯性测量单元的加速度计z轴的零偏值,表示导航装置与母船之间的安装
误差角,
所述卡尔曼滤波算法的量测向量Z的表达式如下:
所述信号转换模块还用于接收导航计算模块计算的导航结果并输出至蛙人运载器;
所述数据存储模块与导航计算模块相连接,用于存储微机电惯性测量单元采集的角速度和加速度数据,多普勒计程仪采集的运动速度数据,导航计算模块计算的导航结果以及信号转换模块接收到的母船导航***位置、速度和方位角数据。
2.根据权利要求1所述的一种水下一体式自主导航装置,其特征在于,还包括顶端开口的密封舱体和扣合至密封舱体顶端的舱盖,所述密封舱体内侧中部凸设有卡阶,所述卡阶固接有安装板,所述微机电惯性测量单元固接至安装板下方,所述安装板上方固接有数据存储模块、导航计算模块和信号转换模块,所述多普勒计程仪固接至密封舱体外侧下方,所述舱盖设有第一水密头插接件和第二水密头插接件,所述第一水密头插接件用于供电,所述第二水密头插接件用于数据传输。
3.根据权利要求2所述的一种水下一体式自主导航装置,其特征在于,所述舱盖下侧设有与密封舱体顶端开口过盈配合的插接部,所述插接部与密封舱体间设有密封圈,所述舱盖表面还设有泄压孔,所述泄压孔内设有相适配的气密塞。
4.根据权利要求2所述的一种水下一体式自主导航装置,其特征在于,所述多普勒计程仪上侧面通过第一螺栓固接有安装底板,所述安装底板设有凸出多普勒计程仪上侧面外周的外延部,所述密封舱体的底端设有与安装底板相适配的凹槽,所述安装底板嵌设于凹槽内,所述安装底板的外延部通过第二螺栓固接至凹槽底面。
5.根据权利要求4所述的一种水下一体式自主导航装置,其特征在于,所述第二螺栓在凹槽底面相对应的螺栓孔为盲孔。
6.根据权利要求1所述的一种水下一体式自主导航装置,其特征在于,所述信号转换模块设有以太网输出接口、RS422输出接口和CAN输出接口。
7.根据权利要求1所述的一种水下一体式自主导航装置,其特征在于,所述数据存储模块设有USB输出接口。
8.一种导航装置初始对准方法,用于权利要求1至7任一项所述的水下一体式自主导航装置,其特征在于,包括如下步骤:
S10,将导航装置固定到母船,使导航装置与母船导航***建立通讯连接;
S20,导航计算模块获取母船导航***的位置数据、速度数据装订至导航装置;
S30,导航计算模块获取母船导航***的方位角数据,微机电惯性测量单元采集的角速度、加速度数据,多普勒计程仪采集的运动速度数据,结合母船导航***的位置数据、速度数据,通过卡尔曼滤波算法得出导航装置的姿态误差角、微机电惯性测量单元的陀螺仪常值误差和微机电惯性测量单元的加速度计常值误差。
9.根据权利要求8所述的一种导航装置初始对准方法,其特征在于,所述S30步骤中,卡尔曼滤波算法的状态方程为:
状态转移矩阵F的表达式如下,
其中,
其中,矩阵为状态转移矩阵F的子块,矩阵均为矩阵的
子块,矩阵均为矩阵的子块,为的转置矩阵,为载体坐标系与导航坐标系
之间的姿态变换矩阵,通过粗对准得到,为地球自转角速度,为地球卯酉圈主曲率半
径,为地球子午圈主曲率半径,表示由微机电惯性测量单元测得的母船在导航坐标
系相对于惯性坐标系的旋转角速度在地球坐标系的投影,表示由微机电惯性测量单元
测得的母船在导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系的投影,表示
由微机电惯性测量单元测得的母船在导航坐标系相对于地球坐标系的旋转角速度在导航
坐标系的投影,为微机电惯性测量单元的加速度计所测量的比力值,表示微机电惯性
测量单元在导航坐标系下测量得到的速度值,表示微机电惯性测量单元在导航坐标系下
测量得到的东向速度值,表示微机电惯性测量单元在导航坐标系下测量得到的北向速度
值,
卡尔曼滤波算法的量测方程为:
其中,H为量测矩阵,V为量测噪声,
量测矩阵H的表达式如下,
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210984493.1A CN115060274B (zh) | 2022-08-17 | 2022-08-17 | 一种水下一体式自主导航装置及其初始对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210984493.1A CN115060274B (zh) | 2022-08-17 | 2022-08-17 | 一种水下一体式自主导航装置及其初始对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115060274A CN115060274A (zh) | 2022-09-16 |
CN115060274B true CN115060274B (zh) | 2022-11-18 |
Family
ID=83208288
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210984493.1A Active CN115060274B (zh) | 2022-08-17 | 2022-08-17 | 一种水下一体式自主导航装置及其初始对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115060274B (zh) |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109324330B (zh) * | 2018-09-18 | 2022-11-18 | 东南大学 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
CN109443379B (zh) * | 2018-09-28 | 2020-07-21 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN110763872A (zh) * | 2019-11-21 | 2020-02-07 | 中国船舶重工集团公司第七0七研究所 | 一种多普勒测速仪多参数在线标定方法 |
CN110940340A (zh) * | 2019-12-23 | 2020-03-31 | 中科探海(苏州)海洋科技有限责任公司 | 基于小型uuv平台的多传感器信息融合方法 |
CN111412912A (zh) * | 2020-04-14 | 2020-07-14 | 上海华测导航技术股份有限公司 | 导航板卡、用于导航板卡的多源数据融合方法以及运载体 |
CN111829512B (zh) * | 2020-06-08 | 2024-04-09 | 中国航天空气动力技术研究院 | 一种基于多传感器数据融合的auv导航定位方法及*** |
CN112097763B (zh) * | 2020-08-28 | 2022-07-05 | 西北工业大学 | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 |
CN112378400A (zh) * | 2020-10-30 | 2021-02-19 | 湖南航天机电设备与特种材料研究所 | 一种双天线gnss辅助的捷联惯导组合导航方法 |
CN112798016A (zh) * | 2020-12-22 | 2021-05-14 | 中国航天空气动力技术研究院 | 一种基于sins与dvl组合的auv行进间快速初始对准方法 |
CN112798021B (zh) * | 2021-04-15 | 2021-07-13 | 中国人民解放军国防科技大学 | 基于激光多普勒测速仪的惯导***行进间初始对准方法 |
CN113108783B (zh) * | 2021-05-09 | 2022-06-14 | 中国人民解放军国防科技大学 | 一种无人潜航器惯性/多普勒组合导航方法 |
-
2022
- 2022-08-17 CN CN202210984493.1A patent/CN115060274B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN115060274A (zh) | 2022-09-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
Kinsey et al. | A survey of underwater vehicle navigation: Recent advances and new challenges | |
CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航*** | |
CN210719199U (zh) | 一种水下机器人多设备组合导航*** | |
CN109782323A (zh) | 一种深海水下自主航行器导航定位及校准方法 | |
CN101832775B (zh) | 深海作业水下航行器组合导航***及水下初始对准方法 | |
CN111829512B (zh) | 一种基于多传感器数据融合的auv导航定位方法及*** | |
CN105928515B (zh) | 一种无人机导航*** | |
US20180106619A1 (en) | Mobile Structure Heading and Piloting Systems and Methods | |
CN110806209A (zh) | 一种水下机器人多设备组合导航***及方法 | |
CN102288170A (zh) | 一种水下航行器内电子罗盘的校正方法 | |
US11280896B2 (en) | Doppler GNSS systems and methods | |
CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 | |
CN105928519B (zh) | 基于ins惯性导航与gps导航以及磁力计的导航算法 | |
CN113155134B (zh) | 一种基于惯性信息辅助的水声信道跟踪与预测方法 | |
Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
CN115060274B (zh) | 一种水下一体式自主导航装置及其初始对准方法 | |
Zhao et al. | The experimental study on GPS/INS/DVL integration for AUV | |
Walchko | Low cost inertial navigation: Learning to integrate noise and find your way | |
CN109459017B (zh) | 一种利用外部基准辅助的星座自主导航方法 | |
CN103697887B (zh) | 一种基于捷联惯导***和多普勒计程仪的优化导航方法 | |
CN110954097A (zh) | 一种用于机器人组合的导航定位方法 | |
CN115542363A (zh) | 一种适用于垂直下视航空吊舱的姿态测量方法 | |
Cruz et al. | DART—A portable deep water hovering AUV | |
CN114018255B (zh) | 一种水下滑翔机的智能组合导航方法、***、设备和介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |