一种基于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＝σ1²,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组合导航定位。

本发明的优点在于：

为解决水下环境复杂、多径效应等导致的usbl量测噪声异常(非高斯分布)现象，使用混合高斯分布模拟该现象，应用广义最大似然估计算法(m-估计)，通过影响函数与权值函数对量测残差与状态预测残差进行加权，减小量测异常峰值给导航系统带来的影响，将本发明方法应用于sins/usbl组合导航系统中，提出基于m估计的auv组合导航方法，明显降低基阵增益等影响，提高组合精度。

附图说明

图1.混合分布干扰因子不同时的观测值示意图

图2.混合分布中准差不同时位置观测噪声值示意图

图3.∈＝0.05,σ＝100位置误差对比示意图

图4.∈＝0.05,σ＝100速度误差对比示意图

图5.∈＝0.05,σ＝100姿态误差对比示意图

图6.∈＝0.35,σ＝100位置误差对比示意图

图7.∈＝0.35,σ＝100速度误差对比示意图

图8.∈＝0.35,σ＝100姿态误差对比示意图

图9.∈＝0.35,σ＝50位置误差对比示意图

图10.∈＝0.35,σ＝50速度误差对比示意图

图11.∈＝0.35,σ＝50姿态误差对比示意图

图12为本发明流程图

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

标准卡尔曼滤波是基于量测噪声服从高斯分布的情况应用于组合导航中，但水下潜航器导航定位应用的水声传感器会由于多径效应，存在非高斯量测噪声、量测异常的情况。基于上述问题，本发明提出一种基于m估计的auv组合导航方法，在系统量测噪声为非高斯分布，特别是混合高斯分布(高斯附近对称干扰)条件下，该组合导航方法可以提高auv组合导航精度，如图12所示，具体步骤如下：

步骤一：建立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组合导航模型的状态方程如下所示：

系统噪声为：

η(t)＝[nax,nay,naz,nwx,nwy,nwz]^t(1-2)

其中，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(1-5)

式中：

xk＝x(tk)(1-6)

ts＝tk-tk-1

其中：i为单位阵，wk为高斯白噪声，即wk～(0,q)，q是指高斯白噪声方差；

(1.4)sins/usbl组合导模型的量测方程如下：

yk＝hkxk+vk(1-7)

其中，yk为量测量，hk为量测矩阵，vk为量测噪声。

量测矩阵为：

式中，

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

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

本发明中，应答器是一种已知位置的、固定在海底的发射/接收器。

导航坐标系(n系)：定义为e-n-u的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向东，yn轴沿当地纬线的切线向北，zn轴垂直向上构成右手坐标系。

平台坐标系：与导航坐标系存在姿态误差角的坐标系。

地球坐标系(e系)：原点位于地球中心，ze轴沿极轴方向，xe轴沿格林威治子午线方向，ye轴在赤道平面内与xe,ze构成右手坐标系，随地球转动而转动。

载体坐标系(b系)：原点位于载体质心处，xb沿载体右向，yb沿载体轴向向前，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。

量测方程确定过程如下：

载体在地球坐标系中的位置坐标为p＝[lλh]^t，载体在地球直角坐标系下的位置坐标为(x,y,z)^t，两种坐标之间互相进行转换的方程如下所示

对式(1-10)两侧求全微分并转换成矩阵相乘形式如下

usbl输出的导航信息为位置信息，因此，我们常采用sins与usbl输出的位置之差作为卡尔曼滤波器的外部量测量。sins/usbl组合导航系统的量测方程具体推导内容如下。

载体坐标系下usbl系统所估计出应答器的解算位置为

br＝r^t(s-p)+nr(1-12)

上式中s∈r³代表地球坐标系下应答器的位置信息，由于应答器是事先布置在海底的，为已知量；p∈r³代表地球坐标系下载体的位置信息，理想情况下它和sins解算出的位置信息一致；nr代表超短基线系统的位置量测噪声。

上式的矩阵形式如下

捷联惯导系统进行自身导航解算过程中，n系与p系存在平台失准角，p系到n系的旋转矩阵可用该角度表示为

