一种基于M估计的AUV组合导航方法及系统技术方案

本发明专利技术专利公开一种基于M估计的AUV组合导航方法及系统，提出一种基于广义极大似然估计(M估计)的滤波算法，在系统量测噪声为非高斯分布，特别是混合高斯分布(高斯附近对称干扰)条件下，应用M‑估计算法，通过影响函数与权值函数对量测残差与状态预测残差进行加权，减小量测异常峰值给导航系统带来的影响。并将该算法应用于SINS/USBL组合导航系统中，在水声传感器由于多径效应产生的非高斯量测噪声、量测异常的条件下，对比标准卡尔曼滤波算法与M估计滤波算法，后者的位置、速度误差精度相对于前者均有显著提高且混合高斯模型受污染越严重的情况下，基于M估计滤波的SINS/USBL组合导航系统滤波效果越明显，鲁棒性、抗干扰性能相对更好。

【技术实现步骤摘要】
一种基于M估计的AUV组合导航方法及系统
本专利技术涉及一种基于M估计的AUV组合导航方法及系统，属于组合导航

技术介绍
水下潜航器(AUV)作为开发探索海洋资源的设备，多种多样的水下潜航器替代潜水员进行水下资源探索和深海作业等工作。获取水下潜航器准确的位置信息是水下潜航器执行作业的基础保障。现阶段，恶劣水下环境中的高精度导航定位技术俨然已成为自主水下潜航器发展所不得不面对的难题。单一传感器不能准确提供载体的导航定位参数，组合导航系统采用两个或两个以上优势互补的传感器，达到比任何单一传感器都好的导航效果。现阶段，水下载体导航定位任务采用组合导航的方式，可以有效的满足深海潜航器的探索及高难度作业过程。基于所需载体精度要求，我们选择将性能各异的传感器组合，从而提高水下潜航器导航定位系统的精度与鲁棒性。具有代表性的捷联惯导/全球导航定位系统(GlobalPositioningSystem,GPS)的组合导航技术已经发展成熟，是载体导航定位的首选。在海洋环境下，由于GPS在部分海域内存在接收不满四颗以上卫星信号以及电磁波在水下传播能量衰减严重等因素，使得GPS不能一直满足水中载体导航定位的条件。而声波在水下传播衰减很小，传播距离更远，可以用于水下信息传递。有“水下GPS”美誉的超短基线定位系统(Ultra-ShortBaseline,USBL)逐渐受到越来越多学者的关注，基于水声定位的USBL凭借着其使用简易、灵活，定位精度较高，渐渐成为研究水下载体导航定位种倍受青睐的测距传感器。标准卡尔曼滤波是组合导航系统中量测噪声服从高斯分布情况的最优解，但是，在AUV在水下作业时，异常杂波会对量测信息产生很大影响，并且在很多时候噪声呈现厚尾分布特性，势必造成状态估计精度下降，严重的情况下可能会造成协同导航算法定位误差发散。
技术实现思路
本专利技术的目的是为了：针对上述问题，提出一种基于M估计的AUV组合导航方法及系统，在系统量测噪声为非高斯分布，特别是混合高斯分布(高斯附近对称干扰)条件下，该方法可以提高AUV组合导航滤波效果。本专利技术包括以下几个步骤：一种基于M估计的AUV组合导航方法，包括以下几个步骤：步骤一：建立SINS/USBL组合导航模型；步骤二：对组合导航模型的量测方程进行重构；步骤三：根据重构后的量测方程，对组合导航模型中的状态变量X求解，获得X的最优估计值；步骤四：根据步骤三得到的X最优估计值进行校正，从而实现AUV组合导航定位。进一步的，建立SINS/USBL组合导航模型，具体为：(1.1)选取东北天E-N-U地理坐标系为导航坐标系，选取捷联惯导系统导航参数的误差量作为系统状态量，即选取位置误差[δLδλδh]、速度误差[δVEδVNδVU]、姿态误差角[φEφNφU]、沿载体坐标系X-Y-Z轴上的陀螺漂移[εxεyεz]和加速度计零偏作为状态量，建立状态向量如下(1.2)SINS/USBL组合导航模型的状态方程如下所示：为X(t)的一阶导数；系统噪声为：η(t)＝[nax,nay,naz,nwx,nwy,nwz]T；其中，nax,nay,naz,nwx,nwy,nwz为三轴加速度计和三轴陀螺仪零均值高斯白噪声；G(t)为噪声驱动矩阵：其中，T为捷联矩阵，o为零矩阵；F(t)为状态转移矩阵，把F(t)变成分块矩阵形式如下所示，其中，FINS,9×9代表SINS误差方程；(1.3)将状态方程进行一阶离散化Xk＝Φk/k-1Xk-1+wk-1式中：Xk＝X(tk)Ts＝tk-tk-1其中：I为单位阵，wk为高斯白噪声，即wk～(0,Q)；Q是指高斯白噪声方差；(1.4)SINS/USBL组合导模型的量测方程如下.yk＝HkXk+vk其中，yk为量测量，Hk为量测矩阵，vk为量测噪声；量测矩阵为：式中，其中：R为地球半径，h为高度，L为纬度，λ为经度，e为地球曲率半径，为SINS计算的载体坐标系下应答器的位置信息；为平台坐标系到载体坐标系转换矩阵，为地球坐标系到平台坐标系转换矩阵。应答器是指固定在海底的信号发射器或者接收器；导航坐标系：定义为E-N-U的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向东，yn轴沿当地纬线的切线向北，zn轴垂直向上构成右手坐标系；平台坐标系：与导航坐标系存在姿态误差角的坐标系；地球坐标系：原点位于地球中心，ze轴沿极轴方向，xe轴沿格林威治子午线方向，ye轴在赤道平面内与xe,ze构成右手坐标系，随地球转动而转动；载体坐标系：原点位于载体质心处，xb沿载体右向，yb沿载体轴向向前，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。进一步的，量测噪声呈厚尾特性，服从混合高斯分布如下：f(vk)是vk的概率密度分布，式中，σ1、σ2分别为两个标准高斯噪声标准差；干扰因子∈用来表征厚尾噪声的程度，在理想条件下，∈＝0量测噪声呈现为式中前一部分高斯分布，σ1为其标准差，在实际水下环境中，量测噪声呈现厚尾特性，在前一部分标准高斯分布中，加入标准差为σ2的异常量测。进一步的，对组合导航模型的量测方程进行重构：(2.1)设真实状态值与一步预测关系方程如下：式中：δXk为真实值与预测值之差；(2.2)对组合导航模型进行变换，得到：定义如下变量：其中：Rk＝σ12,Pk/k-1＝Φk/k-1Pk-1ΦTk/k-1+Q；(2.3)重构后的量测方程如下：zk＝MkXk+ξk。进一步的，根据重构后的量测方程，应用M估计方法对组合导航模型中的状态变量X求解，获得X的最优估计值。进一步的，根据得到的X最优估计值进行校正，具体包括如下步骤：(4.1)姿态校正：q＝m2qnb(T)其中，m2qnb为通过捷联矩阵T计算四元数q的函数；(4.2)速度校正：其中，V＝[VEVNVU]为导航坐标系下载体运动的速度；(4.3)位置校正：其中，L为载体所在的纬度，λ为载体所在的经度。进一步的，本专利技术还提出一种AUV组合导航系统，包括：组合导航模型建立模块：建立SINS/USBL组合导航模型；量测方程重构模块：对组合导航模型的量测方程进行重构；最优估计值求解模块：根据重构后的量测方程，对组合导航模型中的状态变量X求解，获得X的最优估计值；校正模块：根据得到的X最优估计值进行校正，从而实现AUV组合导航定位。本专利技术的本文档来自技高网...

