一种基于改进的S-H自适应联邦滤波的智能水下机器人多源组合导航方法技术

本发明专利技术公开了一种基于改进的S

【技术实现步骤摘要】
一种基于改进的S
‑
H自适应联邦滤波的智能水下机器人多源组合导航方法


[0001]本专利技术涉及一种基于改进的S
‑
H自适应联邦滤波的智能水下机器人多源组合导航方法，属于智能水下机器人领域。

技术介绍

[0002]高精度导航系统是智能水下机器人AUV设计过程中的重点和难点，也是其能够顺利完成任务并成功返回的保障。目前，捷联惯性导航系统SINS由于其在短时间内导航精度高，更新信息速度快，且不需外部信息输入，已成为水下常用的导航传感器，但是其误差会随着时间的增加而不断累积，无法单独完成导航任务，需要与其他导航传感器，如超短基线定位系统(Ultra Short Baseline Positioning System，USBL)、多普勒测速仪(Doppler Velocity Log，DVL)、磁航向仪(Magnetic Compass，MCP)进行组合使用来抑制随时间积累产生的误差。
[0003]随着科技的发展，水下导航传感器的种类和功能也越加多样化，如何有效的对多传感器组成的组合导航系统进行信息融合得到最本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于改进的S
‑
H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，所述改进的S
‑
H自适应联邦滤波方法包括以下步骤：S100、进行水下多源组合导航系统建模，得到导航传感器及其误差模型；S200、以误差模型为基础，提出改进S
‑
H自适应联邦滤波方法。2.根据权利要求1所述的一种基于改进的S
‑
H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，在S100中，具体包括以下步骤：S110、首先建立导航系统常用坐标系和坐标转换关系；S120、在S110的基础上获得SINS、USBL、DVL和MCP四种传感器的误差模型。3.根据权利要求2所述的一种基于改进的S
‑
H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，在S110中，具体包括以下步骤：S111、假定SINS、USBL、DVL和MCP四种传感器均安装于水下机器人载体中心，则所述四种传感器所获得的数据都是基于载体坐标系下的，需要将其统一到导航坐标系下；S112、导航坐标系围绕OZ
n
轴旋转ψ，围绕OX
n1
轴旋转θ，然后绕OY
n2
旋转γ，即：其中，ψ为航向角，以地理北向为起点沿顺时针方向运动为正，范围为0
°
至360
°
；θ为俯仰角，以水平面为基准，向上为正，向下为负，范围为
‑
90
°
至90
°
；γ为横滚角，以铅垂平面为基准，右倾为正，左倾为负，范围为
‑
90
°
至90
°
；转换矩阵分别对应于：转换矩阵分别对应于：转换矩阵分别对应于：则导航坐标系与水下机器人载体坐标系之间的转换矩阵为：则导航坐标系与水下机器人载体坐标系之间的转换矩阵为：4.根据权利要求3所述的一种基于改进的S
‑
H自适应联邦滤波的智能水下机器人多源组合导航方法，其特征在于，在S120中，具体的：所述SINS的误差包括速度、位置和姿态误差，其中，姿态误差方程为：
式中，表示东、北、天三向平台失准角，为地球自转角速度，表示水下机器人载体位置变化导致的角速度，表示水下机器人载体位置变化导致的角速度，上式中，V＝[V
E V
N V
U
]
T
分别表示水下机器人载体在地理坐标系中的东、北、天三向速度，L,λ,h分别表示经度、纬度和高度，R
M
、R
N
分别表示地球参考椭球子午圈、椭球卯酉圈上各点的曲率半径，将姿态误差方程展开得：速度误差方程：上式中，f
n
表示比力，将其展开如下：
位置误差方程:所述USBL的误差为：建立基阵坐标系oxyz，声学信标固定在水下机器人载体上，其位置设为(X
a
,Y
a
,Z
a
)，R为位置矢量，三个水听器分别放置在原点、x轴和y轴上，声学信标和原点之间连线与坐标轴夹角分别是θ
mx
、θ
my
和θ
m
，信标与基阵之间的距离R为：而X
a
＝Rcosθ
mx
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)Y
a
＝Rcosθ
my
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)由上两式得：将式(12)代入式(13)、式(14)得：
再将式(15)代入式(16)得：解得：同理得：θ
m...

【专利技术属性】
技术研发人员：孙玉山，张力文，马陈飞，张国成，刘继骁，王旭，张家利，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：