[发明专利]一种水面船自主定位方法及系统

说明书

本发明涉及一种水面船自主定位方法及系统，仅依靠偏航角信息建立状态方程，通过电机电压和水面船稳态速度的非线性函数关系建立观测方程，根据前一时刻的系统状态向量均值的后验估计值和前一时刻的协方差矩阵平方根因子的后验估计值，利用状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，根据这两项先验估计值计算得到观测量测容积点，利用观测方程确定卡尔曼系数矩阵，然后确定当前时刻的协方差矩阵平方根因子的后验估计值，最后得到水面船系统当前时刻的系统状态向量均值的后验估计值，即为水面船当前时刻的位置，实现了在缺失GPS/北斗等外部传感器信号的恶劣环境下的自主准确定位。

技术领域

本发明涉及水面船自主定位领域，特别是涉及一种水面船自主定位方法及系统。

背景技术

随着海洋科学技术及计算机技术的不断进步，自主式水面船已经在民用和军事领域扮演着越来越重要的角色。目前，自主式水面船已广泛应用于海洋资源探测、海上巡逻、侦查和搜救等作业。水面船的高精度导航定位是保证水面船能顺利完成既定任务的重要前提，也是当前自主式水面船领域的热点问题。

通用的水面船导航定位系统搭载有捷联惯性导航和GPS/北斗等传感器。捷联惯性导航系统内部的加速度计通过对获取的加速度信息两次积分操作求得水面船的实际位置。但是一方面加速度计存在零漂和温漂等效应，另一方面加速度计测量的是包括水面船的线加速度、重力加速度和由于水面船旋转运动产生的向心加速度等的叠加值，以上缺点使得单独利用加速度计对水面船导航定位误差较大。GPS/北斗导航系统已经广泛应用在军事和民用等各个领域，但是当水面船在狭窄水域作业或执行隐秘军事任务时，GPS/北斗信号会受周围环境和地形的影响，甚至导致信号丢失，无法实现水面船的自主定位。

目前，通常采用滤波技术实现多源传感器的信息融合，以期获得较为准确的水面船位置信息。较为典型的，卡尔曼滤波器(Kalman Filter，KF)已经广泛应用于移动机器人、水面船等的导航定位问题。理论表明，当系统的状态方程和观测方程都为线性且噪声满足高斯分布时，KF是最优的线性滤波器。然而现实世界中几乎所有的系统都是非线性的，针对非线性系统的导航定位，人们提出了各种非线性滤波方法。扩展卡尔曼滤波器(ExtendedKF，EKF)通过对非线性方程线性化来处理非线性问题，然而，由于EKF采用一阶的泰勒展开近似非线性方程，引入了高阶项截断误差，只适用于弱非线性系统。无迹卡尔曼滤波器(Unscented KF，UKF)作为常用的非线性滤波器已经广泛应用在自主车辆定位和水下航行器导航定位等领域。UKF是基于“对概率分布进行近似要比对非线性函数进行近似容易得多”的思想而提出的。针对n维非线性系统，UKF通过无迹变换(Unscented Transform，UT)，选择2n+1个不同权值的Sigma点来近似计算状态变量的均值。和EKF相比，UKF通过Sigma点经过非线性函数传播后导出的均值和方差能够达到非线性函数真实值的三阶精度，因此精度高于EKF，但是在高维系统中UKF容易出现数值不稳定的现象，甚至会导致发散。

因此，提出一种保障水面船在缺失GPS/北斗信号等恶劣环境下的准确定位的方法变得十分重要和有意义。

发明内容

本发明的目的是提供一种水面船自主定位方法及系统，以实现在缺失GPS/北斗信号下水面船的自主准确定位。

为实现上述目的，本发明提供了如下方案：

一种水面船自主定位方法，所述定位方法包括：

根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程；

根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程；

根据前一时刻的系统状态向量均值的后验估计值和前一时刻的协方差矩阵平方根因子的后验估计值，利用所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值；