上式中i是3维单位矩阵，[φ×]是由平台失准角φ＝[φxφyφz]^t三轴分量组成的反对称矩阵。

sins可以解算出e系下载体的位置和旋转矩阵如下:

因此，sins所估计出b系下应答器的位置信息如下：

式(1-16)的矩阵形式为.

由捷联惯导系统的误差方程可知δp和分别为.

将式(1-18)与(1-19)代入到式(1-16)中可得.

忽略[φ×]δp二阶小量的影响，把上式写成如下形式：

将式(1-16)和式(1-21)做差，得到usbl与sins解算的位置信息之差如下：

根据式(1-11)把地球直角坐标系中的x,y,z转换为经纬度坐标，列成矩阵形式如下.

捷联惯导与超短基线系统量测量位置误差为：

其矩阵形式为.

因此量测矩阵为.

量测噪声呈厚尾特性，服从混合高斯分布。由于水下复杂环境引起多径效应后，usbl获得位置信息中含有混合高斯量测项，即两个高斯分布的合成，如下：

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

∈>0.5时，由于干扰模型所占比重已经超越高斯分布模型，此时该模型就不能再称作高斯分布模型了。保持其他条件统一，对于该混合高斯分布的观测值，改变干扰因子∈与标准差σ1均能够产生较大影响，令σ2＝5σ1。这里通过控制变量方法，固定某一因素，改变另一因素，分析比较两者对观测值产生的影响。

图1保持标准差为10不变，设置∈分别为0.05、0.35、0.65，比较各条件下观测噪声值分布。图2保持∈为0.05不变，设置σ1分别为10、50、100，比较各条件下观测噪声值分布。从图1和图2中可以看出，无论是增大干扰因子还是增加标准差的值对观测值的大小与稳定性都会产生较大的影响。

步骤二：对所述组合导航模型的量测方程进行重构；

(2.1)设真实状态值与一步预测关系方程如下：

式中：δxk为真实值与预测值之差，

(2.2)对系统模型进行变换，得到：

定义如下变量：

其中：rk＝σ1²,pk/k-1＝φk/k-1pk-1φ^tk/k-1+q

(2.3)重构后的量测方程如下：

zk＝mkxk+ξk(2-4)

步骤三：根据重构后的量测方程，应用m估计方法对组合导航模型中的状态变量x求解，获得x的最优估计值。

定义m估计的代价函数如下所示：

式中，ek+1,i＝(zk-mkxk)i，函数ρ满足下式：

式中，γ为可调节参数。调节γ，即可得到具有较高估计效率的估计结果。

最小化代价函数的解满足如下条件：

式中，φ(ek+1,i)＝ρ'(ek+1,i)。

定义函数如下所示：

定义矩阵如下：

ψ＝diag[ψ(ek+1,i)](3-6)

把对角矩阵ψ分解为两块分别对应于量测残差与状态预测残差，

式中ψy代表量测残差的加权矩阵，ψx代表状态预测残差的加权矩阵。

则矩阵ψ修正量测噪声如下结果：

因此(3-3)转化为矩阵形式为：

h^tψ(hx-z)＝0(3-9)

使用用加权迭代求解上式，则状态变量x的解表示为：

x^(j+1)＝(h^tψ^(j)h)^-1h^tψ^(j)z(3-10)

式中，j表示迭代次数。迭代过程稳定后的x^(j+1)即为最终的状态估计，状态估计初值可表示为：

x⁽⁰⁾＝(h^th)^-1h^ty(3-11)

将上述m估计方法应用到标准卡尔曼滤波系统中，得到基于m估计的卡尔曼滤波更新算法，获得x的最优估计值：

时间更新

量测更新

步骤四：根据步骤三得到的x最优估计值进行校正，从而实现组合导航定位。

(4.1)姿态校正：

其中，m2qnb为通过捷联矩阵t计算四元数q的函数。

(4.2)速度校正：

其中，v＝[vevnvu]为导航坐标系下载体运动的速度。

(4.3)位置校正：

其中，l为载体所在的纬度，λ为载体所在的经度。

实施过程：

实施例一：

