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

权利要求书

1.一种水面船自主定位方法，其特征在于，所述定位方法包括：

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

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

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

根据当前时刻的系统状态向量均值的先验估计值和协方差矩阵平方根因子的先验估计值，计算得到观测量测容积点；

根据当前时刻的系统状态向量均值的先验估计值和所述观测量测容积点，利用所述观测方程，确定卡尔曼系数矩阵；

根据所述卡尔曼系数矩阵和当前时刻的系统观测噪声协方差的平方根因子，确定当前时刻的协方差矩阵平方根因子的后验估计值；

根据所述当前时刻的系统状态向量均值的先验估计值和所述卡尔曼系数矩阵，确定水面船系统当前时刻的系统状态向量均值的后验估计值，并将所述水面船系统当前时刻的系统状态向量均值的后验估计值作为水面船当前时刻的位置。

2.根据权利要求1所述的水面船自主定位方法，其特征在于，所述根据姿态传感器测量的水面船的偏航角信息建立水面船系统的状态方程，具体包括：

根据姿态传感器测量的水面船的偏航角信息构建水面船的初始状态方程；

利用一阶前向差分方法对所述初始状态方程进行离散化，获得水面船的离散状态方程矩阵；

在所述离散状态方程矩阵的基础上加入水面船系统的过程噪声，获得水面船系统的状态方程为X_k+1＝A_kX_k+ω_X,k；

其中，X_k+1为k+1时刻的水面船系统的系统状态向量，A_k为系统矩阵，X_k为k时刻的水面船系统的系统状态向量，ω_X,k为k时刻的过程噪声。

3.根据权利要求1所述的水面船自主定位方法，其特征在于，所述根据电机电压和水面船稳态速度的非线性函数关系，建立水面船系统的观测方程，具体包括：

建立电机电压和水面船稳态速度的二次函数关系式为

设定在载体坐标系下水面船的横向速度为0，根据所述二次函数关系式，确定水面船系统的观测方程为Z_k＝h(X_k)+ω_Z,k；

其中，U为电机电压，u为水面船稳态速度，Z_k为k时刻的电压测量值，h(X_k)＝U(u_k)，h()为X_k与Z_k的非线性关系函数，X_k为k时刻的水面船系统的系统状态向量，U(u_k)为水面船稳态速度为u_k时的电机电压，ω_Z,k为k时刻的测量噪声。