【技术保护点】
1.一种基于M估计的AUV组合导航方法，其特征在于，包括以下几个步骤：/n步骤一：建立SINS/USBL组合导航模型；/n步骤二：对组合导航模型的量测方程进行重构；/n步骤三：根据重构后的量测方程，对组合导航模型中的状态变量X求解，获得X的最优估计值；/n步骤四：根据步骤三得到的X最优估计值进行校正，从而实现AUV组合导航定位。/n

【技术特征摘要】
1.一种基于M估计的AUV组合导航方法，其特征在于，包括以下几个步骤：
步骤一：建立SINS/USBL组合导航模型；
步骤二：对组合导航模型的量测方程进行重构；
步骤三：根据重构后的量测方程，对组合导航模型中的状态变量X求解，获得X的最优估计值；
步骤四：根据步骤三得到的X最优估计值进行校正，从而实现AUV组合导航定位。


2.根据权利要求1所述的基于M估计的AUV组合导航方法，其特征在于：建立SINS/USBL组合导航模型，具体为：
(1.1)选取东北天E-N-U地理坐标系为导航坐标系，选取捷联惯导系统导航参数的误差量作为系统状态量，即选取位置误差[δLδλδh]、速度误差[δVEδVNδVU]、姿态误差角[φEφNφU]、沿载体坐标系X-Y-Z轴上的陀螺漂移[εxεyεz]和加速度计零偏作为状态量，建立状态向量如下