干扰因子不同条件下m估计算法有效性验证，设置组合导航系统中sins具体参数为加速度计零偏和陀螺漂移分别为5×10^-4g,0.1°/h，噪声分别为10^-6g,0.001°/h，usbl测量误差为0.5％斜距，usbl的量测噪声为非高斯即服从混合高斯分布，对比混合高斯分布中干扰因子不同条件下m估计滤波与标准卡尔曼滤波的滤波效果。这里令干扰因子等于0.05，干扰分布标准差等于100，仿真时间为3600s，对sins/usbl组合导航系统基于量测噪声混合高斯分布下的位置误差、速度误差、姿态误差进行仿真验证，变化曲线如图3、图4、图5所示。令干扰因子等于0.35，干扰分布标准差等于100，仿真时间为3600s。对sins/usbl组合导航系统基于量测噪声服从混合高斯分布下的位置误差、速度误差、姿态误差进行仿真验证，变化曲线如图6、图7、图8所示，将上述两个仿真进行rms值统计，结果如表1所示。

表1干扰因子不同时的导航参数rms值

实施例二：

标准差不同条件下m估计算法有效性验证，本节基于量测噪声服从混合高斯模型中标准差不同条件下，对m估计滤波与标准卡尔曼滤波进行分析比较。对比3.2中干扰因子等于0.35，标准差等于100的混合高斯分布，这里采用控制变量法，固定干扰因子等于0.35不变，设置标准方差为50。仿真时间为3600s，对sins/usbl组合导航系统基于量测噪声混合高斯分布下的位置误差、速度误差、姿态误差进行仿真验证变化曲线如图9、图10、图11所示。将以上仿真得到具体的导航参数进行rms值统计，整理后得到表2如下所示：

表2标准差不同时的导航参数rms值

实施效果：

由实施例一通的曲线和表格可知：

1、图3-5与表格1表明：在sins/usbl组合导航系统中，当量测噪声服从∈＝0.05的混合高斯分布时，标准kalman与m估计滤波算法的效果有明显差异，前者的各项导航参数误差在数值上要明显大于后者，m估计的位置、速度误差精度分别提高约40％、60％以上并且在量测噪声为非高斯、异常时，m估计鲁棒性较强，具有更好的有效性与稳定性。

2、为了进一步验证在水下环境更为恶劣，比如声波干涉、介质分层、多径效应更为严重环境下m-估计算法的鲁棒性能，图6至图8采取增大干扰因子的比重的方法进行仿真，也意味着量测噪声服从的混合高斯模型不确定性更强，即∈＝0.35。图6至图8与表格1可以看出，各项导航参数误差与∈＝0.05时比都明显增大。经比较，m-估计滤波的位置、速度误差精度相对于标准卡尔曼滤波分别提高约70％、85％以上。这说明，量测噪声服从混合高斯模型受污染越严重的情况下，基于m估计的sins/usbl组合导航方法效果越明显，鲁棒性、抗干扰性能相对更好。

由实施例二的曲线图9-11与表格2可以看出，量测噪声服从∈＝0.35，σ1＝50的混合高斯分布条件下，标准卡尔曼滤波的各项导航参数误差在数值上要明显大于m-估计滤波，m-估计的位置、速度误差精度分别提高约60％、70％以上，说明了该条件下m-估计滤波的有效性。在保证干扰因子∈大小不变，提高σ1＝100，m-估计的位置、速度误差精度分别提高到了70％、85％以上，说明该混合噪声不确定峰值越大即异常量测噪声尖峰越大，m-估计算法应用在组合导航系统中滤波效果越明显，对异常噪声的削弱作用越显著，更好的保证了滤波精度。

从以上实施例不难看出，针对水下环境恶劣，声波传播可能出现干涉、多径效应等情况导致量测异常的现象，混合高斯分布受污染不同的条件下，对比了m估计算法与标准卡尔曼滤波在处理该噪声后组合导航系统各导航参数误差情况，通过计算机轨迹模拟与半实物试验对算法可行性进行验证，结果表明本文提出的基于m估计的组合导航方法能够有效估计出导航误差，且量测噪声服从的混合高斯分布受污染情况越严重，m-估计算法对于该异常噪声的削弱作用越显著。