一种水面船自主定位方法及系统技术方案

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

【技术实现步骤摘要】
一种水面船自主定位方法及系统
本专利技术涉及水面船自主定位领域，特别是涉及一种水面船自主定位方法及系统。
技术介绍
随着海洋科学技术及计算机技术的不断进步，自主式水面船已经在民用和军事领域扮演着越来越重要的角色。目前，自主式水面船已广泛应用于海洋资源探测、海上巡逻、侦查和搜救等作业。水面船的高精度导航定位是保证水面船能顺利完成既定任务的重要前提，也是当前自主式水面船领域的热点问题。通用的水面船导航定位系统搭载有捷联惯性导航和GPS/北斗等传感器。捷联惯性导航系统内部的加速度计通过对获取的加速度信息两次积分操作求得水面船的实际位置。但是一方面加速度计存在零漂和温漂等效应，另一方面加速度计测量的是包括水面船的线加速度、重力加速度和由于水面船旋转运动产生的向心加速度等的叠加值，以上缺点使得单独利用加速度计对水面船导航定位误差较大。GPS/北斗导航系统已经广泛应用在军事和民用等各个领域，但是当水面船在狭窄水域作业或执行隐秘军事任务时，GPS/北斗信号会受周围环境和地形的影响，甚至导致信号丢失，无法实现水面船的自主定位。目前，通常采用滤波技术实现多源传感器的信息融合，以期获得较为准确的水面船位置信息。较为典型的，卡尔曼滤波器(KalmanFilter，KF)已经广泛应用于移动机器人、水面船等的导航定位问题。理论表明，当系统的状态方程和观测方程都为线性且噪声满足高斯分布时，KF是最优的线性滤波器。然而现实世界中几乎所有的系统都是非线性的，针对非线性系统的导航定位，人们提出了各种非线性滤波方法。扩展卡尔曼滤波器(ExtendedKF，EKF)通过对非线性方程线性化来处理非线性问题，然而，由于EKF采用一阶的泰勒展开近似非线性方程，引入了高阶项截断误差，只适用于弱非线性系统。无迹卡尔曼滤波器(UnscentedKF，UKF)作为常用的非线性滤波器已经广泛应用在自主车辆定位和水下航行器导航定位等领域。UKF是基于“对概率分布进行近似要比对非线性函数进行近似容易得多”的思想而提出的。针对n维非线性系统，UKF通过无迹变换(UnscentedTransform，UT)，选择"2n+1"个不同权值的Sigma点来近似计算状态变量的均值。和EKF相比，UKF通过Sigma点经过非线性函数传播后导出的均值和方差能够达到非线性函数真实值的三阶精度，因此精度高于EKF，但是在高维系统中UKF容易出现数值不稳定的现象，甚至会导致发散。因此，提出一种保障水面船在缺失GPS/北斗信号等恶劣环境下的准确定位的方法变得十分重要和有意义。
技术实现思路
本专利技术的目的是提供一种水面船自主定位方法及系统，以实现在缺失GPS/北斗信号下水面船的自主准确定位。为实现上述目的，本专利技术提供了如下方案：一种水面船自主定位方法，所述定位方法包括：根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程；根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程；根据前一时刻的系统状态向量均值的后验估计值和前一时刻的协方差矩阵平方根因子的后验估计值，利用所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值；根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点；根据当前时刻的系统状态向量均值的先验估计值和所述观测量测容积点，利用所述观测方程，确定卡尔曼系数矩阵；根据所述卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值；根据所述当前时刻的系统状态向量均值的先验估计值和所述卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将所述水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置。可选的，所述根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程，具体包括：根据姿态传感器测量的水面船的偏航角信息构建水面船的初始状态方程；利用一阶前向差分方法对所述初始状态方程进行离散化，获得水面船的离散状态方程矩阵；在所述离散状态方程矩阵的基础上加入水面船系统的过程噪声，获得水面船系统的状态方程为Xk+1＝AkXk+ωX,k；其中，Xk+1为k+1时刻的水面船系统的系统状态向量，Ak为系统矩阵，Xk为k时刻的水面船系统的系统状态向量，ωX,k为k时刻的过程噪声。可选的，所述根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程，具体包括：建立电机电压和水面船稳态速度的二次函数关系式为设定在载体坐标系下水面船的横向速度为0，根据所述二次函数关系式，确定水面船系统的观测方程为Zk＝h(Xk)+ωZ,k；其中，U为电机电压，u为水面船稳态速度，Zk为k时刻的电压测量值，h(Xk)＝U(uk)，h()为Xk与Zk的非线性关系函数，Xk为k时刻的水面船系统的系统状态向量，U(uk)为水面船稳态速度为uk时的电机电压，ωZ,k为k时刻的测量噪声。可选的，所述利用当前时刻的前一时刻的系统状态向量均值的后验估计值和当前时刻的前一时刻的协方差矩阵平方根因子的后验估计值和所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，具体包括：根据k-1时刻的系统状态向量均值的后验估计值和k-1时刻的协方差矩阵平方根因子的后验估计值，利用公式计算得到k-1时刻的第i个状态容积点Xi,k-1k-1；通过公式利用所述状态方程对每个状态容积点进行传播，得到k-1时刻的传播后的每个状态容积点根据所有传播后的状态容积点，利用公式确定水面船系统k时刻的系统状态向量均值的先验估计值根据所有传播后的状态容积点和k时刻的系统状态向量均值的先验估计值，利用公式确定第一加权中心矩阵根据所述第一加权中心矩阵，采用QR分解法，确定水面船系统k时刻的协方差矩阵平方根因子的先验估计值；其中，Sk-1|k-1为k-1时刻的协方差矩阵平方根因子的后验估计值，ξi为第i个状态容积点的中间变量，n为系统状态向量的维度，[1]i为中的任意一个列向量，为k-1时刻的传播后的第i个状态容积点，为k-1时刻的系统状态向量均值的后验估计值，Ak为系统矩阵，m为容积点的数量，分别为k-1时刻的传播后的第1个、第2个、第m个状态容积点。可选的，所述根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点，具体包括：根据k时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，利用公式获得k时刻的第i个观测量测容积点Xi,k|k-1；其中，Sk|k-1为k时刻的协方差矩阵平方根因子的先验估计值，ξi′为第i个观测量测容积点的中间变量，n为系统状态向量的维度，[1]i为中的任意一个列向量，为k时刻的系统状态向量均值的先验估本文档来自技高网...