(1.2)SINS/USBL组合导航模型的状态方程如下所示：




为X(t)的一阶导数；
系统噪声为：
η(t)＝[nax,nay,naz,nwx,nwy,nwz]T；
其中，nax,nay,naz,nwx,nwy,nwz为三轴加速度计和三轴陀螺仪零均值高斯白噪声；
G(t)为噪声驱动矩阵：



其中，T为捷联矩阵，o为零矩阵；
F(t)为状态转移矩阵，把F(t)变成分块矩阵形式如下所示，



其中，FINS,9×9代表SINS误差方程；
(1.3)将状态方程进行一阶离散化
Xk＝Φk/k-1Xk-1+wk-1
式中：
Xk＝X(tk)






Ts＝tk-tk-1
其中：I为单位阵，wk为高斯白噪声，即wk～(0,Q)；Q是指高斯白噪声方差；(1.4)SINS/USBL组合导模型的量测方程如下.
yk＝HkXk+vk
其中，yk为量测量，Hk为量测矩阵，vk为量测噪声；
量测矩阵为：



式中，






其中：R为地球半径，h为高度，L为纬度，λ为经度，e为地球曲率半径，为SINS计算的载体坐标系下应答器的位置信息；

为平台坐标系到载体坐标系转换矩阵，为地球坐标系到平台坐标系转换矩阵。


3.根据权利要求2所述的基于M估计的AUV组合导航方法，其特征在于：应答器是指固定在海底的信号发射器或者接收器；
导航坐标系：定义为E-N-U的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向东，yn轴沿当地纬线的切线向北，zn轴垂直向上构成右手坐标系；
平台坐标系：与导航坐标系存在姿态误差角的坐标系；
地球坐标系：原点位于地球中心，ze轴沿极轴方向，xe轴沿格林威治子午线方向，ye轴在赤道平面内与xe,ze构成右手坐标系，随地球转动而转动；
载体坐标系：原点位于载体质心处，xb沿载体右向，yb沿载体轴向向前，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。


4.根据权利要求2所述的基于M估计的AUV组合导航方法，其特征在于：量测噪声呈厚尾特性，服从混合高斯分布如下：



f(vk)是vk的概率密度分布，式中，σ1、σ2分别为两个标准高斯噪声标准差；干扰因子c用来表征厚尾噪声的程度，在理想条件下，c＝0量测噪声呈现为式中前一部分高斯分布，σ1为其标准差，在实际水下环境中，量测噪声呈现厚尾特性，在前一部分标准高斯分布中，加入标准差为σ2的异常量测。


5.根据权利要求2所述的基于M估计的AUV组合导航方法，其特征在于：对组合导航模型的量测方程进行重构：
(2.1)设真实状态值与一步预测关系方程如下：



式中：δXk为真实值与预测值之差；
(2.2)对组合导航模型进行变换，得到：



定义如下变量：












其中：Rk＝σ12,Pk/k-1＝Φk/k-1Pk-1ΦTk/k-1+Q；
(2.3)重构后的量测方程如下：
zk＝MkXk+ξk。


6.根据权利要求1所述的基于M估计的AUV组合导航方法，其特征在于：根据重构后的量测方程，应用M估计方法对组合导航模型中的状态变量X求解，获得X的最优估计值。


7.根据权利要求5所述的基于M估计的AUV组合导航方法，其特征在于：根据得到的X最优估计值进行校正，具体包括如下步骤：
(4.1)姿态校正：



q＝m2qnb(T)
其中，m2qnb为通过捷联矩阵T计算四元数q的函数；
(4.2)速度校正：
<...

【专利技术属性】
技术研发人员：卢曼曼，张南南，李宇航，郭眀禹，
申请(专利权)人：中国航天空气动力技术研究院，
类型：发明
国别省市：北京;11