一种水面船自主定位方法及系统与流程

本发明涉及水面船自主定位领域，特别是涉及一种水面船自主定位方法及系统。
背景技术：
：随着海洋科学技术及计算机技术的不断进步，自主式水面船已经在民用和军事领域扮演着越来越重要的角色。目前，自主式水面船已广泛应用于海洋资源探测、海上巡逻、侦查和搜救等作业。水面船的高精度导航定位是保证水面船能顺利完成既定任务的重要前提，也是当前自主式水面船领域的热点问题。通用的水面船导航定位系统搭载有捷联惯性导航和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时刻的系统状态向量均值的先验估计值。可选的，所述根据当前时刻的系统状态向量均值的先验估计值和所述观测量测容积点和所述观测方程，确定卡尔曼系数矩阵，具体包括：通过公式zi,k|k-1＝h(xi,k|k-1)，将k时刻的第i个观测量测容积点经过所述观测方程传播，得到k时刻的传播后的第i个观测量测容积点zi,k|k-1；根据所有传播后的观测量测容积点，利用公式确定水面船系统k时刻的系统观测向量均值的估计值根据所有传播后的观测量测容积点和k时刻的系统观测向量均值的估计值，利用公式确定第二加权中心矩阵zk|k-1；根据所述第二加权中心矩阵，采用qr分解法，确定水面船系统k时刻的观测自相关协方差矩阵平方根因子的后验估计值；根据所有传播后的观测量测容积点和k时刻的系统状态向量均值的先验估计值，利用公式确定第三加权中心矩阵χk|k-1；根据所述第二加权中心矩阵和所述第三加权中心矩阵，利用公式确定互协方差矩阵pxz,k|k-1；根据所述互协方差矩阵和所述k时刻的观测自相关协方差矩阵平方根因子的后验估计值，利用公式确定k时刻的卡尔曼系数矩阵wk；其中，z1,k|k-1、z2,k|k-1、zm,k|k-1分别为k时刻的传播后的第1个、第2个、第m个观测量测容积点，x1,k|k-1、x2,k|k-1、xi,k|k-1、xm,k|k-1分别为k时刻的第1个、第2个、第i个、第m个观测量测容积点，为第二加权中心矩阵zk|k-1的转置矩阵，为k时刻的观测自相关协方差矩阵平方根因子的后验估计值的转置。可选的，所述根据所述卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值，具体包括：根据所述卡尔曼系数矩阵，利用qr分解法，通过公式[qr]＝qr([χk|k-1-wkzk|k-1wksr,k]t)，获得上三角形矩阵r；根据所述上三角形矩阵r，利用公式sk|k＝rt，确定k时刻的协方差矩阵平方根因子的后验估计值；其中，q为正规正交矩阵，χk|k-1为第三加权中心矩阵，zk|k-1为第二加权中心矩阵，wk为k时刻的卡尔曼系数矩阵，sr,k为k时刻的系统观测噪声协方差的平方根因子，rt为上三角形矩阵r的转置矩阵。可选的，所述根据所述当前时刻的系统状态向量均值的先验估计值和所述卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，具体包括：根据所述k时刻的系统状态向量均值的先验估计值和所述k时刻的卡尔曼系数矩阵，利用公式确定水面船系统k时刻的系统状态向量均值的后验估计值其中，为k时刻的系统状态向量均值的先验估计值，wk为k时刻的卡尔曼系数矩阵，zk为k时刻的电压测量值，为水面船系统k时刻的系统观测向量均值的估计值。一种水面船自主定位系统，所述定位系统包括：状态方程建立模块，用于根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程；观测方程建立模块，用于根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程；先验估计值确定模块，用于根据前一时刻的系统状态向量均值的后验估计值、前一时刻的协方差矩阵平方根因子的后验估计值，利用所述状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值；观测量测容积点计算模块，用于根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点；卡尔曼系数矩阵确定模块，用于根据当前时刻的系统状态向量均值的先验估计值、所述观测量测容积点，利用所述观测方程，确定卡尔曼系数矩阵；后验估计值确定模块，用于根据所述卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值；水面船当前时刻位置确定模块，用于根据所述当前时刻的系统状态向量均值的先验估计值和所述卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将所述水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置。可选的，所述状态方程建立模块，具体包括：初始状态方程建立子模块，用于根据姿态传感器测量的水面船的偏航角信息构建水面船的初始状态方程；离散状态方程矩阵获得子模块，用于利用一阶前向差分方法对所述初始状态方程进行离散化，获得水面船的离散状态方程矩阵；状态方程获得子模块，用于在所述离散状态方程矩阵的基础上加入水面船系统的过程噪声，获得水面船系统的状态方程为xk+1＝akxk+ωx,k；其中，xk+1为k+1时刻的水面船系统的系统状态向量，ak为系统矩阵，xk为k时刻的水面船系统的系统状态向量，ωx,k为k时刻的过程噪声。根据本发明提供的具体实施例，本发明公开了以下技术效果：本发明提供了一种水面船自主定位方法及系统，仅依靠姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程，通过实验数值拟合的给定电机电压和水面船稳态速度的非线性函数关系建立水面船系统的观测方程，无需gps/北斗和外部环境交互的定位传感器，根据前一时刻的系统状态向量均值的后验估计值、前一时刻的协方差矩阵平方根因子的后验估计值，利用状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，根据这两项的先验估计值计算得到观测量测容积点，根据当前时刻的系统状态向量均值的先验估计值和观测量测容积点，利用观测方程确定卡尔曼系数矩阵，根据卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值，最后根据当前时刻的系统状态向量均值的先验估计值和卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，即得到水面船当前时刻的位置，实现了在缺失gps/北斗等外部传感器信号的恶劣环境下的自主准确定位。本发明采用的平方根容积卡尔曼滤波器无需开方操作，预测和更新过程中传播的仅是协方差矩阵的平方根因子，计算复杂度低。附图说明为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。图1为本发明提供的一种水面船自主定位方法的流程图；图2为本发明提供的一种水面船自主定位方法的原理图；图3为电机电压和水面船稳态速度之间的拟合曲线图；图4为利用本发明提供的一种水面船自主定位方法的水面船的自主定位效果图。具体实施方式下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。本发明的目的是提供一种水面船自主定位方法及系统，以实现在缺失gps/北斗信号下水面船的自主准确定位。为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。为了解决水面船在缺失gps/北斗等外部传感器信号的恶劣环境下的定位问题，本发明提供了一种水面船自主定位方法，水面船仅配置姿态传感器用于测量偏航角，无需感知外部环境信息的定位传感器，如gps/北斗导航模块、激光雷达和摄像头等，如图1和图2所示，定位方法包括：s101，根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程，具体包括：根据姿态传感器测量的水面船的偏航角信息构建水面船的初始状态方程；水面船在进行水下巡逻和环境监测任务时通常以匀速状态运动，基于此，水面船的状态方程建模如下：式中，表示惯性坐标系下水面船的位置坐标，ψ表示惯性坐标系下水面船的偏航角，分别表示水面船的纵向加速度和横向加速度都为零，u、v分别表示在载体坐标系下水面船的纵向和横向速度。利用一阶前向差分方法对初始状态方程进行离散化，获得水面船的离散状态方程矩阵；为了方便利用数字滤波器对水面船进行位置估计，采用一阶前向差分方法将水面船模型近似表示为以下离散形式：式中，xk和yk表示k时刻系统状态的水面船的位置坐标采样值，xk+1和yk+1表示k+1时刻系统状态的水面船的位置坐标采样值，uk、vk分别表示在载体坐标系下k时刻水面船的纵向和横向速度，uk+1、vk+1分别表示在载体坐标系下k+1时刻水面船的纵向和横向速度，δt表示k时刻和k+1时刻的时间间隔，ψk表示k时刻惯性坐标系下水面船的偏航角。离散状态方程矩阵表示如下：式中：xk＝[xk，yk，uk，vk]t表示k时刻的系统状态向量。在离散状态方程矩阵的基础上加入水面船系统的过程噪声，获得水面船系统的状态方程为xk+1＝akxk+ωx，k。其中，xk+1为k+1时刻的水面船系统的系统状态向量，ak为系统矩阵，xk为k时刻的水面船系统的系统状态向量，ωx，k为k时刻的过程噪声，ωx，k服从n(0，qk)的高斯分布，qk为ωx，k的方差。s102，根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程，具体包括：不考虑水面船后向运动的情况，根据实验数值拟合得到给定电机电压和水面船稳态速度的二次函数关系式为电机电压和水面船稳态速度之间的拟合曲线如图3所示；设定在载体坐标系下水面船的横向速度为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时刻的测量噪声，ωz，k服从n(0，rk)的高斯分布，rk为ωz，k的方差。根据步骤s101和s102，水面船系统可用如下状态方程和观测方程刻画其中，过程噪声ωx，k和观测噪声ωz.k互不相关，即cov(ωx，k，ωz，k)＝0。根据平方根容积卡尔曼滤波器(squarerootcubaturekalmanfiltering，sckf)，结合水面船的状态方程和观测方程对水面船的位置进行估计。主要包括先验定位预测(步骤s103-s104)和后验定位更新(步骤s105-s107)两部分内容。s103，根据前一时刻的系统状态向量均值的后验估计值和前一时刻的协方差矩阵平方根因子的后验估计值，利用状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，具体包括：根据k-1时刻的系统状态向量均值的后验估计值和k-1时刻的协方差矩阵平方根因子的后验估计值，利用公式计算得到k-1时刻的第i个状态容积点xi,k-1|k-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个状态容积点。s104，根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点，具体包括：根据k时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，利用公式获得k时刻的第i个观测量测容积点xi,k|k-1。其中，sk|k-1为k时刻的协方差矩阵平方根因子的先验估计值，ξ′i为第i个观测量测容积点的中间变量，n为系统状态向量的维度，[1]i为中的任意一个列向量，为k时刻的系统状态向量均值的先验估计值。s105，根据当前时刻的系统状态向量均值的先验估计值和观测量测容积点，利用观测方程，确定卡尔曼系数矩阵，具体包括：通过公式zi,k|k-1＝h(xi,k|k-1)，将k时刻的第i个观测量测容积点经过观测方程传播，得到k时刻的传播后的第i个观测量测容积点zi,k|k-1。根据所有传播后的观测量测容积点，利用公式确定水面船系统k时刻的系统观测向量均值的估计值根据所有传播后的观测量测容积点和k时刻的系统观测向量均值的估计值，利用公式确定第二加权中心矩阵zk|k-1。根据第二加权中心矩阵，采用qr分解法，确定水面船系统k时刻的观测自相关协方差矩阵平方根因子的后验估计值。根据所有传播后的观测量测容积点和k时刻的系统状态向量均值的先验估计值，利用公式确定第三加权中心矩阵χk|k-1。根据第二加权中心矩阵和第三加权中心矩阵，利用公式确定互协方差矩阵pxz,k|k-1。根据互协方差矩阵和k时刻的观测自相关协方差矩阵平方根因子的后验估计值，利用公式确定k时刻的卡尔曼系数矩阵wk。其中，z1,k|k-1、z2,k|k-1、zm,k|k-1分别为k时刻的传播后的第1个、第2个、第m个观测量测容积点，x1,k|k-1、x2,k|k-1、xi,k|k-1、xm,k|k-1分别为k时刻的第1个、第2个、第i个、第m个观测量测容积点，为第二加权中心矩阵zk|k-1的转置矩阵，为k时刻的观测自相关协方差矩阵平方根因子的后验估计值的转置。s106，根据卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值，具体包括：根据卡尔曼系数矩阵，利用qr分解法，通过公式[qr]＝qr([χk|k-1-wkzk|k-1wksr,k]t)，获得上三角形矩阵r。根据上三角形矩阵r，利用公式sk|k＝rt，确定k时刻的协方差矩阵平方根因子的后验估计值。其中，q为正规正交矩阵，χk|k-1为第三加权中心矩阵，zk|k-1为第二加权中心矩阵，wk为k时刻的卡尔曼系数矩阵，sr,k为k时刻的系统观测噪声协方差的平方根因子，rt为上三角形矩阵r的转置矩阵。s107，根据当前时刻的系统状态向量均值的先验估计值和卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置，具体包括：根据k时刻的系统状态向量均值的先验估计值和k时刻的卡尔曼系数矩阵，利用公式确定水面船系统k时刻的系统状态向量均值的后验估计值其中，为k时刻的系统状态向量均值的先验估计值，wk为k时刻的卡尔曼系数矩阵，zk为k时刻的电压测量值，为水面船系统k时刻的系统观测向量均值的估计值。此外，为了提高水面船在运动状态突变(水面船的直角转弯)时的定位精度，在系统观测方程中引入低通滤波器，通过对给定电压信息的滤波，减少定位误差。本发明针对提供的一种水面船自主定位方法，以一艘长52cm，宽20cm的微型水面船为例对本发明做进一步详细描述。需要指出的是，其他各种尺寸和型号的水面船的基于平方根容积卡尔曼滤波的水面船定位算法类似，都被认为在本发明的范围之内。以下实施例旨在便于对本发明的理解，而对其不起任何限定作用。一种水面船自主定位方法，包括以下步骤：步骤1、系统建模，如图2的建模部分。主要包括建立系统的状态方程和观测方程。建立状态方程时，偏航角信息的采样时间和程序控制周期一致，均为50ms，即δt＝0.05。系统状态方程如下：根据实验数据拟合给定电机电压u和水面船的稳态速度u的非线性关系，并以此建立系统的输出方程。为避免单次测量的随机性，针对某一给定电压u，测量5次水面船的稳态速度并取平均作为最终的稳态速度值，即给定电机电压u和水面船的稳态速度u测量数据如下表：表1给定电机电压u和水面船的稳态速度u试验数据表给定电压(标幺值)0.1000.2000.3000.3330.4000.4670.567稳态速度(m/s)0.0000.2550.3970.4330.5000.5680.650利用数值拟合易得给定电机电压u和水面船的稳态速度u满足如下关系：拟合曲线如图3所示。本发明不考虑水面船后退运动的情况。假设水面船在匀速运动时横向速度v＝0，结合上式，水面船系统的观测方程如下：步骤2、位置估计，如图2的预测和更新部分。系统状态向量维度为4，则预测流程为：sk|k-1＝rt更新流程为：zi，k|k-1＝h(xi，k|k-1)[q·r]＝qr([zk|k-1，sr，k]t)szz，k|k-1＝rt[q·r]＝·qr([χk|k-1-wkzk|k-1wksr，k]t)sk|k＝rt步骤3、低通滤波处理。在水面船实际运行过程中，尤其是直角转弯这种状态变化迅速的情况，由于水面船的惯性存在，即使给定电机电压为0，水面船也会继续向前运动一段距离。基于以上事实，为了提高水面船定位的精度，在实际的给定电机电压u引入一个低通滤波器。图4展示了水面船的定位效果，由图可知，加上低通滤波之后的定位结果sckf2比不加低通滤波的定位结果sckf1效果更好。综上，使用本发明提出的基于平方根容积卡尔曼滤波的水面船自主定位算法，无需gps/北斗等和外部环境交互的定位传感器，通过实验数值拟合的给定电机电压u和水面船稳态速度u的非线性函数关系，结合姿态传感器的偏航角信息，可以在缺失gps/北斗等外部传感器信号的恶劣环境下实现定位功能。与传统水面船定位技术相比，本发明具有以下有益效果：1、本发明无需任何的水面船动力学参数，实现较为简单，方便应用于实际场景；2、本发明仅需要测量水面船的偏航角，无需其他和外部环境交互的定位传感器，如gsp/北斗定位系统，激光雷达和视觉传感器等；3、本发明采用的是平方根容积卡尔曼滤波器实现数据融合，与传统容积卡尔曼滤波器相比，平方根容积卡尔曼滤波器无需开方操作，预测和更新过程中传播的仅是协方差矩阵的平方根因子，计算复杂度低，可以确保协方差矩阵的半正定性和非负性，有助于改进数值精度和稳定性。本发明还提供了一种水面船自主定位系统，定位系统包括：状态方程建立模块、观测方程建立模块、先验估计值确定模块、观测量测容积点计算模块、卡尔曼系数矩阵确定模块、后验估计值确定模块和水面船当前时刻位置确定模块。状态方程建立模块，用于根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程。观测方程建立模块，用于根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程。先验估计值确定模块，用于根据前一时刻的系统状态向量均值的后验估计值、前一时刻的协方差矩阵平方根因子的后验估计值，利用状态方程，确定水面船系统当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值。观测量测容积点计算模块，用于根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点。卡尔曼系数矩阵确定模块，用于根据当前时刻的系统状态向量均值的先验估计值、观测量测容积点，利用观测方程，确定卡尔曼系数矩阵。后验估计值确定模块，用于根据卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值。水面船当前时刻位置确定模块，用于根据当前时刻的系统状态向量均值的先验估计值和卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置。状态方程建立模块，具体包括：初始状态方程建立子模块、离散状态方程矩阵获得子模块和状态方程获得子模块。初始状态方程建立子模块，用于根据姿态传感器测量的水面船的偏航角信息构建水面船的初始状态方程。离散状态方程矩阵获得子模块，用于利用一阶前向差分方法对初始状态方程进行离散化，获得水面船的离散状态方程矩阵。状态方程获得子模块，用于在离散状态方程矩阵的基础上加入水面船系统的过程噪声，获得水面船系统的状态方程为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-1|k-1。传播后的状态容积点获得子模块，用于通过公式利用状态方程对每个状态容积点进行传播，得到k-1时刻的传播后的每个状态容积点系统状态向量均值的先验估计值确定子模块，用于根据所有传播后的状态容积点，利用公式确定水面船系统k时刻的系统状态向量均值的先验估计值第一加权中心矩阵确定子模块，用于根据所有传播后的状态容积点和k时刻的系统状态向量均值的先验估计值，利用公式确定第一加权中心矩阵协方差矩阵平方根因子的先验估计值确定子模块，用于根据第一加权中心矩阵，采用qr分解法，确定水面船系统k时刻的协方差矩阵平方根因子的先验估计值。其中，sk-1k-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时刻的系统状态向量均值的先验估计值。卡尔曼系数矩阵确定模块，具体包括：传播后的观测量测容积点获得子模块、系统观测向量均值的估计值确定子模块、第二加权中心矩阵确定子模块、观测自相关协方差矩阵平方根因子的后验估计值确定子模块、第三加权中心矩阵确定子模块、互协方差矩阵确定子模块和卡尔曼系数矩阵确定子模块。传播后的观测量测容积点获得子模块，用于通过公式zi,k|k-1＝h(xi,k|k-1)，将k时刻的第i个观测量测容积点经过观测方程传播，得到k时刻的传播后的第i个观测量测容积点zi,k|k-1。系统观测向量均值的估计值确定子模块，用于根据所有传播后的观测量测容积点，利用公式确定水面船系统k时刻的系统观测向量均值的估计值第二加权中心矩阵确定子模块，用于根据所有传播后的观测量测容积点和k时刻的系统观测向量均值的估计值，利用公式确定第二加权中心矩阵zk|k-1。观测自相关协方差矩阵平方根因子的后验估计值确定子模块，用于根据第二加权中心矩阵，采用qr分解法，确定水面船系统k时刻的观测自相关协方差矩阵平方根因子的后验估计值。第三加权中心矩阵确定子模块，用于根据所有传播后的观测量测容积点和k时刻的系统状态向量均值的先验估计值，利用公式确定第三加权中心矩阵χk|k-1。互协方差矩阵确定子模块，用于根据第二加权中心矩阵和第三加权中心矩阵，利用公式确定互协方差矩阵pxz,k|k-1。卡尔曼系数矩阵确定子模块，用于根据互协方差矩阵和k时刻的观测自相关协方差矩阵平方根因子的后验估计值，利用公式确定k时刻的卡尔曼系数矩阵wk。其中，z1,k|k-1、z2,k|k-1、zm,k|k-1分别为k时刻的传播后的第1个、第2个、第m个观测量测容积点，x1,k|k-1、x2,k|k-1、xi,k|k-1、xm,k|k-1分别为k时刻的第1个、第2个、第i个、第m个观测量测容积点，为第二加权中心矩阵zk|k-1的转置矩阵，为k时刻的观测自相关协方差矩阵平方根因子的后验估计值的转置。后验估计值确定模块，具体包括：上三角形矩阵获得子模块和协方差矩阵平方根因子的后验估计值确定子模块。上三角形矩阵获得子模块，用于根据卡尔曼系数矩阵，利用qr分解法，通过公式[qr]＝qr([χk|k-1-wkzk|k-1wksr,k]t)，获得上三角形矩阵r。协方差矩阵平方根因子的后验估计值确定子模块，用于根据上三角形矩阵r，利用公式sk|k＝rt，确定k时刻的协方差矩阵平方根因子的后验估计值。其中，q为正规正交矩阵，χk|k-1为第三加权中心矩阵，zk|k-1为第二加权中心矩阵，wk为k时刻的卡尔曼系数矩阵，sr,k为k时刻的系统观测噪声协方差的平方根因子，rt为上三角形矩阵r的转置矩阵。水面船当前时刻位置确定模块，具体包括：系统状态向量均值的后验估计值确定子模块。系统状态向量均值的后验估计值确定子模块，用于根据k时刻的系统状态向量均值的先验估计值和k时刻的卡尔曼系数矩阵，利用公式确定水面船系统k时刻的系统状态向量均值的后验估计值其中，为k时刻的系统状态向量均值的先验估计值，wk为k时刻的卡尔曼系数矩阵，zk为k时刻的电压测量值，为水面船系统k时刻的系统观测向量均值的估计值。本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。当前第1页12