【技术保护点】
1.一种水面船自主定位方法，其特征在于，所述定位方法包括：/n根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程；/n根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程；/n根据前一时刻的系统状态向量均值的后验估计值和前一时刻的协方差矩阵平方根因子的后验估计值，利用所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值；/n根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点；/n根据当前时刻的系统状态向量均值的先验估计值和所述观测量测容积点，利用所述观测方程，确定卡尔曼系数矩阵；/n根据所述卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值；/n根据所述当前时刻的系统状态向量均值的先验估计值和所述卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将所述水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置。/n

【技术特征摘要】
1.一种水面船自主定位方法，其特征在于，所述定位方法包括：
根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程；
根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程；
根据前一时刻的系统状态向量均值的后验估计值和前一时刻的协方差矩阵平方根因子的后验估计值，利用所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值；
根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点；
根据当前时刻的系统状态向量均值的先验估计值和所述观测量测容积点，利用所述观测方程，确定卡尔曼系数矩阵；
根据所述卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值；
根据所述当前时刻的系统状态向量均值的先验估计值和所述卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将所述水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置。


2.根据权利要求1所述的水面船自主定位方法，其特征在于，所述根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程，具体包括：
根据姿态传感器测量的水面船的偏航角信息构建水面船的初始状态方程；
利用一阶前向差分方法对所述初始状态方程进行离散化，获得水面船的离散状态方程矩阵；
在所述离散状态方程矩阵的基础上加入水面船系统的过程噪声，获得水面船系统的状态方程为Xk+1＝AkXk+ωX,k；
其中，Xk+1为k+1时刻的水面船系统的系统状态向量，Ak为系统矩阵，Xk为k时刻的水面船系统的系统状态向量，ωX,k为k时刻的过程噪声。


3.根据权利要求1所述的水面船自主定位方法，其特征在于，所述根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程，具体包括：
建立电机电压和水面船稳态速度的二次函数关系式为
设定在载体坐标系下水面船的横向速度为0，根据所述二次函数关系式，确定水面船系统的观测方程为Zk＝h(Xk)+ωZ,k；
其中，U为电机电压，u为水面船稳态速度，Zk为k时刻的电压测量值，h(Xk)＝U(uk)，h()为Xk与Zk的非线性关系函数，Xk为k时刻的水面船系统的系统状态向量，U(uk)为水面船稳态速度为uk时的电机电压，ωZ,k为k时刻的测量噪声。


4.根据权利要求1所述的水面船自主定位方法，其特征在于，所述利用当前时刻的前一时刻的系统状态向量均值的后验估计值和当前时刻的前一时刻的协方差矩阵平方根因子的后验估计值和所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，具体包括：
根据k-1时刻的系统状态向量均值的后验估计值和k-1时刻的协方差矩阵平方根因子的后验估计值，利用公式计算得到k-1时刻的第i个状态容积点Xi,k-1k-1；
通过公式利用所述状态方程对每个状态容积点进行传播，得到k-1时刻的传播后的每个状态容积点
根据所有传播后的状态容积点，利用公式确定水面船系统k时刻的系统状态向量均值的先验估计值
根据所有传播后的状态容积点和k时刻的系统状态向量均值的先验估计值，利用公式确定第一加权中心矩阵
根据所述第一加权中心矩阵，采用QR分解法，确定水面船系统k时刻的协方差矩阵平方根因子的先验估计值；
其中，Sk-1|k-1为k-1时刻的协方差矩阵平方根因子的后验估计值，ξi为第i个状态容积点的中间变量，n为系统状态向量的维度，[1]i为中的任意一个列向量，为k-1时刻的传播后的第i个状态容积点，为k-1时刻的系统状态向量均值的后验估计值，Ak为系统矩阵，m为容积点的数量，分别为k-1时刻的传播后的第1个、第2个、第m个状态容积点。


5.根据权利要求1所述的水面船自主定位方法，其特征在于，所述根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点，具体包括：
根据k时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，利用公式获得k时刻的第i个观测量测容积点Xi,k|k-1；
其中，Sk|k-1为k时刻的协方差矩阵平方根因子的先验估计值，ξ′i为第i个观测量测容积点的中间变量，n为系统状态向量的维度，[1]i为中的任意一个列向量，为k时刻的系统状态向量均值的先验估计值。


6.根据权利要求5所述的水面船自主定位方法，其特征在于，所述根据当前时刻的系统状态向量均值的先验估计值和所述观测量测容积点和所述观测方程，确定卡尔曼系数矩阵，具体包括：
通过公式Zi,k|k-1＝h(...

【专利技术属性】
技术研发人员：项基，寇立伟，
申请(专利权)人：浙江傲宋智能科技有限公司，
类型：发明
国别省市：浙江;33